位姿描述和齐次变换

最新推荐文章于 2023-05-23 17:15:46 发布

葱花　　　

最新推荐文章于 2023-05-23 17:15:46 发布

阅读量6.3k

点赞数 16

分类专栏：机器人技术基础文章标签：线性代数矩阵

本文链接：https://blog.csdn.net/shenmideshuaige/article/details/122269840

版权

机器人技术基础专栏收录该内容

6 篇文章 7 订阅

订阅专栏

一、刚体位姿描述

描述刚体的位姿最常用的方法是在基座上建立一个固定坐标系{A}，在运动刚体上建立运动坐标系{B}，通过描述运动坐标系在固定坐标系的位置和姿态来描述刚体的空间位姿。

刚体的空间位置可用运动坐标系的原点坐标 ${ }^{A} \boldsymbol{P}_{B_{0}}=\left[\begin{array}{lll} p_{x} & p_{y} & p_{z} \end{array}\right]^{\mathrm{T}}$ 表示，运动坐标系的原点可以建立在刚体的质心或几何中心上，通过描述该点的坐标，可以很好地体现刚体的位置特征。

当刚体的姿态发生改变时，刚体上固连的运动坐标系的方向也会发生改变，如下图所示：

我们可以利用运动坐标系{A}的三个坐标轴在固定坐标系{B}的三个坐标轴的方向上的投影，即方向余弦来表示刚体的姿态：

${ }_{B}^{A} R=\left[\begin{array}{lll} { }^{A} x_{B} & { }^{4} y_{B} & { }^{A} z_{B} \end{array}\right]=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]$

等式右边三乘三的矩阵称为姿态矩阵，描述了运动坐标系{B}在固定坐标系{A}中的姿态。下面分别介绍绕X,Y,Z轴旋转 $\theta$ 角的姿态矩阵来帮助我们理解其含义。

我们可以用姿态矩阵 $R(x, \theta)$ 来表示运动坐标系{B}在固定坐标系{A}中的姿态：

$R(x, \theta)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & c \theta & -s \theta \\ 0 & s \theta & c \theta \end{array}\right]$

其中{B}的X轴与{A}的X轴同向，即 $X_B$ 与 $X_A$ 同向，所以方向向量 $_B^AX$ 为： $[1\quad 0\quad 0]^T$

$Y_B$ 在 $Y_AZ_A$ 平面，与 $Y_A$ 成θ角，因此方向向量 $^A_BY$ 为： $[0\quad c\theta \quad s\theta]^T$ $(c \theta=\cos\theta,\;s \theta=\sin\theta )$

$Z_B$ 在 $Y_AZ_A$ 平面，与 $Z_A$ 成θ角，因此方向向量 $^A_BZ$ 为： $[0\quad -s\theta \quad c\theta]^T$

将三者结合便可得到:

${ }_{B}^{A} R=\left[\begin{array}{lll} { }^{A} X_{B} & { }^{4} Y_{B} & { }^{A} Z_{B} \end{array}\right]=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & c \theta & -s \theta \\ 0 & s \theta & c \theta \end{array}\right]$

同理：

姿态矩阵为：

$R(y, \theta)=\left[\begin{array}{ccc} c \theta & 0 & s \theta \\ 0 & 1 & 0 \\ -s \theta & 0 & c \theta \end{array}\right]$

姿态矩阵为：

$\boldsymbol{R}(z, \theta)=\left[\begin{array}{ccc} c \theta & -s \theta & 0 \\ s \theta & c \theta & 0 \\ 0 & 0 & 1 \end{array}\right]$

由于姿态矩阵中三个列向量都为单位向量，且这三个列向量两两垂直，所以满足以下六个约束方程：

$\left.\right|^{A} x_{B}|=1 \quad|^{A} y_{B}|=1 \quad|^{A} z_{B} \mid=1$

${ }^{A} x_{B} \cdot{ }^{A} y_{B}=0 \quad{ }^{A} y_{B} \cdot{ }^{A} z_{B}=0 \quad{ }^{A} z_{B} \cdot{ }^{A} \cdot x_{B}=0$

因此可以看出，姿态矩阵中只有三个数是相互独立的，说明空间运动的刚体姿态变化包含三个独立的转动，需要三个独立的参数来描述。

