【机器人学】正运动学详解

【机器人学】正运动学详解

  • 机器人学中的正运动学
    • 一、位置 ( P o s i t i o n ) (Position) (Position)表示
    • 二、方向 ( O r i e n t a t i o n ) (Orientation) (Orientation)表示
      • 2.1 基本公式
      • 2.2 RPY角
      • 2.3 欧拉角
    • 三、平移运动 ( D i s p l a c e m e n t ) (Displacement) (Displacement)
    • 四、旋转运动 ( R o t a t i o n ) (Rotation) (Rotation)
    • 五、复合运动 ( C o m p o u n d ) (Compound) (Compound)
    • 六、改进 D H \bf DH DH
      • 6.1 D H \bf DH DH定义与建模
      • 6.2 改进 D H \bf DH DH法下的齐次变换矩阵
      • 6.3 工具箱 T o o l b o x \bf Toolbox Toolbox的部分使用
      • 6.4 一个简单例子
    • 七、下载资源
    • 参考目录

机器人学中的正运动学

本文对机器人学基本知识中的正运动学(Forward Kinematics)进行介绍,适合入门学习机器人学,如果你发现本文有疏漏、错误之处,欢迎读者批评指正。如果你喜欢本文,可以收藏本文,也可将本文分享给你的朋友、同学,但是转载不被允许

一、位置 ( P o s i t i o n ) (Position) (Position)表示

  • 笛卡尔坐标
    P = ( x y z ) \it{P}=\left(\begin{matrix} x\\y\\z \end{matrix}\right) P= xyz

  • 柱坐标
    变量: v a r i a b l e s = ( ρ θ z ) variables=\left(\begin{matrix} \rho\\\theta\\z \end{matrix}\right) variables= ρθz
    因此,位置 P \it P P 可以表示为:
    P = ( ρ cos ⁡ θ ρ sin ⁡ θ z ) \it{P}=\left(\begin{matrix} \rho\cos\theta\\\rho\sin\theta\\z \end{matrix}\right) P= ρcosθρsinθz
    【机器人学】正运动学详解_第1张图片

  • 球坐标
    变量: v a r i a b l e s = ( r θ ϕ ) variables=\left(\begin{matrix} r\\\theta\\\phi \end{matrix}\right) variables= rθϕ
    因此,位置 P \it P P 可以表示为:
    P = ( r sin ⁡ θ cos ⁡ ϕ r sin ⁡ θ sin ⁡ ϕ r cos ⁡ θ ) \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θ
    【机器人学】正运动学详解_第2张图片

二、方向 ( O r i e n t a t i o n ) (Orientation) (Orientation)表示

2.1 基本公式

X X X轴旋转角 γ \gamma γ,旋转矩阵为:
R γ = [ 1 0 0 0 cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ 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γ0sinγcosγ
Y Y Y轴旋转角 β \beta β,旋转矩阵为:
R β = [ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β ] R_\beta=\left[\begin{matrix} \cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta \end{matrix}\right] Rβ= cosβ0sinβ010sinβ0cosβ
Z Z Z轴旋转角 α \alpha α,旋转矩阵为:
R α = [ cos ⁡ α − sin ⁡ α 0 sin ⁡ α cos ⁡ α 0 0 0 1 ] R_\alpha=\left[\begin{matrix} \cos\alpha&-\sin\alpha&0\\ \sin\alpha&\cos\alpha&0\\ 0&0&1 \end{matrix}\right] Rα= cosαsinα0sinαcosα0001
一个坐标系 { B } \{B\} {B}相对另一个坐标系 { A } \{A\} {A}
B A R = [ n → o → a → ] = [ n x o x a x n y o y a y n z o z a z ] (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=[n o a ]= nxnynzoxoyozaxayaz (1)
【机器人学】正运动学详解_第3张图片

我们容易知道三维空间中的方向只有3个自由度,而上述表达方式含有9个变量,因此我们需要添加6个约束如下:

  • n → ⋅ o → = 0 \overrightarrow{\bf n}\cdot\overrightarrow{\bf o}=0 n o =0
  • n → ⋅ a → = 0 \overrightarrow{\bf n}\cdot\overrightarrow{\bf a}=0 n a =0
  • a → ⋅ o → = 0 \overrightarrow{\bf a}\cdot\overrightarrow{\bf o}=0 a 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}=0 n 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} X A X_A XA轴旋转角 γ \gamma γ,再绕坐标系 { A } \{A\} {A} Y A Y_A YA轴旋转角 β \beta β,最后绕坐标系 { A } \{A\} {A} Z A Z_A ZA轴旋转角 α \alpha α。在上述过程中,各个旋转量表达如下:

Name 中文名 表达式 所绕轴
R o l l Roll Roll 横滚角 γ \gamma γ X X X
P i t c h Pitch Pitch 俯仰角 β \beta β Y Y Y
Y a w Yaw Yaw 航向角 α \alpha α Z Z Z
【机器人学】正运动学详解_第4张图片

{ B } \{B\} {B}相对于 { A } \{A\} {A}最终的姿态为:
B A R = R Z ( α ) R Y ( β ) R X ( γ ) (2) ^A_BR=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\tag{2} BAR=RZ(α)RY(β)RX(γ)(2)
具体计算为:
B A R = R Z ( α ) R Y ( β ) R X ( γ ) = [ cos ⁡ α − sin ⁡ α 0 sin ⁡ α cos ⁡ α 0 0 0 1 ] × [ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β ] × [ 1 0 0 0 cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ ] = [ c α − s α 0 s α c α 0 0 0 1 ] × [ c β 0 s β 0 1 0 − s β 0 c β ] × [ 1 0 0 0 c γ − s γ 0 s γ 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 γ ] = [ n x o x a x n y o y a y n z o z a z ] (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α0sinαcosα0001 × cosβ0sinβ010sinβ0cosβ × 1000cosγsinγ0sinγcosγ = cαsα0sαcα0001 × cβ0sβ010sβ0cβ × 1000cγsγ0sγ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} Z B Z_B ZB轴旋转角 α \alpha α,再绕坐标系 { B } \{B\} {B} Y B Y_B YB轴旋转角 β \beta β,最后绕坐标系 { B } \{B\} {B} X B X_B XB轴旋转角 γ \gamma γ。则 { B } \{B\} {B}相对于 { A } \{A\} {A}最终的姿态为:
B A R = R Z ( α ) R Y ( β ) R X ( γ ) = [ 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} Z B Z_B ZB轴旋转角 α \alpha α,再绕坐标系 { B } \{B\} {B} Y B Y_B YB轴旋转角 β \beta β,最后绕坐标系 { B } \{B\} {B} Z B Z_B ZB轴旋转角 γ \gamma γ。则 { B } \{B\} {B}相对于 { A } \{A\} {A}最终的姿态为:
B A R = R Z ( α ) R Y ( β ) R Z ( γ ) = [ 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)

三、平移运动 ( D i s p l a c e m e n t ) (Displacement) (Displacement)

不妨假设坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵为 B A T ^A_BT BAT
B A T = [ 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 ] = [ 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 =[n o a p ]
【机器人学】正运动学详解_第5张图片

第一种情况:坐标系 { B } \{B\} {B}沿着坐标系 { A } \{A\} {A}的坐标轴平移 A Δ p ⃗ = [ Δ p x Δ p x Δ p x 0 ] 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}的齐次变换矩阵为 B A T ′ ^A_BT' BAT
B A T ′ = T d i s p × B A T = [ 1 0 0 Δ p x 0 1 0 Δ p y 0 0 1 Δ p z 0 0 0 1 ] × [ 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 ] = [ n x o x a x p x + Δ p x n y o y a y p y + Δ p y n z o z a z p z + Δ p z 0 0 0 1 ] = [ 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 =[n o a p +AΔp ](6)
第二种情况:坐标系 { B } \{B\} {B}沿着坐标系 { B } \{B\} {B}的坐标轴平移 B Δ p ⃗ = [ Δ p x ′ Δ p y ′ Δ p z ′ ] 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}的齐次变换矩阵为 B A T ′ ^A_BT' BAT

