欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

robot_pose_ekf 和IMU 使用配置

程序员文章站 2022-07-12 09:49:05
...

EKF

To integrating odometry information with IMU information and test the accuracy using bags from real experiments on husky/scout.

源码

git clone https://github.com/ros-planning/robot_pose_ekf.git

Ref:

robot_pose_ekf 源码解读1

robot_pose_ekf 源码解读2

贝叶斯滤波器包bfl 1

贝叶斯滤波器包bfl 2

配置

robot_pose_ekf.launch or test_robot_pose_ekf.launch

You need to change the topic and frame name

  1. /odometry_throttle: Throttle messages on odometry to a particular rate(2hz)
    topic_tools/throttle
  2. base_footprint_frame: make sure you have publish the tf tree between base_footprint_frame and imu’s frame like this<node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 base_link imu 100" />

[ WARN] [1601430080.460107326, 1601016468.862749824]: Could not transform imu message from /imu to base_link. Imu will not be activated yet.

  1. Change the topic name and path of bags like <remap from="odom" to="base_odometry/odom" /> <remap from="imu_data" to="torso_lift_imu/data" />
    is correspond to <node pkg="rosbag" name="rosbag" type="play" args="--clock -hz 100 --d .4 $(find robot_pose_ekf)/test/ekf_test2_indexed.bag" />

  2. The frequency of odom and imu needs to be the same(within 1s).

    [ERROR] [1601430156.351775252, 1601016452.792816901]: Timestamps of odometry and imu are 1.051269 seconds apart.

  <!-- Robot pose ekf -->
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
    <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="false"/>
    <param name="base_footprint_frame" value="base_link"/>
    <param name="output_frame" value="odom"/>

    <!-- <remap from="odom" to="base_odometry/odom" /> -->
    <!-- <remap from="imu_data" to="torso_lift_imu/data" /> -->

    <remap from="odom" to="/odometry" />
    <!-- <remap from="odom" to="/odometry_throttle" /> -->
    <remap from="imu_data" to="/imu/data" />
  </node>
  
  <node name="throttler1" type="throttle" pkg="topic_tools" args="messages /odometry 2/odometry_throttle" /> 

  <!-- <node pkg="rosbag" name="rosbag" type="play" args="-clock -hz 100 -d .4 $(find robot_pose_ekf)/test/ekf_test2_indexed.bag" /> -->
  <node pkg="rosbag" name="rosbag" type="play" args="--clock --hz 100 -d .4 /home/xywang/WORKSPACE/scout_cpc_ws/LOG/EKF/2020-09-25-14-47-30.bag" />
  
<node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 base_link imu 100" />
  

协方差参数的设置

[ERROR] [1587284286.565322789]: filter time older than odom message buffer
[ERROR] [1587284286.581064157]: Covariance specified for measurement on topic wheelodom is zero

If your odom and imu don’t have covariance, then add following code in odom_estimation_node.cpp

    // receive data 
    odom_stamp_ = odom->header.stamp;
    odom_time_  = Time::now();
    Quaternion q;
    tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
    odom_meas_  = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
    for (unsigned int i=0; i<6; i++)
      for (unsigned int j=0; j<6; j++)
        odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];

    if (odom_covariance_(1,1) == 0.0){//xinyi
      SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
      measNoiseOdom_Cov(1,1) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(2,2) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(3,3) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(4,4) = pow(0.007175,2);  // = 0.41 degrees / sec
      measNoiseOdom_Cov(5,5) = pow(0.007175,2);  // = 0.41 degrees / sec
      measNoiseOdom_Cov(6,6) = pow(0.007175,2);  // = 0.41 degrees / sec
      odom_covariance_ = measNoiseOdom_Cov;
    }
    // manually set covariance untile imu sends covariance
    if (imu_covariance_(1,1) == 0.0){
      SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
      measNoiseImu_Cov(1,1) = pow(0.00017,2);  // = 0.01 degrees / sec
      measNoiseImu_Cov(2,2) = pow(0.00017,2);  // = 0.01 degrees / sec
      measNoiseImu_Cov(3,3) = pow(0.00017,2);  // = 0.01 degrees / sec
      imu_covariance_ = measNoiseImu_Cov;
    }

Ref:

参考配置1

参考配置2

problem about tf tree

参考配置3

参考配置4

EKF 原理 & QR参数设置