根据姿态矩阵的特性，我们可以推导出其以下具有的独特的性质，在之后的使用中会带来极大的方便：

$\left|{ }_{B}^{A} R\right|=1 \quad{ }_{B}^{A} R^{-1}={ }_{B}^{A} R^{T}$

小结

位置描述： ${ }^{A} \boldsymbol{P}_{B_{0}}=\left[\begin{array}{lll} p_{x} & p_{y} & p_{z} \end{array}\right]^{\mathrm{T}}$

姿态描述： ${ }_{B}^{A} R=\left[\begin{array}{lll} { }^{A} x_{B} & { }^{4} y_{B} & { }^{A} z_{B} \end{array}\right]=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]$

刚体位姿描述： $\left\{\begin{array}{ll} { }_{B}^{A} \boldsymbol{R} & { }^{A} \boldsymbol{P}_{B_{0}} \end{array}\right\}$

二、坐标变换

1.坐标平移：

已知 $^BP$ 、 $^AP_{B0}$ ，求 $^AP$ ：

假设两个坐标系具有相同的方位，但两个坐标系的原点不重合，我们可以用{B}原点在{A}中的坐标来描述{B}在{A}中的位置，即矢量 $^AP_{B0}$ 。

点P在坐标系B中的位置为 $^BP$ ，则其在坐标系A中的位置 $^AP$ 可以通过矢量加法得到：

${ }^{A} \boldsymbol{P}={ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$

2.坐标旋转

已知 $^BP$ 、 $^AP_{B0}$ ，求 $^AP$ ：

假设两个坐标系具有相同的原点，但方位不同，已知B 点在坐标系B中的坐标

${ }^{B} \boldsymbol{P}=\left\{\begin{array}{c} { }^{B} P_{x} \quad { }^{B} P_{y} \quad { }^{B} P_{z} \end{array}\right\}^T$

我们可以将点P在B系中三个轴上的分量分别投影到A系的X,Y,Z轴上，得到P点在A系中的描述：

$^AP=\left\{\begin{array}{c} { }^{A} P_{x} \\ { }^{A} P_{y} \\ { }^{A} P_{z} \end{array}\right\}=\left\{\begin{array}{c} { }^{B} P_{x} \cos \left(x_{B}, x_{A}\right)+{ }^{B} P_{y} \cos \left(y_{B}, x_{A}\right)+{ }^{B} P_{z} \cos \left(z_{B}, x_{A}\right) \\ { }^{B} P_{x} \cos \left(x_{B}, y_{A}\right)+{ }^{B} P_{y} \cos \left(y_{B}, y_{A}\right)+{ }^{B} P_{z} \cos \left(z_{B}, y_{A}\right) \\ { }^{B} P_{x} \cos \left(x_{B}, z_{A}\right)+{ }^{B} P_{y} \cos \left(y_{B}, z_{A}\right)+{ }^{B} P_{z} \cos \left(z_{B}, z_{A}\right) \end{array}\right\}$

将其变为矩阵相乘的形式：

${ }^{1} P=\left\{\begin{array}{c} { }^{A} P_{x} \\ { }^{A} P_{y} \\ { }^{A} P_{z} \end{array}\right\}=\left[\begin{array}{lll} \cos \left(x_{B}, x_{A}\right) & \cos \left(y_{B}, x_{A}\right) & \cos \left(z_{B}, x_{A}\right) \\ \cos \left(x_{B}, y_{A}\right) & \cos \left(y_{B}, y_{A}\right) & \cos \left(z_{B}, y_{A}\right) \\ \cos \left(x_{B}, z_{A}\right) & \cos \left(y_{B}, z_{A}\right) & \cos \left(z_{B}, z_{A}\right) \end{array}\right]\left[\begin{array}{c} { }^{B} P_{x} \\ { }^{B} P_{y} \\ { }^{B} P_{z} \end{array}\right]$

等式右边左式即为两个坐标系之间的姿态矩阵 $^A_BR$ ，因此：

${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}$

因此姿态矩阵 $^A_BR$ 有两重含义：

（1）坐标系{B}相对于坐标系{A}的姿态矩阵

（2）坐标系{B}和{A}之间的坐标旋转矩阵

3.一般变换

已知 $^BP$ 、 $^AP_{B0}$ 、 $^A_BR$ ，求 $^AP$ ：

两个坐标系原点不同，方位也不同。我们可以用 $^AP_{B0}$ 来描述坐标系{B}的原点在坐标系{A}中的位置，用旋转矩阵 $^A_BR$ 描述{B}系相对于{A}系的姿态。

为了方便理解，我们可以建立中间坐标系{C}：

{C}与{B}的原点重合： ${ }^{A} P_{C_{0}}={ }^{A} P_{B_{0}}$
{C}与{A}的方位相同： $^C_BR=^A_BR$

{B}与{A}之间的变换可因此分为以下两步：

（1）从{B}到{C}：旋转 ${ }^{\mathrm{c}} \boldsymbol{P}={ }_{B}^{C} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}$

