Autopilot positioning system – vehicle attitude representation based on basic knowledge

Time:2020-9-11

Vehicle position and attitude is a basic problem in automatic driving. Only when the vehicle position and attitude are solved can the various modules of automatic driving be associated. The position and attitude of the vehicle are generally output by the positioning module of automatic driving.

Taking Apollo as an example, the definition of vehicle’s pose is as follows:

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. Vehicle location

Vehicle reference point (VRP) generally selects the position of a vehicle reference point in the world coordinate system as the vehicle position.

Autopilot positioning system - vehicle attitude representation based on basic knowledge

The world geodetic system dating from 1984 is adopted as the world coordinate system in Apollo, as shown in the figure below.

Autopilot positioning system - vehicle attitude representation based on basic knowledge

The local coordinate system of Apollo’s pose is the ENU (East North up) coordinate system.

Autopilot positioning system - vehicle attitude representation based on basic knowledge

Vehicle attitude 2

2.1 Euler angle

In a right-handed Cartesian coordinate system, the rotation angles along the x-axis, Y-axis and z-axis are called roll, pitch, and yaw, respectively.

Autopilot positioning system - vehicle attitude representation based on basic knowledge

PitchIt’s the angle of rotation around the X axis, also known as pitch angle. When the positive half axis of the X axis is above the horizontal plane (head up) passing through the origin of the coordinates, the pitch angle is positive, otherwise it is negative. The rotation matrix is as follows:

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

Autopilot positioning system - vehicle attitude representation based on basic knowledge

YawIs the angle of rotation around the Y axis, also known as yaw angle. That is, the right yaw of the nose is positive, otherwise it is negative. The rotation matrix is as follows:

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

Autopilot positioning system - vehicle attitude representation based on basic knowledge

RollIt’s rotation around the Z axis, also known as the roll angle. The body roll to the right is positive, otherwise it is negative. The rotation matrix is as follows:

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

Autopilot positioning system - vehicle attitude representation based on basic knowledge

It’s not enough to have a pitch, raw, roll angleOrder of rotationandReference frame of rotationDifferent rotation order and different rotation reference coordinate system will lead to different rotation results.

The first is rotation sequence, which can be divided into two types
Proper Euler angles: the rotation order is z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y. the first and last rotation axes are the same.
Tait–Bryan anglesThe rotation order is X-Y-Z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z.

The second isReference coordinate system of rotationAccording to the rotation coordinate system, Euler angle can be divided into:
Internal rotationThat is, according to the object’s own coordinate system to rotate, the coordinate system will follow the rotation and produce an offset with the world coordinate system.
External rotationIn other words, it rotates according to the world coordinate system.

The disadvantages of Euler angle are as follows

A major drawback of Euler angle is that it will encounter the famous gimbal lock problem: when the pitch angle is $/ PM 90 ^ {0} $, the first rotation and the third rotation will use the same axis, which makes the system lose one degree of freedom (from three rotation to two rotation). It can be proved theoretically that if we want to express three-dimensional rotation by three real numbers, we will inevitably encounter the problem of singularity. For this reason, Euler angle is not suitable for interpolation and iteration, and is often used only in human-computer interaction. We also rarely use Euler angle to represent attitude directly in slam program, and we do not use Euler angle to represent rotation in filtering or optimization (because it has singularity).

2.2 quaternion

Quaternion is another expression of rotation in three-dimensional space. Compared with rotation matrix and Euler angle, quaternion has the following advantages:

1. Quaternion avoids the universal lock problem in Euler angle representation;

2. Compared with the nine components of the three-dimensional rotation matrix, the quaternion is more compact, and all attitudes can be expressed with four components.

The definition of quaternion is as follows:

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

Where I, J, K are the three imaginary parts of quaternion. The three imaginary parts satisfy the relation

$$\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}$$

There are two problems to be solved when using quaternion to represent rotation. One is how to use quaternion to represent points in 3D space, and the other is how to use quaternion to represent rotation in 3D space.

Quaternions represent points in a space

Suppose the coordinates of the points in three-dimensional space are(x,y,z)Then its quaternion form is $Xi + YJ + ZK $, which is a pure quaternion (quaternion with real part 0).

