我最难忘的一天基于Frenet坐标系的⽆⼈车轨迹规划详解与实现
⽬的
本⽂详细讲解Frenet坐标系中的⽆⼈车轨迹规划⽅法，并探讨在ROS下的代码实现，先看下仿真的效果。
1 简介
Frenet坐标系中的⽆⼈车轨迹规划⽅法（简称Frenet⽅法）是 Moritz Werling 在2010年的论⽂《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》⾥⾸次提出的。其思想被⼴泛采⽤，甚⾄Matlab官⽅都加⼊了相应的函数：，效果如下⾯左侧的。此外，⼀些开源的⽆⼈驾驶项⽬也借鉴了其核⼼思想，例如百度的Apollo⽆⼈驾驶项⽬就使⽤了Frenet坐标系。因为很出名，所以⽹上已经有很多基于Frenet⽅法的规划模块开源代码，我们后⾯的实现也基于⼀个公开的开源项⽬：。
2 Frenet⽅法讲解
2.1 规划的⽬标
如果问：⽆⼈车规划轨迹的宗旨是什么？笔者认为有两个：安全和舒适。安全肯定是⾸要的，如果规划出来的轨迹导致撞车显然是任何⼈都⽆法接收的。⾄于舒适，则主要⾯向有⼈乘坐的⽆⼈车，如
降尿酸茶
果⽆⼈车动不动就加速或者急刹车你坐在上⾯肯定会抱怨，当然对于拉货的车辆也不是说可以任意加减速，这对车辆的执⾏器件也是伤害。因此这两个指标是⼀般的规划⽅法都要考虑的。Frenet⽅法设计的出发点也考虑了这两个⽅⾯。
2.2 Frenet坐标系
Frenet 坐标系是这个⽅法最⼤的特点。既然我们选择了Frenet ⽅法，那么原因是什么？它的特点⼜是什么？
笔者认为Frenet ⽅法的⼀个优点是思想简单，容易理解，⽽且性能不错。当然Frenet ⽅法也有很多缺点，例如依赖参考线、坐标系的变换⽐较⿇烦等等，这些我们后⾯再说。⽹上可以搜到很多作者对这种⽅法的介绍（例如），但是他们描述的都很粗糙⽽且不够形象，很多细节没有提，导致读者不能快速⽽深刻地领会这个⽅法，后果就是不能⾃⼰实现，接下来笔者仔细介绍下Frenet ⽅法。
既然⽅法的名字叫“基于Frenet 坐标系的轨迹规划”，那⾸先必须理解什么是“Frenet 坐标系”。我们想象⼀下⾃⼰开车时，最简单省劲的策略就是沿着道路上的导引线开。这也是车道线存在的意义。我们把车道线想象成⼀条连续的曲线。对于⼀条曲线，如果它是光滑可微的，那么曲线上的每⼀点处都可以求切线。求切线的⽬的是它定义了⼀个⽅向向量。切线有了，再逆时针旋转90°就能得到⼀条法线，如下图所⽰。很显然，切线和法线还有它们的交点就构成了⼀个直⾓坐标系，这个坐标系就是传说中
细胞膜的基本支架
的Frenet 坐标系。所以Frenet 坐标系也是⼀个直⾓坐标系，没什么神秘的，⼤家不⽤害怕。为了便于理解，我特意⽤Mathematica 做了⼀个演⽰动画。⿊⾊曲线代表道路参考线，⽤⿏标能选择曲线上不同的点，随着⿏标移动道路上的点也在不断变化，对应的Frenet
坐标系就跟着变化，很形象了吧。
总结报告
2.3 Frenet 坐标系中的规划
现在你知道了什么是Frenet 坐标系？那么“在Frenet 坐标系下进⾏轨迹规划”⼜是什么意思呢？这其实就是说我们计算的轨迹是相对于Frenet 坐标系表⽰的。⽤⽂字描述这个过程实在是不够直观，所以
笔者⼜制作了⼀个动画，见下图。笔者⽤⼀个最简单的例⼦，就是⼀次多项式来展⽰。下⾯的左图是⼀次多项式的图像，当然了⼀次多项式就是⼀个直线。注意，这是在⼀个直⾓坐标系中画出来的，如果我们把这个坐标系的横轴当成曲线的弧长，纵轴当成到曲线的距离，那么在⼀个参考曲线中画出来⼜是什么样的呢，就是下图右侧的图展⽰的。这跟Frenet 坐标系有什么关系呢？如果把这个直线离散成⼀些点，那把每个点的横坐标看成弧长，根据弧长能找到曲线上⼀个唯⼀的点，这个点决定⼀个唯⼀的Frenet 坐标系。我们再利⽤这个点的纵坐标，在这个曲线上的点的基础上沿着Frenet 坐标系的法向量移动长度就得到⼀个新的点，这个新的点就是Frenet 坐标系中的点。所以，我们在规划时依然是在直⾓坐标系中的进⾏的，规划出来曲线后再离散，然后转换到Frenet 坐标系中就⾏了。这⾥不太好理解的⼀点就是，左图的坐标系到底是什么。想象⼀下，如果你把右图中的⿊⾊参考曲线拉直了，拉成⼀条直线，那两边的图像就变成完全⼀样的了。所以，左图其实是相对于参考曲线的变化，⽽右图是相对变化叠加到（⼀个固定的）参考
曲线上得到的曲线，这么说应该容易理解了。
现在我们理解“在Frenet 坐标系下进⾏轨迹规划”的含义了，说⽩了就是两条曲线的叠加。虽说原理清楚了，但是笔者还要交代⼏句要注意的细节。上⾯提到的弧长指的是道路参考线（center line ）上的动点与道路参考线的起点之间曲线的长度（也就是上⾯右图中的红⾊曲线，可能看不太清），⽽不是你规划的轨迹（蓝⾊曲线）的长度。这个初学者很容易傻傻分不清楚，因为笔者⼀开始也搞混了，导致看后⾯的推导公式总是理解不了。
所以，Frenet 坐标系仅仅是⼀种描述⽅式，地位和全局直⾓坐标系⼀样。规划使⽤的曲线表达式与坐标系和参考曲线也没关系，例如你可以在Frenet 坐标系下使⽤任何⼀种曲线表达⽅式，例如B 样条、贝塞尔曲线、Cycloid 都可以，不必像原始论⽂⼀样局限于单段多次多项式。举个例⼦，华为的专利中就采⽤了分段样条曲线的表达⽅式，如下图。
s d d s
2.4 参考线
Frenet坐标系是依附于参考线的，因此参考线在整个规划过程中是必不可少的。在百度Apollo⾃动驾驶项⽬中其也被称为参考线（Reference Line），读者请注意区分参考线与车道线，车道线其实是指车道两侧的分界线，是两条；参考线可以取为车道线边界围成区域的中⼼线，是⼀条。在轨迹规划之前要先⽣成参考线，那么参考线是怎么来的呢？⼀般是先根据起点和⽬标点进⾏路线规划，根据路线经过的道路从⾼精地图中查找对应的道路中⼼线或者边界线，将所有道路的道路中⼼线拼接起来并进⾏光滑处理，防⽌出现间断或者跳变，因为⼀旦有不连续或者不光滑就会导致Frenet坐标系的不连续。这样就得到了⼀条光滑的参考线。
2.5 为什么⽤多项式？
⽆⼈车轨迹规划的⽬的就是得到⼀条曲线。为了在计算机中处理，这条曲线就必须可以⽅便地描述，这样⼀来我们可选的曲线并不是⾮常多，常⽤的有B 样条曲线、Bezier 曲线、Dubins 曲线等等。我们在初中就已经认识多项式了，可以说是⽼熟⼈了，对它的性质也⽐较熟悉。⽐如，⼤名⿍⿍的泰勒公式就告诉我们，光滑的函数可以⽤很多个多项式来近似表⽰。这⾥说的多项式是指像这样 的⼀元幂次多项式。那么⽤⼏次多项式好呢？如果画出函数的图像，多项式其实也是⼀种曲线。跟前⾯这些曲线相⽐，多项式的形式更简单，⽆论是求导还是积分都⽐较容易。所以，为什么不试试⽤多项式
来表⽰⽆⼈车的轨迹呢？显然论⽂作者就是这么考虑的。如果次数太少，曲线的表达能⼒不⾜，给不出我们想要的形状，如果次数太多，曲线过于扭曲，也不容易求解。经过仔细的思考，作者提出使⽤4次和5次这两种多项式。为什么选择这两种？作者给出了理由。⾸先，决定车辆乘坐舒适与否的指标不是速度、也不是加速度、⽽是取决于加加速度（英语叫Jerk ）。根据最优控制的原理，对曲线上的加加速度进⾏积分，积分值最⼩的曲线刚好就是5次多项式。其次，5次多项式有6个待定系数，如果给定3个初始时刻的位置、速度、加速度三元组和终⽌时刻的三元组这六个已知量，刚好可以求出6个待定系数从⽽完全确定这个5次多项式。当然，有时我们不关⼼终⽌时刻的速度，所以有时也⽤4次多项式。
插图简笔画
2.6 Frenet 坐标系与笛卡尔坐标系的转换
轨迹规划出的曲线是叠加在Frenet 坐标系下的结果。⽽计算轨迹的最⼤速度、判断有没有碰撞这些操作显然必须要在全局世界（笛卡尔）坐标系中进⾏，所以不可避免要在这两个坐标系之间转换。转换的⽅法如下：
1 Frenet Cartesian ：即已知坐标计算坐标。这个计算简单，假设我们知道道路参考线的参数⽅程（⽤弧长参数化表⽰的），是⽤弧长作为⾃变量，参考线的坐标作为的函数，即。根据算出参考线的坐标，再对样条曲线求导算出处参考线的单位切线，再逆时针旋转得到单位法线向量，所以最终
；
2 Cartesian Frenet ：即已知坐标计算坐标。这个问题不容易，相当于在曲线上找到⼀个点，使得它离曲线外⾯⼀个固定点的距离最⼩。当然，如果这个曲线⾮常扭曲，找起来确实很难。但是如果曲线（参考线）是通过三次样条曲线插值离散参考点得到的，所以它就是个三次多项式，⽽且我们假设每两个参考点之间⼀段的曲线要么是直线要么是凸的，不会出现两次扭曲。当参考点取的⾜够密时，这样的假设是合理的。所以现在曲线的形式简单了，这样我们可以通过解⽅程求出这个点。
假设曲线外的点的坐标是，三次曲线⽅程是，点到曲线的距离平⽅如下（记为）：
其中唯⼀的未知量是，所以我们求的导数，并令其等于0即可。求出的点最多有五个，但是前⾯假设曲线只有⼀个弯曲的⽅向（三次项的系数），所以实际最多三个点，我们挑出距离最⼩的那个即可。
注意，弧长总是正的（我们假设车辆不能逆⾏或者倒车），但是可以是负的，道路左侧可以定义成，右侧可以定义成。 在百度Apollo ⽆⼈驾驶项⽬中，这⼆者的转换函数在modules\common\math 路径下的cartesian_frenet_conversion ⽂件。但
是cartesian_to_frenet 函数只是把算出来了，⽽最难的没算（直接作为已知输⼊）。注意百度把坐标记为坐标，这是因为符号被⽤于表⽰求导，防⽌弄混。
JMT 函数中的polyeval(s0,T)是什么意思？polyeval 是求多项式在某点的所有导数。这⾥的多项式是由系数s0定义的，s0是个状态类，它其实是。所以对应的多项式就是按照系数从低到⾼：。这时再经过polyeval 得到的输出就是三个导数项：零阶导数、⼀阶导数、⼆阶导数。
所以B 就是两个三维状态向量之差： generate_函数代码⽐较多，既然多我们就切成⼏部分来看。⾸先第⼀部分⽣成所有侧向轨迹，第⼆部分⾃然⽽然就是⽣成所有横向轨迹。第三部分则是将前⾯两部分的轨迹组合得到完整的轨迹，然后再根据代价从⼩到⼤排序，最后剔除掉可能碰撞的轨迹，最后剩下的最⼩轨迹就是输出的轨迹。y (x )=a +0a x +1a x +22a x +33a x +44a x 55→(s ,d )(x ,y )s (x ,y )r r s x =r x (s ),y =r r y (s )r s (x ,y )r r (x ,y )r r 90°n (x ,y )=(x ,y )+r r dn →(x ,y )(s ,d )(p ,p )x y y =ax +3bx +2cx +e g (x )g (x )=(x −p )+x 2(ax +3bx +2cx +e −p )y 2
x g (x )a =0s d d >0d <0d s (s ,d )(s ,l )d (s ,,)0s ˙021s ¨0f (t )=s +0t +s ˙0t 21
s ¨02s +0T +s ˙0T 21s ¨02+s ˙0T s ¨0s ¨0s −1(s +0T +s ˙0T )−21
s ¨02s ˙1(+s ˙0T )−s ¨0s ¨1s ¨0
美国运动鞋品牌 两个向量的叉积可以⽤来判断⼀个向量在另⼀个向量的那⼀侧，叉积的赋值是夹⾓的正弦。
{v1,v2}=m =RandomReal [{0,1},{2,2}];
ArcSin [Det [m ]/Norm [v1]/Norm [v2]]
VectorAngle [v1,v2]
Graphics [{Arrow [{{0,0},v1}],Red ,Arrow [{{0,0},v2}]},PlotRange ->2]
3 ⾏为决策
⾏为决策的意思是⽆⼈车应该采取什么⾏为，例如是变道还是停车。作者将所有可能的⾏为总结为4类：纵向⽅向的模式分别是
Following ，Merging ，Stopping 和Velocity Keeping 。注意这⾥只提到纵向⽅向（也就是纵向坐标维度）。⾄于变道还是不变道则由侧向⽅向决定。侧向⽅向可以通过采样不同的侧向位置实现。此外，
受到5次多项式本⾝表⽰能⼒的限制，规划出来的轨迹只能实现有限的⾏为，例如可以实现变道，但是⽆法⽣成连续的变道再回到原来车道的⼀段轨迹。
根据末端状态，前三种Following ，Merging ，Stopping 可以归为⼀类，Velocity Keeping 单独归为⼀类。为什么呢，因为前三种都需要限制轨迹末端的位置、速度和加速度，⽽Velocity Keeping 只需要限制末端的速度和加速度，对位置不限制。从多项式函数的⾓度看，前三种⽤5次多项式表⽰，后⼀种⽤的是4次多项式。所以在代码中定义了following_merging_stoping 函数统⼀处理。对于侧向⽅向不作分类，总是5次多项式，因为我们⼀定要约束侧向⽅向的末端位置在车道某处，⽽不希望⽆⼈车⾃由发挥。在实际应⽤中，笔者认为Merging 遇到的情况不多，所以暂时不考虑，本⽂也不过多讨论。对于Following 和Stopping 来说，我们都要明确知道前车的状态（主要是位置和速度）。作者只考虑了Following ，Merging 和Velocity Keeping 。Merging 出现最少，只出现了2次。Following 和Velocity Keeping 分别都出现了5次。 下⾯重点分析Following 和Stopping 。
Stopping ：在原程序中，作者没有考虑Stopping 这种情况（所以TrajectoryGenerator.h 中的stopping 函数始终没被调⽤过）。当然这也是有道理的，因为⽆⼈车在正常执⾏任务时，我们希望Stopping 最好不要出现，除⾮遇到特殊情况不得不停车，这时⽆⼈车可能⽆路可⾛了。 Following ：跟车时，我们最关⼼的决策信息是被跟随者的位置和速度。⾸先，被跟随者必须在⽆⼈车的前⽅，如果被跟随者与⽆⼈车齐头并进或者在⽆⼈车后⾯，那显然⽆⼈车不能跟随它。如果只考虑速度还不够，
如果被跟随者的速度为0甚⾄为负，那么⽆⼈车显然应该避让，不能再跟随。所以跟随模式的条件可以设计成下表这样的形式。其中，是被跟者的纵向位置，是⽆⼈车的纵向位置，是被跟者
亲子课教案
的纵向速度。只有当间距⾜够⼤和速度⾜够快同时满⾜，⽆⼈车才会考虑跟车。 BehaviorModule.h ⽂件，其中Search_Center 函数⽤于搜索⽆⼈车⾃⼰所在车道的情况，并以此进⾏决策，决定是采⽤FOLLOWING 还是VELOCITY_KEEPING 亦或MERGING 。决策过程的判断逻辑是如下：
⾸先，查看⽬标车道Goal_Lane 的前后有没有其它车辆，分成两种情况：
1 前⾯有车，此时根据后⾯有没有车再分成两种情况：
1.1 后⾯有车，此时计算前后车的间距，称为⾃由空间free_space 。如果⾃由空间⽐Max_Tight_M
erge_Offt ⼤，再看与汽车的距离nf ，如果nf ⼩于Consider_Distance ，则采⽤FOLLOWING 模式， 采⽤前车的速度跟车⾏驶。如果nf 不⼩于Consider_Distance ，则采⽤VELOCITY_KEEPING 模式，以v_ref 参考速度速度保持。如果⾃由空间不⽐Max_Tight_Merge_Offt ⼤，⽽是只
大门设计图⽐Min_Tight_Merge_Offt ⼤，说明⾃由空间不宽裕，此时采⽤MERGING 模式，保持在前后车中点位置⾏驶。如果⾃由空间⽐Min_Tight_Merge_Offt 还⼩，此时不管，该情况应该是危险的特殊情况，应该单独考虑。
1.2 后⾯没车，此时如果⽆⼈车与前车的距离⼩于Consider_Distance ，则采⽤FOLLOWING 模式， 采⽤前车的速度跟车⾏驶。如果与前车的距离⼤于Consider_Distance ，则采⽤VELOCITY_KEEPING 模式，以恒定的v_ref 参考速度。
前⾯有车时还做了⼀件事，就是看前车的车速，如果车速⼩于v_ref ，说明前车太墨迹开得太慢，那么就要考虑变道超车，将变道超车的标志位attempt_switch 置为true 。
2 前⾯没车，以v_ref 参考速度速度保持。此时不管后⾯有没有车，后⾯有车也不理它。
乍⼀看，上⾯的模式好像很多，有些复杂，但是实际上这些条件判断都是排他性的，所以最后只能有⼀种模式被选中，不会同时选择多个。最后把搜索模式和attempt_switch 返回。
如果后⽅来车⽐较快，⽆⼈车应该怎么反应？
4 代码分析s s c s e v c
