CN105277950B - LiDAR coordinate transformation method based on car body coordinate system - Google Patents

Abstract

The laser radar coordinate conversion method based on the vehicle body coordinate system comprises the specific steps that an instantaneous laser beam coordinate system is converted into a laser scanning reference coordinate system, the coordinate of a laser foot point relative to the laser radar coordinate system is calculated according to the included angle between instantaneous laser and the laser radar coordinate system and the laser flying distance, the laser scanning reference coordinate system is converted into an inertial platform reference coordinate system, the arrangement included angle and the displacement of a laser radar and an inertial navigation platform are measured, and the coordinate of the laser foot point relative to the inertial platform reference coordinate system is calculated according to a rotation translation formula; the inertial platform reference coordinate system is converted into a period starting instantaneous coordinate system, laser foot point data are not required to be converted into a WGS-84 coordinate system, but are converted into an automobile carrier instantaneous coordinate system at the starting moment of a scanning period, so that the calculation amount and data dependence are reduced, and the relative coordinates between the points and an automobile can be kept; the method does not depend on GPS data, and particularly avoids interpolation or prediction errors caused by GPS failure.

Description

LiDAR coordinate transformation method based on car body coordinate system

technical field

The invention relates to a coordinate conversion method, in particular to a laser radar coordinate conversion method based on a car body coordinate system.

Background technique

Statistics show that the number of civil vehicles in my country has reached 150 million, followed by the continuous increase of traffic accidents. According to statistics, the number of traffic accident deaths in my country has exceeded 100,000 people every year; therefore, it is necessary to use advanced technology to Ensure safe driving and reduce traffic accidents.

As an important part of the intelligent transportation system, lidar plays an important role in vehicle safety and automatic driving. It can accurately measure the three-dimensional coordinates of objects in the scanning direction, and has the characteristics of high measurement accuracy and high acquisition frequency. However, the data collected by lidar needs to go through complex coordinate conversion to convert to the same coordinate system WGS-84 coordinate system. During the conversion process, real-time lidar data, inertial navigation INS data, and global positioning system GPS data are required. However, the data of these data There is a big gap in the acquisition frequency. The frequency of the lidar is 100KHZ or higher, the frequency of the INS is about 200HZ, and the frequency of the GPS is only 20HZ, so interpolation is required. This high-frequency interpolation will bring errors and affect the speed and accuracy of data acquisition. .

There is an essential difference between surveying and mapping lidar and anti-collision lidar. Surveying and mapping lidar requires the data in the measurement area to be as accurate as possible, but there is no requirement for data processing time; anti-collision lidar has higher requirements for real-time data processing. The total processing time should reach the level of 100 milliseconds, that is, the total time for scanning, solving, identifying, and decision-making is about 100 milliseconds. Compared with the aircraft carrier, the vehicle carrier has a high degree of uncertainty. The flight attitude of the aircraft is relatively stable after take-off, the INS data changes smoothly, and the GPS can accurately obtain data without obstruction; Places will cause GPS failure, and there will be continuous time without GPS data. At this time, interpolation or prediction will increase the error; changes in vehicle speed, ups and downs of the road surface, and even vehicle lane changes will lead to the accumulation of INS errors. The focus of anti-collision lidar is to scan whether there are obstacles on the road. However, the position of vehicles and pedestrians on the road changes over time, so it becomes difficult and meaningless to process the data of different scanning cycles at the same time. Since the navigation information is generated by the integration of the INS, the positioning error increases with time, the long-term accuracy is poor, and a long initial alignment time is required before each use. Traditional lidar data coordinate conversion goes through the following steps:

1. The instantaneous laser beam coordinate system is converted into the laser scanning reference coordinate system. According to the angle between the instantaneous laser and the laser radar coordinate system and the distance of the laser flight, the coordinates of the laser foot point relative to the laser radar coordinate system are calculated:

Where (X _L Y _L Z _L ) ^T is the coordinates of the laser foot point in the laser scanning reference coordinate system, a is the angle between the projection of the instantaneous laser beam in the XOZ plane and the X axis; β is the angle of the instantaneous laser beam in the XOY plane The angle between the projection and the X axis.

2. Transform the laser scanning reference coordinate system into the inertial platform reference coordinate system, measure the installation angle and displacement between the laser radar and the inertial navigation platform, and calculate the coordinates of the laser foot point relative to the inertial platform reference coordinate system according to the rotation and translation formula; after installation After that, the coordinate conversion matrix will not change, and the coordinate conversion formula is:

