长沙市师德师风建设网站,网站建设j基本步骤,青县网站建设公司,word网站超链接怎么做动作规划#xff08;Motion Control#xff09;在⾃动驾驶汽⻋规划模块的最底层#xff0c;它负责根据当前配置和⽬标配置⽣成⼀序列的动作#xff0c;本⽂介绍⼀种基于Frenet坐标系的优化轨迹动作规划⽅法#xff0c;该⽅法在⾼速情况下的ACC辅助驾驶和⽆⼈驾驶都具有较强…动作规划Motion Control在⾃动驾驶汽⻋规划模块的最底层它负责根据当前配置和⽬标配置⽣成⼀序列的动作本⽂介绍⼀种基于Frenet坐标系的优化轨迹动作规划⽅法该⽅法在⾼速情况下的ACC辅助驾驶和⽆⼈驾驶都具有较强的实⽤性是⽬前普遍采⽤的⼀种动作规划算法。基于Frenet坐标系的动作规划⽅法是由宝⻢的Moritz Werling提出的为了简便我们在后⽂中也会使⽤Werling⽅法简称。在讨论基于Frenet坐标系的动作规划⽅法之前⾸先定义什么是最优的动作序列对于横向控制⽽⾔假定由于⻋辆因为之前躲避障碍物或者变道或者其他制动原因⽽偏离了期望的⻋道线那么此时最优的动作序列或者说轨迹是在⻋辆制动能⼒的限制下相对最安全舒适简单和⾼效的轨迹。同样的纵向的最优轨迹也可以这么定义如果⻋辆此时过快或者太接近前⽅⻋辆那么就必须做减速具体什么是“舒适⽽⼜简单的”减速呢我们可以使⽤ Jerk 这个物理量来描述Jerk即加速度的变化率也即加加速度通常来说过⾼的加加速度会引起乘坐者的不适所以从乘坐舒适性⽽⾔应当优化Jerk这个量同时引⼊轨迹的制动周期 T , 即⼀个制动的操作时间1为什么使⽤Frenet坐标系在Frenet坐标系中使⽤道路的中⼼线作为参考线使⽤参考线的切线向量t和法线向量n建⽴⼀个坐标系如下图的右图所⽰这个坐标系即为Frenet坐标系它以⻋辆⾃⾝为原点坐标轴相互垂直分为s⽅向即沿着参考线的⽅向通常被称为纵向Longitudinal和d⽅向即参考线当前的法向被称为横向Lateral相⽐于笛卡尔坐标系下图的左图Frenet坐标系明显地简化了问题因为在公路⾏驶中我们总是能够简单的找到道路的参考线即道路的中⼼线那么基于参考线的位置的表⽰就可以简单的使⽤纵向距离即沿着道路⽅向的距离和横向距离即偏离参考线的距离来描述同样的两个⽅向的速度的计算也相对简单。那么现在动作规划问题中的配置空间就⼀共有三个维度是规划出来的每⼀个动作的时间点轨迹和路径的本质区别就是轨迹考虑了时间这⼀维度。Werling的动作规划⽅法⼀个很关键的理念就是将动作规划这⼀⾼维度的优化问题分割成横向和纵向两个⽅向上的彼此独⽴的优化问题具体来看下⾯的图假设⾏为规划层要求当前⻋辆在越过虚线完成⼀次变道即⻋辆在横向上需要完成⼀个以及纵向上完成⼀个 的移动则可以将s和d分别表⽰为关于t的函数上图右图那么ds关于时间t的最优轨迹应该选择哪⼀条呢通过这种转换原来的动作规划问题被分割成了两个独立的优化问题对于横向和纵向的轨迹优化我们选取损失函数C将使得C最⼩的轨迹作为最终规划的动作序列。而Werling方法中损失函数的定义则与我们前⾯提到的加加速度 Jerk 相关。2Jerk最⼩化和5次轨迹多项式求解由于我们将轨迹优化问题分割成了s和d两个⽅向所以Jerk最⼩化可以分别从横向和纵向进⾏令p为我们考量的配置即s或d加加速度关于配置p在时间段内累计的Jerk的表达式为现在我们的任务是找出能够使得Takahashi的⽂章motion control for AGV in positioning中已经证明任何Jerk最优化问题中的解都可以使⽤⼀个5次多项式来表⽰要解这个⽅程组需要⼀些初始配置和⽬标配置以横向路径规划为例初始配置为即时刻⻋辆的横向偏移横向速度和横向加速度为即可得⽅程组为了区分横向和纵向我们使⽤来分别表⽰d和s⽅向的多项式系数同理根据横向的⽬标配置可得⽅程组该⽅程的解可以通过Python的Numpy中的 np.linalg.solve简单求得。⾄此我们在给定任意的初始配置⽬标配置以及制动时间T的情况下可以求的对应的d⽅向关于时间t的五次多项式的系数同理可以使⽤相同的⽅法来求解纵向即s⽅向的五次多项式系数。那么问题来了我们如何去确定最优的轨迹呢Werling⽅法的思路是通过⼀组⽬标配置来求得轨迹的备选集合然后在备选集合中基于Jerk最⼩化的原则选择最优轨迹 我们仍然以d⽅向的优化轨迹为例讲解我们可以取如下⽬标配置集合来计算出⼀组备选的多项式集合对于优化问题⽽⾔我们实际上希望⻋辆最终沿着参考线道路中⼼线平⾏的⽅向⾏驶所以我们令那么⽬标配置只涉及两个变量的组合⽽这两个变量在⽆⼈驾驶的应⽤场景中实际上是受限的我们可以通过定义来约束⽬标配置的取值范围通过 和 来限制采样密度从⽽在每⼀个制动周期获得⼀个有限的备选轨迹集合如下图所⽰要在备选集合中选择最优轨迹即上图中的绿⾊轨迹我们需要设计损失函数对于不同的场景损失函数也不相同以横向轨迹为例在较⾼速度的情况下损失函数为该损失函数包含三个惩罚项其中是这三个惩罚项的系数它们的⽐值⼤⼩决定了我们的损失函数更加注重哪⼀个⽅⾯的优化由此我们可以算出所有备选轨迹的损失取损失最⼩的备选轨迹作为我们最终的横向轨迹。值得注意的是以上的损失函数仅适⽤于相对⾼速度的场景在极端低速的情况下⻋辆的制动能⼒是不完整的我们不再将d表⽰为关于时间t的五次多项式损失函数也会略有不同但是这种基于有限采样轨迹通过优化损失函数搜索最优轨迹的⽅法仍然是⼀样的在此不再赘述。讨论完横向的轨迹优化问题我们再来看看纵向的轨迹优化在不同的场景下纵向轨迹的优化的损失函数也各不相同Werling⽅法中将纵向轨迹的优化场景⼤致分成如下三类跟⻋汇流和停⻋⻋速保持在本⽂中我们详细了解⻋速保持场景下的纵向轨迹优化在⾼速公路等应⽤场景中⽬标配置中并不需要考虑⽬标位置即 所以在该场景下⽬标配置仍然是 ⽬标配置变成了 损失函数为其中 是我们想要保持的纵向速度第三个惩罚项的引⼊实际上是为了让⽬标配置中的纵向速度尽可能接近设定速度该情景下的⽬标配置集为即优化过程中的可变参数为同样也可以通过设置来设置轨迹采样的密度从⽽获得⼀个有限的纵向轨迹集合其中绿线即为纵向最优轨迹。以上我们分别讨论了横向和纵向的最优轨迹搜索⽅法在应⽤中我们将两个⽅向的损失函数合并为⼀个即这样我们就可以通过最⼩化得到优化轨迹集合我们不能得到“最优”的轨迹多项式参数还可以得到“次优”“次次优”轨迹等等。3事故避免Collision Avoiding显然上⾯的轨迹优化损失函数中并没有包含关于障碍物躲避的相关惩罚并且我们的损失函数中也没有包含最⼤速度最⼤加速度和最⼤曲率等制动限制也就是说我们的优化轨迹集合并没有考虑障碍物规避和制动限制因素不将障碍物避免加⼊到损失函数中的⼀个重要的原因在于碰撞惩罚项的引⼊将代⼊⼤量需要⼈⼯调整的参数即权重是的损失函数的设计变得复杂 Werling⽅法将这些因素的考量独⽴出来在完成优化轨迹以后进⾏。具体来说我们会在完成所有备选轨迹的损失计算以后进⾏⼀次轨迹检查过滤掉不符合制动限制的可能碰撞障碍物的轨迹检查内容包括s⽅向上的速度是否超过设定的最⼤限速s⽅向的加速度是否超过设定的最⼤加速度轨迹的曲率是否超过最⼤曲率轨迹是否会引起碰撞事故通常来说障碍物规避⼜和⽬标⾏为预测等有关联本⾝即使⼀个复杂的课题⾼级⾃动驾驶系统通常具备对⽬标⾏为的预测能⼒从⽽确定轨迹是否会发⽣事故。在本节中我们关注的重点是⾃动驾驶汽Ctotal klat Cd k C lonsC total⻋的动作规划故后⾯的实例仅涉及静态障碍物的规避和动作规划。4基于Frenet优化轨迹的⾃动驾驶汽⻋动作规划实例由于planner的代码篇幅过⻓本实例完整代码请⻅⽂末链接在此仅讲解算法核⼼代码内容。和之前⼀样我们仍然使⽤Python来实现该动作规划算法。⾸先我们⽣成要追踪的参考线以及静态障碍物参考线的⽣成只要使⽤了我们上⼀节提到的⽴⽅样条插值代码如下⽣成如下参考路径以及障碍物其中红线就是我们的全局路径蓝点为障碍物。定义⼀些参数使⽤基于Frenet的优化轨迹⽅法⽣成⼀系列横向和纵向的轨迹并且计算每条轨迹对应的损失其中⼀个重要的类是五次多项式类其定义如下这⾥的五次多项式的系数的求解过程和我们前⾯的理论讲解是⼀样的只不过我们使⽤Numpy中的 np.linalg.solve(A, b)⽅法将矩阵解了出来。最后我们来看⼀下障碍物规避是如何实现的由于我们将障碍物规避问题都简化为静态了所以在这⾥我们只简单地计算了所有规划点到障碍物的距离⼀句距离预计是否会发⽣碰撞来看看完整的优化轨迹检查函数由此可以看出最终的优化轨迹的选择并不单纯基于最⼩损失函数轨迹检查还会过滤掉⼀些轨迹所以使⽤基于Frenet的优化轨迹来做⾃动驾驶汽⻋的动作规划通常能够找到有限集的最优解当最优解⽆法通过检查是⾃会采⽤“次优解”甚⾄更加“次优的”解。最后我们来看⼀下完整的动作规划效果后台回复“21”领取完整代码~
