CN105953797A - Combined navigation device based on single-axis gyroscope, inclinometer and milemeter and method - Google Patents

Abstract

The invention discloses a combined navigation device based on a single-axis gyroscope, an inclinometer and a milemeter and a method. The device comprises the single-axis gyroscope, the inclinometer, the milemeter, a space projection transformation module, a dead reckoning module and a Kalman filtering module; the single-axis gyroscope and the inclinometer are installed on a same carrier plane, the single-axis gyroscope provides the angular rate of self rotation, the inclinometer provides a pitch angle, and through a space projection transformation algorithm, the rotating angular rate of the gyroscope on an oblique plane is projected into the angular rate on the horizontal plane; meanwhile, the advancing distance provided by the milemeter is projected into the plane distance; the current coordinates and the azimuthal angle are reckoned according to the position coordinates and the azimuthal angle of an initial point through a dead reckoning algorithm. The combined navigation device and the method have the advantages that the system structure and the algorithms are simple, navigating and positioning during no GNSS signal are achieved, and meanwhile the equipment cost is reduced on the condition that the certain precision is achieved.

Description

Utilize combined navigation device and the method for single axis gyroscope, inclinator and speedometer

Technical field

Patent of the present invention relates to integrated navigation system and method, especially, relates to a kind of comprehensive utilization single axis gyroscope, inclines Angle instrument and the combined navigation device of speedometer and method.

Background technology

The carrier location such as conventional vehicle generally use GNSS (Global Navigation Satellite System, GLONASS) satellite navigation location, the most this mode can obtain good result.But in cave, In the environment such as tunnel, Goaf Area, it is impossible to when receiving GNSS satellite signal, it will usually use inertial navigation navigation mode.Conventional Inertial navigation system need the equipment such as three-axis gyroscope, accelerometer realize location, but mechanical gyroscope precision is relatively low, three axle light Fiber gyroscope is expensive, and substantially price is about three times of single axis fiber gyro instrument.

The onboard combined navigation system of Chinese patent CN201210096869.1, single axis gyroscope and single-axis accelerometer and Alignment system disclosed in method includes GNSS module, single axis gyroscope sensor, individual axis acceleration flowmeter sensor, DR module, patrols Collecting control module and Kalman filter, when GNSS signal is effective, GNSS module positions with DR module cooperative；Work as GNSS signal Time invalid, DR module is individually navigated, and algorithm is extremely complex.Chinese patent CN20061011810209, utilize GPS and gyroscope, The integrated positioning device of speedometer, when gps signal is good, utilizes gps signal to position, when gps signal is lost, and switching To MEMS gyroscope, speedometer location.Chinese patent CN201520358904.1, based on accelerometer and the crane of gyroscope Running stroke and deflection monitoring device is using 3-axis acceleration sensor to gather linear acceleration signal, with MEMS tri-axle gyro Instrument gathers crane and runs the angular velocity signal of deflection；After again collection numerical value being processed, it is converted into corresponding change in displacement Amount and magnitude of angular velocity.

Therefore, in order to reduce production cost under reaching certain required precision as far as possible, market demand is a kind of cannot In the case of receiving GNSS satellite signal, cost is lower and can meet the navigator of location requirement of low-speed motion carrier.

Patent of invention content

In order to realize the navigator fix without vehicle carrier during GNSS signal etc., the present invention proposes a kind of comprehensive utilization single shaft The combined navigation device of gyroscope, inclinator and speedometer and method, the method algorithm is simple, and equipment cost is relatively low, energy Enough meet the location requirement of low-speed motion carrier.

The technical scheme provided according to the present invention, a kind of integrated navigation based on single axis gyroscope, inclinator and speedometer Device, including horizontal plane angular speed calculation module 1, dead-reckoning module 2, speedometer 3, starting point coordinate and azimuth module 4, Kalman filter 5；

Described horizontal plane angular speed computing module 1 includes single axis gyroscope 11, inclinator 12, space projection conversion module 13；Space projection conversion module 13 obtain the anglec of rotation in the unit interval that single axis gyroscope 11 records on arbitrary plane and The angle of pitch that inclinator 12 records, it is thus achieved that the angle rotated in the unit interval on horizontal plane；