I still have a problem with this part and if someone understands how to set Q and R in odom_estimation.cpp please feel free to tell me.

NonLinear System Model

    // create SYSTEM MODEL
    ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
    SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
    for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000,2);
    Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
    sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
    sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);

    // create MEASUREMENT MODEL ODOM
    ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
    SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
    for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
    Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
    Matrix Hodom(6,6);  Hodom = 0;
    Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
    odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
    odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);

    // create MEASUREMENT MODEL IMU
    ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;
    SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
    for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
    Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
    Matrix Himu(3,6);  Himu = 0;
    Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
    imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
    imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);

Ref:
EKF原理理解1
EKF原理理解2
参数理解
设置QR
problem about filter update

R measNoiseImu_Cov and measNoiseOdom_Cov

检查方法

  1. Troubleshooting
  2. I wrote a node to check the performance in rviz
<launch>
   <node pkg="robot_pose_ekf" type="showpath_robot_pose_ekf" name="showpath_robot_pose_ekf" output="screen" >
        <!-- <remap from="/odom" to="/UgvOdomTopic" /> -->
          <remap from="odom" to="/odometry" />
        <remap from="/combined_odom" to="/robot_pose_ekf/odom_combined" />
   </node>
   <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find model_test)/cfg/showpath_ekf.rviz"/>
</launch>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

ros::Publisher path_pub;
ros::Publisher path_pub2;
ros::Time current_time, last_time;
ros::Time current_time2, last_time2;
nav_msgs::Path path;
nav_msgs::Path path2;


void slam_odo_call_back(const nav_msgs::Odometry::ConstPtr &slam_odom) //for rplidar and carto
{

    current_time = ros::Time::now();

    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = slam_odom->pose.pose.position.x;
    this_pose_stamped.pose.position.y = slam_odom->pose.pose.position.y;

    this_pose_stamped.pose.orientation.x = slam_odom->pose.pose.orientation.x;
    this_pose_stamped.pose.orientation.y = slam_odom->pose.pose.orientation.y;
    this_pose_stamped.pose.orientation.z = slam_odom->pose.pose.orientation.z;
    this_pose_stamped.pose.orientation.w = slam_odom->pose.pose.orientation.w;


    this_pose_stamped.header.stamp=current_time;
    this_pose_stamped.header.frame_id="world";
    path.poses.push_back(this_pose_stamped);

    path_pub.publish(path);
    // ROS_INFO("success!");
    last_time = current_time;
}
void combined_odo_call_back(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &combined_odom)
{
    
    current_time2 = ros::Time::now();

    geometry_msgs::PoseStamped this_pose_stamped2;
    this_pose_stamped2.pose.position.x = combined_odom->pose.pose.position.x;
    this_pose_stamped2.pose.position.y = combined_odom->pose.pose.position.y;
    
    this_pose_stamped2.pose.orientation.x = combined_odom->pose.pose.orientation.x;
    this_pose_stamped2.pose.orientation.y = combined_odom->pose.pose.orientation.y;
    this_pose_stamped2.pose.orientation.z = combined_odom->pose.pose.orientation.z;
    this_pose_stamped2.pose.orientation.w = combined_odom->pose.pose.orientation.w;

    this_pose_stamped2.header.stamp=current_time2;
    this_pose_stamped2.header.frame_id="world";
    path2.poses.push_back(this_pose_stamped2);

    path_pub2.publish(path2);
    // ROS_INFO("success2!");
    last_time2 = current_time2;
}


main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
    path_pub = ph.advertise<nav_msgs::Path>("traj_slam",1, true);
    path_pub2 = ph.advertise<nav_msgs::Path>("traj_ekf",1, true);
    ros::Subscriber slam_odom_sub = ph.subscribe("/odom", 1, &slam_odo_call_back);
    ros::Subscriber combined_odom_sub = ph.subscribe("/combined_odom", 1, &combined_odo_call_back);

    current_time = ros::Time::now();
    last_time = ros::Time::now();

    path.header.stamp=current_time;
    path.header.frame_id="world";

    current_time2 = ros::Time::now();
    last_time2 = ros::Time::now();

    path2.header.stamp=current_time2;
    path2.header.frame_id="world";
    
    ros::spin();

    return 0;
}

参考使用

https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

Xsens MTI使用

基于ros melodic 及 MTI-G-710测试
官方教程
this works for me

rviz display
MTi 600-series MT manager does not work on ubuntu16.04. Need to use it on Windows