上一篇讲解了《增广拉格朗日方法求解带约束优化问题》，套路是外层ALM和内层ilqr进行无约束优化。内层可以用ilqr做无约束优化问题求解，那么可以用其他方法求解吗？答案是可以的，如lbfgs也可以用于无约束优化问题。

恰好最近看到一篇论文，用的是ALM(增广拉格朗日法)+LBFGS(拟牛顿法)，下边让我们来看一下。

让人兴奋的一点是，论文里提到：

❝

一个统一的轨迹可以同时处理前进和后退运动，而不是由规划前端指定

这里无需前端提供换挡点，也就是无需混合a星来提供初始解。那这不就是直接革了hybrid a star的命了嘛。让我们来仔细的学习这篇文章，看一看到底是怎么回事。后续要结合开源代码看一下实际效果。

这篇论文提出了一个面向差速驱动机器人（包括轮式与履带式）的统一轨迹优化框架。作者基于瞬时旋转中心（ICR）建立通用运动学模型，并用运动状态表示法（在姿态与弧长空间构造多段多项式轨迹）使轨迹天然满足连续性和运动学约束。在约束处理上，论文创新性地结合了惩罚函数（处理速度、加速度、安全等不等式约束）与增广拉格朗日方法（仅处理终点位置等式约束），既保证了精度又提高了数值稳定性。

算法的示意图如下，非常有意思的研究，其中的一些处理技巧，可以在自己的工作中灵活应用，建议收藏和点赞，有空再看！

1.轨迹表示

在本节中，我们重点介绍一种基于运动状态多项式参数化的全新轨迹表示方法。

A. 差分驱动机器人的运动学模型

绿色点：表示机器人机体的瞬时旋转中心（ICR of body）。

蓝色点：表示左右驱动轮或履带与地面的接触点的 ICR。

机体坐标系：

原点在几何中心（两轮中点或履带几何中心）。

x 轴：与机器人前进方向对齐。

y 轴：与车体横向方向对齐。

我们采用 瞬时旋转中心（Instantaneous Centers of Rotation, ICRs） [1] 来描述差分驱动机器人的运动，如图 3 所示。通过引入 ICR，可以方便地将机器人几何中心定义为机体坐标系的原点，并使 x 轴与机器人朝向保持一致。此时，机器人的瞬时平移与旋转速度可以表示为：

：机器人机体的角速度（绕 z 轴）。

：右轮（或右履带）、左轮（或左履带）的线速度。

：左右驱动轮（或履带）的瞬时旋转中心在机体坐标系 y 轴上的位置。

含义：角速度由左右轮速差决定，并与轮距成反比。

：机体坐标系下的纵向速度（沿 x 轴）。

：左右轮速度的平均值，表示机器人前进/后退的主体速度。

：对应角速度 。

：左右轮 y 坐标的平均值。

含义：纵向速度不仅取决于左右轮速度的平均值，还受到旋转引起的几何中心偏移的影响。

：机体坐标系下的横向速度（沿 y 轴）。

对于标准两轮差速车（SDD）， ，因为几何中心始终在两轮连线的中点，不存在横向滑移。

对于滑移转向或履带机器人（SKDD/TDD），由于接触点与几何中心存在偏差， 。

：机体 ICR 的 x 坐标。

：表明横向速度与旋转半径和角速度有关。

对于 SDD 机器人，其车轮的 ICR 位于与地面的接触点，满足 ，其中 为轮距，且不存在横向滑移。机体坐标系的原点位于两驱动轮的中点。为了保证方法的普适性，我们在不同驱动机制下均采用式 (1)–(3) 进行轨迹计算。

B. 运动状态轨迹

在移动机器人规划中，常见的做法是直接在平面坐标 上规划轨迹。但这样对差速车、履带车等 非完整约束 的机器人来说会遇到几个问题：

一旦轨迹包含前进/后退切换，轨迹在 空间会出现奇异点（不可导、跳变）。

控制器在跟踪这些轨迹时会不稳定。

