CN104049269A - Target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation system - Google Patents

Abstract

The invention discloses a target navigation mapping method based on laser ranging and an MEMS/GPS integrated navigation system. The position and posture of an observation point are detected out through the MEMS/GPS integrated navigation system, the distance between the observation point and a target is measured out through LDS and is solved according to the target navigation mapping method, and information such as the position, posture, slope distance and elevation difference of a target carrier can be obtained; the posture precision of the MEMS/GPS integrated navigation system is improved through a posture correction algorithm, and then the target positioning precision is improved. By means of the target navigation mapping method, navigation mapping on dynamic targets can be effectively achieved, and navigation equipment does not need to be mounted on the target carrier.

Description

A kind of target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation system

Technical field

The present invention relates to target navigation positioning field, particularly a kind of method based on laser ranging and MEMS/GPS integrated navigation system realize target navigation mapping.

Technical background

The navigation positioning system of realizing based on optical instrument, electricity means, acoustic means, mechanics means is the method that tradition navigation survey field the most often adopts always.These systems are when realizing navigation mapping function, and its common feature is navigation sensor need to be installed in destination carrier, by obtaining acceleration, speed, position and the attitude information of destination carrier, characterizes the motion state of target.For example, geo-navigation and celestial navigation system need to be installed optical observation equipment on carrier, could measure the carrier attitude vector of other reference point relatively; By gyroscope and accelerometer, three along carrier coordinate system axially install inertial navigation, and then measure line motion and the angular motion information of carrier; Radio navigation and satellite navigation need to be placed without signal on electrical wire receiving equipment on carrier, could measure locus and the speed of carrier.

But navigation, mapping and guidance application for some special dimensions, be difficult to even navigation sensor cannot to be directly installed on to destination carrier and carry out navigator fix, for example volcano monitoring, Terrain Elevation are measured, difference of elevation is measured, and the monitoring of the enemy's important goal in military field and mapping etc.

Conventional optical observation equipment in survey field, without scope is arranged in destination carrier, just can complete the mapping of destination carrier smoothly.Total powerstation for example, under the known condition of self-position, utilize electronic distance measuring instrument (Electronic Distance Measuring, EDM) measure the accurate slope distance from self-position to impact point, utilize transit survey self-position to point to horizontal angle and the vertical angle of impact point vector, finally can realize the accurate measurement to target location.But because transit can only be worked under relative static conditions, therefore cannot realize the navigation of moving target and mapping.

The present invention is based on laser range finder (Laser Distance Finder, LDF), and microelectromechanicmachine machine system (Micro-electromechanical Systems, MEMS) with GPS (Global Positioning System, GPS) integrated navigation and location system (being MEMS/GPS integrated navigation system), the range finding providing, attitude and positional information, design a kind of new target navigation and mapping algorithm, in the situation that destination carrier itself does not have navigation sensor, the problem that still can navigate and survey and draw, and introduce a kind of rational correcting algorithm, the precision of navigation mapping is significantly improved, and be applicable to navigation and the mapping of Static and dynamic destination carrier simultaneously.

Summary of the invention

The object of the invention is to propose a kind of mapping precision higher and be applicable to the target navigation mapping method, particularly a kind of target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation location of dynamic object carrier.

For achieving the above object, the technical solution used in the present invention comprises the following steps:

(1) near observation station o, choose a reference point R, utilize the three-dimensional location coordinates of the accurate witness mark R of MEMS/GPS integrated navigation system

(2) MEMS/GPS integrated navigation system is moved to observation station o, laser range finder is aimed to reference point R.Utilize MEMS/GPS integrated navigation system to measure the position coordinates of observation station o longitude and latitude observation station o forms the relative geographic coordinate system ox of vector oR with reference point R _ty _tz _tcourse angle and pitch angle utilize laser range finder to measure the oblique distance d of vector oR _oR.

(3) according to the measured value of MEMS/GPS integrated navigation system in step (2) and laser range finder, calculating reference point R is at terrestrial coordinate system o _ex _ey _ez _ein three-dimensional location coordinates

Related reference point R three-dimensional location coordinates expression formula be:

$\left[\begin{array}{c}{\tilde{x}}_R^e\\ {\tilde{y}}_R^e\\ {\tilde{z}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\\ d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\end{array}\right]$

In formula, for the transition matrix between geographic coordinate system and terrestrial coordinate system:

(4) according to calculating the three-dimensional location coordinates of reference point R in step (1) and step (3), then by calculating the relative position error utilize the course error of the relative position error to MEMS/GPS integrated navigation system and pitching error adopt least square method to revise;

Related innovation representation is:

$\left[\begin{array}{c}\mathrm{\Δ}\widehat{\mathrm{\φ}}\\ \mathrm{\Δ}\hat{H}\end{array}\right]={(C^T\·C)}^{-1}{C}^T\left[\begin{array}{c}\mathrm{\Δ}{x}_R^e\\ \mathrm{\Δ}{y}_R^e\\ \mathrm{\Δ}{z}_R^e\end{array}\right]$

In formula,

$C=C_t^e\left[\begin{array}{cc}0& d_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}& -d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}& d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\φ}}}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\end{array}\right]$

