robot pose ekf

参考链接

• github源码 navigation/robot_pose_ekf at kinetic-devel · ros-planning/navigation · GitHub
• 创客智造教程 ROS与navigation教程-robot_pose_ekf - 创客智造
• csdn修改参考ROS下robot_pose_ekf扩展卡尔曼融合包的使用_ros efk 使用-优快云博客
•                       robot_pose_ekf初始化odometry协方差矩阵_如何为ros的ekf设置协方差-优快云博客
• csdn源码分析参考robot_pose_ekf源码解读_bfl库四元数-优快云博客
• ros wiki robot_pose_ekf - ROS Wiki

硬件，运行环境等

笔者使用的是创客智造中diy arduino 底盘的方式，相关信息见链接搭建ROS小车底盘A - 创客智造

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

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

虚拟机软件：vmware

版本：Ubuntu16.04lts + kinetic

底盘控制使用ros arduino birdge 参见链接：ros_arduino_bridge - ROS Wiki

imu使用 saprkfun 的9250版imu 参见链接9DoF Razor IMU M0 Hookup Guide - SparkFun Learn

教程参见链接：razor_imu_9dof - ROS Wiki

有关

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中安装包，并编译后可进行测试和矫正。

现在进入正题：

1. 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_COVARIANCE2
        odom.twist.covariance = ODOM_TWIST_COVARIANCE2
else:
        odom.pose.covariance = ODOM_POSE_COVARIANCE
        odom.twist.covariance = ODOM_TWIST_COVARIANCE

  self.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的源码理解

文件结构