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

一. 逆运动学的解析求解

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

二. 逆运动学的数值求解

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

2.1 牛顿-拉夫森法

  对于给定的微分方程 $g\left(\theta \right)$，数值求解方程 $g\left(\theta \right)=0$。假定 ${\theta }^0$ 为初值，利用泰勒级数在 ${\theta }^0$ 出展开，并截取到第一项，得$$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\left(\theta \right)=0$ ，求解 $\theta$ 。得到$$\theta ={\theta }^0-{\left(\frac{\partial g}{\partial \theta }\left({\theta }^0\right)\right)}^{-1}g\left({\theta }^0\right)$$再将上式求得值作为初值，代入上述方程中，重复求解，得到下面方程$${\theta }^{k+1}={\theta }^k-{\left(\frac{\partial g}{\partial \theta }\left({\theta }^k\right)\right)}^{-1}g\left({\theta }^k\right)$$上述过程不断迭代，直至满足某个指标值。

2.2 逆动力学的数值算法

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

1. 初始化：已知 $T_{sd}$ ，初始估计值 ${\theta }^0\in R^n$ ，设定i=0.
2. 设定 $\left[V_b\right]=\log \left(T_{sb}^{-1}\left({\theta }^i\right)T_{sd}\right)$ ，当 $\Vert w_b\Vert >{\epsilon }_w$ 或者 $\Vert v_b\Vert >{\epsilon }_v$ （${\epsilon }_w$ 和 ${\epsilon }_v$ 为很小值）。
3. 设定 ${\theta }^{i+1}={\theta }^i+J^{†}\left({\theta }^i\right)V_b$。
4. 增加i

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

2.3 雅可比矩阵的求解

  根据以上的步骤，除了物体雅可比矩阵的求解，其余量我们均已经得到，故下面开始求解物体雅可比矩阵。回顾之前的知识，对于n杆串联机器人，其正向运动学的指数积公式为$$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$$空间速度 $V_s$ 可以写成 $\left[V_s\right]=\dot{T}{T}^{-1}$ ，计算可得$$\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$$通过伴随映射写成向量的形式，即
$$V_s=S_1{\dot{\theta }}_1+A{d}_{e^{\left[S_1\right]{\theta }_1}}\left(S_2\right){\dot{\theta }}_2+A{d}_{e^{\left[S_1\right]{\theta }_1}{e}^{\left[S_2\right]{\theta }_2}}\left(S_3\right){\dot{\theta }}_3+\cdots$$ 观察上式可写成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$$写成矩阵的形式，即$$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\\ ⋮\\ {\dot{\theta }}_n\end{array}\right]=J_s\left(\theta \right)\dot{\theta }$$ 矩阵 $J_s\left(\theta \right)$ 即为空间固定坐标形式下的雅可比矩阵，简称空间雅可比。
再将空间雅可比 $J_s$ 转化为物体雅可比矩阵 $J_b$ 。$$J_b\left(\theta \right)=A{d}_{T_{bs}}\left(J_s\left(\theta \right)\right)$$
至此，全部未知量我们均已得到，可以应用算法进行建模了。

算例：对于6R开链机器人的逆向运动学

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

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

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

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