3.5 逆微分运动学

正问题「给 $\dot q$ 求 $v_e$」一次矩阵乘法就能解决；逆问题「给 $v_e$ 求 $\dot q$」却要解线性方程组，麻烦全在 $J$ 的形状上：方阵满秩（求逆）、长方矮胖（冗余）、退化（奇异）。本节按这三种情形给出三套答案——直接逆、伪逆 + 零空间、阻尼最小二乘——三套合起来，就是数值逆运动学的发动机。录播 1:56:39–2:19:53。

3.5.0 方阵情形：直接求逆 + 数值积分

正运动学 $x_e=k(q)$ 高度非线性，封闭形式的逆解「很可能不存在」。实际操作中，我们用微分运动的思想 + 数值计算来求运动学逆解——雅可比正是数值计算的桥梁（1:56:40）。最简单的起点是 $n=r$、$J$ 为方阵且满秩的情形：

$$\dot q=J^{-1}(q),v_e,\qquad q(t_{k+1})=q(t_k)+\int_{t_k}^{t_{k+1}}\dot q,\mathrm dt\approx q(t_k)+J^{-1}(q(t_k)),v_e(t_k),\Delta t$$

「这样不受方程结构是否可解的影响，没有解析解也能解」（1:58:24）。注意这一招的前提是 $J$ 为方阵且满秩：不满秩是奇异，长方阵是冗余，两者都要另作处理（分别见 3.5.2 与 3.5.1）。

3.5.1 冗余机械手（$r<n$）：伪逆与零空间

冗余情形的一般思路，是把它等效为一个有约束的线性最优化问题（1:59:00）：在约束 $J\dot q=v_e$ 下最小化关节速度的加权范数

$$\min_{\dot q}\ g(\dot q)=\tfrac12,\dot q^{\mathrm T}W\dot q\qquad\text{s.t.}\quad J\dot q=v_e$$

其中 $W$ 为 $n\times n$ 对称正定加权矩阵（$W=I$ 时即关节速度范数最小）。下面照 preview 里的三步套路引入拉格朗日乘子：

$$\mathcal L(\dot q,\lambda)=\tfrac12\dot q^{\mathrm T}W\dot q+\lambda^{\mathrm T}(J\dot q-v_e)$$

$W=I$：右伪逆（2:03:27）：

指定 $\dot q_0$ 的通解（2:04:29）：冗余还有一个好处——可以「顺便」让 $\dot q$ 尽量接近某个期望值 $\dot q_0$（相当于在任务约束之外再附加一条约束）：

$$\min_{\dot q}\ \tfrac12(\dot q-\dot q_0)^{\mathrm T}W(\dot q-\dot q_0)\quad\text{s.t.}\quad J\dot q=v_e \qquad\xrightarrow{\ \text{同上推导}\ }\qquad \boxed{\ \dot q=J^\dagger v_e+(I_n-J^\dagger J),\dot q_0\ }$$

（$W=I$ 情形。）

$\dot q_0$ 的三种取法（2:07:04）：先定义标量函数 $g(q)$，再取

$$\dot q_0=k_0\left(\frac{\partial g(q)}{\partial q}\right)^{\mathrm T}\quad(\text{沿梯度方向})$$

所谓三种取法，其实就是三个不同的 $g$：

1. 可操作度（最大化）：$g(q)=w(q)=\sqrt{\det!\big(J(q)J^{\mathrm T}(q)\big)}$。$w$ 在奇点位形处为零；方阵情形直接算 $\det J$，长方阵则算 $\det(JJ^{\mathrm T})$ 再开方——「这个值反映行列式离 0 的远近：离零越远离奇点越远」（2:08:21），最大化 $w$ 就是远离奇异；
2. 与关节极限的距离（最大化）：让各关节角尽量靠近行程中点 $\tfrac12(\vartheta_{i\mathrm M}+\vartheta_{im})$，从而保证不会到达极限位置（2:09:06）；
3. 与障碍物的距离（最大化）：取机械臂上所有点与障碍物距离的最小值作为 $g$，再让它最大化——「使最小距离最大化——最小距离都大了，其他肯定更远」（2:09:31）。

