What about IMU integration to increase localization accuracy?

[VRichardJP]

In current design, ego localization is estimated by EKF, which takes 3 different inputs:

• NDT pose
• the vehicle odometer velocity
• IMU turn rate (around z axis)

In practice, I have found the vehicle odometer data is often unreliable:

• resolution is typically 1km/h (e.g. jumps from 10.0 to 11.0, to 12.0)
• the value itself is often scaled (e.g. if real speed is 30km/h, odometer will report 33km/h)

These design choices are not a problem if you consider this data is ment to be displayed on the vehicle dashboard, but for a self-driving vehicle which needs to locate itself accurately, this is definitely lacking.

I don't mean odometer velocity report should not be used in EKF, but having only this as a velocity measurement is not sufficient.

Then, while IMU sensors can measure both linear acceleration and angular velocity, only the later is used in Autoware (and only angular velocity around z axis). Estimating the vehicle state from IMU measurement is not straight forward because of biases and gravity, but it is not impossible. Actually, this is what many SLAM algorithms have been doing for a while with fairly good results.

For example, the paper "A Contracting Hierarchical Observer for Pose-Inertial Fusion" describes the state observer used by DLIO algorithm. This observer can estimate position, velocity and orientation from IMU measurements integration. This observer is reported to converge exponentially towards the true state (position, velocity, orientation and IMU biases) if position and orientation errors can be measured (e.g. difference between EKF pose and IMU integrated pose). To keep this example, this is what DLIO manages to do with this observer:

https://github.com/vectr-ucla/direct_lidar_inertial_odometry/blob/master/doc/gif/aggressive.gif

Based on my own experimentations, this observer can estimate the vehicle state quite realiably. For example, estimated position drift is typically < 1cm between 2 LIDAR scan, even with very noisy IMU data (uneven ground).

So I am thinking, could IMU integration be used to improve localization accuracy?

For example, IMU measurements would be integrated in a new module implementing its own state observer. The output of this state observer would be fed to EKF as Pose+Twist measurement (so position, orientation and velocity) instead of the current IMU turn rate (or as complement). Finally, the output poses from EKF would be sent back to the state observer as true pose to correct the accelerometer and gyroscope biases.

What do you think?

[KYabuuchi]

Thanks for posting an interesting idea and sharing and sharing the papers.

resolution is typically 1km/h (e.g. jumps from 10.0 to 11.0, to 12.0)

At first, the data I usually handle doesn't have this issue. However, the scale factor sometimes shifts, which can be a problem. I often solve it by recalibrating the scale factor as needed.

For IMU integration, I believe we need to address the following issues: (However, since I am not familiar with the latest methods, these concerns might already be addressed.)

1. Integration suddenly diverges. (In my experience, such a method is unstable because velocity and position can easily diverge due to the gravity if the orientation estimation is shifted. )
2. Precise lidar-to-IMU calibration is required.
3. IMU acceleration also has bias, causing estimated position to drift over long distances.

Personally, the main problem with vehicle velocity is that drift due to scale factor shifts can prevent accurate localization inside tunnels. If this method can solve that issue, I would be very interested in looking into it further.

[VRichardJP]

Thanks for the feedback.

Indeed, gravity compensation requires precise orientation, so if the IMU integration has no correction mechanism, the estimated orientation will slowly drift away, causing the system to fail compensating the gravity and thinking the vehicle is flying away. This problem is addressed in the paper by computing correction factors from the comparison between the system state pose to a "true pose" (e.g. NDT pose). Accelerometer and gyroscope biases are also corrected, so the system will not drift in the long term.

Of course, that means the system can only converge for as long as NDT converges. If NDT pose is lost, the system will eventually fly away (maybe after a minute?).

[VRichardJP]

Here is a rough specification of what the new imu_integration module could look like.

Inputs:

• Raw IMU measurements (sensor_msgs/msg/Imu)
• NDT pose (geometry_msgs/msg/Pose)

Note that EKF pose could also make sense, but correction requires a 3D pose and current EKF implementation is for 2D only.

Outputs:

• IMU integrated odometry (nav_msg/msg/Odometry) = pose + twist

The module behaviour would be split between 2 states: INIT and RUNNING

In INIT state:

• wait for NDT pose data
• accumulate IMU measurements in a buffer while vehicle is stopped (= NDT poses not moving). Reset the buffer if the vehicle moves at any point.
• once the buffer stores enough data, compute the initial state and change to RUNNING state:
  • position = NDT position
  • orientation = NDT orientation
  • velocity = 0
  • accelerometer bias = average linear acceleration - gravity
  • gyroscope bias = average angular velocity

In RUNNING state:

• when new IMU message is received, integrate the IMU measurement to estimate latest state and publish as odometry message. Store both IMU measurements and states for later use (backpropagation)
• when new NDT pose is received (NDT timestamp = t_ndt):
  • find the last computed state before the NDT pose timestamp (state timestamp = t_k, with t_k < t_ndt < t_(k+1)). Discard all states after t_k (we will recompute them)
  • find all IMU measurements after t_k
  • run a "partial" integration of the first IMU measurement on prior state (at time t_k) to estimate state at time t_ndt (we want to compare the observer estimate with NDT pose, so we need to synchronize timestamps)
  • integrate the first IMU measurement again on prior state (at time t_k), but this time with correction factors. NDT is used as the "true pose", while the pose computed from partial integration above is used as the "estimate". Now we have a corrected estimate of the state at t_(k+1) (accel and gyro biases have been adjusted).
  • finally integrate all the remaining IMU measurements from the corrected state at t_(k+1) ("normal" integration, no correction). Note that at this point all IMU measurements and states before t_k can be discarded.

Note: this design assumes the last received NDT pose timestamp is always < to the last IMU measurement timestamp, which is true in practice.

The following items could be checked while in RUNNING state to detect/prevent divergence:

• accel or gyro bias exceeds some threshold
• NDT pose has been missing for X seconds
• estimated pose is far from NDT pose

If divergence is detected, the system would switch back to INIT state. The system would thus re-initialize automatically the next time the vehicle is stopped.