自动驾驶中的欧拉角-四元数-坐标变换

前言

在对角度的描述中，有欧拉角，四元数两中描述方式，其中欧拉角最直观，但是不具备唯一性，即按照不同的旋转方式得到的欧拉角表示结果是不一样的。但四元数的表达方式是唯一的，所以针对不同的设备，如果用欧拉角表示角度，一定得确定是用的哪种旋转顺序，目前使用最多的是ZYX，即先旋转Z轴，再旋转Y轴，再旋转X轴。但是百度apollo用的是ZXY坐标系，这里要特别注意。

一、apollo 中的欧拉角转四元数

ZXY 旋转顺序

首先明确一点不同的旋转顺序，欧拉角和四元数之间转换的公式是不一样的，以下推导使用的旋转顺序为： Z,X,Y;

X ：pitch；
Y: roll;
Z: yaw;
这里注意，绕X 轴旋转为pitch, 绕Y轴旋转为roll,与常用的坐标系有点不一样，这里要特别注意。

1.1.欧拉角转四元数

绕 [公式] 轴旋转角度为，绕 [公式] 轴旋转角度为，绕 [公式] 轴旋转角度为，把这三个角轴转换为四元数得到以下公式：
四元数相乘得到：

其中
q(1):w,
q(2):x,
q(3):y,
q(4): z

1.2，四元数转欧拉角

四元数转欧拉角：
其中x:滚转角roll,y:俯仰角pitch,z:偏航角yaw

二、 ZYX顺序情况

以下推导使用的旋转顺序为： Z,X,Y;

X ：roll；
Y: pitch;
Z: yaw;

2.1 分别绕x,y,z旋转

2.2 欧拉角转化公式ZYX：旋转矩阵

此时，该旋转矩阵表示全局坐标（大地）到局部坐标（车辆），
如roll ,pitch ,yaw ：表示当前车辆在大地坐标下绕x,y,z旋转的角度；
vector[车辆坐标系的x,y,z] = cMw * vector[全局坐标系下的x,y,z];
例如：
世界坐标系下的坐标（Xw,Yw,Zw）转换到相机坐标系下（Xc，Yc,Zc）。
将旋转矩阵取逆即可得到 相机坐标系 到 世界坐标系下的坐标。

三、ZXY代码直接计算

ZXY全局到局部：
X：roll；
Y：pitch;
z: yaw

  double x1 = cos(yaw) * acc_x - sin(yaw) * acc_y;
  double y1 = sin(yaw) * acc_x + cos(yaw) * acc_y;
  double z1 = acc_z;

  double x2 = x1;
  double y2 = cos(roll) * y1 - sin(roll) * z1;
  double z2 = sin(roll) * y1 + cos(roll) * z1;

  acc_x = cos(pitch) * x2 + sin(pitch) * z2;
  acc_y = y2;
  acc_z = -sin(pitch) * x2 + cos(pitch) * z2;

  // AERROR << "out=" << acc_x << "," << acc_y << "," << acc_z;
  Eigen::Vector3d vec(acc_x, acc_y, acc_z);

ZXY局部到全局：

  double x2 = cos(pitch) * acc_x + sin(pitch) * acc_z;
  double y2 = acc_y;
  double z2 = -sin(pitch) * acc_x + cos(pitch) * acc_z;
  double x1 = x2;
  double y1 = cos(roll) * y2 - sin(roll) * z2;
  double z1 = sin(roll) * y2 + cos(roll) * z2;
  acc_x = cos(yaw) * x1 - sin(yaw) * y1;
  acc_y = sin(yaw) * x1 + cos(yaw) * y1;
  acc_z = z1;

四、ZXY 使用Eigen函数方法

X：roll；
Y：pitch;
z: yaw

//欧拉角转四元数
Eigen::Quaterniond tmp_q =
      Eigen::AngleAxisd(yaw_t, Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(roll_t, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(pitch_t, Eigen::Vector3d::UnitY());
  // AERROR << "Eigen: " << tmp_q.x() << "," << tmp_q.y() << "," << tmp_q.z()
  //        << "," << tmp_q.w();
  Eigen::Matrix3d rotation_mat = Eigen::Matrix3d::Identity();

  //四元数转旋转矩阵
  rotation_mat = tmp_q.toRotationMatrix();
  //局部到全局
  Vector3d vec0 = rotation_mat * orig;
  AERROR << "Eigen= " << vec0[0] << "," << vec0[1] << "," << vec0[2];
  //全局到局部
  Vector3d vec1 = rotation_mat.inverse() * orig;
  AERROR << "Eigen= " << vec1[0] << "," << vec1[1] << "," << vec1[2];
  //旋转矩阵按照ZXY方式得到欧拉角
  Vector3d vec_rpy = rotation_mat.eulerAngles(2, 0, 1); //ZXY
  // AERROR << "ypr=" << vec_rpy[0] << "," << vec_rpy[1] << "," << vec_rpy[2];

说明：原创文章，转载请注明出处