Described space projection conversion module 13, described starting point coordinate and azimuth module 4, described speedometer 3 all with boat Position calculates that module 2 connects；Dead-reckoning module 2 utilizes the unit interval inner horizontal that horizontal plane angular speed computing module 1 obtains The starting point coordinate that the mileage information of the angle of interior rotation, speedometer 3 offer and starting point coordinate and azimuth module 4 provide And azimuth, extrapolate the position coordinates of current point；

Described dead-reckoning module 2 is connected with described Kalman filter 5.

Preferred: described single axis gyroscope 11 is single axis fiber gyro instrument.

Preferred: the position coordinates calculated is smoothed by Kalman filter 5.

Preferred: single axis gyroscope 11, inclinator 12 are arranged in the plane of a certain carrier, and the angle of pitch of inclinator turns Moving axis is perpendicular to carrier longitudinal axis.

Disclosed herein as well is the Combinated navigation method utilizing combinations of the above guider, including step:

Space projection conversion module 13 obtains the rotation in the unit interval that single axis gyroscope 11 records on arbitrary plane The angle of pitch that angle and inclinator 12 record, it is thus achieved that the angle rotated in the unit interval on horizontal plane；

Dead-reckoning module 2 rotates in utilizing the unit interval inner horizontal that horizontal plane angular speed computing module 1 obtains Mileage information that angle, speedometer 3 provide and the starting point coordinate that starting point coordinate and azimuth module 4 provide and orientation Angle, extrapolates the position coordinates of current point.

The application comprehensively utilize the combined navigation device of single axis gyroscope, inclinator and speedometer include single axis gyroscope, The conversion of inclinator, speedometer, space projection, dead reckoning, Kalman filter etc..Single axis gyroscope, inclinator are arranged on car On the certain level face of carrier, do rigid motion with carrier.And the rotary shaft of the inclinator angle of pitch is perpendicular to bearer plane The longitudinal axis.Speedometer provides the mileage information of vehicle, i.e. travels how many distances relative to initial position.

Vehicle carrier makees low-speed motion, either linear motion or curvilinear motion, sufficiently small time interval expert Route line all can be considered as straight line.The angle that bearer plane turns in this time interval measured by single axis gyroscope, and inclinator carries The angle of pitch for a certain moment of carrier.

The Combinated navigation method step of this programme comprehensive utilization single axis gyroscope, inclinator and speedometer is as follows:

The first step: obtain the carrier angle of pitch α of previous moment record₁With mileage information S₁。

Second step: obtain the carrier angle of pitch α of current time record₂With mileage information S₂。

3rd step: by single axis gyroscope, angle ω of carrier Plane Rotation in acquisition time interval.Pass through space projection Conversion, angle ω rotated on any inclined-plane by carrier projects in the horizontal plane, obtains angle, θ.

$cos\mathrm{\θ}=\frac{-{\mathrm{sin\α}}_1{\mathrm{sin\α}}_2+cos\mathrm{\ω}}{{\mathrm{cos\α}}_1{\mathrm{cos\α}}_2}$

4th step: starting point plane coordinates is (x₀,y₀,h₀), initial azimuth is A₀.Current point coordinates (x can be obtained₁, y₁,h₁) and azimuth A₁For:

A₁=A₀+θ

x₁=x₀+S*Cosα₂Cos A₁

y₁=y₀+S*Cosα₂Sin A₁

h₁=h₀+S*Sinα₂

5th step: repeat the above steps, constantly obtains position coordinates and the azimuth of carrier.

Compared with prior art, the invention has the beneficial effects as follows:

Make full use of the fibre optic gyroscope that precision is high, it is hereby achieved that relatively accurate equipment level attitudes vibration value, Meanwhile, utilize inclinator and speedometer auxiliary, in the case of there is no GNSS satellite signal, accurately obtain current position coordinates. Whole algorithm is relatively easy, and corresponding equipment manufacturing cost is relatively low.

Accompanying drawing explanation

