CN101672655B - Measuring method for position, posture and course of car on the ground based on double shaft photoelectric measurement system - Google Patents

Abstract

The invention relates to a measuring method for position, posture and course of a car on the ground based on a double shaft photoelectric measurement system, wherein the car is provided with an inertial navigation system and a double shaft photoelectric measurement system; when the car is driven in an appointed area, the inertial navigation system provides initial values of the position, the posture and the course of the car; the three given positioned targets are locked by the double shaft photoelectric measurement system so as to measure the azimuth angle and pitch angle of the optic axis vector corresponding to the car coordinate system at the time; the precise position, posture and course of the car are calculated through certain algorithm with original position, posture and course provided by the inertial navigation system and the information of three groups of azimuth angles and pitch angles detected by the double shaft photoelectric system. The invention is simple, fast and accurate to operate, and can serve for a plurality of cars simultaneously; and the method is a new way for rapidly and accurately measuring the position, posture and course of the car on the ground.

Description

Car position, ground, attitude and course measuring method based on double shaft photoelectric measurement system

Technical field

The present invention relates to the accurate fast location of ground car, directed, survey attitude positioning method, particularly utilize the fast accurate positioning and directing technology of ground car of photoeletric measuring system, belong to technical field of navigation and positioning.

Background technology

Land Navigation System (LNS, Land Navigation System) has become the modern important component part of developed country's army material in the world.LNS is the fundamental that confrontation between systems is realized in information-based battlefield, and the operation precision that it not only can improve ground, road battlebus effectively can also greatly improve the quick-reaction capability (QRC) of battlebus and maneuverability, the independence of operation.The key problem that ground car LNS will solve is the technology of positioning and directing, attitude perception quickly and accurately.

Battlebus LNS great majority in land adopt the integrated navigation system of GPS and inertial navigation system (INS) or heading reference system (DR) formation to realize navigator fix at present.The defective that this method exists is: disturbed or can not use wartime under the situation of GPS at GPS, the INS/DR system can not satisfy needed positioning and directing of land battlebus and attitude accuracy owing to the error that adds up in time.

Can not satisfy under the situation that particular task requires in existing Land Navigation System positioning and directing precision, utilize carrier positions, attitude and the course cognition technology of Photodetection system can well solve the positioning and directing and the attitude perception problems of ground car.Existing ground car precise positioning and directional method (application number 200710063640.7) based on Photodetection system is a kind of algorithm based on photoeletric measuring system single shaft (course axle) metrical information, has the following disadvantages:

Can't resolve the attitude (roll, the angle of pitch) and the elevation information that obtain surface car.

Algorithm is based upon on the areal model basis, can't guarantee ground car level in the practical application, and the attitude angle of car body can cause the Photodetection system course angle to measure coupling error, thereby influences final positioned/oriented precision.

Summary of the invention

The technical problem to be solved in the present invention is: at the problem that has Land Navigation System now and exist based on the quick positioning and directing algorithm of the ground car of single shaft optical detection system, invented a kind of quick position of ground car, attitude, course measuring method based on double shaft photoelectric measurement system.

Based on the quick position of ground car, attitude, the course measuring method (invention is called for short in the back) of double shaft photoelectric measurement system, its summary of the invention mainly comprises:

1, invention proposes on the vehicle inertial navigation system INS and double shaft photoelectric measurement system to be housed.Vehicle utilizes the double shaft photoelectric measurement system optical axis to aim at three known location target centers successively after sailing into and specifying emitting area, utilizes double shaft photoelectric measurement system can record this moment optical axis vector at the position angle and the angle of pitch of carrier coordinate system.Utilize the position angle and the angle of pitch information of the carrier coordinate system of photoeletric measuring system measurement, the car body initial attitude information that inertial navigation system provides is finished the measurement in car position, ground, attitude, course fast.

2, invention proposes the correction algorithm of original double shaft photoelectric measurement system metrical information.Orientation, angle of pitch information under the carrier coordinate system that measures in the car body initial attitude information correction 1 that utilizes inertial navigation to provide obtain locking position angle and the angle of pitch of optical axis vector under approximate geographic coordinate system.

3, invention proposes car body attitude angle, height calculation method.

Invention proposes to utilize the correction back bearing that calculates in 2, the position of target (containing longitude, latitude), car body initial position/course calculate the site error and the course error of car body, and utilize the position/course error correction car body initial position/course that obtains.

Invention proposes to utilize position (contain longitude, latitude, highly), the elemental height of car body, the front of the angle of pitch after the correction that calculates in 2, target to obtain attitude error and the height error that revised car body position/course calculates car body, and utilize the height/attitude error correction car body elemental height/attitude that obtains.