(5), by laser range finder run-home carrier T, utilize MEMS/GPS integrated navigation to measure observation station o and form the relative geographic coordinate system o of vector oT with target T _tx _ty _tz _tcourse angle and pitch angle utilize laser range finder to measure the oblique distance d of vector oT _oT;

(6) utilize the MEMS/GPS integrated navigation system course error obtaining in step (4) and pitching error course and pitching information to the output of MEMS/GPS integrated navigation system are revised;

Related innovation representation is:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{\mathrm{oT}}={\widetilde{\mathrm{\φ}}}_{\mathrm{oT}}-\mathrm{\Δ}\widehat{\mathrm{\φ}}\\ H_{\mathrm{oT}}={\tilde{H}}_{\mathrm{oT}}-\mathrm{\Δ}\hat{H}\end{array}\right.$

(7) utilize the positional information that records observation station o in step (2), the oblique distance d recording in step (5) _oT, and revised course and pitching information in step (6), calculate the three-dimensional position of target T

The three-dimensional position of related target T expression formula is:

$\left[\begin{array}{c}{\tilde{x}}_T^e\\ {\tilde{y}}_T^e\\ {\tilde{z}}_T^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oT}}\sin H_{\mathrm{oT}}\\ d_{\mathrm{oT}}\cos {\mathrm{\φ}}_{\mathrm{oT}}\cos H_{\mathrm{oT}}\\ -d_{\mathrm{oT}}\sin {\mathrm{\φ}}_{\mathrm{oT}}\cos H_{\mathrm{oT}}\end{array}\right]$

(8) calculate the three-dimensional position of target T repeating step (5)～step (7), can obtain another target T ₁three-dimensional location coordinates calculate impact point T and T ₁between oblique distance and difference of elevation

Oblique distance calculation expression be:

$\tilde{D}=\sqrt{{({\tilde{x}}_{T1}^e-{\tilde{x}}_T^e)}^2+{({\tilde{y}}_{T1}^e-{\tilde{y}}_T^e)}^2+{({\tilde{z}}_{T1}^e-{\tilde{z}}_T^e)}^2}$

Difference of elevation calculation expression be:

In formula, the latitude of target following formula operation is tried to achieve respectively:

A is semimajor axis of ellipsoid, and e is eccentricity of the earth.

Adopt the beneficial effect of method proposed by the invention to be:

(1) in destination carrier self, do not install under the condition of navigation sensor, this method can realize the mapping of navigating of static object and dynamic object, comprises the three-dimensional location coordinates of measurement target, the difference of elevation between two targets and the oblique distance between two targets etc.

(2), based on error analysis, the present invention has designed course and the pitching information correction algorithm of MEMS/GPS integrated navigation.After carrying out course and pitching compensation, can effectively improve the navigation mapping precision of target.

Accompanying drawing explanation

Fig. 1 is realization flow schematic diagram of the present invention.

Fig. 2 is that Coordinate system definition and vector are described schematic diagram.

Fig. 3 is the conversion schematic diagram of two rectangular coordinate system in space.

Fig. 4 a～Fig. 4 c is the target location error analogous diagram that MEMS/GPS integrated navigation system error and LDS error cause; Wherein, Fig. 4 a is the target location error analogous diagram that MEMS/GPS integrated navigation system site error causes; Fig. 4 b is the target location error analogous diagram that the site error of MEMS/GPS integrated navigation system and the linear error of LDF cause; Fig. 4 c is the target location error analogous diagram that the linear error of position, attitude error and the LDF of MEMS/GPS integrated navigation system causes.

Fig. 5 a～Fig. 5 c is that MEMS/GPS integrated navigation system attitude error is under normal value condition, the estimated performance analogous diagram of correcting algorithm to attitude error; Wherein, Fig. 5 a is stochastic error and the Δ H=0.2 ° that only has MEMS/GPS integrated navigation system attitude error and R, Δ φ=0.1 °, d _oRduring=200m, the estimated result of correcting algorithm to attitude error; For there is MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R and Δ H=0.2 °, Δ φ=0.1 °, d in Fig. 5 b _oRduring=200m, the estimated result of correcting algorithm to attitude error; For there is MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R and Δ H=0.2 °, Δ φ=0.1 °, d in Fig. 5 c _oRduring=400m, the estimated result of correcting algorithm to attitude error;

Fig. 6 a～Fig. 6 c is MEMS/GPS integrated navigation system attitude error while being sinusoidal variations, the estimated performance analogous diagram of correcting algorithm to course error; Wherein, Fig. 6 a is stochastic error and the Δ H=0.2 ° sin (2 π t/50) that only has MEMS/GPS integrated navigation system attitude error and R, Δ φ=0.1 ° sin (2 π t/50), d _oRduring=200m, the estimated result of correcting algorithm to course error; Fig. 6 b is for having MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R and Δ H=0.2 ° sin (2 π t/50), Δ φ=0.1 ° sin (2 π t/50), d _oRduring=200m, the estimated result of correcting algorithm to course error; Fig. 6 c is for having MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R and Δ H=0.2 ° sin (2 π t/50), Δ φ=0.1 ° sin (2 π t/50), d _oRduring=400m, the estimated result of correcting algorithm to course error.