显然,平移运动不会改变坐标系中轴的方向( O r i e n t a t i o n Orientation Orientation),因此我们只需要关心 [ p x p y p z 0 ] T \begin{matrix}[p_x&p_y&p_z&0]^T\end{matrix} [pxpypz0]T的变化。

因此,为了得到 [ p x p y p z 0 ] 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Δp 0]转化为 [ A Δ p ⃗ 0 ] \left[\begin{matrix}^A\Delta\vec{p}\\0\end{matrix}\right] [AΔp 0],而我们知道 [ B Δ p ⃗ 0 ] \left[\begin{matrix}^B\Delta\vec{p}\\0\end{matrix}\right] [BΔp 0]可以在 B A T ^A_BT BAT的作用下得到 [ A Δ p ⃗ 0 ] \left[\begin{matrix}^A\Delta\vec{p}\\0\end{matrix}\right] [AΔp 0]
[ B Δ p ⃗ 0 ] = B A T × [ B Δ p ⃗ 0 ] = [ 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 ] × [ Δ p x ′ Δ p y ′ Δ p z ′ 0 ] = Δ p x ′ × n ⃗ + Δ p y ′ × o ⃗ + Δ p z ′ × 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Δp 0]=BAT×[BΔp 0]= nxnynz0oxoyoz0axayaz0pxpypz1 × ΔpxΔpyΔpz0 =Δpx×n +Δpy×o +Δpz×a (7)
最终齐次变换矩阵 B A T ′ ^A_BT' BAT为:
B A T ′ = [ n ⃗ o ⃗ a ⃗ p ⃗ + Δ p x ′ × n ⃗ + Δ p y ′ × o ⃗ + Δ p z ′ × a ⃗ ] = [ n ⃗ o ⃗ a ⃗ p ⃗ ] × [ I 3 B Δ p ⃗ 0 ⃗ 1 × 3 1 ] = B A T × [ I 3 B Δ p ⃗ 0 ⃗ 1 × 3 1 ] (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=[n o a p +Δpx×n +Δpy×o +Δpz×a ]=[n o a p ]×[I30 1×3BΔp 1]=BAT×[I30 1×3BΔp 1](8)
多次平移可由重复上述表达式得到。

四、旋转运动 ( R o t a t i o n ) (Rotation) (Rotation)

不妨假设坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵为 B A T ^A_BT BAT

第一种情况:坐标系 { B } \{B\} {B}依次绕绝对坐标系 { A } \{A\} {A}的坐标轴 M i M_i Mi旋转 θ i \theta_i θi角( i = 1 , 2 , 3 , ⋯   , n i=1,2,3,\cdots,n i=1,2,3,,n),每次旋转的齐次变换矩阵为 T i T_i Ti T i = [ R M i ( θ i ) 0 ⃗ 3 × 1 0 ⃗ 1 × 3 1 ] 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)0 1×30 3×11] R M i ( θ i ) R_{M_{i}}(\theta_i) RMi(θi)可由2.1节得到),最终旋转后坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵 B A T ′ ^A_BT' BAT为:
B A T ′ = T n × T n − 1 × ⋯ T i × ⋯ T 1 × B A T (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×Tn1×Ti×T1×BAT(9)
第二种情况:坐标系 { B } \{B\} {B}依次绕相对坐标系 { B } \{B\} {B}的坐标轴 M i M_i Mi旋转 θ i \theta_i θi角( i = 1 , 2 , 3 , ⋯   , n i=1,2,3,\cdots,n i=1,2,3,,n),每次旋转的齐次变换矩阵为 T i T_i Ti T i = [ R M i ( θ i ) 0 ⃗ 3 × 1 0 ⃗ 1 × 3 1 ] 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)0 1×30 3×11] R M i ( θ i ) R_{M_{i}}(\theta_i) RMi(θi)可由2.1节得到),最终旋转后坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵 B A T ′ ^A_BT' BAT为:
B A T ′ = B A T × T 1 × T 2 × ⋯ T i × ⋯ T n (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)
【机器人学】正运动学详解_第6张图片

