Autocalibration of gyroscope based two-step regulator for autonomous vehicles

In this article was presented an auto calibration algorithm based on a two-step regulator and gyroscope readings. Unable to use PID, as the PWM signal causes the motors to overheat, it was decided to introduce a two-step algorithm. This type of controller reaches the setpoint much faster, though an error is present. Gyroscope is used to sample angular speed and then integrate these values to get the angle. To calibrate the offsets the robot performs a calibration run during which it measures and averages the error on each gyroscope axis while no movement is present. Afterwards it makes a characteristics of braking angle and angular speed. Interpolated polynomial from 13 samples allows to designate the point in time where the braking has to start to achieve the desired angle. The calibration aims for adjusting the parameters of movement depending on the surface it runs on. Additional check is made using encoders to estimate the accuracy of gyroscope output.


Introduction
Positioning of a wheeled mobile vehicle is a complex subject. It can be done using plentiful of sensors like accelerometer, gyroscope, encoders or GPS. None of them can calculate position perfectly, that is why for path planning it is necessary to use different types of sensors. Path planning using accelerometer and gyroscope only is difficult due to errors they might produce. It is often done using those two sensors along with magnetometer mounted in the same structure called IMU (inertial measurement unit). Measurements need to be adjusted using reference points that have a fixed, well known position.. GPS has a poor accuracy and cannot be used for indoor positioning. This environment requires good calibration of other sensors.
Usage of gyroscope and accelerometer is burdened with errors resulting from integrating, noise or environmental factors like centrifugal force. Mounting of the sensors is also important and done improperly can affect the readings producing offsets. Encoders often used in mobile vehicles, on the other hand, are sensitive to any kind of slips and cannot detect movements parallel to the axis of the wheels. LIDAR sensor can also be implemented to measure the distance from obstacles. As for static environment they can be treated as reference points.
Mega Sumo robots are specific types of two wheeled platforms. On the steel ring the movement resistances are very high, though the inertia is low. This allows to drive the robot at high velocities on relatively small area. The resistances cause the robot to stop instantly after turning the motors off, and start at minimum 30% of PWM duty. However low PWM duty cycle causes the motor to draw higher current resulting in faster heat buildup. Its coreless design results in lower inductance than traditional core-based motors. The current rises and falls much faster therefore introducing high current spikes and high instantaneous power dissipation in windings. Considering this problem it was decided to use a two state regulator in order to reduce the heating and maintain maximum performance. PID and its derivative forms are not suitable as their output covers the whole PWM range increasing the motor heating. Moreover that kind of regulators reaches the target angle slower than the one implemented in this project. The error resulting from the two-step controller is always present, however an error of ±10% can be treated as insignificant, taking under consideration the high velocities and accelerations occurring while moving.
Positioning the platform is complex due to the lack of any kind of markers with known constant position [1,2]. Walls or other obstacles can be used as reference points, though their coordinates are not exactly known. Different techniques of combining odometry models and gyroscope/accelerometer are used in order to decrease the influence of errors [4]. To reduce IMU sensor error Kalman filter can be applied [3]. It fuses data from different sensors in order to compensate the error resulting from the design of each sensor. Kalman filter requires detailed knowledge about the object, as well as high computing power. Indoor path planning requires proper calibration of sensors built in IMU. Without the calibration they introduce an error basing on their positioning. The article presents a way of calibrating the gyroscope and using its readings to rotate by a set angle.
Calibration of sensors mentioned above is complex subject. It requires plenty of tests and calculations. Because of that the focus of the article is on rotating the robot by a desired angle only. The rotation is done using two-step regulator which requires previous readings to predict the angle the robot will turn after control signal to stop. To get the proper readings offset calculations has to be made. The whole calibration process is based on the gyroscope alone. It can be burdened with error in long-term due to gyroscope's angle drift. Encoders are used only as a comparison of angle calculated from gyroscope readings, and to check if there are slips during the movement.