（2）从{C}到{1}：平移 ${ }^{A} \boldsymbol{P}={ }^{C} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{C_{0}}={ }^{C} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$

因此我们可以得到从{B}到{A}的一般变换公式： ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$

小结

平移变换： ${ }^{A} \boldsymbol{P}={ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$

旋转变换： ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}$

一般变换（平移加旋转）： ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$

三、齐次变换

坐标一般变换公式 ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$ 中既有加法，又有乘法，给运算带来了一定的不变，因此我们需要寻找一种方法对乘法和加法运算进行合并。

在几何学中，如果将n维向量用n+1维向量来表示，就得到了向量的齐次坐标，在三维坐标系中即为

$\boldsymbol{P}=\left[\begin{array}{c} P_{x} \\ P_{y} \\ P_{z} \end{array}\right] \rightarrow \boldsymbol{P}=\left[\begin{array}{c} P_{x} \\ P_{y} \\ P_{z} \\ 1 \end{array}\right]$

将 ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{R} \cdot{ }^{B} \boldsymbol{P}+{ }^{A} \boldsymbol{P}_{B_{0}}$ 变为齐次形式得到：

等式右边左项为4x4齐次变换矩阵，记为 ${ }_{B}^{A} \boldsymbol{T}$ ，即： ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{T} \cdot{ }^{B} \boldsymbol{P}$ 。

齐次变换矩阵具有三个物理意义：

（1）坐标变换：

可以表示同一个点在两个坐标系间的坐标变换矩阵