解决思路：换一种表达方式。我们不直接画 曲线，而是先在 朝向角 和 前进弧长 空间画轨迹，这样轨迹天然光滑、连续，再通过积分还原出 。

这种方法被称为 运动状态轨迹 (Motion State Trajectory, MS Trajectory)。

(1) 如何表示一条轨迹？

我们把轨迹分成多段，每一段的朝向角和前进弧长，都用 时间多项式 来表示：

这里：

：表示机器人朝向角随时间的变化。

：表示机器人在前进方向上的累计弧长。

：多项式基底向量。

：系数向量，决定曲线形状。

重要导数：

：前向速度。

：角速度。 也就是说，轨迹一旦在 空间被描述，就自动包含了速度和角速度信息。

(2) 如何还原平面位置？

虽然轨迹在 空间更好，但最终我们还是要得到 以便检查避障和到达目标点。 机器人在平面中的坐标由积分得到：

解释：

：起点坐标。

：表示正常前进的位移分量。

：车体几何中心和瞬时旋转中心 (ICR) 的偏移量，反映了横滑效应。

当机器人转弯时，如果有横滑，就会额外产生位移，这由 项体现。

(3) 如何计算积分？

直接算上面的积分很难，所以引入 Simpson 公式 来近似：

通俗的解释：

在区间的左端点、中点、右端点各取一个采样值。

用权重 加权平均，就能高精度近似积分。

这种方法既快又准，非常适合轨迹优化中实时计算。

(4) 整条轨迹的数值积分

先分段：整条轨迹切成 段，方便用多项式“拼”出复杂曲线（像用多段样条曲线）。

再分小区间：每段再切成 个小区间。积分就是“面积求和”，我们要算

每个小区间只看三个点：左端、中点、右端，函数值按权重 相加，再乘上小区间长度的 ，就得到这一小块的“面积近似”。

把所有小块加起来：得到该段的近似位移 ；最后把所有段的 累加，再加起点 ，得到整条轨迹末端的 。

为什么可行：Simpson 对平滑函数很准，且误差随 衰减（在式(9)里给出误差上界），所以用少量采样点就能得到高精度的积分，计算量小，适合优化与实时。

一条轨迹通常分为 段，每段再分成 个小区间。对每个小区间应用 Simpson 公式，就能近似整段积分。 整体：

每段的复合 Simpson 近似：

每个小区间的 integrand（被积函数在采样点的值）：

采样时刻（每段时间网格）：

：分段数。整条轨迹被分成 段，每段单独用多项式描述，便于形状控制和数值稳定。

：段索引，指向第 段。

：第 段的持续时间（长度）。不同段可以长短不同。

：每段再切分的小区间数（复合 Simpson 的“复合”来自这里）。每个小区间里用一次 Simpson 三点公式。

：第 段内部的小区间索引。

：小区间内三类采样点的编号，分别代表左端点、中点、右端点。

：第 段第 个小区间内第 个采样点的时间位置。

左端点 ，

中点 ，

右端点 。

等价理解：把第 段的时间 均匀切成 段，节点为 ；则

：起点的 x 坐标。

：用数值近似得到的终点 x 坐标（或某一时刻的累计 x 位移），这里是“近似”的意思，用 表示。

：第 段对 x 位移的近似贡献。依赖于该段的多项式系数 （包括 和 两组）以及段时长 。

：被积函数 在采样时刻 的数值（就是“瞬时 x 方向位移密度”）：

：前向线速度（车体坐标系 x 轴方向的速度），也就是 对时间的导数。

、 ：朝向角及其角速度（第 段的多项式及其导数求得）。

：ICR 偏移/横滑参数。若平台存在横滑或几何中心不在瞬时旋转中心上，这一项就不为 0，它会让角速度对 x 方向位移也产生贡献。

(5) 为什么 空间更好？

如果直接在 空间表示轨迹，当机器人前进一段、再倒车时，曲线会出现奇异点（轨迹不再光滑）。换到 空间后：

前进/后退只需要改变 的正负。

整条轨迹依旧是光滑曲线。 这也是论文中图4所展示的核心对比：