老师一句话点破了这套做法成立的根据：「为什么能优化？因为冗余——不是一个解，有很多个解，可以把我们的目标转化成优化问题」（2:10:06）。

3.5.2 运动学奇点情形：阻尼最小二乘

$J^{-1}$ 与 $J^\dagger$ 都只在满秩时才可计算，而奇点位形处 $v_e=J\dot q$ 里含有线性相关的方程（2:10:14），要分两种情况看：

• 若 $v_e\in\mathcal R(J)$：提取出所有线性无关方程就能解出 $\dot q$，这条路径仍然可实现；
• 若 $v_e\notin\mathcal R(J)$：$v_e$ 无法用关节速度线性表出，方程无解。

为什么奇点邻域也危险（不严密但直观，2:11:18）：由 $J^{-1}=\operatorname{adj}J/\det J$（伴随矩阵除以行列式）可见，$\det J\to0$ 时逆矩阵的元素整体趋于无穷——反解出来的关节速度也趋近无穷大。「这就是为什么要避开。」

解决思路：容许小误差，换回可解性（阻尼最小二乘，DLS，2:12:16）：不再要求微分运动方程严格成立，而是容许 $v_e$ 带一个小的误差 $\delta$：

$$g(\dot q,\delta)=\tfrac12\dot q^{\mathrm T}W\dot q+\tfrac12,\delta^{\mathrm T}K,\delta\qquad\text{s.t.}\quad v_e-J\dot q=\delta$$

$K$ 就是这两项之间的权衡系数：$K$ 取得大，更用力压制误差；$K$ 取得小，则更偏向让关节速度小。

增广变量法（2:14:24）：把误差也并入未知量，令 $w=\begin{bmatrix}\dot q\ \epsilon\end{bmatrix}$（课件记 $\epsilon$）、$A=[J\ \ -I]$，则 $Aw=J\dot q-\epsilon=v_e$；目标随之写成单一二次型 $g=\tfrac12w^{\mathrm T}Qw$，$Q=\operatorname{diag}{W,K}$。接下来套用拉格朗日乘子法，与 3.5.1 完全同型：先有 $w=-Q^{-1}A^{\mathrm T}\lambda$，再由 $AQ^{-1}A^{\mathrm T}=JW^{-1}J^{\mathrm T}+K^{-1}$（$A$ 满秩、$Q$ 正定 ⇒ 可逆，论证同前）解出 $\lambda$，即得

$$\begin{bmatrix}\dot q\ \epsilon\end{bmatrix}=\begin{bmatrix}W^{-1}\ -K^{-1}\end{bmatrix}J^{\mathrm T}\big(JW^{-1}J^{\mathrm T}+K^{-1}\big)^{-1}v_e$$

下图用一条曲线把「为什么要加阻尼」画了出来。横轴是 $J$ 的奇异值 $\sigma$（$\det J\to0$ 即最小奇异值 $\to0$），纵轴是把任务速度映回关节速度的增益：伪逆的增益 $1/\sigma$ 在奇点处发散；DLS 的增益 $\sigma/(\sigma^2+k^2)$ 则处处有界，峰值只有 $1/(2k)$。

3.5 小结与常见错误

• 一条主线：方阵（求逆 + 数值积分）→ 矮胖（拉格朗日乘子 ⇒ $J^\dagger$ 最小范数解，再加 $P\dot q_0$ 零空间项）→ 退化（DLS：容许 $\delta$、增广同型、$J^*$）。
• 三个矩阵别混：$J^{-1}$（方阵满秩）、$J^\dagger=J^{\mathrm T}(JJ^{\mathrm T})^{-1}$（冗余、行满秩）、$J^*$（奇异邻域、阻尼）——分别对应三种「不方 / 退化」的处境。