Target localization performance comparison analogous diagram before and after Fig. 7 a～Fig. 7 c proofreaies and correct; Wherein, for only there is stochastic error and the Δ H=0.2 ° of MEMS/GPS integrated navigation system attitude error and R in Fig. 7 a, Δ φ=0.1 °, d _oRduring=400m, the comparison of target localization performance before and after proofreading and correct; Fig. 7 b is for having MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R, and Δ H=0.2 °, and Δ φ=0.1 °, time, the comparison of target localization performance before and after proofreading and correct; Fig. 7 c is for having MEMS/GPS integrated navigation system position, attitude error, the stochastic error of LDF linear error and R, and Δ H=0.2 °, and during Δ φ=0.1 °, d _oR=400m and d _oRthe comparison of target localization performance after=200m proofreaies and correct.

Embodiment

1. the location algorithm of destination carrier

(1) coordinate system is described

Describe for convenience destination carrier location algorithm, first define two coordinate systems, as Fig. 1.In figure, terrestrial coordinate system (o _ex _ey _ez _e) coordinate axis and the earth's axis fixed, true origin (o _e) at the earth's core of the earth, o _ex _eaxle under the line with the intersection of Greenwich meridian ellipse on, o _ez _eaxle directed north, o _ey _eaxle is under the line in plane and and o _ex _e, o _ez _eaxle forms right hand rectangular coordinate system.Geographic coordinate system (ox _ty _tz _t) true origin for observation carrier place point on, ox _taxle level refers to east, oy _taxle level refers to north, oz _taxle and ox _t, oy _taxle forms right hand rectangular coordinate system.

O _ex _ey _ez _eand ox _ty _tz _tbetween can pass through cosine transition matrix conversion:

In formula, λ _obe illustrated in o _ex _ey _ez _ethe latitude of observation station o and longitude under coordinate system.

(2) space vector is described

Target T is in the locus of terrestrial coordinate system, and the vector that can consist of the earth's core and T point line is described.For describing observation station o, target T, can be by vector o at the spatial relation of terrestrial coordinate system _eo, o _et and oT are by terrestrial coordinate system vector of unit length (i ₁, i ₂, i ₃) describe, that is:

$\left\{\begin{array}{c}{o}_{e}o=x_o^e{i}_1+y_o^e{i}_2+z_o^e{i}_3\\ o_{e}T=x_T^e{i}_1+y_T^e{i}_2+z_T^e{i}_3\\ \mathrm{oT}=x_{\mathrm{oT}}^e{i}_1+y_{\mathrm{oT}}^e{i}_2+z_{\mathrm{oT}}^e{i}_3\end{array}\right.---\left(2\right)$

In formula, three-dimensional location coordinates for observation station o in terrestrial coordinate system; three-dimensional location coordinates for target T in terrestrial coordinate system; for o _ex _ey _ez _ein system, T point and O point coordinate are poor.

Three vectors in formula (2), meet relation:

o _eT＝o _eo+oT (3)

From (3) formula, the in the situation that of observation station o location aware, if can calculate oT, can obtain impact point T in the locus of terrestrial coordinate system.

(3) position calculation of target T

For vectorial oT, can be by ox _ty _tz _tvector of unit length in coordinate system characterizes:

$\mathrm{oT}=x_{\mathrm{oT}}^t{e}_1+y_{\mathrm{oT}}^t{e}_2+z_{\mathrm{oT}}^t{e}_3---\left(4\right)$

In formula, for the phasor difference between T point in geographic coordinate system and O point.

For compute vectors oT, at ox _ty _tz _tin coordinate system, define a new vector oC:

oC＝0e ₁+de ₂+0e ₃ (5)

In formula, d is the distance that o point is ordered to T.

Known according to formula (5), vectorial oC and oy _tin the same way, its mould length equals the distance that o point is ordered to T to axle.According to Eulerian angle rotation rule (as shown in Figure 3), oT can be by oC successively around oz _t, ox ' _taxle (ox ' _tfor ox _taround oz _tnew coordinate axis behind rotation H angle) rotation H and φ angle obtain, and the transition matrix of twice rotation is as follows respectively.

Rotate for the first time H angle:

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

Rotate for the second time φ angle:

$C_t^1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]---\left(7\right)$

According to the definition of oT and oC, vector oT can be expressed as:

$\mathrm{oT}=C_1^2{C}_t^1\mathrm{oC}---\left(8\right)$

By in formula (6) and (7) substitution formula (8), calculate oT and obtain:

$\left[\begin{array}{c}{x}_{\mathrm{oT}}^t\\ y_{\mathrm{oT}}^t\\ z_{\mathrm{oT}}^t\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{ccc}\cos H& \mathrm{sonH}& 0\\ -\sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ d\\ 0\end{array}\right]=\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]---\left(9\right)$

For the vector of unit length in terrestrial coordinate system and geographic coordinate system, its pass is:

$\left[\begin{array}{c}{i}_1\\ i_2\\ i_3\end{array}\right]=C_t^e\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\end{array}\right]---\left(10\right)$

Known according to formula (10), T point and the coordinate difference of O point in terrestrial coordinate system are:

$\left[\begin{array}{c}{x}_{\mathrm{oT}}^e\\ y_{\mathrm{oT}}^e\\ y_{\mathrm{oT}}^e\end{array}\right]=C_t^e\left[\begin{array}{c}{x}_{\mathrm{oT}}^t\\ y_{\mathrm{oT}}^t\\ y_{\mathrm{oT}}^t\end{array}\right]---\left(11\right)$