红点：在 空间出现的奇异点。

平滑曲线：在 空间的自然表示。

2. 轨迹优化问题

接下来让我们基于 MS 轨迹表示 构建轨迹优化问题。 先看一下最终的目标函数：

将目标展开是这样的：

下边我们开始逐步讲解优化问题如何得到的。

A. 轨迹优化问题的形式化

基于前面提出的新型轨迹表示，我们构造优化问题如下：

：将所有分段多项式系数纵向拼接后的系数向量（见式(11)）。

：各段时长。

：对 与 的 第 阶导（若 就是 jerk）。

：对角权重矩阵（分配角/弧长方向的平滑权重）。

：时间正则权重（鼓励短时长）。

含义：让高阶导数小（轨迹更平滑），同时抑制时长过大。

约束条件为：

(1) 初始条件：

这里就相当于在笛卡尔系时，我们指定起点的位置，加速度等起始条件。这里用的是朝向 和弧长 .

：从 0 阶到 阶导数组成的列向量：

。

：起点处上述各量的给定值（其中 ）。

(2) 终止条件：

类比在笛卡尔系规划时，我们指定的终止条件如终点的位置，速度，加速度，曲率等。这里用的是角度，角速度，角加速度，以及弧长，纵向速度，纵向加速度。

： 的 0～ 阶导数组合。

：从 1 阶到 阶的导数组合（不含 0 阶位置）。

含义：最终角度及其导数被固定； 的速度/加速度等被固定，但 （终止弧长）不固定，这为后续自动确定“行驶距离 / 前后切换”留下自由度。

(3) 最终位置：

：由 数值积分（Simpson，见前文(8)）从 推到笛卡尔得到的近似终点。

：目标终点。

含义：轨迹的积分终点要落在目标点（后文(22)–(25)用增广拉格朗日迭代逼近）。

(4) 分段连续性：

整条轨迹被划分为 段，每一段由一个 阶的二维多项式表示。因此，每段之间需要一个等式约束。这里就类似于分段贝塞尔曲线了。

要求相邻段在 0～ 阶全部连续（位置、速度、加速度…）。

(5) 时间：

轨迹执行时间许越小越好，因此这里把时间总和也作为代价。

含义：时长为正且可分配。

(6) 约束集合：

：具体约束（如速度/加速度界、运动学界、安全距离等）。

(7) 采用ΣMINCO轨迹类来表示轨迹

为了解决该优化问题，我们采用 ΣMINCO [25] 轨迹类 来表示轨迹，并将优化变量进行变换。通过 ΣMINCO，可以把多项式系数 唯一地表示为：

：所有分段多项式系数拼接向量（优化变量之一）。

：由时长 决定的可逆带状矩阵（参考[25]），把“边界/拼接点状态”与“系数”一一映射。

：第 段末端状态（中间点）的某种拼接形式；其后的 “ ” 表示按 ΣMINCO 约定在向量中补零以成固定维度。

然而，目标终点一般是用笛卡尔坐标 表示，而不是终止弧长 。 因此式 (11) 中的最终状态 并不是固定的，特别是 并不预先给定，而应作为优化变量的一部分。 这意味着轨迹的行驶距离在优化前是未知的。 因此需要考虑代价函数 对 的梯度。假设 已经通过 ΣMINCO 的方法求得，则有：

其中 是单位矩阵 的第 列。 本文选择 ，以最小化线性与角加加速度（jerk），从而生成平滑轨迹。

B. 不等式约束与梯度传播

对于目标函数 (10a)，其梯度可以直接计算。 由于时间变量 要求严格为正 (10f)，我们将 转换为无约束变量 ，通过如下光滑双射：

对于不等式约束 (10g)，我们采用惩罚方法，结合一阶松弛函数 [26]。 通过对轨迹上的采样点进行约束，可以确保整个轨迹满足约束：

：在 第 段、第 个采样点 上的约束值。

：第 段的多项式系数。

：基函数。 是其高阶导数。