Among them, (X _I Y _I Z _I ) ^T is the coordinates of the laser foot point relative to the inertial platform reference coordinate system; R _I is the installation angle (a, b, c) between the laser scanning reference coordinate system and the inertial platform reference coordinate system ) rotation matrix; the rotation matrix is fixed after installation; (ΔX _L ΔY _L ΔZ _L ) ^T is the displacement vector between the origin of the laser scanning reference coordinate system and the origin of the inertial platform reference coordinate system, and the displacement vector is fixed after installation.

3. The inertial platform reference coordinate system is transformed into the local horizontal reference coordinate system (antenna GPS), according to the three attitude angles measured by the inertial navigation system, H (Heading direction angle, rotating around the Z axis), P (Pitch pitch angle, around the Y axis Axis rotation), R (Roll side roll angle, rotation around the X axis) and the coordinates of the target point of the placement displacement calculation relative to the GPS coordinate system; the coordinate transformation matrix changes in real time due to the different attitude angles measured by the INS, but the displacement vector will not change, and the coordinate conversion formula at this time is:

Where (X _G Y _G Z _G ) ^T is the coordinate of the laser foot point relative to the local horizontal reference coordinate system; (ΔX _I ΔY _I ΔZ _I ) ^T is the displacement vector between the inertial platform reference coordinate system and the local horizontal reference coordinate system ; R _G is the rotation formula composed of three included angles (H, R, P) measured by the inertial navigation system.

4. The local horizontal reference coordinate system is transformed into the WGS-84 coordinate system. According to the latitude (B), longitude (L), height (H) data measured by GPS and the relevant constant coefficient of WGS-84, the laser foot point relative to The coordinates of the WGS-84 coordinate system; the coordinate conversion matrix changes in real time due to the difference in latitude, longitude and altitude measured by GPS. At this time, the coordinate conversion formula is:

Where (X ₈₄ Y ₈₄ Z ₈₄ ) ^T represents the coordinates of the laser foot point relative to the WGS-84 coordinate system; R _W is the rotation publicity from the local horizontal coordinate system to the longitude and latitude (B, L) of the geodetic coordinate system, obtained from the longitude and latitude of GPS ; (ΔX _G ΔY _G ΔZ _G ) ^T is the displacement of the antenna (GPS) to the center of the earth, obtained by GPS.

In summary, the coordinate conversion formula is:

The traditional vehicle lidar data coordinate system conversion requires four conversions, and the conversion matrix of the three coordinate conversions is different. Therefore, the conversion matrix needs to be calculated for each laser foot point, and the calculation amount is quite huge. The INS frequency and GPS frequency are much smaller than the laser frequency, so prediction or interpolation is required, which will bring errors; cars passing by urban areas, tunnels and other sheltered places will cause GPS failure, and there will be continuous time without GPS data, and interpolation is performed at this time Or the prediction will increase the error; the INS positioning error increases with time, and the long-term accuracy is poor.

Ignore the data changes of INS and GPS within the period, adopt the simplest coordinate transformation model, regard the scanning period as a moment, select a certain INS data and GPS data in this moment, that is, the last three matrices of the above coordinate transformation matrix It remains unchanged within one scan cycle, reducing the amount of calculations for matrix multiplication. Although the algorithm is simple, the error is too large. Just take the displacement error of the car as an example. If the car travels at a speed of 25m/s (90km/h) and the laser scanning frequency is 10HZ, the car moves 2.5m in one cycle, that is The car motion error alone reaches 2.5m, which is obviously unacceptable.

Contents of the invention

The invention provides a laser radar coordinate conversion method based on the vehicle body coordinate system, which does not need to convert the laser foot point data into the WGS-84 coordinate system, but converts it into the instantaneous coordinate system of the vehicle carrier at the beginning of the scanning cycle, which reduces the The amount of calculation is dependent on data, and the relative coordinates between the point and the car can be maintained; this method does not depend on GPS data, especially to avoid interpolation or prediction errors caused by GPS failure.

In order to achieve the above object, the technical solution adopted in the present invention is: based on the laser radar coordinate conversion method of the car body coordinate system, the specific steps are as follows:

S1: The instantaneous laser beam coordinate system is transformed into the laser scanning reference coordinate system. According to the angle between the instantaneous laser and the laser radar coordinate system and the distance of the laser flight, the coordinates of the laser foot point relative to the laser radar coordinate system are calculated:

Where (X _L Y _L Z _L ) ^T is the coordinates of the laser foot point in the laser scanning reference coordinate system, the laser scanning reference coordinate system is a right-handed coordinate system, a is the projection of the instantaneous laser beam in the XOZ plane and the clip of the X axis β is the angle between the projection of the instantaneous laser beam on the XOY plane and the X axis.

S2: Transform the laser scanning reference coordinate system into the inertial platform reference coordinate system, measure the installation angle and displacement between the laser radar and the inertial navigation platform, and calculate the coordinates of the laser foot point relative to the inertial platform reference coordinate system according to the rotation and translation formula; this step The formula for rotation and translation in is:

Among them, (X _I Y _I Z _I ) ^T is the coordinates of the laser foot point relative to the inertial platform reference coordinate system; R _I is the rotation matrix obtained from the installation angle between the laser scanning reference coordinate system and the inertial platform reference coordinate system ; (ΔX _L ΔY _L ΔZ _L ) ^T is the displacement vector between the origin of the laser scanning reference coordinate system and the origin of the inertial platform reference coordinate system, and the rotation matrix and displacement vector are fixed after the lidar is installed in the vehicle.

S3: The reference coordinate system of the inertial platform is converted into the instantaneous coordinate system at the beginning of the cycle, and the conversion formula is:

Where (X _n Y _n Z _n ) ^T is the moving coordinate relative to the cycle start coordinate axis at n*t time; (ΔX _i ΔX _i ΔX _i ) ^T represents the displacement vector of the vehicle at time i.

In summary, the final coordinate transformation formula is obtained:

Further, in step S3, the displacement vector is obtained through the following steps:

A1. In the instantaneous coordinate system at the beginning of the cycle, differentiate the driving trajectory of the car in each scanning cycle. In this cycle, the car moves in a straight line at a uniform speed, and the distance traveled by the car is:

l=v×t

Where v is the speed of the car, t is the running time;

A2. Initially, point A arrives at point B after t time, and the distance between AB is regarded as equal to the driving trajectory from A to B; at this time, the instantaneous coordinate (ΔX ΔY ΔZ) ^T of the AB vector relative to A has the following formula:

Where R _ΔI is the rotation formula: R _ΔI = R(H)R(P)R(R)

Among them, R(H), R(R), and R(P) are the attitude angles of the inertial platform reference coordinate system relative to the instantaneous coordinate system at the beginning of the cycle, and l is the displacement of the vehicle during this time period;

A3. From this, the coordinates of point B relative to the starting time A are calculated as (X _B Y _B Z _B ) ^T :

(ΔX ΔY ΔZ) The displacement vector of the vehicle ^T ; from this, the coordinates of each point relative to the instantaneous coordinate system of A can be obtained.

Due to the adoption of the above technical solutions, the patent of the present invention can achieve the following technical effects: the present invention can simplify the coordinate conversion model of the laser radar system without converting the coordinate system into the WGS-84 coordinate system, reducing the size of the overall data value; The coordinate system is established based on the INS position at each moment, which simplifies the data processing process; in a very short period of time, the INS system difference value is considered to be unchanged, reducing the amount of calculation and speeding up the data calculation speed. No GPS data is used, so that there is no need to interpolate data in low-frequency GPS, which improves the data accuracy. Even in urban areas with tunnels, forests, and high-rise buildings, there is no need to worry about data errors caused by GPS failure.

The change of the inertial navigation system in a small time reduces the sampling frequency of the INS, avoids the increase of the INS positioning error with the passage of time, and does not require a long initial alignment time before each use, so that it can be applied to the car is possible. Differentiate the trajectory and adopt the form of cumulative sum to simplify the vehicle trajectory model and approach the original trajectory as much as possible.

Description of drawings

The present invention has 7 accompanying drawings:

Fig. 1 is coordinate conversion flowchart in the prior art;

Fig. 2 is the laser scanning reference coordinate system in step S1;

Fig. 3 is a schematic diagram of coordinate conversion in step S2;

Figure 4 is the local horizontal coordinate system and the WGS-84 coordinate system;

Fig. 5 is a schematic diagram of the differentiation of the running track of the vehicle;

Fig. 6 is a schematic diagram of the direction angle of automobile movement;

