自动驾驶定位系统-基础知识之车辆姿态表达

辆位置和姿态是自动驾驶中的一个基础问题，只有解决了车辆的位置和姿态，才能将自动驾驶的各个模块关联起来。车辆的位置和姿态一般由自动驾驶的定位模块输出。

以Apollo为例，对车辆的Pose的定义如下:

message Pose {
  // Position of the vehicle reference point (VRP) in the map reference frame.
  // The VRP is the center of rear axle.
  optional apollo.common.PointENU position = 1;

  // A quaternion that represents the rotation from the IMU coordinate
  // (Right/Forward/Up) to the
  // world coordinate (East/North/Up).
  optional apollo.common.Quaternion orientation = 2;

  // Linear velocity of the VRP in the map reference frame.
  // East/north/up in meters per second.
  optional apollo.common.Point3D linear_velocity = 3;

  // Linear acceleration of the VRP in the map reference frame.
  // East/north/up in meters per second.
  optional apollo.common.Point3D linear_acceleration = 4;

  // Angular velocity of the vehicle in the map reference frame.
  // Around east/north/up axes in radians per second.
  optional apollo.common.Point3D angular_velocity = 5;

  // Heading
  // The heading is zero when the car is facing East and positive when facing
  // North.
  optional double heading = 6;

  // Linear acceleration of the VRP in the vehicle reference frame.
  // Right/forward/up in meters per square second.
  optional apollo.common.Point3D linear_acceleration_vrf = 7;

  // Angular velocity of the VRP in the vehicle reference frame.
  // Around right/forward/up axes in radians per second.
  optional apollo.common.Point3D angular_velocity_vrf = 8;

  // Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
  // in world coordinate (East/North/Up)
  // The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
  // The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
  // The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
  // The direction of rotation follows the right-hand rule.
  optional apollo.common.Point3D euler_angles = 9;
}

1.车辆的位置

车辆的位置(VRP， Vehicle Reference Point)一般选取一个车辆的基准点在世界坐标系的位置作为车辆位置。

Apollo中的世界坐标系采用WGS-84坐标系（the World Geodetic System dating from 1984），如下图所示。

Apollo的Pose的局部坐标系是ENU(East-North-Up)坐标系。

2. 车辆的姿态角

2.1 欧拉角

在右手笛卡尔坐标系中沿X轴、Y轴和Z轴的旋转角分别叫Roll,Pitch和Yaw。

Pitch是围绕X轴旋转的角度，也叫做俯仰角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。旋转矩阵如下:

$$ R_x(\theta) = \left [ \begin{matrix} 1 & 0 & 0 \\ 0 & cos \theta & -sin \theta \\ 0 & sin \theta & cos \theta \end{matrix} \right] $$

Yaw是围绕Y轴旋转的角度，也叫偏航角。即机头右偏航为正，反之为负。旋转矩阵如下:

$$ R_y(\theta) = \left [ \begin{matrix} cos \theta & 0 & sin \theta \\ 0 & 1 & 0 \\ -sin \theta & 0 & cos \theta \end{matrix} \right] $$

Roll是围绕Z轴旋转，也叫翻滚角。机体向右滚为正，反之为负。旋转矩阵如下:

$$ R_z(\theta) = \left [ \begin{matrix} cos \theta & -sin \theta & 0 \\ sin \theta & cos \theta & 0 \\ 0 & 0 & 1 \end{matrix} \right] $$

仅仅有旋转角度(Pitch, Raw, Roll)是不够的，还依赖于旋转的顺序和旋转的参考坐标系，不同的旋转顺序和不同的旋转参考坐标系都会导致不同的旋转结果。

首先是旋转顺序，旋转顺序分为两类:
Proper Euler angles：旋转顺序为z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y，第一个旋转轴和最后一个旋转轴想同。
Tait–Bryan angles：旋转顺序为x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z。