${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{T} \cdot{ }^{B} \boldsymbol{P}$

（2）坐标系的描述：

可以描述坐标系{B}相对于{A}的位置和姿态

其中 $^A_BR$ 为姿态矩阵， $^AP_{B0}$ 为位置向量。

（3）运动算子：

可以作为运动算子来表示刚体的运动情况

首先建立固定坐标系{A}，并假设在初始时刻运动坐标系{B}与{A}完全重合，点P在B坐标系中的位置 $^BP$ 可以看做起始位置， ${ }_{B}^{A} \boldsymbol{T}$ 可以看做运动算子， ${ }_{B}^{A} \boldsymbol{T} \cdot{ }^{B} \boldsymbol{P}$ 可以看做让点P发生了某种运动，从而到达终点位置 $^AP$ 。对齐次变换矩阵进行分析可以看出该运动可以分为两部分来理解，其中包含坐标平移 $Trans(^AP_{B0})$ 和坐标旋转 $Rot(k,\theta)$ ：

小结

齐次变换矩阵： ${ }_{B}^{A} \boldsymbol{T}$

齐次变换： ${ }^{A} \boldsymbol{P}={ }_{B}^{A} \boldsymbol{T} \cdot{ }^{B} \boldsymbol{P}$

齐次变换的三个物理意义：

坐标变换
坐标系描述
运动算子

四、齐次变换矩阵的运算

1.齐次变换矩阵相乘

假设有三个坐标系{A}、{B}、{C}，{A}、{B}间的齐次变换矩阵为 $^A_BT$ ， {B}、{C}间的齐次变换矩阵为 $^B_CT$ :

任给一点P，则P在三个坐标系中的描述 $^AP\;^BP\; ^CP$ 都不同。我们可以知道这三者间的坐标变换公式为：

$\begin{aligned} &{ }^{B} P={ }_{C}^{B} \boldsymbol{T}^{C} P\quad\quad (1)\\ &{ }^{A} P={ }_{B}^{A} \boldsymbol{T}{ }^{B} P\quad\quad (2)\end{aligned}$

将（1）式代入（2）式，得到：

$^AP={ }_{B}^{A} \boldsymbol{T}_{C}^{B} \boldsymbol{T}^{C} P=_{C}^{A} \boldsymbol{T}^{C} P$

由于 $^A_CT$ 为{C}在{A}中的描述， $^B_CT$ 为{C}在{B}中的描述，因此 $^A_BT$ 可以看做是坐标系描述 $^B_CT$ 到坐标系描述 $^A_CT$ 的坐标系变换。利用齐次变换矩阵相乘，可以实现坐标系描述之间的变换。

2.齐次变换矩阵相乘的物理意义

${ }_{B}^{A} \boldsymbol{T}_{C}^{B} \boldsymbol{T}=_{C}^{A} \boldsymbol{T}$

（1）相对固定坐标系

最初，A、B、C系完全重合，

第一步：C相对于A(B)系做运动 $^B_CT$

第二步：BC系再一起相对A系做运动 $^A_BT$

因此，齐次变换矩阵相乘可以看做相对固定坐标系先后发生的若干运动的合成。可见，当前后两个运动都是相对于固定坐标系发生的，那么后发生的运动要乘在左边，即从右往左乘。

例1：绕固定坐标系连续运动

{B}与{A}重合，绕 $X_A$ 转动90°，再绕 $Y_A$ 转动90°，再绕 $Z_A$ 转动90°，求 $^A_BT$ 。

（2）相对运动坐标系

最初，A、B、C系完全重合，

第一步：BC系一起相对A系做运动 $^A_BT$

第二步：C相对于B系做运动 $^B_CT$

因此，齐次变换矩阵相乘可以看做相对运动坐标系先后发生的若干运动的合成。可见，当前后两个运动都是相对于运动坐标系发生的，那么后发生的运动要乘在左边，即从左向右乘。

例2：绕运动坐标系连续运动

{B}与{A}重合，绕 $Z_B$ 转动90°，再绕 $Y_B$ 转动90°，再绕 $X_B$ 转动90°，求 $^A_BT$ 。

3.齐次变换矩阵的逆变换

已知 {B}相对 {A} 的描述 ${ }_{B}^{A} T$ ，为了求解 {A}相对{B} 的描述 ${ }_{A}^{B} T$ ，最直接的方法是通过矩阵求逆 ${ }^{}{ }_{A}^{B} T={ }_{B}^{A} T^{-1}$ ，但这过于繁琐。根据齐次变换矩阵的定义， ${ }_{B}^{A} T$ 由 $^A_BR$ 和 $^AP_{B0}$ 组成，因此可以将问题转换为已知 $^A_BR$ 和 $^AP_{P0}$ ，求解 $^B_AR$ 和 $^BP_{A0}$ 的问题。

根据旋转矩阵的正交性，可以得到： ${ }_{A}^{B} R={ }_{B}^{A} \boldsymbol{R}^{-1}={ }_{B}^{A} \boldsymbol{R}^{\mathrm{T}}$

又由于 ${ }^{B} \boldsymbol{P}_{B_{0}}={ }_{A}^{B} \boldsymbol{R}^{A} \boldsymbol{P}_{B_{0}}+{ }^{B} \boldsymbol{P}_{A_{0}}=\boldsymbol{0}$

可得： ${ }^{B} \boldsymbol{P}_{A_{0}}=-{ }_{A}^{B} \boldsymbol{R}^{A} \boldsymbol{P}_{B_{0}}=-{ }_{B}^{A} \boldsymbol{R}^{\mathrm{T} A} \boldsymbol{P}_{B_{0}}$

可直接构造出：

小结

齐次变换矩阵相乘： ${ }_{B}^{A} \boldsymbol{T}_{C}^{B} \boldsymbol{T}=_{C}^{A} \boldsymbol{T}$

物理意义：

坐标系变换
相对固定坐标系的合成运动：从右往左乘
相对运动坐标系的合成运动：从左往右乘

齐次变换矩阵求逆：

求 $^B_AR$ ： ${ }_{A}^{B} R={ }_{B}^{A} R^{\mathrm{T}}$
求 $^BP_{A0}$ ： ${ }^{B} \boldsymbol{P}_{A_{0}}=-{ }_{B}^{A} \boldsymbol{R}^{\mathrm{T} A} \boldsymbol{P}_{B_{0}}$

五、固定角与欧拉角

因为姿态矩阵的九个元素中只有三个是独立的，那么我们就可以用绕三个坐标轴的三个转角作为三个独立的参数来表示刚体的姿态。常用的姿态角表示方法有两种：RPY角和欧拉角。

1.RPY角

RPY角普遍应用于航空、航海及机器人等领域。在航空领域，飞行过程表示为三种姿态调整：

取这三种姿态调整的英文首字母便组成RPY。

RPY角同样可以表示机器人手爪的三个转动：

我们可以将RPY角看做绕固定坐标系X轴、Y轴、Z轴连续发生的三个旋转。由此可以衍生出正反两个问题：

正问题：已知RPY角，求姿态矩阵

因为RPY角为绕固定坐标轴X-Y-Z依次旋转γ、β、α，所以旋转矩阵可以通过三种基本旋转矩阵从右到左依次相乘来表示，即：

$\begin{aligned} &{ }_{B}^{A} R_{x z}(\gamma, \beta, \alpha) \\ &=R\left(z_{A}, \alpha\right) R\left(y_{A}, \beta\right) R\left(x_{A}, \gamma\right) \\ &=\left[\begin{array}{ccc} c \alpha & -s \alpha & 0 \\ s \alpha & c \alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{ccc} c \beta & 0 & s \beta \\ 0 & 1 & 0 \\ -s \beta & 0 & c \beta \end{array}\right] \cdot\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & c \gamma & -s \gamma \\ 0 & s \gamma & c \gamma \end{array}\right] \\ &=\left[\begin{array}{cccc} c \alpha \cdot c \beta & c \alpha \cdot s \beta \cdot s \gamma-s \alpha \cdot c \gamma & c \alpha \cdot s \beta \cdot c \gamma+s \alpha \cdot s \gamma \\ s \alpha \cdot c \beta & s \alpha \cdot s \beta \cdot s \gamma+c \alpha \cdot c \gamma & s \alpha \cdot s \beta \cdot c \gamma-c \alpha \cdot s \gamma \\ -s \beta & c \beta \cdot s \gamma & c \beta \cdot c \gamma \end{array}\right] \end{aligned}$

