Compass Calibration for Mobile Robots

A compass would initially seem a good way, perhaps the obvious way, to give a robot a global rotational co-ordinate frame. Relying on the Earth's magnetic field would appear to give a robot navigational aid without depending on external infrastructure, if you do want to class Earth's magnetic field as internal infrastructure then just be glad it is a lot less likely to go down than any remote server farms which some robots rely on.

In practice though the magnetic field as read by a robot's magnetometer is strongly distorted by magnetic and metallic parts within the robot. A normal hand compass placed on a robot will not reliably track north as the robot rotates, it will swing wildly back and forth as different items within the robot cause contributions to the measured magnetic field which outweigh those of the Earth's weak field, 49.5μT where we are, with only 18.3μT of this being horizontal.

These distortions come in two types, hard iron and soft iron. Hard iron distortions come from magnetised objects within the robot, soft iron distortions from where metal objects provide a preferential path for magnetic fields.

By considering all 3 components of a measured magnetic frield, X, Y and Z, rather than simply the overall direction we can see that a magnetometer in ideal* circumstances we get X,Y,Z readings which form a sphere when plotted as the magnetometer is rotated about all axes. If one imagines a compass sited at the centre then it has the needle point to the edges of the circle as rotations happen and north is correctly followed.

*in pratice the magnetometer by itself is not ideal either, every device will need somewhat different calibration constants to account for effects such as the stresses on a MEMS device caused by solder joints

Idealised magnetometer readings, an X-Y plot of all readings taken while rotating in a sphere

Hard iron distortions shift this sphere away from the centre. A normal compass, or performing basic trigonometry on raw magnetometer readings to get directions, would in the example below point within just a ±45° region for most of a rotation. If the hard iron distortion were more severe it could give identical readings when facing in directions 180° apart.

Hard iron distortions

 

Soft iron distortions turn the sphere in to an ellipse, causing the measured strength to reduce in some directions and increase in others. They are less common in largely plastic robots such as Omni-Pi-tent but do occur.

Soft iron distortions

 

With both effects in play the ideal sphere can become something like this, note how a compass needle in this example could give the same reading for orientations 180° apart given that the direction from the centre to the edge of the ellipse is the same on opposite sides of the point cloud. 

 

Combined hard and soft iron magnetometer distortions, note the arrow showing how the same direction is read even for rotations on opposite sides of the point cloud

Calibration lets us convert the off-centre distorted ellipsoid back to a centred sphere. Each reading can be passed through a series of linear equations to remove the hard and soft effects and give an idealised reading. This is done by rotating the robot about all axes while taking a constant stream of magnetometer readings to produce a table of X,Y,Z values.

At this point we can turn to a very useful C implementation of Li's least squares ellipsoid specific fitting algorithm, written in 2013 for maritime purposes by  M.Boulanger (Bermerlin) , which measures the ellipsoid shape of the measured point cloud and calculates a series of transforms by which it can be converted to a centred sphere. The C code compiles under gcc on linux, although benefits from minor modifications to the filepath and data reading format. sscanf(buf, "%lf,%lf,%lf", &x, &y, &z);, is more applicable to most data files than the \t separators in the original code.

The C code outputs a series of 3 numbers for the hard iron compensation and a matrix with 9 elements for the soft iron compensation. These are then used as follows to convert later magnetometer readings:

 X1 = RawX - (8.032211);// #X-axis removal of hard distortions, values as printed from the C code are always subtracted
 Y1 = RawY - (-16.737712);//#Y-axis
 Z1 = RawZ - (-23.744605);// #Z-axis
 
 Xout =  1.176539*X1 + 0.002427*Y1 +(-0.011834)*Z1;//#X-axis removal of soft distortions, values are used with the sign they are printed with
 Yout =  0.002427*X1 + 1.222201*Y1 + 0.006130*Z1;
 Zout =  (-0.011834)*X1 + 0.006130*Y1 + 1.275629*Z1;

