【学习笔记005】基于优化的轨迹规划

运动规划通常采用“前端全局搜索确定可行路径通道、后端局部优化生成平滑且动力学可行轨迹”的弱耦合架构，以实现全局探索与快速优化的优势互补，但仍可能受到搜索效率、初值敏感性和模块衔接的限制。

Trajectory Planning

轨迹（trajectory）就是加入时间参数的路径，不仅描述“从哪里经过”，还描述“每个时刻的位置、速度、姿态等状态”。

轨迹的平滑性不只是几何曲线看起来圆滑，更重要的是满足系统动力学约束，并使速度、加速度、控制能量等变化合理。

前端规划主要负责找到可行路线；即使路线已经满足动力学约束，也可能不够好。

后端轨迹优化用于进一步改善时间、能耗、安全距离、执行器限制和任务要求。

轨迹是带时间信息的路径，轨迹优化则是在前端可行解的基础上，使运动进一步满足动力学约束，并在时间、能耗、安全性和执行器限制等方面达到更优。

微分平坦（Differential Flatness）

微分平坦性的定义

考虑动力系统：

其中：

：系统状态；

：控制输入；

f(x)：系统自身的动力学；

g(x)u：控制输入对状态变化的影响；

：控制输入彼此独立。

如果能够找到一个低维变量

使系统的全部状态 x 和控制输入 u，都可以由 z 及其有限阶导数唯一确定：

那么系统称为微分平坦系统，z 称为平坦输出。

它的核心意义是：不再直接规划所有状态和控制量，只需要规划平坦输出 z(t)，然后通过代数关系恢复 x(t) 和 u(t)

因此，原来的动力学微分约束

可以被自动满足，原问题转换为关于 z 及其导数的代数约束：

准确地说，是通过平坦输出参数化，让动力学微分方程自动成立，而不是把物理动力学真正删除。

微分平坦性可以应用于多种多旋翼构型和动力学模型，但是多旋翼微分平坦模型还要处理两个问题。

第一，不能完全忽略空气阻力。

在高速飞行时，例如速度超过，只考虑重力和推力的无阻力模型精度不够。

加入空气阻力后，模型才能更准确地描述真实受力。因此“不考虑空气阻力的动力学模型保真度较低”。

第二，偏航角的定义可能出现奇异性。

一些姿态或偏航角表示方式在特定姿态下会失效，例如：

横滚角达到；

俯仰角达到；

机体 z 轴与世界竖直方向完全相反，即。

简单例子

最简单的微分平坦例子是“一维小车”，它只能沿直线运动，控制量是加速度。

设：

p(t)：位置；

v(t)：速度；

u(t)：加速度控制量。

系统状态为

动力学方程为.

现在选择位置作为平坦输出：

由于p(t)=z(t),

因此速度可以由 z(t) 的一阶导数得到：.

控制输入——加速度可以由二阶导数得到：.

于是系统的所有状态和控制输入都能写成：

这就说明该系统是微分平坦的，z=p 是它的平坦输出。

一个具体轨迹

假设我们直接设计位置轨迹：.

那么不需要重新求解动力学方程，直接求导就能得到：

例如：

时间 t	位置	速度	加速度
0	0	0	0
1	1	3	6
2	8	12	12

只要设计了平坦输出曲线 z(t)，小车的位置、速度和所需加速度就全部确定了：

对应关系是：

所以微分平坦性的直观含义就是：

找到一个容易规划的“核心输出” \(z(t)\)，只需设计它的时间曲线，就能通过有限次求导恢复整个系统的状态和控制输入。

多旋翼飞行器动力学和微分平坦性

多旋翼的状态、输入和动力学

系统状态

分别表示：

：位置；

：速度；

：姿态旋转矩阵；

：机体系角速度。

控制输入

将每个电机的推力经过输入映射后，控制量写成：

其中：

：所有螺旋桨产生的总推力；

：总控制力矩。

非线性动力学

表示位置的导数是速度。

右侧三项依次为：

重力；

空气阻力；

螺旋桨总推力。

姿态运动学为：

其中是把角速度向量转换成反对称矩阵。

转动动力学为：

平坦输出

选择：

也就是三维位置 r 和偏航角。

从位置恢复速度和推力方向

因为位置 r 是平坦输出的一部分，所以：.

接下来需要从位置轨迹恢复姿态。

定义机体坐标轴：.

其中是多旋翼的机体竖直轴，也就是总推力方向。

把牛顿方程分别投影到机体和方向。由于总推力沿方向，而

所以投影以后，推力项消失，得到：

意思是：括号中的向量同时垂直于和。

恢复机体 z 轴和总推力

定义：前面已经得到：

因为同时垂直于和的方向就是，所以 h 与平行。

因此：

其中归一化函数为：

展开就是：

由于、，所以机体 z 轴可以由位置的一阶、二阶导数确定。

然后把牛顿方程投影到方向，得到总推力：

所以位置轨迹的加速度确定了：

推力方向；

推力大小 f。

恢复完整姿态 R

目前已经知道：

机体 z 轴方向；

平坦输出给出的偏航角。

这两项共同确定多旋翼的完整姿态。

首先，偏航旋转四元数为：

然后根据构造倾斜旋转四元数：

这里：

负责偏航；

负责把竖直方向倾斜到；

