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包中约定坐标系选取及数据单位:
总结上面两条:
- 坐标系约定为: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
坐标系输出。
如果以上条件都不具备,那么进行人为转换,先进行约定:
roll
、pitch
、yaw
对应x、y、z,下面将四元数表示的旋转进行转换:- 变换顺序:x、y、z,将坐标系变为目标坐标系,绕定轴旋转,左乘,
R = R(z)R(y)R(x)
- 微调:此时我们只是将imu坐标系进行转换,但实际的姿态还尚未对齐坐标系,绕动轴旋转,使其对齐,右乘
- 变换顺序:x、y、z,将坐标系变为目标坐标系,绕定轴旋转,左乘,
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()
#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;
...
转换结果:采用绝对航向,x
轴指向东时,与世界坐标系对齐
经验证,上述代码转换正确,但当进行更复杂旋转变换时,请参考:四元数旋转理解法、欧拉角与四元数互相转换
2.3 绝对航向、相对航向
从字面意思可知:
- 绝对航向:融合磁力计信息,得到imu与
磁极北
之间的夹角关系 - 相对航向:以imu上电为基准点,基于陀螺仪得到的相对航向信息
简单看很好理解,那么问题出现在什么地方?
问题:
- 如果我需要imu能够在绝对与相对之间切换怎么办?
- imu支持绝对航向,但是给了欧拉角数据,我想要四元数版的,怎么办?
- imu不支持绝对航向,但有磁力计,怎么办?
- 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
感谢您的来访,欢迎交流学习分享~