逆问题：已知旋转后的姿态矩阵，求解RPY角

$\operatorname{RPY}(\gamma, \beta, \alpha)=\left[\begin{array}{lll} n_{x} & o_{x} & a_{x} \\ n_{y} & o_{y} & a_{y} \\ n_{z} & o_{z} & a_{z} \end{array}\right]=\left[\begin{array}{ccc} c \alpha \cdot c \beta & c \alpha \cdot s \beta \cdot s \gamma-s \alpha \cdot c \gamma & c \alpha \cdot s \beta \cdot c \gamma+s \alpha \cdot s \gamma \\ s \alpha \cdot c \beta & s \alpha \cdot s \beta \cdot s \gamma+c \alpha \cdot c \gamma & s \alpha \cdot s \beta \cdot c \gamma-c \alpha \cdot s \gamma \\ -s \beta & c \beta \cdot s \gamma & c \beta \cdot c \gamma \end{array}\right]$

通过各项之间的关系可以求出：

2.欧拉角

欧拉角是一种与RPY角类似的姿态描述方法，也是通过绕坐标轴的三次转动的转角大小来表示刚体的姿态。但与RPY角不同的是，欧拉角是利用绕运动坐标系坐标轴的三次转动的转角大小来表示刚体的姿态。

根据三次绕坐标轴转动的顺序不同，欧拉角共可分为12种，可分为两大类：

非对称型：ZYX、ZXY、YZX、YXZ、ZYZ、XZY

对称型：ZYZ、ZXZ、YXY、YZY、XZX、XYX

以ZYX欧拉角为例，ZYX欧拉角是绕运动坐标系z-y-z轴依次旋转α、β、γ，因此旋转矩阵可以用三个基本旋转矩阵从左到右相乘来表示：

