这里写目录标题
- 实验内容
- 实验准备
- msg数据类型
- 给uwb和odom增加噪声
- robot_pose_ekf
- 发布路径
 
- 实验结果
实验内容
本实验将在gazebo仿真环境中使用ekf进行传感器数据融合。本文使用turtlebot3进行实验,turtlebot本身会发布odom和imu。imu的误差可以在urdf文件中进行调整,但是gazebo提供的odom却是完美的,因此我们需要手动给其添加误差,这个误差是累积的。另外我们还将提供一个全局的定位观测(通常为gps或uwb),这个我们将直接获取机器人的真实位姿并加上误差。
 实验代码在github中,里面robot_pose_ekf是和官网上有点不同的(增加了gps,并且取消了发布odom的tf,因为会冲突)。sensor_data_processing则是实验内容。
实验准备
msg数据类型
首先我们对传感器的消息类型要有所了解,需要用到的基本上都在下面三个包中:
 geometry_msgs最基本的包,包含了速度、加速度、角速度、角加速度、位姿的各种表示方法等多种基本的消息类型,被其他自定义数据类型包裹
 sensor_msgs提供了imu,可以看到里面包含了四元数表示的旋转、角速度、三轴加速度以及各自的误差矩阵。其中会被用到的其实只有旋转,这也说明这个程序中imu无法单独使用。
 nav_msg提供了odom,odom有位姿pose和速度twist两个数据,被用到的只有pose。
 gps/uwb只能提供位置信息,没有旋转信息,我们还是使用odom类型,但是把误差矩阵后三项和旋转有关的都置一个很大的数字。
