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

时间 2020-02-16

原文原文链接

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

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

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)通常选取一个车辆的基准点在世界坐标系的位置做为车辆位置。git

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

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

2. 车辆的姿态角

2.1 欧拉角

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

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轴旋转的角度，也叫偏航角。即机头右偏航为正，反之为负。旋转矩阵以下:spa

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

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 四元数

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

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

二、相对于三维旋转矩阵的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);

参考连接

一、四元数: https://www.cnblogs.com/gaoxiang12/p/5120175.html
二、维基百科：四元数与空间旋转
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
三、eigen 中四元数、欧拉角、旋转矩阵、旋转向量 https://www.cnblogs.com/lovebay/p/11215028.html
四、视觉SLAM十四讲:从理论到实践
五、百度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