两种旋转通过 Hopf 分解进行组合。

完整姿态四元数为：

再把四元数转换成旋转矩阵：

因此，知道和后，完整姿态 R 就能唯一确定。

恢复机体角速度

姿态动力学为：.

左乘，得到：.

因此：

符号表示把反对称矩阵重新转换成三维向量。

也可以使用四元数求导来计算：

所以角速度由下面这些量决定：

而是一个归一化向量：.

因此：

其中归一化函数的 Jacobian 为：

因为 h 中包含位置加速度，所以中包含位置的三阶导数：,即 jerk。

因此：

恢复控制力矩

现在已经恢复了：

r；

v；

R；

总推力 f。

只剩下控制力矩。

由转动动力学：

移项得到：

因为中包含和，再求一次导数后，会包含：.

因此，控制力矩可以由位置的四阶导数和偏航角的二阶导数确定。

整体推导关系

所以只要规划具有足够高阶连续性的

就能恢复全部状态

和全部输入.

我需要掌握的

微分平坦性的用途

找到少量平坦输出 z(t)，通过它及其有限阶导数恢复全部状态和控制输入：

这组无人机公式的结论

对这里的多旋翼模型，平坦输出是：

也就是位置和偏航角。它们的导数可以依次恢复：

为什么规划轨迹要具有高阶连续性

如果控制量需要，那么规划出的至少要能够连续地求到相应阶数，否则控制输入会发生跳变，轨迹难以执行。

基于微分平坦性的规划—控制闭环

整体流程是：

1. 轨迹规划器

规划器输出期望轨迹：

分别是：

：期望位置；

：期望速度；

：期望加速度；

：期望偏航角。

这些量就是由平坦输出

及其导数得到的。

2. 位置控制器

位置控制器比较：

根据位置和速度误差，计算：

总推力指令；

期望横滚角；

期望俯仰角；

期望偏航角。

位置控制器解决的是：

无人机应产生多大的推力、朝哪个方向倾斜，才能跟上期望位置轨迹？

3. 姿态控制器

姿态控制器根据期望姿态和实际姿态、角速度，计算：

它们对应三个方向上的控制力矩，使无人机达到期望的横滚、俯仰和偏航姿态。

4. 反馈闭环

无人机将实际状态反馈给控制器：

反馈给位置控制器；

和反馈给姿态控制器。

因此，即使模型不准确或存在外界扰动，控制器也能根据误差进行修正。

这一页的核心是：

规划器提供期望轨迹，微分平坦映射提供相应的期望姿态和推力信息，反馈控制器负责让真实系统跟踪这条轨迹。

用样条曲线参数化平坦输出

平坦输出为：

也就是三维位置和偏航角。

一条平坦输出轨迹表示为：

意思是：对于任意时刻 t，轨迹都会给出对应的位置和偏航角。

规划时通常不用无限多个 z(t) 作为优化变量，而是用少量样条参数表示它，例如

其中：

是样条基函数；

是待优化的控制点或系数。

使用样条的优点是：

容易保证平滑性

通过选择样条阶数以及相邻曲线段的连续性，可以控制位置、速度、加速度等是否连续。

导数容易计算

都能直接由样条公式求出，方便计算速度、加速度和控制量。

三个方向可以分别参数化

可以分别构造：

再组合成三维位置轨迹。

数值稳定性较好

优化器只需要调整有限个样条系数或控制点，不需要逐时刻优化整条连续轨迹。

一个样条参数化例子

假设小车要从运动到

中间有障碍物，所以希望轨迹向上绕行。我们再设置两个控制点：

使用一段三次样条曲线

其中.

这里就是描述整条轨迹的有限个参数。

当s=0 时：.

当s=1 时：.

当s=0.5 时：.

因此，轨迹大致是：

注意：小车并不一定经过和，它们主要负责“拉动”曲线的形状。

提高，曲线绕得更高；

降低，曲线更加平直；

移动控制点，就能改变整条轨迹。·

怎样加入时间？

如果总运动时间为 T，令

那么轨迹就变成：

这样对于任意时刻 t，都能计算小车的位置 p(t)。

对它求导，就能获得速度：

继续求导可以获得加速度：

“参数化”到底是什么意思？

一般形式是：

其中：

：预先确定的样条基函数；

：需要规划器寻找的控制点或系数。

原本要寻找的是一整条函数：

它包含无限多个时刻的位置。使用样条后，只需要寻找有限个参数：

因此，轨迹优化就从“寻找一整条连续曲线”变成了“调整少量控制点”。

如果一段曲线不够，就使用多段多项式连接起来，并让连接处的位置、速度、加速度连续：

这就是“用样条参数化轨迹”：用少量控制点或系数表示整条连续、可求导的轨迹，规划器实际优化的是这些参数。

轨迹优化 Trajectory Optimization

轨迹优化不是随便找一条从起点到终点的曲线，而是在满足避障、动力学和边界条件的情况下，寻找代价最小的轨迹：寻找满足约束且尽可能平滑、快速、节能的轨迹

目标函数

这里：

：需要规划的轨迹，例如位置轨迹；

T：轨迹总时间；

：轨迹的第s阶导数；

W：权重矩阵；

：关于总时间 T 的惩罚项。

需要特别注意：这里的 v(t) 是为了简写而引入的变量，表示，不一定是物理速度。