$\begin{aligned} { }_{B}^{A} R_{s \gamma x} &(\alpha, \beta, \gamma)=R\left(z_{s}, \alpha\right) R\left(y_{B}, \beta\right) R\left(x_{s}, \gamma\right) \\ &=\left[\begin{array}{ccc} c \alpha & -s \alpha & 0 \\ s \alpha & c \alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{ccc} c \beta & 0 & s \beta \\ 0 & 1 & 0 \\ -s \beta & 0 & c \beta \end{array}\right] \cdot\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & c \gamma & -s \gamma \\ 0 & s \gamma & c \gamma \end{array}\right] \\ &=\left[\begin{array}{ccc} c \alpha \cdot c \beta & c \alpha \cdot s \beta \cdot s \gamma-s \alpha \cdot c \gamma & c \alpha \cdot s \beta \cdot c \gamma+s \alpha \cdot s \gamma \\ s \alpha \cdot c \beta & s \alpha \cdot s \beta \cdot s \gamma+c \alpha \cdot c \gamma & s \alpha \cdot s \beta \cdot c \gamma-c \alpha \cdot s \gamma \\ -s \beta & c \beta \cdot s \gamma & c \beta \cdot c \gamma \end{array}\right] \end{aligned}$

通过对比发现，ZYX欧拉角绕运动坐标系z-y-z轴依次旋转α、β、γ的旋转矩阵与RPY角绕固定坐标轴X-Y-Z依次旋转γ、β、α的旋转矩阵完全相同，可以从下面的例子中对比：

ZYZ欧拉角是绕运动坐标系z-y-z轴依次旋转α、β、γ，因此旋转矩阵可以用三个基本旋转矩阵从左到右相乘来表示：

$\begin{aligned} { }_{B}^{A} \boldsymbol{R}_{x y z} &(\alpha, \beta, \gamma)=\boldsymbol{R}\left(z_{B}, \alpha\right) \boldsymbol{R}\left(y_{B}, \beta\right) \boldsymbol{R}\left(z_{B}, \gamma\right) \\ &=\left[\begin{array}{ccc} c \alpha & -s \alpha & 0 \\ s \alpha & c \alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{ccc} c \beta & 0 & s \beta \\ 0 & 1 & 0 \\ -s \beta & 0 & c \beta \end{array}\right] \cdot\left[\begin{array}{ccc} c \gamma & -s \gamma & 0 \\ s \gamma & c \gamma & 0 \\ 0 & 0 & 1 \end{array}\right] \\ &=\left[\begin{array}{ccc} c \alpha \cdot c \beta \cdot c \gamma-s \alpha \cdot s \gamma & -c \alpha \cdot c \beta \cdot s \gamma-s \alpha \cdot c \gamma & c \alpha \cdot s \beta \\ s \alpha \cdot c \beta \cdot c \gamma+c \alpha \cdot s \gamma & -s \alpha \cdot c \beta \cdot s \gamma+c \alpha \cdot c \gamma & s \alpha \cdot s \beta \\ -s \beta \cdot c \gamma & s \beta \cdot s \gamma & c \beta \end{array}\right] \end{aligned}$

逆问题：已知刚体的姿态矩阵，求解ZYZ欧拉角的三个转角

小结

RPY角：用相对固定坐标系的三个转角来描述刚体姿态

欧拉角：用相对运动坐标系的三个转角来描述刚体姿态

正问题：已知三个转角，求解姿态矩阵

逆问题：已知姿态矩阵，求解三个转角

六、旋转变换通式

在之前的旋转表示方法中，只是绕X、Y、Z轴进行旋转，下面介绍绕经过原点的任意旋转轴旋转的选转变换通式。

假设刚体绕坐标轴发生一系列连续转动，我们可以利用矩阵的乘法将这一系列转动的旋转矩阵进行合成，得到刚体的最终姿态，而绕坐标轴的这一系列转动等效于绕经过坐标原点的旋转轴k进行的一次角度为θ的转动，即：

${ }_{B}^{A} R=R(k, \theta)$

其中同样包含两个问题：

（1）正问题：已知k、θ，求 $^A_BR$

假设运动坐标系{B}与固定坐标系{A}初始位置重合，{B}相对于{A}绕k轴旋转θ角 ${ }_{B}^{A} R=R(k, \theta)$ 。因为绕某个坐标轴发证旋转运动的旋转矩阵很容易写出，因此我们需要将问题转化为绕坐标轴的一系列旋转。

我们建立两个相互重合的辅助坐标系{A'}、{B'}，并使两者的z轴与k轴重合，

假设{A'}与{A}的关系为 $^A_{A'}R$ ，{B'}与{B}的关系为 $^B_{B'}R$ ，可以得到如下关系：

可以看出坐标系{B}相对于{A}的一次转动可以分为三个步骤进行：

