机器人静力学与雅可比矩阵
引言
两大问题
-
问题一：机器人静力学建模问题。已知外界环境对机器人手部作用力，求相应的满足静力学平衡条件的关节驱动力矩。
输入力与输出力间的传递(映射)关系
-
问题二：刚度问题。在外力和负载的作用下，机器人末端执行器会偏离理想的位置。
载荷与变形间对应关系
虚功与虚功原理
虚功原理：具有理想约束力学体系，其平衡的充要条件是所有主动力在任意虚位移中所作元功之和等于零。
虚功：作用在质点上的力(含约束力)在任意虚位移中所作的功。
虚位移：质点在约束允许情况下可能发生的无限小位置变更。
操作臂力和力矩的平衡
关节驱动力计算
对于每个杆件而言，考虑
负载力
(从末端传递到各关节)，
重力
(作用于质心)，和
关节力
(关节载荷，由此获得关节驱动力)
计算时，采用前置DH法，即改进的DH(MDH)法创建坐标系。
使用MDH法，使得每个杆件的坐标系建立在输入端，便于在列写平衡方程时，力与杆件的下标相对应，不易出现错误。
在这其中：
-
fi
f_i
f
i
：连杆i-1作用在连杆i上的力 -
Mi
M_i
M
i
：连杆i-1作用在连杆i上的力矩 -
fi
+
1
f_{i+1}
f
i
+
1
：连杆i作用在连杆i+1上的力 -
Mi
+
1
M_{i+1}
M
i
+
1
：连杆i作用在连杆i+1上的力矩 -
mi
g
m_i g
m
i
g
：连杆i的重力，作用在质心上 -
ir
c
i
^ir_{ci}
i
r
c
i
：连杆i上的质心位置 -
iP
i
+
1
^iP_{i+1}
i
P
i
+
1
：坐标系{i+1}的原点相对于坐标系{i}的表示
单独分析连杆i，其处于平衡状态有：
力平衡
i
f
i
−
i
f
i
+
1
+
i
m
i
g
=
0
^if_i – ^if_{i+1} + ^im_i g = 0
i
f
i
−
i
f
i
+
1
+
i
m
i
g
=
0
各力均在坐标系{i}下表示，相互作用力之间使用负号标注。
力矩平衡
i
M
i
−
i
M
i
+
1
−
i
P
i
+
1
×
i
f
i
+
1
+
i
r
c
i
i
m
i
g
=
0
^iM_i – ^iM_{i+1} – ^iP_{i+1} \times ^if_{i+1} + ^ir_{ci} {^im_i g} = 0
i
M
i
−
i
M
i
+
1
−
i
P
i
+
1
×
i
f
i
+
1
+
i
r
c
i
i
m
i
g
=
0
如果忽略掉连杆本身重量，上两式可以写成反向迭代的形式
i
f
i
=
i
f
i
+
1
^if_i = ^if_{i+1}
i
f
i
=
i
f
i
+
1
i
M
i
=
i
M
i
+
1
+
i
P
i
+
1
×
i
f
i
+
1
^iM_i = ^iM_{i+1} + ^iP_{i+1} \times ^if_{i+1}
i
M
i
=
i
M
i
+
1
+
i
P
i
+
1
×
i
f
i
+
1
建立虚功方程：
δ
w
=
(
i
F
i
+
1
)
′
i
D
i
+
1
=
(
i
+
1
F
i
+
1
)
′
i
+
1
D
i
+
1
\delta w = (^iF_{i+1})^{\prime} {^iD_{i+1}} = (^{i+1}F_{i+1})^{\prime} {^{i+1}D_{i+1}}
δ
w
=
(
i
F
i
+
1
)
′
i
D
i
+
1
=
(
i
+
1
F
i
+
1
)
′
i
+
1
D
i
+
1
D表示杆件微分运动(虚位移)
F表示杆件力(六维向量)
微分运动之间存在这样的关系：
i
D
i
+
1
=
[
i
R
i
+
1
0
0
i
R
i
+
1
]
^iD_{i+1} = \begin{bmatrix} ^iR_{i+1} & 0\\ 0 & ^iR_{i+1} \end{bmatrix}
i
D
i
+
1
=
[
i
R
i
+
1
0
0
i
R
i
+
1
]
记：
J
D
F
=
[
i
R
i
+
1
0
0
i
R
i
+
1
]
J_{DF} = \begin{bmatrix} ^iR_{i+1} & 0\\ 0 & ^iR_{i+1} \end{bmatrix}
J
D
F
=
[
i
R
i
+
1
0
0
i
R
i
+
1
]
有：
i
D
i
+
1
=
J
D
F
i
+
1
D
i
+
1
^iD_{i+1} = J_{DF} {^{i+1}D_{i+1}}
i
D
i
+
1
=
J
D
F
i
+
1
D
i
+
1
整理可得：
i
F
i
+
1
=
J
D
F
i
+
1
F
i
+
1
^iF_{i+1} = J_{DF} {^{i+1}F_{i+1}}
i
F
i
+
1
=
J
D
F
i
+
1
F
i
+
1
把该式带入上述的反向迭代公式的矩阵形式中：
[
i
f
i
i
M
i
]
=
[
E
0
i
P
i
+
1
×
E
]
[
i
f
i
+
1
i
M
i
+
1
]
\begin{bmatrix} ^if_i \\ ^iM_i \end{bmatrix} = \begin{bmatrix} E & 0\\ ^iP_{i+1} \times & E \end{bmatrix} \begin{bmatrix} ^if_{i+1}\\ ^iM_{i+1} \end{bmatrix}
[
i
f
i
i
M
i
]
=
[
E
i
P
i
+
1
×
0
E
]
[
i
f
i
+
1
i
M
i
+
1
]
得到：
[
i
f
i
i
M
i
]
=
[
i
+
1
i
R
0
i
P
i
+
1
×
i
+
1
i
R
i
+
1
i
R
]
[
i
+
1
f
i
+
1
i
+
1
M
i
+
1
]
\begin{bmatrix} ^if_i \\ ^iM_i \end{bmatrix} = \begin{bmatrix} ^i_{i+1}R & 0\\ ^iP_{i+1} \times ^i_{i+1}R & ^i_{i+1}R \end{bmatrix} \begin{bmatrix} ^{i+1}f_{i+1}\\ ^{i+1}M_{i+1} \end{bmatrix}
[
i
f
i
i
M
i
]
=
[
i
+
1
i
R
i
P
i
+
1
×
i
+
1
i
R
0
i
+
1
i
R
]
[
i
+
1
f
i
+
1
i
+
1
M
i
+
1
]
进一步可以得到力和力矩的传递公式：
i
f
i
=
i
+
1
i
R
i
+
1
f
i
+
1
^if_i = {^i_{i+1}R} \, {^{i+1}f_{i+1}}
i
f
i
=
i
+
1
i
R
i
+
1
f
i
+
1
i
M
i
=
i
+
1
i
R
i
+
1
M
i
+
1
+
i
P
i
+
1
×
i
f
i
^iM_i = {^i_{i+1}R} \, {^{i+1}M_{i+1}} + ^iP_{i+1} \times ^if_i
i
M
i
=
i
+
1
i
R
i
+
1
M
i
+
1
+
i
P
i
+
1
×
i
f
i
如果不考虑关节之间的摩擦，旋转关节除了绕转轴的扭矩外，其余各方向的力和力矩都由机械构件承受。故关节驱动力矩为：
τ
i
=
i
M
i
T
i
Z
i
\tau_i = ^iM_i^T \,{^iZ_i}
τ
i
=
i
M
i
T
i
Z
i
同理，关节的驱动力为
τ
i
=
i
f
i
T
i
Z
i
\tau_i = ^if_i^T \,{^iZ_i}
τ
i
=
i
f
i
T
i
Z
i
我们可以以此为根据，在已知负载的情况下，从末端执行元件开始，递推得到各个关节所需的驱动力矩。从而进一步建立力雅可比矩阵关系。
雅可比矩阵
雅可比矩阵
机器人速度雅可比矩阵(也称为运动雅可比矩阵)可定义为由机器人关节空间速度到操作空间速度的线形变换
在向量微积分中，雅可比矩阵是一阶导数以一定方式排列成的矩阵，其行列式成为雅可比行列式。
解析雅可比矩阵
微分运动学方程：
P
˙
=
d
f
d
q
q
˙
\dot{P} = \frac{df}{dq} \dot{q}
P
˙
=
d
q
d
f
q
˙
P可以理解为广义的机器人的运动输出量，q为广义的机器人的输入量。
P
=
f
(
q
)
→
{
p
1
(
t
)
=
f
1
(
q
1
,
q
2
,
…
,
q
n
)
p
2
(
t
)
=
f
2
(
q
1
,
q
2
,
…
,
q
n
)
…
p
m
(
t
)
=
f
m
(
q
1
,
q
2
,
…
,
q
n
)
P=f(q) \to \left\{ \begin{aligned} p_1(t) & = f_1(q_1,q_2,\dots,q_n) \\ p_2(t) & = f_2(q_1,q_2,\dots,q_n) \\ \dots \\ p_m(t) & = f_m(q_1,q_2,\dots,q_n) \end{aligned} \right.
P
=
f
(
q
)
→
⎩
⎪
⎪
⎪
⎪
⎨
⎪
⎪
⎪
⎪
⎧
p
1
(
t
)
p
2
(
t
)
…
p
m
(
t
)
=
f
1
(
q
1
,
q
2
,
…
,
q
n
)
=
f
2
(
q
1
,
q
2
,
…
,
q
n
)
=
f
m
(
q
1
,
q
2
,
…
,
q
n
)
d
d
t
f
i
(
q
(
t
)
)
=
d
d
t
f
i
(
q
i
(
t
)
,
…
,
q
j
(
t
)
,
…
,
q
n
(
t
)
)
=
∂
f
i
∂
q
1
q
1
˙
(
t
)
+
⋯
+
∂
f
i
∂
q
j
q
j
˙
(
t
)
+
⋯
+
∂
f
i
∂
q
n
q
n
˙
(
t
)
\frac{d}{dt}f_i(q(t)) = \frac{d}{dt}f_i(q_i(t), \dots, q_j(t), \dots,q_n(t)) = \frac{\partial f_i}{\partial q_1} \dot{q_1}(t) + \dots + \frac{\partial f_i}{\partial q_j} \dot{q_j}(t) + \dots + \frac{\partial f_i}{\partial q_n} \dot{q_n}(t)
d
t
d
f
i
(
q
(
t
)
)
=
d
t
d
f
i
(
q
i
(
t
)
,
…
,
q
j
(
t
)
,
…
,
q
n
(
t
)
)
=
∂
q
1
∂
f
i
q
1
˙
(
t
)
+
⋯
+
∂
q
j
∂
f
i
q
j
˙
(
t
)
+
⋯
+
∂
q
n
∂
f
i
q
n
˙
(
t
)
d
d
t
f
i
(
q
(
t
)
)
=
(
∂
f
i
∂
q
1
…
∂
f
j
∂
q
j
…
∂
f
n
∂
q
n
)
(
q
1
˙
(
t
)
⋮
q
j
˙
(
t
)
⋮
q
n
˙
(
t
)
)
=
J
f
i
(
q
(
t
)
)
q
t
˙
\frac{d}{dt}f_i(q(t)) = \begin{pmatrix} \frac{\partial f_i}{\partial q_1} & \dots & \frac{\partial f_j}{\partial q_j} & \dots & \frac{\partial f_n}{\partial q_n} \end{pmatrix} \begin{pmatrix} \dot{q_1}(t) \\ \vdots \\ \dot{q_j}(t) \\ \vdots \\ \dot{q_n}(t) \end{pmatrix}= J_{f_i}(q(t)) \dot{q_t}
d
t
d
f
i
(
q
(
t
)
)
=
(
∂
q
1
∂
f
i
…
∂
q
j
∂
f
j
…
∂
q
n
∂
f
n
)
⎝
⎜
⎜
⎜
⎜
⎜
⎜
⎛
q
1
˙
(
t
)
⋮
q
j
˙
(
t
)
⋮
q
n
˙
(
t
)
⎠
⎟
⎟
⎟
⎟
⎟
⎟
⎞
=
J
f
i
(
q
(
t
)
)
q
t
˙
得到：
这种求解雅可比矩阵的方法称为求导法，所构建的雅可比矩阵称为解析雅可比矩阵。
几何雅可比矩阵
机器人关节速度向量定义为：
q
˙
=
[
q
1
˙
q
2
˙
…
q
n
˙
]
T
\dot{q}= \begin{bmatrix} \dot{q_1} & \dot{q_2} & \dots & \dot{q_n} \end{bmatrix}^{T}
q
˙
=
[
q
1
˙
q
2
˙
…
q
n
˙
]
T
执行器在基系中的广义速度向量：
V
=
[
v
ω
]
=
[
x
˙
y
˙
z
˙
ω
x
˙
ω
y
˙
ω
z
˙
]
T
V= \begin{bmatrix} v \\ \omega \end{bmatrix}= \begin{bmatrix} \dot{x} & \dot{y} & \dot{z} & \dot{\omega_x} & \dot{\omega_y} & \dot{\omega_z} \end{bmatrix}^{T}
V
=
[
v
ω
]
=
[
x
˙
y
˙
z
˙
ω
x
˙
ω
y
˙
ω
z
˙
]
T
V
=
J
(
q
)
q
˙
V = J(q) \dot{q}
V
=
J
(
q
)
q
˙
前三行称为位置雅可比矩阵，代表对末端执行器线速度v的传递比，后三行称为姿态雅可比矩阵，代表相应的关节速度对末端执行器角速度
ω
\omega
ω
的传递比
根据虚功原理，还可以推导出速度雅可比与力雅可比的关系，推导过程这里暂时省略，可能后续会添加：
矢量积法构建雅可比矩阵
对于移动关节：
[
v
ω
]
=
[
z
i
0
]
q
i
˙
J
i
=
[
z
i
0
]
\begin{bmatrix} v\\ \omega \end{bmatrix}= \begin{bmatrix} z_i\\ 0 \end{bmatrix} \dot{q_i} \quad J_i = \begin{bmatrix} z_i\\ 0 \end{bmatrix}
[
v
ω
]
=
[
z
i
0
]
q
i
˙
J
i
=
[
z
i
0
]
z
i
z_i
z
i
是i坐标系
z
z
z
轴单位矢量在基系中的表示。
对于转动关节：
[
v
ω
]
=
[
z
i
×
0
P
(
n
,
i
)
z
i
]
q
i
˙
\begin{bmatrix} v\\ \omega \end{bmatrix}= \begin{bmatrix} z_i \times ^0P_{(n,i)}\\ z_i \end{bmatrix} \dot{q_i}
[
v
ω
]
=
[
z
i
×
0
P
(
n
,
i
)
z
i
]
q
i
˙
0
P
(
n
,
i
)
^0P_{(n,i)}
0
P
(
n
,
i
)
为末端坐标原点相对于i系的位置矢量在基系的表示。
最终有：