Therefore, based on formula (3) and (11), can obtain the position of destination carrier

$\left[\begin{array}{c}{x}_T^e\\ y_T^e\\ z_T^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]---\left(12\right)$

All variablees in formula (12) are all set in the ideal situation, for calculating the position of destination carrier T, and the distance that o point and T are ordered, and the direction of oT can obtain from suitable measurement mechanism.If MEMS/GPS integrated navigation system is arranged on observation carrier along carrier coordinate system, the position of o just can be calculated and be obtained by the outgoing position of MEMS/GPS so, and the direction of oT can obtain by MEMS/GPS Attitude Calculation.In addition, if MEMS/GPS integrated navigation system along carrier coordinate system oy _baxle is accurately installed, and can record distance so by LDF.

Here, define new measurand:

1) the measurement output of MEMS/GPS latitude, longitude and height:

2) measurement of MEMS/GPS course angle, pitch angle and roll angle output:

3) LDF range finding output:

Utilize the measurement output of MEMS/GPS integrated navigation system and LDF, can calculate according to formula (13) three-dimensional location coordinates of observation station o:

In formula, a is the major axis of the earth, and e is eccentricity of the earth.

By measurement result, replace desirable variable, the coordinate of destination carrier is:

(4) oblique distance and difference of elevation calculate

Except to the function of target localization, the present invention can also measure other information, comprising: the oblique distance of two impact points and difference of elevation etc.For calculating two impact point T and T ₁oblique distance and difference of elevation can adopt respectively step (14) to calculate T and T ₁three-dimensional location coordinates according to formula (15) and formula (16), calculate oblique distance and difference of elevation again.

Oblique distance computing method be:

$\tilde{D}=\sqrt{{({\tilde{x}}_{T1}^e-{\tilde{x}}_T^e)}^2+{({\tilde{y}}_{T1}^e-{\tilde{y}}_T^e)}^2+{({\tilde{z}}_{T1}^e-{\tilde{z}}_T^e)}^2}---\left(15\right)$

Difference of elevation computing method be:

In formula, the latitude of target by formula (17), (18) computing, tried to achieve respectively:

In formula, a is semimajor axis of ellipsoid a=6378137.00 rice, and e is eccentricity of the earth: e ²=0.00669438002290.2. the error analysis of location algorithm

Destination carrier navigation mapping algorithm depends on position and the attitude information of MEMS/GPS integrated navigation system, and the ranging information of LDF.Because measuring equipment unavoidably can exist measuring error, will inevitably exert an influence to the position calculation of impact point.Therefore, by error analysis, can grasp impact point site error distribution situation, and can introduce outer secondary supplementary information and proofread and correct.

(1) positioning error being caused by MEMS/GPS integrated navigation system site error

There are two errors in MEMS/GPS integrated navigation system site error, puts site error and the cosine transition matrix of o in the time of will causing calculating destination carrier according to formula (12) and (14), the destination carrier positioning error being caused by MEMS/GPS integrated navigation system site error can be calculated by other error sources.

$\begin{array}{c}{\mathrm{\Δ}}_1={\mathrm{\Δ}}_{11}+{\mathrm{\Δ}}_{12}\text{=}\left[\begin{array}{c}{\mathrm{\Δx}}_o^e\\ {\mathrm{\Δy}}_o^e\\ {\mathrm{\Δz}}_o^e\end{array}\right]+{\mathrm{\ΔC}}_t^e\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]\\ =\left[\begin{array}{c}{\mathrm{\Δx}}_o^e\\ {\mathrm{\Δy}}_o^e\\ {\mathrm{\Δz}}_o^e\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\Δc}}_{11}& {\mathrm{\Δc}}_{12}& {\mathrm{\Δc}}_{13}\\ {\mathrm{\Δc}}_{21}& {\mathrm{\Δc}}_{22}& {\mathrm{\Δc}}_{23}\\ {\mathrm{\Δc}}_{31}& {\mathrm{\Δc}}_{32}& {\mathrm{\Δc}}_{33}\end{array}\right]\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]\end{array}---\left(19\right)$

In formula, ${\mathrm{\Δx}}_o^e={\tilde{x}}_o^e-x_o^e,{\mathrm{\Δy}}_o^e={\tilde{y}}_o^e-y_o^e,{\mathrm{\Δz}}_o^e={\tilde{z}}_o^e-z_o^e,{\mathrm{\ΔC}}_t^e={\tilde{C}}_t^e-C_t^e\·$

Because the error of transition matrix is very little, by with Δ ₁₁item compares, known Δ ₁₂item can be ignored.For example, the first row has following characteristic.

Adopting uses the same method can obtain

For the distance that is less than 1 kilometer, the relation of d and earth radius meets:

d<<R

In formula, R is earth radius.

:

That is:

|Δ ₁₂|<<|Δ ₁₁|

Therefore,

${\mathrm{\&Delta;P}}_1=\left|{\mathrm{\&Delta;}}_1\right|\&ap;\sqrt{{\left({\mathrm{\&Delta;x}}_o^e\right)}^2+{\left({\mathrm{\&Delta;y}}_o^e\right)}^2+{\left(\mathrm{\&Delta;}{z}_o^e\right)}^2}---\left(23\right)$

