CN104864874A - Low-cost single-gyroscope dead reckoning navigation method and system - Google Patents

Abstract

The invention aims to solve the problems of relatively high cost, relatively large size and weight, course error accumulation and the like of the conventional navigation system, and provides a low-cost single-gyroscope dead reckoning navigation method and system. The system adopts a course induction sensor such as a magnetic sensor or a polarized light sensor to provide course reference, adopts an optical gyroscope, of which the sensitive axis points to the sky, to be combined with the course induction sensor to provide a high-precision dynamic course, adopts two accelerometers to measure the posture of a carrier, adopts a mileage recorder to obtain the mileage increment of the carrier, and performs dead reckoning according to the course, the posture and the mileage information, so as to calculate the position of the carrier; the system is small in size, low in power consumption, low in cost and the like.

Description

A kind of low-cost single gyro dead reckoning navigation method and system

Technical field

The present invention relates to a kind of single gyro airmanship, be specifically related to a kind of low-cost single gyro dead reckoning navigation method and system.

Background technology

In various kinds of equipment in military and civilian field, inertial navigation system is widely used as a kind of modernization navigator.Inertial navigation system is mainly divided into gimbaled inertial navigation system and the large class of strap-down inertial navigation system two.Strapdown inertial navigation system is the same with gimbaled inertial navigation system, accurately can provide the navigational parameter such as attitude, ground velocity, longitude and latitude of carrier.

Strapdown inertial navigation system has following particular advantages: eliminate complicated platform mechanical system, and system architecture is very simple, reduces the volume and weight of system, reduces cost simultaneously, simplify maintenance, improve reliability.Current navigational system is made up of 3 gyros, 3 accelerometers usually, although have the dirigibility of apolegamy, causes the increase of cost, volume, weight etc. owing to using 3 gyros.

Therefore, be necessary to work out a kind of low-cost single gyro dead reckoning navigation method and system, thus solve the above-mentioned defect of prior art.

Summary of the invention

The cost existed for strap-down navigation system is higher, course error accumulates, volume, the problems such as weight is larger, the invention provides a kind of low-cost single gyro dead reckoning navigation method and system, this system utilizes course induction pick-up (as Magnetic Sensor, also can be polarized light sensor) course reference is provided, adopt 1 optical gyroscope (sensitive axes refer to sky to) and heading sensor to combine and Dynamic High-accuracy course is provided, two accelerometers are responsible for measuring attitude of carrier, an odometer provides carrier mileage increment, utilize course, attitude, mileage information carries out dead reckoning, thus calculating carrier positions, there is small size, low-power consumption, the plurality of advantages such as low cost.

Request of the present invention provides a kind of low-cost single gyro dead reckoning navigation method, and the method comprises the following steps:

step S101, utilizes Gauss meter and optical gyroscope to calculate the course angle ψ of carrier system x-axis;

Suppose the starting stage, Gauss meter is less by magnetic field environment disturbance, then by the stationary state of 1 minute, export do smoothly, to obtain angle, initial heading ψ to the course angle of Gauss meter ₀;

Adopt optical gyroscope to assist Gauss meter to carry out course angle calculating, utilize optical gyroscope to export and carry out course angle renewal:

ψ _i＝ψ _i-1+Ω _i-1-Ω _iesin(L)cos(θ) (1)

Wherein, (i-1)-i represents a sampling period (Ts), ψ _ifor the course angle in i moment, Ω _i-1for the gyro in (i-1)-i moment exports (unit is radian), Ω _iebe the earth rotation angle in the sampling period, L is local latitude, and θ is the carrier angle of pitch;

There is drift in gyro to measure value, course angle error can be caused to accumulate in time, utilize the accumulated error of Gauss meter to gyro to compensate, make ψ ^g=ψ _irepresent the course angle that i moment gyro resolves, ψ ^mrepresent the course angle that Gauss meter resolves;

When meeting following condition, utilize ψ ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift:

Condition 1:t _gyro> T, wherein t _gyrofor gyro calculates the time, T is certain constant parameter;

Condition 2: formula (2) is set up.

$\left(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^M}\right)+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

Wherein represent a smoothness period T _smoothinterior ψ ^gaverage, represent ψ in a smoothness period ^maverage; A smoothness period is divided into k subcycle, i.e. T _smooth=k*Ts; δ ψ _irepresent the ψ in i-th subcycle ^gwith ψ ^mdifference, namely j is course angle fluctuation threshold value;

