（二）IMU采集、过滤，利用EKF将IMU与odom融合，发布新的odom话题
____先说整体处理流程：底层使用STM32F4，采集MPU9250、编码器，通过串口在一个数据帧内将两个数据送入ROS；ROS采集节点将二者数据解析，对IMU发布为imu_data_raw话题，使用 imu_tools订阅过滤后再发布为imu_data_filtered，同时，采集节点将编码器数据航迹推演，发布为odom_raw；接着，使用 robot_pose_ekf订阅imu_data_filtered、odom_raw话题，融合后发布为odom_merger；最后，odom_merger话题数据类型为 geometry_msgs::PoseWithCovarianceStamped，而后续movebase需要类型为 nav_msgs/Odometry，前者是后者内容的一部分，需要简单转换下再发布为odom话题。在这过程中，多看wiki把各个数据类型具体意义搞清楚，数据对应上才能不出错，下面详细介绍我的实现步骤：
后面需要使用到串口和贝叶斯滤波器库，先安装下
sudo apt-get install ros-melodic-serial
sudo apt-get install ros-melodic-bfl
然后，为用户配置串口权限：
sudo gedit /etc/udev/rules.d/70-ttyUSB.rules
##输入：KERNEL=="ttyUSB*", OWNER="root", GROUP="root", MODE="0666"
保存退出，不用重启电脑，配置完成。
A、ROS采集节点
采集节点是最繁琐步骤，因为要和底层打交道，先上代码，再具体结合代码说吧：
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <sstream>
#include <tf/transform_broadcaster.h>
typedef union
{
float f;
unsigned char u[4];
}Float4Byte;
signed short left_vel, right_vel; //单位：rps
const float wheel_dist = 0.52; //轮间距0.52m
const float circumference = 0.628; //周长0.628m
const float linesNum = 1024;
serial::Serial ser;
void write_callback(const geometry_msgs::Twist& cmd_vel)
{
left_vel = (cmd_vel.linear.x - cmd_vel.angular.z * wheel_dist / 2) / circumference;
right_vel = (cmd_vel.linear.x + cmd_vel.angular.z * wheel_dist / 2) / circumference;
ROS_INFO("I heard linear velocity: x-[%f],y-[%f],",cmd_vel.linear.x,cmd_vel.linear.y);
ROS_INFO("I heard angular velocity: [%f]",cmd_vel.angular.z);
// ser.write(msg->data); //发送串口数据,用来控制底盘运动
}
unsigned int modbus_CRC(unsigned char *arr_buff, unsigned char len)
{
unsigned short int crc=0xFFFF;
unsigned char i, j, Data;
for( j=0; j < len; j++)
{
crc=crc ^*arr_buff++;
for ( i=0; i<8; i++)
{
if( ( crc&0x0001) >0)
{
crc=crc>>1;
crc=crc^ 0xa001;
}
else
crc=crc>>1;
}
}
return crc;
}
int main (int argc, char** argv){
ros::init(argc, argv, "serial_imu_node");
ros::NodeHandle nh;
ros::Subscriber IMU_write_pub = nh.subscribe("cmd_vel", 1000, write_callback);
ros::Publisher imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu_data_raw", 1000);
ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom_raw", 500);
Float4Byte quaternion_q0, quaternion_q1, quaternion_q2, quaternion_q3;
// Float4Byte vx, vy; //单位：m/s
float delta_dist, delta_speed, delta_x, delta_y, pos_x, pos_y; //单位：m
signed short encoder_right = 0, encoder_left = 0;
signed short angular_velocity_x, angular_velocity_y, angular_velocity_z;
signed short linear_acceleration_x, linear_acceleration_y, linear_acceleration_z;
geometry_msgs::Quaternion odom_quat;
float delta_th = 0, dt = 0;
int count = 0;
unsigned short int crc;
float th = 0;
try {
ser.setPort("/dev/ttyUSB0");
ser.setBaudrate(460800);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException& e){
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
if(ser.isOpen()){
ROS_INFO_STREAM("Serial Port initialized");
}
else{
return -1;
}
ros::Time current_time = ros::Time::now();
ros::Time last_time = ros::Time::now();
ros::Rate loop_rate(200);
while(ros::ok()){
//处理从串口来的Imu数据
//串口缓存字符数
unsigned char data_size;
if(data_size = ser.available()) { //ser.available(当串口没有缓存时，这个函数会一直等到有缓存才返回字符数
unsigned char tmpdata[data_size];
current_time = ros::Time::now();
//打包IMU数据
sensor_msgs::Imu imu_data_raw;
nav_msgs::Odometry odom;
imu_data_raw.header.stamp = ros::Time::now();
imu_data_raw.header.seq = count;
imu_data_raw.header.frame_id = "base_link";
imu_data_raw.orientation_covariance = {1000000.0, 0, 0,
0, 1000000, 0,
0, 0, 0.000001};
imu_data_raw.angular_velocity_covariance = imu_data_raw.orientation_covariance;
imu_data_raw.linear_acceleration_covariance = {-1,0,0,0,0,0,0,0,0};
odom.header.stamp = ros::Time::now();
odom.header.seq = count;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.covariance = {0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 1000000, 0, 0, 0,
0, 0, 0, 1000000, 0, 0,
0, 0, 0, 0, 1000000, 0,
0, 0, 0, 0, 0, 1000};
odom.twist.covariance = odom.pose.covariance;
ser.read(tmpdata, data_size);
if((tmpdata[0] == 0xA5) && (tmpdata[1] == 0x5A))
{
crc = modbus_CRC(tmpdata, 34);
if((crc>>8 == tmpdata[35]) && (crc|0xFF == tmpdata[34]))
{
quaternion_q0.u[0] = tmpdata[2]; quaternion_q0.u[1] = tmpdata[3]; quaternion_q0.u[2] = tmpdata[4];
quaternion_q0.u[3] = tmpdata[5];
quaternion_q1.u[0] = tmpdata[6]; quaternion_q1.u[1] = tmpdata[7]; quaternion_q1.u[2] = tmpdata[8];
quaternion_q1.u[3] = tmpdata[9];
quaternion_q2.u[0] = tmpdata[10]; quaternion_q2.u[1] = tmpdata[11]; quaternion_q2.u[2] = tmpdata[12];
quaternion_q2.u[3] = tmpdata[13];
quaternion_q3.u[0] = tmpdata[14]; quaternion_q3.u[1] = tmpdata[15]; quaternion_q3.u[2] = tmpdata[16];
quaternion_q3.u[3] = tmpdata[17];
linear_acceleration_x = (tmpdata[19] << 8) | tmpdata[18];
linear_acceleration_y = (tmpdata[21] << 8) | tmpdata[20];
linear_acceleration_z = (tmpdata[23] << 8) | tmpdata[22];
angular_velocity_x = (tmpdata[25] << 8) | tmpdata[24];
angular_velocity_y = (tmpdata[27] << 8) | tmpdata[26];
angular_velocity_z = (tmpdata[29] << 8) | tmpdata[28];
encoder_right = (tmpdata[31] << 8) | tmpdata[30];
encoder_left = (tmpdata[33] << 8) | tmpdata[32];
// vx.f = (((tmpdata[30] << 8) | tmpdata[31]) + ((tmpdata[32] << 8) | tmpdata[33]))/1024 * 0.628;
// vy.f = 0;
// ROS_INFO("linear_acceleration_x: [%d], linear_acceleration_y: [%d], linear_acceleration_z: [%d]", \
// linear_acceleration_x, linear_acceleration_y, linear_acceleration_z);
// ROS_INFO("angular_velocity_x: [%d], angular_velocity_y: [%d], angular_velocity_z: [%d]", \
angular_velocity_x, angular_velocity_y, angular_velocity_z);
imu_data_raw.orientation.x = quaternion_q0.f;
imu_data_raw.orientation.y = quaternion_q1.f;
imu_data_raw.orientation.z = quaternion_q2.f;
imu_data_raw.orientation.w = quaternion_q3.f;
imu_data_raw.angular_velocity.x = ((float)(angular_velocity_x)/131.0)*3.1415/180.0;
imu_data_raw.angular_velocity.y = ((float)(angular_velocity_y)/131.0)*3.1415/180.0;
imu_data_raw.angular_velocity.z = ((float)(angular_velocity_z)/131.0)*3.1415/180.0;
imu_data_raw.linear_acceleration.x = ((float)(linear_acceleration_x)/16386.0)*9.80665;
imu_data_raw.linear_acceleration.y = ((float)(linear_acceleration_y)/16386.0)*9.80665;
imu_data_raw.linear_acceleration.z = ((float)(linear_acceleration_z)/16386.0)*9.80665;
// encoder_right = 50;
// encoder_left = 50;
dt = (current_time - last_time).toSec();
delta_th = imu_data_raw.angular_velocity.z * dt;
delta_dist = (encoder_right + encoder_left)/linesNum * circumference / 2;
// delta_speed = delta_dist / dt;
delta_x = cos(delta_th) * delta_dist;
delta_y = -sin(delta_th) * delta_dist;
th += delta_th;
pos_x += (cos(th) * delta_x - sin(th) * delta_y);
pos_y += (sin(th) * delta_x + cos(th) * delta_y);
odom_quat = tf::createQuaternionMsgFromYaw(th);
ROS_INFO("th: %f, pos_x: %f, pos_y: %f", th, pos_x, pos_y);
odom.pose.pose.position.x = pos_x;
odom.pose.pose.position.y = pos_y;
odom.pose.pose.position.z = 0;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = 0;
odom.twist.twist.linear.y = 0;
odom.twist.twist.angular.z = imu_data_raw.angular_velocity.z;
count++;
imu_raw_pub.publish(imu_data_raw); //imu_raw_pub 节点发布消息至imu_data_raw topic
odom_pub.publish(odom);
}
}
last_time = current_time;
ros::spinOnce();
}
}
}
____首先，明确下，从串口接收的数据协议：起始帧（0xA5 0x5A）、四元数（float）、三轴加速度计（float）、三轴陀螺仪（float）、左右轮编码器（short）、CRC校验码（标准modbus），发往串口数据：左右轮速度。float类型数据使用联合体传输，不用找博客看的云里雾里，代码一看就明白。
____从main函数看起，建立一个速度控制cmd_vel订阅，两个imu_data_raw、odom_raw发布，再定义一些变量（串口接收数据缓存、航迹推演过程变量）。节点和stm32使用串口通信，在try语句中尝试打开串口，具体串口设置见代码，接着将串口数据依据协议转换，注意imu_data_raw和odom有几个协方差矩阵，设置见代码。加速度计、陀螺仪数据单位为标准单位m/s2，rad/s，从原始数据转换为标准单位要依据IMU初始化设置，对应关系在这里，也可以直接查datasheet,我在初始化时FS_SEL、AFS_SEL都为0，转换过程见代码。航迹推演部分见链接博客，略不同的是，角速度我采样IMU数据，累加计算yaw角度后，转换为四元数发布进odom。到这里，发布imu、odom数据，完成采集、解析。
B、imu_tools过滤imu数据
____做过嵌入式的都知道，传感器的原始数据是不能直接使用的，需要进行标定、过滤，mpu9250标定在这不说了，有兴趣的以后我再写一篇，这里只讲数据过滤。imu过滤有两种方式，一种是在ros里过滤，二是直接在下位机里过滤后上传，比如下位机使用滑动滤波、卡尔曼这样，当然卡尔曼效果更好。可惜下位机卡尔曼的代码好些年没使用，找不到了…所以这部分过滤放ros里做了，直接使用的imu_tools。git一通扒拉，复制里面的imu_filter_madgwick文件夹到我们的ros工作空间源码目录，根目录catkin_make一下就能用了。
____在src/complementary_filter_ros.cpp中67行左右，有句代码修改如下，订阅我们自己的话题名。
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, "/imu_data_raw", queue_size));
52行左右修改如下，发布过滤后的数据。
// Register publishers:
imu_publisher_ = nh_.advertise<sensor_msgs::Imu>("/imu_data_filtered", queue_size);
修改launch文件如下：
<!-- ComplementaryFilter launch file -->
<launch>
<node pkg="imu_complementary_filter" type="complementary_filter_node"
name="complementary_filter_gain_node" output="screen">
<param name="do_bias_estimation" value="true"/>
<param name="do_adaptive_gain" value="true"/>
<param name="use_mag" value="false"/>
<param name="gain_acc" value="0.01"/>
<param name="gain_mag" value="0.01"/>
<param name="publish_debug_topics" value="false"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="rplidar_ros" type="imu_encoder_com"
name="imu_encoder_com_node" output="screen">
</node>
</launch>
____做一些设置，记得关闭发布tf，顺手启动下采集节点。现在可以启动了，rostopic echo、rviz都可以查看过滤后的数据。launch文件里可以看到，实际上是可以地磁信息也放进去的，而mpu9250是带有地磁计的，可以从串口传进来送进去过滤，但是我在下位机计算四元数时已经把地磁信息加进去了，所以imu_tools这里没使用地磁。
C、使用 robot_pose_ekf 对imu和odom进行融合
robot_pose_ekf包的使用，主要是参考了这里，下载下来，catkin_make就可以，编译robot_pose_ekf包时候，可能出现错误：No package ‘orocos-bfl’ found，解决：
sudo apt-get install ros-melodic-bfl.
然后按照博客里的设置修改下去，注意话题名称一定要对应上，还有就是TF树，在odom_estimation_node.cpp中，odom_broadcaster.sendTransform(StampedTransform(tmp, tmp.stamp, output_frame, base_footprint_frame))函数会发布一个odom到base_footprint的TF，在这我注释掉了不让他发布。 我的launch文件如下：
<launch>
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="output_frame" value="odom_combined"/>
<param name="base_footprint_frame" value="base_footprint"/>
<param name="freq" value="50.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
</node>
</launch>
到这里，融合完成，这时候odom是PoseWithCovarianceStamped格式，需要自己转换成后面导航部分要求的格式Odometry，，网上找的都是python，比较简单就自己写了个cpp，同时发布一下TF：
//
// Created by smith on 7/24/20.
//
#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
class OdomEKFTransform
{
public:
OdomEKFTransform()
{
pub_ = n_.advertise<nav_msgs::Odometry>("/odom", 100);
sub_ = n_.subscribe("/robot_pose_ekf/odom_combined", 100, &OdomEKFTransform::callback, this);
}
void callback(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odom_trans;
static tf::TransformBroadcaster odom_broadcaster;
odom.header = msg.header;
odom.header.frame_id = "odom";
odom.pose = msg.pose;
pub_.publish(odom);
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
odom_trans.transform.translation.x = odom.pose.pose.position.x;
odom_trans.transform.translation.y = odom.pose.pose.position.x;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom.pose.pose.orientation;
odom_broadcaster.sendTransform(odom_trans);
}
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Subscriber sub_;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "OdomEKF");
OdomEKFTransform odomEKF;
ros::Rate loop_rate(50);
while(ros::ok()) {
ros::spin();
}
return 0;
}
//最后一句，航迹推演放下位机去处理后，把坐标发上来，不要在ros做，可能自己水平不够，真是坑。。。
//odom发布频率，尽量高于40hz，太低了会影响cartographer定位效果，我用的80hz。–2021.7.10
957
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