Minimum jerk

取 s=3：

目标函数为：.

也就是尽量减小 jerk。最小化 jerk，会间接减小姿态角速度，使视觉跟踪更稳定。

Minimum snap

取 s=4：

目标函数为：

也就是尽量减小 snap。最小化 snap，会减小角加速度和推力变化，使轨迹更加平顺。

约束

。这是在定义优化目标中使用的最高阶导数 \(v(t)\)。

它统一表示轨迹需要满足的各种限制，例如：

速度上限；
加速度上限；
控制输入上限；
其他动力学或任务约束。

其中表示自由空间。避障约束

其中：它表示把轨迹及其前阶导数组合起来。例如 minimum jerk 中 s=3，需要指定起点和终点的：即位置、速度和加速度。

它也很难直接求解，因为 z(t) 是一整条连续函数，而且约束要求在无限多个时刻都成立。后面的内容会通过多项式或样条对 z(t) 参数化，把“寻找整条函数”转换为“寻找有限个多项式系数”。

无不等式约束情形 Unconstrained Case

轨迹优化中的无不等式约束情形（Unconstrained Case）：给定起点、终点、中间路点和每段时间，只寻找最平滑的轨迹，暂时不考虑障碍物、速度上限和加速度上限等约束。

整条轨迹经过：并被划分成若干段：

第 i 段的持续时间为：.不同颜色表示不同的多项式轨迹段，所有轨迹段在中间路点处连接。

优化目标、边界条件参考上面的。但是注意蓝色的中间路点条件

蓝色部分是：.

表示在第 i 个中间点规定了多少阶状态。

例如：

只规定位置:

则：.

规定位置和速度:

则：

规定位置、速度和加速度:

则：

所以中间条件不一定只包含路点位置，也可以规定经过路点时的速度、加速度等。

“无约束”更准确地说是：暂时没有以下不等式约束：

也就是暂时不考虑速度上限、加速度上限和避障等约束。因此可以理解为：只有插值和边界条件，没有安全与动力学不等式约束.

两个定理

定理一

最优轨迹的每一段都是次多项式：

对应关系为：

最小化的量	\(s\)	最优多项式次数
速度	1	1次
加速度	2	3次
Jerk	3	5次
Snap	4	7次

原因是最小化

得到的最优性条件为：.

一个函数的 2s 阶导数为零，说明它最高是 2s-1 次多项式。

定理二：连接处的连续性

最优轨迹在中间时刻具有：连续性。

最常见的情况是中间点只指定位置：.

此时连续阶数为：.

例如 minimum jerk：

得到：.

因此在中间路点处：都连续，即轨迹具有连续性。

也就是说，不同颜色的多项式段虽然分别计算，但连接之后不会出现位置、速度、加速度等突然跳变。

核心结论

给定起终点状态、中间路点和每段时间后，求解最小 jerk/snap 问题会得到唯一的分段多项式轨迹：

无不等式约束情形：BVP

对于minimum jerk是五次多项式，为什么是五次的证明可以回顾笔记004的Pontryagin最小值原理。以及上面的定理一

假设一维物体需要在时间 T 内从位置 a 运动到位置 b。边界条件设置为：

在 t=0 处

由

得到：.

所以：.

在 t=T 处

位置条件是：

速度条件是：

加速度条件是：.

将六个方程写成矩阵：

也就是课件中的：.

因此：

求出 c 后，整条轨迹 x(t) 就确定了。

这条轨迹的显式形式

当起终点速度、加速度均为零时，令：

最终轨迹可以写成：

它满足：

因此物体从静止开始，平滑加速，最后又平滑减速到静止。

怎样扩展到多段轨迹？

假设轨迹经过多个路点：

可以在每两个相邻路点之间分别生成一条五次多项式：

对于其中一段，可以规定：

对应的边界向量变成：

仍然通过：求出每段的六个多项式系数。

中间路点的速度为什么重要？

假设两段轨迹在连接：.

为了让连接处速度连续，需要：.

也就是前一段的终点速度必须等于后一段的起点速度。

如果在每个中间路点都设置：

那么机器人会在每个路点完全停下来，再重新启动，轨迹虽然连续，但运动效率较低。

因此课件建议在中间路点设置合适的通过速度 v，例如沿相邻直线段的平均方向，从而把原来的折线路径圆滑化。

总结

前端路径通常由直线段组成，在路点处有尖角。五次多项式轨迹可以把尖角变得平滑。

在中间路点指定合适的通过速度 v，让机器人尽量以近似恒定速度通过，而不是每到一个路点就停下。

在中间点将加速度设为零，可以得到比较平稳的连接，但这是一种人为设置，不一定是全局最优选择。

如果某段距离非常短，但要求在很短时间内保持较大的起终点速度，那么五次多项式可能产生：

很大的加速度；
很大的 jerk；
轨迹过冲；
动力学不可执行。

所以过短的路段通常需要合并、重新分配时间，或者重新设置中间速度。

三维轨迹可以分别求解：

然后组合成：.

需要提供：

起点和终点的位置、姿态等边界条件；

中间路点；

各段时间。

中间路点可以由 A*、RRT* 等前端路径规划器得到。图中：

黑色折线：前端搜索得到的粗糙路径；

红色曲线：多项式优化后得到的平滑轨迹。