：采样点的位置信息（通过数值积分得到，不是直接由多项式表达的）。

罚项构造：

：约束 的权重（控制罚项的力度）。

：梯形积分的系数。

用它可以把区间积分近似为采样点加权求和。

：一阶松弛罚函数（smooth penalty function）。

当 （满足约束）时， 。

当 （违反约束）时， ，随违反程度增加。

：整个轨迹在约束 下的总罚项。

其中， 是约束 的权重，

为梯形积分系数。

与积分区间数保持一致，避免过计算。

于是，优化问题可以重新写为：

：原始目标（平滑 + 时间）。

：所有约束的罚项。

优化变量：

：轨迹中间状态，

：时间变量的无约束形式，

：末端弧长。

梯度为什么重要？

公式（15）是一个 无约束非线性优化问题，本质上靠数值迭代解。在本文中:

等式约束（终点、段间连续性）：用 PHR-ALM 增广拉格朗日，逐步修正，确保严格满足。

不等式约束（避障、速度等）：用 罚函数法，把它们变成目标函数的附加项。 处理完之后，优化问题就变成了一个 光滑的无约束优化问题。最后的无约束优化问题是用 L-BFGS（Limited-memory BFGS）来解的。因为 L-BFGS 是一个 梯度型方法,所以我们要准确计算相应的梯度。涉及到的梯度如下表：

梯度类型

对应公式

数学对象

物理意义

约束对系数 的梯度

(16)

如果改变多项式系数（轨迹形状），约束值（如避障距离）会怎样变化；告诉优化器应如何调整轨迹形状来满足约束。

约束对时间 的梯度

(17)

如果某段轨迹的时间长短调整，约束会怎样变化；例如加快/减慢速度是否有助于避障。

罚项对系数 的梯度

(18)

综合所有采样点，整体衡量轨迹形状改变对约束罚分的影响。

罚项对时间 的梯度

(19)

综合所有采样点，衡量时间分配改变对约束罚分的影响。

积分位置对系数的梯度

(20)

因为 是积分结果，早期段的系数会影响后续位置；描述这种连锁效应如何传递到约束。

积分位置对时间的梯度

(21)

早期段的时间分配改变，会影响后续点位置；描述时间变化如何影响整条轨迹的位置和约束。

下表开始讨论梯度的计算公式。

(1) 约束对系数 的梯度

：某个不等式约束函数。

：多项式系数向量。

：状态的 阶导数。

：位置信息（通过积分得到）。

通过链式法则，把约束对 的梯度拆成三部分：

对状态的直接依赖；

对 的间接依赖；

对 的间接依赖。

(2) 约束对时间 的梯度

类似 (16)，只不过这里是约束对时间 的导数。 因为 会影响采样点位置和积分结果，所以必须单独考虑。

(3) 罚项 对系数 的梯度

：约束权重。

：梯形积分权重 。

：松弛罚函数。

罚项对 的梯度就是所有采样点梯度的加权和。

(4) 罚项 对时间 的梯度

：单位向量，确保只作用在对应的 上。

罚项对 的梯度有两部分：

直接来自 ；

来自 对 的依赖。

(5) 对系数 的梯度

：第 段第 个采样点的 坐标。

：第 段的多项式系数。

：Simpson 加权和。

：约束函数对 的敏感度。

来自积分，所以它不仅依赖本段，还依赖所有前面段的系数。

(6) 对时间 的梯度

与 (20) 类似，不过这里是 对时间的梯度。 它表明 的值会受前面所有段的时间分配影响。

C. 终点位置约束

与基于微分平坦性的轨迹不同，MS 轨迹的最终位置是通过积分近似计算得到的，因此可能与期望目标 存在偏差。 为减少这种误差，需要专门引入终点位置约束。

以 轴为例：

通常允许存在最大误差 。

虽然可以通过增大惩罚权重来提升精度，但这可能会改变轨迹拓扑，或导致收敛速度下降。

如果仅使用惩罚函数方法，优化结果可能达不到精度要求。

