Apollo: 如何理解gps、corrimu的坐标转换(How to understand the coordinate conversion of gps and corrimu)

[xinwf]

in data_parser.cpp, function DataParser::publish_corrimu_message, the code segment is in the following

  imu_msg->mutable_linear_acceleration()->set_x(
      -ins->linear_acceleration().y());
  imu_msg->mutable_linear_acceleration()->set_y(ins->linear_acceleration().x());
  imu_msg->mutable_linear_acceleration()->set_z(ins->linear_acceleration().z());

  imu_msg->mutable_angular_velocity()->set_x(-ins->angular_velocity().y());
  imu_msg->mutable_angular_velocity()->set_y(ins->angular_velocity().x());
  imu_msg->mutable_angular_velocity()->set_z(ins->angular_velocity().z());

  imu_msg->mutable_euler_angles()->set_x(ins->euler_angles().x());
  imu_msg->mutable_euler_angles()->set_y(-ins->euler_angles().y());
  imu_msg->mutable_euler_angles()->set_z(ins->euler_angles().z() -
                                         90 * DEG_TO_RAD_LOCAL);

I think the real installtion of apollo's IMU device with relative to RFU coordinate system is like this:

<------- Heading direction of the car (top view)

                                                              /\ X
                                                              |
                                                              |
                                                              |
                                                              |
                    IMU                                  RFU  |
x <-----------------z(UP)                Y<-------------------Z(UP)
                    |                                                                                       
                    |                                                                                       
                    |                                                                                       
                    |                                                                                       
                   \/ y                                                                                     
Real IMU's coordinate system               Apollo's RFU coordinate system

So, the IMU's coordinate need to be translated to RFU coordinate, according to the relative position between imu's coordinate and rfu, the following conversion should be right.

X = -y
Y = x
Z = z

In abovemetioned code, it does do this conversion when solving linear acceleration and angular velocity. But I am really confused about the conversion of euler angles. In third line, why it needs to minus 90 degree? Does this need a transformation matrix to get the corrimu's linear acceleration, angular velocity and euler anlges or just do a simple axis transformation of values?

  imu_msg->mutable_euler_angles()->set_x(ins->euler_angles().x());
  imu_msg->mutable_euler_angles()->set_y(-ins->euler_angles().y());
  imu_msg->mutable_euler_angles()->set_z(ins->euler_angles().z() -
                                         90 * DEG_TO_RAD_LOCAL);

the same thing happens in function publish odometry

      Eigen::Quaterniond q =
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

[jilinzhou]

Take a look of this issue: https://github.com/ApolloAuto/apollo/issues/5676

[hillbrook]

1.Applying the right-hand rule on the x, y and z axes of vehicle frame, the Euler angles (roll, pitch and yaw) are defined, respectively.

• Yaw is positive when the vehicle turns left;
• Pitch is positive when the vehicle is nose up;

• Roll is positive when the left side of the vehicle is up.

  In novatel_parser.cc, vehicle frame is FLU. The original idea is publish sensor-independent
  vehicle frame in driver.
  In data_parser.cc, vehicle frame is converted back to RFU(Localization module use RFU vehicle
  body frame);
  Because yaw is positive when the vehicle turns left, yaw is counterclockwise from North(opposite
  to azimuth).
  In the following code,

  // 2. orientation
  Eigen::Quaterniond q =
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

Since euler angle in ins messgae is FLU frame, the auler angle in RFU frame is below:

 yaw_rfu = ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL;
 pitch_rfu = -ins->euler_angles().y();
 roll_rfu = ins->euler_angles().x();

Using intrinsic z-x-y sequence, quaternion is below:

  Eigen::Quaterniond q =
      Eigen::AngleAxisd(yaw_rfu,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(pitch_rfu, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(roll_rfu, Eigen::Vector3d::UnitY());

2.In Planning, heading is counterclockwise from x-axis (ENU frame).

[xinwf]

Sorry for my guess about the installation of apollo's IMU, in fact, the coordinate system of apollo's IMU is consistent with RFU, but as @jilinzhou mentioned at #5676, in novatel_parser.cc, they convert the imu's coordinate to FLU's coordinate system, and then converting it back in data_parser.cc.