CN105277950A - Laser radar coordinate conversion method based on vehicle body coordinate system - Google Patents

Abstract

The invention discloses a laser radar coordinate conversion method based on a vehicle body coordinate system, and the method comprises the specific steps: converting an instant laser beam coordinate system into a laser scanning reference coordinate system; solving the coordinates of a laser foot point relative to the laser radar coordinate system according to an inclined angle between the instant laser beam coordinate system and the laser radar coordinate system and a laser flight distance; converting the laser scanning reference coordinate system into an inertia platform reference coordinate system; measuring an installation inclined angle between a laser radar and an inertia navigation platform, and the displacements; solving the coordinates of the laser foot point relative to the inertia platform reference coordinate system according to a rotation and translation formula; and converting the inertia platform reference coordinate system into a period start instant coordinate system. The method does not need to convert laser foot point data into the WGS-84 coordinate system, converts the data into a vehicle carrier instant coordinate system at a starting moment of scanning, reduces the operation quantity and data dependence, and also can maintain the relative coordinates of the point relative to a vehicle. The method does not depend on GPS data, and especially avoids interpolation or prediction errors caused by GPS failures.

Description

Based on the laser radar coordinate transformation method of bodywork reference frame

Technical field

The present invention relates to a kind of coordinate transformation method, particularly relate to a kind of laser radar coordinate transformation method based on bodywork reference frame.

Background technology

Data show, and China's motor vehicles for civilian use recoverable amount has reached 1.5 hundred million, and the thing followed is the continuous increase of traffic hazard, and according to statistics, the annual traffic hazard of China number of dying has exceeded 100,000 people; Therefore, need to utilize advanced technology to ensure safe driving, reduce the generation of traffic hazard.

Laser radar, as the important component part of intelligent transportation system, plays an important role in automotive safety and automatic Pilot.It can accurately measure the three-dimensional coordinate of direction of scanning object, has the advantages that measuring accuracy is high, frequency acquisition is high.But the data that laser radar gathers could will be changed in same coordinate system WGS-84 coordinate system through complicated coordinate conversion, real-time laser radar data, inertial navigation INS data, global position system GPS data are needed in conversion process, but the acquisition frequency of these data has very large gap, the frequency of laser radar is that 100KHZ is even higher, INS frequency is approximately 200HZ, GPS frequency only has 20HZ, therefore interpolation is needed, this high-frequency interpolation can bring error, affects acquisition speed and precision.

Mapping laser radar and crashproof laser radar are essentially different, and mapping laser radar requires that in measured zone, data are accurate as far as possible, but to data processing time not requirement; The real-time processing capacity of crashproof laser radar to data has higher requirement, should reach 100 milliseconds of ranks its total processing time, and namely it scans, resolves, identifies, decision-making T.T. is about 100 milliseconds.Truck carrier has the uncertainty of height relative to aircraft carrier, and take off rear flight attitude relatively steady, INS data variation is steady, and GPS does not block and can implement Obtaining Accurate data; But passing by city, tunnel etc. in vehicle traveling process has the place of blocking that GPS can be caused to lose efficacy, and occurs that continuous print does not have the time of gps data, now carry out interpolation or predict increasing error; The change of car speed, the height on road surface rises and falls, and even the lane change of automobile all can cause the accumulation of INS error.The emphasis of crashproof laser radar is whether scanning road has barrier, but vehicle on road and pedestrian are As time goes on change in location, therefore the data in different scanning cycle is processed simultaneously the difficulty of change and nonsensical.Because navigation information is that INS produces through integration, positioning error is passed in time and increases, and long-term accuracy is poor, and needs the longer initial alignment time before each use.Traditional laser radar data coordinate conversion will through following steps:

1, temporal laser beam coordinate system is converted into laser scanning reference frame, according to transient laser and the angle of laser radar coordinate system and the distance of laser flying, calculates the coordinate of laser footpoint relative to laser radar coordinate system:

$\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]=\left[\begin{array}{c}L\cos acos\mathrm{\β}\\ L\cos asin\mathrm{\β}\\ L\sin a\end{array}\right]$

Wherein (X _ly _lz _l) ^tfor laser footpoint is at the coordinate of laser scanning reference frame, a is the projection of temporal laser beam in XOZ plane and the angle of X-axis; β is the projection of temporal laser beam in XOY plane and the angle of X-axis.

2, laser scanning reference frame transforms inertial platform reference frame, measures arrangement angle and the displacement of laser radar and Inertial Navigation Platform, according to rotation translation formula, resolves the coordinate of laser footpoint relative to inertial platform reference frame; After having settled, this coordinate conversion matrix can not change, and Formula of Coordinate System Transformation is:

$\left[\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right]=R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ \mathrm{\Δ}{Y}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]$

Wherein, (X _iy _iz _i) ^tfor laser footpoint is relative to the coordinate of inertial platform reference frame; R _ifor arrangement angle (a, b, rotation matrix c) obtained by laser scanning reference frame and inertial platform reference frame; After installing, rotation matrix is fixed; (Δ X _lΔ Y _lΔ Z _l) ^tfor the motion vector between laser scanning reference frame initial point and inertial platform reference frame initial point, after installation, this motion vector is fixed.

3, inertial platform reference frame is converted into local horizontal reference coordinate system (antenna GPS), according to three attitude angle that inertial navigation system is measured, H (Heading deflection, rotate around Z axis), P (the Pitch angle of pitch, rotate around Y-axis), R (the Roll angle of roll, rotates around X-axis) and settle displacement resolve the coordinate of impact point relative to gps coordinate system; The difference of the attitude angle that this coordinate conversion matrix is measured due to INS and real-time change, but motion vector can not change, and now Formula of Coordinate System Transformation is:

$\left[\begin{array}{c}{X}_G\\ y_G\\ Z_G\end{array}\right]=R_G\[\left[\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right]-\left[\begin{array}{c}\mathrm{\Δ}{X}_I\\ \mathrm{\Δ}{Y}_I\\ {\mathrm{\ΔZ}}_I\end{array}\right]\]$

Wherein (X _gy _gz _g) ^tfor laser footpoint is relative to the coordinate of local horizontal reference coordinate system; (Δ X _iΔ Y _iΔ Z _i) ^tfor the motion vector between inertial platform reference frame and local horizontal reference coordinate system; R _gfor the rotation formula that three angles (H, R, P) of inertial navigation system measurement form.

4, local horizontal reference coordinate system is converted into WGS-84 coordinate system, the latitude (B) measured according to GPS, the relevant constant coefficient of longitude (L), highly (H) data and WGS-84, resolve the coordinate of laser footpoint relative to WGS-84 coordinate system; The longitude and latitude that this coordinate conversion matrix is measured due to GPS and highly different and real-time change, now Formula of Coordinate System Transformation is:

$\left[\begin{array}{c}{X}_{84}\\ Y_{84}\\ Z_{84}\end{array}\right]=R_W\left[\begin{array}{c}{X}_G\\ Y_G\\ Z_G\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_G\\ {\mathrm{\ΔY}}_G\\ {\mathrm{\ΔZ}}_G\end{array}\right]$

Wherein (X ₈₄y ₈₄z ₈₄) ^trepresent the coordinate of laser footpoint relative to WGS-84 coordinate system; R _wfor local horizontal coordinate is tied to the rotation publicity of earth coordinates longitude and latitude (B, L), obtained by the longitude and latitude of GPS; (Δ X _gΔ Y _gΔ Z _g) ^tfor antenna (GPS) is to the displacement in the earth's core, obtained by GPS.

Formula of Coordinate System Transformation is in sum:

$\left[\begin{array}{c}{X}_{84}\\ Y_{84}\\ Z_{84}\end{array}\right]=R_W{R}_G\[R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ {\mathrm{\ΔY}}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]-\left[\begin{array}{c}\mathrm{\Δ}{X}_I\\ \mathrm{\Δ}{Y}_I\\ {\mathrm{\ΔZ}}_I\end{array}\right]\]+\left[\begin{array}{c}\mathrm{\Δ}{X}_G\\ {\mathrm{\ΔY}}_G\\ {\mathrm{\ΔZ}}_G\end{array}\right]$

Traditional mobile lidar data coordinate system conversion needs through four conversions, and has the transition matrix of three coordinate conversion different, and therefore need to calculate transition matrix to each laser footpoint, its calculated amount is quite huge.INS frequency and GPS frequency are far smaller than laser frequency, need to carry out predicting or interpolation, bring error; Automobile passes by city, tunnel etc. has the place of blocking that GPS can be caused to lose efficacy, and occurs that continuous print does not have the time of gps data, now carries out interpolation or predict increasing error; INS positioning error increases in time, and long-term accuracy is poor.

Ignore the data variation of INS, GPS in the cycle, adopt the simplest Coordinate Transformation Models, a moment will be regarded as in this scan period, choose the some INS data in this moment and gps data, namely rear three matrixes of above coordinate conversion matrix are constant within a scan period, decrease the operand of matrix multiple.Although this algorithm is simple, but error is too large, only for automobile displacement error, if automobile is with the speeds of 25m/s (90km/h), the frequency of laser scanning is 10HZ, then moving vehicle 2.5m in one-period, namely only motor racing error just reaches 2.5m, and this is obviously difficult to accept.

Summary of the invention

The invention provides a kind of laser radar coordinate transformation method based on bodywork reference frame, not needing laser footpoint data transformations is WGS-84 coordinate system, but be converted into start time scan period truck carrier instantaneous coordinate system, namely operand and data dependence is decreased, again can relative coordinate between holding point and automobile; The method does not rely on gps data, particular avoid the interpolation or predicated error brought when GPS lost efficacy.

For achieving the above object, the technical solution adopted in the present invention is: based on the laser radar coordinate transformation method of bodywork reference frame, concrete steps are as follows:

S1: temporal laser beam coordinate system is converted into laser scanning reference frame, according to transient laser and the angle of laser radar coordinate system and the distance of laser flying, calculates the coordinate of laser footpoint relative to laser radar coordinate system:

$\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]=\left[\begin{array}{c}L\cos acos\mathrm{\β}\\ L\cos asin\mathrm{\β}\\ L\sin a\end{array}\right]$

Wherein (X _ly _lz _l) ^tfor laser footpoint is at the coordinate of laser scanning reference frame, this laser scanning reference coordinate is right-handed coordinate system, and a is the projection of temporal laser beam in XOZ plane and the angle of X-axis; β is the projection of temporal laser beam in XOY plane and the angle of X-axis.

S2: laser scanning reference frame transforms inertial platform reference frame, measures arrangement angle and the displacement of laser radar and Inertial Navigation Platform, according to rotation translation formula, resolves the coordinate of laser footpoint relative to inertial platform reference frame; Rotation translation formula in this step is:

$\left[\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right]=R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ \mathrm{\Δ}{Y}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]$

Wherein, (X _iy _iz _i) ^tfor laser footpoint is relative to the coordinate of inertial platform reference frame; R _ifor the rotation matrix obtained by the arrangement angle between laser scanning reference frame and inertial platform reference frame; (Δ X _lΔ Y _lΔ Z _l) ^tfor the motion vector between laser scanning reference frame initial point and inertial platform reference frame initial point, laser radar is arranged on rotation matrix and motion vector after in car and fixes.

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

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{\mathrm{\ΔX}}_i\\ {\mathrm{\ΔY}}_i\\ {\mathrm{\ΔZ}}_i\end{array}\right]$

Wherein (X _ny _nz _n) ^tstart the moving coordinate in coordinate axis n*t moment relative to the cycle; (Δ X _iΔ X _iΔ X _i) ^trepresent the motion vector of i moment running car.