The accompanying drawing of the part constituting the application is used for providing being further appreciated by patent of the present invention, patent of the present invention Schematic description and description is used for explaining patent of the present invention, is not intended that the improper restriction to patent of the present invention.At accompanying drawing In:

Fig. 1 is the overall structure schematic diagram of the present patent application；

Fig. 2 is the inclinator angle of pitch direction schematic diagram of the present patent application；

Fig. 3 is the schematic diagram used in formulation process of the present invention；

Fig. 4 is the right view of Fig. 3；

Fig. 5 is that circle cuts into polygonal schematic diagram；

Fig. 6 is the track schematic diagram of motion carrier；

Fig. 7 is the direction change schematic diagram of motion carrier；

Wherein, 1, horizontal plane angular speed calculation module, 11, single axis gyroscope, 12, inclinator, 13, space projection conversion mould Block,

2, dead-reckoning module, 3, speedometer, 4, starting point coordinate and azimuth module, 5, Kalman filter；

6, motion carrier, 7, wheel.

Detailed description of the invention

Below in conjunction with accompanying drawing, the embodiment of patent of the present invention is described in detail, but patent of the present invention can be according to power Profit requires that the multitude of different ways limiting and covering is implemented.

Embodiment 1:

A kind of combined navigation device comprehensively utilizing single axis gyroscope, inclinator and speedometer, including horizontal plane angular velocity Computing module 1, dead-reckoning module 2, speedometer 3, starting point coordinate and azimuth module 4, Kalman filter 5.Horizontal plane Angular speed computing module 1 includes single axis gyroscope 11, inclinator 12, space projection conversion module 13；Space projection conversion module 13 obtain the anglec of rotation in unit interval of recording on arbitrary plane of single axis gyroscopes 11 and pitching that inclinator 12 records Angle, it is thus achieved that the angle rotated in the unit interval on horizontal plane.

Space projection conversion module 13, described starting point coordinate and azimuth module 4, described speedometer 3 all push away with boat position Calculate module 2 to connect；Dead-reckoning module 2 utilizes the unit interval inner horizontal inward turning that horizontal plane angular speed computing module 1 obtains Mileage information that the angle that turns, speedometer 3 provide and the starting point coordinate that starting point coordinate and azimuth module 4 provide and side Parallactic angle, extrapolates the position coordinates of current point.Dead-reckoning module 2 is connected with described Kalman filter 5.

Single axis gyroscope, inclinator are arranged on the certain level face of vehicle carrier, do rigid motion with motion carrier 6. And the rotary shaft of inclinator 12 angle of pitch is perpendicular to the longitudinal axis of motion carrier 6 place plane, see Fig. 2, the placement of inclinator 12 The direction advanced with the wheel 7 of motion carrier 6 in direction is consistent, and its angle of pitch direction is shown in figure upward arrow, with X-axis for rotating Axle, rotates to paper rear from paper.Speedometer provides the mileage information of vehicle, i.e. relative to initial position travel how much away from From.

The plane coordinates obtaining current time in initial point position is (x₀,y₀,h₀), carrier longitudinal axis initial azimuth is A₀。 Arranging the sampling interval is 50Hz, ensures time synchronized when obtaining angular speed, the angle of pitch, mileage value.

Vehicle carrier makees low-speed motion, either linear motion or curvilinear motion, sufficiently small time interval expert Route line all can be considered as straight line.

Single axis gyroscope can be single axis fiber gyro instrument.Single axis fiber gyro instrument provides the angular speed of carrier in real timeTime Between be spaced in dt, the angle that carrier rotates on any inclined-planeFor this time interval, the load that previous moment obtains The body angle of pitch is α₁, the angle of pitch of current time is α₂, the chamfer distance of traveling is S, its figure formed is carried out geometry and divides Analysis, is converted by space projection, the angle that the angular transformation that can be rotated on inclined-plane by carrier to plane rotates

Current point coordinates (x₁,y₁,h₁) and azimuth A₁Can be calculated by following formula:

A₁=A₀+θ

x₁=x₀+S*Cosα₂Cos A₁

