jacobian 雅克比求解与零空间映射

零空间及零空间映射

#

零空间集合： \( Null(A) = \{x|Ax=0\} \); for any \( x \), 若 \(ANx=0 \) 恒成立，那么 \( N \) 矩阵是 A 的零空间矩阵。 并且， \[ N = I - A^{\dagger}A \]

雅克比伪逆及零空间映射

#

在满足约束情况下，最小化速度范数 \[\begin{align*} \min_{\dot{q}} \; \| \dot{q} \|^{2} \\ \text{subject to } \dot{x} = J \dot{q} \end{align*} \] 用拉格朗日乘数法（Lagrange Multiplier）,这个问题可以变成：

\[ \min_{\dot{q}} \; \frac{1}{2}\|\dot{q}\|^{2} + \lambda^{T}(\dot{x} - J\dot{q}) \]

求出伪逆为： \( J^{T} (JJ^{T})^{-1} \)

在满足约束情况下，最小化速度误差 \( \dot{q} - \dot{q_{n}} \)范数 \[\begin{align*} \min_{\dot{q}} \; \| \dot{q} - \dot{q_{n}} \|^{2} \\ \text{subject to } \dot{x} = J \dot{q} \end{align*} \]

用拉格朗日乘数法（Lagrange Multiplier）,这个问题可以变成：

\[ \min_{\dot{q}} \; \frac{1}{2}\|\dot{q} - \dot{q_{n}} \|^{2} + \lambda^{T}(\dot{x} - J\dot{q}) \]

求出伪逆为： \( J^{\dagger} = J^{T} (JJ^{T})^{-1} \) 求解出零空间映射为 \( I - J^{\dagger}J \), 也即 \( I - J^{T} (JJ^{T})^{-1}J \)

基于伪逆运动学反解问题

#

使用雅克比矩阵求逆来求解运动学反解存在的问题 ¹

• 从原理上讲，使用这个方法最明显的一点要求是—— \( dx \) 不能过大。因为 Jacobian 是随着关节位置变化 不断在变化的，一旦关节位置变化很大，算出来的 Jacobian Inverse 就不再准确了。这个问题通常我们 可以用轨迹线性插值（linear interpolation）或限制 dx 的大小（clamping）来避免。
• 这个方法的第二个难点是雅可比矩阵求逆运算。矩阵求逆是一个非常消耗计算资源的运算（在写程序 时能避免就绝对不要求逆！）。当然，我们总是可以使用各种各样的解线性方程的方法来避开求逆运 算，比如 LU 分解、CHELOSKY 分解、QR 分解、SVD（SINGULAR VALUE DECOMPOSITION）等等
• 这个方法最大的问题还是在于它无法很好地对付机器人 SINGULARITY 或接近 SINGULARITY 的情况。从线性 方程的角度看，当机器人接近 SINGULARITY 时，雅可比矩阵也越来越“病态”（ILL-CONDITIONED），很小的 DX 可能求得很大的 DQ，方程对数值误差也更加敏感；而当机器人处于 SINGULARITY 时，线性方程可能无解、 也可能有无数多个解。

阻尼最小平方法

#

阻尼最小平方法 DLS(Damped Least Square)

为了避免利用雅可比矩阵求逆法控制机械臂时由于接近 Singularity 而产生非常大的关节速度，一个自然 的想法就是在求解过程中限制关节速度—— 既要尽可能地满足方程条件、也要尽可能地让关节速度不要太大 。

无约束的加权最小化 \[ \min_{\dot{q}} \;\| J\dot{q} - \dot{x} \|^{2} + \lambda^{2} \| \dot{q} \|^{2} \]

求出伪逆为： \( J^{\dagger}_{\lambda} = J^{T} (JJ^{T} + \lambda^{2}I)^{-1} \)

带权重的雅克比伪逆

#

\[\begin{align*} \min_{\dot{q}} \; \dot{q}^{T} W \dot{q} \\ \text{subject to } \dot{x} = J \dot{q} \end{align*} \]

用拉格朗日乘数法（Lagrange Multiplier）,这个问题可以变成：

\[ \min_{\dot{q}} \; \frac{1}{2} \dot{q}^{T} W \dot{q} + \lambda^{T}(\dot{x} - J\dot{q}) \]

求出伪逆为： \( J^{\dagger}_{W} = W^{-1} J^{T} (JW^{-1}J^{T})^{-1} \)

关节惯量作为权重

#

对于使用关节惯量加权的雅克比伪逆求解 \[ \min_{\dot{q}} \; \frac{1}{2} \dot{q}^{T} A \dot{q} + \lambda^{T}(\dot{x} - J\dot{q}) \]

