从零开始学习自动驾驶系统(八)-基础知识之车辆姿态表达

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

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

代码语言：javascript复制

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中选择车辆后轴中心作为车辆的基准点

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

1

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

2

2. 车辆的姿态角

2.1 欧拉角

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

在机器人行业中我们常说的roll、yaw、pitch是什么

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

在机器人行业中我们常说的roll、yaw、pitch是什么

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

在机器人行业中我们常说的roll、yaw、pitch是什么

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

在机器人行业中我们常说的roll、yaw、pitch是什么

仅仅有旋转角度(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)问题：在俯仰角为±90deg时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性的问题。由于这种原因，欧拉角不适于插值和迭代，往往只用在人机交互中。我们也很少在SLAM程序中直接使用欧拉角表示姿态，同样不会在滤波或优化中使用欧拉角表示旋转（因为它具有奇异性）。

2.2 四元数

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

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

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

四元数的定义如下:

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

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

四元数表示空间中的点

假设三维空间里的点坐标为 (x,y,z)，则它的四元数形式为

:，这是一个纯四元数(实部为0的四元数)。

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

设 q 为一个单位四元数，而 p 是一个纯四元数，则

也是一个纯四元数，可以证明

表示一个旋转，将点p旋转到空间的另一个点

。

旋转角度与四元数的转化

四元数将绕坐标轴的旋转转化为绕向量的旋转，假设某个旋转是绕单位向量

进行了角度为

的旋转，那么这个旋转的四元数形式为：

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

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

代码语言：javascript复制

#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;
}

代码的输出如下:

代码语言：javascript复制

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为:

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

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

定义旋转向量

代码语言：javascript复制

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

旋转向量到旋转矩阵:

代码语言：javascript复制

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

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

旋转矩阵到欧拉角:

代码语言：javascript复制

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

旋转矩阵到四元数:

代码语言：javascript复制

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

Eigen::Quaterniond quaternion = rotation_vector;

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

旋转矩阵:

代码语言：javascript复制

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

旋转矩阵转旋转向量:

代码语言：javascript复制

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector = rotation_matrix;

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

旋转矩阵转欧拉角

代码语言：javascript复制

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

旋转矩阵转四元数

代码语言：javascript复制

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion = rotation_matrix;

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

初始化欧拉角:

代码语言：javascript复制

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

欧拉角转旋转向量：

代码语言：javascript复制

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;

欧拉角转旋转矩阵：

代码语言：javascript复制

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;

欧拉角转四元数

代码语言：javascript复制

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 四元数到旋转向量、旋转矩阵、四元数的转换

四元数

代码语言：javascript复制

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

四元数转旋转向量

代码语言：javascript复制

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector = quaternion;

四元数转旋转矩阵

代码语言：javascript复制

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

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

四元数转欧拉角

代码语言：javascript复制

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

参考链接

1、四元数: https://www.cnblogs.com/gaoxiang12/p/5120175.html

2、维基百科：四元数与空间旋转 https://zh.wikipedia.org/wiki/四元数与空间旋转

3、eigen 中四元数、欧拉角、旋转矩阵、旋转向量 https://www.cnblogs.com/lovebay/p/11215028.html

4、视觉SLAM十四讲:从理论到实践

5、百度Apollo项目: https://github.com/ApolloAuto/apollo

javascript https 网络安全 自动驾驶 无人驾驶

0 人点赞