Fig. 7 is a flow chart of coordinate conversion in the present invention.

detailed description

The technical solutions of the present invention will be further explained below through specific embodiments in conjunction with the accompanying drawings.

The specific steps of the laser radar coordinate conversion method based on the car body coordinate system are as follows:

S1: The instantaneous laser beam coordinate system is transformed into the laser scanning reference coordinate system. According to the angle between the instantaneous laser and the laser radar coordinate system and the distance of the laser flight, the coordinates of the laser foot point relative to the laser radar coordinate system are calculated:

Where (X _L Y _L Z _L ) ^T is the coordinates of the laser foot point in the laser scanning reference coordinate system, the laser scanning reference coordinate system is a right-handed coordinate system, a is the projection of the instantaneous laser beam in the XOZ plane and the clip of the X axis angle; β is the angle between the projection of the instantaneous laser beam on the XOY plane and the X axis.

S2: Transform the laser scanning reference coordinate system into the inertial platform reference coordinate system, measure the installation angle and displacement between the laser radar and the inertial navigation platform, and calculate the coordinates of the laser foot point relative to the inertial platform reference coordinate system according to the rotation and translation formula; this step The formula for rotation and translation in is:

Among them, (X _I Y _I Z _I ) ^T is the coordinates of the laser foot point relative to the inertial platform reference coordinate system; R _I is the rotation matrix obtained from the installation angle between the laser scanning reference coordinate system and the inertial platform reference coordinate system ; (ΔX _L ΔY _L ΔZ _L ) ^T is the displacement vector between the origin of the laser scanning reference coordinate system and the origin of the inertial platform reference coordinate system, and the rotation matrix and displacement vector are fixed after the lidar is installed in the vehicle.

S3: The inertial platform reference coordinate system is transformed into the instantaneous coordinate system at the beginning of the cycle. The direction angle used is the difference between the instantaneous INS value and the direction angle at the beginning of the cycle, which avoids the increase of the INS positioning error with time, and before each use A long initial alignment time is not required; the conversion formula is:

Where (X _n Y _n Z _n ) ^T is the moving coordinate relative to the cycle start coordinate axis at n*t time; (ΔX _i ΔX _i ΔX _i ) ^T represents the displacement vector of the vehicle at time i.

In summary, the final coordinate transformation formula is obtained:

Further, in step S3, the displacement vector is obtained through the following steps:

A1. In the instantaneous coordinate system at the beginning of the cycle, differentiate the driving trajectory of the car in each scanning cycle, and the time t of each scan cycle is 0.001s. In this cycle, the car moves in a straight line at a uniform speed, and the distance traveled by the car is:

l=v×t

Where v is the speed of the car, t is the running time;

A2. Initially, the instantaneous direction of motion of the car is the direction of the X-axis, as shown at point A in Figure 5, after t time to reach point B, when t is small enough, the distance between AB is equal to the driving trajectory from A to B; At this time, the instantaneous coordinate (ΔXΔY ΔZ) ^T of the AB vector relative to A has the following formula:

Where R _ΔI is the rotation formula: R _ΔI = R(H)R(P)R(R)

Among them, R(H), R(R), and R(P) are the attitude angles of the inertial platform reference coordinate system relative to the instantaneous coordinate system at the beginning of the cycle, and l is the displacement of the vehicle during this time period;

A3. From this, the coordinates of point B relative to the starting time A are calculated as (X _B Y _B Z _B ) ^T :

(ΔX ΔY ΔZ) The displacement vector of the vehicle ^T ; from this, the coordinates of each point relative to the instantaneous coordinate system of A can be obtained.

The present invention can simplify the coordinate conversion model of the laser radar system without converting the coordinate system into the WGS-84 coordinate system, which reduces the size of the overall data value; the coordinate system is established based on the INS position at a certain moment, which simplifies the data processing process ; In a very short period of time, it is considered that the difference of the INS system remains unchanged, reducing the amount of calculation and speeding up the speed of data calculation.

The above is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Anyone familiar with the technical field within the technical scope disclosed in the present invention, according to the technical solution of the present invention Any equivalent replacement or change of the inventive concepts thereof shall fall within the protection scope of the present invention.

Claims (1)

1. the laser radar coordinate transformation method based on bodywork reference frame, it is characterised in that comprise the following steps that：