其次是旋转的参照坐标系，欧拉角按旋转的坐标系分为：
内旋（intrinsic rotation）即按照物体本身的坐标系进行旋转，坐标系会跟随旋转与世界坐标系产生偏移。
外旋（extrinsic rotation）即根据世界坐标系进行旋转。

欧拉角的缺点:

欧拉角的一个重大缺点是会碰到著名的万向锁(Gimbal Lock)问题：在俯仰角为$\pm 90^{0}$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性的问题。由于这种原因，欧拉角不适于插值和迭代，往往只用在人机交互中。我们也很少在SLAM程序中直接使用欧拉角表示姿态，同样不会在滤波或优化中使用欧拉角表示旋转（因为它具有奇异性）。

2.2 四元数

四元数是三维空间旋转的另一种表达形式。相对于旋转矩阵和欧拉角，四元数的优势如下:

1、四元数避免了欧拉角表示法中的万向锁问题；

2、相对于三维旋转矩阵的9个分量，四元数更紧凑，用4个分量就可以表达所有姿态。

四元数的定义如下:

$$\mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k$$

其中i,j,k为四元数的三个虚部。这三个虚部满足关系式：

$$\begin{equation} \label{eq:quaternionVirtual} \left\{ \begin{array}{l} {i^2} = {j^2} = {k^2} = - 1\\ ij = k,ji = - k\\ jk = i,kj = - i\\ ki = j,ik = - j \end{array} \right. \end{equation}$$

用四元数来表示旋转要解决两个问题，一是如何用四元数表示三维空间里的点，二是如何用四元数表示三维空间的旋转。

四元数表示空间中的点

假设三维空间里的点坐标为 (x,y,z)，则它的四元数形式为:$xi + yj + zk$，这是一个纯四元数(实部为0的四元数)。

单位四元数表示一个三维空间旋转

设 q 为一个单位四元数，而 p 是一个纯四元数，则${R_{q}(p)=qpq^{-1}}$也是一个纯四元数，可以证明$R_q$表示一个旋转，将点p旋转到空间的另一个点$R_q(p)$。

旋转角度与四元数的转化

四元数将绕坐标轴的旋转转化为绕向量的旋转，假设某个旋转是绕单位向量$n=[n_x,n_y,n_z]^T$进行了角度为$\theta$的旋转，那么这个旋转的四元数形式为：

$$\begin{equation} \label{eq:ntheta2quaternion} \mathbf{q} = \left[ \cos \frac{\theta}{2}, n_x \sin \frac{\theta}{2}, n_y \sin \frac{\theta}{2}, n_z \sin \frac{\theta}{2}\right]^T \end{equation}$$

四元数与旋转角度/旋转轴的转化

$$ \begin{equation} \begin{cases} \theta = 2\arccos {q_0}\\ {\left[ {{n_x},{n_y},{n_z}} \right]^T} = {{{\left[ {{q_1},{q_2},{q_3}} \right]}^T}}/{\sin \frac{\theta }{2}} \end{cases} \end{equation} $$

C++中使用Eigen定义四元数的代码如下，该代码定义了一个绕z轴30度的旋转操作。

#include <Eigen/Geometry>
#include <Eigen/Core>
#include <cmath>
#include <iostream>

int main() {
  Eigen::Quaterniond q(cos(M_PI / 6.0), 0 * sin(M_PI / 6.0), 0 * sin(M_PI / 6.0), 1 * sin(M_PI / 6.0));
  // 输出所有系数
  std::cout << q.coeffs() << std::endl;
  // 输出虚部系数
  std::cout << q.vec() << std::endl;

  Eigen::Vector3d v(1, 0, 0);

  Eigen::Vector3d v_rotated = q * v;

  std::cout << "(1,0,0) after rotation = " << v_rotated.transpose() << std::endl;

  return 0;
}

代码的输出如下:

0
0
0.5
0.866025
// q.coeffs()先输出四元数的实部，再输出四元数的虚部
  0
  0
0.5
// q.vec()输出四元数的虚部

(1,0,0) after rotation = (0.5 0.866025 0)

四元数到旋转矩阵的转换

设四元数$q=q_0 + q_1i + q_2j + q_3k$，则对应的旋转矩阵为:

$$ R= \left [ \begin{matrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3 + 2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3 + 2q_0q_1 & 1-2q_1^2-2q_2^2 \end{matrix} \right ] $$

旋转矩阵到四元数的转换

假设矩阵为$R={m_{ij}},i,j \in [1,2,3]$，其对应的四元数q为:

$$q_0=\frac{\sqrt{tr(R)+1}}{2}$$
$$q_1=\frac{m_{23}-m_{32}}{4q_0}$$
$$q_2=\frac{m_{31}-m_{13}}{4q_0}$$
$$q_3=\frac{m_{12}-m_{21}}{4q_0}$$

2.3 四元数、欧拉角、旋转矩阵、旋转向量转换

2.3.1 旋转向量到旋转矩阵、欧拉角、四元数的转换

定义旋转向量

// 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha, Eigen::Vector3d(x,y,z))

旋转向量到旋转矩阵:

//旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix = rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();

旋转矩阵到欧拉角:

// 旋转向量转欧拉角(Z-Y-X)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

旋转矩阵到四元数:

// 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion = rotation_vector;

2.3.2 旋转矩阵到旋转向量、欧拉角、四元数的转换

旋转矩阵:

Eigen::Matrix3d rotation_matrix;
rotation_matrix << x_00, x_01, x_02, x_10, x_11, x_12, x_20, x_21, x_22;

旋转矩阵转旋转向量:

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector = rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

旋转矩阵转欧拉角

Eigen::Vector3d eulerAngle = rotation_matrix.eulerAngles(2,1,0);

旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion = rotation_matrix;

2.3.3 欧拉角到旋转向量、旋转矩阵、四元数的转换

初始化欧拉角:

Eigen::Vector3d eulerAngle(yaw, pitch, roll);

欧拉角转旋转向量：

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2), Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1), Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0), Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vector = yawAngle * pitchAngle * rollAngle;