4, invention proposes correction, iterative calculation method, and revising iterative calculation method is that algorithm is realized the key that high precision is resolved.The correction iterative calculation method is: calculate calibration corrections and do not reach designated precision (greater than preset threshold value) when 3,, repeated 2,3,4 and carry out iterative calculation as initial value by 3 revised car body position, attitude and the courses that calculate; When 3 calibration corrections that calculate reach designated precision (smaller or equal to preset threshold value), export revised car body position, attitude, course as the accurate position of ground car, attitude, course information.

Characteristic feature of an invention mainly comprises:

The present invention proposes to utilize double shaft photoelectric measurement system.Double shaft photoelectric measurement system is position angle, the angle of pitch information of the relative bodywork reference frame of measurement target drone simultaneously.

The present invention proposes and utilize the car body initial attitude that position angle, angle of pitch metrical information under the car body system are revised, obtain position angle, angle of pitch information in the approximate geographic coordinate system.Utilize the position angle that is similar under the Department of Geography to carry out the cocked hat method and resolve, can avoid the coupling of car body attitude to cause car body position, course resolution error.

The present invention proposes to utilize angle of pitch metrical information, course angle information, the positional information under the approximate geographic coordinate system, calculates the algorithm of height of the carbody, attitude angle.

The present invention proposes with the iterative calculation method of inertial navigation position/attitude/course as initial value.In the iterative algorithm metrical information under the carrier system is transformed into approximate geographic coordinate system, utilize the measurement of angle information under the approximate Department of Geography to carry out resolving of position/attitude/course, can guarantee that position/attitude/course algorithm always works in the low-angle linear areas, and finally accomplish to resolve accurately by continuous correction, avoided because the approximate error of bringing of linearization of attitude algorithm has also been avoided because position/course error of calculation that attitude error causes simultaneously.

Description of drawings:

Fig. 1 is an invention practical application sight synoptic diagram;

Fig. 2 is based on the leg-of-mutton quick positioning and directing algorithm synoptic diagram of plane error;

Fig. 3 is double shaft photoelectric measurement system optical axis azimuth vector, angle of pitch measuring principle synoptic diagram in the invention;

Fig. 4 is car position, ground, attitude, the quick Measurement Algorithm schematic flow sheet in course in the invention.

Embodiment:

Navigational system INS or DR system and double shaft photoelectric measurement system are housed on the car of ground, and three accurate targets in position are laid at the launching site.INS or DR system can provide vehicle rough position and attitude/course information

Three target exact positions, launching site are known

Surface car sail into specify locating area (the triangle center zone that target constitutes, as shown in Figure 1), algorithm of the present invention implements mainly to be divided into that initial value is given, optical measurement, iterative calculation three parts:

● utilize the twin shaft Photodetection system that three targets are aimed at measurement

At first by car body initial position/course information The positional information of target

By formula (1) calculate the relative optical axis of first target angle (α ', β '), be used for rotating automatically optical axis, the target point enters the visual field of Photodetection system, can adopt artificial this moment or automated manner aims at (guaranteeing that target's center is positioned at the center, visual field) to target, can measure twin shaft photodetection platform at this moment the relative bodywork reference frame of optical axis vector orientation/angle of pitch (α, β) _c, as shown in Figure 3.

R wherein _eBe earth radius.

The process of repetition is finished the measurement of three targets, obtains one group of metrical information (α _i, β _i) _c

● the metrical information in step is resolved car body position, attitude, course in the utilization

The information that resolving car body position/attitude/course needs comprises:

Initial position/attitude/course

Target position information

Measurement of angle information (α _i, β _i) _c

Through after measuring information, the algorithm by the present invention's proposition can exact solution be calculated the position/attitude/course of car body.Algorithm resolves process can be divided into for five steps as shown in Figure 4, specific as follows:

The first step: calculate initial value

Position/the course of car body/attitude

Assignment is that vehicle-mounted inertial navigation provides

Second step: the metrical information under the car body system is transformed into approximate geographic coordinate system

The measurement course, the angle of pitch that obtain when utilizing photoelectric platform locking target i are (α _i, β _i) _c, the attitude angle information (ψ of bodywork reference frame ₀, θ ₀, γ ₀).

Utilizing down, formula calculates the measurement of angle information (α that is similar under the geographic coordinate system _i, β _i) _e:

${\left[\begin{array}{c}-{\cos \mathrm{\β}}_i\sin {\mathrm{\α}}_i\\ \cos {\mathrm{\β}}_i\cos {\mathrm{\α}}_i\\ \sin {\mathrm{\β}}_i\end{array}\right]}_e=\left(\begin{array}{ccc}\cos {\mathrm{\ψ}}_0& -\sin {\mathrm{\ψ}}_0& 0\\ \sin {\mathrm{\ψ}}_0& \cos {\mathrm{\ψ}}_0& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\θ}}_0& -\sin {\mathrm{\θ}}_0\\ 0& \sin {\mathrm{\θ}}_0& \cos {\mathrm{\θ}}_0\end{array}\right)\left(\begin{array}{ccc}\cos {\mathrm{\γ}}_0& 0& \sin {\mathrm{\γ}}_0\\ 0& 1& 0\\ -\sin {\mathrm{\γ}}_0& 0& \cos {\mathrm{\γ}}_0\end{array}\right){\left[\begin{array}{c}-\cos {\mathrm{\β}}_i\sin {\mathrm{\α}}_i\\ \cos {\mathrm{\β}}_i\cos {\mathrm{\α}}_i\\ \sin {\mathrm{\β}}_i\end{array}\right]}_c$

