机器人基础之雅克比矩阵

概述

雅克比矩阵$J(q)$是从关节空间向操作空间的速度传递的线性关系，借助于机械原理中的概念，可以理解为广义传动比。
对于$n$个关节的机器人，其雅克比矩阵是$6\times n$阶矩阵。其中前3行代表机器人末端坐标系线速度$v$的传递比；后3行代表对手爪的角速度$\omega$的传递比。另一方面，每一列代表相应的关节速度对末端坐标系线速度和角速度的传递比。因此，可将雅克比矩阵分块，即$$\left[\begin{array}{c}v\\ \omega \end{array}\right]=\left[\begin{array}{cccc}{J}_{l1}& J_{l2}& \cdot \cdot \cdot & J_{ln}\\ J_{a1}& J_{a2}& \cdot \cdot \cdot & J_{an}\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ {\dot{q}}_1\\ {\dot{q}}_2\\ \cdot \\ \cdot \\ \cdot \\ {\dot{q}}_n\end{array}\right]\text{\hspace{1em}(1)}$$

雅克比矩阵的构造

微分运动和广义速度

刚体或坐标系的微分运动包含微分移动矢量$d$和微分转动矢量$\delta$。前者由沿三个坐标轴的微分移动组成，后者由绕三坐标轴的微分转动组成。将两者合并为6维列矢量$D$，称为刚体或坐标系的微分运动矢量。$$D=\left[\begin{array}{c}d\\ \delta \end{array}\right]\text{\hspace{1em}(2)}$$ 相应地，刚体或坐标系的广义速度$V$是由线速度$v$和角速度$\omega$组成的6维列矢量。即$$V=\dot{D}=\left[\begin{array}{c}\dot{d}\\ \dot{\delta }\end{array}\right]\text{\hspace{1em}(3)}$$ 微分运动矢量D和广义速度V也是相对于某坐标系而言的，在不同坐标系中表达式不同。若坐标系{T}相对于基坐标系的齐次变换矩阵为 $$T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}n& o& a& p\end{array}\right]\text{\hspace{1em}(4)}$$ 则坐标系{T}中的微分运动${}^{T}D$和基坐标系中的微分运动$D$之间的关系为： $${}^{T}D=\left[\begin{array}{cccccc}{n}_x& n_y& a_x& (p\times n{)}_x& (p\times n{)}_y& (p\times n{)}_z\\ o_x& o_y& o_z& (p\times o{)}_x& (p\times o{)}_y& (p\times o{)}_z\\ a_x& a_y& a_z& (p\times a{)}_x& (p\times a{)}_y& (p\times a{)}_z\\ 0& 0& 0& n_x& n_y& a_x\\ 0& 0& 0& o_x& o_y& o_z\\ 0& 0& 0& a_x& a_y& a_z\end{array}\right]D\text{\hspace{1em}(5)}$$ 上式简写为 $${}^{T}D=\left[\begin{array}{cc}{R}^T& -R^{T}S(p)\\ 0& R^T\end{array}\right]D\text{\hspace{1em}(6)}$$ 其中 $$R=\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]\text{\hspace{1em}(7)}$$ 反对称矩阵$S(p)$定义为$$S(p)=\left[\begin{array}{ccc}0& -p_z& p_y\\ p_z& 0& p_x\\ -p_y& p_x& 0\end{array}\right]\text{\hspace{1em}(8)}$$ 类似的，任意两坐标系{A}和{B}之间微分运动的坐标变换为$${}^{B}D=\left[\begin{array}{cc}{}^B{R}^T& -{}_A^B{R}^{T}S({}^B{p}_{BO})\\ 0& {}_A^B{R}^T\end{array}\right]{}^{A}D\text{\hspace{1em}(9)}$$

微分变换法

下面采用构造性的方法，不需要求导而直接构造出$J_{li}$和$J_{ai}$。
以六自由度机械臂为例。

1. 首先计算出各连杆变换矩阵${}_1^{0}T$、${}_2^{1}T$、${}_3^{2}T$、${}_4^{3}T$、${}_5^{4}T$、${}_6^{5}T$。
2. 然后利用公式$${}_{i-1}^{i}T=\left[\begin{array}{cc}{}_i^{i-1}{R}^T& {}_i^{i-1}{R}^T{}^{i-1}{P}_{BO}\\ 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$计算得${}_0^{1}T$、${}_1^{2}T$、${}_2^{3}T$、${}_3^{4}T$、${}_4^{5}T$、${}_5^{6}T$。
3. 依次计算末端坐标系在各连杆坐标系中的变化矩阵${}_0^{6}T$、${}_1^{6}T$、${}_2^{6}T$、${}_3^{6}T$、${}_4^{6}T$、${}_5^{6}T$。
4. 利用公式9计算微分运动变化矩阵。
5. 若关节$i$为直线运动，则取微分运动变化矩阵的第三列作为雅克比矩阵的第$i$列；
   若关节$i$为旋转运动，则取微分运动变化矩阵的第六列作为雅克比矩阵的第$i$列；
   对于六自由度机械臂而言，显然取微分运动变化矩阵的第六列作为雅克比矩阵的第$i$列。

至此，构造出机器人的雅克比矩阵。

MATLAB实现

function J = calJacobi(DH_parameter)
% 计算雅克比矩阵
% DH_parameter：DH参数表；

[dim_row, dim_col] = size(DH_parameter);
J = zeros(6, dim_row);

for i = 1 : dim_row
    T = calT(DH_parameter, i - 1, dim_row);
    R = T(1:3, 1:3);
    P = T(1:3, 4);
    DiffM = [R', -R' * antiSymMatrix(P); zeros(3, 3), R'];
    J(:, i) = DiffM(:, 6);
end