QBlog 03 |运动轨迹仅仅是2D的?——论运动规划的维度
运动规划的维度
运动规划是自动驾驶技术栈中，信息整合的关键一步，负责把上游的孤立、异构、模糊的多方面信息整合成自洽的运动轨迹（trajectory）;好的轨迹需要满足多方面的要求，其中最重要的方面包括安全性（安全）和舒适性（舒适）。对于车辆自动驾驶来说，车辆的控制是限制在地面这个二维世界里（严格来讲，地面空间是二维流形，即地面空间与image ²局部同胚（local homeomorphism）。但轨迹并不在这个空间中，因为轨迹并不仅是简单的一个瞬时的状态，也需要考虑时间这一维度。因此，它是一条在二维空间和一维时间组成的三维空间中的曲线。轨迹的时间跨度，称为时域（horizon），通常要达到8-10秒这个范围，才能满足城市复杂场景公开道路产品级自动驾驶的需求。
为什么要在比较长的时间跨度上规划完整的运动，而不能只是简单地规划当前瞬间的动作（油门、刹车、转向）呢？目前很多基于端到端学习的规划方法都是这么做的，毕竟人类司机驾驶时，很多时候也只考虑眼前需要的动作而已。进行长时程的完整规划主要是为了保证未来的规划问题仍然可解。车辆从动力学上来说，惯性很大，从运动学上来说，又属于非完整系统（non-holonomic，即车辆不能瞬间改变朝向），所以如果只考虑当前位姿，是不能作出有前瞻性的驾驶动作的。缺乏前瞻性的驾驶动作虽然目前看来不错，但很容易把自己开进死胡同，造成一段时间之后无解。
比如如果观察到路口的红灯，无人车必须根据当前速度提前刹车才能保证不闯红灯；如果只考虑眼前，会发现在到达路口之前都没有刹车的必要（不会立刻违反交通规则），而到了路口，再刹车就来不及了。这就是为什么这个模块的名字叫做运动规划（motion planning）——规划就是计划未来。端到端学习的方法只输出当前驾驶动作，实际上相当于把控制（控制）也囊括在内了，但通常模型内部还是需要通过辅助损失函数（auxiliary loss）等方式对未来有一个预测和考量的（比如Waymo的ChauffeurNet）。在时空的三维空间中的轨迹，可以完整地描述未来一段时间之内的运动的全貌，因此对这个完整运动的最优解算就可以实现有前瞻性的运动规划。这就是为什么我们要在运动规划的时候考虑未来时间这个维度。
体感
运动规划考虑空间和未来时间这三个维度，就够了吗？要回答这个问题，绕不开体感这个话题。体感也就是物理舒适度（physical comfort），是驾驶舒适度的一个重要方面，反映了车辆运动的不平滑性。体感与舒适度的其他方面一样，比较主观，评估需要从多种指标考虑。常用的指标包括横向和纵向（lateral and longitudinal）的加速度（acceleration）和加加速度（jerk）等。从这些指标可以看出，对于体感来说，运动轨迹对时间的导数是非常重要的，理想的运动轨迹在时间上必须足够平滑。
图：平滑的速度（v-t）曲线。轨迹的平滑性是舒适体感的核心。
上面说到，在运动规划给出的轨迹（对未来运动的规划）里，可以清楚地评估和优化未来运动的平滑性，但过去的运动轨迹则是另一番景象。过去是不可改写的，更重要的是，过去的轨迹是在过去每一个时间点的未来轨迹的初始段积累而成的，类似于控制中的滚动时域优化（后退地平线控制）。过去某时刻对当时的未来的规划，也可以因时间推移，感知预测等信息的更新而更改；比如，两秒钟之前规划的轨迹中预计行驶两秒后到达的位置，可以不同于一秒前规划的轨迹中预计行驶一秒后到达的位置，而二者又皆可不同于当前实际所在位置。因此过去这个时间维度不仅仅是未来时间向相反方向的延伸。尤其是这两个时间维度的接合处，也就是当前时刻，是需要额外细致处理的。体感的效果，必须这些地方每一处都精心优化，才能得以保证。
两个时间维度的可视化
过去历史和未来规划两个时间维度，给可视化增加了难度，因为我们运动规划工程师作为三维空间内的生物，最多只能利用三个维度来做直观的可视化。为了方便在三维空间中可视化，我们这里只讨论一个空间维度，就是纵向（对应时空解耦方法中的速度规划）；实际产品中需要用到的时空联合规划方法要求考虑完整的四维空间，不方便可视化，我们会在后面讨论。这样，我们以某一次运动规划给出的轨迹的未来时间为x轴，以过去作过的历次运动规划的时间点为y轴，以规划出的速度（v）为z轴，可以把从过去到现在所有轨迹组成的轨迹簇画成三维空间中的一个曲面：
图：轨迹簇的三维可视化。横轴为两个时间轴，纵轴为规划的速度。这个例子中的轨迹簇是在方向上是平移不变的，因为规划一直保持恒定状态。图中，如果把历史上每个时间点规划的轨迹的当前时间连起来，得到的就是蓝色的曲线，其右侧为每条历史上的轨迹对未来的规划，过去为（对于当时来说）已不可改写的历史；如果把历史上每个时间点规划的轨迹中的现在时刻（对于越久以前规划的轨迹，这个时间就是越远的未来）连起来，就得到红色的曲线，对应的是同一个。
为了方便观察，我们把可视化的这个三维空间做了一个斜拉，使得过去不同时间规划的轨迹中，对应现实世界的相同时间的点对应在同一个x值上。比如说，在当前时间，规划给出一条10秒钟长的轨迹，其时间在可视化中对应x值(值)的范围就是[0, 10]，而一秒以前的规划给出的轨迹，在可视化中对应的x值范围就变成了[-1, 9]；两秒前规划的轨迹，对应的x值范围就是[-2, 8]。这样做的好处是，轨迹簇中的不同轨迹对应现实世界同一个时间的规划都并排对齐在一起;如果上游信息没有变化，决策没有变化，理想的规划是完全稳定的，轨迹簇随历史时间划出的曲面就在y方向上满足平移不变性（translational invariant），便于我们观察规划的变化（例如上图就属于这种情况）。
一个cut in的例子
在上游输入（定位，感知等）保持稳定的情况下给出平滑的规划是比较容易的，但无人车在城市公开道路上需要处理大量不可预料的情况，不论是因为上游定位感知等模块给出不稳定、跳变的结果，还是因为现实世界中实际存在突变（比如被遮挡的物体突然可见，或者近距离的邻车突然决定变道等），都会造成规划的输入无法保证稳定连续。下面这个图中展示的是一个比较常见的场景，就是我们通常称为cut in的情况：
这些情况很常见，而经验丰富的人类司机总是能泰然处之，很少需要做急刹车之类的紧急操作，这就给致力于打造自动驾驶"老司机"的轻舟智航团队设立了很高的目标。在输入发生突变的情况下，保证安全性是首要任务，但通常在保证安全的前提下，我们仍然有很多轨迹的选择，这时候如何选择体感最优的轨迹就尤为重要。人类司机向我们证明了，只要耐心打磨驾驶技术，就能在绝大多数情况下，找到又安全又舒适的驾驶轨迹。那么根据我们上面对轨迹平滑性和体感之间关系的分析，我们应该怎样处理cut in这个场景呢？
尝试一：直接刹车
图：直接刹车响应的轨迹簇
按照基本的运动学公式，我们可以很容易计算出按匀加速运动避免碰撞需要的加速度，然后直接根据这个加速度构建出速度曲线。但允许加速度从0直接跳跃到避免碰撞需要的加速度，这样产生的轨迹，其速度曲线只有 连续，是很不理想的。从加加速度的角度来看，阶跃的加速度对应的加加速度是一个冲击函数（又叫狄拉克 delta function），短时间内无穷大，所以相当不舒适。另外，虽然主流自动驾驶车辆平台的线控方案都支持直接指定加速度值（或者踏板开度，跟加速度有直接对应关系），但不论是发动机输出加速扭矩还是刹车积累制动压力都需要时间，阶跃的加速度都不利于执行。
尝试二：渐强刹车
图：渐强刹车响应的轨迹簇
在匀加速运动的基础之上，一个很显然的改进就是让加速度渐变。如果用基于规则的规划方法，由于加速度是路程s的二阶导，我们只要使用三次以上多项式就可以实现逐渐变化的加速度（连续加加速度）;大家常用的是分段五次多项式（piecewise quintic polynomial），这样起止点的加速度都可以手动指定。更好的方式是利用数值优化（数值优化），通过惩罚加速度和加加速度等来保证平滑。
上述都是业内常用的方法，但在执行时，惯常的做法都是把s或者v作为未来规划时间的函数，并没有
考虑。这样规划出来的轨迹，可以保证对平滑，但经过执行后，在方向上仍然是不平滑的。尤其是像cut in这样的场景，因为规划的输入突然变化，不显式考虑这个维度的话很容易有体感问题。
尝试三：基于时间的可变刹车
图：基于时间的可变刹车响应的轨迹簇。注意突变发生处，轨迹簇的曲面在两个方向上都有较好的平滑性。
上图中的轨迹，可以看到的问题主要集中在v-曲线从直线（匀速正常行驶）突然变到渐强刹车曲线的过程。显然这个过程对应着输入信息的突变（邻车cut in），为了保证在新信息下的行车安全，规划轨迹发生相应的变化是自然的，但上述方法没有考虑方向的连续性，所以如果比较输入突变前后的轨迹，两者在越远未来（越大），差异越大（比如五次多项式一开始与原来的直线相切，之后偏离越来越远）。如上图所示，在离当前时间不远处时，两者已经有了显著的差异。这就是执行结果体感不好的来源。虽然看每一条规划的轨迹(某特定下v对的函数)都是平滑的，但不同之间的轨迹发生变化，也会造成体感不舒适。可能有朋友会问，本次规划改变了上次规划的未来部分的轨迹，但上次规划的未来部分还没执行到呢，怎么会影响体感呢？本次规划不会改变过去的部分，而且只要拼接（splinting）得当，应该能保证本次轨迹未来部分（未执行）和过去部分（已执行）之间的连续性。这样还不够吗？
这个问题的答案跟控制算法有关。目前主流的控制算法都不会只看规划轨迹在=0的这一瞬间的数据，而是会看一定时间的未来的规划(>0)。循迹算法和PID等控制算法都通常会有一个预瞄距离（look-ahead），而像模型预测控制（model predictive control）这样的方法更是会对∈ [0, T]的整个控制时域内的轨迹做优化。这样的话，在连续两次规划之间改变轨迹的未来部分，是会造成控制的结果跳变的，如果不考虑轨迹在方向上的平滑性，就有可能产生不舒适的体感。那么，难道规划的未来运动（>0的部分）都不能改变吗？那样的话一开始做好的规划以后都不能改了，不就失去了应急反应的能力了吗？其实不是不能改变，而是需要考虑和优化改变的代价，也就是改变造成的不舒适度，把它尽可能最小化。这跟在上面第二种方法中，优化方向上的不平滑性是相同的道理，只要我们能做到在保证安全的范围内尽可能平滑、连续地变化，就可以大幅度降低不舒适体感。另外，既然优化方向上的不平滑性是为了控制输入的稳定，那么在设计这个方向的目标函数（objective function，或者叫做loss function）的时候就需要考虑控制的设计方案，比如如果控制的时域是3s，就没有必要维护轨迹>3s部分的平滑性。
这样，我们的任务就变成了优化和两个方向上的运动的平滑性。具体怎么做呢？在上面这个例子中，我们为了可视化方便而只考虑纵向规划，于是速度v就是和两个变量的函数，也就是一个曲面。描述曲面的平滑性，在微分几何（differential geometry）中有成熟的方法，像描述曲率（curvature）的第二基本形式（second fundamental form）张量等。
如果我们考虑时空联合规划（space-time joint planning），空间和时间维度各有两个，那么就需要把上述模型推广一下：轨迹簇变成四维空间中的二维流形（manifold），第二基本形式在这个场景下也有对应的推广形式。
由上可见，对和这两个时间维度上的平滑性都做优化，得到的轨迹在车辆执行后就会比较平顺，就算周围环境有突发变化，也能尽可能消化掉，形成像老司机一样，让人感觉游刃有余，处变不惊的驾驶行为。当然这样优化的前提是保证安全性，如果真的情况危急，可能在安全性的限制条件下不存在小幅度调整的空间，那么像紧急刹车，紧急转向这样的操作就会自然成为最优选择，这也是应急反应的需要。
总结
这篇文章跟大家讨论了运动规划中的两个时间维度，以及它们跟体感的关系。运动规划有两个时间维度，这个说法似乎有悖常识。在深入探讨不舒适体感产生的原因之前，我们并没有办法事先预料到问题的根源在于规划在两个不同的维度上产生不平滑性，也没有刻意地套用某个具体的理论：这两个时间维度的区别是在工程实践中自然体现出来的。我们作为自动驾驶工程师，任务就是提炼、抽象、理解这些问题，然后在我们的工具箱中找出合适的工具予以解决。
关于轻舟智航：
轻舟智航（QCraft）成立于美国硅谷，是世界前沿的无人驾驶公司，致力于打造适应城市复杂交通环境的“老司机”，将无人驾驶带进现实。基于大规模智能仿真系统、可自主学习决策规划框架和多传感器融合和时序融合方案等核心技术，轻舟智航推出了「Driven-by-QCraft」L4自动驾驶方案。
「Driven-by-QCraft」 是专注城市复杂交通场景的自动驾驶方案，早期聚焦于固定路线的中低速场景，目前基于该方案部署的城市公开道路无人小巴已在国内启动示范运营，接下来将迅速扩展至智慧城市的多个无人驾驶移动空间场景。
其核心团队成员来自Waymo、特斯拉、Uber、福特、英伟达等世界顶级公司，实现了无人驾驶关键技术模块的全栈覆盖。目前，轻舟智航在美国硅谷、中国苏州、北京、深圳等地都设有办公室，多个社招及校招岗位正面向全球持续热招中。