给uwb和odom增加噪声
gazebo中给的odom是完美的,所以我们需要给它增加噪声,这个噪声是累加的,因此我们需要记录前一次odom数据做差,加上噪声融入到新的odom_noise中去。另外,噪声我设置xy轴一样,theta单独误差,可以通过launch配置。并且只有在机器人移动时才增加误差,避免漂移,平移时增加xy方向误差,旋转时增加theta误差。
#include"ros/ros.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"sensor_msgs/Imu.h"
#include"tf2/LinearMath/Quaternion.h"
#include"tf2/LinearMath/Matrix3x3.h"
// boost
#include"boost/thread/thread.hpp"
class NoiseAdder
{
public:
    NoiseAdder(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
    :nh_(nh),
    private_nh_(private_nh)
    {
        // 初始化参数
        if(!private_nh_.getParam("uwb_noise",uwb_std))
            uwb_std=0.1;
        if(!private_nh_.getParam("odom_noise_xy",odom_noise_xy))
            odom_noise_xy=0.05;
        if(!private_nh_.getParam("odom_noise_theta",odom_noise_theta))
            odom_noise_theta=0.01;
        // 订阅odom
        odom_sub = nh_.subscribe("odom", 10, &NoiseAdder::odomCallback, this);
        // 订阅imu(为了时间同步)
        imu_sub = nh_.subscribe("imu", 10, &NoiseAdder::imuCallback, this);
        // 发布odom_noise
        odom_noise_pub = nh_.advertise<nav_msgs::Odometry>("odom_noise", 10);
        // 发布uwb
        uwb_noise_pub = nh_.advertise<nav_msgs::Odometry>("uwb", 10);
        // 发布imu
        imu_pub = nh_.advertise<sensor_msgs::Imu>("imu_fake", 10);
    }
    // imu回调函数
    void imuCallback(const sensor_msgs::Imu::ConstPtr &imuu)
    {
        // 上锁
        boost::mutex::scoped_lock lock(mutex);
        // copy imu
        imu = *imuu;
        publish();
    }
    // odom回调函数
    void odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
    {
        // 上锁
        boost::mutex::scoped_lock lock(mutex);
        // 判断是否为第一次收到
        if(first_odom)
        {
            last_odom=*odom;
            odom_noise=*odom;
            first_odom=false;
            return ;
        }
        // 发布uwb_noise
        uwb_noise.header = odom->header;
        uwb_noise.child_frame_id = odom->child_frame_id;
        uwb_noise.pose.pose.position.x = odom->pose.pose.position.x + gaussianNoise(0, uwb_std);
        uwb_noise.pose.pose.position.y = odom->pose.pose.position.y + gaussianNoise(0, uwb_std);
        uwb_noise.pose.pose.position.z = odom->pose.pose.position.z;
        uwb_noise.pose.pose.orientation.w = 1.0;
        // 修改协方差
        uwb_noise.pose.covariance[0] = uwb_std;
        uwb_noise.pose.covariance[7] = uwb_std;
        uwb_noise.pose.covariance[14] = 0.000001;
        uwb_noise.pose.covariance[21] = 9999999;
        uwb_noise.pose.covariance[28] = 9999999;
        uwb_noise.pose.covariance[35] = 9999999;
        uwb_noise_pub.publish(uwb_noise);
        // 判断与上一次有没有运动,eps表示误差
        if (fabs(odom->pose.pose.position.x - last_odom.pose.pose.position.x) < eps &&
            fabs(odom->pose.pose.position.y - last_odom.pose.pose.position.y) < eps &&
            fabs(odom->pose.pose.orientation.w - last_odom.pose.pose.orientation.w) < eps)
        {
            ROS_INFO("Stop");
            last_odom = *odom;
            odom_noise_pub.publish(odom_noise);
            return;
        }
        
        ROS_INFO("Move");
        odom_noise.header = odom->header;
        odom_noise.child_frame_id = odom->child_frame_id;
        // 如发生平移,位置累加噪声
        if(fabs(odom->pose.pose.position.x - last_odom.pose.pose.position.x) > eps ||
            fabs(odom->pose.pose.position.y - last_odom.pose.pose.position.y) > eps )
        {
            odom_noise.pose.pose.position.x = odom_noise.pose.pose.position.x + (odom->pose.pose.position.x-last_odom.pose.pose.position.x) + gaussianNoise(0, odom_noise_xy);
            odom_noise.pose.pose.position.y = odom_noise.pose.pose.position.y + (odom->pose.pose.position.y-last_odom.pose.pose.position.y) + gaussianNoise(0, odom_noise_xy);
            odom_noise.pose.pose.position.z = odom->pose.pose.position.z;
        }
        // 添加姿态噪声
        tf2::Quaternion q;
        q.setX(odom->pose.pose.orientation.x);
        q.setY(odom->pose.pose.orientation.y);
        q.setZ(odom->pose.pose.orientation.z);
        q.setW(odom->pose.pose.orientation.w);
        double roll, pitch, yaw;
        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
        tf2::Quaternion q_last;
        q_last.setX(last_odom.pose.pose.orientation.x);
        q_last.setY(last_odom.pose.pose.orientation.y);
        q_last.setZ(last_odom.pose.pose.orientation.z);
        q_last.setW(last_odom.pose.pose.orientation.w);
        double roll_last, pitch_last, yaw_last;
        tf2::Matrix3x3(q_last).getRPY(roll_last, pitch_last, yaw_last);
        // 计算和上一帧累计误差
        double delta_yaw = yaw - yaw_last;
        // 如发生旋转,则增加旋转误差
        if(fabs(delta_yaw)> eps)
        {
            tf2::Quaternion q_noise;
            q_noise.setX(odom_noise.pose.pose.orientation.x);
            q_noise.setY(odom_noise.pose.pose.orientation.y);
            q_noise.setZ(odom_noise.pose.pose.orientation.z);
            q_noise.setW(odom_noise.pose.pose.orientation.w);
            double roll_noise, pitch_noise, yaw_noise;
            tf2::Matrix3x3(q).getRPY(roll_noise, pitch_noise, yaw_noise);
            //odom_noise旋转delta_yaw
            yaw_noise += delta_yaw + gaussianNoise(0, odom_noise_theta);
            // RPY转四元数
            q_noise.setRPY(roll_noise, pitch_noise, yaw_noise);
            odom_noise.pose.pose.orientation.x = q_noise.x();
            odom_noise.pose.pose.orientation.y = q_noise.y();
            odom_noise.pose.pose.orientation.z = q_noise.z();
            odom_noise.pose.pose.orientation.w = q_noise.w();
            // 修改协方差
            odom_noise.pose.covariance[0] = odom_noise_xy;
            odom_noise.pose.covariance[7] = odom_noise_xy;
            odom_noise.pose.covariance[14] = 0.000001;
            odom_noise.pose.covariance[21] = 9999999;
            odom_noise.pose.covariance[28] = 9999999;
            odom_noise.pose.covariance[35] = odom_noise_theta;
        }
        // 更新odom
        last_odom = *odom;
    }
    // 生成高斯噪声
    double uniform_rand(double lowerBndr, double upperBndr)
    {
        return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
    }
    double gaussianNoise(double mean, double sigma)
    {
        double x, y, r2;
        do {
            x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
            y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
            r2 = x * x + y * y;
        } while (r2 > 1.0 || r2 == 0.0);
        return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
    }
    // 发布所有数据,并同步时间戳
    void publish()
    {
        if(first_odom)
        {
            return;
        }
        // 发布imu
        imu.header.stamp = ros::Time::now();
        imu_pub.publish(imu);
        // 发布odom_noise
        odom_noise.header.stamp = ros::Time::now();
        odom_noise_pub.publish(odom_noise);
        // 发布uwb_noise
        uwb_noise.header.stamp = ros::Time::now();
        uwb_noise_pub.publish(uwb_noise);
    }
private:
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::Subscriber odom_sub;
    ros::Subscriber imu_sub;
    ros::Publisher odom_noise_pub;
    ros::Publisher uwb_noise_pub;
    ros::Publisher imu_pub;
    nav_msgs::Odometry odom_noise;
    nav_msgs::Odometry uwb_noise;
    sensor_msgs::Imu imu;
    // 上一次odom
    nav_msgs::Odometry last_odom;
    // eps
    const double eps = 0.005;
    // 噪声大小
    double uwb_std;
    double odom_noise_xy;
    double odom_noise_theta; 
    // 互斥锁
    boost::mutex mutex;
    // 判断是否为第一次收到odom
    bool first_odom = true;
};
int main(int argc,char**argv)
{
    ros::init(argc, argv, "add_noise");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");
    NoiseAdder noise_adder(nh, private_nh);
    ros::spin();
    return 0;
}
robot_pose_ekf
首先我们需要安装robot_pose_ekf。
 我们直接看launch文件,这个包只支持三种传感器(odom、imu和vo)其中imu类型对应imu,而odom和vo都对应odom。因为gps/uwb只提供位置,这个页面说可以用vo姿态协方差填99999,但是我失败了。我们这里是通过修改代码可以直接增加gps,可以看我的github链接,里面有改过的robot_pose_ekf包。
<launch>
  <!-- 发布传感器 -->
  <node pkg="sensor_data_processing" type="add_noise" name="noise_publisher" >
    <param name="uwb_noise" value="0.1" />
    <param name="odom_noise_xy" value="0.05" />
    <param name="odom_noise_theta" value="0.1" />
  </node>
  <!-- odom_combined和odom_noise到odom静态变换 -->
  <node pkg="tf" type="static_transform_publisher" name="odom_combined_to_odom" args="0 0 0 0 0 0 odom odom_combined 100" />
  <node pkg="tf" type="static_transform_publisher" name="odom_noise_to_odom" args="0 0 0 0 0 0 odom odom_noise 100" />
  <!-- Robot pose ekf 拓展卡尔曼滤波-->
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
    <param name="output_frame" value="odom_combined"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="2.0"/>
    <param name="odom_used" value="false"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="true"/>
    <remap from="imu_data" to="imu_fake" />
    <remap from="odom" to="odom_noise" />
    <remap from="gps" to="uwb" />
  </node>
</launch>
发布路径
// 本文件会发布odom和EKF修正后的odom的路径
#include"ros/ros.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"geometry_msgs/PoseWithCovarianceStamped.h"
// path
#include"nav_msgs/Path.h"
// odom路径
nav_msgs::Path odom_path;
// odom_combined路径
nav_msgs::Path odom_combined_path;
// publisher和subscriber
ros::Publisher odom_path_pub;
ros::Publisher odom_combined_path_pub;
ros::Subscriber odom_sub;
ros::Subscriber odom_combined_sub;
// odom回调函数
void odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
{
    // 将odom插入odom_path
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header = odom->header;
    pose_stamped.pose = odom->pose.pose;
    odom_path.poses.push_back(pose_stamped);
    // 发布odom_path
    odom_path_pub.publish(odom_path);
    // sleep
    ros::Duration(0.3).sleep();
    ROS_INFO("odom_path size: %d",odom_path.poses.size());
}
// odom_combined回调函数
void odomCombinedCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &odom_combined)
{
    // 将odom_combined插入odom_combined_path
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header = odom_combined->header;
    pose_stamped.pose = odom_combined->pose.pose;
    odom_combined_path.poses.push_back(pose_stamped);
    // 发布odom_combined_path
    odom_combined_path_pub.publish(odom_combined_path);
    // sleep
    ros::Duration(0.3).sleep();
    ROS_INFO("odom_combined_path size: %d",odom_combined_path.poses.size());
}
int main(int argc,char **argv)
{
    ros::init(argc,argv,"publish_path");
    ros::NodeHandle nh;
    // 初始化odom_path
    odom_path.header.frame_id = "odom";
    odom_path.poses.clear();
    // 初始化odom_combined_path
    odom_combined_path.header.frame_id = "odom";
    odom_combined_path.poses.clear();
    odom_path_pub = nh.advertise<nav_msgs::Path>("odom_path",10);
    odom_combined_path_pub = nh.advertise<nav_msgs::Path>("odom_combined_path",10);
    odom_sub = nh.subscribe("odom_noise",10,odomCallback);
    odom_combined_sub = nh.subscribe("/robot_pose_ekf/odom_combined",10,odomCombinedCallback);
    ros::spin();
    return 0;
}
将融合前融合后的odom分别用nav_msgs/Path来记录,并在rviz中显示出来。
实验结果
执行roslaunch sensor_data_processing create_environment.launch 用于创建环境
 执行roslaunch sensor_data_processing sensors_ekf.launch 用于发布数据
 可以看到,noise_publisher将gazebo发布的odom增加噪声后发布了odom_noise,并且创造了uwb这个数据,imu_fake则是为了控制时间戳一致。三者被ekf融合输出了odom_combined
 
 执行rosrun sensor_data_processing publish_path 
 使用rosrun teleop_twist_keyboard teleop_twist_keyboard.py操纵机器人。可以看到红色为融合后的数据,绿色为带噪声的里程计。
 



















