CN105068104A - Positioning method based on inertia/double-star discontinuous pseudo-range constraint - Google Patents

Abstract

The invention discloses a positioning method based on inertia/double-star discontinuous pseudo-range constraints. A stable and reliable positioning result can be obtained by means of position information and pseudo-range information about two satellites in many periods as well as relative movement information of inertia measurement, and will not be affected by inertia navigation existing position errors. Discontinuous satellite information is utilized, and requirements for time intervals are not fixed. Therefore, a constraint method can be utilized for positioning as long as a GNSS provides positions and pseudo-ranges of the two satellites in many periods, even though satellite signals are extremely ineffective. In this way, the stability and the precision of a combined navigation system are effectively improved.

Description

A kind of localization method being interrupted pseudorange constraint based on inertia/double star

Technical field

The invention belongs to integrated navigation technology field, relating to a kind of few visible star combined positioning method newly, stable, believable positioning result can be obtained when only having two visible Navsats, and not by the impact of the existing site error of inertial navigation.

Background technology

The navigational system being applicable to carrier navigation at present has a variety of, single navigational system generally has its weak point, GNSS/INS (GPS (Global Position System)/inertial navigation system) integrated navigation system, overcome the shortcoming of INS errors accumulation, and satellite navigation system renewal frequency is low, the shortcoming be easily disturbed, becomes the navigate mode of current a kind of main flow.

Such as, but in some cases, under receiving antenna is blocked or carrier height dynamic mobility, satellite navigation signals continued disturbed condition, satellite navigation performance significantly reduces and even lost efficacy.When visible satellite number is less than four, GNSS/INS pine integrated positioning precision will decline, and when satellite-signal interrupts providing locating information, loose combined system is destroyed, and whole navigational system performance reduces rapidly.The GNSS/INS tight integration navigational system being observed quantity with pseudorange and pseudorange rates, can utilize and be less than four satellite informations and combine, the result that output accuracy combines higher than pine, has certain antijamming capability.But due to the disappearance that systematic perspective is measured, tight integration navigational system positioning error is still comparatively large, even disperses.Lost efficacy in GNSS signal, when only having two star signals, conventional GNSS/INS tight integration navigation cannot provide for a long time, navigator fix result accurately.How to lose efficacy at satellite-signal, and when visible satellite number is less than four, provided stable, reliable navigator fix result significant.

When satellite-signal lost efficacy, for improving precision and the robustness of system, scholars proposed many algorithms in recent years.Solve the deficiency of observed quantity, can adopt virtual satellite method, the method is by increasing the quantity of virtual satellite, and completion is observed, thus constructs the observed quantity made new advances, and improves system stability and precision.Odometer and GPS pseudo-range information are carried out the method merged by nonlinear filtering wave pattern, compare with the navigational system of routine based on EKF, when visible star less, its precision increases.Volume Kalman filtering navigational system to a certain degree also can improve the navigation accuracy under satellite-signal deletion condition, improves the ability that system adapts to complex environment.

Summary of the invention

Severe at satellite-signal, under only having two visible satellite situations, the satellite information of single point in time is difficult to the position obtaining carrier exactly, and conventional Kalman filtering integrated navigation and location is easily dispersed and is difficult to ensure precision; If utilize the satellite information in multiple moment and inertial navigation to combine, then carrier positions can be exported.The object of the invention is to overcome the scarce deficiency of existing GNSS/INS integrated navigation technology when satellite-signal lost efficacy, in view of under two visible satellite conditions, between multiple moment, GNSS is to the constraint of carrier absolute position and INS to the constraint of carrier relative position, provides a kind of localization method being interrupted pseudorange constraint based on inertia/double star.The inventive method make full use of not in the same time between satellite, Inertia information, use discontinuous satellite information, and requirement is not fixed for time at intervals size, even if so when satellite-signal lost efficacy serious, as long as GNSS provides satellite position and the pseudorange in several moment, just with the method location of constraint, can effectively improve stability and the precision of integrated navigation system.

A kind of localization method being interrupted pseudorange constraint based on inertia/double star of the present invention, has been come by following step:

Step one: choose the moment point that more than 4 possess two visible navigation satellite signals, moment point interval is determined according to carrier movement speed, with first moment for reference point, inertial navigation system (InertialNavigationSystem, INS) is utilized to calculate relative displacement between each moment and reference point under navigational coordinate system;

Step 2: according to position, the pseudo-range information of two satellites of each moment chosen, calculate the parameter of this moment point carrier positions feasible solution track, comprising: the track center of circle, orbit radius, orbit plane normal vector;