In formula, Δ P ₁the positioning error causing for MEMS/GPS integrated navigation system site error.

(2) positioning error being caused by LDF error

According to formula (12) and (14), the target location error being caused by LDF can calculate by ignoring other error sources.

${\mathrm{\&Delta;}}_2=C_t^e\left[\begin{array}{c}(\tilde{d}-d)\sin H\\ (\tilde{d}-d)\cos \mathrm{\&phi;}\cos H\\ -(\tilde{d}-d)\sin \mathrm{\&phi;}\cos H\end{array}\right]=C_t^e\left[\begin{array}{c}\mathrm{\&Delta;}d\sin H\\ \mathrm{\&Delta;}d\cos \mathrm{\&phi;}\cos H\\ -\mathrm{\&Delta;}d\sin \mathrm{\&phi;}\cos H\end{array}\right]$

Therefore,

ΔP ₂＝|Δ ₂|＝|Δd| (24)

(3) positioning error that MEMS/GPS integrated navigation system attitude error causes

The target location error being caused by MEMS/GPS integrated navigation system attitude error also can calculate by ignoring other error sources.

${\mathrm{\&Delta;}}_3=C_t^e\left[\begin{array}{c}d(\sin \tilde{H}-\sin H)\\ d(\cos \widetilde{\mathrm{\&phi;}}\cos \tilde{H}-\cos \mathrm{\&phi;}\cos H)\\ -d(\sin \widetilde{\mathrm{\&phi;}}\cos \tilde{H}-\sin \mathrm{\&phi;}\cos H)\end{array}\right]=C_t^e\left[\begin{array}{c}d[\sin (H+\mathrm{\&Delta;H})-\sin H]\\ d[\cos (\mathrm{\&phi;}+\mathrm{\&Delta;\&phi;})\cos (H+\mathrm{\&Delta;H})-\cos \mathrm{\&phi;}\cos H]\\ -d[\sin (\mathrm{\&phi;}+\mathrm{\&Delta;\&phi;})\cos (H+\mathrm{\&Delta;H})-\sin \mathrm{\&phi;}\cos H]\end{array}\right]---\left(25\right)$

Because Δ φ and Δ H are low-angle,

