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

2023-05-16

（二）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;
            }
            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/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

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)