Step 3: utilize planar process vector, obtain the rotation angle value of twice rotational transform that each moment by orbit coordinate is with being tied to the earth's core admittedly;

Step 4: according to each moment feasible solution orbit parameter, by coordinate transform, will not in the same time under navigational coordinate system feasible solution track unified under heart is admittedly;

Step 5: export height or height subsidiary and the initial ranging in feasible solution parametric orbit equations determination reference point moment according to inertial navigation interval;

Step 6: the relative movement orbit obtained with inertial navigation system between each moment is constraint, carry out fast search in initial ranging interval, according to search error size determination Search Results, reduce the region of search, search is until meet accuracy requirement repeatedly, exports positioning result.

The invention has the advantages that:

(1) the inventive method make full use of not in the same time between satellite, Inertia information, according to restriction relation between the two, single moment that can realize position exports or exports continuously, not by the impact of the existing site error of inertial navigation, improve the positioning precision of integrated navigation system under two visible satellites;

(2) the present invention uses discontinuous satellite information, and requirement is not fixed for time at intervals size, even if so when satellite-signal lost efficacy serious, as long as GNSS provides position and the pseudorange of several moment two satellites, just with the method location of constraint, good algorithm stability and system robustness can be had;

(3) the present invention does not produce complicated matrix operation in computation process, and the use of fast search more can reduce searching times, avoids unwanted calculating, and its calculated amount, much smaller than conventional tight integration algorithm, has good practicality.

(4) this algorithmic method is simple, is easy to operation.

Accompanying drawing explanation

Fig. 1 is the inventive method process flow diagram;

Fig. 2 is the spatial relation figure of two visible satellites and carrier in ECEF coordinate system;

Fig. 3 is that inertia of many moment/double star is interrupted pseudorange constraint localization method schematic diagram;

Fig. 4 a is under this example condition, the latitude error of conventional tight integration under two visible satellites;

Fig. 4 b is under this example condition, the longitude error of conventional tight integration under two visible satellites.

Embodiment

Below in conjunction with drawings and Examples, the present invention is described in further detail.

In the present embodiment the initial position of carrier be east longitude 110 °, north latitude 30 °, highly for 1000m; Carrier in east orientation, north orientation and sky to initial velocity be respectively 0m/s, 298m/s, 26m/s; Initial horizontal roll angle 0 °, the angle of pitch 5 °, course angle 0 °.Carrier remains a constant speed rectilinear flight in motion process, and its attitude angle is constant.The error characteristics of inertia device 3 axles are consistent, and gyroscope zero partially error is 1 (°)/h, and Gyroscope Random Drift is 200 (°)/h; Accelerometer bias error is 1mg/h, and accelerometer stochastic error is 20mg/h; GPS pseudorange stochastic error is 10m.Subsystems and integrated navigation resolve the cycle: flight path sampling period T=0.01s, strapdown resolves cycle 2T=0.02s, satellite position and pseudorange cycle 100T=1s, conventional tight integration navigation cycle 100T=1s.Initial ranging precision λ in pseudorange bounding algorithm ₀for 500m.Apply a kind of localization method being interrupted pseudorange constraint based on inertia/double star provided by the invention, process flow diagram as shown in Figure 1, realize constraint location through the following steps:

Step one: choose the moment point k, the k that possess two visible navigation satellite signals ₁, k ₂, k ₃, the moment point time interval is determined according to the precision of carrier movement speed and inertial navigation system.With the k moment for reference point, utilize subsidiary to correct the initial velocity of INS and initial attitude, utilize inertial navigation system to calculate each moment point and the alternate position spike vector with reference to moment point.K, k ₁, k ₂, k ₃the position that moment inertial navigation system exports is P _ik, P _ik1, P _ik2, P _ik3, then position vector is poor

Step 2: according to position, the pseudo-range information of two satellites of each moment chosen, calculate the parameter of this moment point carrier positions feasible solution track, comprising: the track center of circle, orbit radius, orbit plane normal vector;

As shown in Figure 2, satellite S ₁(x ₁y ₁z ₁), S ₂(x ₂y ₂z ₂) around earth high-speed cruising, satellite receiver can obtain the pseudorange ρ of carrier to satellite ₁, ρ ₂with the positional information of satellite.Carrier positions feasible solution track be respectively with two satellites be the centre of sphere, with the joining of respective pseudorange two Spatial Spheres that are radius, equation expression formula:

$\left\{\begin{array}{c}\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}={\mathrm{\ρ}}_1\\ \sqrt{{(x-x_2)}^2+{(y-y_2)}^2+{(z-z_2)}^2}={\mathrm{\ρ}}_2\end{array}\right.$