平滑性通常通过最小化控制输入变化率对应的高阶导数实现，例如：

即 minimum jerk。

概括：

前端规划提供三维路点，BVP根据起终点条件和路点直接求出各段多项式系数，从而将折线路径转换为平滑轨迹。

检查运动基元的可行性check feasibility of primitives

规划时通常只确定当前状态，接下来往哪里走尚未确定。因此会采样多个终端状态。每个终端状态分别构成一个 BVP。

需要检查哪些约束？

对于多项式轨迹

常见约束包括：

速度不能超过上限：.

推力必须位于允许范围：.

轨迹不能碰撞障碍物。

轨迹必须始终位于安全走廊内部。

这些约束统一写成：

关键是：

意味着不是只检查几个离散时刻，而是必须保证整个连续时间区间都满足约束。

检测方法

1. 离散时间采样

选取若干时刻：逐点检查约束。

优点是简单灵活；缺点是采样太稀可能漏掉两个采样点之间的违规情况：

增加采样密度可以减少漏检，但计算会变慢。

2. 递归边界检查

利用多项式的上下界，判断整段曲线是否在允许范围内。如果无法确定，就继续把时间区间分成更小的区间检查。

它比密集采样高效，但：

可能依赖划分精度；

某些情况下无法直接判断；

3. 极值检查

为了保证：

可以寻找 G(t)在区间中的最大值。候选极值点满足：.

只要检查：

区间起点；

区间终点；

所有内部极值点；

就能判断约束是否始终成立。

这种方法比较准确，但求解所有极值点通常需要数值迭代，计算可能较慢。

将检查转换为求多项式根

由于 p(t) 是多项式，所以把它代入约束以后：

通常也是关于 t 的一元多项式。

约束边界对应：G(t)=0.

因此可以通过判断区间 [0,T] 中有没有根，检查轨迹是否接触或穿过约束边界。

严格来说，需要两个条件：

[0,T] 内没有 G(t)=0 的根；

任取一个时刻，例如 t=0，确认 G(0)<0。

这样才能推出：.

因为“没有根”只能说明符号不会变化；如果一开始就是正数，那么整段仍然是不满足约束的。

Sturm定理

Sturm定理可以精确计算一个多项式在区间内有多少个不同实根。

首先构造 Sturm 序列：

后面的多项式通过带余除法得到：

在区间两个端点分别计算序列的符号变化次数：.

那么区间 (a,b) 内的实根数量为

一个例子是：G(t)=-t(t-1)(t-3)(t-6).

显然它有四个根：.

在区间 (-1,7) 上：

因此根的数量是：4-0=4.

Sturm定理主要使用多项式除法等代数运算，不需要密集采样整个时间区间。

一个例子

假设二维轨迹为

速度是位置的一阶导数：

假设最大速度是，那么约束可以写成

代入这条轨迹：.

要让轨迹在整个时间段都可行，必须满足.

但是当时，，说明速度超过了 2。因此这条运动基元不可行。

这里各符号的意思是：

：三维位置的三个分量；

：第 j 个分量的第 i 阶导数；

G<0：约束得到满足；

：不是只检查几个时刻，而是整段轨迹的每个时刻都必须满足。

无不等式约束情形：BIVP

BVP（边值问题）只规定首尾条件：

BIVP（边界—中间值问题）还要求轨迹经过若干中间点：.

图中的黑色折线连接了这些路径点，红色曲线则是优化后得到的光滑轨迹。这里的“无约束”不是完全没有约束，而是没有速度上限、障碍物等不等式约束；首尾条件和路径点仍然是等式约束。

中间条件表示轨迹必须在时满足指定的中间状态。若是“waypoint-only”，通常只规定位置： ,中间点处的速度、加速度等不事先指定，而是由整体优化自动决定。

为什么得到的是样条

每两个相邻路径点之间使用一段多项式：

这些多项式在路径点处平滑连接，因此整体构成一条样条曲线。

对于仅规定中间位置的情况：

优化目标	每段多项式	路径点处的连续性
Minimum jerk，s=3	5次	直到 snap 连续，即
Minimum snap，s=4	7次	直到 pop 连续，即

例如 minimum jerk 虽然最小化的是三阶导数，但全局最优条件还会使四阶导数 snap 在各段连接处连续。

把每一段多项式的系数全部放进向量 c：

把首尾条件、经过路径点以及各段连续性条件写在一起，就得到线性方程组：

其中：

M：由各段时间和连续性条件构成；

b：已知的起点、终点和路径点；

c：待求的多项式系数。

解出 c，整条轨迹就确定了。PPT最后强调的是：对这种没有不等式约束的问题，可以直接解线性方程，而不必调用通用的迭代优化器。核心流程就是

多项式表示轨迹

把整条轨迹分成 M 段：

每段都是 N 次多项式：.

其中：

：第 j 段的多项式系数；

：第 j 个连接时刻；

：第 j 段的持续时间。

若最小化第 s 阶导数，则每段次数取.

所以：

minimum jerk：s=3，使用五次多项式；

minimum snap：s=4，使用七次多项式。

这一方法首先要求每段的持续时间已知。

条件一每段端点的导数条件

意思是规定第 j 段起点和终点的位置、速度、加速度等。

例如：表示第 j 段必须到达路径点。

