Ask Your Question
0

3D pose with IMU and odometry using robot localization

asked 2021-02-03 14:07:53 -0600

alduxvm gravatar image

updated 2021-02-23 03:58:12 -0600

Tom Moore gravatar image

Hi all!

My objective is simple, yet I haven't been able to successfully tackle it. I have a small robot, it is a 4wd robot, 4 motors, 4 encoders, but it behaves like a differential robot actually. Odometry from encoders is working well (except when one of the wheels gets stuck in the terrain and starts to slip...). I'm using ROS Melodic on ubuntu 18.04.

You can see my robot here:

4wd robot

I want to fuse odom, with an IMU and later with gps in order to get a nice 3d pose, not just 2d.

So, I started with odom + imu (as suggested in all r_l guides) in 2d. I actually have 2 imus: * imu0 -> swift piksi multi via ethernet to my robot computer at 25hz * imu2 -> mpu 6050 at 25hz (flight controller naze32)

The axis of the IMUs align physically with the robot odometry, x forward... and bla bla, as showed in this image:

imu axis

This is a sensor sample for the odom, and both imus:

MPU6050

Piksi multi

Odometry

Of course, I follow the instructions to check gravity and the signs. My ekf launch files looks like this:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />

    <param name="frequency" value="20." />  
    <param name="sensor_timeout" value="0.2" />  
    <param name="publish_tf" value="true" />
    <!-- <param name="map_frame" value="map" /> -->
    <param name="odom_frame" value="odom" />
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="odom" />
    <param name="print_diagnostics" value="true" />
    <param name="debug" value="true" />

    <param name="use_control" value="false" />
    <param name="two_d_mode" value="true" />

    <param name="odom0" value="odom_front"/>
    <param name="imu0" value="/imu2/data"/> 

    <rosparam param="odom0_config">[true,  true,  false, 
                                  false, false, true, 
                                  true, true, false, 
                                  false, false, true,
                                  false, false, false]</rosparam>                                                                   

    <rosparam param="imu0_config">[false, false, false, 
                                 true, true, true,
                                 true, true, true, 
                                 true, true, true,
                                 true, true, true]</rosparam>

    <param name="odom0_differential" value="true"/> 
    <param name="imu0_differential" value="true"/>

    <param name="odom0_queue_size" value="10"/>
    <param name="imu0_queue_size" value="10"/>

   <param name="imu0_remove_gravitational_acceleration" value="true" /> 

    <rosparam param="process_noise_covariance">[0.05, 0,    0, 0, 0, 0, 0, 0, 0, 0, 0, 0,    0, 0, 0,
                                               0, 0.05, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0.06, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0.03, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0, 0.03, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0, 0, 0.06, 0, 0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0, 0, 0, 0.025,0, 0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0, 0, 0, 0, 0.025,0,  0, 0, 0, 0, 0, 0,
                                               0, 0, 0, 0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-02-23 04:10:37 -0600

Tom Moore gravatar image

There are a bunch of things you'll want to change here.

  1. Your IMU specification says you are measuring all orientation variables, all linear velocity variables, all angular velocity variables, and all linear acceleration variables. However, if you look at your IMU message, linear velocity is not a quantity that your IMU can provide. So the 7th, 8th, and 9th values (I'm not indexing from 0 here) in that specification should be false.
  2. Following from that, you don't have anything measuring Z or Z velocity. You have something measuring Z acceleration, but that means your only reference for Z (without the IMU, anyway) is from acceleration, which is (a) noisy and (b) has to be integrated twice to get position. If I were you, I would fuse your Z velocity from your wheel encoders instead. The value will be 0, but this is accurate, as your robot cannot fly. :) But the general rule of thumb for the EKF is that you need to provide a reference (measurement) for every pose variable or its velocity. This can be a challenge when you move to 3D.
  3. You are fusing absolute yaw from your wheel encoders _and_ your IMU. This won't go well. They aren't going to agree, so the estimate will jump back and forth.
  4. Even with that, though, your yaw from your IMU will be subject to drift, because it comes from a magnetometer, and you appear to be inside. Just be aware that metal, including metal used in building construction, can have strong effects on magnetometers.
  5. I assume you have a base_link->imu transform specified somewhere? If not, the IMU data will just be ignored.

I recommend this for your setup:

    <rosparam param="odom0_config">[false, false, false, 
                                    false, false, false, 
                                    true,  true,  true, 
                                    false, false, true,
                                    false, false, false]</rosparam>                                                                   

    <rosparam param="imu0_config">[false, false, false, 
                                   true,  true,  true,
                                   false, false, false, 
                                   true,  true,  true,
                                   true,  true,  true]</rosparam>

Also, if you are still struggling, try not fusing acceleration until the rest is working.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-02-03 14:07:53 -0600

Seen: 36 times

Last updated: Feb 23