Keyboard shortcuts

Press ← or → to navigate between chapters

Press S or / to search in the book

Press ? to show this help

Press Esc to hide this help

3.7 逆运动学算法

把误差当作「要消除的速度」输入，逆微分运动学就变成反馈控制器：q˙\dot q 是控制量、ee 是偏差，目标让 e→0e\to0。本节把三套算法一次讲完——逆雅可比、雅可比转置、二阶算法。快讲：老师说考试不考，但实际开发必用。录播 2:24:15–2:27:34。

本节的定位（老师原话）

「目的就是建立 q˙\dot q 与误差 ee 之间的关系——从控制的角度也就是设计一个控制器」（2:24:22）。数值逆解的细节「考试后面也不会考」（0:10:02），但实际开发要用——本节正是控制视角的桥，为后续动力学 / 控制章节铺路。节奏上快讲：课件后几页（算法比较、方向误差）「大家下面自己看一下」（2:25:53）。

3.7.1 逆雅可比方案

逆雅可比方案的做法最直接：正运动学的微分关系是 x˙=JAq˙\dot x=J_A\dot q，既然能由 q˙\dot q 算出 x˙\dot x，把 JAJ_A 求逆就能反过来由期望的末端速度推出关节速度。

设任务误差 e=xd−xe=x_d-x（期望位姿 −- 当前位姿），希望 ee 指数收敛，于是让末端速度取「期望速度 x˙d\dot x_d 加一个正比于误差的修正项 KeKe」，再映射回关节空间，即

q˙=JA−1(q) (x˙d+K e)\dot q=J_A^{-1}(q),\big(\dot x_d+K,e\big)

验证一下：把 x˙=JAq˙\dot x=J_A\dot q 代入误差的导数 e˙=x˙d−x˙\dot e=\dot x_d-\dot x（e=xd−xe=x_d-x 两边求导即得）：

e˙=x˙d−JAq˙=x˙d−(x˙d+Ke)=−Ke⟹e˙+Ke=0\dot e=\dot x_d-J_A\dot q=\dot x_d-(\dot x_d+Ke)=-Ke \qquad\Longrightarrow\qquad \dot e+Ke=0

可见误差动态被化成了一阶线性系统：只要 KK 正定，e→0e\to0 就指数收敛，收敛快慢由 KK 的取值决定。

冗余情形

JAJ_A 非方阵（冗余）时，把 JA−1J_A^{-1} 换成右伪逆 JA†J_A^\dagger（3.5.1）；再带上零空间项，就能在完成末端任务的同时优化内部姿态。这正是 3.5 里「直接求逆」与「伪逆 + 零空间」两套解的复用。

3.7.2 雅可比转置方案

转置方案干脆绕开求逆：不解方程，直接用 JTJ^{\mathrm T} 把误差映回关节空间——

q˙=JTK e\dot q=J^{\mathrm T}K,e

为什么用转置（2:24:56，老师特意点出动机）：「因为求逆很花计算量——知道 JJ 要求 J−1J^{-1} 很花时间；用 JTJ^{\mathrm T} 直接调换一下元素位置就可以了。」省下求逆是有代价的——把两种方案摆在一起，得失才看得清：

  • 逆方案能完全线性化：误差动态严格是一阶线性（e˙=−Ke\dot e=-Ke）；
  • 转置方案不能完全线性化：误差动态是非线性的，e˙\dot e 不再是干净的 −Ke-Ke，而是多出含 JJ 随位形变化的项——稳定性分析只能借助 Lyapunov 函数（学控制的同学有了解）。
直觉

q˙=JTKe\dot q=J^{\mathrm T}Ke 是「沿误差在关节空间的负梯度方向走」——这里的「负梯度」是对关节变量 qq 而言，12∥e∥2\tfrac12\|e\|^2 的负梯度正是 JTeJ^{\mathrm T}e。JTeJ^{\mathrm T}e 把操作空间的误差映射回关节空间（与静力学 τ=JTFe\tau=J^{\mathrm T}F_e 是同一个 JTJ^{\mathrm T} 机制——3.8），步长由 KK 控制。转置省去了求逆，但步长随位形变化，因此收敛只是局部的。

两种方案看着不同，放进同一张闭环框图（下图）却是同一个结构：q˙\dot q 是控制量、ee 是偏差，「机器人 k(q)k(q)」是被控对象，反馈把当前位姿送回输入端——这正是把逆微分运动学当成控制器设计的视角。

