【机器人学】微分运动
【机器人学】微分运动
一、预备知识
1.1 正逆运动学
1.2 齐次变换矩阵的逆
不妨假设已知坐标系
{
B
}
\{B\}
{B}相对于坐标系
{
A
}
\{A\}
{A}的齐次变换矩阵为
B
A
T
^A_BT
BAT,求坐标系
{
A
}
\{A\}
{A}相对于坐标系
{
B
}
\{B\}
{B}的齐次变换矩阵,那么我们知道:
B
A
T
=
[
B
A
R
A
p
⃗
B
o
0
⃗
1
]
=
[
n
o
a
p
0
0
0
1
]
,
A
B
T
=
[
A
B
R
B
p
⃗
A
o
0
⃗
1
]
^A_BT=\left[\begin{matrix}^A_BR&^A\vec{p}_{Bo}\\\vec{0}&1\end{matrix}\right]=\left[\begin{matrix}\bf{n}&\bf{o}&\bf{a}&\bf{p}\\0&0&0&1\end{matrix}\right],\\ ^B_AT=\left[\begin{matrix}^B_AR&^B\vec{p}_{Ao}\\\vec{0}&1\end{matrix}\right]
BAT=[BAR0ApBo1]=[n0o0a0p1],ABT=[ABR0BpAo1]
利用旋转矩阵正交性,我们可以得到:
B
A
R
−
1
=
B
A
R
T
=
A
B
R
^A_BR^{-1}=^A_BR^{T}=^B_AR
BAR−1=BART=ABR
对于
B
p
⃗
A
o
^B\vec{p}_{Ao}
BpAo,我们可以有如下推导:
B
(
A
p
⃗
B
o
)
=
0
⃗
=
A
B
R
A
p
⃗
B
o
+
B
p
⃗
A
o
^B\left(^A\vec{p}_{Bo}\right)=\vec{0}=^B_AR \ \ ^A\vec{p}_{Bo}+^B\vec{p}_{Ao}
B(ApBo)=0=ABR ApBo+BpAo
因此,
B
p
⃗
A
o
=
−
A
B
R
A
p
⃗
B
o
=
−
B
A
R
T
A
p
⃗
B
o
=
−
[
n
T
o
T
a
T
]
p
=
[
−
n
T
⋅
p
−
o
T
⋅
p
−
a
T
⋅
p
]
^B\vec{p}_{Ao}=-^B_AR\ \ ^A\vec{p}_{Bo}=-^A_BR^T\ \ ^A\vec{p}_{Bo}=-\left[\begin{matrix}{\bf n}^T\\{\bf o}^T\\{\bf a}^T\end{matrix}\right]{\bf p}=\left[\begin{matrix}-{\bf n}^T\cdot{\bf p}\\-{\bf o}^T\cdot{\bf p}\\-{\bf a}^T\cdot{\bf p}\end{matrix}\right]
BpAo=−ABR ApBo=−BART ApBo=−⎣
⎡nToTaT⎦
⎤p=⎣
⎡−nT⋅p−oT⋅p−aT⋅p⎦
⎤
综上所述,我们可以得到:
A
B
T
=
B
A
T
−
1
=
[
B
A
R
T
−
B
A
R
T
A
p
⃗
B
o
0
⃗
1
]
=
[
n
T
−
n
T
⋅
p
o
T
−
o
T
⋅
p
a
T
−
a
T
⋅
p
0
⃗
1
]
^B_AT=^A_BT^{-1}=\left[\begin{matrix}^A_BR^{T}&-^A_BR^T\ \ ^A\vec{p}_{Bo}\\\vec{0}&1\end{matrix}\right]=\left[\begin{matrix}{\bf n}^T&-{\bf n}^T\cdot{\bf p}\\{\bf o}^T&-{\bf o}^T\cdot{\bf p}\\{\bf a}^T&-{\bf a}^T\cdot{\bf p}\\\vec{0}&1\end{matrix}\right]
ABT=BAT−1=[BART0−BART ApBo1]=⎣
⎡nToTaT0−nT⋅p−oT⋅p−aT⋅p1⎦
⎤
二、坐标变换的微分
2.1 简单的微分引例
先看上图,经过简单推导不难得到:
v
B
=
v
A
+
v
B
/
A
=
l
1
θ
˙
1
+
l
2
(
θ
˙
1
+
θ
˙
2
)
=
−
l
1
sin
θ
˙
1
i
+
l
1
θ
˙
1
cos
θ
1
j
−
l
2
(
θ
˙
1
+
θ
˙
2
)
sin
(
θ
1
+
θ
2
)
i
+
l
2
(
θ
˙
1
+
θ
˙
2
)
cos
(
θ
1
+
θ
2
)
j
\begin{aligned} {\bf v}_B&={\bf v}_A+{\bf v}_{B/A}\\ &=l_1\dot{\theta}_1+l_2(\dot{\theta}_1+\dot{\theta}_2)\\ &=-l_1\sin\dot{\theta}_1{\bf{i}}+l_1\dot{\theta}_1\cos\theta_1{\bf j}-l_2(\dot{\theta}_1+\dot{\theta}_2)\sin(\theta_1+\theta_2){\bf i}+l_2(\dot{\theta}_1+\dot{\theta}_2)\cos(\theta_1+\theta_2){\bf j}\\ \end{aligned}
vB=vA+vB/A=l1θ˙1+l2(θ˙1+θ˙2)=−l1sinθ˙1i+l1θ˙1cosθ1j−l2(θ˙1+θ˙2)sin(θ1+θ2)i+l2(θ˙1+θ˙2)cos(θ1+θ2)j
以矩阵形式示之:
[
d
x
B
d
y
B
]
⏟
B
点位置微分
=
[
−
l
1
sin
θ
1
−
l
2
sin
(
θ
1
+
θ
2
)
−
l
2
sin
(
θ
1
+
θ
2
)
l
1
cos
θ
1
+
l
2
cos
(
θ
1
+
θ
2
)
l
2
cos
(
θ
1
+
θ
2
)
]
⏟
雅可比矩阵
[
d
θ
1
d
θ
2
]
⏟
关节微分
\underbrace{\left[\begin{matrix}dx_B\\dy_B\end{matrix}\right]}_{B点位置微分}= \underbrace{\left[\begin{matrix}-l_1\sin\theta_1-l_2\sin(\theta_1+\theta_2)&-l_2\sin(\theta_1+\theta_2)\\ l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2)&l_2\cos(\theta_1+\theta_2)\end{matrix}\right]}_{雅可比矩阵} \underbrace{\left[\begin{matrix}d\theta_1\\d\theta_2\end{matrix}\right]}_{关节微分}
B点位置微分
[dxBdyB]=雅可比矩阵
[−l1sinθ1−l2sin(θ1+θ2)l1cosθ1+l2cos(θ1+θ2)−l2sin(θ1+θ2)l2cos(θ1+θ2)]关节微分
[dθ1dθ2]
2.2 雅可比矩阵
2.2.1 定义
假设我们有一系列关于变量
x
j
(
j
=
1
,
2
,
…
,
m
)
x_j(j=1,2,\dots,m)
xj(j=1,2,…,m)的方程
y
i
(
i
=
1
,
2
,
…
,
n
)
y_i(i=1,2,\dots,n)
yi(i=1,2,…,n)如下:
y
i
=
f
i
(
x
1
,
x
2
,
…
,
x
m
)
y_i=f_i\left(x_1,x_2,\dots,x_m\right)
yi=fi(x1,x2,…,xm)
对每个
y
i
y_i
yi求其微分:
{
δ
y
1
=
∂
f
1
∂
x
1
δ
x
1
+
∂
f
1
∂
x
2
δ
x
2
+
⋯
+
∂
f
1
∂
x
m
δ
x
m
δ
y
2
=
∂
f
2
∂
x
1
δ
x
1
+
∂
f
2
∂
x
2
δ
x
2
+
⋯
+
∂
f
2
∂
x
m
δ
x
m
⋮
δ
y
n
=
∂
f
n
∂
x
1
δ
x
1
+
∂
f
n
∂
x
2
δ
x
2
+
⋯
+
∂
f
n
∂
x
m
δ
x
m
\left\{ \begin{array}{lr} \delta y_1=\frac{\partial f_1}{\partial x_1}\delta{x_1}+\frac{\partial f_1}{\partial x_2}\delta{x_2}+\cdots+\frac{\partial f_1}{\partial x_m}\delta{x_m}\\\\ \delta y_2=\frac{\partial f_2}{\partial x_1}\delta{x_1}+\frac{\partial f_2}{\partial x_2}\delta{x_2}+\cdots+\frac{\partial f_2}{\partial x_m}\delta{x_m}\\ \ \ \vdots\\ \delta y_n=\frac{\partial f_n}{\partial x_1}\delta{x_1}+\frac{\partial f_n}{\partial x_2}\delta{x_2}+\cdots+\frac{\partial f_n}{\partial x_m}\delta{x_m} \end{array} \right.
⎩
⎨
⎧δy1=∂x1∂f1δx1+∂x2∂f1δx2+⋯+∂xm∂f1δxmδy2=∂x1∂f2δx1+∂x2∂f2δx2+⋯+∂xm∂f2δxm ⋮δyn=∂x1∂fnδx1+∂x2∂fnδx2+⋯+∂xm∂fnδxm
那么,可以得到:
[
δ
y
1
δ
y
2
⋮
δ
y
n
]
=
[
∂
f
1
∂
x
1
∂
f
1
∂
x
2
⋯
∂
f
1
∂
x
m
∂
f
2
∂
x
1
∂
f
2
∂
x
2
⋯
∂
f
2
∂
x
m
⋮
⋮
⋯
⋮
∂
f
n
∂
x
1
∂
f
n
∂
x
2
⋯
∂
f
n
∂
x
m
]
⏟
雅可比矩阵
[
∂
x
1
∂
x
2
⋮
∂
x
m
]
\begin{aligned}\left[\begin{matrix}\delta y_1\\\delta y_2\\\vdots\\\delta y_n\end{matrix}\right]\end{aligned}= \underbrace{\begin{aligned}\left[\begin{matrix}\frac{\partial f_1}{\partial x_1}&\frac{\partial f_1}{\partial x_2}&\cdots&\frac{\partial f_1}{\partial x_m}\\\\\frac{\partial f_2}{\partial x_1}&\frac{\partial f_2}{\partial x_2}&\cdots&\frac{\partial f_2}{\partial x_m}\\\vdots&\vdots&\cdots&\vdots\\\frac{\partial f_n}{\partial x_1}&\frac{\partial f_n}{\partial x_2}&\cdots&\frac{\partial f_n}{\partial x_m}\end{matrix}\right]\end{aligned}}_{雅可比矩阵} \begin{aligned}\left[\begin{matrix}\partial x_1\\\partial x_2\\\vdots\\\partial x_m\end{matrix}\right]\end{aligned}
⎣
⎡δy1δy2⋮δyn⎦
⎤=雅可比矩阵
⎣
⎡∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋯⋯∂xm∂f1∂xm∂f2⋮∂xm∂fn⎦
⎤⎣
⎡∂x1∂x2⋮∂xm⎦
⎤
简单表达为:
[
D
]
=
[
J
]
[
D
θ
]
[D]=[J][D_\theta]
[D]=[J][Dθ]
2.2.2 计算
对于机器人末端执行器中常常使用的变量而言,一般含有六个变量,三个位置变量和三个方向变量,因此,可以表达如下:
以下计算不提供证明
- 矩阵元素几何计算
一般认为,讨论雅可比矩阵时关注的是机械臂的末端执行器主要是位置与方向的微分:
[
O
δ
p
⃗
O
δ
o
⃗
]
=
[
O
J
⃗
p
1
O
J
⃗
p
2
⋯
O
J
⃗
p
n
O
J
⃗
o
1
O
J
⃗
o
2
⋯
O
J
⃗
o
n
]
6
×
n
[
O
δ
θ
1
O
δ
θ
2
⋮
O
δ
θ
n
]
n
×
1
\begin{aligned}\left[\begin{matrix}^O\delta \vec{p}\\^O\delta \vec{o}\end{matrix}\right] =\left[\begin{matrix}^O\vec{J}_{p_1}&^O\vec{J}_{p_2}&\cdots&^O\vec{J}_{p_n}\\ ^O\vec{J}_{o_1}&^O\vec{J}_{o_2}&\cdots&^O\vec{J}_{o_n}\end{matrix}\right]_{6\times n} \left[\begin{matrix}^O\delta \theta_1\\^O\delta \theta_2\\\vdots\\^O\delta \theta_n\end{matrix}\right]_{n\times 1}\end{aligned}
[OδpOδo]=[OJp1OJo1OJp2OJo2⋯⋯OJpnOJon]6×n⎣
⎡Oδθ1Oδθ2⋮Oδθn⎦
⎤n×1
其中,雅可比矩阵各矢量的计算方法如下:
[
O
J
⃗
p
i
O
J
⃗
o
i
]
=
{
[
O
z
i
0
]
移动关节
[
O
z
i
×
(
O
p
⃗
n
−
O
p
⃗
i
)
O
z
i
]
转动关节
\begin{aligned}\left[\begin{matrix}^O\vec{J}_{p_i}\\^O\vec{J}_{o_i}\end{matrix}\right]\end{aligned}=\left\{\begin{array}{lr} \left[\begin{matrix}^O{\bf{z}}_{i}\\\bf{0}\end{matrix}\right]&移动关节\\ \left[\begin{matrix}^O{\bf{z}}_{i}\times (^O\vec{p}_n-^O\vec{p}_{i})\\^O{\bf{z}}_{i}\end{matrix}\right]&转动关节 \end{array}\right.
[OJpiOJoi]=⎩
⎨
⎧[Ozi0][Ozi×(Opn−Opi)Ozi]移动关节转动关节
举个小栗子:不想画图,就拿出某课堂教师笔记的简图来充数看一下。
其雅可比矩阵就可以表示为:
J
=
[
O
a
1
×
(
O
P
N
−
O
P
1
)
⋯
O
a
N
−
1
×
(
O
P
N
−
O
P
N
−
1
)
0
⃗
O
a
1
⋯
O
a
N
−
1
O
a
N
]
J=\begin{aligned}\left[\begin{matrix}^Oa_1\times (^OP_N-^OP_1)&\cdots&^Oa_{N-1}\times (^OP_N-^OP_{N-1})&\vec{0}\\ ^Oa_1&\cdots&^Oa_{N-1}&^Oa_N\end{matrix}\right]\end{aligned}
J=[Oa1×(OPN−OP1)Oa1⋯⋯OaN−1×(OPN−OPN−1)OaN−10OaN]
2.3 位置和方向微分
- 位置微分
假设位置矢量的微分为 d p ⃗ = [ d x , d y , d z ] T d\vec{p}=[dx,dy,dz]^T dp=[dx,dy,dz]T,那么对应的其次变换矩阵为 [ I d p ⃗ 0 ⃗ 1 ] \begin{aligned}\left[\begin{matrix}I&d\vec{p}\\\vec{0}&1\end{matrix}\right]\end{aligned} [I0dp1],多次位置微分的齐次变换矩阵为 [ I ∑ d p ⃗ i 0 ⃗ 1 ] \begin{aligned}\left[\begin{matrix}I&\sum d\vec{p}_i\\\vec{0}&1\end{matrix}\right]\end{aligned} [I0∑dpi1]。
- 方向微分
假设方向矢量的微分为
d
o
⃗
=
[
δ
n
,
δ
o
,
δ
a
]
T
d\vec{o}=[\delta n,\delta o,\delta a]^T
do=[δn,δo,δa]T,由于对于无穷小量而言有:
lim
δ
t
→
0
sin
δ
t
=
δ
t
,
cos
δ
t
=
1
\lim\limits_{\delta t\to 0}\sin\delta t=\delta t,\cos \delta t=1
δt→0limsinδt=δt,cosδt=1,因此:
R
o
t
(
x
,
δ
n
)
=
[
1
0
0
0
0
1
−
δ
n
0
0
δ
n
1
0
0
0
0
1
]
,
R
o
t
(
y
,
δ
o
)
=
[
1
0
δ
o
0
0
1
0
0
−
δ
o
0
1
0
0
0
0
1
]
,
R
o
t
(
z
,
δ
a
)
=
[
1
−
δ
a
0
0
δ
a
1
0
0
0
0
1
0
0
0
0
1
]
Rot(x,\delta n)=\begin{aligned}\left[\begin{matrix}1&0&0&0\\ 0&1&-\delta n&0\\ 0&\delta n&1&0\\0&0&0&1\end{matrix}\right]\end{aligned} ,Rot(y,\delta o)=\begin{aligned}\left[\begin{matrix}1&0&\delta o&0\\0&1&0&0\\-\delta o&0&1&0\\0&0&0&1\end{matrix}\right]\end{aligned},Rot(z,\delta a)=\begin{aligned}\left[\begin{matrix}1&-\delta a&0&0\\ \delta a&1&0&0\\0&0&1&0\\0&0&0&1\end{matrix}\right]\end{aligned}
Rot(x,δn)=⎣
⎡100001δn00−δn100001⎦
⎤,Rot(y,δo)=⎣
⎡10−δo00100δo0100001⎦
⎤,Rot(z,δa)=⎣
⎡1δa00−δa10000100001⎦
⎤
接下来,我们验证一件事:微分运动下,矩阵乘法满足交换律:
微分运动下,忽略高阶无穷小,那么上图两者的乘积结果相同,因此,对于微分
d
o
⃗
=
[
δ
n
,
δ
o
,
δ
a
]
T
d\vec{o}=[\delta n,\delta o,\delta a]^T
do=[δn,δo,δa]T,我们能够得到方向微分的齐次变换矩阵为:
T
R
o
t
a
t
i
o
n
=
[
1
−
δ
a
δ
o
0
δ
a
1
−
δ
n
0
−
δ
o
δ
n
1
0
0
0
0
1
]
T_{Rotation}=\begin{aligned}\left[\begin{matrix}1&-\delta a&\delta o&0\\\delta a&1&-\delta n&0\\-\delta o&\delta n&1&0\\0&0&0&1\end{matrix}\right]\end{aligned}
TRotation=⎣
⎡1δa−δo0−δa1δn0δo−δn100001⎦
⎤
2.4 微分算子
一个坐标系的微分变换是位置微分和方向微分以任何次序的结合:
[
T
+
d
T
]
=
[
T
r
a
n
s
(
d
x
,
d
y
,
d
z
)
R
o
t
(
q
,
d
o
⃗
)
]
]
[
T
]
[T+dT]=\begin{aligned}\left[\begin{matrix}Trans(dx,dy,dz)Rot(q,d\vec{o})]\end{matrix}\right]\end{aligned}[T]
[T+dT]=[Trans(dx,dy,dz)Rot(q,do)]][T]
因此变换矩阵的微分为:
[
d
T
]
=
[
T
r
a
n
s
(
d
x
,
d
y
,
d
z
)
R
o
t
(
q
,
d
o
⃗
)
−
I
]
[
T
]
=
[
Δ
]
[
T
]
[dT]=[Trans(dx,dy,dz)Rot(q,d\vec{o})-I][T]=[\Delta][T]
[dT]=[Trans(dx,dy,dz)Rot(q,do)−I][T]=[Δ][T]
则相对于固定坐标系的微分算子
[
Δ
]
[\Delta]
[Δ]满足:
[
Δ
]
=
T
r
a
n
s
(
d
x
,
d
y
,
d
z
)
×
R
o
t
(
q
,
d
o
⃗
)
−
I
4
=
[
1
0
0
d
x
0
1
0
d
y
0
0
1
d
z
0
0
0
1
]
[
1
−
δ
a
δ
o
0
δ
a
1
−
δ
n
0
−
δ
o
δ
n
1
0
0
0
0
1
]
−
I
4
=
[
0
δ
a
δ
o
d
x
δ
a
0
−
δ
n
d
y
δ
o
−
δ
n
0
d
z
0
0
0
1
]
\begin{aligned}\left[\Delta\right]&=Trans(dx,dy,dz)\times Rot(q,d\vec{o})-I_4\\ &=\left[\begin{matrix}1&0&0&dx\\0&1&0&dy\\0&0&1&dz\\0&0&0&1\end{matrix}\right]\left[\begin{matrix}1&-\delta a&\delta o&0\\ \delta a&1&-\delta n&0\\ -\delta o&\delta n&1&0\\0&0&0&1\end{matrix}\right]-I_4\\ &=\left[\begin{matrix}0&\delta a&\delta o&dx\\ \delta a&0&-\delta n&dy\\ \delta o&-\delta n&0&dz\\0&0&0&1\end{matrix}\right]\end{aligned}
[Δ]=Trans(dx,dy,dz)×Rot(q,do)−I4=⎣
⎡100001000010dxdydz1⎦
⎤⎣
⎡1δa−δo0−δa1δn0δo−δn100001⎦
⎤−I4=⎣
⎡0δaδo0δa0−δn0δo−δn00dxdydz1⎦
⎤
结合之前所学,我们知道,坐标系的变化有相对于固定坐标系和相对于自身坐标系两种,上述得到的是相对于固定坐标系的微分算子,那么,我们假设相对于自身坐标系的微分算子为
T
Δ
^T\Delta
TΔ。
[
d
T
]
=
[
Δ
]
T
=
T
[
T
Δ
]
⟶
[
T
Δ
]
=
T
−
1
[
Δ
]
T
⟶
[
Δ
]
=
T
[
T
Δ
]
T
−
1
\begin{aligned}&\left[dT\right]=[\Delta]T=T[^T\Delta]\\ \longrightarrow&[^T\Delta]=T^{-1}[\Delta]T\\ \longrightarrow&[\Delta]=T[^T\Delta]T^{-1} \end{aligned}
⟶⟶[dT]=[Δ]T=T[TΔ][TΔ]=T−1[Δ]T[Δ]=T[TΔ]T−1
其中,
T
−
1
=
[
n
T
−
n
T
⋅
p
o
T
−
o
T
⋅
p
a
T
−
a
T
⋅
p
0
⃗
1
]
T^{-1}=\left[\begin{matrix}{\bf n}^T&-{\bf n}^T\cdot{\bf p}\\{\bf o}^T&-{\bf o}^T\cdot{\bf p}\\{\bf a}^T&-{\bf a}^T\cdot{\bf p}\\\vec{0}&1\end{matrix}\right]
T−1=⎣
⎡nToTaT0−nT⋅p−oT⋅p−aT⋅p1⎦
⎤
三、机器人的微分运动
机器人的运动是由两种关节驱动的,一种是旋转关节,一种是移动关节,我们都定义为
δ
θ
i
\delta \theta_i
δθi,那么机器人的变换后的其次变换矩阵可以表达为:
T
n
e
x
=
T
o
l
d
+
d
T
=
T
o
l
d
+
[
Δ
]
T
o
l
d
=
T
o
l
d
+
T
o
l
d
[
T
Δ
]
T_{nex}=T_{old}+dT=T_{old}+[\Delta]T_{old}=T_{old}+T_{old}[^T\Delta]
Tnex=Told+dT=Told+[Δ]Told=Told+Told[TΔ]
其雅可比矩阵可以表示为:
[
O
δ
p
⃗
O
δ
o
⃗
]
=
[
O
J
⃗
p
1
O
J
⃗
p
2
⋯
O
J
⃗
p
n
O
J
⃗
o
1
O
J
⃗
o
2
⋯
O
J
⃗
o
n
]
6
×
n
[
O
δ
θ
1
O
δ
θ
2
⋮
O
δ
θ
n
]
n
×
1
\begin{aligned}\left[\begin{matrix}^O\delta \vec{p}\\^O\delta \vec{o}\end{matrix}\right] =\left[\begin{matrix}^O\vec{J}_{p_1}&^O\vec{J}_{p_2}&\cdots&^O\vec{J}_{p_n}\\ ^O\vec{J}_{o_1}&^O\vec{J}_{o_2}&\cdots&^O\vec{J}_{o_n}\end{matrix}\right]_{6\times n} \left[\begin{matrix}^O\delta \theta_1\\^O\delta \theta_2\\\vdots\\^O\delta \theta_n\end{matrix}\right]_{n\times 1}\end{aligned}
[OδpOδo]=[OJp1OJo1OJp2OJo2⋯⋯OJpnOJon]6×n⎣
⎡Oδθ1Oδθ2⋮Oδθn⎦
⎤n×1
[ O J ⃗ p i O J ⃗ o i ] = { [ O z i 0 ] 移动关节 [ O z i × ( O p ⃗ n − O p ⃗ i ) O z i ] 转动关节 \begin{aligned}\left[\begin{matrix}^O\vec{J}_{p_i}\\^O\vec{J}_{o_i}\end{matrix}\right]\end{aligned}=\left\{\begin{array}{lr} \left[\begin{matrix}^O{\bf{z}}_{i}\\\bf{0}\end{matrix}\right]&移动关节\\ \left[\begin{matrix}^O{\bf{z}}_{i}\times (^O\vec{p}_n-^O\vec{p}_{i})\\^O{\bf{z}}_{i}\end{matrix}\right]&转动关节 \end{array}\right. [OJpiOJoi]=⎩ ⎨ ⎧[Ozi0][Ozi×(Opn−Opi)Ozi]移动关节转动关节