逆运动学（Inverse Kinematics）

注

本文的代码实现使用了pinocchio库

原理讲解

逆运动学这里主要方法总结为：Pseudo inversing a Jacobian iteratively until convergence

对于Jacobian以及其在李代数流形下的求导，可以参考这个链接

Jacobian of the task(关节角度到姿态误差)

（PS：速度Jacobian是力Jacobian的转置）设当前机械臂位姿为 $q$ ，这里我们可以设位姿误差写为 $e (q)$ .在逆运动学中，我们需要的就是位姿误差收敛到 $0$ 。所以 Jacobian of the task 就是 Jacobian of the $e (q)$ .

为了让误差收敛，我们希望误差满足如下方程：

\dot{e} (q) = - α e (q), (α > 0)

这里 $\dot{e} (q)$ 可以理解成期望的执行器的位姿速度

这里的 $J (q)$ 是 $e$ 对 $q$ 的Jacobian

J_{e r r} (q) := \frac{\partial e}{\partial q} (q)

由于我们希望 $\dot{e} (q) = - α e (q)$ 成立，又因为

\dot{e} (q) = \frac{d}{d t} e (q) = \frac{\partial e}{\partial q} (q) \frac{d}{d t} q = J_{e r r} (q) \dot{q}

PS: 全微分公式为 $d e = \frac{\partial e}{\partial q} d q + \frac{\partial e}{\partial t} d t$

因为， $e$ 为位姿误差，不显含 $t$ 所以 $\frac{\partial e}{\partial t} = 0$

姿态误差的定义（SE(3)中的度量）

此处，我们定义，基坐标系为 $f r a m e 0$ ，机器人执行器坐标系为 $f r a m e b$ ，目标坐标系为 $f r a m e t$

在特殊欧几里德群中，执行器和目标姿态相对于 $f r a m e 0$ 的姿态差分别为 $T_{0 b} \in S E (3)$ 和 $T_{0 t} \in S E (3)$

需要指出的是，要定义误差，就要定义度量(metrics)，这里我们首先需要进行对数映射

S E (3) \in R^{4 \times 4} \to s e (3) \in R^{6}

为了方便书写，我们记 $\log_{6}$ 代表了如上式从 $S E (3)$ 到 $s e (3)$ 的映射

然后再对 $s e (3)$ 六维向量求范数，从而得到度量。需要指出的是， $S E (3)$ 上并没有公认的权威度量定义

具体而言，本文中我们定义姿态误差如下

e (q) := T_{0 t} ⊖ T_{0 b} = \log_{6} (T_{b 0} T_{0 t}) = \log_{6} (T_{b t})

可以看出，以上 $e (q)$ 的定义本质上也是从关节流形空间到李代数空间的一个映射。

Frame Jacobian(从关节角度到执行器姿态)

\begin{aligned} _{b} J_{0 b} (q) & = \frac{\partial T_{0 b}}{\partial q} (q) \\ = lim_{τ \to 0} \frac{T_{0 b} (q \oplus τ) ⊖ T_{0 b} (q)}{τ} \\ = {\frac{\partial \log_{6} (T_{b 0} (q) \cdot (T_{0 b} (q) \cdot \exp_{c} (τ)))}{\partial τ} |}_{τ = 0} \end{aligned}

上式中 $\exp_{c}$ 是从切空间位移映射到关节空间位移，即

τ = v δ t \in c \to C

这里具体细节推导请参考A micro Lie theory for state estimation in robotics这篇文章。

在实际的pinocchio使用中，指数映射运算其实就对应了pinocchio.integrate，具体来说

python

tau = velocity * dt
q * exp(tau) := pinocchio.integrate(model, q, tau)

PS:(:= 符号代表“定义为”)

Pose task Jacobian (从关节角度到误差，并写成与关节角度以及姿态相关的形式)

由于我们最终要求“误差Jacobian的表达式”，并且希望误差Jacobian中的内容都写成和关节角相关的形式。

这一小节的目的就是结合上面两小节，分别从“误差和执行器姿态关系”以及“执行器姿态到关节角度关系”两个方面，将上面两小节推导结果进行整合，从而得到最终的“由关节角度表示的任务Jacobian”

由于有

e (q) = \log_{6} (T_{b t})

又因为

J_{e r r} (q) := \frac{\partial e}{\partial q} (q)

故得到