Obtain final Formula of Coordinate System Transformation in sum:

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=R_G\[R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ {\mathrm{\ΔY}}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]\]+\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]$

Further, motion vector is drawn by following steps in step s3:

A1, start instantaneous coordinate fasten in the cycle, the driving trace in each for automobile scan period is carried out differential, and in this 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 working time;

A2, initial time A point arrive B point through the t time, the distance between AB is regarded as the driving trace equaling A to B; The coordinate (Δ X Δ Y/delta Z) that now AB vector is instantaneous relative to A ^tthere is following formula:

$\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]=R_{\mathrm{\Δ}I}\left[\begin{array}{c}I\\ 0\\ 0\end{array}\right]$

Wherein R _{Δ I}for rotation formula: R _{Δ I}=R (H) R (P) R (R)

$R\left(R\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos R& -\sin R\\ 0& \sin R& conR\end{array}\right]$

$R\left(P\right)=\left[\begin{array}{ccc}cosP& 0& \sin P\\ 0& 1& 0\\ -\sin P& 0& \cos P\end{array}\right]$

$R\left(H\right)=\left[\begin{array}{ccc}\cos H& -\sin H& 0\\ \sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]$

Wherein R (H), R (R), R (P) are that inertial platform reference frame starts the attitude angle of instantaneous coordinate system relative to the cycle respectively, and l is the displacement of automobile within this time period;

A3, to calculate B point be thus (X relative to the coordinate of initial time A _by _bz _b) ^t:

$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]$

(Δ X Δ Y/delta Z) ^tthe motion vector of running car; Each coordinate relative to A instantaneous coordinate system can be obtained thus.

Patent of the present invention, owing to adopting above technical scheme, can obtain following technique effect: the present invention can simplify laser radar system Coordinate Transformation Models, coordinate system need not be changed into WGS-84 coordinate system, reduce the size of overall data value; Based on INS position sometime, set up coordinate system, simplify data handling procedure; Within very short time, think that INS system difference amount is constant, reduce calculated amount, accelerate data computing speed.Do not adopt gps data, make not need interpolated data in low-frequency GPS, improve data precision, even if in tunnel, mountain forest, built-up city all need not worry to lose efficacy the data error caused due to GPS.

Adopt the variable quantity of inertial navigation system in tiny time, reduce the sample frequency of INS, avoid the passing in time of INS positioning error and increase, and each use before do not need the longer initial alignment time, making it be applied to vehicle-mountedly becomes possibility.Movement locus is carried out differential, adopts the form of cumulative sum, simplify motor racing locus model, approach original motion trajectory as much as possible simultaneously.

Accompanying drawing explanation

The present invention has accompanying drawing 7 width:

Fig. 1 is coordinate conversion process flow diagram in prior art;

Fig. 2 is laser scanning reference frame in step S1;

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

Fig. 4 is local horizontal coordinates and WGS-84 coordinate system;

Fig. 5 is automobilism track differential schematic diagram;

Fig. 6 is motor racing deflection schematic diagram;

Fig. 7 is coordinate conversion process flow diagram of the present invention.

Embodiment

Below by specific embodiment, and by reference to the accompanying drawings, explanation that the technical solution of the present invention is further explained.

Based on the laser radar coordinate transformation method of bodywork reference frame, concrete steps are as follows:

S1: temporal laser beam coordinate system is converted into laser scanning reference frame, according to transient laser and the angle of laser radar coordinate system and the distance of laser flying, calculates the coordinate of laser footpoint relative to laser radar coordinate system:

$\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]=\left[\begin{array}{c}L\cos acos\mathrm{\β}\\ L\cos asin\mathrm{\β}\\ L\sin a\end{array}\right]$

Wherein (X _ly _lz _l) ^tfor laser footpoint is at the coordinate of laser scanning reference frame, this laser scanning reference coordinate is right-handed coordinate system, and a is the projection of temporal laser beam in XOZ plane and the angle of X-axis; β is the projection of temporal laser beam in XOY plane and the angle of X-axis.

