Robot_localization，将NED imu转为相对、绝对航向的 “ENU“ 数据

Robot_localization，将NED imu转为相对、绝对航向的 "ENU"数据

文章约定：

• 谈及 NED、ENU、NWU 坐标系都是指的x y z对应顺序
• ROS中，x y z 轴对应红、绿、蓝
• 如有错误，请包容，以及麻烦在评论区勘误
• 书山有路勤为径，学海无涯苦作舟

1. 问题来源

使用robot_localization进行：imu融合gps
问题：

• robot_localization中坐标系关系不明
• NED imu转 "NEU" imu
• sensor_msgs/Imu 中的四元数从 NED 转 "ENU"
• 融合磁力计的绝对航向、相对航向

2. 解决

2.1 robot_localization 坐标系约定

首先，对robot_localization包中约定坐标系选取及数据单位：

• 坐标系遵循：Rep-105
• 数据单位遵循：Rep-103

总结上面两条：

• 坐标系约定为：map -> odom -> base_link -> imu、lidar、camera …
• 单位采用国际单位

上面是基本内容，但是对于理解robot_localization坐标系关系还差一脚。

首先明确robot_localization规定的：

• imu：采用 "ENU" 坐标系
• gps：只要有经纬度信息就行，它转换后的utm也是采用 "ENU" 坐标系，注意消息格式
• wheel odom：轮式里程计，又称为 encoder，特别的来了，采用的是 NWU 坐标系！

对wheel odom进行解释，官方对轮式里程计坐标系的定义：

原文：

翻译：

如果你有一个地面机器人：
逆时针旋转它，那么它的偏航角yaw应该增加，即为 U，
向前驱动它，它的 P x P_x Px​ 位置应该增加，即为 N
根据右手定则，显然为 NWU 坐标系

虽然robot_localization给出了坐标系约定，理论上根据这个进行转换就可以了，但是，坐标系跟实际中传感器安装顺序也有关，这样一综合考虑，就更加云里雾里了。

2.2 理解 robot_localization 坐标系

问题：为什么我要对 ENU 加引号？

答：那是因为，ENU不一定就必须是ENU，也可以是NWU，他们只是在偏航角上相差一个90°，并且这与放置方向有关。

例如：将robot底盘水平向右放置，那么，NWU 不就变成了 ENU 了吗？

浅浅解释一下：

• 首先，wheel odom的x轴与车头对齐，z轴朝上，当小车水平放置：x y z对应 ENU，垂直向上放置：EWU
• 其次，imu也是x轴与车头对齐，z轴朝上，当小车水平放置：x y z对应 ENU，垂直向上放置：EWU
• 最后，base_link与wheel odom就是“一体关系”，当然base_link也跟你/cmd_vel运动模型解算方向有关，但推荐与wheel odom一致

故当小车水平放置，结合上图，并将传感器坐标系按上面对齐，此时只有一个要求：

• 小车水平放置在地球坐标系中，朝向东边，yaw或者heading为 0。

出处：在配置navsat_trasform_node节点时，yaw_offset参数

这就是robot_localization官方为什么之前说的是x轴朝向北为0，变为现在朝向东为0，因为传感器安装以及小车放置问题，结合官方图就很容易理解了。

2.3 NED imu转为 “ENU” imu

根据以上内容，NED下imu数据转为 "ENU" ，实际是将其从NED转为NWU坐标系。

提问：大师好像翻个面就行？
答：不行，物理翻面相当于绕x轴旋转 π \pi π ，imu自身硬件解算出来的数据会导致x轴多了一个 π \pi π的旋转值，故需要合理进行转换。

PS：市面上有现成的 NWU imu，或者提前问商家ROS驱动是否支持 NWU 坐标系输出。

如：NWU 十轴imu：

如果以上条件都不具备，那么进行人为转换，先进行约定：

• roll、pitch、yaw对应x、y、z，下面将四元数表示的旋转进行转换：
  • 变换顺序：x、y、z，将坐标系变为目标坐标系，绕定轴旋转，左乘，R = R(z)R(y)R(x)
  • 微调：此时我们只是将imu坐标系进行转换，但实际的姿态还尚未对齐坐标系，绕动轴旋转，使其对齐，右乘

ps:：微调是由于imu特性导致的，我们只是将理论坐标系对齐，但是实际imu硬件测的姿态向量还尚未对齐，需要进行微调。
简单理解就是：将imu翻面，坐标系对齐了，但是你发现x轴多了一个 π \pi π 值。
还有一点儿：在欧拉角与四元数的转换中，需要注意：

• 欧拉角的定义方式
• 旋转变换的先后顺序，不同旋转顺序，解算结果不同
• 旋转是绕定轴旋转还是动轴旋转，即左乘和右乘的区别
• R = R(z)R(y)R(x)，R = R(y)R(x)R(z)，分别对应 tf2::quaternion q：q.setRPY()，q.setEuler()