${ }_{B}^{A} R=R(\boldsymbol{k}, \theta)={ }_{A^{\prime}}^{A} R_{B^{\prime}}^{A^{\prime}} R_{B}^{B^{\prime}} R$

（1）：{A}到{A'}： $^A_{A'}R$

（2）：{A'}到{B'}: $^{A'}_{B'}R$

（3）：{B'}到{B}: $^{B'}_{B}R$

其中：

${ }_{A'}^{A} R={ }_{B'^{}}^{B} R=\left[\begin{array}{ccc} n_{x} & o_{x} & k_{x} \\ n_{y} & o_{y} & k_{y} \\ n_{z} & o_{z} & k_{z} \end{array}\right]$

这两个旋转矩阵的第三列均为旋转轴线k。

通过对比可以看出，坐标系{B}绕k轴旋转可以等价为坐标系{B'}绕Z轴旋转，所以：

$^{A'}_{B'}R=\left[\begin{array}{ccc} c \theta & -s \theta & 0 \\ s \theta & c \theta & 0 \\ 0 & 0 & 1 \end{array}\right]$

因此可以得到：

$^A_BR=\left[\begin{array}{lll} n_{x} & o_{x} & k_{x} \\ n_{y} & o_{y} & k_{y} \\ n_{z} & o_{z} & k_{z} \end{array}\right] \cdot\left[\begin{array}{ccc} \mathrm{c} \theta & -s \theta & 0 \\ s \theta & \mathrm{c} \theta & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{lll} n_{x} & n_{y} & n_{z} \\ o_{x} & o_{y} & o_{z} \\ k_{x} & k_{y} & k_{z} \end{array}\right]$

即：

$R(\mathrm{k}, \quad \theta)=\left[\begin{array}{ccc} k_{x} k_{x} \operatorname{Vers} \theta+c \theta & k_{y} k_{x} \operatorname{Vers} \theta-k_{z} s \theta & k_{z} k_{x} \operatorname{Vers} \theta+k_{y} s \theta \\ k_{x} k_{y} \operatorname{Vers} \theta+k_{z} s \theta & k_{\mathrm{y}} k_{y} \operatorname{Vers} \theta+c \theta & k_{z} k_{y} \operatorname{Vers} \theta-k_{x} s \theta \\ k_{x} k_{z} \operatorname{Vers} \theta-k_{y} s \theta & k_{y} k_{z} \operatorname{Vers} \theta+k_{x} s \theta & k_{z} k_{z} \operatorname{Vers} \theta+c \theta \end{array}\right]$

其中Versθ=1-cosθ。该式中仅包含旋转轴k以及旋转角度θ

如果令旋转轴k分别为X、Y、Z轴，可以分别得到绕X、Y、Z轴的旋转矩阵：

可见之前我们得到的绕某个坐标轴的旋转矩阵是该旋转变换通式的特殊形式。

（2）逆问题：已知 $^A_BR$ ，求k、θ

设：

${ }_{B}^{A} R=\left[\begin{array}{lll} n_{x} & o_{x} & a_{x} \\ n_{y} & o_{y} & a_{y} \\ n_{z} & o_{z} & a_{z} \end{array}\right]$

利用旋转矩阵9个元素之间的关系，可以求解得到 $k_x\;k_y\;k_z$ 以及θ。

$\left[\begin{array}{lll} n_{x} & o_{x} & a_{x} \\ n_{y} & o_{y} & a_{y} \\ n_{z} & o_{z} & a_{z} \end{array}\right]=\left[\begin{array}{ccc} k_{x} k_{x} \operatorname{Vers} \theta+c \theta & k_{y} k_{x} \operatorname{Vers} \theta-k_{z} s \theta & k_{z} k_{x} \operatorname{Vers} \theta+k_{y} s \theta \\ k_{x} k_{y} \operatorname{Vers} \theta+k_{z} s \theta & k_{y} k_{y} \operatorname{Vers} \theta+c \theta & k_{z} k_{y} \operatorname{Vers} \theta-k_{x} s \theta \\ k_{x} k_{z} \operatorname{Vers} \theta-k_{y} s \theta & k_{y} k_{z} \operatorname{Vers} \theta+k_{x} s \theta & k_{z} k_{z} \operatorname{Vers} \theta+c \theta \end{array}\right]$