\begin{aligned} J (q) & := \frac{\partial e}{\partial q} (q) \\ = \frac{\partial \log_{6} (T_{b t})}{\partial q} \\ = \frac{\partial \log_{6} (T_{b 0} T_{0 t})}{\partial q} \\ = \frac{\partial \log_{6} (T_{b 0} T_{0 t})}{\partial T_{b 0}} \frac{\partial T_{b 0}}{\partial T_{0 b}} \frac{\partial T_{0 b}}{\partial q} \end{aligned}

我们假定 $T_{0 t}$ 是恒定的，所以可以忽略掉 $T_{0 t}$ 对 $T_{b 0}$ 的导数。通过一些推导， $J (q)$ 可以写成如下形式。关于 $\frac{\partial l o g_{6} (A B)}{\partial A}$ 形式的具体的推导还是看这篇文章，此处略去一些细节

\begin{aligned} J_{e r r} (q) & = \frac{\partial \log_{6} (T_{b 0} T_{0 t})}{\partial T_{b 0}} \frac{\partial T_{b 0}}{\partial T_{0 b}} \frac{\partial T_{0 b}}{\partial q} \\ = - J l o g_{6} (T_{t b})_{b} J_{0 b} (q) \end{aligned}

上式中，我们 $J l o g_{6}$ 的定义如下

J l o g_{6} (T) = \frac{\partial \log_{6} (T)}{\partial T}

至此，我们可以去处理一些 $J (q)$ 非奇异情况的问题了。

Effect of the log-derivative

Log-derivative with a single non-singular task

首先引入两个公式，具体证明参考官方Jlog6()文档

J l o g_{6} (T^{- 1})^{- 1} \log_{6} T = \log_{6} (T) J l o g_{6} (T^{- 1}) = - J l o g_{6} (T)

可以使用如下程序验证

python

import pinocchio as pin
import numpy as np
T = pin.SE3.Random()
print(pin.log6(T))
print(pin.Motion(np.linalg.inv(pin.Jlog6(T.inverse()))@pin.log6(T)))

需要注意的是，在实际使用中，我们通常不写成

python

np.linalg.inv(A) @ b

为了避免求逆，最好还是写成

python

np.linalg.solve(A, b)

IK最终结论（单点目标）

因为

\begin{aligned} J_{e r r} (q) & := \frac{\partial e}{\partial q} (q) \\ \Leftrightarrow J_{e r r} (q) \frac{d q}{d t} & = \frac{d e}{d t} \end{aligned}

又因为，我们希望 $d e / d t = - α e (q), (α > 0)$ ，可令 $α = 1$ ，故有

\begin{aligned} \Rightarrow J_{e r r} (q) v^{*} = - e (q) \\ \Leftrightarrow - J l o g_{6} (T_{t b})_{b} J_{0 b} (q) v^{*} = - e (q) \\ \Leftrightarrow_{b} J_{0 b} (q) v^{*} = J l o g_{6} (T_{t b})^{- 1} e (q) \end{aligned}

由于

e (q) = \log_{6} (T_{b t}) = - \log_{6} (T_{t b})

故可得：

\Rightarrow_{b} J_{0 b} (q) v^{*} = - J l o g_{6} (T_{t b})^{- 1} \log_{6} (T_{t b})

又因为

J l o g_{6} (T^{- 1})^{- 1} \log_{6} T = \log_{6} (T)

最终得到

\begin{aligned} \Rightarrow v^{*} & = -_{b} J_{0 b} (q)^{- 1} J l o g_{6} (T_{t b})^{- 1} \log_{6} (T_{t b}) \\ =_{b} J_{0 b} (q)^{- 1} J l o g_{6} (T_{t b}^{- 1})^{- 1} \log_{6} (T_{t b}) \\ =_{b} J_{0 b} (q)^{- 1} \log_{6} (T_{t b}) \\ = -_{b} J_{0 b} (q)^{- 1} e (q) =_{b} J_{0 b} (q)^{- 1} \dot{e} (q) \end{aligned}

其实根据 $_{b} J_{0 b} (q)$ 作为Frame Jacobian的定义也知道 $_{b} J_{0 b} (q) v^{*} = \dot{e} (q)$ 。

优化视角看IK问题

无damping， $λ = 0$

以优化的视角来看，上述的无约束情况的IK问题，可以写成如下的二次规划问题，（这里用到了导数为0的性质）此处之所以要把求逆变成优化问题，其实是为了方便计算机求解

v^{*} = \arg min_{v} \frac{1}{2} v^{T} P v + Q^{T} v \Leftrightarrow P v^{*} = - Q

