基于PoE公式的UR5机械臂正运动学分析
下一篇：【机器人2】基于POE公式的UR5机械臂逆运动学建模求解与matlab仿真文章来源地址https://www.toymoban.com/news/detail-455661.html
1.1 运动旋量的螺旋释义（理论基础）
螺旋轴表示成：绕某个轴的转动与沿该轴的移动的复合。一种形式是
{
q
,
s
^
,
h
}
\{\boldsymbol{q}, \boldsymbol{\hat{s}}, h\}
{q,s^,h}：
其中，
q
∈
R
3
\boldsymbol{q} \in \mathbb{R}^{3}
q∈R3为轴上任一点；
s
^
\boldsymbol{\hat{s}}
s^为螺旋轴单位向量；
h
h
h为螺旋的节距，大小为沿螺旋轴方向线速度与绕该轴角度上比值。
运动旋量 V \mathcal{V} V与 { q , s ^ , h } \{\boldsymbol{q}, \boldsymbol{\hat{s}}, h\} {q,s^,h}及速度 θ ˙ \dot{\theta} θ˙关系： V = [ ω v ] = [ s ^ θ ˙ − s ^ θ ˙ × q + h s ^ θ ˙ ] \mathcal{V}=\left[\begin{array}{l} \boldsymbol{\omega} \\ \boldsymbol{v} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{\hat{s}} \dot{\theta} \\ -\boldsymbol{\hat{s}} \dot{\theta} \times \boldsymbol{q} +h \boldsymbol{\hat{s}}\dot{\theta} \end{array}\right] V=[ωv]=[s^θ˙−s^θ˙×q+hs^θ˙] ω \boldsymbol{\omega} ω是坐标系的角速度，由轴 s ^ \boldsymbol{\hat{s}} s^和绕轴角速度 θ ˙ \dot{\theta} θ˙产生； v \boldsymbol{v} v是坐标系的线速度，由沿轴移动和转动产生。
然而，一般不采用
{
q
,
s
^
,
h
}
\{\boldsymbol{q}, \boldsymbol{\hat{s}}, h\}
{q,s^,h}描述螺旋运动，而通常采用运动旋量正交化的形式来描述：
① 若
ω
≠
0
\boldsymbol{\omega}\neq 0
ω=0，有
s
^
=
ω
∥
ω
∥
\boldsymbol{\hat{s}}=\frac{\boldsymbol{\omega}}{\boldsymbol{\|\omega\|}}
s^=∥ω∥ω，
θ
˙
=
∥
ω
∥
\dot{\theta}=\boldsymbol{\|\omega\|}
θ˙=∥ω∥，
h
=
ω
^
T
v
θ
˙
h=\frac{\boldsymbol{\hat{\omega}}^{\mathrm{T}} \boldsymbol{v}} { \dot{\theta}}
h=θ˙ω^Tv；则有
S
=
V
∥
ω
∥
=
(
ω
∥
ω
∥
,
v
∥
ω
∥
)
\mathcal{S}=\frac{\mathcal{V}} {\boldsymbol{\|\omega\|}}=(\frac{\boldsymbol{\omega}}{\boldsymbol{\|\omega\|}}, \frac{\boldsymbol{v}}{\boldsymbol{\|\omega\|}})
S=∥ω∥V=(∥ω∥ω,∥ω∥v)，
S
θ
˙
=
V
\mathcal{S} \dot{\theta}=\mathcal{V}
Sθ˙=V
② 若
ω
=
0
\boldsymbol{\omega}= 0
ω=0，有
s
^
=
v
∥
v
∥
\boldsymbol{\hat{s}}=\frac{\boldsymbol{v}}{\boldsymbol{\|v\|}}
s^=∥v∥v，
θ
˙
=
∥
v
∥
\dot{\theta}=\boldsymbol{\|v\|}
θ˙=∥v∥，
h
h
h无限大；则有
S
=
V
∥
v
∥
=
(
0
,
v
∥
v
∥
)
\mathcal{S}=\frac{\mathcal{V}} {\boldsymbol{\|v\|}}=(0, \frac{\boldsymbol{v}}{\boldsymbol{\|v\|}})
S=∥v∥V=(0,∥v∥v)，
S
θ
˙
=
V
\mathcal{S} \dot{\theta}=\mathcal{V}
Sθ˙=V
因此，螺旋轴的定义如下：
S
=
[
ω
v
]
∈
R
6
\mathcal{S}=\left[\begin{array}{l} \boldsymbol{\omega} \\ \boldsymbol{v} \end{array}\right] \in \mathbb{R}^{6}
S=[ωv]∈R6 ① 若
∥
ω
∥
=
1
\boldsymbol{\|\omega\|}=1
∥ω∥=1；则
v
=
−
ω
×
q
+
h
ω
\boldsymbol{v}=-\boldsymbol{\omega }\times \boldsymbol{q}+h \boldsymbol{\omega}
v=−ω×q+hω，
q
\boldsymbol{q}
q为轴上任一点，
h
h
h是螺旋的节距（线速度与角速度比值，纯转动是为0）。
② 若
∥
ω
∥
=
0
,
∥
v
∥
=
1
\boldsymbol{\|\omega\|}=0,\boldsymbol{\|v\|}=1
∥ω∥=0,∥v∥=1；则节距无穷大，因此为纯移动，参数中只有
v
\boldsymbol{v}
v不是零向量。
值得注意的是，虽然可以同时用 ( ω , v ) (\boldsymbol{\omega},\boldsymbol{v}) (ω,v)描述正则化的螺旋轴 S \mathcal{S} S（ ∥ ω ∥ = 1 \boldsymbol{\|\omega\|}=1 ∥ω∥=1或 ∥ v ∥ = 1 \boldsymbol{\|v\|}=1 ∥v∥=1）和运动旋量 V \mathcal{V} V（对 ω \boldsymbol{\omega} ω和 v \boldsymbol{v} v没有限制），但它们的物理意义不同，必须区分开来。
1.2 基于PoE公式的UR5机械臂正运动学模型
本文研究对象为丹麦优傲机器人Universal Robots的UR5六自由度协作机械臂，由UR5机械臂尺寸规格图得到各关节及连杆的参数。由于在实际使用过程中，绝大多数的在售机器人还是用D-H参数法进行建模，因此为了验证PoE公式的正确性，螺旋轴的选取和D-H模型方向对正。
UR5机械臂尺寸数据及螺旋轴的建立如下图左、右所示。
图中给出了UR5机械臂在初始位置时的螺旋轴
S
1
,
…
,
S
6
\bm{\mathcal{S}_{1}, \ldots, \mathcal{S}_{6}}
S1,…,S6。处于初始位置时的末端坐标系对应的齐次变换矩阵
M
\boldsymbol{M}
M：
M
=
[
1
0
0
−
(
L
1
+
L
2
)
0
0
−
1
−
(
W
1
+
W
2
)
0
1
0
H
1
−
H
2
0
0
0
1
]
\boldsymbol{M}=\left[\begin{array}{cccc} 1 & 0 & 0 & -\left(L_{1}+L_{2}\right) \\ 0 & 0 & -1 & -\left(W_{1}+W_{2}\right) \\ 0 & 1 & 0 & H_{1}-H_{2} \\ 0 & 0 & 0 & 1 \end{array}\right]
M=
100000100−100−(L1+L2)−(W1+W2)H1−H21
因此空间坐标系下的螺旋轴
S
i
=
(
ω
i
,
v
i
)
,
i
=
1
,
…
,
6
\boldsymbol{\mathcal { S }}_{i}=\left(\boldsymbol{\omega}_{i}, \boldsymbol{v}_{i}\right), i=1, \ldots, 6
Si=(ωi,vi),i=1,…,6，坐标如下表所示：
| i i i | ω i \boldsymbol{\omega}_{i} ωi | v i \boldsymbol{v}_{i} vi |
|---|---|---|
| 1 | ( 0 , 0 , 1 ) (0,0,1) (0,0,1) | ( 0 , 0 , 0 ) (0,0,0) (0,0,0) |
| 2 | ( 0 , − 1 , 0 ) (0,-1,0) (0,−1,0) | ( H 1 , 0 , 0 ) \left(H_{1}, 0,0\right) (H1,0,0) |
| 3 | ( 0 , − 1 , 0 ) (0,-1,0) (0,−1,0) | ( H 1 , 0 , L 1 ) \left(H_{1}, 0,L_{1}\right) (H1,0,L1) |
| 4 | ( 0 , − 1 , 0 ) (0,-1,0) (0,−1,0) | ( H 1 , 0 , L 1 + L 1 ) \left(H_{1}, 0, L_{1}+L_{1}\right) (H1,0,L1+L1) |
| 5 | ( 0 , 0 , − 1 ) (0,0,-1) (0,0,−1) | ( W 1 , − L 1 − L 1 , 0 ) \left(W_{1},-L_{1}-L_{1}, 0\right) (W1,−L1−L1,0) |
| 6 | ( 0 , − 1 , 0 ) (0,-1,0) (0,−1,0) | ( H 1 − H 1 , 0 , L 1 + L 1 ) \left(H_{1}-H_{1}, 0, L_{1}+L_{1}\right) (H1−H1,0,L1+L1) |
因此，由螺旋轴可以计算空间坐标系下的UR5机械臂末端位姿：
T
(
θ
)
=
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
e
[
S
5
]
θ
5
e
[
S
6
]
θ
6
M
\boldsymbol{T}(\boldsymbol{\theta})=e^{\left[\mathcal{S}_{1}\right] \theta_{1}} e^{\left[\mathcal{S}_{2}\right] \theta_{2}} e^{\left[\mathcal{S}_{3}\right] \theta_{3}} e^{\left[\mathcal{S}_{4}\right] \theta_{4}} e^{\left[\mathcal{S}_{5}\right] \theta_{5}} e^{\left[\mathcal{S}_{6}\right] \theta_{6}} \boldsymbol{M}
T(θ)=e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6M其中，指数坐标
e
[
S
]
θ
=
[
e
[
ω
]
θ
∗
0
1
]
e^{[\mathcal{S}] \theta}=\left[\begin{array}{cc} e^{[\boldsymbol{\omega}] \theta} & * \\ 0 & 1 \end{array}\right]
e[S]θ=[e[ω]θ0∗1]，
e
[
ω
]
θ
=
I
+
sin
θ
[
ω
]
+
(
1
−
cos
θ
)
[
ω
]
2
e^{[\boldsymbol{\omega}] \theta}=I+\sin \theta[\boldsymbol{\omega}]+(1-\cos \theta)[\boldsymbol{\omega}]^{2}
e[ω]θ=I+sinθ[ω]+(1−cosθ)[ω]2，
∗
=
(
I
θ
+
(
1
−
cos
θ
)
[
ω
]
+
(
θ
−
sin
θ
)
[
ω
]
2
)
v
*=\left(I \theta+(1-\cos \theta)[\omega]+(\theta-\sin \theta)[\omega]^{2}\right) v
∗=(Iθ+(1−cosθ)[ω]+(θ−sinθ)[ω]2)v
1.3 matlab仿真验证
在matlab准备相关数据，如UR5机械臂各个关节的长度、初始位形
M
\boldsymbol{M}
M、各关节螺旋轴
S
1
,
…
,
S
6
\mathcal{S}_{1}, \ldots, \mathcal{S}_{6}
S1,…,S6，并设定一组关节角
θ
0
=
(
0
,
−
π
2
,
0
,
−
π
2
,
0
,
0
)
\boldsymbol{\theta_{0}}=\left(0,-\frac{\pi}{2}, 0,-\frac{\pi}{2}, 0,0\right)
θ0=(0,−2π,0,−2π,0,0)
计算空间坐标系下的末端位姿
T
(
θ
0
)
\boldsymbol{T}(\boldsymbol{\theta_{0}})
T(θ0)
FKinSpace：空间坐标系下正运动学计算方法：
VecTose3：将运动旋量转化为se3矩阵：
VecToso3：将旋量角速度部分的向量转化为so3矩阵：
MatrixExp6：SE3矩阵的指数坐标计算方法
so3TcVec：将[ωθ]这个so3反对称矩阵转化为向量ωθ
AxisAng3：将ωθ拆开成ω和θ（因为||ω||=1，就可以用向量二范数拆分）
MatrixExp3：SO3矩阵的指数坐标计算方法
因此，空间坐标系下的末端位姿
T
(
θ
0
)
\boldsymbol{T}(\boldsymbol{\theta_{0}})
T(θ0)计算结果为：
用robotic toolbox工具箱观察关节角
θ
0
=
(
0
,
−
π
2
,
0
,
−
π
2
,
0
,
0
)
\boldsymbol{\theta_{0}}=\left(0,-\frac{\pi}{2}, 0,-\frac{\pi}{2}, 0,0\right)
θ0=(0,−2π,0,−2π,0,0)时的位姿：
再看看关节角
θ
1
=
(
0
,
0
,
0
,
0
,
0
,
0
)
\boldsymbol{\theta_{1}}=\left(0,0, 0,0, 0,0\right)
θ1=(0,0,0,0,0,0)时的位姿：
可以看出，末端位姿计算结果正确。文章来源：https://www.toymoban.com/news/detail-455661.html
下一篇：【机器人2】基于POE公式的UR5机械臂逆运动学建模求解与matlab仿真
到了这里，关于【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