$\left\{\begin{array}{ccc}\sin \mathrm{\&Delta;\&phi;}\&ap;\mathrm{\&Delta;\&phi;}& \cos \mathrm{\&Delta;H}\&ap;1& \mathrm{\&Delta;\&phi;}\&CenterDot;\mathrm{\&Delta;H}<<\mathrm{\&Delta;\&phi;}\\ \cos \mathrm{\&Delta;\&phi;}\&ap;1& \sin \mathrm{\&Delta;H}\&ap;\mathrm{\&Delta;H}& \mathrm{\&Delta;\&phi;}\&CenterDot;\mathrm{\&Delta;H}<<\mathrm{\&Delta;H}\end{array}\right.---\left(26\right)$

According to the condition of formula (25), can obtain:

${\mathrm{\&Delta;}}_3=C_t^e\left[\begin{array}{c}\mathrm{d\&Delta;}H\cos H\\ -\mathrm{d\&Delta;}H\cos \mathrm{\&phi;}\cos H-\mathrm{d\&Delta;\&phi;}\sin \mathrm{\&phi;}\cos H\\ \mathrm{d\&Delta;}H\sin \mathrm{\&phi;}\sin H-\mathrm{d\&Delta;\&phi;}\cos \mathrm{\&phi;}\cos H\end{array}\right]---\left(27\right)$

${\mathrm{\&Delta;P}}_3=\left|{\mathrm{\&Delta;}}_3\right|=d\sqrt{\begin{array}{c}{\left(\mathrm{\&Delta;H}\right)}^2({\cos }^{2}H+{\cos }^2\mathrm{\&phi;}-\cos 2\mathrm{\&phi;}{\sin }^{2}H)+{\left(\mathrm{\&Delta;\&phi;}\right)}^2{\cos }^{2}H\\ +\mathrm{\&Delta;H\&Delta;\&phi;}\sin 2\mathrm{\&phi;}\cos H(\cos H-\sin H)\end{array}}---\left(28\right)$

3. algorithm is proofreaied and correct

In all errors, the positioning precision of attitude error major effect target, while being particularly less than 500m apart from d, the site error that o is ordered and the distance error of d are negligible.For example, when the attitude error of MEMS/GPS integrated navigation system is 0.1deg, apart from d=500 rice in the situation that, the impact point site error causing reaches 0.87 meter.Because the attitude accuracy of MEMS/GPS integrated navigation is on the low side, directly use its attitude output can cause meeting the requirement of destination carrier high precision navigation mapping.Therefore design a kind of correcting algorithm, can improve system performance, thereby improve navigation accuracy.

(1) definition known reference point

At observation station o annex, choose a reference point R, the distance of reference point and observation station is less than 500 meters, and uses MEMS/GPS integrated navigation system to measure its three-dimensional position because MEMS/GPS integrated navigation positioning precision is very high, therefore can think for true value.

For reference point R, can adopt the mode of formula (12) to be described.That is:

$\left[\begin{array}{c}{\tilde{x}}_R^e\\ {\tilde{y}}_R^e\\ {\tilde{z}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin H_{\mathrm{oR}}\\ d_{\mathrm{oR}}\cos {\mathrm{\&phi;}}_{\mathrm{oR}}\cos H_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\mathrm{\&phi;}}_{\mathrm{oR}}\cos H_{\mathrm{oR}}\end{array}\right]---\left(29\right)$

(2) the MEMS/GPS integrated navigation attitude error based on reference point calculates

At observation station o, laser range finder is aimed to reference point R, ignoring error under the condition of Δ d, according to the measured value of MEMS/GPS integrated navigation system and laser range finder, calculating reference point R is at terrestrial coordinate system o _ex _ey _ez _ethree-dimensional location coordinates

$\left[\begin{array}{c}{\tilde{x}}_R^e\\ {\tilde{y}}_R^e\\ {\tilde{z}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\&CenterDot;\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\\ d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\end{array}\right]---\left(30\right)$

By formula (29) and (30), can obtain and know new observational variable, that is:

$\left[\begin{array}{c}{\mathrm{\&Delta;x}}_R^e\\ {\mathrm{\&Delta;y}}_R^e\\ {\mathrm{\&Delta;z}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_R^e-x_R^e\\ {\tilde{y}}_R^e-y_R^e\\ {\tilde{y}}_R^e-y_R^e\end{array}\right]=C_t^e\left[\begin{array}{c}{d}_{\mathrm{oR}}(\sin {\tilde{H}}_{\mathrm{oR}}-\sin H_{\mathrm{oR}})\\ d_{\mathrm{oR}}(\cos \widetilde{\mathrm{\&phi;}}\cos {\tilde{H}}_{\mathrm{oR}}-\cos {\mathrm{\&phi;}}_{\mathrm{oR}}\cos H_{\mathrm{oR}})\\ -d_{\mathrm{oR}}(\sin {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}-\sin {\mathrm{\&phi;}}_{\mathrm{oR}}\cos H_{\mathrm{oR}})\end{array}\right]---\left(31\right)$

In formula, and d _oRcan obtain by MEMS/GPS.By calculating φ and H replaces with $\tilde{H}.$

But because formula (31) is difficult to solve for transcendental equation.For this reason, can carry out linearization to it, order:

$\begin{array}{c}\mathrm{\&phi;}=\widetilde{\mathrm{\&phi;}}-\mathrm{\&Delta;\&phi;}\\ H=\tilde{H}-\mathrm{\&Delta;H}\end{array}---\left(32\right)$

By in formula (32) substitution formula (31), and simplify, formula (31) can be converted into linear equation:

$\left[\begin{array}{c}{\mathrm{\&Delta;x}}_R^e\\ {\mathrm{\&Delta;y}}_R^e\\ {\mathrm{\&Delta;z}}_R^e\end{array}\right]=C_t^e\left[\begin{array}{c}{d}_{\mathrm{oR}}\cos \tilde{H}\mathrm{\&Delta;H}\\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\&phi;}}\cos \tilde{H}\mathrm{\&Delta;\&phi;}-d_{\mathrm{oR}}\cos \widetilde{\mathrm{\&phi;}}\sin \tilde{H}\mathrm{\&Delta;H}\\ -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\&phi;}}\cos \tilde{H}\mathrm{\&Delta;\&phi;}+d_{\mathrm{oR}}\sin \widetilde{\mathrm{\&phi;}}\sin \tilde{H}\mathrm{\&Delta;H}\end{array}\right]---\left(33\right)$

Formula (33) also can be expressed as:

$\left[\begin{array}{c}{\mathrm{\&Delta;x}}_R^e\\ {\mathrm{\&Delta;y}}_R^e\\ {\mathrm{\&Delta;z}}_R^e\end{array}\right]=C_e^t\left[\begin{array}{cc}0& d_{\mathrm{oR}}\cos \tilde{H}\\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\&phi;}}\cos \tilde{H}& -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\&phi;}}\sin \tilde{H}\\ -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\&phi;}}\cos \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\&phi;}}\sin \tilde{H}\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;H}\end{array}\right]=C_t^e{C}_1\left[\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;H}\end{array}\right]---\left(34\right)$

Order to C ^t, Δ φ and Δ H carry out least-squares estimation and obtain:

$\left[\begin{array}{c}\mathrm{\&Delta;}\widehat{\mathrm{\&phi;}}\\ \mathrm{\&Delta;}\hat{H}\end{array}\right]={(C^T\&CenterDot;C)}^{-1}{C}^T\left[\begin{array}{c}{\mathrm{\&Delta;x}}_R^e\\ {\mathrm{\&Delta;y}}_R^e\\ {\mathrm{\&Delta;z}}_R^e\end{array}\right]---\left(35\right)$

(3) MEMS/GPS integrated navigation system attitude output calibration

Utilize the MEMS/GPS integrated navigation system course error having recorded and pitching error the course of MEMS/GPS integrated navigation system and pitching are proofreaied and correct:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{\mathrm{oT}}={\widetilde{\mathrm{\&phi;}}}_{\mathrm{oT}}-\mathrm{\&Delta;}\widehat{\mathrm{\&phi;}}\\ H_{\mathrm{oT}}={\tilde{H}}_{\mathrm{oT}}-\mathrm{\&Delta;}\hat{H}\end{array}\right.---\left(36\right)$