基于ROS sensor_msgs/Imu消息：

#include <Eigen/Eigen>
    ...
    sensor_msgs::Imu imu_data;
    std::string imu_frame_id_ = "imu_link";
    imu_data.header.stamp = ros::Time::now();
    imu_data.header.frame_id = imu_frame_id_.c_str();
    Eigen::Quaterniond q_ned(w, x, y, z);

    Eigen::Quaterniond q_rL =                          
        Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) * 
        Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX()); // 左乘矩阵 使得向量变换为目标坐标系
        
    Eigen::Quaterniond q_rR =                          
        Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) * 
        Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX()); // 右乘矩阵 使得向量对齐坐标系
        
    Eigen::Quaterniond q_nwu =  q_rL * q_ned* q_rR; // 转换四元数
    imu_data.orientation.w = q_nwu.w();
    imu_data.orientation.x = q_nwu.x();
    imu_data.orientation.y = q_nwu.y();
    imu_data.orientation.z = q_nwu.z();
    imu_data.angular_velocity.x =  imu.gyroscope_x; // 角速度变换，只有方向变换
    imu_data.angular_velocity.y = -imu.gyroscope_y;
    imu_data.angular_velocity.z = -imu.gyroscope_z;
    imu_data.linear_acceleration.x = imu.accelerometer_x; // 线加速度变换，只有方向变换
    imu_data.linear_acceleration.y = -imu.accelerometer_y;
    imu_data.linear_acceleration.z = -imu.accelerometer_z;
    ...

经验证，上述代码转换正确，但当进行更复杂旋转变换时，请参考：四元数旋转理解法、欧拉角与四元数互相转换

2.3 绝对航向、相对航向

从字面意思可知：

• 绝对航向：融合磁力计信息，得到imu与 磁极北 之间的夹角关系
• 相对航向：以imu上电为基准点，基于陀螺仪得到的相对航向信息

简单看很好理解，那么问题出现在什么地方？
问题：

1. 如果我需要imu能够在绝对与相对之间切换怎么办？
2. imu支持绝对航向，但是给了欧拉角数据，我想要四元数版的，怎么办？
3. imu不支持绝对航向，但有磁力计，怎么办？
4. imu没磁力计怎么办？

q1：

• 答：首先考虑能够提供该功能驱动的imu厂家，如果不支持，请看问题：q2、q3、q4

q2：

• 答：魔改驱动，将硬件融合磁力计得到的RPY转为四元数，替代硬件解包出来的四元数

下面给出两种方法：

• 基于 tf2 库：

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/Quaternion.h>
	...
    tf2::Quaternion quaternion; // 定义四元数
    quaternion.setRPY(roll_nwu, pitch_nwu, yaw_nwu); // 旋转变换顺序看上文 已经归一化了
    
    geometry_msgs::Quaternion quat_msg;
    quat_msg.x = quaternion.x();
    quat_msg.y = quaternion.y();
    quat_msg.z = quaternion.z();
    quat_msg.w = quaternion.w();
    ...

• 基于 Eigen 库：

#include <Eigen/eigen>
#include <Eigen/Geometry>
	...
	Eigen::AngleAxisd yawAngle(yaw_nwu, Eigen::Vector3d::UnitZ()); // 创建单轴旋转矩阵
	Eigen::AngleAxisd pitchAngle(pitch_nwu, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(roll_nwu, Eigen::Vector3d::UnitX());
    
    Eigen::Quaterniond q= yawAngle * pitchAngle * rollAngle; // 采用外旋 Z-Y-X 左乘
    Eigen::Quaterniond q_nwu= q.normalized(); // 归一化处理
    
    imu_data.orientation.w = q_nwu.w(); // 替换
    imu_data.orientation.x = q_nwu.x();
    imu_data.orientation.y = q_nwu.y();
    imu_data.orientation.z = q_nwu.z();
    ...

注意：yaw_nwu、pitch_nwu、roll_nwu表示的是NWU坐标系下的欧拉角，与NED的关系：

• yaw_nwu = - yaw_ned
• pitch_nwu = - pitch_ned
• roll_nwu = roll_ned ± Pi：根据实际来调整

q3：

请参考：

• madgwick滤波：ROS imu_filter_madgwick
• 互补滤波：ROS imu_complementary_filter

q4：

如果没有磁力计：

• 将GNSS或者定位定向RTK提供的航向角信息接入到 imu 中（如果可接入）
• 设置 robot_localization 中的yaw_offset参数
• 采用相对航向，将小车车头方向始终对齐地理东启动

我们需要的是与地理北之间的偏航关系，但是实际指向的是磁极北，误差叫 磁偏角 ，我们需要在 robot_localization 中给出这个值：

• magnetic_declination_radians：在线计算，单位：Rad

感谢您的来访，欢迎交流学习分享~