（二）IMU采集、过滤，利用EKF将IMU与odom融合，发布新的odom话题

A、ROS采集节点
B、imu_tools过滤imu数据
C、使用 robot_pose_ekf 对imu和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;}elsecrc=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/sfloat delta_dist, delta_speed, delta_x, delta_y, pos_x, pos_y; //单位：msigned 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 topicodom_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/s²，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

ROS小车记录系列（二）IMU采集、过滤，与odom数据融合，发布新的odom话题相关推荐

ROS小车记录系列（四）利用cartographer定位，修改源码发布pose话题
(四)利用cartographer定位 __半个月没有更新了,一直在解决建图效果问题,因为是用的自己的IMU,误差有点大(-_-!!!),陆续买了两个imu,误差依旧大,转几圈都找不到北了.加上我的o ...
ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境
前言:该篇是ROS学习记录的第二篇,如果还没关注过之前文章的读者,如有需要可以通过下方传送门去看之前的文章: ROS学习记录(一) Plugin插件这两天关注了古月老师的公众号,看到了其中一篇课程推 ...
Android 音频开发（二）采集一帧音频数据
这一节主要介绍如何采集一帧音频数据,如果你对音频的基础概念比较陌生,建议看我的上一篇Android 音频开发(一) 基础入门篇.因为音频开发过程中,经常要涉及到这些基础知识,掌握了这些重要的基础知识后 ...
ROS学习记录（二）玩转海龟——海龟保姆级教程
前言--环境变量那些事儿编译工作空间后,一定要先设置环境变量,此工作空间下的代码才能有效运行在工作空间目录下打开终端,输入 source devel/setup.bash 此次source仅对当前 ...
Hadoop运维记录系列(二十二)
今天下午写了一会代码,然后帮同事解决了一个hbase相关的故障分析,定位了问题根源,觉得比较有代表性,记录一下. 先说一下问题的发生与背景. 这个故障其实是分为两个故障的,第一个比较简单,第二个相对复 ...
【python+ROS+路径规划】二、理解并处理地图数据
目前打算使用python写出一个Astar的全局路径算法,总体分为三个部分:接收地图数据,设计路径(当然是顺过来的),发布路径. 文章目录一.建立功能包二.接受地图数据(处理上游) 查看地图发布的 ...
从零开始学ros小车仿真
从零开始学ros小车仿真从零开始学ros小车仿真目录 1.从零开始学ros小车仿真(一)在solidworks中建模小车并转出为urdf文件 2.从零开始学ros小车仿真(二)在rviz中检验导入 ...
postek二次开发_使用PX4的ECL进行多传感器数据融合的后处理
写在前边ecl是开源无人机项目PX4使用的算法库,使用ekf(扩展卡尔曼滤波)进行imu等多种传感器的数据融合然而ecl不提供数据后处理功能能使用ecl进行多传感器数据融合的后处理是很有必要的,这 ...
学习ROS topic_tools/throttle并发布指定名字的话题
最近开始学习ORB-SLAM3,按照官方文档运行EuRoc的rosbag测试时,发现rosbag里的话题名(/cam0/image_raw和/cam1/image_raw)和ORB-SLAM3源码中订 ...

ROS小车记录系列（二）IMU采集、过滤，与odom数据融合，发布新的odom话题

（二）IMU采集、过滤，利用EKF将IMU与odom融合，发布新的odom话题

A、ROS采集节点

B、imu_tools过滤imu数据

C、使用 robot_pose_ekf 对imu和odom进行融合

ROS小车记录系列（二）IMU采集、过滤，与odom数据融合，发布新的odom话题相关推荐

最新文章

热门文章