矢量积法求解雅可比矩阵

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

另一方面,每一列代表相应的关节速度 q i ˙ \dot{q_{i}} qi˙ 对末端执行器线速度和角速度的传递比。
x ˙ = J ( q ) q ˙ (1) \dot{x}=J(q)\dot{q} \tag{1} x˙=J(q)q˙(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]=[Jl1Ja1Jl2Ja2Jl3Ja3Jl4Ja4Jl5Ja5Jl6Ja6] q1˙q2˙q3˙q4˙q5˙q6˙ (2)

{ v = J l 1 q 1 ˙ + J l 2 q 2 ˙ + J l 3 q 3 ˙ ) + J l 4 q 4 ˙ + J l 5 q 5 ˙ + J l 6 q 6 ˙ w = J a 1 q 1 ˙ + J a 2 q 2 ˙ + J a 3 q 3 ˙ + J a 4 q 4 ˙ + J a 5 q 5 ˙ + J a 6 q 6 ˙ (3) \left\{\begin{matrix} 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{matrix}\right. \tag{3} {v=Jl1q1˙+Jl2q2˙+Jl3q3˙)+Jl4q4˙+Jl5q5˙+Jl6q6˙w=Ja1q1˙+Ja2q2˙+Ja3q3˙+Ja4q4˙+Ja5q5˙+Ja6q6˙(3)
公式(3)中, J l i J_{li} Jli J a i J_{ai} Jai 分别表示关节 i i i 的单位关节速度引起的末端执行器的线速度和角速度。

下面基于矢量积法,不求导而直接构造出 J l i J_{li} Jli J a i J_{ai} Jai

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

(1) 对于移动关节 i i i,在末端执行器上产生与 z i z_{i} zi 相同的线速度 v v v
[ v w ] = [ z i 0 ] q i ˙ ⇒ J i = [ z i 0 ] (4) \begin{bmatrix} v\\ w \end{bmatrix}=\begin{bmatrix} z_{i}\\ 0 \end{bmatrix}\dot{q_{i}} \Rightarrow J_{i}=\begin{bmatrix} z_{i}\\ 0 \end{bmatrix} \tag{4} [vw]=[zi0]qi˙Ji=[zi0](4)
(2) 对于旋转关节 i i i ,在末端执行器上产生的角速度 w w w 为:
w = z i q i ˙ (5) w=z_{i}\dot{q_{i}} \tag{5} w=ziqi˙(5)
同时在末端执行器上产生的线速度为矢量积,即:
v = ( z i × i p n 0 ) q ˙ i (6) v=(z_{i} \times {^{i}p^{0}_{n}}) \dot{q}_{i} \tag{6} v=(zi×ipn0)q˙i(6)
因此,雅可比矩阵的第 i i i 列为:
J i = [ z i × i p n 0 z i ] = [ z i × ( i 0 R i p n ) z i ] (7) J_{i}=\begin{bmatrix} z_{i} \times {^{i}p^{0}_{n}}\\ z_{i} \end{bmatrix}=\begin{bmatrix} z_{i} \times ({^{0}_{i}R {^{i}p_{n}}})\\ z_{i} \end{bmatrix} \tag{7} Ji=[zi×ipn0zi]=[zi×(i0Ripn)zi](7)
其中, z i z_{i} zi 是坐标系 { i i i} 的 z 轴单位矢量(在基座坐标系{0}中表示)。

公式(7)中, i p n 0 ^{i}p^{0}_{n} ipn0 表示末端执行器坐标原点想对于坐标系 { i i i} 的位置矢量在基坐标系{ 0 0 0}中的表示,即:
i p n 0 = i 0 R i p n (8) ^{i}p^{0}_{n}={^{0}_{i}R{^{i}p_{n}}} \tag{8} ipn0=i0Ripn(8)
有时,要求沿着工具坐标系的某轴进行控制,因此需要将线速度和角速度在工具坐标系{ T T T}中进行表示。为此,需要在 v v v w w w 前乘以 3 × 3 3\times3 3×3 的旋转矩阵 n 0 R T ^{0}_{n}R^{T} n0RT,即:
[ n v n w ] = [ n 0 R 0 0 n 0 R T ] [ v w ] = [ n 0 R 0 0 n 0 R T ] J ( q ) q ˙ = T J ( q ) q ˙ (9) \begin{bmatrix} ^{n}v\\ ^{n}w \end{bmatrix}=\begin{bmatrix} ^{0}_{n}R & 0\\ 0 & ^{0}_{n}R^{T} \end{bmatrix}\begin{bmatrix} v\\ w \end{bmatrix}=\begin{bmatrix} ^{0}_{n}R & 0\\ 0 & ^{0}_{n}R^{T} \end{bmatrix}J(q)\dot{q}={^{T}J(q)\dot{q}} \tag{9} [nvnw]=[n0R00n0RT][vw]=[n0R00n0RT]J(q)q˙=TJ(q)q˙(9)
公式(9)中, T J ( q ) {^{T}J(q)} TJ(q)表示在工具坐标系{ T T T}中的雅可比矩阵。

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

G . P a n G.Pan G.Pan 先生原创文章,禁止抄袭,转载请注明出处,谢谢

你可能感兴趣的:(笔记,矩阵,线性代数)