y₁=y₀+S*Cosα₂Sin A₁

h₁=h₀+S*Sinα₂

The position coordinates of subsequent time can be obtained by repeat the above steps.

The derivation of above formula can be found in Fig. 3 and Fig. 4, and as a example by a universal process, previous moment motion exists Position is at O point, and the direction of traveling is OA, and subsequent time carrier drives to B point along OB, due to when time interval is the least The form track waiting carrier can be considered as straight line, the most assumed above feasible.

OCD place plane is horizontal plane, and OAB place plane is inclined-plane, and OA length is set to S1, OB length and is set to S2, ∠ AOC (being set to α 1) is carrier vertical dip angle at O point, inclinator measure, and ∠ BOD (being set to α 2) is vertical when B point of carrier Inclination angle, inclinator is measured.Wherein, C point is the focus that A point makees vertical line gained to horizontal plane, and D point is in like manner.E point is to put down at ABCD In face, make the intersection point of vertical line gained to AC from B point.BE⊥AC.∠ AOB (being set to ω) is that carrier moves to the rotation of B point from O point Angle, is measured by gyroscope, and ∠ COD (being set to θ) is ω projection in the horizontal plane, is also that computer azimuth angle needs to add Angle.

By geometrical relationship in Fig. 3,

AC=S1*sin α 1 BD=S2*sin α 2 OC=S1*cos α 1 OD=S2*cos α 1

By the cosine law

${\mathrm{AB}}^2=S_1^2+S_2^2-2*S_1{S}_{2}Cos\mathrm{\ω}$

Can be obtained by Fig. 3 relation

AE=AC-BD

Can be obtained by Pythagorean theorem

CD²=AB²-AE²

Above-mentioned relation formula is substituted into, the most available

$cos\mathrm{\θ}=\frac{-{\mathrm{sin\α}}_1{\mathrm{sin\α}}_2+cos\mathrm{\ω}}{{\mathrm{cos\α}}_1{\mathrm{cos\α}}_2}$

More than it is the derivation of equation of space projection conversion.

In actual applications, if using the data that physical device gathers, due to precision problem, it is impossible to verification algorithm is just Really property, therefore uses analog data.

Such as: carrier runs a circumference in the horizontal plane.Conventional circumference is a smooth arc, at actual motion In, polygon can be cut into, such as Fig. 5 by as much as possible for circle.

When polygon edge number is abundant, it is believed that the track that motion carrier runs is circular.

Such as the track of Fig. 6, x-axis point to the north to, y-axis point to east to, initial position is initial point, and initial orientation angle is 0 °, point to the north to.Assume circular to be cut into the polygon that limit number is 20000, then polygon interior angle and be 180* (20000- 2)=3599640 °, the supplementary angle of each interior angle is 0.018 °, as can see from Figure 7, i.e. and each polygon in carrier movement The angle that the length of side rotates is 0.018 °.Assume that each length of side is 1.

In starting point O, coordinate is that east northeast is high (0,0,0), and azimuth is 0 °, moves at A through first moment t, by In in the plane, it is not necessary to space projection converts, 0.018 ° is the θ that a problem is derived.By following formula,

A₁=A₀+θ

x₁=x₀+S*Cosα₂Cos A₁

y₁=y₀+S*Cosα₂Sin A₁

h₁=h₀+S*Sinα₂

At A point, coordinate is:

A1=0+0.018=0.018 °

X1=0+1*cos0*cos0.018=0.999999

Y1=0+1*cos0*sin0.018=0.00031416

H1=0+1*sin0=0

Continue to B point:

A2=0.018+0.018=0.036 °

X2=0.999999+1*cos0*cos0.036=1.9999999

Y2=0.00094248

H2=0+1*sin0=0

Calculate always, finally can return to O point, it was demonstrated that algorithm is correct.

The foregoing is only the preferred embodiment of patent of the present invention, be not limited to patent of the present invention, for this For the technical staff in field, patent of the present invention can have various modifications and variations.All spirit in patent of the present invention and principle Within, any modification, equivalent substitution and improvement etc. made, within should be included in the protection domain of patent of the present invention.

