导航中的姿态变换

最新推荐文章于 2024-03-01 13:09:02 发布

刘乐去哪儿了

最新推荐文章于 2024-03-01 13:09:02 发布

阅读量1.9k

点赞数 3

分类专栏：激光SLAM算法文章标签：算法

本文链接：https://blog.csdn.net/qq_37423490/article/details/132655448

版权

激光SLAM算法专栏收录该内容

6 篇文章 1 订阅

订阅专栏

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言
一、姿态角与欧拉角
- 1. 姿态角
- 2. 欧拉角
二、旋转矩阵
- 1. 什么是旋转矩阵
- 2. 旋转矩阵（坐标系的旋转矩阵）为什么是`右乘`

前言

本文主要介绍导航定位或SLAM中关于坐标旋转的一些基础知识，包括什么是姿态角，什么是欧拉角、如何由姿态角得到旋转矩阵、所得到的旋转矩阵有什么几何意义、旋转矩阵的左乘和右乘分别代表什么等

一、姿态角与欧拉角

1. 姿态角

在导航解算中，通常有三种姿态表示方式，欧拉角、方向余弦矩阵、四元数，一般选取较为常用的方向余弦矩阵来描述姿态。对于姿态角，是根据导航系（当地水平坐标系 n系）与载体系(b系)之间的转动关系进行定义的，而且从n系转到b系，首先假想载体完全水平且前向指北（假想载体系与导航系完全重合），那么经过旋转，该假想载体可以完全与真实载体重合，在这个过程中三轴旋转角度即为姿态角，姿态角就是一种特殊的欧拉角。
其中，我们假定载体的xyz三轴指向为前右下，则：
绕x轴的旋转角为翻滚角(roll)
绕y轴的旋转角为俯仰角(pitch)
绕z轴的旋转角为航向角(yaw)
注意：roll和pitch的正负都符合右手定律，顺时针为负，逆时针为正；yaw角为北偏东为正，范围为(0～360度)，因此当z轴指下时，符合右手定律，当z轴指上时（例如：右前上），其旋转方向不符合右手定律，因此这时一般会将yaw角范围转换到(-180～180)来使用。

2. 欧拉角

上文说到姿态角就是一组特殊的欧拉角，用来表示导航系与载体系之间的旋转关系，也可以说是载体系在导航系中的姿态。空间内的任何一个旋转运动，都可以分解为绕xyz三个轴的三次旋转，这三个角就是欧拉角，但是欧拉角的定义不是唯一的，根据旋转方式（内旋、外旋）和旋转顺序的不同存在着多种定义，因此我们在讨论欧拉角时要确定其旋转方式和旋转顺序。
在导航领域中讨论的欧拉角默认旋转方式为内旋，旋转顺序为*yaw-pitch-roll*
在这里插入图片描述

二、旋转矩阵

我们上文谈到了用姿态角和欧拉角来表示b系在n系中的姿态，这种表示方式是直观的，但是不能直接用于计算，因此我们一般根据姿态角计算旋转矩阵(姿态矩阵)来表示姿态或旋转（注意，我们这里提到的旋转是坐标系的旋转，不是坐标的旋转）。

1. 什么是旋转矩阵

假设空间内有一点P，在导航系中坐标为P_n，在载体系中坐标为P_b，则：
$\begin{bmatrix}e_n^1 &e_n^2&e_n^3\end{bmatrix}P_n = \begin{bmatrix}e_b^1 &e_b^2&e_b^3\end{bmatrix}P_b \quad\quad\quad（1）\\ \quad \\ 其中\begin{bmatrix}e_n^1 &e_n^2&e_n^3\end{bmatrix}表示导航系的正交基组成的矩阵\\ \quad \\ \begin{bmatrix}e_b^1 &e_b^2&e_b^3\end{bmatrix}表示载体系的正交基组成的矩阵$
$P_n =\begin{bmatrix}e_n^1 &e_n^2&e_n^3\end{bmatrix}^T \begin{bmatrix}e_b^1 &e_b^2&e_b^3\end{bmatrix}P_b \quad\quad\quad（2）\\ \quad \\ P_n=\begin{bmatrix}{e_n^1}^T \\ {e_n^2}^T \\ {e_n^3}^T\end{bmatrix} \begin{bmatrix}e_b^1 &e_b^2&e_b^3\end{bmatrix}P_b \quad\quad (3)\quad\\ \quad \\ P_n=\begin{bmatrix} {e_n^1}^T e_b^1 &{e_n^1}^T e_b^2 & {e_n^1}^T e_b^3 \\{e_n^2}^T e_b^1 &{e_n^2}^T e_b^2 & {e_n^2}^T e_b^3 \\{e_n^3}^T e_b^1 &{e_n^3}^T e_b^2 & {e_n^3}^T e_b^3 \end{bmatrix}P_b \quad\quad（4）$
式(4)等号右侧的矩阵即为旋转矩阵或姿态矩阵，由向量内积的几何意义可知，该矩阵第一列表示载体系x轴在导航系三轴上的投影，第二列表示载体系y轴在导航系三轴上的的投影，第三列同理。因此可以说导航坐标系到载体坐标系的旋转矩阵就是载体系的基底在导航系的投影。上式也可以这么理解，向量P在导航系下的坐标为P~n~，在载体系下的坐标为P~b~.