Therefore any time, can determine a feasible solution track by the position of two satellites and pseudorange, this orbital curve is a space circle, and make two satellite distances be d, track radius of circle is r, central coordinate of circle C (x _cy _cz _c), disk normal vector is $\stackrel{\→}{n}={\left(\begin{array}{ccc}{x}_n& y_n& z_n\end{array}\right)}^T,$ Wherein z _n>=0, then:

Wherein: p is intermediate variable, the leg-of-mutton semi-perimeter that two satellites and carrier are formed is represented.

Track center of circle C is positioned at S ₁s ₂on line, make C to S ₂distance is Δ, represents the center of circle with co-ordinates of satellite:

$\left\{\begin{array}{c}\mathrm{\Δ}=\sqrt{{\mathrm{\ρ}}_2^2-r^2}\\ \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\frac{\mathrm{\Δ}}{d}(\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]-\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right])+\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]\end{array}\right.$

Step 3: utilize planar process vector, obtain the rotation angle value of twice rotational transform that each moment by orbit coordinate is with being tied to the earth's core admittedly:

$\left\{\begin{array}{c}\mathrm{\θ}=\frac{\mathrm{\π}}{2}-si{n}^{-1}\left(\frac{n_z}{\left|\right|\stackrel{\→}{n}\left|\right|}\right)\\ \mathrm{\ψ}=ta{n}^{-1}\left(\frac{n_y}{n_x}\right)\end{array}\right.$

Wherein: ψ and θ be respectively by normal vector determine around the anglec of rotation of z ' axle and the anglec of rotation around y ' axle.

Step 4: according to each moment feasible solution orbit parameter, by coordinate transform, will not in the same time under navigational coordinate system feasible solution track unified under heart is admittedly;

As shown in Figure 3, T _k, with for the carrier positions feasible solution track do not determined according to two visible satellite information in the same time, with the feasible solution track center of circle for initial point, with normal vector for z ' axle forms the parametric orbit equations separated in Descartes's right-handed coordinate system:

Wherein: for equation parameter.Position can be separated Standard parametric equation through the change of twice rotational coordinates and be transformed into parametric equation under ECEF coordinate system:

$\left\{\begin{array}{c}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=C_1{C}_2\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]+\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]\\ C_1=\left[\begin{array}{ccc}cos\left(\mathrm{\ψ}\right)& -\sin \left(\mathrm{\ψ}\right)& 0\\ sin\left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\ψ}\right)& 0\\ 0& 0& 1\end{array}\right]\\ C_2=\left[\begin{array}{ccc}\cos \left(\mathrm{\θ}\right)& 0& sin\left(\mathrm{\θ}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\θ}\right)& 0& cos\left(\mathrm{\θ}\right)\end{array}\right]\end{array}\right.$

Wherein: C ₁with C ₂transformation matrix of coordinates corresponding to twice rotational transform.

Step 5: export height or height subsidiary and the initial ranging in feasible solution parametric orbit equations determination reference point moment according to inertial navigation interval;

P _kfor k moment feasible solution track T _kon point, the interval L of initial ranging separates the segmental arc on track near carrier positions, by external height information, as barometric altimeter is determined:

L＝{P _k|h∈[h _ob-3Δh,h _ob+3Δh],P _k∈T _k}

Wherein: h _obfor the Aided height information that inertial navigation or external height measurement provide, Δ h is the square error of elevation carrection, and h is P _kthe height value that point is corresponding.Based on parametric equation, by calculating the parameter value that in above formula, two height boundary track points are corresponding the interval available parameter of L come equivalently represented:

Step 6: the relative movement orbit obtained with inertial navigation system between each moment is constraint, carries out P in initial ranging interval _kthe fast search of position optimum solution, according to search error size determination Search Results, reduce the region of search, search is until meet accuracy requirement repeatedly, exports positioning result.

The radian step-length of searching in L interval can according to the desired locations precision λ of search, the scouting interval distance namely on feasible solution track, obtains

With the λ that numerical value is larger ₀for initial coarse search precision, step-size in search corresponding can be obtained fom the above equation to each step-length in L interval corresponding spaced points P _k, calculate its site error Δ:

Wherein for track on point, if alternate position spike is vectorial with P _kfor starting point, then with its terminal and track distance as site error component Δ _i.

The site error Δ of each Searching point embodies the effect constrained each other between this some place feasible solution track and alternate position spike vector, the T that the minimum Min of Select Error (Δ) is corresponding in all Search Results _kupper some P _k' as first result of searching for, its corresponding equation parameter is the region of search is reduced into:

Adjustment search precision is λ ₁(λ ₁< λ ₀), at interval L ₁search obtains result again, so circulates, until precision meets, the result of getting final search is final outgoing position.

According to above-mentioned steps, this simulation results on examples is as shown in Fig. 4, table 1.Fig. 4 is the simulation result of standard Kalman filtering algorithm under two visible satellites, and wherein Fig. 4 a is latitude error, and Fig. 4 b is longitude error.Table 1 is adjustment double star interval time, after carrying out Multi simulation running, and the pseudorange constraint positioning error table obtained.

Table 1 pseudorange constraint positioning result square error (m)

I, can be found out by Fig. 4 a, Fig. 4 b: in 500s simulation time, when only having two visible satellites, standard Kalman filtering system GNSS and INS can also array output, has certain anti-interference.But the resultant error that location exports increases sharply, and system model is no longer stable, and about 100s system is dispersed.

II, as seen from Table 1: different two visible satellite information of being interrupted the moment divide the alternate position spike obtained vectorial with the product of inertia, the method retrained by pseudorange, can obtain a stable positioning result, positioning error is between 100m to 300m.

Under position difference vector accurately prerequisite, be improve positioning precision, choose the satellite spacing time longer, it is larger that orbital curve spatial diversity is separated in the position in two moment, and effect of contraction between alternate position spike vector is stronger, obtains positioning result more accurate.But interval time is longer, inertial navigation integration accumulated error is larger, therefore should determine the concrete time interval according to inertial navigation system precision.In view of in this example, inertance element measuring accuracy is low, have employed the time interval of 20 ~ 60s.

The key that inertia/double star is interrupted pseudo-range integration location obtains multiple not carrier relative movement track in the same time, owing to there is the multiple subsidiary such as air speed, magnetic heading in real system, relative motion in short time interval can be obtained by inertia resolving high precision, the method only utilizes 3 ~ 6 interruption double star pseudo range measurements to realize effective location, and has nothing to do with inertial navigation initial position error.

Pseudorange constraint is compared when only having two visible satellites with conventional tight integration system, output one more stable with positioning result accurately, and be based on interruption satellite information, improve the anti-interference of system.

The present invention is based on the relative movement information of the positional information of two satellites in multiple moment, pseudo-range information and inertia measurement, make full use of the restriction relation between them, thus can effectively suppress the inertia error of calculation to be accumulated, realize effective estimation of carrier positions, there is actual engineer applied meaning.

Claims (5)

1. be interrupted a localization method for pseudorange constraint based on inertia/double star, comprise following step:

Step one: choose the moment point k, the k that possess two visible navigation satellite signals ₁, k ₂, k ₃, the moment point time interval is determined according to the precision of carrier movement speed and inertial navigation system; With the k moment for reference point, utilize subsidiary to correct the initial velocity of INS and initial attitude, utilize inertial navigation system to calculate each moment point and the alternate position spike vector with reference to moment point; K, k ₁, k ₂, k ₃the position that moment inertial navigation system exports is P _ik, then position vector is poor

Step 2: according to position, the pseudo-range information of two satellites of each moment chosen, calculate the parameter of this moment point carrier positions feasible solution track, comprising: the track center of circle, orbit radius, orbit plane normal vector;

Step 3: utilize planar process vector, obtain the rotation angle value of twice rotational transform that each moment by orbit coordinate is with being tied to the earth's core admittedly:

Step 4: according to each moment feasible solution orbit parameter, by coordinate transform, will not in the same time under navigational coordinate system feasible solution track unified under heart is admittedly;

Step 5: export height or height subsidiary and the initial ranging in feasible solution parametric orbit equations determination reference point moment according to inertial navigation interval;

P _kfor k moment feasible solution track T _kon point, the interval L of initial ranging separates the segmental arc on track near carrier positions:

L＝{P _k|h∈[h _ob-3Δh,h _ob+3Δh],P _k∈T _k}

Wherein: h _obfor the Aided height information that inertial navigation or external height measurement provide, Δ h is the square error of elevation carrection, and h is P _kthe height value that point is corresponding; Based on parametric equation, by calculating the parameter value that in above formula, two height boundary track points are corresponding l interval parameter equivalently represented:

Step 6: the relative movement orbit obtained with inertial navigation system between each moment is constraint, carries out P in initial ranging interval _kthe fast search of position optimum solution, according to search error size determination Search Results, reduce the region of search, search is until meet accuracy requirement repeatedly, exports positioning result.

2. a kind of localization method being interrupted pseudorange constraint based on inertia/double star according to claims 1, described step 2 is specially:

