SLAM笔记——turtlebot传感器ekf实验实验

news2024/10/5 23:28:34

这里写目录标题

  • 实验内容
  • 实验准备
    • 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操纵机器人。可以看到红色为融合后的数据,绿色为带噪声的里程计。
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/173971.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

追梦之旅【数据结构篇】——对数据结构的认知 + 初识时间复杂度和空间复杂度~

详解C语言函数模块知识(下篇&#xff09;&#x1f60e;前言&#x1f64c;浅谈数据结构~&#x1f64c;1、什么是数据结构&#xff1f;(ˇˍˇ) 想&#xff5e;2、什么是算法&#xff1f;ˇˍˇ) 想&#xff5e;3、数据结构和算法的重要性&#x1f60a;4、如何才能学好数据结构呢…

初识 NodeJS(基于 Chrome V8 引擎的 JavaScript 运行时环境)

初识 NodeJS&#xff08;基于 Chrome V8 引擎的 JavaScript 运行时环境&#xff09;参考描述NodeJSNodeJS 可以做什么&#xff1f;特点用武之地获取检测运行JavaScript 运行时环境JavsScript 引擎浏览器中的 JavaScript 运行时环境Chrome 浏览器运行时环境NodeJS 中的 JavaScri…

【着色器实现海面效果_菲尼尔/Unlit/Transparent】

1.水体颜色 2.反射,水面波纹流动 3.折射、水底、水底透明度和折射 4.焦散,在水底接近岸边的水域 5.岸边泡沫,水花接近岸边的泡沫 6.水体运动,顶点动画 用灯光模式是Light Model :Unilt Render Type:Transparent 获取水面深度 利用这个节点,从深度图获取世界空间的位…

如何做流程图?这几个实用的制作流程图方法分享给你

说到流程图的制作&#xff0c;相信大家都并不陌生&#xff0c;在日常的工作和学习中&#xff0c;我们都会根据需求接触到各种各样的流程图&#xff0c;有时还要自己动手绘制流程图并使用&#xff0c;但你是否会因为不会绘制流程图而感到苦恼呢&#xff1f;没关系&#xff0c;今…

vue中利用ref实现更灵活的子向父传值

目录前言一&#xff0c;基础代码二&#xff0c;层次递进的讲解用法2.1 给子组件设置ref2.2 自定义事件2.3 给子组件设置一个自定义事件三&#xff0c;灵活性四&#xff0c;注意后记前言 目前我们熟知的子向父传值有两种方式&#xff1a; 一种是在父组件中定义函数&#xff0c;…

【AI】Windows配置GPU Cuda驱动和Pytorch框架

目录 1、Cuda驱动安装 1.1 查看显卡硬件 1.2 查看cuda版本 2、Annaconda python环境准备 2.1 创建pytorch_gpu 2.2 查看python版本 3、Pytorch和torchVsion软件安装 4、验证测试 在进行AI项目开发的时候&#xff0c;经常要在GPU环境中运行代码&#xff0c;对于没有配置…

动手深度学习-pytorch线性代数

标量简单操作长度向量简单操作长度其他操作矩阵简单操作乘法&#xff08;矩阵*向量&#xff09;乘法&#xff08;矩阵*矩阵&#xff09;范数取决于如何衡量b和c的长度常见范数矩阵范数&#xff1a;最小的满足的上面公式的值Frobenius范数特殊矩阵对称和反对称正定正交矩阵置换矩…

Solidity 中的数学(第 4 部分:复利)

本文是关于在 Solidity 中进行数学运算的系列文章中的第四篇。这次的主题是&#xff1a;复利。 介绍 在我们之前的文章中&#xff0c;我们讨论了百分比以及它们是如何在 Solidity 中计算的。在金融数学中&#xff0c;百分比通常与贷款和存款支付的利息有关。在每个时间段结束时…

深度学习入门基础CNN系列——批归一化(Batch Normalization)和丢弃法(dropout)

想要入门深度学习的小伙伴们&#xff0c;可以了解下本博主的其它基础内容&#xff1a; &#x1f3e0;我的个人主页 &#x1f680;深度学习入门基础CNN系列——卷积计算 &#x1f31f;深度学习入门基础CNN系列——填充&#xff08;padding&#xff09;与步幅&#xff08;stride&…

CSS 搜索框

CSS 搜索框 <!DOCTYPE html> <html><head><meta charset"utf-8"><title>搜索框</title><style type"text/css">* {margin: 0;padding: 0;}.search-container {margin: 50px;display: flex;width: 500px;height:…

rtthread pwm

1、配置PWM设备驱动相关宏定义 1.1 配置PWM和TIM设备驱动 在RT-Thread Settings 配置界面中&#xff0c;在设备驱动程序目录下勾选 HWTIMER 和 PWM设备驱动程序&#xff0c;如下图所示&#xff1a; 配置完后在 rtconfig.h 中可以查看刚刚配置的 RT_USING_HWTIMER 和 RT_U…

Ribbon策略改变实现步骤(基于Eureka服务注册中心)

前言 Ribbon作为服务调用的作用&#xff0c;自带的默认负载均衡机制是轮询机制也就是轮流访问机制。当然有时候并不是业务上都需要这种机制&#xff0c;这时候就需要改变。 机制类型 RoundRobinRule 轮询 RandomRule 随机 RetryRule 先进行轮询策略获取服务&#xff0c;如…

本地怎么画最简单的监控图

当我们需要监测程序是否正常运行&#xff0c;或者需要观测程序运行是否平稳&#xff0c;或者需要知道特定场景下指标的状态时&#xff0c;那我们都需要将监控可视化。今天我简单记录下&#xff0c;本地是如何监控可视化~ 目录简单调试阶段——仅使用prometheus1.安装promethues…

【Redis】GEO(地理坐标)数据结构

【Redis】GEO(地理坐标)数据结构 文章目录【Redis】GEO(地理坐标)数据结构1. GEO简介1.1 GEO常用命令2. 实战示例2.1 场景介绍2.2 实战2.2.1 将店铺数据导入Redis2.2.2 根据地理坐标进行分页查询1. GEO简介 GEO就是Geolocation的简写形式&#xff0c;代表地理坐标。Redis在3.2…

BFS判重和双向广搜

目录 一、前言 二、BFS判重 1、set 判重 2、字典判重 3、跳蚱蜢&#xff08;2017年省赛&#xff0c;lanqiaoOJ题号642&#xff09; &#xff08;1&#xff09;字典去重、用 list 实现队列 &#xff08;2&#xff09;set() 去重、用 list 实现队列 &#xff08;3&#x…

setDaemon python守护进程,队列通信子线程

使用setDaemon()和守护线程这方面知识有关&#xff0c; 比如在启动线程前设置thread.setDaemon(True)&#xff0c;就是设置该线程为守护线程&#xff0c;表示该线程是不重要的,进程退出时不需要等待这个线程执行完成。这样做的意义在于&#xff1a;避免子线程无限死循环&#x…

IK分词工具

https://code.google.com/archive/p/ik-analyzer/ IK Analyzer是一个开源的&#xff0c;基于java语言开发的轻量级的中文分词工具包。从2006年12月推出1.0版开始&#xff0c; IKAnalyzer已经推出了4个大版本。最初&#xff0c;它是以开源项目Luence为应用主体的&#xff0c;结合…

ruoyi-vue版本(八)登陆页面的验证码是咋实现的

目录1 需求2 配置类3 逻辑1 需求 我们打开若依项目的登陆页面&#xff0c;看到有一个验证码功能&#xff0c;点击一下这个验证码&#xff0c;还会进行变换验证码&#xff0c;那么这个逻辑是咋实现的&#xff1b; 我们刚进这个页面&#xff0c;其实就调用了一个接口&#xff1…

整理指针相关练习

这里收录的是相关指针的练习&#xff0c;主要针对的是指针与sizeof之间的练习&#xff0c;练完你对指针的理解将更进一层喔一维数组指针练习字符数组指针练习二维数组指针练习练习总结&#xff1a;指针笔试真题一维数组指针练习 一维数组相关练习&#xff0c;下面答案是多少呢…

负载均衡的在线OJ

文章目录1.项目宏观结构(1)三个模块(2)项目宏观结构(3)编写顺序2.compile_server(1)compiler.hpp(2)runner.hpp(3)compile_run.hpp(4)compile_server.cc(5)Makefile(6)temp(7)编译运行模块总结3.comm(1)util.hpp(2)log.hpp(3)httplib.h4.基于MVC结构的OJ服务设计(oj_server)(1)…