求出伪逆为： \( J^{\dagger}_{A} = A^{-1} J^{T} (JA^{-1}J^{T})^{-1} \) 其中 \(\Lambda = (JA^{-1}J^{T})^{-1} \) 为操作空间等效质量/惯量

速度层冗余运动学

#

关节空间冗余速度量不影响主任务空间速度向量 \[ \dot{q} = J^{+} \dot{x} + N \dot{q_{n}} \]

其中，\( J^{\dagger} = J^{T} (JJ^{T})^{-1} \), \( N = I - J^{\dagger}J = I - J^{T} (JJ^{T})^{-1}J \) 如果使用关节惯量对关节速度加权，那么求出的伪逆为：\( J^{\dagger} = J^{\dagger}_{A} = A^{-1} J^{T} (JA^{-1}J^{T})^{-1} \)

推导零空间映射，关节空间向量 \( q_{n} \) 对 \( J \) 主任务无影响；推导从关节速度推导末端速度向量恒等于 \( \dot{x} \) \[\begin{align*} J \dot{q} &= JJ^{\dagger} \dot{x} + JN \dot{q_{n}} \\ &=JJ^{\dagger} \dot{x} + J(I-J^{\dagger}J) \dot{q_{n}} \\ &=JJ^{\dagger} \dot{x} + (J-JJ^{\dagger}J) \dot{q_{n}} \\ &=JJ^{\dagger} \dot{x} + (J-J) \dot{q_{n}} \\ &= \dot{x} \end{align*} \]

特例分析

#

对于分优先级的序列映射，若不同任务中的雅克比相同，也即 \( J_{2} = J_{1} \) 则低层次任务无效，应把这种情况的任务先综合到统一任务中运算

• 低于第一级任务 \( J_{1} \) 来说，求解 \( X_{1} \) 对应的关节角度

  \[ \dot{q_{1}} = J_{1}^{\dagger} X_{1} \]

• 低于第二级任务 \( J_{2} \) 来说，求解 \( X_{2} \) 对应的关节角度 \[\begin{align*} \dot{q_{2}} &= \dot{q_{1}} + N_{1}(J_{2})^{\dagger}(X_{2} - J_{2} \dot{q_{1}}) \\ &= \dot{q_{1}} + [I-(J_{1})^{\dagger}J_{1}](J_{2})^{\dagger}(X_{2} - J_{2} \dot{q_{1}}) \\ &= \dot{q_{1}} + [I-(J_{1})^{\dagger}J_{1}](J_{1})^{\dagger}(X_{2} - J_{1} \dot{q_{1}}) \\ &= \dot{q_{1}} + [(J_{1})^{\dagger}-(J_{1})^{\dagger}J_{1}(J_{1})^{\dagger}](X_{2} - J_{1} \dot{q_{1}}) \\ &= \dot{q_{1}} + [(J_{1})^{\dagger}-(J_{1})^{\dagger}](X_{2} - J_{1} \dot{q_{1}}) \\ &= \dot{q_{1}} \end{align*} \]

力层冗余解

#

关节空间冗余力矩不影响主任务空间力向量 参考：(Sentis, L. and Khatib, O., 2006)², (Sentis, L. and Khatib, O., 2005)³

\[ \tau = J^{T} F + N^{T} \tau_{n} \]

其中，\( J^{\dagger} = J^{T} (JJ^{T})^{-1} \), \( N = I - J^{\dagger}J = I - J^{T} (JJ^{T})^{-1}J \) 如果使用关节惯量对关节速度加权，那么求出的伪逆为：\( J^{\dagger} = J^{\dagger}_{A} = A^{-1} J^{T} (JA^{-1}J^{T})^{-1} \)

推导零空间映射，关节空间向量 \( \tau_{n} \) 对 \( J^{T} \) 主任务无影响，从关节力矩推导末端力向量，在推导过程中使用了一个矩阵性质⁴

\[\begin{align*} (J^{T})^{\dagger} \tau &= (J^{T})^{\dagger}J^{T} F + (J^{T})^{\dagger} N^{T} \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F + (J^{T})^{\dagger} (I-J^{\dagger}J)^{T} \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F + (J^{T})^{\dagger} (I-J^{T}(J^{\dagger})^{T}) \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F + [(J^{T})^{\dagger} - (J^{T})^{\dagger} J^{T}(J^{\dagger})^{T}] \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F + [(J^{T})^{\dagger} - (J^{T})^{\dagger} J^{T}(J^{T})^{\dagger}] \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F + [(J^{T})^{\dagger} - (J^{T})^{\dagger})] \tau_{n} \\ &=(J^{T})^{\dagger}J^{T} F \\ &=F \end{align*} \]