Article directory

Coordinate Transformation in Autonomous Driving
reference link

Coordinate Transformation in Autonomous Driving

In the autonomous driving task, we perceive the surrounding environment through various sensors and obtain pictures. For various data such as point clouds, due to the differences in the position of the sensor, the form of the data, and the meaning of different data, it is necessary to unify the coordinates and data forms before performing environmental perception tasks, and coordinate transformation is an extremely important part.

Coordinate systems in autonomous driving

According to different reference objects, the coordinate system is mainly divided into world coordinate system, sensor coordinate system and vehicle coordinate system. Different sensors correspond to different data types and coordinate systems.

Each sensor has its own coordinate system, which can be divided into relative measurement (measurement of environmental information) and absolute measurement (measurement of its own information) according to its measurement principle. Relative measurement sensors such as cameras and laser radars perform indirect positioning by measuring environmental information, while absolute measurements such as IMUs, wheel speedometers, and GPS perform positioning by directly measuring their own motion status (speed, position, attitude, etc.) . For a relative measurement sensor, its measurement value is usually under its own sensor; while the measurement result of an absolute measurement is generally its own coordinate system relative to other coordinate systems (local coordinate system or world coordinate system, etc.). The common measurement sensors are summarized below.

1. Camera coordinate system

The left picture in the figure below is a schematic diagram of the relationship between the world coordinate system and the camera coordinate system. In the camera coordinate system, the origin is the center of the imaging plane, and the plane perpendicular to the z-axis corresponds to the focal length plane. The right picture shows the pixel image coordinate system and physical image coordinates A schematic diagram of the relationship between the pixel image coordinate system, the origin of the pixel image coordinate system is at the upper left corner of the image.

World coordinate system : Choose a reference coordinate system in the environment to describe the position of the camera, and use it to describe the position of any object in the environment. This coordinate system is called the world coordinate system.

Camera coordinate system : a three-dimensional rectangular coordinate system established with the focus center of the camera as the origin and the optical axis as the Z axis.

Pixel image coordinate system : In the image obtained after the image in the real world is imaged through a small hole, the coordinate system is in pixels and the coordinate origin is at the upper left corner of the image.

Physical image coordinate system : The position of the pixel is expressed in physical units, and the origin of the coordinates is the intersection of the camera optical axis and the physical coordinate system of the image.

The camera projects the image in the physical world onto the imaging plane through the principle of pinhole imaging. The imaging plane actually exists in the physical world and has a corresponding physical size, usually relatively small, in millimeters. The final image we get is in pixels, so the coordinate system of the image is divided into the pixel image coordinate system and the physical image coordinate system.

2. LiDAR coordinate system

64-line laser radar for environmental scanning process: single frame scanning data

Please add a picture description

64-line laser radar for environmental scanning process: full-circle scanning data

Please add a picture description

LiDAR coordinate system

Rotary lidar generally selects the laser emission center as the origin of the coordinate system, the upward direction is the positive direction of the Z axis, and the X axis and the Y axis form a horizontal plane. The red line in the figure is the laser beam emitted by the lidar, which forms a fan-shaped scanning area parallel to the Z axis and perpendicular to the XY plane at any time. $\theta_j$ of each outgoing laser in the vertical direction $i_{j}$ It is a fixed value, determined during design and manufacture, and is an internal parameter of the lidar. The rotation angle φ ( t ) φ(t)φ(t) of the sector scan plane around the Z axis varies with time and is given in the raw measurement data.

The i-th laser beam is at $t_0$ Point P on the surface of an object is irradiated at all times, and the ranging result shows that the distance between point P and the lidar is L, then the original measurement data of the measurement point P can be expressed in polar coordinates ( φ ( t 0 ) , θ i , $(\varphi(t_0),\theta_i,L)$ to represent. At the same time, point P is in the orthogonal coordinate system of the lidar $X_L,Y_L,Z_L)$ expressed as $P(x_L,y_L,z_L)$ , there is the following conversion relationship. Point cloud data represented by an orthogonal coordinate system is more commonly used in practice.
$X_L=Lcos(\theta_i)cos(\ varphi(t_0))\\ Y_L=Lcos(\theta_i)sin(\varphi(t_0))\\ Z_L=Lsin(\theta_i)$

3. Body coordinate system

The vehicle body coordinate system is used to describe the relative positional relationship between the objects around the vehicle and the vehicle. At present, there are several commonly used ways to define the body coordinate system in academia and industry. They are the ISO international standard definition, the SAE (Society of Automotive Engineers) Society of Automotive Engineers definition, and the coordinate definition based on the inertial measurement unit IMU.

In vehicle dynamics analysis, the body coordinate system defined by ISO is more common. The body coordinate system defined by SAE is consistent with the body coordinate system commonly used in the aerospace field. The car body coordinate system defined based on the IMU is more common in IMU-related applications. No matter which coordinate system definition is used, as long as it is used correctly, the description of the body pose can be completed, and the relative position relationship between the surrounding objects and the workshop can be determined. R&D personnel can choose the car body coordinate system according to application requirements and usage habits.

Examples of definition of various car body coordinate systems

4. World coordinate system