On the left of inequality, Section 1 reflection Gauss meter and gyro resolve the constant value deviation of course angle, the degree of fluctuation that Section 2 reflection Gauss meter is affected by magnetic fields; When inequality is set up, show the fluctuation without exception of Gauss meter surrounding magnetic field, can ψ be used ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift;

step S102, utilizes accelerometer A1 to calculate carrier pitching angle theta;

Accelerometer A1 calculates the formula following (see formula 3) of carrier pitching angle theta,

$\mathrm{\&theta;}=\arcsin (-\frac{a1-a_b}{g})---\left(3\right)$

Wherein a1 represents the measurement output quantity of accelerometer A1, and g is acceleration of gravity, and when definition carrier headstock raises up, θ angle is just;

A _bcomputation process as formula 5, shown in 6:

a _b＝x ₂(5)

x ₂(k+1)＝x ₂(k)-h*r*sat(g(k)，δ) (6)

Wherein: $sat\left(x,\mathrm{\&delta;}\right)=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{g},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\left(k\right)\right)\frac{r\left(h-\sqrt{\frac{8\left|z_1\left(k\right)\right|}{2}+h^2}\right)}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&le;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

z ₁(k)＝e(k)+hx ₂(k) (9)

δ＝hr，δ ₁＝hδ (10)

e(k)＝x ₁(k)-v(k) (11)

x ₁(k+1)＝x ₁(k)+hx ₂(k) (12)

X ₂k () is acceleration output, h is sampling step length, and r is input regulating parameter, and according to actual signal behavior people for choosing, be used for regulating differentiator performance, δ is intermediate variable, is smoothness period; V (k) is speed, x ₁k () represents predetermined speed, the discrete form that v (k) is v;

By formula 6-12 simultaneous, and then pass through a _b=x ₂, can a be obtained _b;

Finally obtain carrier pitching angle theta by formula 3;

step S103, utilizes odometer to calculate the mileage increment Delta s in a sampling period;

The mileage increment in a sampling period is:

Δs＝KN (13)

Wherein, umber of pulse N is the umber of pulse in the sampling period of odometer; K is distance factor, needs to demarcate in advance, and N is odometer umber of pulse;

step S104, calculates carrier position in the process of moving (coordinate);

Suppose that the initial coordinate of carrier is for (X ₀, Y ₀), carrier in the process of moving, the coordinate (X in k moment _k, Y _k) obtain by formula (14):

$\left\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}\right.---\left(14\right)$

Wherein ψ _i, θ _i, Δ s _ibe respectively the course angle in i-th sampling period, the angle of pitch, mileage increment.

The application also asks to protect a kind of low-cost single gyro dead reckoning navigation system, comprising:

Attitude Measuring Unit, comprises optical gyroscope, Gauss meter, two accelerometers, before wherein Attitude Measuring Unit coordinate system is chosen-left-on respectively as x-y-z; Optical gyroscope sensitive axes overlap with z-axis point to sky to; Two accelerometer sensitive axles point to x and y-axis direction respectively; Gauss meter zero-bit points to x-axis direction; Attitude Measuring Unit by each optical gyroscope, Gauss meter, the Signal transmissions of two accelerometers is to signal acquisition circuit;

Signal acquisition circuit, for being changed through A/D by the signal of reception, sends to processor by serial communication interface;

Odometer, for the rotation of carrier tire is converted to pulse, gives processor by signal acquisition circuit;

Processor, the signal that Received signal strength Acquisition Circuit sends, obtains the position of carrier, speed, attitude, and calculates the position of carrier.

Further, processor calculates the position of carrier is obtained by above-mentioned air navigation aid.

Accompanying drawing explanation

Fig. 1 is the schematic flow sheet of low-cost single gyro dead reckoning navigation method of the present invention;

Fig. 2 is the structural representation of navigational system of the present invention;

Embodiment

Below in conjunction with drawings and Examples, the invention will be further described.

As shown in Figure 1, the present invention proposes a kind of low-cost single gyro dead reckoning navigation method, in the method with local geographic coordinate system (east-north-sky, E-N-U) as navigational coordinate system (n system), comprise the steps:

step S101, utilizes Gauss meter and optical gyroscope to calculate the course angle ψ of carrier system (b system) x-axis;

Suppose the starting stage, Gauss meter is less by magnetic field environment disturbance, then by the stationary state of 1 minute, export do smoothly, to obtain angle, initial heading ψ to the course angle of Gauss meter ₀.

The output of Gauss meter is subject to the impact of magnetic fluctuation, and therefore the present invention adopts optical gyroscope to assist Gauss meter to carry out course angle calculating, utilizes optical gyroscope to export and carries out course angle renewal (formula (1)),

ψ _i＝ψ _i-1+Ω _i-1-Ω _iesin(L)cos(θ) (1)

Wherein, (i-1)-i represents a sampling period (Ts), ψ _ifor the course angle in i moment, Ω _i-1for the gyro in (i-1)-i moment exports (unit is radian), Ω _iebe the earth rotation angle in the sampling period, L is local latitude, and θ is the carrier angle of pitch.

There is drift in gyro to measure value, course angle error can be caused to accumulate in time, and the present invention utilizes the accumulated error of Gauss meter to gyro to compensate.Make ψ ^g=ψ _irepresent the course angle that i moment gyro resolves, ψ ^mrepresent the course angle that Gauss meter resolves.

When meeting following condition, utilize ψ ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift:

Condition 1:t _gyro> T, wherein t _gyrofor gyro calculates the time, T is certain constant parameter;

Condition 2: formula (2) is set up.

$\left(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{T_{smooth}}^M}\right)+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

Wherein represent a smoothness period T _smoothinterior ψ ^gaverage, represent ψ in a smoothness period ^maverage; A smoothness period is divided into k subcycle, i.e. T _smooth=k*Ts; δ ψ _irepresent the ψ in i-th subcycle ^gwith ψ ^mdifference, namely j is course angle fluctuation threshold value.

On the left of inequality, Section 1 reflection Gauss meter and gyro resolve the constant value deviation of course angle, the degree of fluctuation that Section 2 reflection Gauss meter is affected by magnetic fields.When inequality is set up, show the fluctuation without exception of Gauss meter surrounding magnetic field, can ψ be used ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift.

step S102, utilizes accelerometer A1 to calculate carrier pitching angle theta;

Accelerometer A1 calculates the formula following (see formula 3) of carrier pitching angle theta,

$\mathrm{\&theta;}=\arcsin (-\frac{a1-a_b}{g})---\left(3\right)$

Wherein a1 represents the measurement output quantity of accelerometer A1, and g is acceleration of gravity, and when definition carrier headstock raises up, θ angle is just.Carrier has forward acceleration a _btime, A1 acceleration exports as a1=gsin (θ)+a _b, therefore need deduction acceleration on the impact of the angle of pitch.

The present invention utilize odometer signal through second-order differential obtain x-axis to acceleration.Odometer output signal is the mileage increment Delta s in a sampling period, then speed is:

v＝Δs/Ts (4)

To obtain carrier forward acceleration a _b, need to carry out differential to speed v, and again utilize above formula direct differentiation can amplify noise further.V (k) is the discrete form of v.

A _bcomputation process as formula 5, shown in 6:

a _b＝x ₂(5)

x ₂(k+1)＝x ₂(k)-h*r*sat(g(k)，δ) (6)

Wherein: $sat\left(x,\mathrm{\&delta;}\right)=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{g},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\left(k\right)\right)\frac{r\left(h-\sqrt{\frac{8\left|z_1\left(k\right)\right|}{2}+h^2}\right)}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&le;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

z ₁(k)＝e(k)+hx ₂(k) (9)

δ＝hr，δ ₁＝hδ (10)

e(k)＝x ₁(k)-v(k) (11)

x ₁(k+1)＝x ₁(k)+hx ₂(k) (12)

X ₂k () is acceleration output, h is sampling step length, and r is input regulating parameter, and according to actual signal behavior people for choosing, be used for regulating differentiator performance, δ is intermediate variable, may be interpreted as smoothness period; V (k) is speed, x ₁k () represents predetermined speed, the discrete form that v (k) is v.

By formula 6-12 simultaneous, and then pass through a _b=x ₂, can a be obtained _b.

Finally obtain carrier pitching angle theta by formula 3.

step S103, utilizes odometer to calculate the mileage increment Delta s in a sampling period;