条件二相邻段的连续性条件

在连接时刻：.

例如要求连续，就需要满足：

也就是位置、速度和加速度都不能在连接点发生跳变。

全局时间与局部时间

课件推导采用统一的全局时间 t：.

这样写数学上比较清楚，但实际编程通常让每一段从零开始计时： .

于是第 j 段写成 .

使用局部时间可以避免计算很大的，数值上通常更加稳定。

得到线性方程组

把所有段的多项式系数放入向量：

再把边界条件、路径点条件和连续性条件全部写在一起，就得到 .

其中：

c：所有待求的多项式系数；

b：已知的起点、终点和中间路径点；

M：由求导公式、时间和连续性条件构成的矩阵。

因为每一段通常只与相邻两段有关，所以 M 是带状矩阵：

因此可以高效求解。只要每段持续时间都大于零，课件给出的条件保证 M 可逆，轨迹解是唯一的

分层方法

首先使用路径规划算法寻找无碰撞路径。因为它主要考虑几何位置，搜索维度较低，计算比较快。然后把路径上的拐点作为中间点，使用 BIVP 将折线路径拟合成光滑的多项式轨迹。

虽然黑色折线是无碰撞的，但平滑后的红色曲线可能偏离折线，进入障碍物。因此：这是“先规划、后平滑”方法的主要问题。

迭代 BIVP

解决思路是反复修正：

生成一条平滑轨迹；

检查轨迹是否碰撞；

在碰撞附近增加中间路径点；

重新求解 BIVP；

直到轨迹与障碍物分离。

新加入的路径点会把轨迹逐渐“推回”安全区域。

RRT* + BIVP

这里把两种方法组合起来：

RRT*：负责在障碍物中找到无碰撞的粗路径；

BIVP：负责把粗路径转换成连续、光滑的多段多项式轨迹。

带约束轨迹优化

约束主要有三类：表示速度、加速度、控制输入等动力学限制；表示轨迹必须始终位于无碰撞的安全空间中；表示满足起点和终点条件。

这个问题很难，因为要直接优化一条连续曲线、运动时间、动力学可行性和避障约束。可能的简化方法是：

用多项式样条表示轨迹，只优化有限个系数；

先固定各段时间，只优化空间形状；

将复杂动力学简化为速度、加速度等范数限制；

用局部凸安全区域表示自由空间。

带约束轨迹优化：凸优化

如何把“在安全区域内规划一条平滑轨迹”，写成凸优化问题。

整套推导围绕三件事展开：

用分段多项式表示轨迹，把多项式系数作为未知量。

把“不平滑程度”写成系数的二次型。

把起终点、段间衔接、安全区域要求写成线性约束。

最后得到我们熟悉的形式：

图中的是预先构造的凸安全区域，可以理解为允许轨迹通过的“走廊”。蓝色曲线由几段轨迹拼接而成，每段分别限制在对应区域内。

例如二维轨迹可以写成：

其中每个坐标都用多项式表示。先只研究一个坐标：

把这一段的系数收集成向量：

优化的任务，就是寻找一组好的系数 。系数确定以后，整段曲线就确定了。

还有一个容易混淆的地方：

轨迹可以是关于时间 t 的高次多项式，但它关于待求系数是线性的。

比如：

在给定时刻 t，括号中的数都是已知的。这一点是后面线性约束成立的基础。

为什么平滑性代价能写成二次型？

对于minimum-snap，即最小化四阶导数平方积分。

从多项式开始：.

求四阶导数：

令

那么：.

平方时要把所有交叉项都乘出来：.

所以在区间上：

这里的是两个独立的求和下标，表示平方后不同系数之间的组合。

把每个前面的已知量定义为矩阵元素：

就得到：.

所谓二次型，就是把 这些二次项统一装进一个矩阵表达式。

用一个小例子看得更直观。设：]. 那么：.

因此：

非对角元素是，因为矩阵乘法中两个对称位置会共同产生。没有直接出现在代价里，是因为它们在求四阶导数后消失了；它们仍然会受到端点和连续性约束的限制。

为什么这个二次型是凸的？

“二次函数”本身不一定凸。这里凸，是因为 Q 是半正定矩阵。

把四阶导数写为：

那么：.

对任意向量，都有：

因此：.

目标函数的 Hessian 是 2Q，所以目标函数凸。

直观上，这是因为我们累加的是“一系列线性表达式的平方”。

不过，半正定不一定意味着解唯一。例如，四阶导数代价无法直接区分不同的三次及以下多项式，是否唯一还取决于约束。

为什么位置、速度等要求都是线性等式？

多项式的 k 阶导数为：

如果要求在时刻 T 满足：

那么：

这显然是关于系数的线性方程。

尤其是 k=0 时：

所以位置约束、经过某个航点的约束，就是零阶导数约束。

用五次多项式举例：.

如果指定起终点的位置、速度、加速度，则：

这就是课件中的：.

每一行代表一个要求，矩阵的列对应各个待求系数。

顺便注意：对于单段五次多项式，固定 T>0 后，这六个边界条件已经能唯一确定六个系数，此时没有剩余自由度可优化。多段轨迹或更高次多项式往往会留下可供优化的自由度。

两段轨迹如何平滑地接起来？

假设两段轨迹在一个连接点相遇。

仅要求位置连续：

