0 引言
基于指数积的正运动学
现代机器人学名词概念
有了以上的基础,我们现在利用指数积来对机器人的逆运动学进行求解,有一点需要注意,需要先对机器人进行指数积的正运动学建模,然后才能利用指数积来对逆运动学求解,求解逆运动学的方法我们这里采用数值求解的方式来求解关节角。
众所周知,对于通用的n自由度开链机器人而言,若其正向运动学写成
T
(
θ
)
,
θ
∈
R
n
T\left( \theta \right) ,\theta \in R^n
T(θ),θ∈Rn的形式,而其逆运动学问题可以描述成:当给定齐次变换矩阵
X
∈
S
E
(
3
)
X\in SE\left( 3 \right)
X∈SE(3),找出满足
T
(
θ
)
=
X
T\left( \theta \right) =X
T(θ)=X 的关节角
θ
\theta
θ 。
1 逆运动学的解析求解
对于求解析解,即找出机器人的连杆运动关系,通过几何的方法来求得具体的解析解,这种方法有很大的局限性,它只能适用于一些特定的机械臂,如要求关节4,5,6相互正交且共点。
2 逆运动学的数值求解
如果逆运动学方程无解析解时,可采用迭代数值方法求解。即使存在解析解,数值算法也经常用于改善求解精度。这里我们介绍逆运动学的数值求解,它采用的方法是牛顿-拉夫森法,用来求解非线性方程。
2.1 牛顿-拉夫森法
对于给定的微分方程 g ( θ ) g\left( \theta \right) g(θ),数值求解方程 g ( θ ) = 0 g\left( \theta \right)=0 g(θ)=0。假定 θ 0 \theta ^0 θ0 为初值,利用泰勒级数在 θ 0 \theta ^0 θ0 出展开,并截取到第一项,得 g ( θ ) = g ( θ 0 ) + ∂ g ∂ θ ( θ 0 ) ( θ − θ 0 ) + h . o . t . g\left( \theta \right) =g\left( \theta ^0 \right) +\frac{\partial g}{\partial \theta}\left( \theta ^0 \right) \left( \theta -\theta ^0 \right) +h.o.t. g(θ)=g(θ0)+∂θ∂g(θ0)(θ−θ0)+h.o.t.若只保留到第一阶,令 g ( θ ) = 0 g\left( \theta \right)=0 g(θ)=0 ,求解 θ \theta θ 。得到 θ = θ 0 − ( ∂ g ∂ θ ( θ 0 ) ) − 1 g ( θ 0 ) \theta =\theta ^0-\left( \frac{\partial g}{\partial \theta}\left( \theta ^0 \right) \right) ^{-1}g\left( \theta ^0 \right) θ=θ0−(∂θ∂g(θ0))−1g(θ0)再将上式求得值作为初值,代入上述方程中,重复求解,得到下面方程 θ k + 1 = θ k − ( ∂ g ∂ θ ( θ k ) ) − 1 g ( θ k ) \theta ^{k+1}=\theta ^k-\left( \frac{\partial g}{\partial \theta}\left( \theta ^k \right) \right) ^{-1}g\left( \theta ^k \right) θk+1=θk−(∂θ∂g(θk))−1g(θk)上述过程不断迭代,直至满足某个指标值。
2.2 逆动力学的数值算法
我们用坐标向量
x
x
x 及其正运动学方程
x
=
f
(
θ
)
x=f\left( \theta \right)
x=f(θ) 表示末端坐标,
θ
\theta
θ 为关节角,令
x
d
x_d
xd 为目标末端坐标,牛顿-拉夫森法中的
g
(
θ
)
g\left( \theta \right)
g(θ) 可以定义成
g
(
θ
)
=
x
d
−
f
(
θ
)
g\left( \theta \right) =x_d-f\left( \theta \right)
g(θ)=xd−f(θ) ,目标是找到关节坐标
θ
d
\theta _d
θd ,满足
g
(
θ
d
)
=
x
d
−
f
(
θ
d
)
=
0
g\left( \theta _d \right) =x_d-f\left( \theta _d \right) =0
g(θd)=xd−f(θd)=0由前可得
x
d
=
f
(
θ
d
)
=
f
(
θ
0
)
+
∂
f
∂
θ
∣
θ
0
(
θ
d
−
θ
0
)
+
h
.
o
.
t
.
x_d=f\left( \theta _d \right) =f\left( \theta ^0 \right) +\frac{\partial f}{\partial \theta}|_{\theta ^0}\left( \theta _d-\theta ^0 \right) +h.o.t.
xd=f(θd)=f(θ0)+∂θ∂f∣θ0(θd−θ0)+h.o.t.式中
∂
f
∂
θ
∣
θ
0
\frac{\partial f}{\partial \theta}|_{\theta ^0}
∂θ∂f∣θ0 为
J
(
θ
0
)
∈
R
m
×
n
J\left( \theta ^0 \right) \in R^{m\times n}
J(θ0)∈Rm×n 为
θ
0
\theta ^0
θ0 处的坐标雅可比。进一步简化得
J
(
θ
0
)
Δ
θ
=
x
d
−
f
(
θ
0
)
J\left( \theta ^0 \right) \varDelta \theta =x_d-f\left( \theta ^0 \right)
J(θ0)Δθ=xd−f(θ0)假设
J
(
θ
0
)
J\left( \theta ^0 \right)
J(θ0) 为方阵且可逆,便继续可得
Δ
θ
=
J
−
1
(
θ
0
)
(
x
d
−
f
(
θ
0
)
)
\varDelta \theta =J^{-1}\left( \theta ^0 \right) \left( x_d-f\left( \theta ^0 \right) \right)
Δθ=J−1(θ0)(xd−f(θ0)) 由于正运动学是
θ
\theta
θ 的非线性函数,这时新的估计值
θ
1
\theta^1
θ1 比
θ
0
\theta^0
θ0 更接近真实值,迭代过程不断重复,产生一系列的
θ
\theta
θ 值,并最终在
θ
d
\theta_d
θd 处收敛。
注:如果逆运动学存在多组解,迭代过程趋向于收敛到与初始值
θ
0
\theta^0
θ0 最接近的解,如果初始值与真实值没有足够接近,则迭代过程可能不收敛。另外雅可比矩阵有时会成为奇异,故我们采用求广义逆的方法来求雅可比矩阵的逆矩阵。
为了能应用在预期末端位形为
T
s
d
∈
S
E
(
3
)
T_{sd}\in SE\left( 3 \right)
Tsd∈SE(3) 而不是坐标向量
x
d
x_d
xd 的情况,将关节坐标雅可比矩阵J替换为末端物体雅可比矩阵
J
b
∈
R
6
×
n
J_b\in R^{6\times n}
Jb∈R6×n 。
J
b
J_b
Jb 的伪逆应该作用于物体速度旋量
V
b
∈
R
6
V_b\in R^6
Vb∈R6 。为找到这个
V
b
V_b
Vb ,首先需计算相对物体坐标系的预期位形,即
T
b
d
(
θ
i
)
=
T
s
b
−
1
(
θ
i
)
T
s
d
T_{bd}\left( \theta ^i \right) =T_{sb}^{-1}\left( \theta ^i \right) T_{sd}
Tbd(θi)=Tsb−1(θi)Tsd 然后利用矩阵对数确定
V
b
V_b
Vb ,即
[
V
b
]
=
log
T
b
d
(
θ
i
)
\left[ V_b \right] =\log T_{bd}\left( \theta ^i \right)
[Vb]=logTbd(θi)
至此,可得逆运动学的步骤:
- 初始化:已知 T s d T_{sd} Tsd ,初始估计值 θ 0 ∈ R n \theta ^0\in R^n θ0∈Rn ,设定i=0.
- 设定 [ V b ] = log ( T s b − 1 ( θ i ) T s d ) \left[ V_b \right] =\log \left( T_{sb}^{-1}\left( \theta ^i \right) T_{sd} \right) [Vb]=log(Tsb−1(θi)Tsd) ,当 ∥ w b ∥ > ε w \lVert w_b \rVert >\,\,\varepsilon _w ∥wb∥>εw 或者 ∥ v b ∥ > ε v \lVert v_b \rVert >\,\,\varepsilon _v ∥vb∥>εv ( ε w \varepsilon _w εw 和 ε v \varepsilon _v εv 为很小值)。
- 设定 θ i + 1 = θ i + J † ( θ i ) V b \theta ^{i+1}=\theta ^i+J^{\dag}\left( \theta ^i \right) V_b θi+1=θi+J†(θi)Vb。
- 增加i
注: J † ( θ i ) J^{\dag}\left( \theta ^i \right) J†(θi) 代表物体雅可比矩阵的广义逆。以上方法是物体雅可比矩阵 J b ( θ ) J_b\left( \theta \right) Jb(θ)和物体空间速度旋量 V b V_b Vb 来求解的过程,另一种等效的方式是利用空间雅可比矩阵 J s ( θ ) J_s\left( \theta \right) Js(θ) 和空间速度旋量 V s V_s Vs 来求解,只需对 J b J_b Jb 和 V b V_b Vb 作一下矩阵变换即可。
2.3 雅可比矩阵的求解
根据以上的步骤,除了物体雅可比矩阵的求解,其余量我们均已经得到,故下面开始求解物体雅可比矩阵。回顾之前的知识,对于n杆串联机器人,其正向运动学的指数积公式为
T
(
θ
)
=
e
[
S
1
]
θ
1
⋯
e
[
S
n
−
1
]
θ
n
−
1
e
[
S
n
]
θ
n
M
T\left( \theta \right) =e^{\left[ S_1 \right] \theta _1}\cdots e^{\left[ S_{n-1} \right] \theta _{n-1}}e^{\left[ S_n \right] \theta _n}M
T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnM空间速度
V
s
V_s
Vs 可以写成
[
V
s
]
=
T
˙
T
−
1
\left[ V_s \right] =\dot{T}T^{-1}
[Vs]=T˙T−1 ,计算可得
[
V
s
]
=
[
S
1
]
θ
˙
1
+
e
[
S
1
]
θ
1
[
S
2
]
e
−
[
S
1
]
θ
1
θ
˙
2
+
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
[
S
3
]
e
−
[
S
2
]
θ
2
e
−
[
S
1
]
θ
1
θ
˙
3
+
⋯
\left[ V_s \right] =\left[ S_1 \right] \dot{\theta}_1+e^{\left[ S_1 \right] \theta _1}\left[ S_2 \right] e^{-\left[ S_1 \right] \theta _1}\dot{\theta}_2+e^{\left[ S_1 \right] \theta _1}e^{\left[ S_2 \right] \theta _2}\left[ S_3 \right] e^{-\left[ S_2 \right] \theta _2}e^{-\left[ S_1 \right] \theta _1}\dot{\theta}_3+\cdots
[Vs]=[S1]θ˙1+e[S1]θ1[S2]e−[S1]θ1θ˙2+e[S1]θ1e[S2]θ2[S3]e−[S2]θ2e−[S1]θ1θ˙3+⋯通过伴随映射写成向量的形式,即
V
s
=
S
1
θ
˙
1
+
A
d
e
[
S
1
]
θ
1
(
S
2
)
θ
˙
2
+
A
d
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
(
S
3
)
θ
˙
3
+
⋯
V_s=S_1\dot{\theta}_1+Ad_{e^{\left[ S_1 \right] \theta _1}}\left( S_2 \right) \dot{\theta}_2+Ad_{e^{\left[ S_1 \right] \theta _1}e^{\left[ S_2 \right] \theta _2}}\left( S_3 \right) \dot{\theta}_3+\cdots
Vs=S1θ˙1+Ade[S1]θ1(S2)θ˙2+Ade[S1]θ1e[S2]θ2(S3)θ˙3+⋯ 观察上式可写成n个空间速度的向量和形式
V
s
=
J
s
1
θ
˙
1
+
J
s
2
(
θ
)
θ
˙
2
+
⋯
J
s
n
(
θ
)
θ
˙
n
V_s=J_{s1}\dot{\theta}_1+J_{s2}\left( \theta \right) \dot{\theta}_2+\cdots J_{sn}\left( \theta \right) \dot{\theta}_n
Vs=Js1θ˙1+Js2(θ)θ˙2+⋯Jsn(θ)θ˙n写成矩阵的形式,即
V
s
=
[
J
s
1
J
s
2
(
θ
)
⋯
J
s
n
(
θ
)
]
[
θ
˙
1
θ
˙
2
⋮
θ
˙
n
]
=
J
s
(
θ
)
θ
˙
V_s=\left[ J_{s1}\,\,J_{s2}\left( \theta \right) \,\,\cdots \,\,J_{sn}\left( \theta \right) \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\ \vdots\\ \dot{\theta}_n\\ \end{array} \right] =J_s\left( \theta \right) \dot{\theta}
Vs=[Js1Js2(θ)⋯Jsn(θ)]
θ˙1θ˙2⋮θ˙n
=Js(θ)θ˙ 矩阵
J
s
(
θ
)
J_s\left( \theta \right)
Js(θ) 即为空间固定坐标形式下的雅可比矩阵,简称空间雅可比。
再将空间雅可比
J
s
J_s
Js 转化为物体雅可比矩阵
J
b
J_b
Jb 。
J
b
(
θ
)
=
A
d
T
b
s
(
J
s
(
θ
)
)
J_b\left( \theta \right) =Ad_{T_{bs}}\left( J_s\left( \theta \right) \right)
Jb(θ)=AdTbs(Js(θ))
至此,全部未知量我们均已得到,可以应用算法进行建模了。
3 仿真分析
这里我设定初始关节角为零位角,即 q0=[0;0;0;0;0;0] 。设定Td为关节角为[0.1;0.2;0.3;0.4;0.5;0.6]时的末端位姿,即
开始求逆解,设定迭代1000次,如果1000次以内没有迭代出来,则认为求不出逆解,同时设定误差 ε w \varepsilon _w εw 和 ε v \varepsilon _v εv 小于1e-8时停止迭代,迭代结果如下:
结果分析,在迭代69次后,停止了迭代,此时 ε w \varepsilon _w εw 和 ε v \varepsilon _v εv 的误差分别为1e-14方和1e-12的数量级,迭代效果完美,与所求的角度完全吻合,且因为雅可比不是通过微分求得,是直接建立得到,所以运算速度非常快,秒算出结果。
结论
对于这种需要收敛的逆运动学数值算法,初始估计值 θ 0 \theta _0 θ0 应尽量与真实值 θ d \theta _d θd 接近,只要从初始零位开始操作机器人,便可以满足这个条件。这时实际的末端位形与关节角已知,并确保所需要的的末端位置 T s d T_{sd} Tsd 相对逆运动学的计算频次缓慢发生变化。不断迭代更新 θ d \theta _d θd 直至满足要求。