参考链接

github源码 https://github.com/ros-planning/navigation/tree/kinetic-devel/robot_pose_ekf
创客智造教程 https://www.ncnynl.com/archives/201708/1909.html
csdn修改参考https://blog.csdn.net/shenghuaijing3314/article/details/78220151
https://blog.csdn.net/xiekaikaibing/article/details/80403264
csdn源码分析参考https://blog.csdn.net/zhxue_11/article/details/83828877
ros wiki http://wiki.ros.org/robot_pose_ekf

硬件，运行环境等

笔者使用的是创客智造中diy arduino 底盘的方式，相关信息见链接https://www.ncnynl.com/category/ros-car/

上位机没有使用树莓派，而是使用了如下：

联想笔记本 i5 6267u 8g 内存

虚拟机软件：vmware

版本：Ubuntu16.04lts + kinetic

底盘控制使用ros arduino birdge 参见链接：http://wiki.ros.org/ros_arduino_bridge

imu使用 saprkfun 的9250版imu 参见链接https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide

教程参见链接：http://wiki.ros.org/razor_imu_9dof

有关

Go to "Tools" → "Board" and select "Arduino Pro or Pro Mini (3.3v, 8mhz) w/ATmega328". Note: in Aduino 1.5+, the board menu doesn't allow selecting the voltage/frequency; go to the Processor menu after selecting "Arduino Pro or Pro Mini" and select "ATMega 328 (3.3V, 8Mhz)"

这里板子的型号应为arduino zero 笔者Ubuntu中arduino IDE 无法添加新的开发板和库，故在windows环境下完成固件的烧写工作；

（某宝买的板子micro USB 口相当不结束，直接就掉下来了，还好找人把他搞上去了，吐槽一句就不能用那种带固定引脚的口么）

选择：

Caution: choose the correct Razor hardware revision when compiling and uploading the firmware to the board. The setting is located in Razor_AHRS.ino under "HARDWARE OPTIONS"!

更新IMU固件，并在ROS中安装包，并编译后可进行测试和矫正。

现在进入正题：

robot_pose _ekf的使用

修改robot_pose _ekf的launch文件，

仅使用里程计和IMU，故将VO参数改为false，

修改output_frame为odom_combined，

修改base_footprint_frame为base_link

重映射robot_pose_ekf/odom_combined到odom_combined

<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_link"/><remap from="robot_pose_ekf/odom_combined" to="odom_combined"/><param name="freq" value="30.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="flase"/><param name="debug" value="true"/></node></launch>

首先启动底盘节点，启动键盘控制包测试底盘是否正常运行，使用rostopic命令查看话题发布和里程计信息，查看tf tree ,可以看到，此时发布tf的节点为arduino

启动IMU，启动robot_pose_ekf节点，发现此时出现两个错误，即wiki中的两个error：

Covariance specified for measurement on topic xxx is zero

这里的xxx为odom，这是因为ros arduino birdge中并没有发布odom的协方差矩阵，所以在这里我们在base controller文件添加Covariance的发布代码

ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3]
ODOM_POSE_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9]ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3]
ODOM_TWIST_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9]

以及

......................... odom.twist.twist.angular.z = vth# todo sensor_state.distance == 0
if self.v_des_left == 0 and self.v_des_right == 0:odom.pose.covariance = ODOM_POSE_COVARIANCE2odom.twist.covariance = ODOM_TWIST_COVARIANCE2
else:odom.pose.covariance = ODOM_POSE_COVARIANCEodom.twist.covariance = ODOM_TWIST_COVARIANCEself.odomPub.publish(odom)...........................

此时里程计相关的协方差矩阵为0错误消失

（这里使用的IMU包已经发布了协方差，故不存在错误，有相关错误的可以参考上面的修改文章）

还有一点需要注意的是，我们需要让robot pose ekf 来发布odom到base link或footprint的tf，解决方案是屏蔽base controller中相关的tf 如图

            quaternion.w = cos(self.th / 2.0)#           # Create the odometry transform frame broadcaster.#           self.odomBroadcaster.sendTransform(#               (self.x, self.y, 0), #               (quaternion.x, quaternion.y, quaternion.z, quaternion.w),#               rospy.Time.now(),#               self.base_frame,#               "odom"#               )odom = Odometry()

此时运行rqt tf tree以及rqt graph即可看到正确的node topic 以及tf tree

使用rqt plot可以绘制相应的坐标，更好的办法是将数据导入到matlab中，详细分析。

第一次修改：将imu相关的tf静态发布

将以下代码添加到arduino.launch 中，注意添加的位置

<node name="base_imu_link" pkg="tf" type="static_transform_publisher" args="0 0 0 0 3.1415926 0  /base_link /base_imu_link 50"/>

添加成功后运行

rosrun robot_pose_ekf wtf.py

查看odom 和imu 的运行状态

2.robot_pose _ekf的源码理解

文件结构

robot pose ekf相关推荐

SLAM笔记——turtlebot传感器ekf实验实验
这里写目录标题实验内容实验准备 msg数据类型给uwb和odom增加噪声 robot_pose_ekf 发布路径实验结果实验内容本实验将在gazebo仿真环境中使用ekf进行传感器数据融合 ...
ros ekf融合odom imu ov信息
1.这里是ros_ekf_pose包的简单介绍: 这个包用于评估机器人的3D位姿,使用了来自不同源的位姿测量信息,它使用带有6D(3D position and 3D orientation)模型信息 ...
ekf 传感器数据融合
robot_pose_ekf.launch 默认配置: 2d odom imu vo <launch> <node pkg="robot_pose_ekf&quo ...
ROS与navigation教程——概述
navigation是ROS的二维导航功能包,简单来说,就是根据输入的里程计等传感器的信息流和机器人的全局位置,通过导航算法,计算得出安全可靠的机器人速度控制指令. 代码库:https://githu ...
AMCL定位融合UWB
2021/12/26,实验效果不好,发现多传感器融合好像不像我想得那么简单,还不太懂- 2021/12/27,一个大问题ekf并没有融合odom,odom_combined发布的数据和uwb(gps) ...
智能小车传感器（摄像头、雷达、红外传感器、RFID、AGV、IMU）总结及数据获取打印（ROS、C++）
Camera 1.简介: 摄像头由镜头.镜头模组.滤光片.CMOS/CCD.ISP.数据传输部分组成.光线经过光学镜头和滤光片后聚焦到传感器上,通过 CMOS 或 CCD 集成电路将光信号转换成电 ...
[学习SLAM]基于EKF使用 robot_pose_ekf 对imu和odom进行融合定位（robot_pose_ekf）篇章 (2)
使用 robot_pose_ekf 对imu和odom进行融合 robot_pose_ekf 是 ROS Navigation stack 中的一个包,通过扩展卡尔曼滤波器对 imu.里程计 odom ...
【全局地图mapping】二维全局地图的开源方案及对比--gmapping、hector、karto、cartographer
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.gmapping算法建图[基于Raoo-Blackwellize ...
EKF-SLAM simulated in MATLAB
Reference: Simulataneous localization and mapping with the extended Kalman filter - `A very quick gu ...

robot pose ekf

参考链接

硬件，运行环境等

教程参见链接：http://wiki.ros.org/razor_imu_9dof

robot_pose _ekf的使用

2.robot_pose _ekf的源码理解

robot pose ekf相关推荐

最新文章

热门文章