【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真 _螺旋

基于PoE公式的UR5机械臂正运动学建模求解与仿真
基于PoE公式的UR5机械臂正运动学分析
下一篇：【机器人2】基于POE公式的UR5机械臂逆运动学建模求解与仿真
1.1 运动旋量的螺旋释义（理论基础）
螺旋轴表示成：绕某个轴的转动与沿该轴的移动的复合。一种形式是 { q , s ^ , h } \{\{q}, \{\hat{s}}, h\} {q,s^,h}：
其中，q ∈ R 3 \{q} \in \{R}^{3} q∈R3为轴上任一点； s ^ \{\hat{s}} s^为螺旋轴单位向量； h h h为螺旋的节距，大小为沿螺旋轴方向线速度与绕该轴角度上比值。
运动旋量 V \{V} V与 { q , s ^ , h } \{\{q}, \{\hat{s}}, h\} {q,s^,h}及速度 θ ˙ \dot{\theta} θ˙关系： V = [ ω v ] = [ s ^ θ ˙ ? s ^ θ ˙ × q + h s ^ θ ˙ ] \{V}=\left[\begin{array}{l} \{\omega} \\ \{v} \end{array}\right]=\left[\begin{array}{c} \{\hat{s}} \dot{\theta} \\ -\{\hat{s}} \dot{\theta} \times \{q} +h \{\hat{s}}\dot{\theta} \end{array}\right] V=[ωv?]=[s^θ˙?s^θ˙×q+hs^θ˙?] ω \{\omega} ω是坐标系的角速度，由轴 s ^ \{\hat{s}} s^和绕轴角速度 θ ˙ \dot{\theta} θ˙产生； v \{v} v是坐标系的线速度，由沿轴移动和转动产生。
然而，一般不采用 { q , s ^ , h } \{\{q}, \{\hat{s}}, h\} {q,s^,h}描述螺旋运动，而通常采用运动旋量正交化的形式来描述：
① 若 ω ≠ 0 \{\omega}\neq 0 ω=0，有 s ^ = ω ∥ ω ∥ \{\hat{s}}=\frac{\{\omega}}{\{\|\omega\|}} s^=∥ω∥ω?，θ ˙ = ∥ ω ∥ \dot{\theta}=\{\|\omega\|} θ˙=∥ω∥，h = ω ^ T v θ ˙ h=\frac{\{\hat{\omega}}^{\{T}} \{v}} { \dot{\theta}} h=θ˙ω^Tv?；则有 S = V ∥ ω ∥ = ( ω ∥ ω ∥ , v ∥ ω ∥ ) \{S}=\frac{\{V}} {\{\|\omega\|}}=(\frac{\{\omega}}{\{\|\omega\|}}, \frac{\{v}}{\{\|\omega\|}}) S=∥ω∥V?=(∥ω∥ω?,∥ω∥v?)，S θ ˙ = V \{S} \dot{\theta}=\{V} Sθ˙=V
② 若 ω = 0 \{\omega}= 0 ω=0，有 s ^ = v ∥ v ∥ \{\hat{s}}=\frac{\{v}}{\{\|v\|}} s^=∥v∥v?，θ ˙ = ∥ v ∥ \dot{\theta}=\{\|v\|} θ˙=∥v∥，h h h无限大；则有 S = V ∥ v ∥ = ( 0 , v ∥ v ∥ ) \{S}=\frac{\{V}} {\{\|v\|}}=(0, \frac{\{v}}{\{\|v\|}}) S=∥v∥V?=(0,∥v∥v?)，S θ ˙ = V \{S} \dot{\theta}=\{V} Sθ˙=V
因此，螺旋轴的定义如下： S = [ ω v ] ∈ R 6 \{S}=\left[\begin{array}{l} \{\omega} \\ \{v} \end{array}\right] \in \{R}^{6} S=[ωv?]∈R6 ① 若 ∥ ω ∥ = 1 \{\|\omega\|}=1 ∥ω∥=1；则 v = ? ω × q + h ω \{v}=-\{\omega }\times \{q}+h \{\omega} v=?ω×q+hω，q \{q} q为轴上任一点，h h h是螺旋的节距（线速度与角速度比值，纯转动是为0）。
② 若 ∥ ω ∥ = 0 , ∥ v ∥ = 1 \{\|\omega\|}=0,\{\|v\|}=1 ∥ω∥=0,∥v∥=1；则节距无穷大，因此为纯移动，参数中只有 v \{v} v不是零向量。
值得注意的是，虽然可以同时用 ( ω , v ) (\{\omega},\{v}) (ω,v)描述正则化的螺旋轴 S \{S} S（ ∥ ω ∥ = 1 \{\|\omega\|}=1 ∥ω∥=1或 ∥ v ∥ = 1 \{\|v\|}=1 ∥v∥=1）和运动旋量 V \{V} V（对 ω \{\omega} ω和 v \{v} v没有限制），但它们的物理意义不同，必须区分开来。
1.2 基于PoE公式的UR5机械臂正运动学模型
本文研究对象为丹麦优傲机器人的UR5六自由度协作机械臂，由UR5机械臂尺寸规格图得到各关节及连杆的参数。由于在实际使用过程中，绝大多数的在售机器人还是用D-H参数法进行建模，因此为了验证PoE公式的正确性，螺旋轴的选取和D-H模型方向对正。
UR5机械臂尺寸数据及螺旋轴的建立如下图左、右所示。
图中给出了UR5机械臂在初始位置时的螺旋轴 S 1 , … , S 6 \bm{\{S}_{1}, \ldots, \{S}_{6}} S1?,…,S6? 。处于初始位置时的末端坐标系对应的齐次变换矩阵 M \{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 ] \{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=?1000?0010?0?100??(L1?+L2?)?(W1?+W2?)H1??H2?1??因此空间坐标系下的螺旋轴 S i = ( ω i , v i ) , i = 1 , … , 6 \{\ { S }}_{i}=\left(\{\omega}_{i}, \{v}_{i}\right), i=1, \ldots, 6 Si?=(ωi?,vi?),i=1,…,6，坐标如下表所示：