By proofreading and correct, contribute to improve the attitude accuracy of MEMS/GPS integrated navigation system, and then improve target navigation mapping precision.

specific embodiment

(1) object carrier position error is carried out to emulation

The initial position of supposing observation carrier is 126 ° of east longitudes, 45 ° of north latitude.The latitude of MEMS/GPS and longitude error are 0.2m, and the course angle of MEMS/GPS integrated navigation system and pitch angle error are respectively 0.2 ° and 0.1 °.The linear error scope of LDF is 0.15%.Corresponding following 3 conditions, obtain 3 analogous diagram in Fig. 4.In addition, motion carrier is rocking under state, and course angle and pitch angle are respectively H=30 °+7 ° sin (2 π * t/7), φ=2 °+1 ° of sin (2 π * t/7).

Condition 1: the site error that only has MEMS/GPS integrated navigation system; Target location error simulation result is corresponding to Fig. 4 a;

Condition 2: consider the site error of MEMS/GPS integrated navigation system and the linear error of LDF; Target location error simulation result is corresponding to Fig. 4 b;

Condition 3: the linear error of considering position, attitude error and the LDF of MEMS/GPS integrated navigation system; Target location error simulation result is corresponding to Fig. 4 c.

As shown in Fig. 4 a, the target T positioning error and the Δ that by MEMS/GPS integrated navigation system site error, are caused ₁₁relevant, due to be 0.2828, error is almost uncorrelated with coefficient d.As shown in Figure 4 b, when coefficient d approaches 1km, the positioning error of target will reach 1.7m.Except the 0.28m error being caused by condition 1MEMS/GPS integrated navigation system site error, the main error of emulation is from the linear error of LDF.From Fig. 4 c, while comprising attitude error in condition, the positioning error of destination carrier is accumulated rapidly.

In sum, need to effectively proofread and correct the attitude error of MEMS/GPS integrated navigation system.

(2) correcting algorithm emulation

For checking correcting algorithm performance, by providing different condition, carry out emulation experiment.Fundamental simulation condition prerequisite is identical to the condition of object carrier position error emulation with (1); There is the measurement stochastic error of 0.05m in reference point R.Condition setting is as follows:

Condition 1: only have the stochastic error of MEMS/GPS integrated navigation system attitude error and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=200m; The course error angle that correcting algorithm estimation obtains and pitching error angular curve are as shown in Figure 5 a.

Condition 2:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=200m; The course error angle that correcting algorithm estimation obtains and pitching error angular curve are as shown in Figure 5 b.

Condition 3:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=400m; The course error angle that correcting algorithm estimation obtains and pitching error angular curve are as shown in Figure 5 c.

Condition 4: only have the stochastic error of MEMS/GPS integrated navigation system attitude error and R, Δ H=0.2 ° sin (2 π t/50), Δ φ=0.1 ° sin (2 π t/50), d _oR=200m; The course error angular curve that correcting algorithm estimation obtains as shown in Figure 6 a.

Condition 5:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 ° sin (2 π t/50), Δ φ=0.1Dsin (2 π t/50), d _oR=200m; The course error angular curve that correcting algorithm estimation obtains as shown in Figure 6 b.

Condition 6:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 ° sin (2 π t/50), Δ φ=0.1 ° sin (2 π t/50), d _oR=400m; The course error angle that correcting algorithm estimation obtains and pitching error angular curve are as shown in Fig. 6 c.

The Δ H that Fig. 5 a～Fig. 5 c is corresponding and Δ φ true value are set to normal value.From Fig. 5 a, this correcting algorithm can be estimated Δ H and Δ φ angle, and the residual error of Δ H and Δ φ has been reduced to 0.02 °, and this belongs to very high precision for target localization and navigation field.When comprising MEMS/GPS integrated navigation system, after all errors of LDF and R, the precision of algorithm decreases.As Fig. 5 b, the residual error of Δ H is approximately 0.7 °, and the residual error of Δ φ is approximately 0.01 °.But due to d _oRbecome large, the residual error of Δ H drops to 0.04 °, and Δ φ drops to 0.05 °.If i.e. d _oRdistance is far away, and the estimated performance of this correcting algorithm is better, is more conducive to realize accurate target localization.

The Δ H that Fig. 6 a～Fig. 6 c is corresponding and Δ φ true value are set to sinusoidal variations, but have only provided the correlation curve of course error ideal value and estimated value, and the conclusion obtaining is identical while being set to often be worth with Δ H and Δ φ true value.

(3) the target localization emulation after correction

Final error after MEMS/GPS integrated navigation system compensation of attitude error is analyzed emulation, and in order to compare with uncorrected algorithm, all error of MEMS/GPS integrated navigation system is all with identical to destination carrier location algorithm simulated conditions.

Condition 1: only have the stochastic error of MEMS/GPS integrated navigation system attitude error and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=400m;

Condition 2:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=400m;

Condition 3:MEMS/GPS integrated navigation system, all errors of LDF and R, Δ H=0.2 °, Δ φ=0.1 °, d _oR=200m.