Claims (8)

1. a combined navigation device based on single axis gyroscope, inclinator and speedometer, it is characterised in that: include horizontal plane angle Speed calculation module (1), dead-reckoning module (2), speedometer (3), starting point coordinate and azimuth module (4), Kalman's filter Ripple device (5)；

Described horizontal plane angular speed computing module (1) includes single axis gyroscope (11), inclinator (12), space projection conversion module (13)；Space projection conversion module (13) obtains the rotation in the unit interval that single axis gyroscope (11) records on arbitrary plane The angle of pitch that angle and inclinator (12) record, it is thus achieved that the angle rotated in the unit interval on horizontal plane；

Described space projection conversion module (13), described starting point coordinate and azimuth module (4), described speedometer (3) all with Dead-reckoning module (2) connects；Dead-reckoning module (2) utilizes the unit interval that horizontal plane angular speed computing module (1) obtains Mileage information that the angle that rotates in inner horizontal, speedometer (3) provide and starting point coordinate and azimuth module (4) provide Starting point coordinate and azimuth, extrapolate the position coordinates of current point；

Described dead-reckoning module (2) is connected with described Kalman filter (5).

A kind of combined navigation device based on single axis gyroscope, inclinator and speedometer the most according to claim 1, it is special Levy and be: described single axis gyroscope (11) is single axis fiber gyro instrument.

A kind of combined navigation device based on single axis gyroscope, inclinator and speedometer the most according to claim 1, it is special Levy and be: the position coordinates calculated is smoothed by Kalman filter (5).

A kind of combined navigation device based on single axis gyroscope, inclinator and speedometer the most according to claim 1, it is special Levy and be: single axis gyroscope (11), inclinator (12) are arranged in the plane of a certain carrier, and the angle of pitch rotary shaft of inclinator It is perpendicular to carrier longitudinal axis.

A kind of combined navigation device based on single axis gyroscope, inclinator and speedometer the most according to claim 1, it is special Levy and be:

During carrier movement, single axis gyroscope (11) between unit in rotate angle be ω；Inclinator (12) is measured Before and after unit interval, the angle of pitch of carrier is respectively α₁, α₂；

In calculating the unit interval according to space projection conversion (13), carrier motion projection is to the angle of rotation on horizontal plane

A kind of combined navigation device based on single axis gyroscope, inclinator and speedometer the most according to claim 1, it is special Levy and be: starting point plane coordinates is (x₀,y₀,h₀), initial azimuth is A₀；The unit interval measured by speedometer (3) contains Body carries out distance for S；Current point coordinates (x can be obtained by dead-reckoning module (2)₁,y₁,h₁) and azimuth A₁For:

。

7. utilize the Combinated navigation method of combined navigation device described in any one of claim 1 to 6, it is characterised in that include Step:

Space projection conversion module (13) obtains the rotation in the unit interval that single axis gyroscope (11) records on arbitrary plane The angle of pitch that angle and inclinator (12) record, it is thus achieved that the angle rotated in the unit interval on horizontal plane；

Dead-reckoning module (2) rotates in utilizing the unit interval inner horizontal that horizontal plane angular speed computing module (1) obtains Starting point coordinate that mileage information that angle, speedometer (3) provide and starting point coordinate and azimuth module (4) provide and side Parallactic angle, extrapolates the position coordinates of current point.

Combinated navigation method the most according to claim 7, it is characterised in that step particularly as follows:

The first step: obtain the carrier angle of pitch α of previous moment record₁With mileage information S₁；

Second step: obtain the carrier angle of pitch α of current time record₂With mileage information S₂；

3rd step: by single axis gyroscope, angle ω of carrier Plane Rotation in acquisition time interval；Become by space projection Changing, angle ω rotated on any inclined-plane by carrier projects in the horizontal plane, obtains angle, θ；

4th step: starting point plane coordinates is (x₀,y₀,h₀), initial azimuth is A₀；Current point coordinates (x can be obtained₁,y₁,h₁) And azimuth A₁For:

5th step: repeat the above steps, constantly obtains position coordinates and the azimuth of carrier.