【现代机器人学】基于指数积的机械臂逆运动学

   基于指数积的正运动学    现代机器人学名词概念   有了以上的基础，我们现在利用指数积来对机器人的逆运动学进行求解，有一点需要注意，需要先对机器人进行指数积的正运动学建模，然后才能利用指数积来对逆运动学求解，求解逆运动学的方法我们这里采用数值求解的方式来求解关节角。   众所周知，对于通用的n自由度开链机器人而言，若其正向运动学写成 T ( θ ) , θ ∈ R n T\left( \theta \right) ,\theta \in R^n T(θ),θ∈Rn的形式，而其逆运动学问题可以描述成：当给定齐次变换矩阵 X ∈ S E ( 3 ) X\in SE\left( 3 \right) X∈SE(3)，找出满足 T ( θ ) = X T\left( \theta \right) =X T(θ)=X 的关节角 θ \theta θ 。

  对于求解析解，即找出机器人的连杆运动关系，通过几何的方法来求得具体的解析解，这种方法有很大的局限性，它只能适用于一些特定的机械臂，如要求关节4，5，6相互正交且共点。

   如果逆运动学方程无解析解时，可采用迭代数值方法求解。即使存在解析解，数值算法也经常用于改善求解精度。这里我们介绍逆运动学的数值求解，它采用的方法是牛顿-拉夫森法，用来求解非线性方程。

  对于给定的微分方程 g ( θ ) g\left( \theta \right) g(θ)，数值求解方程 g ( θ ) = 0 g\left( \theta \right)=0 g(θ)=0。假定 θ 0 \theta ^0 θ0 为初值，利用泰勒级数在 θ 0 \theta ^0 θ0 出展开，并截取到第一项，得 g ( θ ) = g ( θ 0 ) + ∂ g ∂ θ ( θ 0 ) ( θ − θ 0 ) + h . o . t . g\left( \theta \right) =g\left( \theta ^0 \right) +\frac{\partial g}{\partial \theta}\left( \theta ^0 \right) \left( \theta -\theta ^0 \right) +h.o.t. g(θ)=g(θ0)+∂θ∂g​(θ0)(θ−θ0)+h.o.t.若只保留到第一阶，令 g ( θ ) = 0 g\left( \theta \right)=0 g(θ)=0 ，求解 θ \theta θ 。得到 θ = θ 0 − ( ∂ g ∂ θ ( θ 0 ) ) − 1 g ( θ 0 ) \theta =\theta ^0-\left( \frac{\partial g}{\partial \theta}\left( \theta ^0 \right) \right) ^{-1}g\left( \theta ^0 \right) θ=θ0−(∂θ∂g​(θ0))−1g(θ0)再将上式求得值作为初值，代入上述方程中，重复求解，得到下面方程 θ k + 1 = θ k − ( ∂ g ∂ θ ( θ k ) ) − 1 g ( θ k ) \theta ^{k+1}=\theta ^k-\left( \frac{\partial g}{\partial \theta}\left( \theta ^k \right) \right) ^{-1}g\left( \theta ^k \right) θk+1=θk−(∂θ∂g​(θk))−1g(θk)上述过程不断迭代，直至满足某个指标值。

  我们用坐标向量 x x x 及其正运动学方程 x = f ( θ ) x=f\left( \theta \right) x=f(θ) 表示末端坐标， θ \theta θ 为关节角，令 x d x_d xd​ 为目标末端坐标，牛顿-拉夫森法中的 g ( θ ) g\left( \theta \right) g(θ) 可以定义成 g ( θ ) = x d − f ( θ ) g\left( \theta \right) =x_d-f\left( \theta \right) g(θ)=xd​−f(θ) ，目标是找到关节坐标 θ d \theta _d θd​ ，满足 g ( θ d ) = x d − f ( θ d ) = 0 g\left( \theta _d \right) =x_d-f\left( \theta _d \right) =0 g(θd​)=xd​−f(θd​)=0由前可得 x d = f ( θ d ) = f ( θ 0 ) + ∂ f ∂ θ ∣ θ 0 ( θ d − θ 0 ) + h . o . t . x_d=f\left( \theta _d \right) =f\left( \theta ^0 \right) +\frac{\partial f}{\partial \theta}|_{\theta ^0}\left( \theta _d-\theta ^0 \right) +h.o.t. xd​=f(θd​)=f(θ0)+∂θ∂f​∣θ0​(θd​−θ0)+h.o.t.式中 ∂ f ∂ θ ∣ θ 0 \frac{\partial f}{\partial \theta}|_{\theta ^0} ∂θ∂f​∣θ0​ 为 J ( θ 0 ) ∈ R m × n J\left( \theta ^0 \right) \in R^{m\times n} J(θ0)∈Rm×n 为 θ 0 \theta ^0 θ0 处的坐标雅可比。进一步简化得 J ( θ 0 ) Δ θ = x d − f ( θ 0 ) J\left( \theta ^0 \right) \varDelta \theta =x_d-f\left( \theta ^0 \right) J(θ0)Δθ=xd​−f(θ0)假设 J ( θ 0 ) J\left( \theta ^0 \right) J(θ0) 为方阵且可逆，便继续可得 Δ θ = J − 1 ( θ 0 ) ( x d − f ( θ 0 ) ) \varDelta \theta =J^{-1}\left( \theta ^0 \right) \left( x_d-f\left( \theta ^0 \right) \right) Δθ=J−1(θ0)(xd​−f(θ0))  由于正运动学是 θ \theta θ 的非线性函数，这时新的估计值 θ 1 \theta^1 θ1 比 θ 0 \theta^0 θ0 更接近真实值，迭代过程不断重复，产生一系列的 θ \theta θ 值，并最终在 θ d \theta_d θd​ 处收敛。 注：如果逆运动学存在多组解，迭代过程趋向于收敛到与初始值 θ 0 \theta^0 θ0 最接近的解，如果初始值与真实值没有足够接近，则迭代过程可能不收敛。另外雅可比矩阵有时会成为奇异，故我们采用求广义逆的方法来求雅可比矩阵的逆矩阵。    为了能应用在预期末端位形为 T s d ∈ S E ( 3 ) T_{sd}\in SE\left( 3 \right) Tsd​∈SE(3) 而不是坐标向量 x d x_d xd​ 的情况，将关节坐标雅可比矩阵J替换为末端物体雅可比矩阵 J b ∈ R 6 × n J_b\in R^{6\times n} Jb​∈R6×n 。 J b J_b Jb​ 的伪逆应该作用于物体速度旋量 V b ∈ R 6 V_b\in R^6 Vb​∈R6 。为找到这个 V b V_b Vb​ ，首先需计算相对物体坐标系的预期位形，即 T b d ( θ i ) = T s b − 1 ( θ i ) T s d T_{bd}\left( \theta ^i \right) =T_{sb}^{-1}\left( \theta ^i \right) T_{sd} Tbd​(θi)=Tsb−1​(θi)Tsd​ 然后利用矩阵对数确定 V b V_b Vb​ ，即 [ V b ] = log ⁡ T b d ( θ i ) \left[ V_b \right] =\log T_{bd}\left( \theta ^i \right) [Vb​]=logTbd​(θi) 至此，可得逆运动学的步骤：