Last formula the right all is known measuring amount, can obtain by following formula $\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\end{array}\right]:$ $\left(\begin{array}{ccc}\cos {\mathrm{\&psi;}}_0& -\sin {\mathrm{\&psi;}}_0& 0\\ \sin {\mathrm{\&psi;}}_0& \cos {\mathrm{\&psi;}}_0& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\&theta;}}_0& -\mathrm{<figure-callout\; id="0"\; label="sin"\; filenames="H2009100931534E00022.png"\; state="\{\{state\}\}">sin</figure-callout>}\\ 0& \sin {\mathrm{\&theta;}}_0& \cos {\mathrm{\&theta;}}_0\end{array},{\mathrm{\&theta;}}_0\right)\left(\begin{array}{ccc}\cos {\mathrm{\&gamma;}}_0& 0& \sin {\mathrm{\&gamma;}}_0\\ 0& 1& 0\\ -\sin {\mathrm{\&gamma;}}_0& 0& \cos {\mathrm{\&gamma;}}_0\end{array}\right){\left[\begin{array}{c}-\cos {\mathrm{\&beta;}}_i\sin {\mathrm{\&alpha;}}_i\\ \cos {\mathrm{\&beta;}}_i\cos {\mathrm{\&alpha;}}_i\\ \sin {\mathrm{\&beta;}}_i\end{array}\right]}_c=\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\end{array}\right]$

Calculate the measurement (α that is similar under the geographic coordinate system by following formula _i, β _i) _e:

β _i＝sin ^-1(C ₃)；

α _i＝tan ^-1(C ₁/C ₂)；

The 3rd step: car body position, course are resolved in the plane

Metrical information (α under the approximate geographic coordinate system that calculated by the last step _i, β _i) _e, the position of target Initial position/the attitude of car body/course

Resolve the position of ground car by the plane error triangulation method With course error Δ ψ ₀

The plane error solution of a triangle is that " based on the ground car positioning and directing method of optical detection system " (200710063640.7) propose, and sees appendix about the detailed description that cocked hat resolves.

Utilization calculates

Upgrade the position/attitude/course of car body according to following formula

The 4th step: solid is resolved height of the carbody, attitude

With bodywork reference frame position, the attitude after the renewal of last step, the angle of pitch under the positional information of target, the approximate Department of Geography

Bring formula (5) into and get i=1,2,3, can be about Δ h ₀, Δ θ ₀, Δ γ ₀System of linear equations, can calculate height, attitude error (the Δ h of car body in view of the above ₀, Δ θ ₀, Δ γ ₀).

$\frac{1/d_i}{1+{\left(\frac{h_i-h_0}{d_i}\right)}^2}\mathrm{\&Delta;}{h}_0-\cos {\mathrm{\&psi;}}_0\mathrm{\&Delta;}{\mathrm{\&theta;}}_0+\sin {\mathrm{\&psi;}}_0\mathrm{\&Delta;}{\mathrm{\&gamma;}}_0=-{\tan }^{-1}\left(\frac{h_i-h_0}{d_i}\right)+{\mathrm{\&beta;}}_{\mathrm{ie}}---\left(5\right)$

D wherein _iBe the distance of target to car, computing formula is

By the position height that calculates, roll angle, angle of pitch error (Δ h ₀, Δ θ ₀, Δ γ ₀) the correction bodywork reference frame

The 5th step: with revised position and attitude

Iterative calculation in second step of substitution reaches predetermined accuracy until the round-off error amount, obtains the accurate position of car body, attitude and course

Specific embodiment:

In order to verify the feasibility of the quick position of ground car that the present invention proposes, attitude, course measuring method based on the biaxial optical detection system, utilize the biaxial optical detection system of Beijing Institute of Aeronautics development, on the river shoal in the Ming Tombs Reservoir, Beijing, carried out confirmatory experiment of the present invention.

