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

i i i ω i \{\omega}_{i} ωi? v i \{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 \{T}(\{\theta})=e^{\left[\{S}_{1}\right] \{1}} e^{\left[\{S}_{2}\right] \{2}} e^{\left[\{S}_{3}\right] \{3}} e^{\left[\{S}_{4}\right] \{4}} e^{\left[\{S}_{5}\right] \{5}} e^{\left[\{S}_{6}\right] \{6}} \{M} T(θ)=e[S1?]θ1?e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?e[S5?]θ5?e[S6?]θ6?M其中，指数坐标e [ S ] θ = [ e [ ω ] θ ? 0 1 ] e^{[\{S}] \theta}=\left[\begin{array}{cc} e^{[\{\omega}] \theta} & * \\ 0 & 1 \end{array}\right] e[S]θ=[e[ω]θ0??1?]，e [ ω ] θ = I + sin ? θ [ ω ] + ( 1 ? cos ? θ ) [ ω ] 2 e^{[\{\omega}] \theta}=I+\sin \theta[\{\omega}]+(1-\cos \theta)[\{\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 仿真验证
在准备相关数据，如UR5机械臂各个关节的长度、初始位形 M \{M} M、各关节螺旋轴 S 1 , … , S 6 \{S}_{1}, \ldots, \{S}_{6} S1?,…,S6?，并设定一组关节角 θ 0 = ( 0 , ? π 2 , 0 , ? π 2 , 0 , 0 ) \{\{0}}=\left(0,-\frac{\pi}{2}, 0,-\frac{\pi}{2}, 0,0\right) θ0?=(0,?2π?,0,?2π?,0,0)
计算空间坐标系下的末端位姿 T ( θ 0 ) \{T}(\{\{0}}) T(θ0?)

文章插图
：空间坐标系下正运动学计算方法：
：将运动旋量转化为se3矩阵：
：将旋量角速度部分的向量转化为so3矩阵：
：SE3矩阵的指数坐标计算方法
：将[ωθ]这个so3反对称矩阵转化为向量ωθ
：将ωθ拆开成ω和θ（因为||ω||=1，就可以用向量二范数拆分）
：SO3矩阵的指数坐标计算方法
因此，空间坐标系下的末端位姿 T ( θ 0 ) \{T}(\{\{0}}) T(θ0?)计算结果为：
用工具箱观察关节角 θ 0 = ( 0 , ? π 2 , 0 , ? π 2 , 0 , 0 ) \{\{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 ) \{\{1}}=\left(0,0, 0,0, 0,0\right) θ1?=(0,0,0,0,0,0)时的位姿：
可以看出，末端位姿计算结果正确。
【【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真】下一篇：【机器人2】基于POE公式的UR5机械臂逆运动学建模求解与仿真