机器人雅可比矩阵

一、雅可比矩阵的作用

机器人雅克比矩阵描述的是关节速度和末端笛卡尔速度和角速度之间的关系。它的行数等于机器人在空间中自由度的数目，列数等于机器人关节的数目。

二、雅可比矩阵的求解

1、矢量积法

对于移动关节${\mathbf{z}}_i$：
仅有移动，仅对末端执行器产生一个与${\mathbf{z}}_i$方向相同的角速度，大小为${\dot{q}}_i$，因此：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right]=\left[\begin{array}{c}0\\ {\mathbf{z}}_i\end{array}\right]{\dot{q}}_i$$
所以，雅可比矩阵第$i$列为：
$${\mathbf{J}}_i=\left[\begin{array}{c}0\\ {\mathbf{z}}_i\end{array}\right]$$
对于转动关节${\mathbf{z}}_i$：
仅有转动，对末端执行器产生一个大小为${\dot{q}}_i$，方向与${\mathbf{z}}_i$方向相同的角速度，以及一个$({\mathbf{z}}_i{\times }^0{\mathbf{p}}_i{\mathbf{p}}_e){\dot{q}}_i$的线速度,因此：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right]=\left[\begin{array}{c}{\mathbf{z}}_i{\times }^0{\mathbf{p}}_i{\mathbf{p}}_e\\ {\mathbf{z}}_i\end{array}\right]{\dot{q}}_i$$
其中，${}^0{\mathbf{p}}_i{\mathbf{p}}_e$为末端执行器原点相对于坐标系{$i$}的位置矢量在世界坐标系{$0$}下的表示，故${}^0{\mathbf{p}}_i{\mathbf{p}}_e$=${}^0{\mathbf{R}}_i{\cdot }^i{\mathbf{p}}_i{\mathbf{p}}_e$
所以，雅可比矩阵第$i$列为：
$${\mathbf{J}}_i=\left[\begin{array}{c}{\mathbf{z}}_i\times {(}^0{\mathbf{R}}_i{\cdot }^i{\mathbf{p}}_i{\mathbf{p}}_e)\\ {\mathbf{z}}_i\end{array}\right]$$

2、微分变换法

绕$x,y,z$旋转$\theta$角的旋转矩阵为：
$$\mathbf{R}(x,\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]\mathbf{R}(y,\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\mathbf{R}(z,\theta )=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]$$
当$\theta$极小时$sin\theta =\theta$,$cos\theta =1$,记分别绕$x,y,z$旋转的角度为${\delta }_x,{\delta }_y,{\delta }_z$,则绕$x,y,z$的旋转矩阵可以写为：
$$\mathbf{R}(x,{\delta }_x)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& -{\delta }_x\\ 0& {\delta }_x& 1\end{array}\right]\mathbf{R}(y,{\delta }_y)=\left[\begin{array}{ccc}1& 0& {\delta }_y\\ 0& 1& 0\\ -{\delta }_y& 0& 1\end{array}\right]\mathbf{R}(z,{\delta }_z)=\left[\begin{array}{ccc}1& -{\delta }_z& 0\\ {\delta }_z& 1& 0\\ 0& 0& 1\end{array}\right]$$
因此，旋转矩阵可以写为：
$$\mathbf{R}=\mathbf{R}(x,{\delta }_x)\mathbf{R}(y,{\delta }_y)\mathbf{R}(z,{\delta }_z)=\left[\begin{array}{ccc}1& -{\delta }_z& {\delta }_y\\ {\delta }_z& 1& -{\delta }_x\\ -{\delta }_y& {\delta }_x& 1\end{array}\right]$$
当微分平移向量为${\mathbf{\delta }}_d=d_x\mathbf{i}+d_y\mathbf{j}+d_z\mathbf{k}$时，结合上述旋转矩阵可以写出变换矩阵为：
$$\mathbf{T}=\left[\begin{array}{cccc}1& -{\delta }_z& {\delta }_y& d_x\\ {\delta }_z& 1& -{\delta }_x& d_y\\ -{\delta }_y& {\delta }_x& 1& d_z\\ 0& 0& 0& 1\end{array}\right]$$
对变换矩阵${}^i{\mathbf{T}}_{i+1}$,它对坐标系{$i$}的微分变换可以表示为：
$$\begin{gathered} {}^i{\mathbf{T}}_{i+1}+d^i{\mathbf{T}}_{i+1}{=}^i{\mathbf{T}}_{i+1}\cdot \mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d) \\ {d}^i{\mathbf{T}}_{i+1}{=}^i{\mathbf{T}}_{i+1}\cdot (\mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d)-\mathbf{I}) \end{gathered}$$
令${}^i{\Delta }_{i,i+1}=\mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d)-\mathbf{I}$,其中${\delta }_x,{\delta }_y,{\delta }_z$分别为绕坐标系{$i$}的$x,y,z$轴的旋转角度，$d$为坐标系{$i$}下的位置矢量，${}^i{\Delta }_{i,i+1}$为坐标系{$i$}下的微分变换算子。
对变换矩阵${}^i{\mathbf{T}}_{i+1}$,它对坐标系{$i+1$}的微分变换可以表示为：
$$\begin{gathered} {}^i{\mathbf{T}}_{i+1}+d^i{\mathbf{T}}_{i+1}=\mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d){\cdot }^i{\mathbf{T}}_{i+1} \\ {d}^i{\mathbf{T}}_{i+1}=(\mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d)-\mathbf{I}){\cdot }^i{\mathbf{T}}_{i+1} \end{gathered}$$
令${}^{i+1}{\Delta }_{i,i+1}=\mathbf{Tran}({\delta }_x,{\delta }_y,{\delta }_z,d)-\mathbf{I}$,其中${\delta }_x,{\delta }_y,{\delta }_z$分别为绕坐标系{$i+1$}的$x,y,z$轴的旋转角度，$d$为坐标系{$i$}下的位置矢量，${}^{i+1}{\Delta }_{i,i+1}$为坐标系{$i+1$}下的微分变换算子。
微分变换算子$\Delta$：
$$\Delta =\left[\begin{array}{cccc}0& -{\delta }_z& {\delta }_y& d_x\\ {\delta }_z& 0& -{\delta }_x& d_y\\ -{\delta }_y& {\delta }_x& 0& d_z\\ 0& 0& 0& 0\end{array}\right]$$



