[Translated English Markdown]
In the 11th edition of our sharing, I used the decoding of the CAN bus as an example to explain how to obtain sensor data by decoding CAN bus messages. This edition of sharing will focus on how to convert data in the sensor coordinate system to the vehicle coordinate system.
The unmanned vehicle has various sensors with different installation positions and angles. For the sensor provider, they do not know the angle and installation position of the sensor, so they can only establish a coordinate system based on the sensor itself. The unmanned driving system is a multi-sensor integrated system, which needs to unify sensor data from different positions to a fixed coordinate system–the vehicle coordinate system, in order to analyze the current road scene where the unmanned vehicle is located.
Vehicle Coordinate System of Unmanned Vehicles
Different autonomous driving teams may have different definitions of the coordinate system, but this does not affect development as long as the team reaches an agreement internally.
Taking the vehicle coordinate system provided by Baidu Apollo as an example, the definition of the vehicle coordinate system is as follows:
z-axis – pointing upwards from the ground through the roof of the vehicle
y-axis – pointing to the front of the vehicle in the direction of travel
x-axis – when the vehicle faces forward, it points to the right of the vehicle
The origin of the vehicle coordinate system is the center of the rear wheel axis of the vehicle, as shown in the figure below.
Conversion between Sensor Coordinate System and Vehicle Coordinate System
For the installation position of sensors on unmanned vehicles, the industry is generally the same. For example, the sensor configuration of Audi A8 is shown in the figure below.
Taking the corner radar installed on the left front of the unmanned vehicle as an example for further discussion, the installation position and coordinate system of the radar are shown as the green line in the figure below.
The obstacles detected by the corner radar are shown as green dots in the coordinate system of the radar, and the coordinates of the green dots in the radar coordinate system are (x1, y1). Z-direction coordinates are temporarily excluded for easy understanding.
To convert the green dots to the vehicle coordinate system, certain mathematical operations are required. The basic idea is:
Translation
First, move the origin of the corner radar coordinate system to overlap with the origin of the vehicle coordinate system. At this time, (x1, y1) needs to subtract the distance between the two coordinate systems in the x and y directions. As shown in the figure below.Rotation
After overlapping the O points of the two coordinate systems, the angle radar coordinate system is rotated along the z-axis by a certain angle, so that (x1, y1) is rotated onto the vehicle coordinate system. This process is mathematically known as Euler rotation.
The translation and rotation of the coordinate system are two independent things. Performing translation first and rotation later, or performing rotation first and translation later will not affect the final result.
The above is an intuitive analysis process. Next, we will implement this entire process mathematically.
Translation
The translation step is calculated based on the sensor installation position and the distance to the rear axle of the vehicle, and is only a simple addition and subtraction operation in the XYZ three directions.
Rotation
Rotational movement about an axis requires the introduction of an angle, and it is not a simple addition and subtraction operation. Therefore, we will derive it through the following graphic.
First, transform the two coordinate systems to a normal viewing angle, as shown below:
The coordinates of the obstacle in the angle radar coordinate system are (x1, y1). Assuming that the coordinates of the obstacle in the vehicle coordinate system are (x0, y0), we need to express x0 and y0 in terms of x1, y1, and the installation angle α (measurable). This relationship can be obtained through calculation of auxiliary lines, as shown by the blue lines in the following figure:
The geometric relationships can be represented by the following two equations:
Using matrix representation can simplify the expression and replace two equations with one. This is:
As the rotation this time is around the z-axis, the z-values before and after rotation remain the same:
By putting the z-direction value into the above equation, we can get:
This means that by performing matrix multiplication between the obstacle coordinate values collected by the lidar and this matrix, the rotation along the Z-axis can be completed.
Here we call this matrix the Z-axis rotation matrix RZ, and there must also be rotation matrices RX and RY along the X-axis and Y-axis, respectively.
The coordinates of the lidar target are successively multiplied by these three matrices to complete the rotation along the Z-axis, Y-axis, and X-axis, and the result is the coordinate value in the vehicle coordinate system.
By adding a translation matrix, the entire coordinate transformation relationship can be fully described.
Different coordinate system definitions will have different RX, RY, and RZ, so the rotation matrix and translation matrix need to be calculated according to the actual situation.
Issues with Euler Rotations
The above process is also called Euler rotation, but Euler rotation has an unavoidable problem – gimbal lock. It can be difficult to explain gimbal lock by text alone.
In order to solve the gimbal lock problem caused by Euler rotation, the industry has introduced quaternions. In addition to solving the gimbal lock problem, quaternions can also simplify calculations to some extent. Therefore, Baidu Apollo has also chosen quaternions as the storage medium for the installation position and orientation of various sensors. The definition and usage of quaternions will not be discussed in detail here and can be referred to in the Apollo Calibration module.
Conclusion
In practice, it is found that the mathematics tools such as calculus and linear algebra learned in college are essential mathematical tools, rather than just exam materials.
The above is the principle and mathematical derivation of the coordinate transformation from the sensor to the vehicle coordinate system in the field of autonomous driving technology. This principle is not only used in the field of autonomous vehicles but also widely used in fields such as robotics, drones, and 3D modeling.
That’s all for this sharing. If you have any questions about coordinate transformation, you can interact with me in the comment section.
This article is a translation by ChatGPT of a Chinese report from 42HOW. If you have any questions about it, please email bd@42how.com.