因此，作者采用 Powell-Hestenes-Rockafellar 增广拉格朗日方法 (PHR-ALM) 来迭代减少误差。 定义新的目标函数：

其中， 为对偶变量， 为增广项权重。

更新方式如下：

其中 确保 单调递增， 用于避免过大。 优化问题由 L-BFGS求解，迭代执行式 (24)，直到满足：

即保证最终位置误差在容许范围内。

公式 (23) 是本文中最终的轨迹优化目标函数，它采用 Powell-Hestenes-Rockafellar 增广拉格朗日方法 (PHR-ALM) 来处理轨迹的最终位置约束。

该公式将需要精确满足的等式约束（最终位置）与所有通过惩罚函数方法处理的不等式约束统一在一个迭代可解的增广目标函数中。

项

含义 (中文)

含义 (英文/数学)

作用与来源

增广拉格朗日目标函数

Augmented Lagrangian Objective Function

这是 ALM 迭代优化中的最小化目标函数。优化器（如 L-BFGS）在每次迭代中求解最小化该函数的问题。

优化变量

Optimization Variables

轨迹优化的变量集： 是中间点的集合； 是段持续时间 经过平滑双射后的无约束变量； 是最终的前向弧长，它是优化过程中必须确定的变量。

对偶变量/拉格朗日乘子

Dual Variable / Lagrange Multiplier

用于最终位置等式约束的对偶变量。在 ALM 迭代的外层循环中根据约束违反程度进行更新 (Eq. 24)。

基础无约束目标函数

Unconstrained Optimization Problem (

)

这是排除了最终位置约束，并将所有不等式约束（集合 ）通过惩罚函数方法转换后的目标函数。 包含：二次控制代价和时间最小化 ( )，以及所有不等式约束的惩罚项 ( )。

求和项

Summation

对笛卡尔坐标系的 轴和 轴的最终位置约束进行求和。

增广项的系数

最终位置约束函数

Final Position Constraint Function

表示最终位置的等式约束 [30d, 41]。具体地，它定义了轨迹的最终近似位置 ( 或 ) 与期望目标位置 ( 或 ) 之间的误差：

对偶变量修正项

Dual Variable Correction Term

这是 ALM 区别于简单二次惩罚的关键部分，用于修正惩罚项的中心，以提高收敛速度和准确性。

增广拉格朗日项

统一约束的机制

公式 (23) 体现了优化框架对不同类型约束的分层处理策略：

内层惩罚 ： 所有的不等式约束（如耦合速度、加速度、安全距离等）都通过惩罚函数 转化为目标函数的一部分，确保轨迹在优化过程中具备可行性和安全性。

外层增广 ： 对于要求高精度的等式约束（即最终位置 ），则使用更强大的 ALM 框架。ALM 在迭代中不断优化轨迹参数 ，使得包含所有惩罚项的 最小化，同时严格满足最终位置约束。这种方法避免了在处理等式约束时简单惩罚函数可能导致的拓扑改变或收敛速度慢的问题。

附件：问题列表 Q1.为什么要用 ， ？

这里对于标准的二轮差分底盘，没有横向滑移， ，即两轮之间的距离，但如果是履带式的，当机器人转弯时，几何中心不再严格处于左右接触点连线的中点，而是发生偏移， 。- 在公式 (1)(2)(3) 里不是单纯几何常量，而是 动力学/运动学模型中的等效参数。这就是为什么论文里要用 EKF 来在线估计 ICR 参数。)

Q2. iLQR / MPC 的线性化 vs Simpson 数值积分

(1) iLQR 的线性化

对象：非线性动力学

方法：在参考轨迹附近做泰勒展开，一阶近似

目的：把非线性 OCP 转化为 LQR 子问题，通过迭代求得全局近似最优解。

特点：迭代优化，全局一致性，计算量大。

(2) MPC 的逐步线性化

对象：非线性动力学

方法：在每个滚动时域的当前状态处线性化，只解一次优化问题。

目的：得到实时可用的控制输入，保证 MPC 能在强约束下运行。