下面我们来讨论下坐标旋转和坐标系旋转的区别，设R_nb表示从导航坐标系到载体坐标系的旋转矩阵(载体系在导航系中的姿态矩阵)，也是载体系坐标到导航系坐标的旋转矩阵（注意：表示坐标系旋转矩阵是n->b，表示坐标旋转矩阵是b->n）；另R_nb的逆矩阵或说转置矩阵(该矩阵由三个正交基组成，因此是正交矩阵)为载体坐标系到导航坐标系的旋转矩阵，也是导航系坐标到载体系坐标的旋转矩阵。

2. 旋转矩阵（坐标系的旋转矩阵）为什么是`右乘`

根据上文的描述，我们用姿态角来计算旋转矩阵：
$\begin{bmatrix}1&0&0\\cos\alpha &-sin\alpha &0 \\sin\alpha&cos\alpha&1\end{bmatrix}\quad \quad (5)\\ \quad \\ R(y) = \begin{bmatrix}cos\alpha&0&sin\alpha\\ 0&1&0 \\-sin\alpha&0&cos\alpha\end{bmatrix}\quad \quad (6)\\ \quad \\ R(z) = \begin{bmatrix}cos\alpha &-sin\alpha &0 \\sin\alpha&cos\alpha&0\\0&0&1\end{bmatrix}\quad \quad (7)\\$
这里根据前右下的定义，旋转顺序为yaw-pitch-roll(321)
$R=R(z)R(y)R(x)\quad \quad (8)$
矩阵R表示的是从导航坐标系到载体坐标系的旋转矩阵，可以看出这里的顺序是右乘，即后转的矩阵乘在右边，与我们平时用的左乘（左乘用于坐标旋转）是不同的。
假设空间内有一点P，在导航系中坐标为P_n，在载体系中坐标为P_b，则：
$P_n = R*P_b=R(z)*R(y)*R(x)*P_b\quad \quad (9)$
现在我们来一步一步的分解，

首先，R(z) 是导航系绕其z 轴旋转了yaw 角，到了C₁ 坐标系
然后，R(y) 是C₁ 坐标系绕其y 轴旋转了pitch 角，到了C₂ 坐标系
最后，R(x) 是C₂ 坐标系绕其x 轴旋转了roll 角，到了载体系
导航系----->C₁ 坐标系----->C₂ 坐标系----->载体系

现在将公式(9)从右往左看，P_b 是点P在载体系下的坐标，那么根据1中所介绍过的坐标系旋转矩阵的意义可知，P_b 左乘R(x) 表示P 点在C₂ 坐标系中的坐标，即
$P_{c2} = R(x)*P_b\quad \quad (10)$
同理，P_c2 左乘R(y)表示点P在C₁坐标系下的坐标P~c1 ，P~c1 左乘R(z)表示点P在导航坐标系下的坐标P_n，即
$P_{c1} = R(y)*P_{c2} \quad \quad (11)\\ P_n = R(z)*P_{c1} \quad \quad (12)$

注意：左乘右乘的关键点在于谁到谁的什么旋转，例如：
A系到B系的坐标系旋转矩阵为R1，则R1也是B系到A系的坐标旋转矩阵；
B系到C系的坐标系旋转矩阵R2，则R2也是C系到B系的坐标旋转矩阵；

–	坐标系旋转矩阵	坐标旋转矩阵
A->B	R1	R1^T
B->A	R1^T	R1
B->C	R2	R2^T
C->B	R2^T	R2

$P_a = R_1*R_2*P_c \quad \quad (13)\\ P_c = R_2^T*R_1^T*P_a\quad \quad (14)$
我们利用公式(13)来解释左乘右乘，
如果R₂、R₁分别表示C系到B系的坐标旋转矩阵、B系到A系的坐标旋转矩阵，那式(13)明显是左乘，P_c到P_a ，先转R₂再转R₁;
如果R₂、R₁分别表示B系到C系的坐标系旋转矩阵、A系到B系的坐标系旋转矩阵，那式(13)明显是右乘，A系到C系，先转R₂再转R₁;