1 背景
早期的不少规划算法都是横纵分离的（比如Apollo），先求解path之后，依赖path的结果再进行speed的求解。这种横纵解耦的规划方式具有以下特点：
- 相对较为简单，计算量通常较小，容易实现实时性要求。
- 但是由于分别规划横向和纵向运动，可能会忽略两者之间的耦合关系，导致规划结果不够准确。
- 在复杂场景下，可能无法很好地适应车辆等其它动态障碍物的变化。
随着技术的发展和性能的要求，横纵耦合算法的弊端越来越明显，而时空联合规划逐渐浮出水面。本篇博客主要介绍时空联合规划的一些知识和特点。
2 时空联合规划
“时空联合规划” 算法打破了业界常用的思维定势，同时考虑空间和时间来规划轨迹，在三维中同时对行驶路径和速度求解，而不是先单独求解路径，在路径基础上求解速度后，再形成最终的轨迹。
其实早在之前的博客中，笔者就介绍过时空联合的方法（此前讲到的是基于车道坐标系SLT三维求解的方法，同理也可使用笛卡尔坐标系下XYT三维求解的方法），有兴趣的读者朋友可以参考一些细节知识：
《自动驾驶---Motion Planning之构建SLT Driving Corridor》
《自动驾驶---Motion Planning之STSC轨迹优化》
2.1 定义
时空联合规划算法是一种在自动驾驶领域中用于同时考虑空间和时间因素来规划车辆行驶轨迹的算法。这种算法相较于传统的时空分离规划方法（即先规划路径再规划速度），具有更强的协调路径和速度的能力，能够更准确地处理复杂的动态交互场景。
2.2 算法背景
在自动驾驶研发或者验收标准中，安全性和舒适性是两大核心要求（还有接管率，这里不做特殊说明）。安全性要求轨迹与所有障碍物进行碰撞检测，而碰撞的定义是在同一时间占据同一空间位置，因此只用路径或速度曲线都无法准确检测碰撞。同时，舒适性要求从多角度全方位地评估运动轨迹的平滑性，这同样需要同时考虑路径和速度（分别体现车辆在横向方向盘的表现以及纵向速度是否合理）。传统的时空分离规划方法在处理这些问题时存在局限性，因此时空联合规划算法应运而生。
2.3 算法原理与特点
- 原理介绍如下：
-
时空联合规划算法将路径和速度曲线同时作为优化问题的变量，通过求解一个复杂的优化问题来找到既安全又舒适的最优轨迹。
-
该算法在规划过程中，同时考虑车辆在空间中的位置变化（路径）和时间上的速度变化（速度曲线），从而得到一个综合最优的行驶轨迹。
-
- 特点介绍如下：
-
同时性：同时求解路径和速度曲线，避免了先规划路径再规划速度时可能出现的不一致性。
-
协调性：能够更好地协调路径和速度的关系，使得车辆在行驶过程中能够更流畅地应对各种动态变化。
-
适应性：能够处理复杂的动态交互场景，如避让行人、车辆等障碍物。
-
2.4 算法实现方法
笔者通过下面几个步骤大致说明整个算法的实现框架。
（1）时空联合决策
在之前传统方案中下，决策模块尚未被模型替代，而是基于动态障碍物的单模或者多模预测轨迹（带时间t的属性）进行推演得到决策，如下图所示。给了障碍物决策标签，那么轨迹的求解就不会陷入局部最优，就会是一个凸优化问题。
（2）时空联合优化
首先需要弄清楚的是时空联合优化的对象，目前笔者所了解到的各大车企的端到端方案，包括华为，特斯拉等，即使是模型生成的轨迹，对于控制层也是无法直接使用的，还是需要一层平滑或者优化，因此优化的步骤在当前阶段还是非常重要的。这里的优化思路有两种：
- 第一种其实笔者在之前的博客 《自动驾驶---Motion Planning之构建SLT Driving Corridor》中也提到过，前向预测模块会生成初始的种子，即初始解。（种子生成：根据车辆的前向预测模块生成一系列离散的点作为种子）
- 第二种就是带时间 t 的参考轨迹，该参考轨迹也是由决策模块推演得到（必须符合车辆运动学以及动力学的约束）。
其实上述两种方案本质是相同的，目的都是为了生成基于时间 t 的初始解（参考轨迹）。
接下来便是生成基于初始解轨迹的可行使空间，或者叫约束空间。
不管是SLT还是XYT都会得到自车的可行使空间，该空间会作为优化求解约束的一部分，称之为SLT Corridor或者XYT Corridor，形式如下所示：
最后就是构建时空联合优化问题，包括横纵向的安全性和舒适性，这里也可参考前面提到的两篇博客。在求解器部分，很多公司测试下来发现开源的求解器，比如SQP，并不能满足求解的需求，因此自研了求解器，比如华为，轻舟等公司。
求解最优的时空轨迹的问题，其实是可以表述为一个以轨迹对时间的函数为变量的数值优化（numerical optimization）问题。问题表述的过程有若干设计需要我们做，而这些设计的合理与否会直接影响优化算法的效果。比如，表示轨迹有多种方式可以选用，前面提到的Frenet坐标系下的l(t)和s(t)就可以完整地描述轨迹，直接在笛卡尔坐标系（Cartesian frame）下描述车辆的位置（x,y）对时间的函数也可以，这两个坐标系各有鲜明的优缺点。
还有一个重要的设计是如何描述碰撞安全，比如避免与静态障碍物（路沿，护栏等）和动态障碍物（车辆，行人等）的碰撞；用硬约束（constraint）或者软约束（penalty）都是可行的，使用得当的话也都能保证安全性。
在上面这些设计做好之后，那么就可以选择合适的优化求解算法了，比如QP二次规划问题（quadratic programming）（在前面规控算法的文章中笔者也强调过很多次了）。不过QP适用于目标函数比较接近理想的二次形式，约束条件接近理想的线性形式的情况下；当问题比较复杂，高度非线性的时候，QP解得的结果经常会不尽如人意。以笛卡尔坐标系的轨迹表示形式为例讨论，轨迹表示为空间位置r(t) = (x(t), y(t))。在转弯时，要想平衡路径的曲率和过弯的速度，可以把横向加速度（lateral acceleration）作为体感的一个指标来约束，而横向加速度与车速和曲率都有关，写成自变量（坐标）的函数是比较复杂的，在QP中用一个二次形式来近似会有很大误差。这里又可以用得上迭代法了这个范式了：对于不适合用QP直接解的高度非线性问题，可以尝试SQP（sequential quadratic programming）这个方法，其核心思想就是重复QP求解---二次形式近似这两个步骤，迭代至收敛。由于需要迭代，在实际使用中有些提高计算效率的技巧是必要的，比如在QP中使用稀疏矩阵（sparse matrix），运用预条件子（matrix preconditioning）等。另外，在优化问题的数学描述中，也有很大的提高空间，有些描述车辆运动学或者控制极限的约束条件，如果能通过数学变形的方法内建到优化问题描述中，而不是以显式的约束的形式出现，就可以大大提高求解的效率和稳定性。
当然，笔者曾经介绍的这篇博客《自动驾驶---Motion Planning之STSC轨迹优化》中，思路并不是直接求解（
, 
, 
......）等状态量，而是通过优化方法得到多段Bezier曲线的控制点信息，最终求解出自车的轨迹。
2.5 应用实例与效果
目前，已有一些自动驾驶公司和研究机构开始尝试应用时空联合规划算法来提升自动驾驶系统的性能。例如，轻舟自研的时空联合规划算法能够同时在xyt（即平面和时间）三个维度的空间中直接求解最优轨迹，使得车辆在行驶时能够比普通驾驶员甚至老司机“开得更好”。这种算法在面临动态障碍物的交互时能够提前把握最好的时机选出最佳行车轨迹更流畅地完成车辆间的博弈并且不会出现反复急刹的情况。
时空联合典型应用场景包括：
-
鬼探头：通过识别路线上被遮挡的感知盲区，系统可以提前减速，防止鬼探头。当鬼探头出现的时候，时空联合规划可以让自动驾驶汽车不拘泥于固定的路径，可以实时计算出来一个最佳的避让轨迹，更好地保证安全性。
-
逆行：城市道路中难免会遇到自行车电动车违规逆行场景。轻舟智航的时空联合规划技术能让自动驾驶汽车像老司机一样找到最合适的时机提前避开逆行车的路线，保证安全的同时，又能让乘客感觉像老司机驾驶一样非常舒适。它比其他算法有更好的体感。
-
变道：时空联合算法能通过计算选择最佳变道时机，做到不依赖预设规则，就能灵活的获得最佳的变道效果。
3 总结
在自动驾驶规划模块的最后一个环节，即轨迹优化求解是非常重要的，在量产当中也会是问题体现比较多的一个环节，因为车辆最终的表现总是集中在横向方向盘或者纵向刹车上。因此不管是大模型还是传统规控，掌握好这一节的内容对于想从事规控的读者朋友是非常有帮助的。