Satellite S ₁(x ₁y ₁z ₁), S ₂(x ₂y ₂z ₂) around earth high-speed cruising, satellite receiver obtains the pseudorange ρ of carrier to satellite ₁, ρ ₂with the positional information of satellite; Carrier positions feasible solution track be respectively with two satellites be the centre of sphere, with the joining of respective pseudorange two Spatial Spheres that are radius, equation expression formula:

$\left\{\begin{array}{c}\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}={\mathrm{\&rho;}}_1\\ \sqrt{{(x-x_2)}^2+{(y-y_2)}^2+{(z-z_2)}^2}={\mathrm{\&rho;}}_2\end{array}\right.$

Any time, determine a feasible solution track by the position of two satellites and pseudorange, this orbital curve is a space circle, and make two satellite distances be d, track radius of circle is r, central coordinate of circle C (x _cy _cz _c), disk normal vector is $\stackrel{\&RightArrow;}{n}={\left(\begin{array}{ccc}{x}_n& y_n& z_n\end{array}\right)}^T,$ Wherein z _n>=0, then:

Wherein: p is intermediate variable, the leg-of-mutton semi-perimeter that two satellites and carrier are formed is represented;

Track center of circle C is positioned at S ₁s ₂on line, make C to S ₂distance is Δ, represents the center of circle with co-ordinates of satellite:

$\left\{\begin{array}{c}\mathrm{\&Delta;}=\sqrt{{\mathrm{\&rho;}}_2^2-r^2}\\ \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\frac{\mathrm{\&Delta;}}{d}(\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]-\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right])+\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]\end{array}\right..$

3. a kind of localization method being interrupted pseudorange constraint based on inertia/double star according to claims 1, described step 3 is specially:

According to following formula, obtain the rotation angle value of twice rotational transform that each moment by orbit coordinate is with being tied to the earth's core admittedly:

$\left\{\begin{array}{c}\mathrm{\&theta;}=\frac{\mathrm{\&pi;}}{2}-si{n}^{-1}\left(\frac{n_z}{\left|\right|\stackrel{\&RightArrow;}{n}\left|\right|}\right)\\ \mathrm{\&psi;}=ta{n}^{-1}\left(\frac{n_y}{n_x}\right)\end{array}\right.$

Wherein: ψ and θ be respectively by normal vector determine around the anglec of rotation of z ' axle and the anglec of rotation around y ' axle.

4. a kind of localization method being interrupted pseudorange constraint based on inertia/double star according to claims 1, described step 4 is specially:

If T _k, with for the carrier positions feasible solution track do not determined according to two visible satellite information in the same time, with the feasible solution track center of circle for initial point, with normal vector for z ' axle forms the parametric orbit equations separated in Descartes's right-handed coordinate system:

Wherein: for equation parameter; Position can be separated Standard parametric equation through the change of twice rotational coordinates and be transformed into parametric equation under ECEF coordinate system:

$\left\{\begin{array}{c}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=C_1{C}_2\left[\begin{array}{c}{x}^{\&prime;}\\ y^{\&prime;}\\ z^{\&prime;}\end{array}\right]+\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]\\ C_1=\left[\begin{array}{ccc}cos\left(\mathrm{\&psi;}\right)& -\sin \left(\mathrm{\&psi;}\right)& 0\\ sin\left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&psi;}\right)& 0\\ 0& 0& 1\end{array}\right]\\ C_2=\left[\begin{array}{ccc}\cos \left(\mathrm{\&theta;}\right)& 0& sin\left(\mathrm{\&theta;}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\&theta;}\right)& 0& cos\left(\mathrm{\&theta;}\right)\end{array}\right]\end{array}\right.$

Wherein: C ₁with C ₂transformation matrix of coordinates corresponding to twice rotational transform.

5. a kind of localization method being interrupted pseudorange constraint based on inertia/double star according to claims 1, described step 6 is specially:

The radian step-length of searching in L interval according to search precision λ, the scouting interval distance namely on feasible solution track, obtains

With the λ that numerical value is larger ₀for initial coarse search precision, obtain corresponding step-size in search by above formula to each step-length in L interval corresponding spaced points P _k, calculate its site error Δ:

Wherein for track on point, if alternate position spike is vectorial with P _kfor starting point, then with its terminal and track distance as site error component Δ _i;

The T that the minimum Min of Select Error (Δ) is corresponding _kupper some P ' _kas the result of first search, its corresponding equation parameter is the region of search is reduced into:

Adjustment search precision is λ ₁, at interval L ₁search obtains result again, so circulates, until precision meets, the result of getting final search is final outgoing position.