特点：不迭代，每步线性化一次，实时性好但最优性弱。

(3) Simpson 数值积分

对象：积分算子，例如轨迹坐标转换

方法：用 Simpson 公式近似积分

目的：在轨迹优化中高效、可微地计算积分值。

特点：数值计算近似，不是模型近似；误差阶次 。

(4) 总结

iLQR/MPC 的线性化 → 针对动力学模型，是“模型近似”，核心目的是 让优化问题可解。

Simpson 积分 → 针对积分运算，是“计算近似”，核心目的是 让积分算得快而准。

方法

面向对象

技术手段

目的

iLQR 线性化

非线性动力学

泰勒展开 (一阶/二阶)

降低优化难度，迭代求近似最优解

MPC 线性化

非线性动力学

每步局部线性化

实时优化，快速可行解

Simpson 积分

积分算子

数值近似（插值求积）

高效计算积分，便于轨迹约束

Q3.前进切换到后退的换挡点，需要依赖初始解吗？

这个特性感觉有点厉害了，类似这种换挡的轨迹，不知道需不需要要依赖初始解，如果它能够自动找到最优换挡点，那就太厉害了。 MS 轨迹具有显著优势，它能够避免由于非完整约束动力学所引起的奇异性。这意味着，一条统一的轨迹就可以同时包含前进和后退两种运动，而不需要由前端规划器显式指定。 换挡点是优化的自由度，优化器会在代价函数（安全性、平滑性、能耗等）和约束条件（动力学、障碍物）下，自动决定最优的切换时机和位置。 这个问题的答案需要在代码里进行确认，之后公布。

Q4.轨迹优化中目标函数都有哪些构成

(A) 最终目标：PHR-ALM 形式

(B) 惩罚基底目标 (C) 平滑度 + 时间正则 (D) 不等式约束惩罚项

采样点约束函数：

(E) 终点位置残差 (F) 末端位置积分（辛普森法）

被积函数：

(G) 多项式系数 (c) 的线性关系

因此 。

(H) 时间正性（平滑双射） (I) 末端弧长 的梯度传导 (J) 总览：所有子项回收至最终式

(1) 第一大块：

： 表示轨迹的 平滑度代价（高阶导数的加权积分） + 时间代价。

： 表示所有 不等式约束（速度、加速度、安全距离、分段时长平衡）在采样点上计算的 惩罚函数项。

因此， 是“平滑 + 时间 + 不等式约束惩罚”的 惩罚基底目标。

(2) 第二大块：ALM 增广项

：表示轨迹数值积分得到的终点位置 与目标位置 的差值。

： 分别是拉格朗日乘子与罚参数。

通过 ALM 增广形式，把“终点位置必须精确到目标点”这一 等式约束 纳入目标函数，保证迭代收敛到高精度。

(3) 最终目标函数总结

Q5.ALM 外层更新中 有没有参与 (1) 最终目标函数

： 平滑度 + 时间 + 不等式约束惩罚

增广项： 针对终点位置等式约束

(2) 外层更新规则

固定 ，最小化内层目标

更新拉格朗日乘子

更新罚参数

收敛条件

(3) ₛ 的参与方式 a) 参与内层最小化

每次优化时， 和 ALM 增广项一起构成目标函数。

它确保轨迹光滑并满足不等式约束。

b) 不参与外层更新

外层更新 仅依赖于 终点位置残差 。

与 无关。

Q6.为什么避障约束没有作为ALM项，而是单纯用罚函数呢

这个论文里没有提到，暂时不清楚原因。以下是推测的原因。

避障约束之所以没有采用增强拉格朗日方法（Augmented Lagrangian Methods, ALM），而是使用单纯的罚函数，主要是因为这两种方法针对的目标约束类型和所需的精度要求有所不同。

(1) 避障约束的处理方式（罚函数）

在所提出的通用轨迹优化框架中，避障约束（即安全约束）属于一般的不等式约束集合 中的一项。

定义和目的： 安全约束 是通过确保机器人轮廓点 在世界坐标系中的位置 处的欧几里德有符号距离场（ESDF）值大于安全距离 来防止碰撞的。