S2: laser scanning reference frame transforms inertial platform reference frame, measures arrangement angle and the displacement of laser radar and Inertial Navigation Platform, according to rotation translation formula, resolves the coordinate of laser footpoint relative to inertial platform reference frame; Rotation translation formula in this step is:

$\left[\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right]=R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ \mathrm{\Δ}{Y}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]$

Wherein, (X _iy _iz _i) ^tfor laser footpoint is relative to the coordinate of inertial platform reference frame; R _ifor the rotation matrix obtained by the arrangement angle between laser scanning reference frame and inertial platform reference frame; (Δ X _lΔ Y _lΔ Z _l) ^tfor the motion vector between laser scanning reference frame initial point and inertial platform reference frame initial point, laser radar is arranged on rotation matrix and motion vector after in car and fixes.

S3: inertial platform reference frame is converted into the cycle and starts instantaneous coordinate system, its use deflection be instantaneous INS numerical value and start time in cycle deflection difference, avoid INS positioning error to increase in time, and do not need the longer initial alignment time before each use; Its conversion formula is:

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{\mathrm{\ΔX}}_i\\ {\mathrm{\ΔY}}_i\\ {\mathrm{\ΔZ}}_i\end{array}\right]$

Wherein (X _ny _nz _n) ^tstart the moving coordinate in coordinate axis n*t moment relative to the cycle; (Δ X _iΔ X _iΔ X _i) ^trepresent the motion vector of i moment running car.

Obtain final Formula of Coordinate System Transformation in sum:

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=R_G\[R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}{X}_L\\ {\mathrm{\ΔY}}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]\]+\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]$

Further, motion vector is drawn by following steps in step s3:

A1, start instantaneous coordinate fasten in the cycle, the driving trace in each for automobile scan period is carried out differential, and the time t of every part is 0.001s, and in this 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 working time;

A2, initial time, the transient motion direction of automobile is X-direction, as shown in the A point in Fig. 5, arrives B point through the t time, and as enough hour of t, the distance between AB equaled the driving trace of A to B; The coordinate (Δ X Δ Y/delta Z) that now AB vector is instantaneous relative to A ^tthere is following formula:

$\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]=R_{\mathrm{\Δ}I}\left[\begin{array}{c}I\\ 0\\ 0\end{array}\right]$

Wherein R _{Δ I}for rotation formula: R _{Δ I}=R (H) R (P) R (R)

$R\left(R\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos R& -\sin R\\ 0& \sin R& conR\end{array}\right]$

$R\left(P\right)=\left[\begin{array}{ccc}cosP& 0& \sin P\\ 0& 1& 0\\ -\sin P& 0& \cos P\end{array}\right]$

$R\left(H\right)=\left[\begin{array}{ccc}\cos H& -\sin H& 0\\ \sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]$

Wherein R (H), R (R), R (P) are that inertial platform reference frame starts the attitude angle of instantaneous coordinate system relative to the cycle respectively, and l is the displacement of automobile within this time period;

A3, to calculate B point be thus (X relative to the coordinate of initial time A _by _bz _b) ^t:

$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]$

(Δ X Δ Y/delta Z) ^tthe motion vector of running car; Each coordinate relative to A instantaneous coordinate system can be obtained thus.

The present invention can simplify laser radar system Coordinate Transformation Models, coordinate system need not be changed into WGS-84 coordinate system, reduce the size of overall data value; Based on INS position sometime, set up coordinate system, simplify data handling procedure; Within very short time, think that INS system difference amount is constant, reduce calculated amount, accelerate data computing speed.

The above; be only the present invention's preferably embodiment; but protection scope of the present invention is not limited thereto; anyly be familiar with those skilled in the art in the technical scope that the present invention discloses; be equal to according to technical scheme of the present invention and inventive concept thereof and replace or change, all should be encompassed within protection scope of the present invention.

Claims (2)

1., based on the laser radar coordinate transformation method of bodywork reference frame, it is characterized in that, concrete steps are as follows:

S1: temporal laser beam coordinate system is converted into laser scanning reference frame, according to transient laser and the angle of laser radar coordinate system and the distance of laser flying, calculates the coordinate of laser footpoint relative to laser radar coordinate system:

$\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]=\left[\begin{array}{c}L\cos acos\mathrm{\β}\\ L\cos asin\mathrm{\β}\\ L\sin a\end{array}\right]$

Wherein (X _ly _lz _l) ^tfor laser footpoint is at the coordinate of laser scanning reference frame, this laser scanning reference coordinate is right-handed coordinate system, and a is the projection of temporal laser beam in XOZ plane and the angle of X-axis; β is the projection of temporal laser beam in XOY plane and the angle of X-axis;

S2: laser scanning reference frame transforms inertial platform reference frame, measures arrangement angle and the displacement of laser radar and Inertial Navigation Platform, according to rotation translation formula, resolves the coordinate of laser footpoint relative to inertial platform reference frame; Rotation translation formula in this step is:

$\left[\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right]=R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ΔX}}_L\\ {\mathrm{\ΔY}}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]$

Wherein, (X _iy _iz _i) ^tfor laser footpoint is relative to the coordinate of inertial platform reference frame; R _ifor the rotation matrix obtained by the arrangement angle between laser scanning reference frame and inertial platform reference frame; (Δ X _lΔ Y _lΔ Z _l) ^tfor the motion vector between laser scanning reference frame initial point and inertial platform reference frame initial point, laser radar is arranged on rotation matrix and motion vector after in car and fixes;

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

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{\mathrm{\ΔX}}_i\\ {\mathrm{\ΔY}}_i\\ {\mathrm{\ΔZ}}_i\end{array}\right]$

Wherein (X _ny _nz _n) ^tstart the moving coordinate in coordinate axis n*t moment relative to the cycle; (Δ X _iΔ X _iΔ X _i) ^trepresent the motion vector of i moment running car;

Obtain final Formula of Coordinate System Transformation in sum:

$\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]=R_G\[R_I\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ΔX}}_L\\ {\mathrm{\ΔY}}_L\\ {\mathrm{\ΔZ}}_L\end{array}\right]\]+\underset{i=0}{\overset{n}{\Σ}}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right].$

2. the laser radar coordinate transformation method based on bodywork reference frame according to claim 1, it is characterized in that, motion vector is drawn by following steps in step s3:

A1, start instantaneous coordinate fasten in the cycle, the driving trace in each for automobile scan period is carried out differential, and in this 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 working time;

A2, initial time A point arrive B point through the t time, the distance between AB is regarded as the driving trace equaling A to B; The coordinate (Δ X Δ Y/delta Z) that now AB vector is instantaneous relative to A ^tthere is following formula:

$\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]=R_{\mathrm{\Δ}I}\left[\begin{array}{c}l\\ 0\\ 0\end{array}\right]$

Wherein R _{Δ I}for rotation formula: R _{Δ I}=R (H) R (P) R (R)

$R\left(R\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos R& -\sin R\\ 0& \sin R& conR\end{array}\right]$

$R\left(P\right)=\left[\begin{array}{ccc}\cos P& 0& \sin P\\ 0& 1& 0\\ -\sin P& 0& \cos P\end{array}\right]$

$R\left(H\right)=\left[\begin{array}{ccc}\cos H& -\sin H& 0\\ \sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]$

Wherein R (H), R (R), R (P) are that inertial platform reference frame starts the attitude angle of instantaneous coordinate system relative to the cycle respectively, and l is the displacement of automobile within this time period;

A3, to calculate B point be (X relative to the coordinate of initial time A _by _bz _b) ^t:

$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}X\\ \mathrm{\Δ}Y\\ \mathrm{\Δ}Z\end{array}\right]$

(Δ X Δ Y/delta Z) ^tthe motion vector of running car; Each coordinate relative to A instantaneous coordinate system can be obtained thus.