Broadly speaking, the world coordinate system is a coordinate system that describes the position of an object in the world. It generally refers to a system that describes the position relationship on the earth. It is mainly divided into a geographic coordinate system and a projected coordinate system. The geographic coordinate system mainly refers to the spherical coordinate system or ellipsoidal coordinate system with the center of the earth as the origin. Generally, the position of a certain place on the surface of the earth is represented by latitude and longitude. The common standard is the WGS-84 longitude and latitude coordinate system. The projected coordinate system mainly refers to the planar rectangular coordinate system that expresses the distance and size in meters. It needs a coordinate transformation or mapping relationship to convert the latitude and longitude information in the geographic coordinate system used for positioning into a plane in meters. Cartesian coordinates, the common standard is the UTM coordinate system. Considering that environmental perception itself does not require in-depth research on positioning-related content, it is not listed here. For more information, please refer to these two links: (1) , (2) .

Geographical coordinate system (English abbreviation GCS, Geographical Coordinate System) geographic coordinate system unit degree ° minutes 'seconds" –> good at positioning
Projected coordinate system (English abbreviation PCS, Projection Coordinate System) projected coordinate system unit meter m –> good at calculation

Orthographic Mercator projection diagram

Coordinate system transformation

Relationship between coordinate systems

In practical applications, we need to determine the relative position between the obstacle and the self-driving car through different sensors, and at the same time need to know the position and attitude of the car in the world coordinates and the map. This requires associating different coordinate systems and establishing a conversion relationship between them. The figure below is a schematic diagram of some coordinate systems commonly used in autonomous driving problems.

Multiple Coordinate Systems in Autonomous Driving Systems

The coordinate transformation in the autonomous driving problem is mainly divided into two categories. The first category is the coordinate transformation between coordinate systems at different positions in the physical space, which involves the translation alignment of the origin and the rotation alignment of the coordinate axes. Generally, we will use this type of transformation The calibration parameters are called extrinsic parameters; the second type is inside sensors such as cameras. Due to their own characteristics, the obtained data has a certain correspondence with the real world. In order to make full use of the data of different sensors, it is necessary to combine them as much as possible. Unified to the same format and coordinate system, the calibration parameters of this type of transformation are called internal references.

The use of extrinsic parameters

The world coordinate system based on O1 and the coordinate system based on O2 are as follows:

When describing the movement of robots, we often refer to "pose". In fact, pose is a compound word, which we can disassemble into "position + posture". The position refers to "the specific position of a certain kinematic joint/measurement sensor of the robot in the world coordinate system, and the attitude is" the rotation of the coordinate system based on this point compared with the world coordinate system , as shown below:

Coordinate transformation can be decomposed into translation and rotation in essence. Translation is well understood. After the two coordinate systems are rotated, there is only one $O_1O_2$ Vectors can be completed by vector addition. The following will focus on the rotation in coordinate transformation.

We know that a unit vector can be decomposed into projections on the x, y, and z axes in a certain space Cartesian coordinate system, as shown in the figure. The projection of the unit vector in the coordinate system is exactly the cosine value of the angle between the vector P and each coordinate axis.

We take the coordinate system A as the reference coordinate system (world coordinate system), and based on the coordinate system A, represent each coordinate axis of the coordinate system B and unitize each vector, so we get a rotation matrix, a schematic diagram and the meaning of each element of the rotation matrix as follows:

As we mentioned earlier, the calculation of vector coordinates is nothing more than projection, so the conversion of vector coordinates from coordinate system B to coordinate system A is essentially two projections:

The conversion process of the vector P involves the projection and accumulation of three axes, which can be expressed very concisely in the form of matrix multiplication:

After understanding the essence of coordinate transformation, that is, rotation and translation, the conversion formula between coordinate systems is as follows:
$P_C = \begin{bmatrix} x_c\\ y_c\\ z_c\\ 1 \end{bmatrix} {=} \begin{bmatrix} R & T\\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} x_w \\ y_w\\ z_w\\ 1 \end{bmatrix} \Leftrightarrow P_c=R \begin{bmatrix} x_w\\ y_w\\ z_w\\ \end{bmatrix} +T$

Use of camera intrinsics

In the coordinate transformation of automatic driving, the camera internal reference is responsible for the coordinate transformation between the camera coordinate system and the pixel image coordinate system, which can be subdivided into the transformation between the physical image coordinate system and the camera coordinate system, as well as the physical image coordinate system and the pixel image. Transformation of the coordinate system.

Transformation between pixel image coordinate system and physical image coordinate system

When we get the data of the image coordinate system, we want to convert it into a pixel coordinate system at this time. If we know a point p(x, y) of the image coordinate system, we should also know that in the abscissa and ordinate How many millimeters (mm) does 1 pixel occupy. That is, the following formula:
$p(u,v)=p(x/dx+u_0,y/dy+v_0)$
is represented by a matrix as follows:
$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} {= } \begin{bmatrix} 1/dx & 0 & u_0\\ 0 & 1/dy & v_0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\ y\\ 1 \end{bmatrix }$

Transformation between physical image coordinate system and camera coordinate system

$P^\prime$ on the imaging plane $P^{The line segment where '}$ is located, the z-axis, the imaging plane and the corresponding plane of point P form a similar triangle. According to the similar triangle:
$\ frac{X_c}{x}=\frac{Z_c}{f}=\frac{Y_c}{y}\Rightarrow x=f\frac{X_c}{Z_c},y=f\frac{Y_c}{Z_c}$
Transform into matrix form:
$Z_c \begin{bmatrix} x\\y\\1 \end{bmatrix} {=} \begin{bmatrix} f&0&0\\0&f&0\\0&0&1 \end{bmatrix} \begin{bmatrix} X_c\\Y_c\\Z_c \end{bmatrix}$
To sum up, for the camera, the coordinate transformation from the world coordinate system to the image pixel coordinate system can be performed with the following matrix.