The unit quaternion represents a rotation in three-dimensional space

set upqIs a unit quaternion, andpIs a pure quaternion, then ${R_ {q} (P) = QPQ {- 1}} $is also a pure quaternion, we can prove that $R_ Q $is a rotation that rotates point P to another point $r in space_ q(p)$。

Rotation angle and transformation of Quaternion

A quaternion converts a rotation about a coordinate axis to a rotation about a vector, assuming that a rotation is about the unit vector $n = [n_ x,n_ y,n_ z] The quaternion form of this rotation is as follows:

$$\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}$$

Quaternion and transformation of rotation angle / rotation axis

$$
\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}
$$

In C + +, the code for defining quaternion with eigen is as follows, which defines a rotation operation of 30 degrees around the Z axis.

#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));
  //Output all coefficients
  std::cout << q.coeffs() << std::endl;
  //Output imaginary part coefficient
  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;
}

The output of the code is as follows:

0
0
0.5
0.866025
//Q. coeffs() first outputs the real part of quaternion, and then outputs the imaginary part of quaternion
  0
  0
0.5
//Q. vec() outputs the imaginary part of quaternion

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

Transformation from quaternion to rotation matrix

Let quaternion $q = Q_ 0 + q_ 1i + q_ 2j + q_ 3K $, then the corresponding rotation matrix is:

$$
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 ]
$$

Transformation from rotation matrix to Quaternion

Suppose the matrix is $r = {M_ The quaternion Q of {ij}}, I, j in [1,2,3] $, is: 1

$$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 quaternion, Euler angle, rotation matrix, rotation vector transformation

2.3.1 transformation from rotation vector to rotation matrix, Euler angle and quaternion

Define rotation vector

//Initialize the rotation vector: the rotation angle is alpha and the rotation axis is (x, y, z)
Eigen::AngleAxisd rotation_vector(alpha, Eigen::Vector3d(x,y,z))

Rotate vector to rotation matrix:

//Rotation vector to rotation matrix
Eigen::Matrix3d rotation_matrix = rotation_vector.matrix();

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

Rotate matrix to Euler angle:

//Rotation vector rotation Euler angle (z-y-x)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

Rotate matrix to quaternion:

//Rotation vector to Quaternion
Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion = rotation_vector;
2.3.2 transformation from rotation matrix to rotation vector, Euler angle and quaternion

Rotation matrix:

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

Rotation matrix to rotation vector:

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector = rotation_matrix;

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

Rotation matrix to Euler angle

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

Rotation matrix to Quaternion

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion = rotation_matrix;
2.3.3 transformation from Euler angle to rotation vector, rotation matrix and quaternion

Initial Euler angle:

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

Euler angular rotation vector:

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;

Euler angular rotation matrix:

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;

Euler angle rotation quaternion

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 transformation from quaternion to rotation vector, rotation matrix and quaternion

Quaternion

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

Quaternion rotation vector

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector = quaternion;

Quaternion rotation matrix

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

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

Quaternion to Euler angle

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

Reference link

1. Quaternion: https://www.cnblogs.com/gaoxiang12/p/5120175.html
2. Wikipedia: quaternion and spatial rotation
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. Quaternion, Euler angle, rotation matrix and rotation vector in eigen https://www.cnblogs.com/lovebay/p/11215028.html
4. Visual slam 14: from theory to practice
5. Baidu Apollo project: https://github.com/ApolloAuto/apollo

Related articles

Error state extend Kalman filter

Automatic driving positioning system extended Kalman filter

Automatic driving positioning system Kalman filter

Automatic driving system positioning and state estimation – Recursive Least Squares Estimation

Automatic driving system positioning and state estimation – Weighted Least Square Method

State Estimation & Localization

Automatic driving location algorithm histogram filter localization

Automatic driving high precision map – overview and analysis

Waymo – automatic driving long tail challenge (2019)


Autopilot positioning system - vehicle attitude representation based on basic knowledge

Personal blog website address: http://www.banbeichadexiaojiubei.com