注： J † ( θ i ) J^{\dag}\left( \theta ^i \right) J†(θi) 代表物体雅可比矩阵的广义逆。以上方法是物体雅可比矩阵 J b ( θ ) J_b\left( \theta \right) Jb​(θ)和物体空间速度旋量 V b V_b Vb​ 来求解的过程，另一种等效的方式是利用空间雅可比矩阵 J s ( θ ) J_s\left( \theta \right) Js​(θ) 和空间速度旋量 V s V_s Vs​ 来求解，只需对 J b J_b Jb​ 和 V b V_b Vb​ 作一下矩阵变换即可。

  根据以上的步骤，除了物体雅可比矩阵的求解，其余量我们均已经得到，故下面开始求解物体雅可比矩阵。回顾之前的知识，对于n杆串联机器人，其正向运动学的指数积公式为 T ( θ ) = e [ S 1 ] θ 1 ⋯ e [ S n − 1 ] θ n − 1 e [ S n ] θ n M T\left( \theta \right) =e^{\left[ S_1 \right] \theta _1}\cdots e^{\left[ S_{n-1} \right] \theta _{n-1}}e^{\left[ S_n \right] \theta _n}M T(θ)=e[S1​]θ1​⋯e[Sn−1​]θn−1​e[Sn​]θn​M空间速度 V s V_s Vs​ 可以写成 [ V s ] = T ˙ T − 1 \left[ V_s \right] =\dot{T}T^{-1} [Vs​]=T˙T−1 ，计算可得 [ V s ] = [ S 1 ] θ ˙ 1 + e [ S 1 ] θ 1 [ S 2 ] e − [ S 1 ] θ 1 θ ˙ 2 + e [ S 1 ] θ 1 e [ S 2 ] θ 2 [ S 3 ] e − [ S 2 ] θ 2 e − [ S 1 ] θ 1 θ ˙ 3 + ⋯ \left[ V_s \right] =\left[ S_1 \right] \dot{\theta}_1+e^{\left[ S_1 \right] \theta _1}\left[ S_2 \right] e^{-\left[ S_1 \right] \theta _1}\dot{\theta}_2+e^{\left[ S_1 \right] \theta _1}e^{\left[ S_2 \right] \theta _2}\left[ S_3 \right] e^{-\left[ S_2 \right] \theta _2}e^{-\left[ S_1 \right] \theta _1}\dot{\theta}_3+\cdots [Vs​]=[S1​]θ˙1​+e[S1​]θ1​[S2​]e−[S1​]θ1​θ˙2​+e[S1​]θ1​e[S2​]θ2​[S3​]e−[S2​]θ2​e−[S1​]θ1​θ˙3​+⋯通过伴随映射写成向量的形式，即 V s = S 1 θ ˙ 1 + A d e [ S 1 ] θ 1 ( S 2 ) θ ˙ 2 + A d e [ S 1 ] θ 1 e [ S 2 ] θ 2 ( S 3 ) θ ˙ 3 + ⋯ V_s=S_1\dot{\theta}_1+Ad_{e^{\left[ S_1 \right] \theta _1}}\left( S_2 \right) \dot{\theta}_2+Ad_{e^{\left[ S_1 \right] \theta _1}e^{\left[ S_2 \right] \theta _2}}\left( S_3 \right) \dot{\theta}_3+\cdots Vs​=S1​θ˙1​+Ade[S1​]θ1​​(S2​)θ˙2​+Ade[S1​]θ1​e[S2​]θ2​​(S3​)θ˙3​+⋯ 观察上式可写成n个空间速度的向量和形式 V s = J s 1 θ ˙ 1 + J s 2 ( θ ) θ ˙ 2 + ⋯ J s n ( θ ) θ ˙ n V_s=J_{s1}\dot{\theta}_1+J_{s2}\left( \theta \right) \dot{\theta}_2+\cdots J_{sn}\left( \theta \right) \dot{\theta}_n Vs​=Js1​θ˙1​+Js2​(θ)θ˙2​+⋯Jsn​(θ)θ˙n​写成矩阵的形式，即 V s = [ J s 1    J s 2 ( θ )    ⋯    J s n ( θ ) ] [ θ ˙ 1 θ ˙ 2 ⋮ θ ˙ n ] = J s ( θ ) θ ˙ V_s=\left[ J_{s1}\,\,J_{s2}\left( \theta \right) \,\,\cdots \,\,J_{sn}\left( \theta \right) \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\ \vdots\\ \dot{\theta}_n\\ \end{array} \right] =J_s\left( \theta \right) \dot{\theta} Vs​=[Js1​Js2​(θ)⋯Jsn​(θ)] ​θ˙1​θ˙2​⋮θ˙n​​ ​=Js​(θ)θ˙ 矩阵 J s ( θ ) J_s\left( \theta \right) Js​(θ) 即为空间固定坐标形式下的雅可比矩阵，简称空间雅可比。 再将空间雅可比 J s J_s Js​ 转化为物体雅可比矩阵 J b J_b Jb​ 。 J b ( θ ) = A d T b s ( J s ( θ ) ) J_b\left( \theta \right) =Ad_{T_{bs}}\left( J_s\left( \theta \right) \right) Jb​(θ)=AdTbs​​(Js​(θ)) 至此，全部未知量我们均已得到，可以应用算法进行建模了。

这里我设定初始关节角为零位角，即 q0=[0;0;0;0;0;0] 。设定Td为关节角为[0.1;0.2;0.3;0.4;0.5;0.6]时的末端位姿，即

开始求逆解，设定迭代1000次，如果1000次以内没有迭代出来，则认为求不出逆解，同时设定误差 ε w \varepsilon _w εw​ 和 ε v \varepsilon _v εv​ 小于1e-8时停止迭代，迭代结果如下：

结果分析，在迭代69次后，停止了迭代，此时 ε w \varepsilon _w εw​ 和 ε v \varepsilon _v εv​ 的误差分别为1e-14方和1e-12的数量级，迭代效果完美，与所求的角度完全吻合，且因为雅可比不是通过微分求得，是直接建立得到，所以运算速度非常快，秒算出结果。

  对于这种需要收敛的逆运动学数值算法，初始估计值 θ 0 \theta _0 θ0​ 应尽量与真实值 θ d \theta _d θd​ 接近，只要从初始零位开始操作机器人，便可以满足这个条件。这时实际的末端位形与关节角已知，并确保所需要的的末端位置 T s d T_{sd} Tsd​ 相对逆运动学的计算频次缓慢发生变化。不断迭代更新 θ d \theta _d θd​ 直至满足要求。