只能保证它们接在一起。

还可以进一步要求：

一般形式是：.

假设是用于计算 k 阶导数的行向量，就有：

拼成矩阵：

连续性约束只要求两边相同，不一定指定它们等于多少。

例如：

允许优化器选择连接处最合适的共同速度；而把两边都指定为 0，则要求轨迹在这里停下来。这两个要求很不一样。

安全走廊为什么给出线性不等式？

图中的安全区域用多面体表示：.

例如二维矩形区域：

等价于：

要求轨迹位于区域内，就是：.

轨迹又能写成：

所以：

在给定时刻 t，这就是系数的线性不等式。

这里要区分一个重要细节：“每个时刻都是线性约束”，不等于“只有有限个约束”。

课件要求整段轨迹始终留在走廊内，即对所有 t 都成立，因此原式实际上包含无限多个线性不等式。实际求解时需要进一步处理，例如：

在有限个时刻采样并添加约束；但仅检查采样点，不能自动保证采样点之间安全。

改用 Bézier 表示，并把所有控制点限制在同一个凸区域内；利用凸包性质，可以保证整段曲线都在区域内，不过这是充分条件，可能偏保守。

因此，第二张图展示了凸问题的结构；要变成可直接输入求解器的有限维二次规划，还需处理整段时间上的走廊约束。

一个例子

以两段一维轨迹为例：.

把两段系数拼接：

总平滑性代价为：

代价矩阵是分块对角的，因为各段分别积分；不同轨迹段主要通过连续性约束联系起来。

例如，让轨迹从出发、经过 w、到达，可以要求：

并补上速度、加速度等连续性：.

这些要求连同起终点导数条件，统一组成：

走廊要求则在适当处理后组成：.

于是得到开头的凸二次规划。只要问题可行且最优解存在，凸性保证局部最优解也是全局最优解。

最后再强调第二张标题中的 “Convex Simplification”：这里的凸化依赖于已选好的凸走廊，以及固定的各段时间。 如果连时间也一起作为未知量，那么 Q(T)、端点矩阵中的都会变化，联合优化一般不再是这里这个凸二次规划。

求解

①凸集和凸函数：分别描述“允许的范围”和“目标的形状”。

凸集：集合内任取两点，连接它们的整条线段都在集合内。

实心圆、矩形是凸集。

圆环、图中的月牙形区域不是凸集。

公式为：

其中就是在两点之间取点。

凸函数：在凸定义域上，函数图像上任意两点之间的连线，都位于对应图像的上方或与之重合：

典型例子是，可以直观理解为“碗形”。左边是中间位置的实际函数值，右边是连线的高度。

当、时始终严格小于，就是严格凸函数。若 -f 是凸函数，则 f 是凹函数。

② 凸优化：在凸的可行范围内，最小化凸函数。

课件给出的标准形式是：

这里要求：

目标函数是凸函数。

不等式左边的都是凸函数。

等式约束是仿射形式 Ax=b。

例如：

就是凸优化，最优解为 x=1。

凸优化的重要性质：局部最优解一定也是全局最优解。 但解不一定唯一；若目标严格凸，则在凸可行集上，最优解存在时是唯一的。

③ LP、QP、QCQP、SOCP：几类常见的标准问题。

类型	主要特征	简单理解
LP：线性规划	线性目标、线性约束	例如最小化 2x+3y
QP：二次规划	二次目标、线性约束	例如最小化
QCQP：二次约束二次规划	二次目标，允许二次不等式约束	例如增加
SOCP：二阶锥规划	线性目标，允许“二范数 ≤ 仿射表达式”的约束	例如

要注意：并非所有 QP、QCQP 都是凸的。

例如 QP 的目标：

要求对称矩阵，也就是半正定，才能保证凸性。课件中的凸 QCQP 对目标和各个二次不等式也有相应要求。

上一节的轨迹问题，在固定各段时间、将走廊约束处理成有限个线性约束后，就是凸 QP：

这里就是待求的多项式系数。

④ 求解工具：先把问题写成标准形式，再交给工具计算。

轨迹规划不必从头编写优化算法，重点是把目标和约束整理好。

课件列举了 CVX、MOSEK、OOQP、GLPK 等工具。其中 CVX 主要是建模层：方便你按数学公式描述问题，再调用底层求解器。

第三张图标题中的 DCP，可以理解为“一套用于构造、检查凸优化模型的规则”。数学上凸的表达式，有时还需要改写成工具能够识别的形式。

⑤ 归一化：把数值调整到合适的量级，减少计算误差。

多项式包含。如果时间很小，例如，那么：.

同一计算中出现相差很大的数，可能让方程求解不稳定。

所以对区间，引入相对时间：

这样每段都有，再用表示多项式。

这只是换了时间坐标，并没有把真实运动时间改成 1 秒。 导数必须相应换算：

例如速度要除以，加速度要除以。最小 snap 的代价也需要保留时间缩放：

空间归一化同理：选择参考位置和尺度 L，令

求解后再恢复：.

归一化主要改善数值稳定性；使用时，位置、导数、约束和代价都要按同一套尺度换算。

更详细的例子

第一步：给出具体的运动任务，确定未知量 。

假设一个物体沿直线运动，要求：