五、复合运动 ( C o m p o u n d ) (Compound) (Compound)

综合3&4节我们可以得到:

不妨假设坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵为 B A T ^A_BT BAT

将坐标系 { B } \{B\} {B}依次相对于坐标系 { A / B } \{A/B\} {A/B}的轴 M M M旋转 θ \theta θ角/平移 d d d位移, ⋯ \cdots

对上述过程进行合理排序,得到如下描述:

将坐标系 { B } \{B\} {B}依次相对于坐标系 { A } \{A\} {A}运动(旋转/平移) o i o_i oi,每次运动对应的齐次变换矩阵为 T i , i = 1 , 2 , 3 , ⋯   , m T_i,i=1,2,3,\cdots,m Ti,i=1,2,3,,m;将坐标系 { B } \{B\} {B}依次相对于坐标系 { B } \{B\} {B}运动(旋转/平移) o j ′ o'_j oj,每次运动对应的齐次变换矩阵为 T j , j = 1 , 2 , 3 , ⋯   , n T_j,j=1,2,3,\cdots,n Tj,j=1,2,3,,n

依据已知知识,我们不难得到:

o i o_i oi为平移量,那么 T i = [ I 3 × 3 Δ p ⃗ i 0 ⃗ 1 × 3 1 ] T_i=\left[\begin{matrix}I_{3\times3}&\Delta\vec{p}_i\\\vec{0}_{1\times3}&1\end{matrix}\right] Ti=[I3×30 1×3Δp i1];如果 o i o_i oi为旋转量,那么 T i = [ R M i ( θ i ) 0 ⃗ 3 × 1 0 ⃗ 1 × 3 1 ] 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)0 1×30 3×11]

o j ′ o'_j oj为平移量,那么 T j = [ I 3 B Δ p ⃗ j 0 ⃗ 1 × 3 1 ] T_j=\left[\begin{matrix}I_3&^B\Delta\vec{p}_j\\\vec{0}_{1\times3}&1\end{matrix}\right] Tj=[I30 1×3BΔp j1];如果 o j ′ o'_j oj为旋转量,那么 T j = [ R M j ( θ j ) 0 ⃗ 3 × 1 0 ⃗ 1 × 3 1 ] 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)0 1×30 3×11]