逆微分运动学方案的闭环框图 期望位姿 xd 与测量反馈的当前位姿 x 相减得误差 e，经增益 K 与前馈 x′d 求和后，由 JA 的逆（冗余时伪逆）映射为关节速度 q′，机器人模型积分得到当前位姿；下方虚线框给出省去求逆的雅可比转置替代方案。 逆微分运动学 = 把误差当速度的反馈控制器 期望位姿 x_d + − e K 增益 x′d（前馈） JA−1(q) 冗余 → 伪逆 J†（3.5.1） J^T K（转置方案） 直接由 e 得 q′ 或 q′ 机器人 k(q) 内部：数值积分 q ← q′Δt 当前位姿 x 测量反馈：e = x_d − x（方向误差的三种表示见 3.7.3） 逆方案完全线性化（e′ = −K e，指数收敛）；转置方案省求逆但误差非线性——K 取大不一定更好，需 Lyapunov 分析
逆运动学算法的统一框图（本教程绘）：实线主通路为逆雅可比方案（3.7.1），虚线为雅可比转置方案（3.7.2）——两者只是「误差 → 关节速度」那一块的实现不同；冗余时把 JA−1J_A^{-1} 换成右伪逆并加零空间项即可。

3.7.3 方向误差与二阶算法

以上两种方案都假定误差 ee 已经算好。剩下两件事要交代：姿态那部分误差用什么表示，以及要不要把加速度也纳入算法。

方向（姿态）误差的表示（2:25:45）：「误差表示方式不同，求逆方法也不同。」同一个姿态误差有好几种写法，常用的有三种：

  • 欧拉角：eO=φd−φe_O=\varphi_d-\varphi 直接相减，写法最省事，却受 定理 2.4.B 的奇点与多值困扰；
  • 轴 / 角（angle-axis）：用转轴加转角的写法，比欧拉角更通用，姿态控制 / 求逆算法常用（2.5）；
  • 四元数（上一章 2.6）：「姿态误差也可以用四元数」——无奇点，适合插值与误差度量。

（课件上的误差公式基于姿态列向量叉积，该处 OCR 受遮挡、不能逐字保证，本编按标准形式给出 eO∝∑irid×riee_O\propto\sum_i r_i^d\times r_i^e；读者以课件 / 教材为准。）

二阶算法（2:26:28）：上面两种一阶算法都只用速度；二阶算法把加速度也纳入进来，同时考虑速度与加速度：

e¨+KD e˙+KP e=0\ddot e+K_D,\dot e+K_P,e=0

误差满足的是一个二阶线性微分方程——KDK_D（阻尼）与 KPK_P（刚度）的取值共同影响收敛的快慢与平稳程度。「后面讲控制（动力学）时可能会涉及。」

算法对比（课件页，自己看）

逆方案误差动态完全线性化，但每次都要算 J−1J^{-1}（计算贵），无奇点处收敛快；转置方案省去了求逆，代价是只保证局部收敛、稳定性要 Lyapunov 分析；二阶算法把加速度也带进来，运动更平滑，但结构复杂（含 J˙\dot J）。冗余情形三者统一换成伪逆。

默写点 · 合上书再写（主动回忆）

不看正文与速查卡，先写出下面几条，再展开核对。写不出来就说明这一节还没「拿走」。

  1. 写出逆方案与转置方案各自的 q˙\dot{\boldsymbol q} 表达式，并各说一句特点。
  2. 为什么转置方案不能按「完全线性化」设计增益？
  3. 二阶算法比一阶多考虑了什么？
核对

逆方案：q˙=JA−1(x˙d+Ke)\dot{\boldsymbol q}=J_A^{-1}(\dot{\boldsymbol x}_d+Ke)——完全线性化，误差方程严格线性；转置方案：q˙=JTKe\dot{\boldsymbol q}=J^{\mathrm T}Ke——省去求逆，但误差方程非线性。

因为它的误差方程没有化成 e˙=−Ke\dot e=-Ke 那样的线性形式：KK 取大不一定更好，稳定性要用 Lyapunov 方法分析。

多考虑了 J˙(q)\dot J(\boldsymbol q) 项——「考虑加速度」意味着误差方程里出现 JJ 的时间导数，二阶算法让误差满足 e¨+KDe˙+KPe=0\ddot e+K_D\dot e+K_Pe=0。

3.7 小结与常见错误

  • 一条主线：误差当输入 → 逆方案（完全线性化，代价是贵）/ 转置方案（省计算，只局部收敛）/ 二阶算法（含加速度）→ 方向误差的三种表示 → 冗余换伪逆。
  • 考试口径：本节不考细节；但「JTJ^{\mathrm T} 为什么省计算」「为何要 Lyapunov」这两问，正是理解 JTJ^{\mathrm T} 双重身份（控制 + 静力学）的好线索。
常见错误清单 · 自查三条
  1. 用转置方案却按「完全线性化」设计增益——它的误差方程是非线性的，KK 取大不一定更好；
  2. 方向误差用欧拉角直接相减却不处理奇点 / 多值——大姿态偏差时应改用轴角 / 四元数；
  3. 二阶算法忽略 J˙(q)\dot J(q) 项——「考虑加速度」意味着误差方程里出现 JJ 的时间导数，实现时要补上这一块。