The next step is to make appropriate use of those Xout, Yout and Zout calibrated magnetometer readings to calculate a heading. At latitudes any distance away from the equator the magnetic field vector has a strong downward component, which means a magnetometer placed at any angle other than exactly upright can give inaccurate headings if you simply attempt trigonometry with the X and Y components. Some attempts to solve this involve masochistic attempts to carry out trigonometry in multiple reference frames with angular separations between them, these methods also tend to have singularities and gimbal lock for certain angular combinations. Instead, as inspired by Pololu's compass compensation library , we can use some beautifully straightforward vector products to get first a West vector from the partly downward (here in the Northern hemisphere) magnetic field vector and downward gravity vector**, then a North vector from the gravity vector and the west vector, and then a heading from projections of the West and North vectors.

**if you define gravity as upward, as Pololu's library does, then you get an East not West vector.

The Omni-Pi-tent modules were designed to make use of BNO 055 IMU chips which combine a magnetometer, accelerometer and gyroscope, these looked a good choice early in the design process due to their internal software which performed self-calibration procedures to remove distoprtion effects. Unfortunately this internal software on the BNO 055 was constantly attemtping to optimise its internal calibrations, and therefore lost the true calibration in favour of values calculated over brief time periods in which the robot had performed only a fraction of a rotation. This meant the robots had to be constantly subjected to manual handling, lifting and rotation about multiple axes, to coax the BNO 055 in to recalibrating again with a correct set of constants. To make use of this mroe manual method the BNO 055 had to be put in to a non-smart mode from which plain magnetometer and accelerometer readings could be read. This was done with some modifications to Adafruit's BNO 055 Arduino library, the changes mainly focused on accessing Page 1 of the BNO's addres space while in CONFIG mode, setting the MAG_CONFIG register to 00010101b, and the ACC_CONFIG register to 00001001b, before switching the device back to Page 0 and entering the ACCMAG running mode. This turns the fancy and expensive BNO 055 in to a simple combined magnetometer and accelerometer chip, many other options for which could have been available had the pinout of the BNO 055 not been fixed in to the robot's design at the point when the flaws in the BNO's self-calibration were discovered.

This method worked ideally with the Omni-Pi-tent modules, giving far better accuracy than trusting the BNO 055 IMU's internal self-calibration, and should work well provided three conditions are met:

• Robots should be designed to keep motors and other magnetised parts as far away as feasible from magnetometer chips, this should ensure that magnetic fields from the motor cannot saturate the magnetometer's reading. So long as hard iron distortions do not take the magnetometer to its limits it should be possible to remove them and centre the point cloud.
• Robots should keep wire and traces which carry varying amounts of high current well away from the magnetometer. Whilst a large, but completely constant, current could be removed by running the calibration procedure while the robot was drawing fullt current, any large current which varies is not so easy to account for.
• Robots should avoid having moving magnetic parts within them. Brushless motors where rotor is the permanently magnetised element would give different distortions depending on their position, Omni-Pi-tent's motors are all brushed DC with magnetised stators so do not cause this problem. Where magneticised parts of a robot are on movable joints it may be necessary to perform a full calibration procedure for a whole variety of joint angles and have the calibration constants in the magnetometer compensation routi8hne switched between different values depending on actuator positions. Omni-Pi-tent does have this situation given the presence of wheel and hook motors within Port 4, the port raised on the 2DoF hinge, but it has not caused difficulties at present because all use of accurate compass navigation presently takes place with the hinge in the centre position.
  

Naturally if your robot is operating in an indoor enviornment where the magnetic field is distorted by metal girders, sewer pipes or electrical mains then the magnetic field will not point to the global north, however the field readings after distortion compensation and tilt compensation will still provide a measure of orientation which two robots close together can agree upon and use for navigation within a smaller area.

We will try to make available the full toolchain for compass calibration and compensation with the BNO 055 at some later point in the project.