运动后坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵 B A T ′ ^A_BT' BAT为:
B A T ′ = T m × T m − 1 × ⋯ T i × ⋯ T 1 × B A T × T 1 × T 2 × ⋯ T j × ⋯ T n (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×Tm1×Ti×T1×BAT×T1×T2×Tj×Tn(11)

六、改进 D H \bf DH DH

6.1 D H \bf DH DH定义与建模

DH法:Denavit-Hartenberg法

改进DH建模:

【机器人学】正运动学详解_第7张图片

上述各个量分别为:

  • a i − 1 a_{i-1} ai1:连杆长度( L i n k   l e n g t h Link\ length Link length),沿着 X → i − 1 \overrightarrow{X}_{i-1} X i1,从 Z → i − 1 \overrightarrow{Z}_{i-1} Z i1移动到 Z → i \overrightarrow{Z}_{i} Z i的距离;
  • α i − 1 \alpha_{i-1} αi1:连杆转角( L i n k   t w i s t Link\ twist Link twist),绕着 X → i − 1 \overrightarrow{X}_{i-1} X i1,从 Z → i − 1 \overrightarrow{Z}_{i-1} Z i1移动到 Z → i \overrightarrow{Z}_{i} Z i的距离;
  • d i d_i di:连杆偏距( L i n k   o f f s e t Link\ offset Link offset),沿着 Z → i \overrightarrow{Z}_{i} Z i,从 X → i − 1 \overrightarrow{X}_{i-1} X i1移动到 X → i \overrightarrow{X}_{i} X i的距离;
  • θ i \theta_i θi:关节角( J o i n t   a n g l e Joint\ angle Joint angle),绕着 Z → i \overrightarrow{Z}_{i} Z i,从 X → i − 1 \overrightarrow{X}_{i-1} X i1移动到 X → i \overrightarrow{X}_{i} X i的距离;

改进DH法建模步骤

  1. 确定每个轴的方向和相邻轴之间的公共法线;
  2. 将轴与公共法线的交点当做所在坐标系的原点;
  3. 将轴的方向确定为 Z → i \overrightarrow{Z}_i Z i,将轴 Z → i \overrightarrow{Z}_i Z i与轴 Z → i + 1 \overrightarrow{Z}_{i+1} Z i+1之间公共法线确定为 X → i \overrightarrow{X}_i X i;
  4. 根据尽可能将参数设置为0的原则,确定坐标系 { 0 } \{0\} {0} X → 0 \overrightarrow{X}_0 X 0和坐标系 { n } \{n\} {n} X → n \overrightarrow{X}_n X n
  5. 根据 X → i \overrightarrow{X}_i X i Z → i \overrightarrow{Z}_i Z i,利用右手定则确定 Y → i \overrightarrow{Y}_i Y i
  6. 将参数填入DH表中

DH表格式

L i n k   i Link\ i Link i α i − 1 \alpha_{i-1} αi1 a i − 1 a_{i-1} ai1 d i d_i di θ i \theta_i θi
⋮ \vdots ⋮ \vdots ⋮ \vdots ⋮ \vdots ⋮ \vdots

6.2 改进 D H \bf DH DH法下的齐次变换矩阵

显然,DH法下的齐次变换矩阵是从坐标系 { i − 1 } \{i-1\} {i1}到坐标系 { i } \{i\} {i},且一切运动都是相对于相对坐标系进行,因此齐次变换矩阵 i − 1 T i ^{i-1}T_{i} i1Ti
i − 1 T i = R X ( α i − 1 ) × D X ( a i − 1 ) × R Z ( θ i ) × D Z ( d i ) = [ 1 0 0 0 0 cos ⁡ α i − 1 − sin ⁡ α i − 1 0 0 sin ⁡ α i − 1 cos ⁡ α i − 1 0 0 0 0 1 ] × [ 1 0 0 a i − 1 0 1 0 0 0 0 1 0 0 0 0 1 ] × [ cos ⁡ θ i − sin ⁡ θ i 0 0 sin ⁡ θ i cos ⁡ θ i 0 0 0 0 1 0 0 0 0 1 ] × [ 1 0 0 0 0 1 0 0 0 0 1 d i 0 0 0 1 ] = [ c θ i − s θ i 0 a i − 1 s θ i c α i − 1 c θ i c α i − 1 − s α i − 1 − s α i − 1 d i s θ i s α i − 1 c θ i s α i − 1 c α i − 1 c α i − 1 d i 0 0 0 1 ] \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} i1Ti=RX(αi1)×DX(ai1)×RZ(θi)×DZ(di)= 10000cosαi1sinαi100sinαi1cosαi100001 × 100001000010ai1001 × cosθisinθi00sinθicosθi0000100001 × 10000100001000di1 = cθisθicαi1sθisαi10sθicθicαi1cθisαi100sαi1cαi10ai1sαi1dicαi1di1
因此,对于整个机械臂系统来说,坐标系 { i } \{i\} {i}相对于坐标系 { 0 } \{0\} {0}的齐次变换矩阵为:
0 T i = 0 T 1   1 T 2 ⋯ i − 1 T i   , 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 1T2i1Ti ,i{1,2,3,,n}(12)