Theory
As it was decided to use two-step regulator there are only two drive states -0 and 100% velocity. In theory the idea was to designate the braking distance from 12 different values of angular velocity read from the gyroscope. Knowing the velocity the robot has to turn with in order to stop at setpoint angle allows to turn off the motors at appropriate moment. Braking distance is a recurrent value dependent on the actual angular velocity, due to the low inertia of the whole system. The value of angular acceleration slightly varies when the voltage on the batteries drops, that is why the curve of degrees per second and angle has to be designated again when the voltage drops significantly.
For the proper readings of gyroscope, calibration of the offsets has to be made. It can be caused by soldering the sensor, internal stresses or by change in ambient temperature. Removing offsets is done by subtraction of measured and averaged values while standing still on the drive surface. After removing offsets any reading on gyroscope is supposed to be equivalent to actual movement, and should be read 0 without the movement.
Braking angle based on angular velocity is useful for the two-step regulator. It allows to predict the angle the vehicle will turn with motors off at given angular velocity. Surely it is impossible to measure braking angle at all the possible points, so the braking curve is fitted with least-square method designated polynomial using several measurements presented on equation 1. The fit is not perfect but it can average differences in angle due to delays between reading and control signal and can be successfully used for the test. Another useful property of the polynomial as fitting curve is that it is easier to calculate using on-board microcontroller without so much computing power.
Polynomial is fitted by solving Matrix equation using Gaussian elimination [6]. [ In equation 1, k is polynomial degree and N is number of measured points. Increasing number of points allows using higher polynomial degree or getting better fit. Calculating a set of 6 equations takes approximately 1,5ms on the microcontroller using single-precision floating-points operations. The precision of the calculations can be improved by using double-precision type but it requires a lot more calculating time due to lack of hardware double-precision unit in microcontroller that was used.
Having the braking angle from angular velocity curve allows creating two-step regulator. The regulator has three inputs: current angle change, angular velocity and targeted angle. When current angle added to braking angle calculated for current angular velocity is higher than targeted angle, the motors are turned off and vehicle starts braking.
Using the regulator a difference between set and actual angle can be calculated. It can be represented as absolute error or relative error. Absolute error gives the difference presented in degrees. This method allows to assess the regulator and apply that as error zone for regulator. Relative error tells the percentage of the angle over-or undershoot for the angle. The difference between both the errors is that same absolute error is different relatively to set point. It is crucial to asses if the regulator can be used for every angle.

Test platform
The platform is two wheeled autonomous robot with a fulcrum on the front and two 40mm diameter wheels on the back. Magnets placed on the chassis give the robot additional downforce due to steel surface at which it operates. It results in compression of the rubber on the wheels up to 1.5mm depending on the surface it is placed on. Spacing between wheels' centers is 128 mm. The platform reaches high velocities of up to 3.6m/s, which means it is capable of crossing the 1,54m diameter Dohyo in about 400ms. High forces present while accelerating, braking and rotating require high performance sensors able to measure them.
ICM-20601 is the 3-axis 4000 dps (degrees per second) gyroscope that has been used in the tests. Built in low-pass filter with cutoff frequency 176 Hz gives more stable readings. Calibration of the gyroscope before each test is needed to overcome the problem of inaccurate data without the movement. AS5045 quadrature encoders for the assessment of the gyroscope's readings have been placed on the wheel axis. Placing of the encoders results in precision decrease, however it became resistant to the backlash of the gears. With this setup we can assume the encoders show the actual distance traveled by each wheel.
All readings are recorded onto micro SD card present on the mainboard of the robot. Samples are saved with 1 kHz frequency basing on the "data ready" interrupts from the gyroscope. Higher frequencies caused problems with synchronization and saving data, leading to delays that are unacceptable, as the system is time-based. Subsequently the recorded data is analyzed with Matlab software to compare it with the computing made with on board microcontroller.
Due to extreme accelerations and velocities occurring in MegaSumo robot operations it is difficult to adjust the regulators and the error that may be present is not significant. However it is then easier to apply these kinds of calibration in other autonomous vehicles working in less harsh conditions.

Calibration process
Mega Sumo robots run on a heavy steel ring usually placed on the floor. The difference in placement of both the ring and the robot itself can affect the sensors' readings. Depending on the surface angle there is a need of changing the default gyroscope offsets to get a reliable output. To calibrate the robot we introduced a single program, performing a few separate tests. After executing it the platform should be calibrated well enough to make the set angles with 10% tolerance. However this implementation is susceptible to any changes of the environment it was calibrated in, as it assumes no distortion of the surface it runs on. The calibration has to be performed before every run when the conditions change significantly. Such change can be produced due to the progressive voltage drop on the batteries that could affect the accelerations and maximal angular speed. Figure 1 shows the control signal and robot's angular velocity. The first vertical red line is start indicator, the other is stop signal. After selecting stop the robot goes further and stops with a significant delay. In this article the dependence of breaking angle from a set angular velocity is used to estimate where the robot is going to stop.

Figure 2 presents gyroscope readings during the calibration of offsets. First one shows angular velocity for the whole test to show the magnitude of changes during movement. Second one presents oscillations caused by noise without movement. Readings change in range of about ±1 dps but its average is not changing significantly for the whole time of the test. The offset calculated during the test is 5.5615 dps and this is the value that is subtracted from the readings during further tests.
Second test is performed to gain information about the braking angle from the set angular speed. The robot is rotated from 200 up to 2300 dps with a step of 200 dps, and half a second delay between tests, to be sure the robot has stopped. The value of 2400 dps becomes unreachable after a few tests when the voltage on the Li-po batteries drops, that is why a safe value of 2300 dps is used. With the zero dps point where surely angle has to be zero there are 13 samples from which the fitted polynomial coefficients are calculated. Designated polynomial is used to predict when the robot has to start braking to stop at setpoint angle. This method gives about ±10% accuracy. Figure 3 presents actual curve from readings during second test and fitted polynomials calculated by microcontroller as well as one calculated with higher precision in Matlab software. Both polynomials were calculated from the same gyroscope readings yet the points differ insignificantly due to difference of floating point precision. It is difficult to distinguish one from another which confirms the correctness of the method used on microcontroller. The difference in precision is also unimportant for this kind of calculations.
Last test is performed to check if the polynomial was accurately calculated and if it can be used for two-step regulator. The robot makes 18 tests -turn every 10° starting from 10° up to 180°. During each test the actual angle is compared to the setpoint angle and an error is appointed. If the error is for consecutive tests greater than 10% the robot notifies the operator it needs another run to gain new data.
At the end the polynomial coefficients are saved onto micro SD card in order to read the calculated values at another run.   Table 1. To assess the significance of the overshooting the angle the absolute and relative error is shown on figure 5. The absolute error gives the difference in degrees which can represent the accuracy of the regulator but cannot assess if it can be used for the specific set angle. This assessment can only be done by relative error. The absolute error during test is in the interval of -0.44° up to 9.24° as presented in table 2. It shows that during the test actual angle was mostly greater than set angle. Introducing error zone as mean of the absolute error would decrease the value of errors with the same magnitude of absolute errors which is 9.68°. The relative error could help assess the accuracy of the test for each angle better than absolute error, e.g for set angle of 10° absolute error is less than 2° which gives relative error of 20%. Relative error for most of the readings is less than 10%. Adding the error zone could also decrease the relative error. The highest errors recorded are for the smallest set angles. This implies that two-step regulator is not useful for small angles. The error for set angle of 80° is higher than 10% but it could be treated as an error caused by slight delay in control signal with high angular velocity. What is important to check if there are no slips during the movement. While testing it turned out there are no slips, so the encoders were the reference for the angle integrated from gyroscope data. As the encoders are directly connected with wheels they are a more reliable source of the distance travelled by the platform. A comparison of encoder data and gyroscope readings presented on figure 6 showed a delay between these two. The gyroscope seems to detect the angle later and output a lower angle, which could be an effect of flexibility of mounting. The change in angle between the two ways of calculating is insignificantly different. The change in gyroscope based angle can be caused by not detecting slight changes in angles while encoder based angle detects it.  Figure 7 and table 3 presents differences between actual angle and angle designated from the polynomial. For lower angles both characteristics are similar. Angle done before reaching angular velocity corresponding to proper angle braking does not exceed 10° during test up to 60° of set angle. Tests for higher angles do not show high angular velocity changes which means that the time to reach those angles does not differ significantly. Reaching higher angles than testes is unnecessary to test due to possible rotation in both directions.

Conclusions
Research conducted using the created algorithm allows to calibrate the two wheeled vehicle. The main idea was to achieve a relatively low error while rotating the robot to a set angle. Two-step regulator using gyroscope gave satisfying results, as other regulators were not taken under consideration due to physical limitations. Even though the platform is specific type of a two wheeled robot, the algorithm can be successfully adapted in other mobile vehicles. High accelerations and velocities reached by Mega Sumo robot required a wide range sensors which tend to be less accurate. Usage of a smaller scale gyroscope may result in a better accuracy of movement. Presented solution of a two-step regulator is suitable for angles bigger than 30°. Lower angles have a big error due angle error that is present in all the readings. While rotating by a small angle the drive time has to be really short and as a result it tends to overshoot the desired angle. High accelerations and frequency of the gyroscope readings can be cause to the errors. Time between the readings is around 1 ms and in that time angular velocity can change significantly causing the regulator to overshoot.
Absolute error present in the readings can be taken into account while building regulator as an error zone at which the robot should begin braking. Using that does not change the outcome of error occurring it just allows for it to be closer to the set angle. Delays between control signals and movement cause problems in proper controlling of the vehicle. They cannot be reduced because they are the property of the mechanical set of the vehicle. Given the time sensitivity for the whole system it is important to be aware of the delays and introduce them in the control algorithms.
Using polynomial as the curve fitting is the most efficient way to be used on microcontrollers. It can be easily designated and calculated with low computing power of microcontroller. Other function used as curve fitting may present more accurate data but are more complex.
Presented two-step regulator cannot produce ideal results but for the operation condition of the testing platform slight change in the angle is accepted. The error during the turn can be later compensated during forward motion. Using only gyroscope in long term can produce errors due to angle drift which can be compensated using sensor fusion. Progressive voltage drop is one of the problems that the operator has to face with. Lower voltage equals to lower acceleration which leads to distortion of the fitted curve. It causes changes in braking angle for given angular velocity. Voltage drop can also be problematic during calibration making it difficult to reach given angular velocity to produce polynomial.
The calibration process can be expanded by adapting the accelerometer for translational movements. Later it could be used for inverse kinematics and full path planning algorithm. For the proper use of path planning algorithm it is necessary to calibrate all the possible movement types (straight forward motion, curved motion and rotation in place) and transition between them. Autonomous vehicle will then be able to change the control signals earlier which results in proper achievement of the set points.