【机器人学】正运动学详解
【机器人学】正运动学详解
- 机器人学中的正运动学
- 一、位置(Position)(Position)(Position)表示
- 二、方向(Orientation)(Orientation)(Orientation)表示
- 2.1 基本公式
- 2.2 RPY角
- 2.3 欧拉角
- 三、平移运动(Displacement)(Displacement)(Displacement)
- 四、旋转运动(Rotation)(Rotation)(Rotation)
- 五、复合运动(Compound)(Compound)(Compound)
- 六、改进DH\bf DHDH法
- 6.1 DH\bf DHDH定义与建模
- 6.2 改进DH\bf DHDH法下的齐次变换矩阵
- 6.3 工具箱Toolbox\bf ToolboxToolbox的部分使用
- 6.4 一个简单例子
- 七、下载资源
- 参考目录
机器人学中的正运动学
本文对机器人学基本知识中的正运动学（Forward Kinematics）进行介绍，适合入门学习机器人学，如果你发现本文有疏漏、错误之处，欢迎读者批评指正。如果你喜欢本文，可以收藏本文，也可将本文分享给你的朋友、同学，但是转载不被允许
一、位置(Position)(Position)(Position)表示
笛卡尔坐标
P=(xyz)\it{P}=\left(\begin{matrix} x\\y\\z \end{matrix}\right) P=⎝⎛xyz⎠⎞柱坐标
变量：variables=(ρθz)variables=\left(\begin{matrix} \rho\\\theta\\z \end{matrix}\right)variables=⎝⎛ρθz⎠⎞
因此，位置 P\it PP 可以表示为：
P=(ρcosθρsinθz)\it{P}=\left(\begin{matrix} \rho\cos\theta\\\rho\sin\theta\\z \end{matrix}\right) P=⎝⎛ρcosθρsinθz⎠⎞
球坐标
变量：variables=(rθϕ)variables=\left(\begin{matrix} r\\\theta\\\phi \end{matrix}\right)variables=⎝⎛rθϕ⎠⎞
因此，位置 P\it PP 可以表示为：
P=(rsinθcosϕrsinθsinϕrcosθ)\it{P}=\left(\begin{matrix} r\sin\theta\cos\phi\\ r\sin\theta\sin\phi\\ r\cos\theta \end{matrix}\right) P=⎝⎛rsinθcosϕrsinθsinϕrcosθ⎠⎞
二、方向(Orientation)(Orientation)(Orientation)表示
2.1 基本公式
绕XXX轴旋转角γ\gammaγ，旋转矩阵为：
Rγ=[1000cosγ−sinγ0sinγcosγ]R_\gamma=\left[\begin{matrix} 1&0&0\\0&\cos\gamma&-\sin\gamma\\0&\sin\gamma&\cos\gamma \end{matrix}\right] Rγ=⎣⎡1000cosγsinγ0−sinγcosγ⎦⎤
绕YYY轴旋转角β\betaβ，旋转矩阵为：
Rβ=[cosβ0sinβ010−sinβ0cosβ]R_\beta=\left[\begin{matrix} \cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta \end{matrix}\right] Rβ=⎣⎡cosβ0−sinβ010sinβ0cosβ⎦⎤
绕ZZZ轴旋转角α\alphaα，旋转矩阵为：
Rα=[cosα−sinα0sinαcosα0001]R_\alpha=\left[\begin{matrix} \cos\alpha&-\sin\alpha&0\\ \sin\alpha&\cos\alpha&0\\ 0&0&1 \end{matrix}\right] Rα=⎣⎡cosαsinα0−sinαcosα0001⎦⎤
一个坐标系{B}\{B\}{B}相对另一个坐标系{A}\{A\}{A}
BAR=[n→o→a→]=[nxoxaxnyoyaynzozaz](1)^A_BR=\left[\begin{matrix} \overrightarrow{n}&\overrightarrow{o}&\overrightarrow{a} \end{matrix}\right]= \left[\begin{matrix} n_x&o_x&a_x\\ n_y&o_y&a_y\\ n_z&o_z&a_z \end{matrix}\right]\tag{1} BAR=[noa]=⎣⎡nxnynzoxoyozaxayaz⎦⎤(1)
我们容易知道三维空间中的方向只有3个自由度，而上述表达方式含有9个变量，因此我们需要添加6个约束如下：
- n→⋅o→=0\overrightarrow{\bf n}\cdot\overrightarrow{\bf o}=0n⋅o=0
- n→⋅a→=0\overrightarrow{\bf n}\cdot\overrightarrow{\bf a}=0n⋅a=0
- a→⋅o→=0\overrightarrow{\bf a}\cdot\overrightarrow{\bf o}=0a⋅o=0
- ∣n→∣=1\left|\overrightarrow{n}\right|=1∣∣n∣∣=1
- ∣o→∣=1\left|\overrightarrow{o}\right|=1∣∣o∣∣=1
- ∣a→∣=1\left|\overrightarrow{a}\right|=1∣∣a∣∣=1
当然我们也可以根据它们的几何关系将上述约束简化为：
- n→⋅o→=0\overrightarrow{\bf n}\cdot\overrightarrow{\bf o}=0n⋅o=0
- a→=n→×o→\overrightarrow{\bf a}=\overrightarrow{\bf n}\times\overrightarrow{\bf o}a=n×o
- ∣n→∣=1\left|\overrightarrow{n}\right|=1∣∣n∣∣=1
- ∣o→∣=1\left|\overrightarrow{o}\right|=1∣∣o∣∣=1
- ∣a→∣=1\left|\overrightarrow{a}\right|=1∣∣a∣∣=1
2.2 RPY角
假设坐标系{B}\{B\}{B}初始时与参考坐标系{A}\{A\}{A}重合，先绕坐标系{A}\{A\}{A}的XAX_AXA轴旋转角γ\gammaγ，再绕坐标系{A}\{A\}{A}的YAY_AYA轴旋转角β\betaβ，最后绕坐标系{A}\{A\}{A}的ZAZ_AZA轴旋转角α\alphaα。在上述过程中，各个旋转量表达如下：
| Name | 中文名 | 表达式 | 所绕轴 |
|---|---|---|---|
| RollRollRoll | 横滚角 | γ\gammaγ | XXX |
| PitchPitchPitch | 俯仰角 | β\betaβ | YYY |
| YawYawYaw | 航向角 | α\alphaα | ZZZ |
则{B}\{B\}{B}相对于{A}\{A\}{A}最终的姿态为：
BAR=RZ(α)RY(β)RX(γ)(2)^A_BR=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\tag{2} BAR=RZ(α)RY(β)RX(γ)(2)
具体计算为：
BAR=RZ(α)RY(β)RX(γ)=[cosα−sinα0sinαcosα0001]×[cosβ0sinβ010−sinβ0cosβ]×[1000cosγ−sinγ0sinγcosγ]=[cα−sα0sαcα0001]×[cβ0sβ010−sβ0cβ]×[1000cγ−sγ0sγcγ]=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsγsβcα−cαsγ−sβcβsγcβcγ]=[nxoxaxnyoyaynzozaz](3)\begin{aligned} ^A_BR&=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\ &=\left[\begin{matrix} \cos\alpha&-\sin\alpha&0\\ \sin\alpha&\cos\alpha&0\\ 0&0&1 \end{matrix}\right]\times \left[\begin{matrix} \cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta \end{matrix}\right]\times \left[\begin{matrix} 1&0&0\\0&\cos\gamma&-\sin\gamma\\0&\sin\gamma&\cos\gamma \end{matrix}\right]\\ &=\left[\begin{matrix} c\alpha&-s\alpha&0\\ s\alpha&c\alpha&0\\ 0&0&1 \end{matrix}\right]\times \left[\begin{matrix} c\beta&0&s\beta\\ 0&1&0\\ -s\beta&0&c\beta \end{matrix}\right]\times \left[\begin{matrix} 1&0&0\\0&c\gamma&-s\gamma\\0&s\gamma&c\gamma \end{matrix}\right]\\ &=\left[\begin{matrix} c\alpha c\beta&c\alpha s\beta s\gamma-s\alpha c\gamma&c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta&s\alpha s\beta s\gamma+c\alpha c\gamma&s\gamma s\beta c\alpha-c\alpha s\gamma\\ -s\beta&c\beta s\gamma&c\beta c\gamma \end{matrix}\right]\\ &=\left[\begin{matrix} n_x&o_x&a_x\\ n_y&o_y&a_y\\ n_z&o_z&a_z \end{matrix}\right] \end{aligned}\tag{3} BAR=RZ(α)RY(β)RX(γ)=⎣⎡cosαsinα0−sinαcosα0001⎦⎤×⎣⎡cosβ0−sinβ010sinβ0cosβ⎦⎤×⎣⎡1000cosγsinγ0−sinγcosγ⎦⎤=⎣⎡cαsα0−sαcα0001⎦⎤×⎣⎡cβ0−sβ010sβ0cβ⎦⎤×⎣⎡1000cγsγ0−sγcγ⎦⎤=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsγsβcα−cαsγcβcγ⎦⎤=⎣⎡nxnynzoxoyozaxayaz⎦⎤(3)
2.3 欧拉角
Z-Y-X\textbf{Z-Y-X}Z-Y-X欧拉角
假设坐标系{B}\{B\}{B}初始时与参考坐标系{A}\{A\}{A}重合，先绕坐标系{B}\{B\}{B}的ZBZ_BZB轴旋转角α\alphaα，再绕坐标系{B}\{B\}{B}的YBY_BYB轴旋转角β\betaβ，最后绕坐标系{B}\{B\}{B}的XBX_BXB轴旋转角γ\gammaγ。则{B}\{B\}{B}相对于{A}\{A\}{A}最终的姿态为：
BAR=RZ(α)RY(β)RX(γ)=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ](4)^A_BR=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\ =\left[\begin{matrix} c\alpha c\beta&c\alpha s\beta s\gamma-s\alpha c\gamma&c\alpha s\beta c\gamma+s\alpha s\gamma\\ s\alpha c\beta&s\alpha s\beta s\gamma+c\alpha c\gamma&s\alpha s\beta c\gamma-c\alpha s\gamma\\ -s\beta&c\beta s\gamma&c\beta c\gamma \end{matrix}\right]\tag{4} BAR=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤(4)
Z-Y-Z\textbf{Z-Y-Z}Z-Y-Z欧拉角
假设坐标系{B}\{B\}{B}初始时与参考坐标系{A}\{A\}{A}重合，先绕坐标系{B}\{B\}{B}的ZBZ_BZB轴旋转角α\alphaα，再绕坐标系{B}\{B\}{B}的YBY_BYB轴旋转角β\betaβ，最后绕坐标系{B}\{B\}{B}的ZBZ_BZB轴旋转角γ\gammaγ。则{B}\{B\}{B}相对于{A}\{A\}{A}最终的姿态为：
BAR=RZ(α)RY(β)RZ(γ)=[cαcβcγ−sαsγ−cαcβsγ−sαcγcαsβsαcβcγ+cαsγ−sαcβsγ+cαcγsαsβ−sβcγsβsγcβ](5)^A_BR=R_Z(\alpha)R_Y(\beta)R_Z(\gamma)\\ =\left[\begin{matrix} c\alpha c\beta c\gamma-s\alpha s\gamma&-c\alpha c\beta s\gamma-s\alpha c\gamma&c\alpha s\beta\\ s\alpha c\beta c\gamma+c\alpha s\gamma&-s\alpha c\beta s\gamma+c\alpha c\gamma&s\alpha s\beta\\ -s\beta c\gamma&s\beta s\gamma&c\beta \end{matrix}\right]\tag{5} BAR=RZ(α)RY(β)RZ(γ)=⎣⎡cαcβcγ−sαsγsαcβcγ+cαsγ−sβcγ−cαcβsγ−sαcγ−sαcβsγ+cαcγsβsγcαsβsαsβcβ⎦⎤(5)
三、平移运动(Displacement)(Displacement)(Displacement)
不妨假设坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵为BAT^A_BTBAT：
BAT=[nxoxaxpxnyoyaypynzozazpz0001]=[n⃗o⃗a⃗p⃗]^A_BT=\left[\begin{matrix} n_x&o_x&a_x&p_x\\ n_y&o_y&a_y&p_y\\ n_z&o_z&a_z&p_z\\ 0&0&0&1 \end{matrix}\right]=\left[\begin{matrix}\vec{n}&\vec{o}&\vec{a}&\vec{p}\end{matrix}\right] BAT=⎣⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎤=[noap]
第一种情况：坐标系{B}\{B\}{B}沿着坐标系{A}\{A\}{A}的坐标轴平移AΔp⃗=[ΔpxΔpxΔpx0]T^A\Delta \vec{p}=\begin{matrix}[\Delta{p_x}&\Delta{p_x}&\Delta{p_x}&0]^T\end{matrix}AΔp=[ΔpxΔpxΔpx0]T。
那么平移后坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵为BAT′^A_BT'BAT′：
BAT′=Tdisp×BAT=[100Δpx010Δpy001Δpz0001]×[nxoxaxpxnyoyaypynzozazpz0001]=[nxoxaxpx+Δpxnyoyaypy+Δpynzozazpz+Δpz0001]=[n⃗o⃗a⃗p⃗+AΔp⃗](6)\begin{aligned}^A_BT'&=T_{disp}\times^A_BT\\ &=\left[\begin{matrix} 1&0&0&\Delta{p_x}\\ 0&1&0&\Delta{p_y}\\ 0&0&1&\Delta{p_z}\\ 0&0&0&1 \end{matrix}\right] \times\left[\begin{matrix} n_x&o_x&a_x&p_x\\ n_y&o_y&a_y&p_y\\ n_z&o_z&a_z&p_z\\ 0&0&0&1 \end{matrix}\right]\\ &=\left[\begin{matrix} n_x&o_x&a_x&p_x+\Delta{p_x}\\ n_y&o_y&a_y&p_y+\Delta{p_y}\\ n_z&o_z&a_z&p_z+\Delta{p_z}\\ 0&0&0&1 \end{matrix}\right]\\ &=\left[\begin{matrix}\vec{n}&\vec{o}&\vec{a}&\vec{p}+^A\Delta \vec{p}\end{matrix}\right] \end{aligned}\tag{6} BAT′=Tdisp×BAT=⎣⎡100001000010ΔpxΔpyΔpz1⎦⎤×⎣⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎤=⎣⎡nxnynz0oxoyoz0axayaz0px+Δpxpy+Δpypz+Δpz1⎦⎤=[noap+AΔp](6)
第二种情况：坐标系{B}\{B\}{B}沿着坐标系{B}\{B\}{B}的坐标轴平移BΔp⃗=[Δpx′Δpy′Δpz′]T^B\Delta \vec{p}=\begin{matrix}[\Delta{p_x'}&\Delta{p_y'}&\Delta{p_z'}]^T\end{matrix}BΔp=[Δpx′Δpy′Δpz′]T。
那么平移后坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵为BAT′^A_BT'BAT′。
显然，平移运动不会改变坐标系中轴的方向（OrientationOrientationOrientation），因此我们只需要关心[pxpypz0]T\begin{matrix}[p_x&p_y&p_z&0]^T\end{matrix}[pxpypz0]T的变化。
因此，为了得到[pxpypz0]T\begin{matrix}[p_x&p_y&p_z&0]^T\end{matrix}[pxpypz0]T的变化，我们需要将[BΔp⃗0]\left[\begin{matrix}^B\Delta\vec{p}\\0\end{matrix}\right][BΔp0]转化为[AΔp⃗0]\left[\begin{matrix}^A\Delta\vec{p}\\0\end{matrix}\right][AΔp0]，而我们知道[BΔp⃗0]\left[\begin{matrix}^B\Delta\vec{p}\\0\end{matrix}\right][BΔp0]可以在BAT^A_BTBAT的作用下得到[AΔp⃗0]\left[\begin{matrix}^A\Delta\vec{p}\\0\end{matrix}\right][AΔp0]：
[BΔp⃗0]=BAT×[BΔp⃗0]=[nxoxaxpxnyoyaypynzozazpz0001]×[Δpx′Δpy′Δpz′0]=Δpx′×n⃗+Δpy′×o⃗+Δpz′×a⃗(7)\begin{aligned}\left[\begin{matrix}^B\Delta\vec{p}\\0\end{matrix}\right]&=^A_BT\times\left[\begin{matrix}^B\Delta\vec{p}\\0\end{matrix}\right]\\ &=\left[\begin{matrix} n_x&o_x&a_x&p_x\\ n_y&o_y&a_y&p_y\\ n_z&o_z&a_z&p_z\\ 0&0&0&1 \end{matrix}\right]\times\left[\begin{matrix}\Delta{p_x'}\\\Delta{p_y'}\\\Delta{p_z'}\\0\end{matrix}\right]\\ &=\Delta{p_x'}\times\vec{n}+\Delta{p_y'}\times\vec{o}+\Delta{p_z'}\times\vec{a} \end{aligned}\tag{7} [BΔp0]=BAT×[BΔp0]=⎣⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎤×⎣⎡Δpx′Δpy′Δpz′0⎦⎤=Δpx′×n+Δpy′×o+Δpz′×a(7)
最终齐次变换矩阵BAT′^A_BT'BAT′为：
BAT′=[n⃗o⃗a⃗p⃗+Δpx′×n⃗+Δpy′×o⃗+Δpz′×a⃗]=[n⃗o⃗a⃗p⃗]×[I3BΔp⃗0⃗1×31]=BAT×[I3BΔp⃗0⃗1×31](8)\begin{aligned}^A_BT'&=\begin{matrix}[\vec{n}&\vec{o}&\vec{a}&\vec{p}+\Delta{p_x'}\times\vec{n}+\Delta{p_y'}\times\vec{o}+\Delta{p_z'}\times\vec{a}]\end{matrix}\\ &=\begin{matrix}[\vec{n}&\vec{o}&\vec{a}&\vec{p}]\end{matrix}\times \left[\begin{matrix} I_3&^B\Delta\vec{p}\\ \vec{0}_{1\times3}&1 \end{matrix}\right]\\ &=^A_BT\times\left[\begin{matrix} I_3&^B\Delta\vec{p}\\ \vec{0}_{1\times3}&1 \end{matrix}\right] \end{aligned}\tag{8} BAT′=[noap+Δpx′×n+Δpy′×o+Δpz′×a]=[noap]×[I301×3BΔp1]=BAT×[I301×3BΔp1](8)
多次平移可由重复上述表达式得到。
四、旋转运动(Rotation)(Rotation)(Rotation)
不妨假设坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵为BAT^A_BTBAT。
第一种情况：坐标系{B}\{B\}{B}依次绕绝对坐标系{A}\{A\}{A}的坐标轴MiM_iMi旋转θi\theta_iθi角（i=1,2,3,⋯,ni=1,2,3,\cdots,ni=1,2,3,⋯,n），每次旋转的齐次变换矩阵为TiT_iTi（Ti=[RMi(θi)0⃗3×10⃗1×31]T_i=\left[\begin{matrix}R_{M_{i}}(\theta_i)&\vec{0}_{3\times1}\\\vec{0}_{1\times3}&1\end{matrix}\right]Ti=[RMi(θi)01×303×11]，RMi(θi)R_{M_{i}}(\theta_i)RMi(θi)可由2.1节得到），最终旋转后坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵BAT′^A_BT'BAT′为：
BAT′=Tn×Tn−1×⋯Ti×⋯T1×BAT(9)\begin{aligned} ^A_BT'&=T_{n}\times T_{n-1}\times\cdots T_i\times\cdots T_1\times{^A_B}T \end{aligned}\tag{9} BAT′=Tn×Tn−1×⋯Ti×⋯T1×BAT(9)
第二种情况：坐标系{B}\{B\}{B}依次绕相对坐标系{B}\{B\}{B}的坐标轴MiM_iMi旋转θi\theta_iθi角（i=1,2,3,⋯,ni=1,2,3,\cdots,ni=1,2,3,⋯,n），每次旋转的齐次变换矩阵为TiT_iTi（Ti=[RMi(θi)0⃗3×10⃗1×31]T_i=\left[\begin{matrix}R_{M_{i}}(\theta_i)&\vec{0}_{3\times1}\\\vec{0}_{1\times3}&1\end{matrix}\right]Ti=[RMi(θi)01×303×11]，RMi(θi)R_{M_{i}}(\theta_i)RMi(θi)可由2.1节得到），最终旋转后坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵BAT′^A_BT'BAT′为：
BAT′=BAT×T1×T2×⋯Ti×⋯Tn(10)\begin{aligned} ^A_BT'&={^A_B}T\times T_{1}\times T_{2}\times\cdots T_i\times\cdots T_n \end{aligned}\tag{10} BAT′=BAT×T1×T2×⋯Ti×⋯Tn(10)
五、复合运动(Compound)(Compound)(Compound)
综合3&4节我们可以得到：
不妨假设坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵为BAT^A_BTBAT。
将坐标系{B}\{B\}{B}依次相对于坐标系{A/B}\{A/B\}{A/B}的轴MMM旋转θ\thetaθ角/平移ddd位移，⋯\cdots⋯。
对上述过程进行合理排序，得到如下描述：
将坐标系{B}\{B\}{B}依次相对于坐标系{A}\{A\}{A}运动（旋转/平移）oio_ioi，每次运动对应的齐次变换矩阵为Ti,i=1,2,3,⋯,mT_i,i=1,2,3,\cdots,mTi,i=1,2,3,⋯,m；将坐标系{B}\{B\}{B}依次相对于坐标系{B}\{B\}{B}运动（旋转/平移）oj′o'_joj′，每次运动对应的齐次变换矩阵为Tj,j=1,2,3,⋯,nT_j,j=1,2,3,\cdots,nTj,j=1,2,3,⋯,n。
依据已知知识，我们不难得到：
若oio_ioi为平移量，那么Ti=[I3×3Δp⃗i0⃗1×31]T_i=\left[\begin{matrix}I_{3\times3}&\Delta\vec{p}_i\\\vec{0}_{1\times3}&1\end{matrix}\right]Ti=[I3×301×3Δpi1]；如果oio_ioi为旋转量，那么Ti=[RMi(θi)0⃗3×10⃗1×31]T_i=\left[\begin{matrix}R_{M_{i}}(\theta_i)&\vec{0}_{3\times1}\\\vec{0}_{1\times3}&1\end{matrix}\right]Ti=[RMi(θi)01×303×11]。
若oj′o'_joj′为平移量，那么Tj=[I3BΔp⃗j0⃗1×31]T_j=\left[\begin{matrix}I_3&^B\Delta\vec{p}_j\\\vec{0}_{1\times3}&1\end{matrix}\right]Tj=[I301×3BΔpj1]；如果oj′o'_joj′为旋转量，那么Tj=[RMj(θj)0⃗3×10⃗1×31]T_j=\left[\begin{matrix}R_{M_{j}}(\theta_j)&\vec{0}_{3\times1}\\\vec{0}_{1\times3}&1\end{matrix}\right]Tj=[RMj(θj)01×303×11]。
运动后坐标系{B}\{B\}{B}相对于坐标系{A}\{A\}{A}的齐次变换矩阵BAT′^A_BT'BAT′为：
BAT′=Tm×Tm−1×⋯Ti×⋯T1×BAT×T1×T2×⋯Tj×⋯Tn(11)\begin{aligned} ^A_BT'&=T_{m}\times T_{m-1}\times\cdots T_i\times\cdots T_1\times{^A_B}T\times T_{1}\times T_{2}\times\cdots T_j\times\cdots T_n \end{aligned}\tag{11} BAT′=Tm×Tm−1×⋯Ti×⋯T1×BAT×T1×T2×⋯Tj×⋯Tn(11)
六、改进DH\bf DHDH法
6.1 DH\bf DHDH定义与建模
DH法：Denavit-Hartenberg法
改进DH建模：
上述各个量分别为：
- ai−1a_{i-1}ai−1：连杆长度（LinklengthLink\ lengthLink length），沿着X→i−1\overrightarrow{X}_{i-1}Xi−1，从Z→i−1\overrightarrow{Z}_{i-1}Zi−1移动到Z→i\overrightarrow{Z}_{i}Zi的距离；
- αi−1\alpha_{i-1}αi−1：连杆转角（LinktwistLink\ twistLink twist），绕着X→i−1\overrightarrow{X}_{i-1}Xi−1，从Z→i−1\overrightarrow{Z}_{i-1}Zi−1移动到Z→i\overrightarrow{Z}_{i}Zi的距离；
- did_idi：连杆偏距（LinkoffsetLink\ offsetLink offset），沿着Z→i\overrightarrow{Z}_{i}Zi，从X→i−1\overrightarrow{X}_{i-1}Xi−1移动到X→i\overrightarrow{X}_{i}Xi的距离；
- θi\theta_iθi：关节角（JointangleJoint\ angleJoint angle），绕着Z→i\overrightarrow{Z}_{i}Zi，从X→i−1\overrightarrow{X}_{i-1}Xi−1移动到X→i\overrightarrow{X}_{i}Xi的距离；
改进DH法建模步骤：
- 确定每个轴的方向和相邻轴之间的公共法线；
- 将轴与公共法线的交点当做所在坐标系的原点；
- 将轴的方向确定为Z→i\overrightarrow{Z}_iZi，将轴Z→i\overrightarrow{Z}_iZi与轴Z→i+1\overrightarrow{Z}_{i+1}Zi+1之间公共法线确定为X→i\overrightarrow{X}_iXi;
- 根据尽可能将参数设置为0的原则，确定坐标系{0}\{0\}{0}的X→0\overrightarrow{X}_0X0和坐标系{n}\{n\}{n}的X→n\overrightarrow{X}_nXn；
- 根据X→i\overrightarrow{X}_iXi和Z→i\overrightarrow{Z}_iZi，利用右手定则确定Y→i\overrightarrow{Y}_iYi
- 将参数填入DH表中
DH表格式：
| LinkiLink\ iLink i | αi−1\alpha_{i-1}αi−1 | ai−1a_{i-1}ai−1 | did_idi | θi\theta_iθi |
|---|---|---|---|---|
| ⋮\vdots⋮ | ⋮\vdots⋮ | ⋮\vdots⋮ | ⋮\vdots⋮ | ⋮\vdots⋮ |
6.2 改进DH\bf DHDH法下的齐次变换矩阵
显然，DH法下的齐次变换矩阵是从坐标系{i−1}\{i-1\}{i−1}到坐标系{i}\{i\}{i}，且一切运动都是相对于相对坐标系进行，因此齐次变换矩阵i−1Ti^{i-1}T_{i}i−1Ti：
i−1Ti=RX(αi−1)×DX(ai−1)×RZ(θi)×DZ(di)=[10000cosαi−1−sinαi−100sinαi−1cosαi−100001]×[100ai−1010000100001]×[cosθi−sinθi00sinθicosθi0000100001]×[10000100001di0001]=[cθi−sθi0ai−1sθicαi−1cθicαi−1−sαi−1−sαi−1disθisαi−1cθisαi−1cαi−1cαi−1di0001]\begin{aligned} ^{i-1}T_i&=R_X(\alpha_{i-1})\times D_X(a_{i-1})\times R_Z(\theta_i)\times D_Z(d_i)\\ &=\left[\begin{matrix}1&0&0&0\\0&\cos\alpha_{i-1}&-\sin\alpha_{i-1}&0\\ 0&\sin\alpha_{i-1}&\cos\alpha_{i-1}&0\\0&0&0&1\end{matrix}\right]\times \left[\begin{matrix}1&0&0&a_{i-1}\\0&1&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right]\times \left[\begin{matrix}\cos\theta_i&-\sin\theta_i&0&0\\\sin\theta_i&\cos\theta_i&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right]\times \left[\begin{matrix}1&0&0&0\\0&1&0&0\\ 0&0&1&d_{i}\\0&0&0&1\end{matrix}\right]\\ &=\left[\begin{matrix}c\theta_i&-s\theta_i&0&a_{i-1}\\ s\theta_ic\alpha_{i-1}&c\theta_ic\alpha_{i-1}&-s\alpha_{i-1}&-s\alpha_{i-1}d_i\\ s\theta_is\alpha_{i-1}&c\theta_is\alpha_{i-1}&c\alpha_{i-1}&c\alpha_{i-1}d_i\\ 0&0&0&1 \end{matrix}\right] \end{aligned} i−1Ti=RX(αi−1)×DX(ai−1)×RZ(θi)×DZ(di)=⎣⎡10000cosαi−1sinαi−100−sinαi−1cosαi−100001⎦⎤×⎣⎡100001000010ai−1001⎦⎤×⎣⎡cosθisinθi00−sinθicosθi0000100001⎦⎤×⎣⎡10000100001000di1⎦⎤=⎣⎡cθisθicαi−1sθisαi−10−sθicθicαi−1cθisαi−100−sαi−1cαi−10ai−1−sαi−1dicαi−1di1⎦⎤
因此，对于整个机械臂系统来说，坐标系{i}\{i\}{i}相对于坐标系{0}\{0\}{0}的齐次变换矩阵为：
0Ti=0T11T2⋯i−1Ti,i∈{1,2,3,⋯,n}(12)^0T_i=^0T_1\ ^1T_2\cdots^{i-1}T_i\ ,i\in\{1,2,3,\cdots,n\}\tag{12} 0Ti=0T1 1T2⋯i−1Ti ,i∈{1,2,3,⋯,n}(12)
6.3 工具箱Toolbox\bf ToolboxToolbox的部分使用
- 安装Toolbox
下载链接（如果嫌麻烦，本文最后将提供相关资料下载的途径）
将下载的文件夹放置进Matlab的安装路径下
将文件夹中的rvctoolsrvctoolsrvctools文件夹加入到路径中
打开Matlab，在命令行中运行startup_rvc，然后再运行rtbdemo
toolbox关于正运动学的基本使用
该工具箱与理论的吻合例证
设置：θ=π/8;d=3.5;a=4.7;α=−3π/7\theta=\pi/8;d=3.5;a=4.7;\alpha=-3\pi/7θ=π/8;d=3.5;a=4.7;α=−3π/7
例证如下：
theta=pi/8;d=3.5;a=4.7;alpha=-3*pi/7;
T1=[cos(theta) -sin(theta) 0 a;sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d;sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) cos(alpha)*d;0 0 0 1];
L=Link([theta,d,a,alpha 0],'modified');%0代表旋转关节
T2=L.A(theta)
T1-T2
创建LinkLinkLink
L=Link([theta,d,a,alpha])%默认为标准DH和旋转关节
L=Link([theta,d,a,alpha,sigma])%sigma代表关节类型，0为旋转关节，1为移动关节
%也可通过外文注明来确定关节类型
L=Link('revolute','d',1.2,'a',0.3,'alpha',pi/2)
L=Link('prismatic','theta',pi/2,'a',0.3,'alpha',pi/2)
%若要使用改进DH法，则可通过下列方式
L=Link([0 0 1 pi/2 1],'modified')
连接LinksLinksLinks
%方法1
L(1)=Link([0,0,1,pi/2],'modified');L(2)=Link([0,0,2,pi/2],'modified');
twolink=SerialLink(L,'name','twolink')
%方法2
DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink')
连接RobotRobotRobot
DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
%方法1
R=SerialLink([R1,R2])
%方法2
R=R1*R2
模型作图
DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
R=SerialLink([R1,R2]);
R.plot([pi/2,-pi/2,pi/2,-pi/2])
改参作图
DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
R=SerialLink([R1,R2]);
R.teach()
6.4 一个简单例子
为了检验学习效果，我将提供一个简单例子，该例子为六自由度驱动机器人，表示如下：
我们对其DH建模，首先对各个坐标系进行标定：
DH表：
| # | θ\thetaθ | ddd | aaa | α\alphaα |
|---|---|---|---|---|
| 0—1 | θ1\theta_1θ1 | 0 | 0 | 0° |
| 1—2 | θ2\theta_2θ2 | 0 | 000 | 90° |
| 2—3 | θ3\theta_3θ3 | 0 | a2a_2a2 | 0° |
| 3—4 | θ4\theta_4θ4 | 0 | a3a_3a3 | 0° |
| 4—5 | θ5\theta_5θ5 | 0 | a4a_4a4 | -90° |
| 5—6 | θ6\theta_6θ6 | 0 | 000 | 90° |
因此我们可以得到相邻坐标系的齐次变换矩阵：
0T1=[cθ1−sθ100sθ1cθ10000100001],1T2=[cθ2−sθ20000−10sθ2cθ2000001],2T3=[cθ3−sθ30a2sθ3cθ30000100001],3T4=[cθ4−sθ40a3sθ4cθ40000100001],4T5=[cθ5−sθ50a40010−sθ5−cθ5000001],5T6=[cθ5−sθ50000−10sθ5cθ5000001]\begin{aligned} &^0T_1=\left[\begin{matrix}c\theta_1&-s\theta_1&0&0\\ s\theta_1&c\theta_1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{matrix}\right],^1T_2=\left[\begin{matrix}c\theta_2&-s\theta_2&0&0\\0&0&-1&0\\ s\theta_2&c\theta_2&0&0\\0&0&0&1\end{matrix}\right], ^2T_3=\left[\begin{matrix}c\theta_3&-s\theta_3&0&a_2\\ s\theta_3&c\theta_3&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right],\\ &^3T_4=\left[\begin{matrix}c\theta_4&-s\theta_4&0&a_3\\ s\theta_4&c\theta_4&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right],^4T_5=\left[\begin{matrix}c\theta_5&-s\theta_5&0&a_4\\0&0&1&0\\ -s\theta_5&-c\theta_5&0&0\\0&0&0&1\end{matrix}\right], ^5T_6=\left[\begin{matrix}c\theta_5&-s\theta_5&0&0\\0&0&-1&0\\ s\theta_5&c\theta_5&0&0\\0&0&0&1\end{matrix}\right] \end{aligned} 0T1=⎣⎡cθ1sθ100−sθ1cθ10000100001⎦⎤,1T2=⎣⎡cθ20sθ20−sθ20cθ200−1000001⎦⎤,2T3=⎣⎡cθ3sθ300−sθ3cθ3000010a2001⎦⎤,3T4=⎣⎡cθ4sθ400−sθ4cθ4000010a3001⎦⎤,4T5=⎣⎡cθ50−sθ50−sθ50−cθ500100a4001⎦⎤,5T6=⎣⎡cθ50sθ50−sθ50cθ500−1000001⎦⎤
因此，该机器人的手（末端执行器）所在坐标系{H}\{H\}{H}相对于机器人基座{R}\{R\}{R}的齐次变换矩阵RTH^RT_HRTH为：
RTH=0T1×1T2×2T3×3T4×4T5×5T6(13)^RT_H=^0T_1\times^1T_2\times^2T_3\times^3T_4\times^4T_5\times^5T_6\tag{13} RTH=0T1×1T2×2T3×3T4×4T5×5T6(13)
七、下载资源
链接：百度网盘下载资源
提取码：noqa
参考目录
[1]Niku, Saeed B . Introduction to robotics : analysis, control, applications[J]. 2011.
【机器人学】正运动学详解相关推荐
- 机器人控制算法一之四轴机械臂正、逆运动学详解
1.余弦定理 推广: a^2=b^2+c^2-2*b*c*cos(A) b^2=a^2
- 大学物理第一章 质点运动学详解
参考系,坐标系,物理模型 一.运动的相对性和绝对性 一般来说,运动是绝对的,任何物体都在运动着,但运动也是相对的,都是在一定环境下和特定条件下运动的 二.参考系 运动是绝对的,但运动的描述却是相对的, ...
- 任正非详解华为不上市与家人不接班问题
外界对我司上市问题议论纷纷,我负责澄清一下.董事会20多年来,从未研究过上市问题,因为我们认为上市不适合我们的发展.最近徐直军(编者注,华为轮值CEO成员.副董事长.高级副总裁)向某运营商高层的讲话, ...
- 机器人学回炉重造（1-2）：各种典型机械臂的正运动学建模（标准D-H法）
文章目录 写在前面 三连杆平面机械臂 平行四边形操作臂 闭链结构 例:平行四边形操作臂 球形臂 拟人臂 球腕 斯坦福机械臂 带球形手腕的拟人化机械臂 DLR机械臂 参考文献 写在前面 本文所有机械臂均 ...
- 机器人学基础（1）-位置运动学-正运动学、逆运动学方程建立及其求解
机器人学基础(1)位置运动学:正运动学.逆运动学方程建立及其求解 机器人学基础学了个知识框架入门,以此来写一下总结笔记,便于以后要用到相关知识点进行翻阅. 本次机器人学基础笔记主要分为几个章节:位置运 ...
- 工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解
[Matlab Robotics Toolbox]robotics toolbox学习及使用记录,方便自己后面复习.改进. 基于Matlab R2019b 9.5; Peter Corke的Robot ...
- 6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要 本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
- pandas dataframe中的列进行重新排序、倒排、正排、自定义排序详解及实践
pandas dataframe中的列进行重新排序,pandas dataframe列重排.倒排.正排.自定义排序详解及实践 实施数据构建: import pandas as pd import nu ...
- 张正友相机标定算法详解
张正友相机标定算法详解 1.齐次表示法与一些基本结论 1.1 点与直线的齐次表示 在射影几何中,通常用齐次方式来表达点与直线.比如p=(u,v)p=(u,v)p=(u,v)被表示成p^=(x1,x ...
- matlab机器人雅可比矩阵实验,机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解（工具：matlab）...
1.利用工具箱建模,供验证用 选用二轴平面机械臂,两个关节角分别是theta1和theta2. view(3); Lnk1 = Link([ 0 0 100 0 0]);%theta,d,a,alph ...
最新文章
- c语言名字程序,c语言获得程序位数和操作系统位数和名称
- 现在的人工智能只是“窄AI”？
- OSPF中 hello报文的 内容
- shell中字符串基本用法
- php鼠标悬停字体变大代码CS6,Dreamweaver cs6设置代码字体大小的方法
- Ribbon-3使用配置文件自定义Ribbon Client
- 圆周率里有每个人的银行卡密码和生日？混知乎的程序员果然都是神一般的存在...
- Python--day26--复习
- 关东升的iOS实战系列图书 《iOS实战：传感器卷（Swift版）》已经上市
- 对象引用 String引用 基本类型引用 差别
- C++ STL string迭代器的使用
- SQL日志文件损坏或丢失造成数据库置疑的解决办法
- MATLAB 字符串与矩阵的学习
- 单片机c语言编程定时,单片机C语言编程定时器的几种表达方式
- 京东联盟高级API-批量创建多个推广位
- 英文文本大小写的转换
- 信息收集--子域名查询
- windows 10修改host，有效解决权限问题，请惠存
- 2020-12-6 CCF201803-2 碰撞的小球
- 大学校园无线覆盖解决方案