由机器人末端相对基坐标系的齐次变换矩阵为：
$${}^0{\mathbf{T}}_6{=}^0{\mathbf{T}}_1{\cdot }^1{\mathbf{T}}_2{\cdot }^2{\mathbf{T}}_3{\cdot }^3{\mathbf{T}}_4{\cdot }^4{\mathbf{T}}_5{\cdot }^5{\mathbf{T}}_6$$
第$i$个关节的微分变换引起机器人末端的微分变换为：
$$d{(}^0{\mathbf{T}}_6){=}^0{\mathbf{T}}_6{\cdot }^6{\Delta }_{0,6}\cdot d{q}_i$$
由${}^{i+1}{\Delta }_{i,i+1}{=}^i{\mathbf{T}}_{i+1}^{-1}{\cdot }^i{\Delta }_{i,i+1}{\cdot }^i{\mathbf{T}}_{i+1}$可得：
$${}^6{\Delta }_{i,6}{=}^i{\mathbf{T}}_6^{-1}{\cdot }^i{\Delta }_{i,6}{\cdot }^i{\mathbf{T}}_6$$
其中，${}^i{\mathbf{T}}_6{=}^i{\mathbf{T}}_{i+1}{\cdot }^{i+1}{\mathbf{T}}_{i+2}\cdot \cdot {\cdot }^5{\mathbf{T}}_6$，故：
$${}^6{\Delta }_{i,6}={(}^i{\mathbf{T}}_{i+1}{\cdot }^{i+1}{\mathbf{T}}_{i+2}\cdot \cdot {\cdot }^5{\mathbf{T}}_6{)}^{-1}{\cdot }^i{\Delta }_{i,6}\cdot {(}^i{\mathbf{T}}_{i+1}{\cdot }^{i+1}{\mathbf{T}}_{i+2}\cdot \cdot {\cdot }^5{\mathbf{T}}_6)$$
$${}^i{\mathbf{T}}_6=\left[\begin{array}{cccc}{}^i{n}_x& {}^i{o}_x& {}^i{a}_x& {}^i{p}_x\\ {}^i{n}_y& {}^i{o}_y& {}^i{a}_y& {}^i{p}_y\\ {}^i{n}_z& {}^i{o}_z& {}^i{a}_z& {}^i{p}_z\\ 0& 0& 0& 1\end{array}\right]$$
$$\left[\begin{array}{c}{}^6{d}_x\\ {}^6{d}_y\\ {}^6{d}_z\\ {}^6{\delta }_x\\ {}^6{\delta }_x\\ {}^6{\delta }_x\end{array}\right]=\left[\begin{array}{cccccc}{}^i{n}_x& {}^i{n}_y& {}^i{n}_z& {(}^{i}p{\times }^{i}n{)}_x& {(}^{i}p{\times }^{i}n{)}_y& {(}^{i}p{\times }^{i}n{)}_z\\ {}^i{o}_x& {}^i{o}_y& {}^i{o}_z& {(}^{i}p{\times }^{i}o{)}_x& {(}^{i}p{\times }^{i}o{)}_y& {(}^{i}p{\times }^{i}o{)}_z\\ {}^i{a}_x& {}^i{a}_y& {}^i{a}_z& {(}^{i}p{\times }^{i}a{)}_x& {(}^{i}p{\times }^{i}a{)}_y& {(}^{i}p{\times }^{i}a{)}_z\\ 0& 0& 0& {}^i{n}_x& {}^i{n}_y& {}^i{n}_z\\ 0& 0& 0& {}^i{o}_x& {}^i{o}_y& {}^i{o}_z\\ 0& 0& 0& {}^i{a}_x& {}^i{a}_y& {}^i{a}_z\end{array}\right]\left[\begin{array}{c}{d}_{ix}\\ d_{iy}\\ d_{iz}\\ {\delta }_{ix}\\ {\delta }_{iy}\\ {\delta }_{iz}\end{array}\right]$$
对于移动关节$i$：
$$d_i=\left[\begin{array}{ccc}{d}_{ix}& d_{iy}& d_{iz}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 1\end{array}\right],{\delta }_i=\left[\begin{array}{ccc}{\delta }_{ix}& {\delta }_{iy}& {\delta }_{iz}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$$
因此：
$${\mathbf{J}}_{li}=\left[\begin{array}{c}{}^i{n}_z\\ {}^i{o}_z\\ {}^i{a}_z\end{array}\right],{\mathbf{J}}_{ai}=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$
对于转动关节$i$：
$$d_i=\left[\begin{array}{ccc}{d}_{ix}& d_{iy}& d_{iz}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\end{array}\right],{\delta }_i=\left[\begin{array}{ccc}{\delta }_{ix}& {\delta }_{iy}& {\delta }_{iz}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$$
因此：
$${\mathbf{J}}_{li}=\left[\begin{array}{c}{(}^{i}p{\times }^{i}n{)}_z\\ {(}^{i}p{\times }^{i}o{)}_z\\ {(}^{i}p{\times }^{i}z{)}_z\end{array}\right],{\mathbf{J}}_{ai}=\left[\begin{array}{c}{}^i{n}_z\\ {}^i{o}_z\\ {}^i{a}_z\end{array}\right]$$

3、世界坐标系下的雅可比矩阵

上述雅可比矩阵为工具坐标系{$e$}下的关节速度与末端速度、角速度之间的映射,即工具坐标系{$e$}下的雅可比矩阵。
世界坐标系{$0$}下的末端速度、角速度与工具坐标系{$e$}下的末端速度、角速度之间的转换关系为：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right]=\left[\begin{array}{cc}{}^0{\mathbf{R}}_e& 0\\ 0& {}^0{\mathbf{R}}_e\end{array}\right]\left[\begin{array}{c}{}^e\mathbf{v}\\ {}^e\mathbf{w}\end{array}\right]$$
因此，当想要获得世界坐标系{$0$}下的关节速度与末端速度、角速度之间的映射即世界坐标系{$0$}下的雅可比矩阵时，只需要将工具坐标系{$e$}下的雅可比矩阵左乘世界坐标系{$0$}与工具坐标系{$e$}之间的旋转矩阵${}^0{\mathbf{R}}_e$,即：
$${\mathbf{J}}_0=\left[\begin{array}{cc}{}^0{\mathbf{R}}_e& 0\\ 0& {}^0{\mathbf{R}}_e\end{array}\right]{\mathbf{J}}_e$$