The mileage increment in a sampling period is:

Δs＝KN (13)

Wherein, umber of pulse N is the umber of pulse in the sampling period of odometer; K is distance factor, needs to demarcate in advance, and N is odometer umber of pulse.

step S104, calculates carrier position in the process of moving (coordinate);

Suppose that the initial coordinate of carrier is for (X ₀, Y ₀), carrier in the process of moving, the coordinate (X in k moment _k, Y _k) obtain by formula (14):

$\left\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}\right.---\left(14\right)$

Wherein ψ _i, θ _i, Δ s _ibe respectively the course angle in i-th sampling period, the angle of pitch, mileage increment.

Below by Fig. 2, navigational system of the present invention is further introduced.

As Fig. 2, low-cost single gyro dead reckoning navigation system 100 comprises:

Attitude Measuring Unit 101, comprises optical gyroscope, Gauss meter, two accelerometers, before wherein Attitude Measuring Unit coordinate system is chosen-left-on respectively as x-y-z; Optical gyroscope sensitive axes overlap with z-axis point to sky to; Two accelerometer sensitive axles point to x and y-axis direction respectively; Gauss meter zero-bit points to x-axis direction; Attitude Measuring Unit by each optical gyroscope, Gauss meter, the Signal transmissions of two accelerometers is to signal acquisition circuit 102;

Signal acquisition circuit 102, for being changed through A/D by the signal of reception, sends to processor by serial communication interface;

Odometer 103, for the rotation of carrier tire is converted to pulse, gives processor by signal acquisition circuit;

Processor 104, the signal that Received signal strength Acquisition Circuit sends, obtains the position of carrier, speed, attitude, and calculates the position of carrier.

The position that processor calculates carrier obtains by weighing aforesaid air navigation aid.

Should be understood that, application of the present invention is not limited to above-mentioned citing, for those of ordinary skills, can be improved according to the above description or convert, and all these improve and convert the protection domain that all should belong to claims of the present invention.

Claims (3)

1. a low-cost single gyro dead reckoning navigation method, it is characterized in that, the method comprises the steps:

step S101, utilizes Gauss meter and optical gyroscope to calculate the course angle ψ of carrier system x-axis;

Suppose the starting stage, Gauss meter is less by magnetic field environment disturbance, then by the stationary state of 1 minute, export do smoothly, to obtain angle, initial heading ψ to the course angle of Gauss meter ₀;

Adopt optical gyroscope to assist Gauss meter to carry out course angle calculating, utilize optical gyroscope to export and carry out course angle renewal:

ψ _i＝ψ _i-1+Ω _i-1-Ω _iθsin(L)cos(θ) (1)

Wherein, (i-1)-i represents a sampling period (Ts), ψ _ifor the course angle in i moment, Ω _i-1for the gyro in (i-1)-i moment exports (unit is radian), Ω _iebe the earth rotation angle in the sampling period, L is local latitude, and θ is the carrier angle of pitch;

There is drift in gyro to measure value, course angle error can be caused to accumulate in time, utilize the accumulated error of Gauss meter to gyro to compensate, make ψ ^g=ψ _irepresent the course angle that i moment gyro resolves, ψ ^mrepresent the course angle that Gauss meter resolves;

When meeting following condition, utilize ψ ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift:

Condition 1:t _gyro> T, wherein t _gyrofor gyro calculates the time, T is certain constant parameter;

Condition 2: formula (2) is set up.

$(\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{Tsmooth}^G}-\stackrel{\&OverBar;}{{\mathrm{\&psi;}}_{Tsmooth}^M})+\frac{{\&Sigma;}_{i=1}^k\frac{{\mathrm{\&delta;\&psi;}}_i-{\mathrm{\&delta;\&psi;}}_{i-1}}{Ts}}{k}<J---\left(2\right)$

Wherein represent a smoothness period T _smoothinterior ψ ^gaverage, represent ψ in a smoothness period ^maverage; A smoothness period is divided into k subcycle, i.e. T _smooth=k*Ts; δ ψ _irepresent the ψ in i-th subcycle ^gwith ψ ^mdifference, namely j is course angle fluctuation threshold value;

