矢量积法求解雅可比矩阵

雅可比矩阵 $J(q)$ 可以看成是关节空间的速度矢量 $\dot{q}$ 向笛卡尔空间的速度矢量 $\dot{x}$ 的线性映射。雅可比矩阵 $J(q)$ 依赖于机器人的构形，是一个依赖于 $q$ 的线性变换矩阵，雅可比矩阵 $J(q)$ 不一定是方阵，它可能是长矩阵，也可能是高矩阵，$J(q)$ 的行数等于机器人在笛卡尔空间中的自由度数，比如平面操作臂的雅可比矩阵有3行，6关节的空间操作臂的雅可比矩阵是6行，对于 $n$ 关节的机器人，其雅可比矩阵 $J\in {\Re }^{6\times n}$ 是 $6\times n$的矩阵，其中前3行表示对末端执行器线速度 $v$ 的传递比，后3行表示对末端执行器角速度 $w$ 的传递比。

另一方面，每一列代表相应的关节速度 $\dot{q_i}$ 对末端执行器线速度和角速度的传递比。
$$\dot{x}=J(q)\dot{q}\text{\hspace{1em}(1)}$$

[ v w ] = [ J l 1 J l 2 J l 3 J l 4 J l 5 J l 6 J a 1 J a 2 J a 3 J a 4 J a 5 J a 6 ] [ q 1 ˙ q 2 ˙ q 3 ˙ q 4 ˙ q 5 ˙ q 6 ˙ ] (2) \begin{bmatrix} v\\ w \end{bmatrix}=\begin{bmatrix} J_{l1} & J_{l2} & J_{l3} & J_{l4} & J_{l5} & J_{l6}\\ J_{a1} & J_{a2} & J_{a3} & J_{a4} & J_{a5} & J_{a6} \end{bmatrix}\begin{bmatrix} \dot{q_{1}}\\ \dot{q_{2}}\\ \dot{q_{3}}\\ \dot{q_{4}}\\ \dot{q_{5}}\\ \dot{q_{6}} \end{bmatrix} \tag{2} [vw​]=[Jl1​Ja1​​Jl2​Ja2​​Jl3​Ja3​​Jl4​Ja4​​Jl5​Ja5​​Jl6​Ja6​​] ​q1​˙​q2​˙​q3​˙​q4​˙​q5​˙​q6​˙​​ ​(2)

$$\{\begin{array}{c}v=J_{l1}\dot{q_1}+J_{l2}\dot{q_2}+J_{l3}\dot{q_3})+J_{l4}\dot{q_4}+J_{l5}\dot{q_5}+J_{l6}\dot{q_6}\\ w=J_{a1}\dot{q_1}+J_{a2}\dot{q_2}+J_{a3}\dot{q_3}+J_{a4}\dot{q_4}+J_{a5}\dot{q_5}+J_{a6}\dot{q_6}\end{array}\text{\hspace{1em}(3)}$$
公式(3)中，$J_{li}$ 和 $J_{ai}$ 分别表示关节 $i$ 的单位关节速度引起的末端执行器的线速度和角速度。

下面基于矢量积法，不求导而直接构造出 $J_{li}$ 和 $J_{ai}$。

Whitney于1972年提出雅可比矩阵的矢量积构造方法。线速度和角速度分别使用 $v$ 和 $w$ 来表示。而 $v$ 和 $w$ 与关节速度 $\dot{q_i}$ 有关。

(1) 对于移动关节 $i$，在末端执行器上产生与 $z_i$ 相同的线速度 $v$ 。
$$\left[\begin{array}{c}v\\ w\end{array}\right]=\left[\begin{array}{c}{z}_i\\ 0\end{array}\right]\dot{q_i}\Rightarrow J_i=\left[\begin{array}{c}{z}_i\\ 0\end{array}\right]\text{\hspace{1em}(4)}$$
(2) 对于旋转关节 $i$ ，在末端执行器上产生的角速度 $w$ 为：
$$w=z_i\dot{q_i}\text{\hspace{1em}(5)}$$
同时在末端执行器上产生的线速度为矢量积，即：
$$v=(z_i\times {}^i{p}_n^0){\dot{q}}_i\text{\hspace{1em}(6)}$$
因此，雅可比矩阵的第 $i$ 列为：
$$J_i=\left[\begin{array}{c}{z}_i\times {}^i{p}_n^0\\ z_i\end{array}\right]=\left[\begin{array}{c}{z}_i\times ({}_i^{0}R{}^i{p}_n)\\ z_i\end{array}\right]\text{\hspace{1em}(7)}$$
其中，$z_i$ 是坐标系 {$i$} 的 z 轴单位矢量（在基座坐标系{0}中表示）。

公式(7)中，${}^i{p}_n^0$ 表示末端执行器坐标原点想对于坐标系 {$i$} 的位置矢量在基坐标系{$0$}中的表示，即：
$${}^i{p}_n^0={}_i^{0}R{}^i{p}_n\text{\hspace{1em}(8)}$$
有时，要求沿着工具坐标系的某轴进行控制，因此需要将线速度和角速度在工具坐标系{$T$}中进行表示。为此，需要在 $v$ 和 $w$ 前乘以 $3\times 3$ 的旋转矩阵 ${}_n^0{R}^T$，即：
$$\left[\begin{array}{c}{}^{n}v\\ {}^{n}w\end{array}\right]=\left[\begin{array}{cc}{}_n^{0}R& 0\\ 0& {}_n^0{R}^T\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right]=\left[\begin{array}{cc}{}_n^{0}R& 0\\ 0& {}_n^0{R}^T\end{array}\right]J(q)\dot{q}={}^{T}J(q)\dot{q}\text{\hspace{1em}(9)}$$
公式(9)中，${}^{T}J(q)$表示在工具坐标系{$T$}中的雅可比矩阵。

——————————————————————————-——————————————————————

$G.Pan$ 先生原创文章，禁止抄袭，转载请注明出处，谢谢