6.3 工具箱 T o o l b o x \bf Toolbox Toolbox的部分使用

  • 安装Toolbox
  1. 下载链接(如果嫌麻烦,本文最后将提供相关资料下载的途径)

  2. 将下载的文件夹放置进Matlab的安装路径下

  3. 将文件夹中的 r v c t o o l s rvctools rvctools文件夹加入到路径中

    【机器人学】正运动学详解_第8张图片
  4. 打开Matlab,在命令行中运行startup_rvc,然后再运行rtbdemo

    【机器人学】正运动学详解_第9张图片
  • 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
【机器人学】正运动学详解_第10张图片

创建 L i n k Link Link

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')
【机器人学】正运动学详解_第11张图片

连接 L i n k s Links Links

%方法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')
【机器人学】正运动学详解_第12张图片

连接 R o b o t Robot Robot

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
【机器人学】正运动学详解_第13张图片

模型作图

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])
【机器人学】正运动学详解_第14张图片

改参作图

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()
【机器人学】正运动学详解_第15张图片

6.4 一个简单例子

为了检验学习效果,我将提供一个简单例子,该例子为六自由度驱动机器人,表示如下:

【机器人学】正运动学详解_第16张图片

我们对其DH建模,首先对各个坐标系进行标定:
【机器人学】正运动学详解_第17张图片

DH表

# θ \theta θ d d d a a a α \alpha α
0—1 θ 1 \theta_1 θ1 0 0
1—2 θ 2 \theta_2 θ2 0 0 0 0 90°
2—3 θ 3 \theta_3 θ3 0 a 2 a_2 a2
3—4 θ 4 \theta_4 θ4 0 a 3 a_3 a3
4—5 θ 5 \theta_5 θ5 0 a 4 a_4 a4 -90°
5—6 θ 6 \theta_6 θ6 0 0 0 0 90°

因此我们可以得到相邻坐标系的齐次变换矩阵:
0 T 1 = [ c θ 1 − s θ 1 0 0 s θ 1 c θ 1 0 0 0 0 1 0 0 0 0 1 ] , 1 T 2 = [ c θ 2 − s θ 2 0 0 0 0 − 1 0 s θ 2 c θ 2 0 0 0 0 0 1 ] , 2 T 3 = [ c θ 3 − s θ 3 0 a 2 s θ 3 c θ 3 0 0 0 0 1 0 0 0 0 1 ] , 3 T 4 = [ c θ 4 − s θ 4 0 a 3 s θ 4 c θ 4 0 0 0 0 1 0 0 0 0 1 ] , 4 T 5 = [ c θ 5 − s θ 5 0 a 4 0 0 1 0 − s θ 5 − c θ 5 0 0 0 0 0 1 ] , 5 T 6 = [ c θ 5 − s θ 5 0 0 0 0 − 1 0 s θ 5 c θ 5 0 0 0 0 0 1 ] \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θ100sθ1cθ10000100001 ,1T2= cθ20sθ20sθ20cθ2001000001 ,2T3= cθ3sθ300sθ3cθ3000010a2001 ,3T4= cθ4sθ400sθ4cθ4000010a3001 ,4T5= cθ50sθ50sθ50cθ500100a4001 ,5T6= cθ50sθ50sθ50cθ5001000001
因此,该机器人的手(末端执行器)所在坐标系 { H } \{H\} {H}相对于机器人基座 { R } \{R\} {R}的齐次变换矩阵 R T H ^RT_H RTH为:
R T H = 0 T 1 × 1 T 2 × 2 T 3 × 3 T 4 × 4 T 5 × 5 T 6 (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.

你可能感兴趣的:(机器人学,线性代数,矩阵)