Integrate IMU+GPS for robot_localization

asked 2020-04-28 10:18:41 -0500

Elisa1697 gravatar image

Hi! I'm trying to integrate imu and gps in robot_localization using the navsat_node but I can't understand if my imu data are correct. I'm working with ros kinetic on ubuntu 16.04

My ground vehicle has:

  • ZED2 camera
  • Lidar tim781
  • board evb2 from inertial sense for gps,imu,magnetometer and it can provide the inertial navigation solution.

For now the vehicle does not have wheel encodeer. everything is mounted on a cart. So i use the linear velocities provided by the navigation solution on /ins topic.

My imu,gps,and ins have the same frame_id that is imu_link. Imu is placed Y(forward) X (left) Z (down) and it uses NED convention. The base_link is like REP-105 X(forward) Y(left) Z (up).

In my model there is also the robot_state_publisher to publish base_link->imu_link transform.

NOW the robot_localization uses ENU convention.

The standard for acceleration is:

Measure +9.81 meters per second squared for the Z axis. If the sensor is rolled +90 degrees (left side up), the acceleration should be +9.81 meters per second squared for the Y axis. If the sensor is pitched +90 degrees (front side down), it should read -9.81 meters per second squared for the X axis.

To conform to this, I create a new node to transform imu data.

new_imu_msg.linear acceleration.x = imu_sensor.linear_acceleration.y;
new_imu_msg.linear.acceleration.y=   imu_sensor.linear_acceleration.x;
new_imu_msg.linear.acceleration.z= - imu_sensor.linear_acceleration.z;

//and for angular_velocity:
new_imu_msg.angular_velocity,x =  imu_sensor,angular_velocity.y;
new_imu_msg.angular_velocity.y =  imu_sensor,angular_velocity.x;
new_imu_msg.angular_velocity.z = -imu_sensor,angular_velocity.z;

the frame_id of this new imu msg is imu_link_rot and is X(forward) Y(left) Z(up).

For the orientation, my imu sensor does not provide it so i use the imu_filter_madgwick with in input the new imu msg. 1) In the filter what convention should i use? enu or nwu) the imu_filter output it will be the input of navsat_node and it will have frame_id imu_link_rot.

for my robot localization:

  1. ekf_node_odom fuse visual odometry, imu for odom-base_link transformation
  2. ekf_node_map fuse visual odometry, imu , odometry/gps for map-odom transformation
  3. navasat_node:

    <node pkg="robot_localization" type="navsat_transform_node" name="test_navsat_node" respawn="true">

    <remap from="/odometry/filtered" to="/odometry/filtered_map"/> </node>

With this configuration in rviz i obtain the wrong movement. if i go forward with real robot, in rviz the model go backwards and and the initial yaw is every time wrong.

Probably it is due to imu data but i can't find the problem.

Is it correct my imu data preparation?

In the filter what should I set for world convention? enu or nwu?

Thank you in advice.

edit retag flag offensive close merge delete

Comments

Can you provide a sample messages for your sensor data? Maybe tell us which way the IMU was oriented when the data was captured.

Tom Moore gravatar image Tom Moore  ( 2020-06-24 02:19:14 -0500 )edit