任务5.6 时空轨迹规划算法-课件

主讲老师：李锋时空轨迹规划算法项目5：智能驾驶AP应用开发与算法简介任务5.6《车载计算平台技术与应用》目录任务导入AssignmentINTRO学习目标LearningobjectivesCONTENTS0102知识准备Backgroundknowledge03课堂总结Classroomsummary0504课堂小测Classroomquizz思政专栏ApolloOpenSpace中的TDR-OBCA算法采用基于对偶问题的求解方法，通过将原优化问题转化为对偶问题，算法能够更有效地处理复杂的约束条件，如车辆动力学约束和避障约束，从而找到更优的解。该算法在规划轨迹时，不仅考虑车辆的空间位置，还同时考虑时间因素，如车速、加速度等，实现了时间空间联合优化，从而能够生成更加平滑、安全的轨迹；TDR-OBCA算法在规划轨迹时，充分考虑了车辆的动力学约束，提高自动驾驶车辆的行驶效率和安全性。TDR-OBCA算法框架思政专栏ApolloOpenSpace中的TDR-OBCA算法基于对偶问题的求解方法，并考虑了车辆动力学约束，实现了时间空间联合的轨迹规划。在自动驾驶技术中，如何平衡轨迹规划算法的创新性与实际应用的安全性？轨迹规划PART01任务导入AssignmentINTRO任务导入在现代人工智能与机器人技术的迅猛发展中，时空轨迹规划算法成为自主导航与路径优化的关键。如何在复杂动态环境中实现高效、安全且智能的轨迹规划，已成为学术界与工业界共同关注的前沿课题。轨迹规划需求PART02学习目标Learningobjectives学习目标素质目标通过SL空间规划的学习，激发并锻炼学生对陌生事物探索的能力；了解轨迹规划的挑战，激发学生创新精神。能力目标能正确排列SL和ST规划的基础流程[A67]；能结合场景讲解轨迹规划算法的算法逻辑[A68]。知识目标能理解轨迹规划的概念[K82]；能描述轨迹规划算法的原理[K83]；能列举常用的轨迹规划算法[K84]。PART03知识准备Backgroundknowledge一、轨迹规划原理整轨迹规划算法是智能驾驶技术的核心算法之一，规划算法结合障碍物、车辆、定位、地图等信息，生成安全平滑的时空轨迹，保证智能驾驶车辆安全舒适地运行。自动驾驶中的轨迹规划是指车辆在时间序列下分布的一系列空间位置点的集合，与路径规划的核心区别在于，轨迹规划具有时间属性，其规定了车辆在每一个时刻点到达的一个位置和速度信息，所以轨迹也称为时空的轨迹。轨迹时空划分一、轨迹规划原理智能驾驶中的轨迹规划算法核心是解决车辆处于交通流中，需要考虑各种交通参与者，包括车辆、行人、自行车等，同时要区别他们不同的运动状态、不同的类型、不同的行为，从而计算出一条安全舒适的行驶轨迹。1.开放道路轨迹规划需求分析轨迹规划需求一、轨迹规划原理MPC是基于模型预测控制的一种规划算法，基于最优控制的思想，可以认为是从控制的角度出发的算法。MPC的控制算法一般是通过建立运动误差和控制量的状态转移方程，也就是运动模型的描述，通过求解最小化的运动误差，计算最优的控制量，这体现在车辆的横向控制和纵向控制上。2.常见轨迹规划算法（1）MPC（模型预测控制）MPC控制一、轨迹规划原理横向：基于车辆的动力学模型，考虑障碍物和道路约束，建立如图红色曲线描述的道路边界；建立车辆最小化横向误差，即如图中每条可行轨迹和黄色参考轨迹的误差；最小化目标=横向参考偏差+加速度及转向控制量输入；计算出最优的横向位置轨迹，即如图中的最优轨迹。2.常见轨迹规划算法（1）MPC（模型预测控制）横向控制一、轨迹规划原理其具体流程，如图所示，最终目的就是求解出其中一条与参考线最接近，同时满足控制量最小的一条轨迹。2.常见轨迹规划算法（1）MPC（模型预测控制）车辆模型模型预测多条轨迹（解空间）二次优化最小化最优轨迹MPC横向控制一、轨迹规划原理纵向：在完成横向控制后，接下来就是纵向控制，考虑匀加速运动规划/基于能量最小化进行一维的速度规划。2.常见轨迹规划算法（1）MPC（模型预测控制）车辆模型匀加速/能量最小速度规划速度信息横向控制一、轨迹规划原理LatticePlanner是一种基于采样的规划算法，该规划算法一般分为六个流程：2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）起始结束状态横向L(s)采样纵向s(t)采样横纵向融合（l<->s）计算轨迹代价最优轨迹LatticePlanner规划算法流程一、轨迹规划原理步骤①和②会将整个道路划分为一个网格，此网格是不均匀的，距离车辆近的网格更密，距离车辆远的网格更加稀疏，每个网格点可以作为一个采样的状态，作为一个末状态。结合起始状态和任何一个状态的末状态，可以拟合出一条多次曲线作为一个横向的路径。2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）划分道路网络一、轨迹规划原理步骤③对纵向进行速度的规划，也就是建立ST图，根据巡航、跟车、超车、停车等不同场景进行不同速度的采样。2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）ST图一、轨迹规划原理步骤④将横向路径和采样的速度进行融合，最总得到一个既包含路径信息又包含速度信息的一条轨迹。2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）s和l的结果插值得到（l,s）配对横纵向融合一、轨迹规划原理步骤⑤对每条轨迹进行评价，评价的维度包括：到达目标点的偏离程度、横向运动最小、是否避免碰撞、纵向加速度、横向加速度、离心力等等。2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）到达目标点横向运动小纵向加速度横向加速度碰撞避免离心力计算轨迹代价一、轨迹规划原理步骤⑥最终选取出评价最高的最优轨迹。2.常见轨迹规划算法（2）LatticePlanner（格点规划控制）起始结束状态横向L(s)采样纵向s(t)采样横纵向融合（l<->s）计算轨迹代价最优轨迹最优轨迹一、轨迹规划原理EMPlanner是一种基于最优化思想的轨迹规划算法，该算法主要分为两个步骤：E-Step（计算期望求解空间）和M-Step（求解空间最优化）。2.常见轨迹规划算法（3）EMPlanner（期望和最大化规划控制）EMPlannerE-StepM-StepEMPlanner一、轨迹规划原理其算法流程是通过预处理后，得到一条参考线和抽象化的环境信息，对环境信息进行横向处理，也就是SL的规划，接着在SL规划的基础上继续进行ST的处理，也就是速度决策以及速度规划，最终得到一条合成的轨迹。2.常见轨迹规划算法（3）EMPlanner（期望和最大化规划控制）EMPlanner处理流程一、轨迹规划原理业界中更常用的方法是LatticePlanner和EMPlanner。在一般场景下，LatticePlanner的计算速度优于EMPlanner，但随着场景复杂度的提升，LatticePlanner的计算耗时大大增加。2.常见轨迹规划算法（4）算法比较因此EMPlanner更适合城市道路等复杂的场景，LatticePlanner则更适合高速道路场景。时间复杂度比较一、轨迹规划原理轨迹规划算法均有横纵向解耦的操作，基于车道中心线去计算参考线，将车辆运动状态分解为空（横向SL）和时（纵向ST）的规划。3.轨迹时空划分原理轨迹时空划分流程一、轨迹规划原理自车（绿车）在世界坐标中具有x、y、θ（航向角）、v（速度）、a（加速度）的运动状态，可将其转换为横向和纵向的位移的映射关系。3.轨迹时空划分原理位移的映射关系一、轨迹规划原理经过转换后将所有的车辆映射到一个直角坐标系下，也就是S-L空间，从而得到自车的可行驶空间，并在可行驶空间内进行一系列的横向规划，进而得到横向L和纵向S之间的关系。3.轨迹时空划分原理S-L空间一、轨迹规划原理下一步再进行速度的规划，只考虑蓝车（动态障碍物），将动态障碍物的轨迹投影到S-T空间上，从而优化出一条不和ST障碍物相碰撞的一条最优ST曲线，实际上得到是纵向S和时间T的关系。3.轨迹时空划分原理S-T空间一、轨迹规划原理该跟车场景中，蓝车的轨迹与自车（绿车）的轨迹有重叠区域，根据该区域所对应的时间和范围，可以得到ST的障碍物的占据空间；而黄车并不回存在和自车的重叠区域。所以根据ST空间可以拟定出最优的行车轨迹（保证和蓝车保持一定距离和相对速度进行跟车）。3.轨迹时空划分原理（1）跟车场景跟车场景的轨迹时空划分一、轨迹规划原理该汇入场景中，自车（绿车)需要汇入到两个交通参与车辆中（蓝车与橙车），两车都与自车的可行驶区域有交集，最终得到S-T空间可得到一条刚好可以汇入到两车之间的轨迹。3.轨迹时空划分原理（2）汇入场景汇入场景的轨迹时空划分一、轨迹规划原理该路口场景中，自车（绿车）跟随前方蓝车，同时前方有一个反方向直行的橙车也与自车的可行驶区域相交，从S-T空间可知，自车与蓝车在T为0的时刻开始相交，自车与橙车则是在未来的某一时刻开始相交，从而可得有两种方式（避让和抢行）进行通过，最终根据驾驶习惯或者其他环境因素，得出最优的驾驶轨迹。3.轨迹时空划分原理（3）路口场景路口场景的轨迹时空划分二、SL空间路径规划SL规划即规划车辆的横向运动，在前述时空轨迹划分的基础下，生成SL空间轨迹点。SL轨迹规划的大致流程可分为三大步骤：可行驶空间划分、参考生成、SL二次优化。可行驶空间划分参考生成SL二次优化SL轨迹规划流程二、SL空间路径规划根据参考线、障碍物预测、障碍物决策、自车信息、上一帧的轨迹等信息生成可行驶空间。针对车辆横纵向与障碍物及道路边界不发生碰撞的区域，划分出图中可行驶空间。1.可行驶空间划分可行驶空间划分二、SL空间路径规划结合可行驶空间，在空间里面得到一个运动的参考线。但该参考线不是直接能够运动的路径，只是一个参考，参考的生成一般分为决策选择和动态规划两种方法。2.参考生成参考生成二、SL空间路径规划方法一：决策选择。2.参考生成结束状态可根据绕障或变道的决策选择相应的目标车道位置参考线生成结合变道时间和目标车道确定变道点，进而生成参考线二、SL空间路径规划方法二：动态规划。2.参考生成结束状态可根据绕障或变道的决策选择相应的目标车道位置道路网格化处理代价评估根据障碍物距离，横向位移等条件计算代价，动态规划选择最优二、SL空间路径规划根据生成的多条参考线，结合一系列的曲率、运动状态的约束，得到一个平滑、舒适的一个横向的可行驶区域。3.SL二次优化其二次优化的目标一般包括：①ref代价：横向的优化结果和参考值尽量小；②速度代价：最小化横向运动的速度，同时最小化横向运动的加速度和加加速度。SL二次优化三、ST空间速度规划SL空间路径规划提供了一条车辆横纵向运动的空间路径，车辆需要沿着该路径以一定的速度进行运动，这就需要在路径上进行ST的速度规划，也就是轨迹时空划分的第二步，ST的空间速度规划。ST速度规划的流程和SL空间规划有很多类似之处，其中也可分为三个步骤：S-T投影、参考生成、ST二次优化。S-T投影参考生成ST二次优化ST轨迹规划流程三、ST空间路径规划ST投影以SL的可行驶区域、障碍物预测、障碍物决策、自车信息、限速信息等进行投影到ST图上。S-T求解空间上的计算主要包含：限速计算和障碍物投影计算。1.ST投影-计算求解空间S-T投影三、ST空间路径规划限速计算：通过地图上的每条车道线的限速，同时根据弯道的曲率，结合向心加速度的约束，计算出不同曲率下的限速约束，结合不同位置的限速约束，可得一条速度约束的曲线。障碍物投影计算：基于运动障碍物和自车行驶空间的重叠区域，计算出重叠区域的起始位置S和对应的时间T，从而依次得到对应ST的角点，构造出ST的障碍区域，如图。1.ST投影-计算求解空间投影计算三、ST空间路径规划参考的生成除了在可行驶的ST空间中寻找速度曲线外，还需要结合纵向策略进行决策，如图中上方的激进策略，加速超车通过，或下方的保守策略，减速跟车通过。通常会将ST空间进行三格化的处理，建立一系列的代价，比如速度代价、加速度代价、障碍物距离代价等等，结合代价，搜索出代价最小的轨迹，作为是否加速超车或减速跟车的决策。2.参考生成参考生成三、ST空间路径规划二次优化在参考基础上进行二次优化的平滑和约束，得到一条最优的速度轨迹。二次优化的最优化目标包括：接近参考线、向心加速度最小、纵向加速度最小、加速度变化率最小。3.ST二次优化二次优化四、轨迹规划的挑战道路中没有车道线，选择规划参考困难，难以生成参考线1.SL横向规划的挑战（1）没有车道线没有车道线四、轨迹规划的挑战SL规划对障碍物的行为和预测有较强的依赖，如果对障碍物行为的预测不够稳定，那么对SL规划的稳定性也有很大的影响。所以障碍物本身行为发生变化或者感知不够稳定带来变化的情况下，会导致SL规划产生不稳定性。1.SL横向规划的挑战（2）障碍物行为变化障碍物决策四、轨迹规划的挑战SL规划是一种基于数值优化的方法，在设定的约束条件基础上，可能会导致在某些场景下的求解空间被压缩使得求解失败。因此，如何在求解失败后进行失败保护，或者避免更多的求解失败也是目标规划算法面临的一大挑战。1.SL横向规划的挑战（3）在约束条件下无解数值优化四、轨迹规划的挑战在生成参考线后，如何确定是以激进还是保守的策略行进，如何保证策略的安全，是ST纵向规划中比较严峻的挑战。2.ST纵向规划的挑战（1）交通参与者间的博弈交通策略的决策四、轨迹规划的挑战ST在做投影的时候强依赖于预测轨迹和自车的可行驶区域的计算，诸如视野盲区（鬼探头）、预测轨迹不稳定等因素也是速度规划面临的挑战。2.ST纵向规划的挑战（2）感知和预测障碍物困难视野盲区四、轨迹规划的挑战在做纵向规划的同时，还需要考虑横向规划，比如前车紧急的切入，如果按照变道的纵向规划是难以避免与前车发生碰撞的，单纯的纵向规划是来不及处理和反应的，这时候就需要横向规划进行躲避协调。2.ST纵向规划的挑战（3）与横向规