机器人学——姿态描述方法（欧拉角，固定角，D-H法，绕定轴旋转）
文章目录
- 绕坐标轴旋转
- 欧拉角
- 固定角
- D-H变换矩阵
- 绕定轴旋转
绕坐标轴旋转
刚体绕X，Y，Z轴旋转θ角的公式
R X ( θ ) = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_{X}(\theta)=\left[ \begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right] RX(θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤
R Y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_{Y}(\theta)=\left[ \begin{array}{ccc}{\cos \theta} & {0} & {\sin \theta} \\ {0} & {1} & {0} \\ {-\sin \theta} & {0} & {\cos \theta}\end{array}\right] RY(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
R Z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_{Z}(\theta)=\left[ \begin{array}{ccc}{\cos \theta} & {-\sin \theta} & {0} \\ {\sin \theta} & {\cos \theta} & {0} \\ {0} & {0} & {1}\end{array}\right] RZ(θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤
欧拉角
例如首先将坐标系{B}和一个已知参考坐标系 { A } \{A\} {A}重合。先将 { B } \{B\} {B}绕 Z B Z_B ZB旋转 α \alpha α，再绕 Y B Y_B YB旋转 β \beta β，最后绕 X B X_B XB旋转 γ \gamma γ
这样三个一组的旋转被称作欧拉角。
上面描述的就是ZYX欧拉角，旋转过程如下图所示：
其旋转矩阵为：
R Z ′ Y ′ X ′ ( α , β , γ ) = 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 γ ] \boldsymbol{R}_{Z^{\prime} Y^{\prime} X^{\prime}}(\alpha, \beta, \gamma) =R_{z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)\\ =\left[ \begin{array}{ccc} {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{array}\right] RZ′Y′X′(α,β,γ)=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γ⎦⎤
所有12种欧拉角坐标系的定义由下式给出
R X ′ Y ′ Z ′ ( α , β , γ ) = [ c β c γ − c β s γ s β s α s β c γ + c α s γ − s α s β s γ + c α c γ − s α c β − c α s β c γ + s α s γ c α s β s γ + s α c γ c α c β ] \boldsymbol{R}_{X^{\prime} Y^{\prime} Z^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc}{c \beta c \gamma} & {-c \beta s \gamma} & {s \beta} \\ {s \alpha s \beta c \gamma+c \alpha s \gamma} & {-s \alpha s \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta } \\{-c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha s \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta }\end{array}\right] RX′Y′Z′(α,β,γ)=⎣⎡cβcγsαsβcγ+cαsγ−cαsβcγ+sαsγ−cβsγ−sαsβsγ+cαcγcαsβsγ+sαcγsβ−sαcβcαcβ⎦⎤
R X ′ Z ′ Y ′ ( α , β , γ ) = [ c β c γ − s β c β s γ c α s β c γ + s α s γ c α c β c α s β s γ − s α c γ s α s β c γ − c α s γ s α c β s α s β s γ + c α c γ ] \boldsymbol{R}_{X^{\prime} Z^{\prime} Y^{\prime} }(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc}{c \beta c \gamma} & {-s \beta} & {c \beta s \gamma} \\{c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha c \beta } & {c \alpha s \beta s \gamma-s \alpha c \gamma} \\ {s \alpha s \beta c \gamma-c \alpha s \gamma} & {s \alpha c \beta } & {s \alpha s \beta s \gamma+c \alpha c \gamma}\end{array}\right] RX′Z′Y′(α,β,γ)=⎣⎡cβcγcαsβcγ+sαsγsαsβcγ−cαsγ−sβcαcβsαcβcβsγcαsβsγ−sαcγsαsβsγ+cαcγ⎦⎤
R Y ′ X ′ Z ′ ( α , β , γ ) = [ s α s β s γ + c α c γ s α s β c γ − c α s γ s α c β c β s γ c β c γ − s β c α s β s γ − s α c γ c α s β c γ + s α s γ c α c β ] \boldsymbol{R}_{Y^{\prime} X^{\prime} Z^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {s \alpha s \beta s \gamma+c \alpha c \gamma} & {s \alpha s \beta c \gamma-c \alpha s \gamma} & {s \alpha c \beta} \\{c \beta s \gamma} & {c \beta c \gamma} & {-s \beta} \\{c \alpha s \beta s \gamma-s \alpha c \gamma} & {c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha c \beta } \end{array}\right] RY′X′Z′(α,β,γ)=⎣⎡sαsβsγ+cαcγcβsγcαsβsγ−sαcγsαsβcγ−cαsγcβcγcαsβcγ+sαsγsαcβ−sβcαcβ⎦⎤
R Y ′ Z ′ X ′ ( α , β , γ ) = [ c α c β − c α s β c γ + s α s γ c α s β s γ + s α c γ s β c β c γ − c β s γ − s α c β s α s β c γ + c α s γ − s α s β s γ + c α c γ ] \boldsymbol{R}_{Y^{\prime} Z^{\prime} X^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {c \alpha c \beta } & {-c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha s \beta s \gamma+s \alpha c \gamma} \\{s \beta} & {c \beta c \gamma} & {-c \beta s \gamma} \\ {-s \alpha c \beta} & {s \alpha s \beta c \gamma+c \alpha s \gamma} & {-s \alpha s \beta s \gamma+c \alpha c \gamma} \end{array}\right] RY′Z′X′(α,β,γ)=⎣⎡cαcβsβ−sαcβ−cαsβcγ+sαsγcβcγsαsβcγ+cαsγcαsβsγ+sαcγ−cβsγ−sαsβsγ+cαcγ⎦⎤
R Z ′ X ′ Y ′ ( α , β , γ ) = [ − s α s β s γ + c α c γ − s α c β s α s β c γ + c α s γ c α s β s γ + s α c γ c α c β − c α s β c γ + s α s γ − c β s γ s β c β c γ ] \boldsymbol{R}_{Z^{\prime} X^{\prime} Y^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {-s \alpha s \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta} & {s \alpha s \beta c \gamma+c \alpha s \gamma} \\ {c \alpha s \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta } & {-c \alpha s \beta c \gamma+s \alpha s \gamma} \\{-c \beta s \gamma} & {s \beta} & {c \beta c \gamma} \end{array}\right] RZ′X′Y′(α,β,γ)=⎣⎡−sαsβsγ+cαcγcαsβsγ+sαcγ−cβsγ−sαcβcαcβsβsαsβcγ+cαsγ−cαsβcγ+sαsγcβcγ⎦⎤
R Z ′ Y ′ 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 γ ] \boldsymbol{R}_{Z^{\prime} Y^{\prime} X^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {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{array}\right] RZ′Y′X′(α,β,γ)=⎣⎡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γ⎦⎤
R X ′ Y ′ X ′ ( α , β , γ ) = [ c β s β s γ s β c γ s α s β − s α c β s γ + c α c γ − s α c β c γ − c α s γ c α s β c α c β s γ + s α c γ c α c β c γ − s α s γ ] \boldsymbol{R}_{X^{\prime} Y^{\prime} X^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc}{c \beta} & {s \beta s \gamma} & {s \beta c \gamma} \\{s \alpha s \beta } & {-s \alpha c \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta c \gamma-c \alpha s \gamma} \\{c \alpha s \beta} & {c \alpha c \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta c \gamma-s \alpha s \gamma} \end{array}\right] RX′Y′X′(α,β,γ)=⎣⎡cβsαsβcαsβsβsγ−sαcβsγ+cαcγcαcβsγ+sαcγsβcγ−sαcβcγ−cαsγcαcβcγ−sαsγ⎦⎤
R X ′ Z ′ X ′ ( α , β , γ ) = [ c β − s β c γ s β s γ c α s β c α c β c γ − s α s γ − c α c β s γ − s α c γ s α s β s α c β c γ + c α s γ − s α c β s γ + c α c γ ] \boldsymbol{R}_{X^{\prime} Z^{\prime} X^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc}{c \beta} & {-s \beta c \gamma}& {s \beta s \gamma} \\{c \alpha s \beta} & {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha c \beta s \gamma-s \alpha c \gamma} \\{s \alpha s \beta } & {s \alpha c \beta c \gamma+c \alpha s \gamma} & {-s \alpha c \beta s \gamma+c \alpha c \gamma} \end{array}\right] RX′Z′X′(α,β,γ)=⎣⎡cβcαsβsαsβ−sβcγcαcβcγ−sαsγsαcβcγ+cαsγsβsγ−cαcβsγ−sαcγ−sαcβsγ+cαcγ⎦⎤
R Y ′ X ′ Y ′ ( α , β , γ ) = [ − s α c β s γ + c α c γ s α s β s α c β c γ + c α s γ s β s γ c β − s β c γ − c α c β s γ − s α c γ c α s β c α c β c γ − s α s γ ] \boldsymbol{R}_{Y^{\prime} X^{\prime} Y^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc}{-s \alpha c \beta s \gamma+c \alpha c \gamma} & {s \alpha s \beta } & {s \alpha c \beta c \gamma+c \alpha s \gamma} \\{s \beta s \gamma} & {c \beta} & {-s \beta c \gamma} \\{-c \alpha c \beta s \gamma-s \alpha c \gamma} & {c \alpha s \beta} & {c \alpha c \beta c \gamma-s \alpha s \gamma} \end{array}\right] RY′X′Y′(α,β,γ)=⎣⎡−sαcβsγ+cαcγsβsγ−cαcβsγ−sαcγsαsβcβcαsβsαcβcγ+cαsγ−sβcγcαcβcγ−sαsγ⎦⎤
R Y ′ Z ′ Y ′ ( α , β , γ ) = [ c α c β c γ − s α s γ − c α s β c α c β s γ + s α c γ s β s γ c β s β c γ − s α c β c γ − c α s γ s α s β − s α c β s γ + c α c γ ] \boldsymbol{R}_{Y^{\prime} Z^{\prime} Y^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha s \beta} & {c \alpha c \beta s \gamma+s \alpha c \gamma} \\{s \beta s \gamma} & {c \beta} & {s \beta c \gamma} \\{-s \alpha c \beta c \gamma-c \alpha s \gamma} & {s \alpha s \beta } & {-s \alpha c \beta s \gamma+c \alpha c \gamma} \end{array}\right] RY′Z′Y′(α,β,γ)=⎣⎡cαcβcγ−sαsγsβsγ−sαcβcγ−cαsγ−cαsβcβsαsβcαcβsγ+sαcγsβcγ−sαcβsγ+cαcγ⎦⎤
R Z ′ X ′ Z ′ ( α , β , γ ) = [ − s α c β s γ + c α c γ − s α c β c γ − c α s γ s α s β c α c β s γ + s α c γ c α c β c γ − s α s γ − c α s β s β s γ s β c γ c β ] \boldsymbol{R}_{Z^{\prime} X^{\prime} Z^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {-s \alpha c \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta c \gamma-c \alpha s \gamma} & {s \alpha s \beta } \\{c \alpha c \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha s \beta} \\{s \beta s \gamma} & {s \beta c \gamma} & {c \beta} \end{array}\right] RZ′X′Z′(α,β,γ)=⎣⎡−sαcβsγ+cαcγcαcβsγ+sαcγsβsγ−sαcβcγ−cαsγcαcβcγ−sαsγsβcγsαsβ−cαsβcβ⎦⎤
R Z ′ Y ′ 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 β ] \boldsymbol{R}_{Z^{\prime} Y^{\prime} Z^{\prime}}(\alpha, \beta, \gamma) =\left[ \begin{array}{ccc} {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{array}\right] RZ′Y′Z′(α,β,γ)=⎣⎡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β⎦⎤
固定角
固定角的描述方法与欧拉角类似只不过是绕基础坐标系的坐标轴旋转：
例如XYZ固定角坐标系，有时把他们定义为回转角、俯仰角和偏转角。
其旋转矩阵为：
R X Y Z ( γ , β , α ) = 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 γ ] \boldsymbol{R}_{XYZ}(\gamma, \beta, \alpha) =R_{z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)\\ =\left[ \begin{array}{ccc} {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{array}\right] RXYZ(γ,β,α)=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γ⎦⎤
可以看出他与ZYX欧拉角结果相同。其实有如下结论：
三次绕固定轴旋转的最终姿态和以相反顺序三次绕运动坐标轴旋转的最终姿态相同
所有12种固定角坐标系的定义由下式给出：
R X Y Z ( γ , β , α ) = [ 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 γ ] \boldsymbol{R}_{XYZ}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {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{array}\right] RXYZ(γ,β,α)=⎣⎡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γ⎦⎤
R X Z Y ( γ , β , α ) = [ c α c β − c α s β c γ + s α s γ c α s β s γ + s α c γ s β c β c γ − c β s γ − s α c β s α s β c γ + c α s γ − s α s β s γ + c α c γ ] \boldsymbol{R}_{XZY}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {c \alpha c \beta } & {-c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha s \beta s \gamma+s \alpha c \gamma} \\{s \beta} & {c \beta c \gamma} & {-c \beta s \gamma} \\ {-s \alpha c \beta} & {s \alpha s \beta c \gamma+c \alpha s \gamma} & {-s \alpha s \beta s \gamma+c \alpha c \gamma} \end{array}\right] RXZY(γ,β,α)=⎣⎡cαcβsβ−sαcβ−cαsβcγ+sαsγcβcγsαsβcγ+cαsγcαsβsγ+sαcγ−cβsγ−sαsβsγ+cαcγ⎦⎤
R Y X Z ( γ , β , α ) = [ − s α s β s γ + c α c γ − s α c β s α s β c γ + c α s γ c α s β s γ + s α c γ c α c β − c α s β c γ + s α s γ − c β s γ s β c β c γ ] \boldsymbol{R}_{YXZ}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {-s \alpha s \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta} & {s \alpha s \beta c \gamma+c \alpha s \gamma} \\ {c \alpha s \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta } & {-c \alpha s \beta c \gamma+s \alpha s \gamma} \\{-c \beta s \gamma} & {s \beta} & {c \beta c \gamma} \end{array}\right] RYXZ(γ,β,α)=⎣⎡−sαsβsγ+cαcγcαsβsγ+sαcγ−cβsγ−sαcβcαcβsβsαsβcγ+cαsγ−cαsβcγ+sαsγcβcγ⎦⎤
R Y Z X ( γ , β , α ) = [ c β c γ − s β c β s γ c α s β c γ + s α s γ c α c β c α s β s γ − s α c γ s α s β c γ − c α s γ s α c β s α s β s γ + c α c γ ] \boldsymbol{R}_{YZX}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc}{c \beta c \gamma} & {-s \beta} & {c \beta s \gamma} \\{c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha c \beta } & {c \alpha s \beta s \gamma-s \alpha c \gamma} \\ {s \alpha s \beta c \gamma-c \alpha s \gamma} & {s \alpha c \beta } & {s \alpha s \beta s \gamma+c \alpha c \gamma}\end{array}\right] RYZX(γ,β,α)=⎣⎡cβcγcαsβcγ+sαsγsαsβcγ−cαsγ−sβcαcβsαcβcβsγcαsβsγ−sαcγsαsβsγ+cαcγ⎦⎤
R Z X Y ( γ , β , α ) = [ s α s β s γ + c α c γ s α s β c γ − c α s γ s α c β c β s γ c β c γ − s β c α s β s γ − s α c γ c α s β c γ + s α s γ c α c β ] \boldsymbol{R}_{ZXY}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {s \alpha s \beta s \gamma+c \alpha c \gamma} & {s \alpha s \beta c \gamma-c \alpha s \gamma} & {s \alpha c \beta} \\{c \beta s \gamma} & {c \beta c \gamma} & {-s \beta} \\{c \alpha s \beta s \gamma-s \alpha c \gamma} & {c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha c \beta } \end{array}\right] RZXY(γ,β,α)=⎣⎡sαsβsγ+cαcγcβsγcαsβsγ−sαcγsαsβcγ−cαsγcβcγcαsβcγ+sαsγsαcβ−sβcαcβ⎦⎤
R Z Y X ( γ , β , α ) = [ c β c γ − c β s γ s β s α s β c γ + c α s γ − s α s β s γ + c α c γ − s α c β − c α s β c γ + s α s γ c α s β s γ + s α c γ c α c β ] \boldsymbol{R}_{ZYX}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc}{c \beta c \gamma} & {-c \beta s \gamma} & {s \beta} \\ {s \alpha s \beta c \gamma+c \alpha s \gamma} & {-s \alpha s \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta } \\{-c \alpha s \beta c \gamma+s \alpha s \gamma} & {c \alpha s \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta }\end{array}\right] RZYX(γ,β,α)=⎣⎡cβcγsαsβcγ+cαsγ−cαsβcγ+sαsγ−cβsγ−sαsβsγ+cαcγcαsβsγ+sαcγsβ−sαcβcαcβ⎦⎤
R X Y X ( γ , β , α ) = [ c β s β s γ s β c γ s α s β − s α c β s γ + c α c γ − s α c β c γ − c α s γ c α s β c α c β s γ + s α c γ c α c β c γ − s α s γ ] \boldsymbol{R}_{XYX}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc}{c \beta} & {s \beta s \gamma} & {s \beta c \gamma} \\{s \alpha s \beta } & {-s \alpha c \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta c \gamma-c \alpha s \gamma} \\{c \alpha s \beta} & {c \alpha c \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta c \gamma-s \alpha s \gamma} \end{array}\right] RXYX(γ,β,α)=⎣⎡cβsαsβcαsβsβsγ−sαcβsγ+cαcγcαcβsγ+sαcγsβcγ−sαcβcγ−cαsγcαcβcγ−sαsγ⎦⎤
R X Z X ( γ , β , α ) = [ c β − s β c γ s β s γ c α s β c α c β c γ − s α s γ − c α c β s γ − s α c γ s α s β s α c β c γ + c α s γ − s α c β s γ + c α c γ ] \boldsymbol{R}_{XZX}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc}{c \beta} & {-s \beta c \gamma}& {s \beta s \gamma} \\{c \alpha s \beta} & {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha c \beta s \gamma-s \alpha c \gamma} \\{s \alpha s \beta } & {s \alpha c \beta c \gamma+c \alpha s \gamma} & {-s \alpha c \beta s \gamma+c \alpha c \gamma} \end{array}\right] RXZX(γ,β,α)=⎣⎡cβcαsβsαsβ−sβcγcαcβcγ−sαsγsαcβcγ+cαsγsβsγ−cαcβsγ−sαcγ−sαcβsγ+cαcγ⎦⎤
R Y X Y ( γ , β , α ) = [ − s α c β s γ + c α c γ s α s β s α c β c γ + c α s γ s β s γ c β − s β c γ − c α c β s γ − s α c γ c α s β c α c β c γ − s α s γ ] \boldsymbol{R}_{YXY}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc}{-s \alpha c \beta s \gamma+c \alpha c \gamma} & {s \alpha s \beta } & {s \alpha c \beta c \gamma+c \alpha s \gamma} \\{s \beta s \gamma} & {c \beta} & {-s \beta c \gamma} \\{-c \alpha c \beta s \gamma-s \alpha c \gamma} & {c \alpha s \beta} & {c \alpha c \beta c \gamma-s \alpha s \gamma} \end{array}\right] RYXY(γ,β,α)=⎣⎡−sαcβsγ+cαcγsβsγ−cαcβsγ−sαcγsαsβcβcαsβsαcβcγ+cαsγ−sβcγcαcβcγ−sαsγ⎦⎤
R Y Z Y ( γ , β , α ) = [ c α c β c γ − s α s γ − c α s β c α c β s γ + s α c γ s β s γ c β s β c γ − s α c β c γ − c α s γ s α s β − s α c β s γ + c α c γ ] \boldsymbol{R}_{YZY}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha s \beta} & {c \alpha c \beta s \gamma+s \alpha c \gamma} \\{s \beta s \gamma} & {c \beta} & {s \beta c \gamma} \\{-s \alpha c \beta c \gamma-c \alpha s \gamma} & {s \alpha s \beta } & {-s \alpha c \beta s \gamma+c \alpha c \gamma} \end{array}\right] RYZY(γ,β,α)=⎣⎡cαcβcγ−sαsγsβsγ−sαcβcγ−cαsγ−cαsβcβsαsβcαcβsγ+sαcγsβcγ−sαcβsγ+cαcγ⎦⎤
R Z X Z ( γ , β , α ) = [ − s α c β s γ + c α c γ − s α c β c γ − c α s γ s α s β c α c β s γ + s α c γ c α c β c γ − s α s γ − c α s β s β s γ s β c γ c β ] \boldsymbol{R}_{ZXZ}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {-s \alpha c \beta s \gamma+c \alpha c \gamma} & {-s \alpha c \beta c \gamma-c \alpha s \gamma} & {s \alpha s \beta } \\{c \alpha c \beta s \gamma+s \alpha c \gamma} & {c \alpha c \beta c \gamma-s \alpha s \gamma} & {-c \alpha s \beta} \\{s \beta s \gamma} & {s \beta c \gamma} & {c \beta} \end{array}\right] RZXZ(γ,β,α)=⎣⎡−sαcβsγ+cαcγcαcβsγ+sαcγsβsγ−sαcβcγ−cαsγcαcβcγ−sαsγsβcγsαsβ−cαsβcβ⎦⎤
R Z Y 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 β ] \boldsymbol{R}_{ZYZ}(\gamma, \beta, \alpha) =\left[ \begin{array}{ccc} {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{array}\right] RZYZ(γ,β,α)=⎣⎡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β⎦⎤
D-H变换矩阵
D-H法建立的变换矩阵的过程类似于欧拉角，其变换顺序为
沿 X i X_i Xi轴从 Z i Z_i Zi向 Z i + 1 Z_{i+1} Zi+1移动 a i a_i ai
绕 X i X_i Xi轴从 Z i Z_i Zi向 Z i + 1 Z_{i+1} Zi+1旋转 α i \alpha_i αi
沿 Z i Z_i Zi轴从 X i − 1 X_{i-1} Xi−1向 X i X_i Xi移动 d i d_i di
绕 Z i Z_i Zi轴从 X i − 1 X_{i-1} Xi−1向 X i X_i Xi旋转 θ i \theta_i θi
所以一个关节的变换矩阵如下
i i − 1 T = R X ( α i − 1 ) D X ( a i − 1 ) R Z ( θ i ) D Z ( d i ) _{i}^{i-1} T=R_{X}\left(\alpha_{i-1}\right) D_{X}\left(a_{i-1}\right) R_{Z}\left(\theta_{i}\right) D_{Z}\left(d_{i}\right) ii−1T=RX(αi−1)DX(ai−1)RZ(θi)DZ(di)
i i − 1 T = [ 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 ] _{i}^{i-1} T=\left[ \begin{array}{cccc}{c \boldsymbol{\theta}_{i}} & {-s \theta_{i}} & {0} & {a_{i-1}} \\ {s \theta_{i} c \alpha_{i-1}} & {c \boldsymbol{\theta}_{i} c \alpha_{i-1}} & {-s \alpha_{i-1}} & {-s \alpha_{i-1} d_{i}} \\ {s \theta_{i} s \alpha_{i-1}} & {c \theta_{i} s \alpha_{i-1}} & {c \alpha_{i-1}} & {c \alpha_{i-1} d_{i}} \\ {0} & {0} & {0} & {1}\end{array}\right] ii−1T=⎣⎢⎢⎡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⎦⎥⎥⎤
绕定轴旋转
矢量 q q q绕单位矢量 k ^ \widehat{k} k 旋转 θ \theta θ角，由Rodriques公式得：
q ′ = q c o s θ + s i n θ ( k ^ × q ) + ( 1 − c o s θ ) ( k ^ ⋅ q ^ ) k ^ q'=qcos\theta+sin\theta(\widehat{k}\times q)+(1-cos\theta)(\widehat{k}\cdot \widehat{q})\widehat{k} q′=qcosθ+sinθ(k ×q)+(1−cosθ)(k ⋅q )k
其旋转矩阵表示为：
R K ( θ ) = [ k x k x v θ + c θ k x k y v θ − k z s θ k x k z v θ + k y s θ k x k y v θ + k z s θ k y k y v θ + c θ k y k z v θ − k x s θ k x k z v θ − k y s θ k y k z v θ + k x s θ k z k z v θ + c θ ] \boldsymbol{R}_{K}(\theta) =\left[ \begin{array}{ccc} {k_xk_xv\theta+c\theta} & {k_xk_yv\theta-k_zs\theta} & {k_xk_zv\theta+k_ys\theta} \\{k_xk_yv\theta+k_zs\theta} & {k_yk_yv\theta+c\theta} & {k_yk_zv\theta-k_xs\theta} \\{k_xk_zv\theta-k_ys\theta} & {k_yk_zv\theta+k_xs\theta} & {k_zk_zv\theta+c\theta} \end{array}\right] RK(θ)=⎣⎡kxkxvθ+cθkxkyvθ+kzsθkxkzvθ−kysθkxkyvθ−kzsθkykyvθ+cθkykzvθ+kxsθkxkzvθ+kysθkykzvθ−kxsθkzkzvθ+cθ⎦⎤
其中
v θ = 1 − c θ v_\theta=1-c\theta vθ=1−cθ