项目	具体要求
起点	t=0 时，位置为 0
中间点	t=1 时，位置为 1
终点	t=2 时，位置为 2
起终点状态	速度、加速度均为 0
中间衔接	位置、速度、加速度连续
位置范围	第一段在 [0,1]，第二段在 [1,2]
优化目标	两段的 jerk 平方积分之和最小

两段时长均为 1 秒，各自使用局部时间：

第二段的局部时间，对应真实时间 t=1。

把系数按固定顺序排列：

所以，这个优化问题的未知量就是 12 个数。

第二步：对多项式求三阶导数，得到目标矩阵 Q。

第一段的 jerk 为.

第一段代价：.

展开、积分：

将这个二次表达式写成矩阵：

其中：

注意，交叉项对应两个对称元素：.

第二段采用同样的计算。两段时长相同，因此：

这就是具体的目标矩阵。

如果求解器要求目标写成，则输入 H=2Q。

第三步：把运动要求逐条代入，得到等式矩阵。

先列出要求对应的方程：

要求	对应的系数方程

现在按照的顺序，把每行方程的系数放进去：

右边是每个方程的已知值：

于是：

一条运动要求，对应矩阵的一行；一个多项式系数，对应矩阵的一列。

这里有 12 个未知量、10 个独立等式，剩下两个自由度供优化器选择。

第四步：将位置范围变成不等式矩阵。

先看最容易理解的方式。

如果要求第一段在时不超过位置 1，直接代入：

即：

对应矩阵的一行：

所以，固定时刻的位置不等式，也只是代入多项式后收集系数。

不过，这仅检查了一个时刻。为了在这个例子里保证整段曲线都不越界，我们使用 Bézier 控制点约束。

同一个五次多项式可以等价写成：

其中六个控制点与原来的系数满足：

这个矩阵通过展开 Bézier 表达式、比较同次幂系数得到，元素的通式是：

为什么控制点能约束整段轨迹？因为在 [0,1] 上，Bézier 表达式中的权重非负，且权重和为 1。因此曲线的位置始终是这些控制点的加权平均。

于是：能保证第一段始终在 [0,1]，

而：能保证第二段始终在 [1,2]。

合并起来，定义：

则：.

统一整理成“”：

这样便得到：

它包含 24 行：12 个控制点，每个分别有一个上界和一个下界。这里控制点约束是保证安全的充分条件，一般可能偏保守；不等式通常使用，不是严格的 <。

补充：关于为什么用控制点

曲线上每个位置，都是六个控制值的加权平均。

前面的五次多项式写法是：.

Bézier 写法则是：

这里：

：六个控制值，是我们可以调整的数。

：预先规定好的权重，随时间变化。

五次 Bézier 的权重具体为：

控制值固定以后，权重随着时间变化，就生成了整条曲线。

为什么这种表示特别适合“轨迹不能越界”的约束？

关键在于，当时：

因此始终是六个控制值的加权平均。

例如六个控制值都在 [0,1]：

那么不论时间怎样变化，都有：

这和“几个介于 0 和 1 之间的数，其加权平均也介于 0 和 1 之间”是同一个道理。

所以，我们可以通过约束六个控制值，保证整段曲线不越界：.

再代入，就得到前面的.

在二维、三维中也一样：所有控制点都在同一个凸安全区域内，整条 Bézier 曲线就一定在该区域内。

补充：符号理解

这里的 表示“由 6 个数字 1 组成的列向量”，

把这些符号放回原来的式子，就容易理解了。

第一段有六个多项式系数：

矩阵 B 把它们换算成六个 Bézier 控制值：

所以：

就是下面六个条件的简写：

再看两段轨迹合并后的符号。

表示把两段的系数上下拼接，得到一个含有 12 个数的列向量

是一个的分块矩阵。因此：

小写字母表示下界向量，表示上界向量：

于是：

表达的就是：

前六个控制值在 [0,1] 内。

后六个控制值在 [1,2] 内。

最后，为什么会出现负号和 24 行？

上下界可以拆成两组：

第二组两边乘以 -1，不等号反向：.

将这两组上下拼接：

其中 ineq 是 inequality（不等式）的缩写。12 个控制值各有一条上界约束、一条下界约束，所以一共是 24 条不等式，也就是矩阵的 24 行。

此外，式子里的是组合数，读作“5 选 k”：

当时，依次为 1,5,10,10,5,1。

第五步：求解，得到真实的轨迹系数。

现在所有矩阵都已确定：

对象	大小	内容
		两段五次多项式的系数，待求
		jerk 平方积分产生的系数
		端点、连接条件
		指定的位置、速度、加速度
		控制点上下界条件
		两段允许的位置范围

这个例子可以进一步手算。

设连接处的速度、加速度分别为：

前面的 10 个等式可以把全部系数表示成这两个量：

代入，整理得到：

因此，满足等式约束时的最低代价在：取得。

对应的多项式系数为：

检查两段控制点：

分别位于 [0,1] 和 [1,2]，因此不等式也满足。这个解就是完整约束问题的最优解。

最终轨迹为

这里各段的。按真实时间写，就是：

代回原始运动要求，结果是：

真实时间	位置	速度	加速度
t=0	0	0	0
t=1	1	15/8	0
t=2	2	0	0

两段的 jerk 平方积分分别是 45，总代价是 90。

怎样保证轨迹在整个运动过程中都安全

