Part II · 第 9 章 机器人形态 · 9.1

移动机器人与非完整约束

一辆汽车的位形有三个自由度 \((x,y,\theta)\)，控制量却只有两个：油门和方向盘，而且它不能横着滑。可是它为什么还能侧方位停车？这一节讲清楚轮式移动机器人的运动学：差速、阿克曼（自行车）和全向底盘的模型，"滚动而不打滑"这条非完整约束，李括号怎样把"前进—转向—后退—回转"变成一次横移，Dubins 最短路径，以及在这样的约束下怎样跟踪路径、怎样停到指定的位姿。

约 35 分钟 前置：5.1 坐标系与位姿 · 2.5 常微分方程 · 3.6 向量场与流 · 5.5 李代数 · 7.1 自由度 · 7.2 构型空间 6 个交互

    1为什么机器人需要它

    前面几节里的机械臂有一个很"诚实"的性质：每个关节配一个电机，关节速度可以任意给定，速度空间的维数就是构型空间的维数（7.1、7.2 节）。想让手爪往哪个方向动，只要解一个雅可比方程（3.4 节）。

    轮式移动机器人把这种对应关系打破了。扫地机器人、仓储 AMR、自动驾驶汽车的位形都是平面上的位姿 \(q=(x,y,\theta)\in SE(2)\)（5.1 节），三个自由度；可是它只有两个执行器：左右轮，或者"油门 + 方向盘"。更要紧的是，轮子只能沿着自己面对的方向滚动，不能侧滑。这样一来，在任何一个时刻机器人都只能朝两个方向运动，却依然能够开到平面上任意一个位置、朝向任意一个方向。

    这是"自由度"和"可控方向"第一次分道扬镳。它带来一串很具体的工程问题：

    • 建模：差速、阿克曼、全向底盘各有什么运动学方程？瞬时转动中心在哪？
    • 可达性：不能横移，到底怎样到达侧面一点？要付出多少代价？
    • 规划：两个位姿之间的最短路径是什么形状（Dubins、Reeds–Shepp）？为什么栅格上的 A* 路径不能直接执行？
    • 控制：怎样跟踪一条路径？怎样"停进车位"？为什么镇定到一个位姿比跟踪一条路径更难？
    • 里程计：只靠轮子编码器积分位姿，误差怎么累积？

    本节用 SE(2) 上的向量场和流（3.6、5.5 节）把这些问题串在一起。

    2直觉：一个轮子只会往前滚

    先看最常见的差速驱动：两个同轴的驱动轮，分别由电机独立驱动，再加一个（或两个）不驱动的万向脚轮保持平衡。每个驱动轮接地点的速度只能沿轮子朝向，也就是垂直于车轴。一个刚体上两点的速度如果都垂直于车轴，那么整个刚体在这一瞬间是在绕某一点转动，这个点就是瞬时转动中心（ICR），而它必须落在车轴所在的直线上：因为每个轮子的速度方向垂直于"它到 ICR 的连线"，所以 ICR 必在两个轮子各自的"垂线"，也就是车轴上。

    两个轮子速度相等，ICR 在无穷远，机器人走直线；两个轮子大小相等方向相反，ICR 在车轴中点，机器人原地转；一个轮子停住，ICR 就在这个轮子上，机器人绕它"画圆"。下面的图让你直接推动两个轮子。把模式切到"全向"，你会看到相反的情形：ICR 可以跑到车轴以外的任何地方，机器人可以直接侧移。

    FIG 1开一台差速机器人
    琥珀色粗箭头是机器人中心的速度，青色箭头是两个轮子的速度（都沿轮子朝向）。紫色点是 ICR，虚线圆是机器人绕它走的圆，灰色长虚线是车轴所在直线。红、绿箭头是机器人坐标系的 \(x\)、\(y\) 轴。轮径 \(r=0.1\) m，轮距 \(W=0.5\) m，轮速限幅 ±10 rad/s。打开"里程计"后，紫色虚线机器人是只用编码器积分得到的估计位姿：右轮真实半径比标称值大 \(e\%\)，编码器还带随机噪声。
    试一试
    • 让左轮停住、右轮转动（左轮速度接近 0，右轮转速大于 3 rad/s）。ICR 在哪里？半径是多少？（可以点"绕一个轮子转"预设。）
    • 切换到"全向"，让机器人纯侧移（\(v_y>0.5\)，\(v_x\) 与 \(\omega\) 接近 0）。差速机器人为什么做不到？
    • 回到差速模式，打开"里程计"，把右轮半径误差设成 3% 左右，让机器人大致走直线：估计位姿与真实位姿相差超过 0.3 m 需要走多远？
    核心洞见

    差速机器人的每一时刻只有一个"自由度对"：线速度 \(v\) 和角速度 \(\omega\)，二者合起来决定 ICR 落在车轴上的哪个位置：\(R=v/\omega\)。机器人沿车头方向的速度 \(v\) 可以随便给，但沿车身侧向的速度恒为零。这条"侧向速度为零"的规则就是本节的主角。

    3动手玩：不能横着滑，却到得了

    先自己试一试那条规则。下图里拖动小车：不管你把鼠标拉向哪里，小车只肯沿着车头所在的直线移动；你拉的横向分量被"禁止"（红色箭头），因为 \(\dot x\sin\theta-\dot y\cos\theta=0\)。

    然后是反转。侧方位停车的秘诀是一个小小的摆动（下图用能原地转向的差速小车演示；汽车做的是同一件事，只是用满舵的圆弧代替原地转向）：前进 \(\varepsilon\)、左转 \(\varepsilon\)、后退 \(\varepsilon\)、右转 \(\varepsilon\)。四个动作每一个都完全合规，车朝向回到原来的方向，位置却向右挪了一点点：横向位移约为 \(\varepsilon^2\)。把 \(\varepsilon\) 减半，位移变成四分之一；但每次摆动的"工作量"只有一半，所以要横移 \(d\)，所需的行驶距离只和 \(\sqrt d\) 成正比。反复摆动几次，把车停进右下角的车位，调节 \(\varepsilon\) 看看效率如何变化。

    FIG 2禁止横移与"摆动"停车
    横移距离 d (m) vs ε（双对数）
    左图：拖动差速小车（车体坐标原点在驱动轮轴中点）。琥珀色是摆动轨迹，每次摆动结束时，琥珀色箭头是实际净位移，紫色箭头是李括号的预测 \(\pm\varepsilon^2[g_1,g_2]\)，二者几乎重合。绿色虚线框是车位。右图：把摆动在积分器里真的跑一遍，得到横移距离随 \(\varepsilon\) 的变化（琥珀色），紫色虚线是斜率为 2 的参考线 \(\varepsilon^2\)；灰色是斜率为 1 的线，作为对照。
    试一试
    • 拖动小车，把鼠标向侧面拉出至少 0.5 m。小车的侧向位置变了吗？
    • 先用 \(\varepsilon\le 0.3\) 摆动一次，再用 \(\varepsilon\ge 0.6\) 摆动一次。第二次的横移是第一次的几倍？和 \(\varepsilon^2\) 的比值一致吗？
    • 用摆动（和前进/后退微调）把小车停进车位：轮轴中点进入车位中心附近，朝向偏差小于 7°。
    核心洞见

    机器人在每一时刻只能朝两个方向走（前进、转），却可以到达三维位姿空间里的任何一点。缺的那个方向（侧移）靠"在两个允许的方向之间来回摆"换来，换算率是二次的：位移 \(\propto\varepsilon^2\)。数学上这个二次项就是两个向量场的李括号。

    4第二、第三视角：底盘家族、可达集与最短路径

    四种常见底盘一张表

    底盘 控制量 速度约束 完整？ 最小转弯半径 例子
    独轮车 / 差速 \((v,\omega)\) 或 \((\omega_L,\omega_R)\) \(\dot x\sin\theta-\dot y\cos\theta=0\) 否：3 自由度，2 个控制 0（能原地转） Roomba、TurtleBot、多数仓储 AMR
    自行车 / 阿克曼 \((v,\delta)\), \(|\delta|\le\delta_{\max}\) 同上，另有转角上限 否 \(L/\tan\delta_{\max}\) 汽车、叉车、F1TENTH
    全向（麦克纳姆、全向轮、舵轮） \((v_x,v_y,\omega)\) 无 是：3 自由度，3 个控制 0 KUKA youBot、RoboCup 小型组、舵轮底盘
    履带 / 滑移转向 \((v,\omega)\) 理想情形同差速；实际轮子必须打滑才能转 近似否 0 Clearpath Husky、拆弹机器人

    全向底盘没有速度约束，可以向任意方向直接开，是"完整"系统，规划起来和一个能在平面上任意移动的点差不多；代价是轮子结构复杂、承载和越障能力差。差速和阿克曼底盘结构简单、可靠，却要面对非完整约束，这也是本节后半部分的内容。

    能到哪里：可达集

    三种机器人从原点出发、朝向 +x，最高线速度都是 1 m/s。在"路径长度（时间）不超过 \(T\)"的条件下，它们能到达的位置形成不同的形状：

    • 全向（青色）：半径为 \(T\) 的圆盘。
    • 差速（紫色）：两个轮速都不超过 1 m/s，所以 \(|v|+\tfrac W2|\omega|\le1\)。转向要花时间，最快的办法是先原地转到对准目标（或对准反方向后倒车）再直行，到达 \((\rho,\phi)\) 的最短时间为 \(\rho+\tfrac W2\,m(\phi)\)，其中 \(m(\phi)=\min(|\phi|,\pi-|\phi|)\) 是要转的角度，图形是一朵在侧面有缺口的"花瓣"。
    • 汽车（琥珀色，只前进，最小转弯半径 \(R\)）：用采样得到。把"满舵左转/右转 + 直行/反向满舵"这些两段式路径密密地抽样，每个像素记下到达它的最短路径长度。这个形状像一滴泪：前方很饱满，侧面却有一块"空洞"，因为到达身边的点必须绕一个大圈。

    拖动目标点，对照三者的代价。把它放到车身正侧面，汽车的代价会比直线距离大好几倍。

    FIG 3三种机器人的可达集
    机器人在原点，朝向 +x（红色箭头）。琥珀色区域是汽车在路径长度 \(\le T\) 内能到达的位置（浅色细线是边界的生成者：满舵转一段再直行）；紫色花瓣是差速（\(W/2=0.25\) m）；青色圆是全向。粗琥珀线是汽车到目标点（终端朝向不限）的最短路径，由第 5 节的 Dubins 解法逐朝向暴力搜索得到。
    • 把目标放在车身正侧面（\(|x|<0.6\)，\(|y|\) 在 1 到 3 之间），让汽车的最短路径长度超过直线距离的 3 倍。减小 \(R\) 会怎样？

    指定终点朝向：Dubins 最短路径

    现实里终点往往要求朝向（进车位、对接充电桩）。只能前进、最小转弯半径为 \(R\) 的车，两个位姿之间最短的路径一定是 "圆弧—直线—圆弧"（CSC）或 "圆弧—圆弧—圆弧"（CCC）（Dubins，1957），共六种组合：LSL、RSR、LSR、RSL、RLR、LRL（L = 左转弧，R = 右转弧，S = 直线）。下图把起点和终点都做成可拖动的位姿（圆圈拖位置，箭头末端的圆圈拖朝向），程序算出六个候选并选最短的。

    FIG 4Dubins 最短路径
    青色是起点位姿，紫色是终点位姿，琥珀色是最短路径，小圆点标出弧与直线的衔接点；浅色虚线圆是起点和终点两侧的最小转弯圆。读数里的"终点误差"是把路径真的积分一遍（用同一个圆弧积分器）得到的终点位姿与目标位姿之差，应在舍入误差的量级（约 \(10^{-14}\) 或更小）。
    • 把终点拖到离起点很近的地方（比如侧前方不到 \(2R\)），让最短路径变成 RLR 或 LRL（三段全是圆弧）。直线段去哪了？

    如果允许倒车，最短路径会换成 Reeds–Shepp 曲线（Reeds & Shepp，1990），有倒车尖点（换向点）的形式；它的组合更多，但结构相同：直线和最大曲率圆弧的拼接。

    完整与非完整：珠子与冰刀

    串在一根弯铁丝上的珠子，速度必须沿着铁丝：这是一条速度约束，但它可以"积分"成位置约束（珠子只能待在铁丝上），构型空间直接降成一维曲线，约束是完整的。冰刀（或小车）的速度约束同样是"只能沿刀刃"，但无法积分成位置约束：它的位置完全自由，平面上每一点、每个朝向都能到，只是路径受限。这种约束限制的是怎么走，而不是能到哪，叫非完整约束。判据在第 5 节：看看沿允许方向"来回摆"能不能产生新的方向（李括号）。

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1、FIG 2、FIG 5、FIG 6 里对应的元素会被高亮。

    运动学模型

    独轮车是最基本的模型：位姿 \(q=(x,y,\theta)\)，控制 \(v\)（沿车头方向的速度）和 \(\omega\)。它是 SE(2) 上的"左不变"系统（5.4–5.5 节）：

    $$ \dot q=\begin{bmatrix}\dot x\\ \dot y\\ \dot\theta\end{bmatrix}=\hl{g1}{\ci{g_1(q)}}\,v+\hl{g2}{\cj{g_2(q)}}\,\omega,\qquad g_1=\begin{bmatrix}\cos\theta\\ \sin\theta\\ 0\end{bmatrix},\quad g_2=\begin{bmatrix}0\\ 0\\ 1\end{bmatrix}. $$

    差速驱动只是它的另一套坐标：轮半径 \(r\)，轮距 \(W\)，左右轮角速度 \(\omega_L,\omega_R\)，则

    $$ v=\frac{r}{2}(\omega_R+\omega_L),\quad \omega=\frac{r}{W}(\omega_R-\omega_L);\qquad \omega_R=\frac{v+\omega W/2}{r},\quad \omega_L=\frac{v-\omega W/2}{r};\qquad \hl{icr}{R=\frac{v}{\omega}}=\frac W2\,\frac{\omega_R+\omega_L}{\omega_R-\omega_L}. $$

    \(R\) 是从机器人中心沿车轴量到 ICR 的有向距离（\(R>0\) 在左边）。ICR 在世界坐标系中的位置是 \(c=p+\tfrac1\omega(-\dot y,\dot x)\)，即把速度向量逆时针转 90° 再除以 \(\omega\)。

    自行车 / 阿克曼模型：把前后两对轮各合并成一个轮子，轴距 \(L\)，参考点取后轴中点，前轮转角 \(\delta\)：

    $$ \dot x=v\cos\theta,\quad \dot y=v\sin\theta,\quad \dot\theta=\frac{v}{L}\tan\delta,\qquad R=\frac{L}{\tan\delta}\ \ge\ R_{\min}=\frac{L}{\tan\delta_{\max}}. $$

    真实汽车内外轮转角不同（内轮转得更多，\(\tan\delta_{\text{in}}=L/(R-w/2)\)，\(w\) 为轮距），使四个轮子的垂线交于同一个 ICR，这就是阿克曼转向几何。全向（麦克纳姆）底盘则没有约束：体坐标系下的速度 \((v_x,v_y,\omega)\) 可以任意给定；四个麦克纳姆轮（常见的 X 形滚子布局，\(\ell\) 为前后半轴距与左右半轮距之和）需要的转速为

    $$ \begin{bmatrix}\omega_{FL}\\ \omega_{FR}\\ \omega_{RL}\\ \omega_{RR}\end{bmatrix}=\frac1r\begin{bmatrix}1&-1&-\ell\\ 1&1&\ell\\ 1&1&-\ell\\ 1&-1&\ell\end{bmatrix}\begin{bmatrix}v_x\\ v_y\\ \omega\end{bmatrix}. $$

    （符号取决于滚子方向和轮子编号，换了安装方式矩阵里的正负号会变；列向量两两正交，所以正解是 \((J^\top J)^{-1}J^\top=r\,\operatorname{diag}(\tfrac14,\tfrac14,\tfrac1{4\ell^2})J^\top\)，\(J\) 为上面的 4×3 矩阵。）三个控制对三个自由度，\(J\) 列满秩（有左逆），所以是完整系统。

    约束：Pfaffian 形式、可积性与 Frobenius

    "轮子不侧滑"写成对速度的线性约束（Pfaffian 约束）：

    $$ A(q)\,\dot q=0,\qquad A(q)=\begin{bmatrix}\sin\theta&-\cos\theta&0\end{bmatrix},\qquad \Delta(q)=\ker A(q)=\operatorname{span}\{g_1(q),g_2(q)\}. $$

    \(\Delta(q)\) 是每一点处"允许的速度方向"，是 \(\mathbb R^3\) 里的一个二维平面（"分布"）。如果存在函数 \(\varphi(q)\) 使得 \(\mathrm d\varphi\) 与 \(A\) 成比例，那么 \(\varphi(q)=\text{const}\) 是一个曲面，机器人永远待在上面，约束是完整的，构型空间降维。Frobenius 定理说：这当且仅当分布 \(\Delta\) 对合（involutive）：\(\Delta\) 中任意两个向量场的李括号仍然在 \(\Delta\) 里。

    李括号、摆动与 ε²

    两个向量场的李括号（3.6 节的流的对易子）定义为 \([f,g]=\tfrac{\partial g}{\partial q}f-\tfrac{\partial f}{\partial q}g\)。独轮车的两个场：

    $$ \hl{bracket}{\cw{[g_1,g_2]}}=\underbrace{\tfrac{\partial g_2}{\partial q}}_{0}g_1-\frac{\partial g_1}{\partial q}\,g_2=-\begin{bmatrix}-\sin\theta\\ \cos\theta\\ 0\end{bmatrix}=\begin{bmatrix}\sin\theta\\ -\cos\theta\\ 0\end{bmatrix}\notin\Delta(q). $$

    它指向车的右侧，不在 \(\Delta\) 里，所以对合性失败，约束是非完整的。它的几何意义是流的对易子：先沿 \(g_1\) 流 \(\varepsilon\)，再沿 \(g_2\) 流 \(\varepsilon\)，再沿 \(-g_1\)，最后沿 \(-g_2\)，则

    $$ q_{\text{end}}=q_0+\varepsilon^2\,[g_1,g_2](q_0)+O(\varepsilon^3). $$

    独轮车的精确结果可以直接算：在车体坐标系下，净位移是 \((\varepsilon(1-\cos\varepsilon),\,-\varepsilon\sin\varepsilon)\approx(\tfrac{\varepsilon^3}{2},\,-\varepsilon^2)\)，朝向变化严格为零。FIG 2 右图用数值积分验证了这一点：横移 \(/\varepsilon^2\) 在 \(\varepsilon=0.0125\) 时等于 0.99997。一次摆动要前后共开 \(2\varepsilon\)，再原地来回转 \(2\varepsilon\) 弧度，取 \(\varepsilon=\sqrt d\) 时行驶距离 \(\approx2\sqrt d\)：想横移 1 cm 就要来回开约 20 cm，外加来回转约 0.2 rad（约 11°），小位移的"汇率"很差。

    可控性：Chow–Rashevskii

    把 \(g_1,g_2\) 以及它们反复取李括号得到的所有向量场放在一起，如果在每一点都张成整个 \(\mathbb R^n\)（"李代数秩条件"），则系统在连通的构型空间里可控：任意两个位姿都能用允许的运动连起来（Chow 1939，Rashevskii 1938）。独轮车：\(\operatorname{span}\{g_1,g_2,[g_1,g_2]\}=\mathbb R^3\) 处处成立（\(\det[g_1,g_2,[g_1,g_2]]=1\)），所以可以停到任何位姿，只是代价是每个"缺失方向"上平方根的路程（亚黎曼几何里的"非完整度"）。对比：一个不可积约束（冰刀）的可达集是整个构型空间，一个可积约束（珠子在铁丝上）的可达集只是一条曲线。

    Dubins 路径的闭式解

    把起点平移旋转到原点、朝向 +x，取 \(R=1\)，终点距离 \(d\)，起终点朝向相对连线方向的夹角为 \(\alpha,\beta\)。每个字都有闭式解，比如 LSL：

    $$ p=\sqrt{2+d^2-2\cos(\alpha-\beta)+2d(\sin\alpha-\sin\beta)},\qquad \varphi=\operatorname{atan2}\!\big(\cos\beta-\cos\alpha,\ d+\sin\alpha-\sin\beta\big),\qquad t=\operatorname{mod}_{2\pi}(\varphi-\alpha),\quad q=\operatorname{mod}_{2\pi}(\beta-\varphi). $$

    （\(t,q\) 是两段圆弧的转角，\(p\) 是直线段长度，总长 \(R(t+p+q)\)；这是 Shkel–Lumelsky 的归一化。）六个字里有些在几何上不存在（\(p^2<0\) 或余弦超出 \([-1,1]\)），取存在的里面最短的即可。我们对 5000 对随机的起终点位姿做了测试：把路径沿弧积分到终点，最大位置误差 \(1.8\times10^{-14}\)，最大朝向误差 \(7.5\times10^{-15}\)；另外对 150 对随机位姿，用 Newton 迭代独立地解"三段式"方程，没有找到比这六个字更短的解。

    里程计：航位推算

    一个采样周期里左右轮编码器给出行驶弧长 \(\Delta s_L,\Delta s_R\)，则 \(\Delta s=\tfrac12(\Delta s_R+\Delta s_L)\)，\(\Delta\theta=(\Delta s_R-\Delta s_L)/W\)。若周期内 \(v,\omega\) 近似不变，机器人走的是一段圆弧，精确更新是（见 2.5 节，对常值输入的 ODE 精确积分）：

    $$ \begin{bmatrix}x\\ y\\ \theta\end{bmatrix}\leftarrow\begin{bmatrix}x\\ y\\ \theta\end{bmatrix}+\begin{bmatrix}\Delta s\,\operatorname{sinc}(\Delta\theta/2)\cos(\theta+\Delta\theta/2)\\ \Delta s\,\operatorname{sinc}(\Delta\theta/2)\sin(\theta+\Delta\theta/2)\\ \Delta\theta\end{bmatrix},\qquad \operatorname{sinc}(u)=\frac{\sin u}{u}. $$

    这是弦长（\(2\tfrac{\Delta s}{\Delta\theta}\sin\tfrac{\Delta\theta}2\)）沿弦的方向走，比"用起始朝向走 \(\Delta s\)"的欧拉法准确得多。误差：朝向误差 \(\delta\theta\) 一旦出现就不会自己消失，之后每走一米，横向误差增加约 \(\delta\theta\) 米，位置误差随路程线性甚至更快增长；轮径、轮距的系统误差还会给出一个"稳定的"圆弧漂移（FIG 1 的任务 3）。编码器噪声积分后成为随机游走，协方差随路程增长（4.5 节的不确定性传播）。所以里程计只在短时间内可靠，长期定位要靠地标、激光或视觉的修正（Part IV：SLAM、定位）。

    纯追踪：一个几何控制律

    在路径上取离后轴中点距离为 \(\hl{look}{L_d}\) 的"前视点"，让车沿着一段过后轴中点、与车头相切、并通过前视点的圆弧走。设前视点在车体系下为 \((l_x,l_y)\)，与车头夹角为 \(\alpha\)，圆弧曲率由弦几何得到：圆心在 \((0,1/\kappa)\)，经过 \((l_x,l_y)\) 即 \(l_x^2+(l_y-1/\kappa)^2=1/\kappa^2\)，化简得

    $$ \kappa=\frac{2\,l_y}{l_x^2+l_y^2}=\frac{2\sin\alpha}{L_d},\qquad \delta=\arctan(L\kappa)=\arctan\!\Big(\frac{2L\sin\alpha}{L_d}\Big). $$

    \(L_d\) 小，车贴近路径但转向激烈，对转向滞后敏感，容易振荡；\(L_d\) 大，运动平顺，却会"切弯"。工程上 \(L_d\) 通常随速度增大（例如 \(L_d=k\,v\)）。

    位姿镇定：极坐标律与 Brockett 障碍

    要把独轮车开到目标位姿 \((x_g,y_g,\theta_g)\)，Siegwart 等的教材（沿用 Astolfi 的极坐标镇定思路）改用极坐标：\(\rho\) 是到目标的距离，\(\hl{alpha}{\alpha}\) 是车头到视线方向的夹角，\(\hl{beta}{\beta}\) 是视线方向到目标朝向的夹角（都取 \((-\pi,\pi]\)）：

    $$ \dot\rho=-v\cos\alpha,\quad \dot\alpha=\frac{v\sin\alpha}{\rho}-\omega,\quad \dot\beta=-\frac{v\sin\alpha}{\rho};\qquad v=k_\rho\rho,\quad \omega=k_\alpha\alpha+k_\beta\beta. $$

    代入后 \(v/\rho=k_\rho\) 是常数，\((\alpha,\beta)\) 在原点附近的线性化为 \(\dot{[\alpha,\beta]}^\top=\begin{bmatrix}-(k_\alpha-k_\rho)&-k_\beta\\-k_\rho&0\end{bmatrix}[\alpha,\beta]^\top\)，特征多项式 \(\lambda^2+(k_\alpha-k_\rho)\lambda-k_\rho k_\beta\)，所以稳定条件为

    $$ k_\rho>0,\qquad k_\beta<0,\qquad k_\alpha-k_\rho>0. $$

    经典取值 \(k_\rho=3,\ k_\alpha=8,\ k_\beta=-1.5\)（Siegwart 等教材）。当目标在车身后方时（\(|\alpha|>\pi/2\)），把"车头"换成车尾，倒车驶向目标。我们在节点脚本里对多组不同的起终点位姿（包括目标在车后的倒车情形，也包括速度、角速度限幅）做了数值验证，终点位置误差趋于 0，朝向误差收敛到 \(10^{-7}\) 以下；FIG 6 就用这个律。

    注意：这个律在目标处 \(\rho=0\) 无定义，\(\omega\) 在 \(\rho\to0\) 时并不趋于 0，所以在原来的 \((x,y,\theta)\) 坐标下它是不连续的。这不是缺陷，而是必然：Brockett 定理（1983）说，系统 \(\dot q=f(q,u)\) 要想被某个连续的、与时间无关的反馈 \(u=k(q)\) 渐近镇定到 \(q=0\)，映射 \((q,u)\mapsto f(q,u)\) 的像必须包含 \(0\) 的一个邻域。独轮车每一点的可用速度只有二维平面 \(\Delta(q)\)，永远盖不住"侧向"一小点速度（比如 \((0,\epsilon,0)\)），所以任何光滑、定常的反馈都镇定不了它，这和它是否可控无关（它是可控的）。出路只有三条：不连续反馈（上面的极坐标律、滑模）、时变反馈、或者干脆先规划一条满足约束的路径（Dubins、MPC 等）再去跟踪：跟踪一条可行轨迹并没有障碍。

    6机器人中的它：跟踪路径，停到位姿

    移动机器人的导航栈通常分两层：全局规划给出一条满足（或近似满足）运动学约束的路径，局部控制器以几十赫兹的频率把它变成速度指令。下面两个图是这两种最常见的局部任务。

    跟踪路径：纯追踪 + 自行车模型

    车是自行车模型（轴距 \(L=0.6\) m，转角限幅 \(\delta_{\max}\)），转向机构还有一阶滞后 \(\tau_s\)（真实的舵机和转向系统都不是瞬时的）。控制器是第 5 节的纯追踪。试试不同的路径与参数：小的 \(L_d\) 贴路径但容易在转向滞后下振荡；大的 \(L_d\) 平顺，却会切掉急弯；转角限幅让车根本转不过去，这时是物理极限在说话，控制器无能为力。

    FIG 5纯追踪路径跟踪
    横向误差 e (m) vs 时间 (s)
    紫色粗线是参考路径，琥珀色是车的轨迹。青色虚线圆是前视圆（半径 \(L_d\)），紫色菱形是前视点，紫色虚线弧是纯追踪此刻想走的圆弧，红色短线是横向误差。右图是横向误差（左偏为正）随时间的曲线。"圆"路径会无限绕圈，其余路径走完即止。
    • 让车振荡：转向滞后 \(\tau_s\ge0.25\) s，前视距离 \(L_d\le0.55\) m，横向误差反复穿过 0 且幅度超过 0.2 m。
    • 选"直角弯"，取 \(L_d\ge1.5\) m，让车切弯：走完全程后最大横向误差超过 0.3 m。再把 \(L_d\) 调到 0.8 m 左右比较。

    停到位姿：极坐标律

    跟踪一条路径没有 Brockett 障碍，但"原地镇定到一个位姿"有。下图用第 5 节的极坐标律：拖动起点和终点的位姿（圆圈拖位置，箭头末端的圆圈拖朝向），点"运行"。画面里的弧线 \(\alpha\)（车头到视线）与 \(\beta\)（视线到目标朝向）就是控制器看到的误差，右图是三个误差的对数曲线：它们按指数衰减，斜率由 \(k_\rho,k_\alpha,k_\beta\) 决定。把 \(k_\beta\) 改成正数，或让 \(k_\alpha<k_\rho\)，看看会发生什么。目标放在车后面时，车会自己决定倒车。

    FIG 6位姿镇定
    ρ、|α|、|β| 的对数 vs 时间 (s)
    青色是起点位姿，紫色是目标位姿，琥珀色是机器人和轨迹。右图里琥珀色是 \(\rho\)，青色是 \(|\alpha|\)，紫色是 \(|\beta|\)（纵轴是 \(\log_{10}\)，下限 \(10^{-4}\)）。线速度限幅 1 m/s，角速度限幅 4 rad/s（与 FIG 1 的轮速限幅相当）。运行 14 s 后停止。
    • 运行并让机器人停到目标位姿：位置误差小于 1 cm，朝向误差小于 1°。试一个目标在车后方的起点。
    • 违反稳定条件（\(k_\beta>0\) 或 \(k_\alpha<k_\rho\)），运行到结束：朝向误差还剩多少？
    真实系统里

    仓储与服务机器人：亚马逊的驱动单元、Locus 等仓储 AMR 和 Roomba 基本都是差速底盘；TurtleBot 系列是教学和研究的标准平台；Clearpath Husky 一类滑移转向底盘用于户外。汽车：阿克曼底盘，规划器里最小转弯半径是硬约束；F1TENTH 是 1/10 比例的开源赛车平台。ROS 2 的 Nav2：全局规划器有 NavFn 和 Smac（二维 A*、Hybrid-A*、状态格，后两者支持 Dubins 与 Reeds–Shepp 运动模型），局部控制器有 DWB、Regulated Pure Pursuit 和 MPPI；输出是 \((v,\omega)\) 的速度指令（cmd_vel），底盘驱动再按第 5 节的公式换成轮速，ros2_control 的 diff_drive_controller 同时做里程计积分并发布 odom 到 base_link 的变换。滑移是现实：本节的运动学模型假设轮子纯滚动，低速、平地、硬轮子下成立；高速、软地面、履带与滑移转向底盘上轮子必然打滑，要换用动力学模型（轮胎侧偏刚度、摩擦圆，见 8.4 节）或者在线标定有效轮距。移动操作：把机械臂装在移动底座上（Fetch、TIAGo、Hello Robot Stretch），构型空间是 \(SE(2)\times\mathbb R^n\)：底座部分是非完整的，臂的部分是完整的，全身运动规划与控制必须同时处理这两种结构（Part IV）。

    7常见误区

    误区 1：把"自由度的个数"当成"控制量的个数"

    车有 3 个自由度，因此可以给它下达任意的 \((\dot x,\dot y,\dot\theta)\) 吗？不行：速度必须落在二维平面 \(\Delta(q)\) 里。把全向机器人的规划器（"往那个方向走"）接到差速底盘上，得到的是一条无法执行的路径。能到达的位姿是 3 维的，能立即朝着它运动的方向只有 2 维：规划时要么用满足运动学约束的"转向函数"（Dubins、Reeds–Shepp、状态格），要么先规划出一条几何路径再用平滑和速度规划使它可行。

    误区 2：直线、栅格路径和"忽略最小转弯半径"

    8 邻域栅格上的 A* 会在每一个拐点处要求车头瞬间转向，对阿克曼车辆这条路径根本不可行；而且如 FIG 3 所示，到身边一点的代价可能是直线距离的好几倍，A* 的"欧氏启发式"在这里严重低估代价。对汽车类机器人，规划（Hybrid-A*、RRT\(^*\) 配合 Dubins/Reeds–Shepp 连接）和启发式都应当把转弯半径考虑进去。

    误区 3：相信里程计会一直准

    编码器没有噪声也不够：轮径磨损、轮距标定误差、轮子打滑、地面不平都会产生系统偏差；朝向误差还会被路程放大（FIG 1）。正确做法是把里程计当作"预测"，用 IMU 提供短时间的角速度，再用激光雷达、视觉或 GPS 做滤波校正（4.8 节卡尔曼滤波，Part IV：定位与 SLAM）。

    误区 4：在高速或有滑移时仍用运动学模型

    "轮子不侧滑"只在轮胎的侧向力远小于摩擦极限时成立。侧向加速度约为 \(v^2\kappa\)，当它接近 \(\mu g\) 时轮胎就开始打滑，实际转弯半径会大于 \(L/\tan\delta\)（不足转向）或车尾甩出（过度转向）。滑移转向和履带车辆本来就靠打滑转弯。在这些场合应使用动力学自行车模型（Kong 等，2015），或把运动学模型当作低速近似，并用更保守的速度和转向限幅。

    误区 5：忘了角度回绕

    朝向是圆周上的量：\(359^\circ\) 与 \(-1^\circ\) 是同一个方向，却相差 \(360^\circ\)。若直接计算 \(\alpha=\psi-\theta\) 而不回绕到 \((-\pi,\pi]\)，当车头越过 \(\pm\pi\) 时误差突然跳 \(2\pi\)，控制器会命令机器人转一整圈。FIG 5、FIG 6 的代码都用 wrapPi。同样的问题出现在四元数与欧拉角的转换、以及横向误差符号的定义里；5.2 节讨论旋转表示时遇到的回绕问题在这里换了个面孔回来。

    8小练习

    1. 独轮车的位形是 \(q=(x,y,\theta)\)。在某一时刻，它能立即朝几个独立的方向运动？最终能到达几维的位姿集合？
    允许的速度是 \(\ker A(q)=\operatorname{span}\{g_1,g_2\}\)，二维；但 \(g_1,g_2,[g_1,g_2]\) 张成 \(\mathbb R^3\)，由 Chow–Rashevskii 定理可控，可达集是整个 \(SE(2)\)（3 维）。这正是非完整约束的特征：约束限制速度，不限制位形。
    2. 轮距 \(W=0.5\) m 的差速机器人，左轮线速度 0.4 m/s，右轮 0.6 m/s。它在做什么？
    \(v=(0.4+0.6)/2=0.5\)，\(\omega=(0.6-0.4)/0.5=0.4\) rad/s（右轮快，逆时针，向左转），\(R=v/\omega=1.25\) m，也即 \(\tfrac W2\cdot\tfrac{v_R+v_L}{v_R-v_L}=0.25\cdot5\)。ICR 在车轴上、中心左侧 1.25 m 处。
    3. 把摆动的幅度 \(\varepsilon\) 减半（前进 \(\varepsilon\)、左转 \(\varepsilon\)、后退 \(\varepsilon\)、右转 \(\varepsilon\)），一次摆动产生的横移距离变为原来的
    净位移 \(\approx\varepsilon^2[g_1,g_2]\)，与 \(\varepsilon^2\) 成正比，所以变成 \(1/4\)；而路程只减半。因此要靠摆动横移一段距离 \(d\)，要行驶约 \(2\sqrt d\)（另加约 \(2\sqrt d\) 弧度的原地转向），小位移很"贵"。
    4. 轴距 \(L=2.7\) m 的汽车（自行车模型），前轮转角 \(\delta=10^\circ\)。后轴中点的转弯半径约为
    \(R=L/\tan\delta=2.7/0.1763\approx15.3\) m。0.065 是曲率 \(1/R\)（单位 1/m），不是半径。注意：转弯半径随转角不是线性的，\(\tan\delta\) 在小角度才近似等于 \(\delta\)。
    5. 独轮车是可控的，那么为什么没有任何连续的、定常的状态反馈 \(u=k(q)\) 能把它渐近镇定到一个位姿？
    Brockett（1983）：渐近镇定的必要条件是 \((q,u)\mapsto f(q,u)\) 的像包含原点的邻域。独轮车的像在每点是平面 \(\Delta(q)\)，缺少侧向分量，所以不满足。可控性（能到达）和可镇定性（用光滑静态反馈到达）是两回事。出路：不连续反馈（极坐标律）、时变反馈，或规划加跟踪。

    9延伸阅读

    • R. Siegwart, I. Nourbakhsh & D. Scaramuzza, Introduction to Autonomous Mobile Robots, 2nd ed., MIT Press, 2011, Ch. 3 — 移动机器人运动学：差速、阿克曼、全向轮模型，运动学约束，极坐标位姿控制律（3.6 节）
    • K. Lynch & F. Park, Modern Robotics, Ch. 13 — 轮式移动机器人：全向轮、非完整约束、里程计、运动规划与反馈控制
    • S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006, Ch. 13 — 微分约束：Pfaffian 约束、可积性、可控性、Dubins 车与 Reeds–Shepp 车（在线免费）
    • R. M. Murray, Z. Li & S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994, Ch. 7 — 机器人系统中的非完整行为：李括号、Chow 定理、路径规划
    • L. E. Dubins, On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents, American Journal of Mathematics 79(3), 1957 — Dubins 最短路径的原始论文
    • J. A. Reeds & L. A. Shepp, Optimal paths for a car that goes both forwards and backwards, Pacific Journal of Mathematics 145(2), 1990 — 允许倒车的最短路径
    • R. W. Brockett, Asymptotic stability and feedback stabilization, in Differential Geometric Control Theory, Birkhäuser, 1983 — Brockett 必要条件
    • M. Aicardi, G. Casalino, A. Bicchi & A. Balestrino, Closed loop steering of unicycle-like vehicles via Lyapunov techniques, IEEE Robotics & Automation Magazine 2(1), 1995 — 用 Lyapunov 方法设计独轮车类车辆的闭环镇定律（与正文的线性极坐标律不同的另一种设计）
    • R. C. Coulter, Implementation of the Pure Pursuit Path Tracking Algorithm, CMU-RI-TR-92-01, 1992; and B. Paden et al., A Survey of Motion Planning and Control Techniques for Self-Driving Urban Vehicles, IEEE Trans. Intelligent Vehicles 1(1), 2016 — 纯追踪与自动驾驶规划、控制综述