欧拉角转旋转矩阵：

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2), Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1), Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0), Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix = yawAngle * pitchAngle * rollAngle;

欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2), Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1), Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0), Vector3d::UnitZ()));
Eigen::Quaterniond quaternion = yawAngle * pitchAngle * rollAngle;

2.3.4 四元数到旋转向量、旋转矩阵、四元数的转换

四元数

Eigen::Quaterniond quaternion(w,x,y,z);

四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector = quaternion;

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix = quaternion.matrix();

Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

四元数转欧拉角

Eigen::Vector3d eulerAngle = quaternion.matrix().eulerAngles(2,1,0);

参考链接

1、四元数: https://www.cnblogs.com/gaoxiang12/p/5120175.html
2、维基百科：四元数与空间旋转
https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B0%E4%B8%8E%E7%A9%BA%E9%97%B4%E6%97%8B%E8%BD%AC
3、eigen 中四元数、欧拉角、旋转矩阵、旋转向量 https://www.cnblogs.com/lovebay/p/11215028.html
4、视觉SLAM十四讲:从理论到实践
5、百度Apollo项目: https://github.com/ApolloAuto/apollo

相关文章

自动驾驶定位系统-Error State Extend Kalman Filter

自动驾驶定位系统-扩展卡尔曼滤波Extend Kalman Filter

自动驾驶定位系统-卡尔曼滤波Kalman Filter

自动驾驶系统定位与状态估计- Recursive Least Squares Estimation

自动驾驶系统定位与状态估计- Weighted Least Square Method

自动驾驶定位系统-State Estimation & Localization

自动驾驶定位算法-直方图滤波定位

自动驾驶高精地图-概述与分析

Waymo-自动驾驶长尾问题挑战(2019)

---

个人博客网站地址: http://www.banbeichadexiaojiubei.com