先找安全走廊，再在走廊里生成平滑轨迹。

图中的流程是：

检测障碍物：确定哪些区域不能通过。

搜索通道：找到一条从起点通向终点的可通行路线。

扩大走廊：在不碰到障碍物的前提下，把通道扩成一组相互衔接的安全区域，给轨迹调整留出空间。

优化轨迹：让曲线完全位于走廊内，同时满足位置、速度、加速度等要求。

图中黄色矩形就是安全走廊的一部分。矩形是凸集，便于写成线性不等式。

它的逻辑是：

实际构造走廊时，还需要考虑机器人自身的尺寸和安全余量。

区分“某个时刻的约束”和“整个时间段的约束”。

类型	约束什么	例子
瞬时约束 Instant	指定时刻的状态	起点位置、终点速度、两段连接处连续
区间约束 Interval	一段时间内的所有状态	全程不出走廊、全程速度不超限

例如，瞬时位置要求：

只需要代入指定时刻，就能得到：.

中间连接位置也可以不固定为某个点，而只要求它处于两个走廊的交叠区域，这就会产生不等式约束。

区间约束则是：

这里的表示“对所有”，所以它要求的是：

从到，任何时刻都不能违反约束。

例如：.

这比只要求 f(0)、f(0.5)、f(1) 在范围内更强——曲线可能在两个检查点之间越界。

速度、加速度也有类似要求。课件中的线性形式通常对应各方向分量的上下界，例如：

怎样检查并保证“全程满足”？

这里的 “globally activated” 可以理解为让约束覆盖整个时间区间，并不是说所有不等式都要取等号

方法一：先求解，再检查极值，发现越界就补约束。

假设要求：.

流程是：

先求出一条候选轨迹。

检查区间端点，以及满足的内部极值候选点。

如果发现某个时刻超过上界，就增加：.

重新优化，再检查。

图中红色箭头标出的，就是需要处理的越界峰值。

优点是针对实际违规位置添加约束；不足是可能反复求解，因为调整曲线后，新的峰值可能出现在其他时刻。课件中的“10 次”是举例，不是固定次数。

方法二：在很多离散时刻添加约束。

例如每隔一小段时间，就限制一次轨迹的位置：

检查时刻越多，约束数量和计算量通常越大。

这里需要分清：

单纯增加采样点，仍不能自动保证采样点之间安全。

若再配合很小的安全区域或额外安全余量来保证安全，就可能过度限制轨迹，使结果偏保守。

简化带来的问题

经过同样的点，时间分配不同，轨迹也可能不同。

假设轨迹有两段：

第一段走 1 米；

第二段走 5 米。

如果都分配 1 秒，第二段就需要更快地运动。为了让连接处的速度、加速度连续，两段多项式都要随之调整。

因此，时间分配不仅影响“走得多快”，在重新优化后，还可能改变空间曲线的形状。

左下图中，多条轨迹经过相同的黄色航点，但有的比较自然，有的出现明显绕行。它说明：即使航点合理，时间分配不合适，也可能得到不理想的轨迹。

右下图则展示了迭代调整后轨迹形状的变化。

这里的难点是

但如果将各段时间也一起作为未知量，目标矩阵和约束矩阵都会随时间变化，联合优化一般就不再是原来的凸 QP。

以先估计时间，再利用走廊中的空间自由度调整轨迹。

左上角画的是梯形速度曲线：

起步加速；

中间匀速；

接近终点时减速。

速度曲线下的面积就是运动距离.

所以，给定距离以及速度、加速度限制，就可以据此估计运动所需时间。距离太短、来不及达到设定速度时，速度图会变成三角形。

这种简单速度模型可以用于估计时间；最终轨迹的速度仍由后续优化决定。

为什么它在安全走廊方法里比较好用？关键是相邻走廊有重叠区域。

左下图中：

黑色矩形：安全走廊；

蓝色斜线区域：相邻走廊的交集；

红点：不同方案中的段间连接位置；

绿色曲线：相应的轨迹。

如果连接位置必须固定在某一个点，优化器调整的余地就比较小。

如果只要求连接位置位于交叠区域：

优化器就能移动连接位置，调整各段曲线，以适应给定的时间分配。右下图中的相互重叠区域，也体现了这种思路。

因此，走廊交叠区域提供了调整空间，使方法对初始时间估计更有适应性；但如果给的时间短到无法满足速度、加速度限制，仍然需要增加或重新分配时间。

有约束情形：时空变形

这里的“时空变形”，就是同时调整轨迹的空间形状和运动时间，接着解决上一节“时间分配不合理会影响轨迹”的问题。

① 空间和时间分别调整什么？

空间调整：改变曲线的形状、分段连接位置，让轨迹绕开障碍物。

时间调整：改变各段耗时，让某些地方走快一些、某些地方走慢一些。

例如通过一个急弯时，可以把路线调整得更平缓，也可以增加通过这段弯道的时间。

② 左边的目标函数：兼顾平滑性和耗时。

这里的符号是：

符号	含义
z(t)	轨迹变量，这里可以先理解为位置向量
T	总运动时间，也是待优化的变量
v(t)	这里定义为，即轨迹的 s 阶导数
W	权重矩阵，用于设置各分量的惩罚权重
	与总时间有关的代价，通常用于惩罚过长的耗时