An intro to dead-reckoning approach, using the Strap-down Inertial Navigation System with an Autonomous Vehicle focus.
Introduction and Motivation
Many drivers are highly curious today about the Autonomous Vehicles (AV) dream. Will this dream come true, and when? One of the core technology that needs to be implemented in AV is the inertial navigation system (INS). These systems integrate many sensors together in what we called “sensor fusion” schemes. These sensors include LiDAR, cameras, GPS receivers, Radars, accelerometers, gyroscopes, and many more. The general sensor fusion scheme integrates all the sensors together using a very common algorithm, named the “Kalman Filter”, to fuse all sensors optimally (in a mean-squared-error sense). So, what is the reason why we haven't seen an AV yet on the road? In this post, we gonna focus on one major problem: how to navigate the AV in tunnels, under bridges, indoor parking lots, urban canyons, and much more difficult environments?
We want our AV to navigate safely in these difficult environments. It appears that out of all the mentioned above sensors, only the accelerometer and gyroscope are still performing under any physical constrain: day or night, light or dark, high or low temperature, etc. Mostly, these two highly-important and low-cost sensors are combined together in an Inertial Measurement Unit (IMU) and can be found almost everywhere. In a modern vehicle, there exist almost 30 IMU sensors to support the ADAS, ABS, cruise control, and many other important systems. One critical drawback of the IMU is its additive noise. When, for example, we integrate the accelerometer measurements to estimate the AV position, we actually account for the noise as well, resulting in a very large accumulated error. So, the AV position solution has a big drift.
What is the Strap-Down INS algorithm?
One of the common algorithms to use for navigating a car using the IMU sensor is the Strap-down Inertial Navigation System, or shortly, just “INS”. This is also known as “Dead Reckoning”. Mostly, we find the INS operates together with additional sensors, such as the GPS receivers or cameras. We call it INS/GPS fusion or vision-aided INS. The fusion involves the Kalman filter and its extended versions. If we operate for a short time (a couple of seconds without GPS signals or non-line-of-sight for vision aiding) and just want to provide our AV a continuous position solution, there is actually no need to take into account all of this fusion- but the IMU.
How the INS works? The Math Beyond.
First, we need to understand that this is a real-time algorithm that considers the accelerations from an accelerometer and the angular velocities from the gyroscope of the vehicle. Once we want to use the INS, we need to provide initial conditions (frequently, we provide the last values that were obtained when the GPS receiver was available). Now, we can define two vectors for the angular velocities and for the accelerations. The angular velocities vector is defined by:
where the notation b is for the body frame and i for inertial (just to mention that measurements are taken in the vehicle body frame and rotate in the inertial frame- which is actually the Earth. The acceleration vector (with the letter f for specific force - equivalent to say acceleration) is given by:
These two measurement vectors are the only vectors we have from the IMU sensor and they allow us to calculate the position, velocity, and orientation of the AV, which is pretty amazing!
We have here the INS equations, neglecting the Earth rotation, in a 3D vector form:
where p stands for a position, v stands for a velocity, R is a rotation matrix as a function of the body orientation angles (provided below), g is just the known Earth gravity vector, and Ω is a skew matrix of the angular velocities, provided by:
Please note the n and b scripts: the n stands for navigation frame, meaning the position, velocity, and orientation of the AV are represented in the navigation map (for example). As the AV moves along a navigation map, we need to rotate the IMU measurement every time step! This is made using the rotation matrix:
The c stands for cos and s stands for sin. AV body angles are provided by three angles, as described in the image:
Summary
So, that's it! Now you can keep navigating for a couple of seconds until your navigation solution (position, velocity, and orientation) starts to drift. But, hey! maybe it is enough for keeping the AV on the road in difficult environments and to increase the safety probability of it- just think of it…