自動駕駛定位系統-基礎知識之車輛姿態表達

輛位置和姿態是自動駕駛中的一個基礎問題，只有解決了車輛的位置和姿態，才能將自動駕駛的各個模塊關聯起來。車輛的位置和姿態通常由自動駕駛的定位模塊輸出。

以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 四元數

四元數是三維空間旋轉的另外一種表達形式。相對於旋轉矩陣和歐拉角，四元數的優點以下:

一、四元數避免了歐拉角表示法中的萬向鎖問題；

二、相對於三維旋轉矩陣的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