实现方法： 对于所有不等式约束（包括速度、加速度、分段时长平衡以及安全约束） ，优化框架采用了罚函数方法，并结合一阶松弛函数 。

离散化： 为了近似罚函数的值并确保整个轨迹满足约束，采用了离散采样点的方法进行约束。罚函数 被表达为对轨迹分段和采样点上约束函数值 的加权和（使用梯形法则的系数 ）：

这种罚函数方法是一种常见且有效的处理非线性不等式约束（如避障）的方式，它将约束项添加到目标函数 中，构成最终的优化问题 。

(2) 终点位置约束的处理方式（ALM）

与避障约束不同，终点位置约束（确保轨迹最终位置 到达期望终点 ）被明确地设计为使用增强拉格朗日方法（PHR-ALM）。

约束的特殊性： 终点位置约束 是一种等式约束（虽然在实际中表现为要求误差小于 的近似）。

使用 ALM 的原因： 文献指出，如果仅使用罚函数方法来处理终点位置约束，可能会有以下问题：

可能会改变轨迹的拓扑结构。

可能会减慢收敛速度。

优化结果可能无法满足所需的精度（最大误差 ）。

Q7.换挡点需要初始解提供吗？

根据提供的文献，该方法的核心在于其创新的运动状态（Motion State, MS）轨迹表示方法，它使得优化器能够自主地确定这些换挡点，而不需要初始解的预先指定。

(1) 换挡点的优化机制（自主确定）

该框架通过运动状态（MS）轨迹表示方法来建模前进和后退运动。

轨迹表示基础： MS 轨迹不是直接在笛卡尔坐标系 中定义的，而是通过方向 和前进弧长 的多项式参数化来表示的。

速度与方向： 机器人的线速度 （沿机器人本体坐标系 x 轴的速度）即为前进弧长 对时间的导数 。

当 时，机器人前进。

当 时，机器人后退。

当 时，即为前进/后退的切换点（换挡点）。

避免奇异性实现平滑过渡： 这种基于运动状态的参数化具有显著的优点，即避免了非完整动力学引起的奇异性。在传统的基于微分平坦性（Differential Flatness, DF）的方法中，当运动方向改变时（即线速度 接近零时），状态表达式中会出现奇异点，这使得连续轨迹中建模前进和后退运动非常困难。

优化过程： 在 MS 轨迹中，前进和后退的改变被表示为 空间中的一条平滑轨迹。优化器通过调整多项式系数 和分段时长 ，使得目标函数（包括平滑度 J0 和各种约束）最小化。由于 是优化变量的线性组合，优化器能够自动、平滑地找到 改变符号的点，从而确定最优的换挡时刻和位置。该框架明确要求“不同方向的运动应该被建模为一条连续轨迹，包括前进和后退运动”。

(2) 初始解是否需要提供换挡点

不需要。 这是 MS 轨迹相对于其他方法的关键优势之一。

与微分平坦性方法的对比： 文献指出，基于微分平坦性的方法（DF）需要依赖前端方法（如 Hybrid A*）来提供初始路径，以预先指定是前进还是后退运动。

MS 轨迹的通用性： 相比之下，MS 轨迹表示方法由于避免了奇异性，因此“一个统一的轨迹可以同时处理前进和后退运动，而不是由规划前端指定”。

尽管规划系统使用了 Jump Point Search (JPS) 来提供全局路径作为前端，以及使用了轨迹预处理，但这些步骤的目的是：

避免初始值导致的拓扑结构变化。

确保初始轨迹的积分终点更接近期望的终点。

生成更接近全局路径且更具动态可行性的初始轨迹。

预处理阶段主要关注位置和方向的平滑过渡，并满足运动学和加速度约束。由于 MS 轨迹本身固有的平滑性和对运动状态的直接参数化，它能够在优化过程中自主地决定最优的换向时机，无需前端路径预先指定运动方向。

未完待续。。。 内容太多了，今天就先学到这里，下一篇继续。