The angle-measurement accuracy of biaxial optical detection system is 30 ", testing the target positional precision that hits is 5cm.In the experiment in the process car body holding position/attitude/course motionless, by the repeated metric algorithm precision of the checkout result under the combination of different targets.Experimental result is as shown in the table:

As can be seen from the results, the algorithm that this patent proposes (under existing experiment condition) site error is better than 0.5m, and the resolution error of course angle/attitude angle is better than 30 ".Experiment show the method that the present invention told be effectively, and can reach very high calculation accuracy, have broad application prospects.

It should be noted last that: replace all modification or the parts that do not break away from basic ideas of the present invention and the scope, all should be encompassed in the claim scope of the present invention.

Appendix 1

Based on the leg-of-mutton quick positioning and directing algorithm of plane error

Ground car stand is O, and this geographic north is to being ON, and is as shown below.A, B, C are target.The ground car is when O, and the car body longitudinal axis north orientation declination error that mobile unit is given is Δ ψ.The target position angle that the photodetection platform measures in Department of Geography's surface level is α _i

As shown in Figure 2, cross the A point and be the straight line AD that is parallel to north orientation.Make straight line AA ' after the A point, the angle between this straight line and AD is ψ _M1, that is: ∠ DAA '=ψ _M1If north orientation error angle Δ ψ=0 that navigator provides, straight line AA ' and AO overlap so.In like manner, cross B, C point and make straight line BB ' and CC ' respectively, three straight line AA ', BB ' and CC ' intersect in twos, formation triangle A ' B ' C ', and this triangle is called cocked hat, and the size of cocked hat is relevant with the size of error angle Δ ψ.If the maximal value of Δ ψ is Δ ψ _MaxAdjust the angle of ∠ DAA ' and straight line BB ' and CC ' correspondence respectively, make it at [ψ _Mi-Δ ψ _Max, ψ _Mi+ Δ ψ _Max] between corresponding change continuously, each angle step that changes is

Wherein n is an angle graduation number, can will ask for according to the orientation accuracy of system.Whenever get one group of angle value, form a cocked hat by corresponding straight line AA ', BB ' and CC ', and the true geographic coordinate of ground car (x is y) in the determined zone of this triangle.When getting certain group angle value, the length of side sum minimum of the cocked hat that obtains, ∠ DAA ', ∠ DBB ' and the ∠ DCC ' that obtain this moment can think the true bearing angle of target with respect to car body, the north orientation that contrast initial time mobile unit provides, just can draw the north orientation error, obtain the planimetric position of ground car simultaneously.

Claims (3)

1. based on car position, ground, attitude, the course measuring method of double shaft photoelectric measurement system, it is characterized in that step is as follows:

(1) inertial navigation system and double shaft photoelectric measurement system are housed on the vehicle, vehicle sails the appointment locating area into, and inertial navigation system provides position, attitude, course initial value for vehicle;

(2) utilize double shaft photoelectric measurement system to lock three known location targets respectively, when the locking target, measure the position angle and the angle of pitch of three relative bodywork reference frames of optical axis vector;

(3) the car body initial attitude that utilizes inertial navigation system to provide is transformed into optical axis vector orientation, angle of pitch information under the bodywork reference frame of measuring in the step (2) under the approximate geographic coordinate system, obtains position angle and the angle of pitch of three optical axis vectors under approximate geographic coordinate system;

(4) calculate car body position and the course error that inertial navigation system provides by the position and the position angle of photoeletric measuring system locking target time axial vector under approximate geographic coordinate system of above-mentioned three targets; Calculate car body attitude and the height error that inertial navigation system provides by target position, car body position and course and the angle of pitch of photoeletric measuring system locking target time axial vector under approximate geographic coordinate system;

(5) when the described car body of step (4) position, car body attitude, course error and height error correction during smaller or equal to a certain pre-set threshold, described car body position and course error and described car body attitude and height error correction car body position, attitude and the course of at first using (4) to calculate are exported the accurate position of car body, attitude and course information then; When the described calibration corrections of step (4) during greater than a certain pre-set threshold, described car body position and course error and described car body attitude and height error correction car body position, attitude and the course of at first using (4) to calculate, and with revised position, attitude and course as initial value, repeating step (3) (4).

2. car position, ground, attitude, course measuring method based on double shaft photoelectric measurement system according to claim 1 is characterized in that: two of orientation, pitching that described double shaft photoelectric measurement system measurement dimension is a carrier are axial.

3. car position, ground, attitude, course measuring method based on double shaft photoelectric measurement system according to claim 1, it is characterized in that: the attitude and the high computational process that propose in the described step (4), adopted near Nonlinear System of Equations linearizing method true value.

Images