On the left of inequality, Section 1 reflection Gauss meter and gyro resolve the constant value deviation of course angle, the degree of fluctuation that Section 2 reflection Gauss meter is affected by magnetic fields; When inequality is set up, show the fluctuation without exception of Gauss meter surrounding magnetic field, can ψ be used ^mreplace ψ _ito eliminate the course angle cumulative errors of the factors such as gyroscopic drift;

step S102, utilizes accelerometer A1 to calculate carrier pitching angle theta;

Accelerometer A1 calculates the formula following (see formula 3) of carrier pitching angle theta,

$\mathrm{\&theta;}=arcsim(-\frac{a1-a_b}{g})---\left(3\right)$

Wherein a1 represents the measurement output quantity of accelerometer A1, and g is acceleration of gravity, and when definition carrier headstock raises up, θ angle is just;

A _bcomputation process as formula 5, shown in 6:

a _b＝x ₂(5)

x ₂(k+1)＝x ₂(k)-h*r*sat(g(k)，δ) (6)

Wherein: $sat(x,\mathrm{\&delta;})=\left\{\begin{array}{cc}sign\left(x\right),& \left|x\right|>\mathrm{\&delta;}\\ \frac{x}{\mathrm{\&delta;}},& \left|x\right|\&le;\mathrm{\&delta;}\end{array}\right.---\left(7\right)$

$g\left(k\right)=\left\{\begin{array}{cc}{x}_2\left(k\right)-sign\left(z_1\right(k\left)\right)\frac{r(h-\sqrt{\frac{s\left|z_1\left(k\right)\right|}{z}+h^z})}{2},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ x_2\left(k\right)+\frac{z_1\left(k\right)}{h},& \left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\end{array}\right.---\left(8\right)$

z ₁(k)＝e(k)+hx ₂(k) (9)

δ＝hr，δ ₁＝hδ (10)

e(k)＝x ₁(k)-v(k) (11)

x ₁(k+1)＝x ₁(k)+hx ₂(k) (12)

X ₂k () is acceleration output, h is sampling step length, and r is input regulating parameter, and according to actual signal behavior people for choosing, be used for regulating differentiator performance, δ is intermediate variable, is smoothness period; V (k) is speed, x ₁k () represents predetermined speed, the discrete form that v (k) is v;

By formula 6-12 simultaneous, and then pass through a _b=x ₂, can a be obtained _b;

Finally obtain carrier pitching angle theta by formula 3;

step S103, utilizes odometer to calculate the mileage increment Delta s in a sampling period;

The mileage increment in a sampling period is:

Δs＝KN (13)

Wherein, umber of pulse N is the umber of pulse in the sampling period of odometer; K is distance factor, needs to demarcate in advance, and N is odometer umber of pulse;

step S104, calculates carrier position in the process of moving (coordinate);

Suppose that the initial coordinate of carrier is for (X ₀, Y ₀), carrier in the process of moving, the coordinate (X in k moment _k, Y _k) obtain by formula (14):

$\{\begin{array}{c}{X}_k=X_0+{\&Sigma;}_{i=1}^k\cos \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\\ Y_k=Y_0+{\&Sigma;}_{i=1}^k\sin \left({\mathrm{\&psi;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right){\mathrm{\&Delta;s}}_i\end{array}---\left(14\right)$

Wherein ψ _i, θ _i, Δ s _ibe respectively the course angle in i-th sampling period, the angle of pitch, mileage increment.

2. a low-cost single gyro dead reckoning navigation system, is characterized in that comprising:

Attitude Measuring Unit, comprises optical gyroscope, Gauss meter, two accelerometers, before wherein Attitude Measuring Unit coordinate system is chosen-left-on respectively as x-y-z; Optical gyroscope sensitive axes overlap with z-axis point to sky to; Two accelerometer sensitive axles point to x and y-axis direction respectively; Gauss meter zero-bit points to x-axis direction; Attitude Measuring Unit by each optical gyroscope, Gauss meter, the Signal transmissions of two accelerometers is to signal acquisition circuit;

Signal acquisition circuit, for being changed through A/D by the signal of reception, sends to processor by serial communication interface;

Odometer, for the rotation of carrier tire is converted to pulse, gives processor by signal acquisition circuit;

Processor, the signal that Received signal strength Acquisition Circuit sends, obtains the position of carrier, speed, attitude, and calculates the position of carrier.

3. system as claimed in claim 2, its feature exists, and the position that processor calculates carrier is obtained by air navigation aid according to claim 1.