From Fig. 7 a and 7b, after MEMS/GPS integrated navigation system compensation of attitude error, positioning error obviously reduces, and with be actually the equivalence of attitude error is estimated, this equivalence attitude error is subject to the impact of o point site error and d range error.In distance, to be reduced to be 0 o'clock, and target location error has also diminished, and this means that correcting algorithm realized partial-compensation to the site error of MEMS/GPS integrated navigation system.In addition, as shown in Figure 7 c, work as d _oR=200m, owing to there being more residual error, the target location error greater than condition 3 that condition 2 produces.

Claims (1)

1. the target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation system, is characterized in that, comprises the steps:

(1) near observation station o, choose a reference point R, utilize the three-dimensional location coordinates of the accurate witness mark R of MEMS/GPS integrated navigation system

(2) MEMS/GPS integrated navigation system is moved to observation station o, laser range finder is aimed to reference point R; Utilize MEMS/GPS integrated navigation system to measure the position coordinates of observation station o longitude and latitude observation station o forms the relative geographic coordinate system ox of vector oR with reference point R _ty _tz _tcourse angle and pitch angle utilize laser range finder to measure the oblique distance d of vector oR _oR;

(3) according to the measured value of MEMS/GPS integrated navigation system in step (2) and laser range finder, calculating reference point R is at terrestrial coordinate system o _ex _ey _ez _ethree-dimensional location coordinates

Related reference point R three-dimensional location coordinates expression formula be:

$\left[\begin{array}{c}{\tilde{x}}_R^e\\ {\tilde{y}}_R^e\\ {\tilde{z}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\&CenterDot;\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\\ d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\end{array}\right]$

In formula, for the transition matrix between geographic coordinate system and terrestrial coordinate system:

(4) according to calculating the three-dimensional location coordinates of reference point R in step (1) and step (3), then by calculating the relative position error utilize the course error of the relative position error to MEMS/GPS integrated navigation system and pitching error adopt least square method to revise;

Related innovation representation is:

$\left[\begin{array}{c}\mathrm{\&Delta;}\widehat{\mathrm{\&phi;}}\\ \mathrm{\&Delta;}\hat{H}\end{array}\right]={(C^T\&CenterDot;C)}^{-1}{C}^T\left[\begin{array}{c}\mathrm{\&Delta;}{x}_R^e\\ \mathrm{\&Delta;}{y}_R^e\\ \mathrm{\&Delta;}{z}_R^e\end{array}\right]$

In formula,

$C=C_t^e\left[\begin{array}{cc}0& d_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}& -d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\\ -d_{\mathrm{oR}}\cos {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\cos {\tilde{H}}_{\mathrm{oR}}& d_{\mathrm{oR}}\sin {\widetilde{\mathrm{\&phi;}}}_{\mathrm{oR}}\sin {\tilde{H}}_{\mathrm{oR}}\end{array}\right]$

(5), by laser range finder run-home carrier T, utilize MEMS/GPS integrated navigation to measure observation station o and form the relative geographic coordinate system o of vector oT with target T _tx _ty _tz _tcourse angle and pitch angle utilize laser range finder to measure the oblique distance d of vector oT _oT;

(6) utilize the MEMS/GPS integrated navigation system course error obtaining in step (4) and pitching error course and pitching information to the output of MEMS/GPS integrated navigation system are revised;

Related innovation representation is:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{\mathrm{oT}}={\widetilde{\mathrm{\&phi;}}}_{\mathrm{oT}}-\mathrm{\&Delta;}\widehat{\mathrm{\&phi;}}\\ H_{\mathrm{oT}}={\tilde{H}}_{\mathrm{oT}}-\mathrm{\&Delta;}\hat{H}\end{array}\right.$

(7) utilize the positional information that records observation station o in step (2), the oblique distance d recording in step (5) _oT, and revised course and pitching information in step (6), calculate the three-dimensional position of target T

The three-dimensional position of related target T expression formula is:

$\left[\begin{array}{c}{\tilde{x}}_T^e\\ {\tilde{y}}_T^e\\ {\tilde{z}}_T^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+C_t^e\&CenterDot;\left[\begin{array}{c}{d}_{\mathrm{oT}}\sin H_{\mathrm{oT}}\\ d_{\mathrm{oT}}\cos {\mathrm{\&phi;}}_{\mathrm{oT}}\cos H_{\mathrm{oT}}\\ -d_{\mathrm{oT}}\sin {\mathrm{\&phi;}}_{\mathrm{oT}}\cos H_{\mathrm{oT}}\end{array}\right]$

(8) calculate the three-dimensional position of target T repeating step (5)～step (7), can obtain another target T ₁three-dimensional location coordinates calculate impact point T and T ₁between oblique distance and difference of elevation

Oblique distance calculation expression be:

$\tilde{D}=\sqrt{{({\tilde{x}}_{T1}^e-{\tilde{x}}_T^e)}^2+{({\tilde{y}}_{T1}^e-{\tilde{y}}_T^e)}^2+{({\tilde{z}}_{T1}^e-{\tilde{z}}_T^e)}^2}$

Difference of elevation calculation expression be:

In formula, the latitude of target following formula operation is tried to achieve respectively:

A is semimajor axis of ellipsoid, and e is eccentricity of the earth.