navsat_transform_node odometry estimate looks to be rotated differently from local map frame, resulting in incorrect estimation

asked 2020-08-26 16:59:09 -0500

mfader18
31 ●1 ●1 ●6

Hello all,

I am trying to get robot_localization and navsat_transform implemented on a Husky UGV to do outdoor navigation and control and I am running into problems with the odometry output from navsat_transform as it does not properly estimate the vehicle position within the local map frame relative to where the robot actually drove. This estimate in (x,y) will change depending on the direction the vehicle is facing before driving as if the vehicle orientation relative to the UTM frame is being accounted for wrong.

Here is more information about our configuration.

Robot background: I have a Husky UGV with a Duro GPS running the latest Piksi ROS driver with an RTK basestation, and a LORD microstrain 3DM-GX5-25 IMU running the latest version of the ros-mscl node written by LORD.

Current Sensor Transforms: With respect to sensor transforms, there are no sensor transforms specified for the Duro as the latitude and longitude are indifferent. The IMU is mounted on top of the robot such that IMU frame on the robot is x=forward, y=right, z=down. This IMU also makes all measurements with respect to NED, so I have written a republishing node to take in the NED IMU data and rotate the data such that the republished IMU data is with respect to ENU as robot_localization expects data in this format. Since the IMU data is transformed into an ENU format, I don't think I need to apply any rotational static transforms to the sensor data since the orientation is in the right frame and the yaw rate has been corrected to increase when the robot turns CCW. Is this assumption valid?

Current Navigation setup: I am running navigation suite as suggested by the dual_ekf_navsat example provided by the robot_localization package. In the first instance of robot_localization for continuous data, I am using the EKF node to fuse linear velocities from the /husky_velocity_controller/odom topic and the gyro rates from the IMU on the /gx5/imu/data topic.

The second instance is the EKF node fusing the same linear velocities from the wheel odometry and rates from the IMU gyro, but now including the odometry/gps output from navsat_transform, which is just the (x,y) coordinates.

As specified navsat_transform_node is subscribed to the /gps/fix topic from the Duro, /gx5/imu/data_ENU topic from the republished ENU IMU data and the output of the second EKF instance (/odometry/filtered_map). Since the IMU data has been transformed into an ENU frame, I have set the yaw_offset=0.0 and set the magnetic declination according to my geographic location.

As an example, we drove our Husky robot straight forward 4 m (with no turning) and the odom frame estimate agrees with the approximate distance travelled reporting minimal lateral translation in y, but the map frame estimate reports that the vehicle is quickly drifting laterally where over the course of a 4 m straight line, the map frame estimate will be reporting a position of (x,y)=(3.36 ... (more)

answered 2020-09-08 09:40:34 -0500

mfader18
31 ●1 ●1 ●6

Update: Through more testing, it was found that the absolute orientation measurement from the IMU we were using was not the direction the robot was actually facing (by a handful of degrees). This resulted in the map frame of the vehicle being oriented in a slightly different direction from where the robot was facing due to the poor initial orientation measurement provided to navsat_transform_node. The observed pose difference between the map and odom frame when driving a straight line makes sense given the map frame is rotated differently from the odom frame. Our solution was to calibrate the IMU and try to move away from any sources of magnetic disturbances and this showed to fix the issue to be somewhat repeatable and make the orientation data useable. The IMU orientation data was not always right but in general the odom and map frame wouldn't be rotated significantly from each other.

We used this navigation setup for autonomous path following where all control actions were based on the pose estimate from the map frame EKF and the desired path also defined in the map frame. Occasionally if the initial pose estimate from the IMU was significantly different from the actual heading of the robot, we would see the vehicle steer aggressively right away to get onto the path that is oriented within the map frame, which is some rotation off from the robots actual initial heading. To fix this problem, restarting or recalibrating the IMU would provide more accurate heading measurements.

edit flag offensive delete link

Comments

In my personal experience, the IMUs are so noisy that they actually make the estimation worse almost all the time. What I do is define a static datum for navsat_transform_node and then reference all map_frame points in that coordinate system. When I run the filter it might take a few filter cycles to integrate enough gps measurements to get the heading correct, but after it has it, it has it, and wont need imu data to maintain it. Does that make sense?

*And thanks for following up and answering your own question, your response is very descriptive and well posed (and btw you can "accept" you answer as correct by clicking the check mark below the up vote and down vote arrows I think)

JackB ( 2020-10-01 13:11:51 -0500 )edit

I should note that my above answer is dependent on having quality corrected GPS like RTK_fixed or RTK_float, or low quality GPS like SBAS and with a large covariance

JackB ( 2020-10-01 13:12:54 -0500 )edit

Thanks for the comments JackB. The IMU orientation was not the most reliable for absolute orientations but of course, the pose estimate would converge after the robot would begin to move. Cheers!

mfader18 ( 2020-10-05 10:37:18 -0500 )edit

Hi,

I am facing the same issue, can you please elaborate how you calibrated the IMU ? Please guide me , I have been searching through ros answers and trying many different things and your post describes the problem that I too face I would really appreciate an answer as I face the same issue for my project and the deadline is close

Thanks

Sid05 ( 2022-04-21 16:25:15 -0500 )edit

Hi there, It has been some time since I was working with this hardware and I don't recall if we used a setting in the launch file to calibrate the sensor but I believe we found that disconnecting and reconnecting the IMU before starting another field trial seemed to recorrect any kind of drift in the compass. Apologies for the lack of detail, I no longer have access to the hardware I was using.

Looks like others are having similar problems with drifting compass measurements. Keep an eye on this post to see if there is any resolution. https://github.com/LORD-MicroStrain/m...

mfader18 ( 2022-04-25 15:30:20 -0500 )edit

Hi,

Thank you for getting back. I could experiment with disconnecting and connecting the IMU cable, will see how it goes I am using a Xsens IMU (https://www.mouser.de/ProductDetail/X...)

Sid05 ( 2022-04-26 00:18:18 -0500 )edit

add a comment

navsat_transform_node odometry estimate looks to be rotated differently from local map frame, resulting in incorrect estimation

1 Answer

Comments

Question Tools

Stats

Related questions

navsat_transform_node odometry estimate looks to be rotated differently from local map frame, resulting in incorrect estimation edit

1 Answer

Comments

Question Tools

Stats

Related questions

navsat_transform_node odometry estimate looks to be rotated differently from local map frame, resulting in incorrect estimation