上述IK求解过程中，若这里 $λ = 0$ .在这种情况下，我们可以直接令其中 $P$ 和 $Q$ 写成如下形式

\begin{aligned} J_{l} & := - J l o g_{6} (T_{t b}) \\ J_{b} & :=_{b} J_{0 b} (q) \\ P & = J_{b}^{T} J_{l}^{T} W J_{l} J_{b} + λ I = (W J_{l} J_{b})^{T} J_{l} J_{b} + λ I \\ Q & = J_{b}^{T} J_{l}^{T} W e (q) = (W J_{l} J_{b})^{T} e (q) \end{aligned}

此前直接推导所得的IK结果就可以改写成

v^{*} = - J_{b}^{- 1} e (q)

需要指出的是，在实际的求解过程中，不应该采用直接求逆，而应该i选择伪逆。

接下来解释论证本小节的优化问题和原IK问题的等价性

由于我们的优化问题等价于如下问题

P v^{*} = - Q = - (W J_{l} J_{b})^{T} e (q)

在上式等号两边均左乘 $(W J_{l} J_{b})^{- T}$ ，可以得到

J_{l} J_{b} v^{*} = - e (q)

又因为，

\begin{aligned} e (q) & := - \log_{6} (T_{t b}) \\ J_{l} & := - J l o g_{6} (T_{t b}) \\ \Rightarrow J_{l}^{- 1} e (q) & = - J l o g_{6} (T_{t b})^{- 1} e (q) \\ = J l o g_{6} (T_{t b}^{- 1})^{- 1} e (q) \\ = - J l o g_{6} (T_{t b}^{- 1})^{- 1} \log_{6} (T_{t b}) \\ = - \log_{6} (T_{t b}) \\ = e (q) \\ \Rightarrow J_{l} e (q) & = e (q) \end{aligned}

故，可得

v^{*} = - J_{b}^{- 1} e (q)

显然，和此前的公式一致。

存在damping， $λ \neq 0$

当存在damping时，我们再通过在 $P v^{*} = - (W J_{l} J_{b})^{T} e (q)$ 等号两边均左乘 $(W J_{l} J_{b})^{- T}$ ，可以得到

\begin{aligned} v^{*} & = - [J_{b} + λ J_{l}^{- 1} W^{- 1} J_{l}^{- T} J_{b}^{- T}]^{- 1} e (q) \\ (J_{b}^{T} J_{l}^{T} W J_{l} J_{b} + λ I) v^{*} & = - J_{b}^{T} J_{l}^{T} W J_{l} e (q) \end{aligned}

不妨令 $W = I$ , 又因为 $J_{l} e (q) = e (q)$ ，又因为 $J_{l} J_{b} = J_{e r r o r}$ ,可得

(J_{e r r}^{T} J_{e r r} + λ I) v^{*} = - J_{e r r}^{T} e (q)

程序里可以写为：

python

T_bt = robot.data.oMi[idx].actInv(T_t)
J_b = pin.computeJointJacobian(robot.model, robot.data, q, idx)
J_l = -pin.Jlog6(T_bt.inverse())

# 接下来会这里会利用J_error v^{*} = -e(q)来求v^{*}
J_error = J_l.dot(J_b)
e_q = pin.log(T_bt).vector

v_star = np.linalg.solve(J_error.T.dot(J_error) + damp * np.eye(6), -J_error.T.dot(e_q))
q = pin.integrate(robot.model, q, v_star*dt)

​

注 ​

原理讲解 ​

Jacobian of the task(关节角度到姿态误差) ​

姿态误差的定义（SE(3)中的度量） ​

Frame Jacobian(从关节角度到执行器姿态) ​

Pose task Jacobian (从关节角度到误差，并写成与关节角度以及姿态相关的形式) ​

Effect of the log-derivative ​

Log-derivative with a single non-singular task ​

IK最终结论（单点目标） ​

优化视角看IK问题 ​

无damping，λ=0 ​

存在damping，λ≠0 ​

注

原理讲解

Jacobian of the task(关节角度到姿态误差)

姿态误差的定义（SE(3)中的度量）

Frame Jacobian(从关节角度到执行器姿态)

Pose task Jacobian (从关节角度到误差，并写成与关节角度以及姿态相关的形式)

Effect of the log-derivative

Log-derivative with a single non-singular task

IK最终结论（单点目标）

优化视角看IK问题

无damping， $λ = 0$

存在damping， $λ \neq 0$