3
$\begingroup$

I'm working with a differential-drive robot that has odometry measurements from wheel shaft encoders and heading information from an imu (I'm using BNO055 in imu mode to get Euler angles, primarily the heading angle).

I'd like to use the imu header angle to augment the odometry which is prone to slipping and other errors, but the imu lags the odometry by up to 100ms.

How can I combine these measurements to get the best estimate of the robot pose?

Thanks for any word on this.

$\endgroup$
1
$\begingroup$

Combining two sensor outputs that should theoretically give you the same information is called "sensor fusion".

The easiest way to go about this is with a tool known as a complimentary filter. The complimentary filter uses a "blend" value, between 0 and 1, to produce the output:

filterOutput = blend*sensor1 + (1-blend)*sensor2;

Typically you have the sensor that is more reliable short term weighted much more than the sensor that is more reliable long term. Blend values around 0.95 are common. If you think both sensors are equally reliable, then set blend to 0.5. This simply averages the two.

In your case you know odometry is unreliable long term because of wheel slip and IMU short term because of delays, so I would weight odometry 0.95 and IMU 0.05 to start. See how that works and adjust as necessary.

$\endgroup$
  • $\begingroup$ Thanks for these replies. I am doing data gathering and some work with off-line algorithms now. I'm concerned that the data from the IMU gets behind the odometry data and stays there; it's not clear how I can compensate for that. Otherwise, the idea of integrating over a few sampling periods to incorporate the delay sounds good. $\endgroup$ – jstanle1 Jan 19 '16 at 23:10
  • $\begingroup$ I've begun to get results from this approach (the 'blend' filter). So far this is working well with a blend value of 1/2. I'll be trying various blend parameter values to see how this works in general, but for now I'm getting really nice sharp squares from a test program that goes straight for a time then makes a 90-degree turn. Thanks for this suggestion. $\endgroup$ – jstanle1 Feb 10 '16 at 23:17
  • $\begingroup$ @jstanle1 - If the answer solves your problem, please accept it. For your reference, it's technically called a "complimentary filter", in the event you wanted to read more about it. Typically you put a higher weight (blend) on the term you think is more correct short-term. But I'm glad it's working well for you! $\endgroup$ – Chuck Feb 11 '16 at 0:18
2
$\begingroup$

You're robot must be moving pretty fast for this delay to cause problems.

Use a circular buffer to store the odometry readings, so you'll have a record of the last 100ms of odometry readings.

Use the IMU and odom combined at time $T_{-100ms}$ to estimate your last known 'accurate' state. Integrate the odom forward from $T_{-100ms}$ to $T_{0}$ to get your current state. Repeat this at every time step.

If your control can handle the delay, don't bother with integrating the odom. Just use your accurate state 100ms ago.

$\endgroup$
  • $\begingroup$ Thanks, I think I understand this. The robot isn't fast, but small errors, especially heading errors, add up -- after a while it's not going the direction it thinks it is. I get the most odometry problems when it's turning -- I suspect there's a lot of slippage. I'm interested now to try what you've suggested. $\endgroup$ – jstanle1 Jan 19 '16 at 23:17
1
$\begingroup$

As the other answers suggest, 100ms of delay in control may not be significant to your application. It would be prudent to first solve your data-fusion problem and then see if the delay is an issue to your controller. I would first record some data of a closed-loop path and do some offline filtering to see if the results are good. For instance, make your robot drive a square and record that data.

Your first problem is then to synchronise the data streams; since your IMU is lagging by 100ms, then simply operate on the newest IMU data you have, with the odometry from the corresponding time.

The data-fusion could be done using a Kalman filter for the 2D case (X,Y,heading). The heading is updated by the IMU and the velocity (and heading, depending on the model) is updated by the odometry. See the system model in this paper, which uses odometry. A good solution would involve a nonlinear KF such as EKF, but I suspect for low speeds and high sensor rates you can get away with a linear KF. I think the states would be $\mathbf{[x, y, \phi, x', y', \phi']}^\top$, where your process model would be the same as the first paper I linked, and the measurement model is simply $\mathbf{y}= \mathbf{u} + v$, where v is Gaussian white noise with known standard deviation. Maybe see this kalman filter framework.

After you have some working results with your offline data, you can worry about implementing it online. The implementation will greatly depend on your choice of data-fusion algorithm.

See also: particle filter methods and a full ROS implementation.

$\endgroup$
  • $\begingroup$ Thanks for the explanation and the links. I'll try out simpler data fusion first, as you suggest, then tackle handling the delay if necessary $\endgroup$ – jstanle1 Jan 19 '16 at 23:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.