S1：Temporal laser beam coordinate system is converted into laser scanning reference frame, according to transient laser and laser radar coordinate system Angle and laser flying distance, calculate coordinate of the laser footpoint relative to laser radar coordinate system：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>L</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>L</mi> <mi> </mi> <mi>cos</mi> <mi> </mi> <mi>a</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;beta;</mi> </mtd> </mtr> <mtr> <mtd> <mi>L</mi> <mi> </mi> <mi>cos</mi> <mi> </mi> <mi>a</mi> <mi> </mi> <mi>sin</mi> <mi>&amp;beta;</mi> </mtd> </mtr> <mtr> <mtd> <mi>L</mi> <mi> </mi> <mi>sin</mi> <mi> </mi> <mi>a</mi> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein (X_L Y_L Z_L)^TIt is laser footpoint in the coordinate of laser scanning reference frame, the laser scanning reference frame is Right-handed coordinate system, a is the angle of projection and X-axis of the temporal laser beam in XOZ planes；β is temporal laser beam in XOY plane Projection and the angle of X-axis；

S2：Laser scanning reference frame conversion inertial platform reference frame, measurement laser radar and Inertial Navigation Platform Angle and displacement are disposed, according to rotation translation formula, coordinate of the laser footpoint relative to inertial platform reference frame is resolved；This Rotation translation formula in step is：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>I</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>I</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>I</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msub> <mi>R</mi> <mi>I</mi> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>L</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;X</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Y</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Z</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein, (X_I Y_I Z_I)^TCoordinate for laser footpoint relative to inertial platform reference frame；R_ITo be referred to by laser scanning The spin matrix that placement angle between coordinate system and inertial platform reference frame is obtained；(ΔX_L ΔY_L ΔZ_L)^TFor laser The motion vector between reference frame origin and inertial platform reference frame origin is scanned, after laser radar is installed in the car Spin matrix and motion vector are fixed；

S3：Inertial platform reference frame is converted into the cycle and starts instantaneous coordinate system, and its conversion formula is：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>n</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mi>n</mi> </munderover> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;X</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Y</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Z</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein (X_n Y_n Z_n)^TIt is the moving coordinate for starting the reference axis n*t moment relative to the cycle；(ΔX_i ΔX_i ΔX_i)^TRepresent The motion vector of i moment running cars；

Final coordinate conversion formula is obtained in summary：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>n</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msub> <mi>R</mi> <mi>G</mi> </msub> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msub> <mi>R</mi> <mi>I</mi> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>L</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>L</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;X</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Y</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;Z</mi> <mi>L</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>+</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mi>n</mi> </munderover> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>i</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>

The motion vector is through the following steps that draw：

A1, start instantaneous coordinate in the cycle and fasten, the driving trace in automobile each scan period is subjected to differential, in the cycle Automobile does linear uniform motion, then the distance of running car is：

L=v × t

Wherein v is the speed of automobile, and t is run time；

A2, it is initial when A points reach B points by the t times, the distance between AB is regarded as to the driving trace equal to A to B；Now AB The vector coordinate (Δ X Δ Y Δ Z) instantaneous relative to A^TThere is below equation：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>X</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>Y</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>Z</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msub> <mi>R</mi> <mrow> <mi>&amp;Delta;</mi> <mi>I</mi> </mrow> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>l</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein R_ΔIFor rotation formula：R_ΔI=R (H) R (P) R (R)

<mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>R</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>R</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <mi>R</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <mi>R</mi> </mrow> </mtd> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>n</mi> <mi>R</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>P</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>P</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <mi>P</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <mi>P</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>P</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>H</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>H</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <mi>H</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <mi>H</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>H</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein R (H), R (R), R (P) are the posture that inertial platform reference frame starts instantaneous coordinate system relative to the cycle respectively Angle, l is the displacement of the automobile within the period；

A3, coordinate of the B points relative to initial time A is calculated for (X_B Y_B Z_B)^T：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>B</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>B</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>B</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>&amp;Delta;</mi> <mi>X</mi> </mtd> </mtr> <mtr> <mtd> <mi>&amp;Delta;</mi> <mi>Y</mi> </mtd> </mtr> <mtr> <mtd> <mi>&amp;Delta;</mi> <mi>Z</mi> </mtd> </mtr> </mtable> </mfenced> </mrow>

(ΔX ΔY ΔZ)^TFor the motion vector of running car；It is hereby achieved that each is relative to A point instantaneous coordinate systems Coordinate.