CN106990426B - Navigation method and navigation device - Google Patents
Navigation method and navigation device Download PDFInfo
- Publication number
- CN106990426B CN106990426B CN201710158060.XA CN201710158060A CN106990426B CN 106990426 B CN106990426 B CN 106990426B CN 201710158060 A CN201710158060 A CN 201710158060A CN 106990426 B CN106990426 B CN 106990426B
- Authority
- CN
- China
- Prior art keywords
- error
- speed
- value
- gyro
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004364 calculation method Methods 0.000 claims abstract description 54
- 230000001133 acceleration Effects 0.000 claims abstract description 52
- 238000005259 measurement Methods 0.000 claims abstract description 49
- 238000001914 filtration Methods 0.000 claims abstract description 47
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 13
- 238000004590 computer program Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000014509 gene expression Effects 0.000 description 3
- 239000013307 optical fiber Substances 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The application discloses a navigation method and a navigation device, wherein the method comprises the following steps: a satellite receiver acquires position and speed; the micro-inertia measurement unit collects acceleration and gyro instantaneous values according to a measurement period; calculating the position, speed and attitude parameter values of each measurement period by using a strapdown inertial navigation algorithm; according to the filtering period, the satellite receiver outputs a position and speed calibration value; and (3) calculating 12 parameters of a pitch angle error, a roll angle error, a course angle error, an east speed error, a north speed error, a precision error, a latitude error, an x-direction gyro error, a y-direction gyro error and an x-direction acceleration error and a y-direction acceleration error by using a discrete Kalman filtering method. The device comprises a micro-inertia measurement unit, a satellite receiver and a processor; the processor further comprises a navigation parameter initialization unit, a strapdown inertial navigation calculation unit and an error estimation filter. The invention has low cost, small volume and high precision.
Description
Technical Field
The application relates to the field of unmanned aerial vehicles, in particular to a method and a device for navigation by means of satellite navigation and local inertia measurement.
Background
Each single navigation system has its own unique capabilities and limitations. The small optical fiber or micro-mechanical inertial navigation device has the advantages of low cost, small volume and light weight, and can autonomously provide attitude, position and speed information for a carrier; the defect is that the navigation error increases rapidly along with time, and particularly, indexes such as the drift, zero offset repeatability and the like of the MEMS inertial device are poor, so that the aim of improving the inertial navigation precision is difficult to achieve through pre-calibration.
The Beidou satellite navigation system is a satellite navigation system developed by China, and can provide navigation, positioning and time service for users in China under the condition that foreign navigation systems such as GPS, GLONASS and the like are closed. At present, a 5GEO +5IGSO +5MEO constellation frame is formed in the Beidou system, positioning, navigation and time service in Asia-Pacific areas can be achieved, the positioning accuracy is 10m, the speed measurement accuracy is 0.2m/s, and the time service accuracy is 50 ns. Satellite navigation has the advantages of all weather and high navigation accuracy, but is limited by the use environment, has poor usability under interference and shielding conditions, and cannot provide carrier attitude information for carrier flight control.
Therefore, a combined navigation system implementation method and system need to be designed, and a system which can provide complete carrier motion parameters and has higher navigation accuracy is formed by combining a Miniature Inertial Measurement Unit (MIMU) and a Beidou satellite receiver.
Disclosure of Invention
The invention discloses a navigation method and a navigation device, which solve the problem of navigation by using a Micro Inertial Measurement Unit (MIMU) under the uncalibrated condition.
The embodiment of the application provides a navigation method for an unmanned aerial vehicle, wherein the unmanned aerial vehicle comprises a micro-inertia measurement unit and a satellite receiver, and the navigation method comprises the following steps:
at an initial moment, the satellite receiver is used for collecting the position and the speed of the unmanned aerial vehicle, and the position and the speed are respectively used as a position initial value and a speed initial value;
acquiring an acceleration instantaneous value and a gyro instantaneous value of the unmanned aerial vehicle according to a measurement period by using the micro-inertia measurement unit from the initial time to the calibration time; calculating an attitude parameter instantaneous value according to the acceleration instantaneous value and the gyro instantaneous value; a filtering period is arranged between the calibration time and the initial time, and comprises a plurality of measurement periods;
obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm according to the position initial value, the speed initial value and the attitude parameter instantaneous value;
at the calibration moment, the satellite receiver is used for collecting the position and the speed of the unmanned aerial vehicle, and the position and the speed are respectively used as a position calibration value and a speed calibration value;
calculating attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method according to the position calculation value, the speed calculation value, the position calibration value and the speed calibration value at the calibration moment; the error variables used by the discrete kalman filtering method include: pitch angle error, roll angle error, course angle error, east speed error, north speed error, precision error, latitude error, gyro error in x, y and z directions and acceleration error in x and y directions are 12 parameters; wherein, x, y, z constitute rectangular coordinate system, and the X direction points to the unmanned aerial vehicle right side, and the Y direction points forward along unmanned aerial vehicle axis of ordinates direction.
Further, the steps are executed circularly by taking the calibration time as a new initial time.
The preferred embodiment of the present application further comprises the steps of: correcting the acceleration instantaneous value of the next filtering period by using the acceleration error; and correcting the gyro instantaneous value of the next filtering period by using the gyro error.
The embodiment of the application also provides a navigation device for the unmanned aerial vehicle, which comprises a micro-inertia measurement unit, a satellite receiver and a processor; the micro-inertia measurement unit is used for generating a gyro instantaneous value and an acceleration instantaneous value according to a measurement period from an initial moment; the satellite receiver is used for receiving satellite positioning information and outputting a position initial value and a speed initial value at an initial time; outputting a position calibration value and a speed calibration value according to a filtering period from an initial moment; the processor is used for obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm according to the position initial value, the speed initial value, the gyro instantaneous value and the acceleration instantaneous value; calculating attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method according to the position calculation value, the speed calculation value, the position calibration value and the speed calibration value at the calibration moment; the error variables used by the discrete kalman filtering method include: pitch angle error, roll angle error, course angle error, east speed error, north speed error, precision error, latitude error, gyro error in x, y and z directions and acceleration error in x and y directions are 12 parameters; wherein, x, y, z constitute rectangular coordinate system, and the X direction points to the unmanned aerial vehicle right side, and the Y direction points forward along unmanned aerial vehicle axis of ordinates direction.
Further, the processor further comprises a navigation parameter initialization unit, a strapdown inertial navigation calculation unit and an error estimation filter; the navigation parameter initialization unit is used for receiving the acceleration instantaneous value and the gyro instantaneous value and calculating the attitude parameter instantaneous value according to the acceleration instantaneous value and the gyro instantaneous value; the strapdown inertial navigation unit is used for receiving the position initial value, the speed initial value and the attitude parameter instantaneous value, and obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm; the error estimation filter calculates attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method.
Preferably, the navigation device further comprises an output correction unit; and the output correction unit corrects the attitude parameter calculation value by using the attitude parameter error.
In any one of the embodiments of the present invention, preferably, the navigation device further includes an input correction unit that corrects an acceleration instantaneous value of a next filtering period with the acceleration error, and corrects a gyro instantaneous value of a next filtering period with the gyro error, and the navigation parameter initializes an input of the unit.
The embodiment of the application adopts at least one technical scheme which can achieve the following beneficial effects:
compared with other integrated navigation system implementation methods, the integrated navigation system has a complete data processing process, and integrated navigation based on a Micro Inertial Measurement Unit (MIMU) and a Beidou satellite receiver can be well realized by adopting the integrated navigation system.
The errors of the micro inertial devices do not need to be calibrated in advance, on one hand, the inertial navigation calculation parameters are initialized by periodically utilizing the position, the speed and the speed course output by the Beidou satellite receiver, on the other hand, the inertial navigation errors are estimated in real time by the error estimation filter and corrected, and finally, the combined navigation result with available precision can be obtained.
Compared with the traditional laser or optical fiber inertia unit, the small optical fiber or micro-mechanical inertia measurement unit has lower cost, smaller volume and lighter weight; the invention adopts the Beidou satellite receiver to ensure that the integrated navigation system can still normally work under the condition that other satellite navigation systems (GPS, GLONASS and the like) are closed.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the application and together with the description serve to explain the application and not to limit the application. In the drawings:
FIG. 1 is a flowchart of an embodiment of a navigation method of the present invention;
FIG. 2 is a flowchart of an embodiment of a navigation method including loop calibration according to the present invention;
FIG. 3 is a flow diagram of an embodiment of error estimation using a discrete Kalman filtering method;
FIG. 4 is a block diagram of an embodiment of a navigation device of the present invention;
FIG. 5 is a block diagram of a further optimized embodiment of the navigation device of the present invention;
FIG. 6 is a schematic diagram of a navigation device for calibrating input and output according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the technical solutions of the present application will be described in detail and completely with reference to the following specific embodiments of the present application and the accompanying drawings. It should be apparent that the described embodiments are only some of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
In all embodiments of the present application, x, y, z constitute a rectangular coordinate system, the x direction points to the right side of the drone, and the y direction points forward along the longitudinal axis of the drone.
The technical solutions provided by the embodiments of the present application are described in detail below with reference to the accompanying drawings.
FIG. 1 is a flowchart illustrating a navigation method according to an embodiment of the present invention. The method specifically comprises the following steps:
at a first initial moment, the calculation method of the attitude parameter instantaneous value comprises the following steps:
pitch0＝arcsin(ay/g0) Equation 1.1
roll0＝-arcsin(ax/g0) Equation 1.2
In the formula0、roll0Initial pitch angle and roll angle; a isx、ayOutputting values for the x and y directions of the accelerometer; g0Is the acceleration of gravity.
For example, gyro and accelerometer information of a Micro Inertial Measurement Unit (MIMU) is acquired at a measurement period T1(20 ms).
It should be noted that the inertial navigation outputs the carrier attitude for carrier attitude control, and the real-time performance is required to be high, so the inertial navigation is designed to be short-period T1(20ms) calculation; the error of the output result (attitude, position and speed) of the inertial navigation increases along with the increase of the calculation time, the information assistance of the Beidou satellite receiver is periodically utilized to improve the precision, and the embodiment of the method adopts a filtering period T2 (which is 1 s).
In step 12, for example, the longitude, latitude, altitude, ground speed heading, the initial pitch angle and the initial roll angle of the Beidou satellite receiver can be used to calculate the initial values of the attitude, the speed and the position of the unmanned aerial vehicle;
the position parameters comprise longitude, latitude and altitude, the speed parameters comprise east speed, north speed and sky speed, and the attitude parameters comprise course, pitch angle and roll angle;
for example, information (longitude, latitude, altitude, east speed, north speed, sky speed, ground speed heading) of the Beidou satellite receiver is collected with a filtering period T2(1 s).
for example, when the number of satellites participating in positioning by the satellite receiver is large and the geometric accuracy factor is smaller than a threshold value, combining the space guiding speed and the altitude and directly outputting the space guiding speed and the altitude of the satellite receiver; otherwise, the combined navigation outputs the space navigation speed and height of a Micro Inertial Measurement Unit (MIMU).
In the embodiments of the present application, the satellite receiver receives a navigation signal, and is specifically applied to a Beidou satellite, a GPS satellite, or other navigation satellites. When the Beidou satellite is applied, the satellite receiver is a Beidou satellite receiver.
FIG. 2 is a flowchart of an embodiment of a navigation method including loop calibration according to the present invention.
The preferred embodiment of the present application further comprises the steps of:
Further, when the lock is lost and recapture is carried out, reinitialization is needed, and the steps 11-17 are repeated.
And when the satellite receiver continuously tracks the satellite positioning signal, further taking the calibration time as a new initial time, and circularly executing the steps 13-17.
Through the process, errors of the micro inertial device do not need to be calibrated in advance, inertial navigation calculation parameters are initialized periodically by using the position, the speed and the speed course output by the Beidou satellite receiver, and the inertial navigation errors are estimated and corrected in real time by designing an error estimation filter, so that a navigation result with available precision can be finally obtained.
FIG. 3 is a flow chart of an embodiment of error estimation using a discrete Kalman filtering method. The method comprises the following steps of adopting an error estimation filter to carry out inertial navigation error estimation, considering the characteristics and the operation time of a Micro Inertial Measurement Unit (MIMU), selecting an error state variable of 12 parameters by the filter, wherein specifically, the elements of the error state variable X comprise the following physical quantities: error of pitch angle delta phieRoll angle error delta phinCourse angle error delta phiu(ii) a East velocity error Δ veNorth velocity error Δ vn(ii) a Longitude error delta lambda and latitude error delta L; gyro error epsilon in x, y and z directionsbx，εby，εbz(ii) a x, y direction accelerometer error
Namely, it is
And step 21, establishing the 12-parameter error state model according to the relation between the inertial navigation system error and the basic navigation parameters. Specifically, the error state model is
Where F is the state transition matrix, the dimension is 12 × 12, and the non-zero entries are:
F(0,1)＝ωiesinL；F(0,2)＝-ωiecosL；
F(1,0)＝-ωiesinL；
F(1,5)＝-ωiesinL；
F(2,0)＝ωiecosL；
F(3,1)＝-g；
F(4,0)＝g；
in the formula, ωieIs the rotational angular velocity of the earth, L is the latitude, R is the radius of the earth,
is an attitude matrix (dimension 3X 3), ve、vnEast-direction velocity and north-direction velocity, g is gravity acceleration, taugyroFor gyro-dependent time, τaccIn the technical scheme of the invention, the relevant time of the gyroscope and the accelerometer takes 5 s.
The inertial navigation system has various error sources, such as position error, speed error, initial attitude error, gyro deviation, accelerometer deviation, scale coefficient error and the like, and each error can be divided into fixed deviation and noise terms. Approximate corresponding relation exists between the inertial navigation system error and the basic navigation parameter, and the change of the inertial navigation system error in a fixed sampling interval can be expressed in a form of multiplying a coefficient by an error term and adding a noise term. The error term coefficient corresponding to each error variation in the expression is determined and can be obtained from the inertial navigation error transfer relationship. And according to different selected error state variables, the error state model is written into different expressions. Specifically, an error state model in the form of equation 2 may be established.
In the error state model, G is a noise matrix with dimension of 12 × 1, which satisfies
Wherein, Wgyro＝[wgxwgywgz]T，Wacc＝[waxwaywaz]T，wgx、wgy、wgzMeasuring noise, w, for a gyroscopeax、way、wazNoise is measured for the accelerometer.
The error estimation filter is calculated according to a Kalman filtering method, the calculation period is the filtering period T2 (for example, 1s), and each filtering period is calculated to obtain a state one-step estimation value:
in the formula (I), the compound is shown in the specification,
respectively estimating a pitch angle error, a roll angle error and a course angle error;
respectively an east direction speed error estimation value and a north direction speed error estimation value;
respectively longitude and latitude error estimated values;
gyro error estimated values in x, y and z directions respectively;
the error estimation values of the accelerometer in the x direction and the y direction are respectively.
In the formula (I), the compound is shown in the specification,
for the current period of the attitude matrix,
the corrected attitude matrix is used for the next period S5 calculation.
And the attitude deviation of the current periodic attitude matrix is corrected to obtain a corrected attitude matrix for subsequent strapdown inertial navigation calculation. And correcting the attitude according to the sequence of the course angle error, the pitch angle error and the roll angle error. And obtaining a corrected attitude matrix according to the angle rotation sequence. The specific relational expression of the corrected attitude matrix, the current period attitude matrix and the coefficient matrix is shown in the circled part.
And step 23, correcting the east speed, the north speed, the latitude and the longitude output by the micro inertial measurement unit by using the state one-step estimation value according to the speed and position correction model.
Establishing an error estimation filter observation model as follows:
the error observation model established in formula 6 uses the velocity-position difference as the observed quantity, and the velocity and position output by the Micro Inertial Measurement Unit (MIMU) and the position-velocity difference output by the Beidou satellite receiver can be expressed as the form of inertial navigation error plus measurement noise.
Wherein v ise_INS、vn_INS、LINS、λINSEast-direction velocity, north-direction velocity, latitude, longitude v for obtaining velocity calculation value and position calculation value according to output of micro-inertia measurement unite_BD、vn_BD、LBD、λBDThe compass is the east speed, the north speed, the latitude and the longitude output by the Beidou satellite receiver; w is ave、wvnMeasuring noise for the speed; w is aλ、wLNoise is measured for the position. w is ave、wvnNoise is measured for the speed of the Beidou receiver, and is determined by the speed precision parameter of the receiver, wλ、wLThe noise of the position measurement of the Beidou receiver is determined by the position precision parameter of the receiver.
Only four inertial navigation error parameters can be directly obtained from equation 6, but the change of the four error parameters is related to the rest inertial navigation error parameters from the state model F matrix relation of equation 2. All error parameters can be solved through Kalman filtering. The method comprises the following steps: attitude parameters (heading, pitch, roll), position parameters (longitude λ)INSLatitude LINSAltitude), speed parameters (east speed v)e_INSVelocity v in the north directionn_INSThe speed in the sky).
Using state one-step estimates, using velocity, position correction models
In the formula ve、vnAnd L and lambda are corrected east-direction speed, north-direction speed, latitude and longitude navigation results.
FIG. 4 is a block diagram of an embodiment of a navigation device of the present invention. The embodiment of the application also provides a navigation device for an unmanned aerial vehicle, which comprises a micro-inertia measurement unit 100, a satellite receiver 200 and a processor 300; the micro-inertia measurement unit is used for generating a gyro instantaneous value and an acceleration instantaneous value according to a measurement period from an initial moment; the satellite receiver is used for receiving satellite positioning information and outputting a position initial value and a speed initial value at an initial time; outputting a position calibration value and a speed calibration value according to a filtering period from an initial moment; the processor is used for obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm according to the position initial value, the speed initial value, the gyro instantaneous value and the acceleration instantaneous value; calculating attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method according to the position calculation value, the speed calculation value, the position calibration value and the speed calibration value at the calibration moment; the error variables used by the discrete kalman filtering method include: pitch angle error, roll angle error, course angle error, east speed error, north speed error, precision error, latitude error, gyro error in x, y and z directions and acceleration error in x and y directions are 12 parameters; wherein, x, y, z constitute rectangular coordinate system, and the X direction points to the unmanned aerial vehicle right side, and the Y direction points forward along unmanned aerial vehicle axis of ordinates direction.
FIG. 5 is a block diagram of a navigation device according to a further optimization of the present invention. Further, the processor 300 further includes a navigation parameter initialization unit 301, a strapdown inertial navigation computation unit 302, and an error estimation filter 303; the navigation parameter initialization unit is used for receiving the acceleration instantaneous value and the gyro instantaneous value and calculating the attitude parameter instantaneous value (as a formula 1.1-1.2) according to the acceleration instantaneous value and the gyro instantaneous value; the strapdown inertial navigation computing unit is used for receiving the position initial value, the speed initial value and the attitude parameter instantaneous value, and obtaining a position calculated value, a speed calculated value and an attitude parameter calculated value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm; the error estimation filter calculates 12 variables (such as formulas 2-5) of attitude parameter error, speed error, position error, gyro error and acceleration error by using a discrete Kalman filtering method.
FIG. 6 is a schematic diagram of a navigation device for calibrating input and output according to an embodiment of the present invention. The navigation device further includes an output correction unit 400; and the output correction unit corrects the attitude parameter calculation value by using the attitude parameter error to obtain the corrected position, speed and attitude parameter. In any one of the embodiments of the present invention, preferably, the navigation device further includes an input correction unit 500, which corrects the acceleration instantaneous value of the next filtering period by the acceleration error, corrects the gyro instantaneous value of the next filtering period by the gyro error, and initializes the input of the unit.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The above description is only an example of the present application and is not intended to limit the present application. Various modifications and changes may occur to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.
Claims (9)
1. A navigation method is used for a unmanned aerial vehicle, the unmanned aerial vehicle comprises a micro inertial measurement unit and a satellite receiver, and the navigation method is characterized by comprising the following steps:
at an initial moment, the satellite receiver is used for collecting the position and the speed of the unmanned aerial vehicle, and the position and the speed are respectively used as a position initial value and a speed initial value;
the error of the micro-inertia device does not need to be calibrated in advance, and the micro-inertia measurement unit is used for collecting the acceleration instantaneous value and the gyro instantaneous value of the unmanned aerial vehicle according to the measurement period from the initial moment to the calibration moment; calculating an attitude parameter instantaneous value according to the acceleration instantaneous value and the gyro instantaneous value; the time length between the calibration time and the initial time is a filtering period and comprises a plurality of measurement periods;
at a first initial time, the attitude parameter instantaneous values are:
pitch0＝arcsin(ay/g0)，roll0＝-arcsin(ax/g0)
wherein the pitch0、roll0Initial pitch angle and roll angle; a isx、ayOutputting values for the x and y directions of the accelerometer; g0Is the acceleration of gravity;
obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm according to the position initial value, the speed initial value and the attitude parameter instantaneous value;
at the calibration moment, the satellite receiver is used for collecting the position and the speed of the unmanned aerial vehicle, and the position and the speed are respectively used as a position calibration value and a speed calibration value;
calculating attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method according to the position calculation value, the speed calculation value, the position calibration value and the speed calibration value at the calibration moment; the error variables used by the discrete kalman filtering method include: pitch angle error, roll angle error, course angle error, east speed error, north speed error, precision error, latitude error, gyro error in x, y and z directions and acceleration error in x and y directions are 12 parameters; the X direction, the Y direction and the Z direction form a rectangular coordinate system, the X direction points to the right side of the unmanned aerial vehicle, and the Y direction points forwards along the longitudinal axis direction of the unmanned aerial vehicle;
said is separated fromIn the scattered Kalman filtering method, the elements of the error state variable X are composed of the following physical quantities: error of pitch angle delta phieRoll angle error delta phinCourse angle error delta phiu(ii) a East velocity error Δ veNorth velocity error Δ vn(ii) a Longitude error delta lambda and latitude error delta L; gyro error epsilon in x, y and z directionsbx,εby,εbz(ii) a x, y direction accelerometer error
Namely, it is
Establishing the 12-parameter error state model according to the relation between the inertial navigation system error and the basic navigation parameter, wherein the error state model is specifically
Where F is the state transition matrix, the dimension is 12 × 12, and the non-zero entries are:
F(0,1)＝ωiesinL；F(0,2)＝-ωiecosL；
F(1,0)＝-ωiesinL；
F(1,5)＝-ωiesinL；
F(2,0)＝ωiecosL；
F(3,1)＝-g；
F(4,0)＝g；
wherein, ω isieIs the rotational angular velocity of the earth, L is the latitude, R is the radius of the earth,
is an attitude matrix with dimensions of 3 x 3, ve、vnRespectively east-direction speed and north-direction speed, g is gravity acceleration, taugyroFor gyro-dependent time, τaccTime associated with the accelerometer;
g is a noise matrix with dimension of 12 × 1, satisfying
Wherein, Wgyro＝[wgxwgywgz]T，Wacc＝[waxwaywaz]T，wgx、wgy、wgzMeasuring noise, w, for a gyroscopeax、way、wazNoise is measured for the accelerometer.
2. The navigation method of claim 1,
and taking the calibration time as a new initial time, and circularly executing the steps.
3. The navigation method of claim 2, further comprising the steps of:
correcting the acceleration instantaneous value of the next filtering period by using the acceleration error;
and correcting the gyro instantaneous value of the next filtering period by using the gyro error.
4. The navigation method according to any one of claims 1 to 3, wherein the measurement period is 20 ms.
5. The navigation method according to any one of claims 1 to 3, wherein the filtering period is 1 s.
6. A navigation device comprising a micro inertial measurement unit, a satellite receiver, a processor, using the method of claim 1,
the micro-inertia measurement unit is used for generating a gyro instantaneous value and an acceleration instantaneous value according to a measurement period from an initial moment;
the satellite receiver is used for receiving satellite positioning information and outputting a position initial value and a speed initial value at an initial time; outputting a position calibration value and a speed calibration value according to a filtering period from an initial moment;
the processor is used for obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm according to the position initial value, the speed initial value, the gyro instantaneous value and the acceleration instantaneous value; calculating attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method according to the position calculation value, the speed calculation value, the position calibration value and the speed calibration value at the calibration moment; the error variables used by the discrete kalman filtering method include: pitch angle error, roll angle error, course angle error, east speed error, north speed error, precision error, latitude error, gyro error in x, y and z directions and acceleration error in x and y directions are 12 parameters; wherein, x, y, z constitute rectangular coordinate system, and the X direction points to the unmanned aerial vehicle right side, and the Y direction points forward along unmanned aerial vehicle axis of ordinates direction.
7. The navigation device of claim 6, wherein the processor further comprises a navigation parameter initialization unit, a strapdown inertial navigation computation unit, an error estimation filter;
the navigation parameter initialization unit is used for receiving the acceleration instantaneous value and the gyro instantaneous value and calculating the attitude parameter instantaneous value according to the acceleration instantaneous value and the gyro instantaneous value;
the strapdown inertial navigation unit is used for receiving the position initial value, the speed initial value and the attitude parameter instantaneous value, and obtaining a position calculation value, a speed calculation value and an attitude parameter calculation value of the unmanned aerial vehicle in each measurement period by using a strapdown inertial navigation algorithm;
the error estimation filter calculates attitude parameter errors, speed errors, position errors, gyro errors and acceleration errors by a discrete Kalman filtering method.
8. The navigation device of claim 7, further comprising an output correction unit;
and the output correction unit corrects the attitude parameter calculation value by using the attitude parameter error.
9. The navigation device according to any one of claims 6 to 8, further comprising an input correction unit; the input correction unit corrects the acceleration instantaneous value of the next filtering period by using the acceleration error, corrects the gyro instantaneous value of the next filtering period by using the gyro error, and initializes the input of the unit.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710158060.XA CN106990426B (en) | 2017-03-16 | 2017-03-16 | Navigation method and navigation device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710158060.XA CN106990426B (en) | 2017-03-16 | 2017-03-16 | Navigation method and navigation device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106990426A CN106990426A (en) | 2017-07-28 |
| CN106990426B true CN106990426B (en) | 2020-04-10 |
Family
ID=59412164
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710158060.XA Active CN106990426B (en) | 2017-03-16 | 2017-03-16 | Navigation method and navigation device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106990426B (en) |
Families Citing this family (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| IT201700087876A1 (en) * | 2017-07-31 | 2019-01-31 | St Microelectronics Srl | SYSTEM FOR THE NAVIGATION OF LAND VEHICLES AND CORRESPONDENT PROCEDURE |
| JP7223542B2 (en) * | 2018-10-03 | 2023-02-16 | 古野電気株式会社 | Navigation device, method for generating navigation support information, and program for generating navigation support information |
| FR3089287B1 (en) * | 2018-12-04 | 2020-10-30 | Thales Sa | Hybrid AHRS system comprising a device for measuring the integrity of the calculated attitude |
| CN110941285A (en) * | 2019-11-29 | 2020-03-31 | 云南大学 | Unmanned aerial vehicle flight control system based on two IP cores |
| CN110986937B (en) * | 2019-12-19 | 2022-05-17 | 北京三快在线科技有限公司 | Navigation device and method for unmanned equipment and unmanned equipment |
| CN111198875A (en) * | 2019-12-26 | 2020-05-26 | 深圳市元征科技股份有限公司 | Vehicle positioning data screening method and device, vehicle-mounted equipment and storage medium |
| CN111006675B (en) * | 2019-12-27 | 2022-10-18 | 西安理工大学 | Self-calibration method of vehicle-mounted laser inertial navigation system based on high-precision gravity model |
| CN111398997A (en) * | 2020-04-10 | 2020-07-10 | 江西驰宇光电科技发展有限公司 | Dam safety monitoring device and method based on Beidou and inertial navigation |
| CN111351508B (en) * | 2020-04-22 | 2023-10-03 | 中北大学 | System-level batch calibration method for MEMS inertial measurement units |
| CN111832690B (en) * | 2020-06-15 | 2022-10-11 | 中国人民解放军海军工程大学 | Calculation method of gyro measurement value of inertial navigation system based on particle swarm optimization algorithm |
| CN111982089A (en) * | 2020-07-28 | 2020-11-24 | 陕西宝成航空仪表有限责任公司 | Real-time calibration and compensation method for magnetic compass total error |
| CN112731320B (en) * | 2020-12-29 | 2024-06-21 | 福瑞泰克智能系统有限公司 | Estimation method, device, equipment and storage medium of vehicle-mounted radar error data |
| CN113551692B (en) * | 2021-07-19 | 2024-04-02 | 杭州迅蚁网络科技有限公司 | Calibration method and device for installation angle of magnetometer and camera of unmanned aerial vehicle |
| CN113885064B (en) * | 2021-12-07 | 2022-04-01 | 天津仁爱学院 | Double-system single-frequency Beidou inertial navigation positioning method and device and storage medium |
| CN114485641B (en) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | Attitude calculation method and device based on inertial navigation device navigation azimuth fusion |
| CN116892898B (en) * | 2023-09-11 | 2024-02-02 | 农业农村部南京农业机械化研究所 | Trajectory error detection method, device and system for agricultural machinery |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102506875A (en) * | 2011-11-30 | 2012-06-20 | 中国南方航空工业(集团)有限公司 | Method and device for navigating unmanned aerial vehicle |
| CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
| CN103852086A (en) * | 2014-03-26 | 2014-06-11 | 北京航空航天大学 | Field calibration method of optical fiber strapdown inertial navigation system based on Kalman filtering |
| CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
-
2017
- 2017-03-16 CN CN201710158060.XA patent/CN106990426B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102506875A (en) * | 2011-11-30 | 2012-06-20 | 中国南方航空工业(集团)有限公司 | Method and device for navigating unmanned aerial vehicle |
| CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
| CN103852086A (en) * | 2014-03-26 | 2014-06-11 | 北京航空航天大学 | Field calibration method of optical fiber strapdown inertial navigation system based on Kalman filtering |
| CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106990426A (en) | 2017-07-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106990426B (en) | Navigation method and navigation device | |
| CN108226980B (en) | Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit | |
| KR101988786B1 (en) | Initial alignment of inertial navigation devices | |
| CN106500693B (en) | A kind of AHRS algorithm based on adaptive extended kalman filtering | |
| CN104655152B (en) | A real-time delivery alignment method for airborne distributed POS based on federated filtering | |
| CN105371844B (en) | A kind of inertial navigation system initial method based on inertia/astronomical mutual assistance | |
| CN103941273B (en) | Adaptive filtering method of onboard inertia/satellite integrated navigation system and filter | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN102252677A (en) | Time series analysis-based variable proportion self-adaptive federal filtering method | |
| CN109708663B (en) | Star sensor online calibration method based on aerospace plane SINS assistance | |
| CN107643088A (en) | Navigation of Pilotless Aircraft method, apparatus, unmanned plane and storage medium | |
| CN110207691A (en) | A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging | |
| CN103245359A (en) | Method for calibrating fixed errors of inertial sensor in inertial navigation system in real time | |
| CN104697526A (en) | Strapdown inertial navitation system and control method for agricultural machines | |
| CN104374388A (en) | Flight attitude determining method based on polarized light sensor | |
| CN107764261B (en) | Simulation data generation method and system for distributed POS (point of sale) transfer alignment | |
| CN111121766A (en) | An Astronomical and Inertial Integrated Navigation Method Based on Starlight Vector | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| CN102937450A (en) | Relative attitude determining method based on gyroscope metrical information | |
| CN113790737B (en) | On-site rapid calibration method of array sensor | |
| CN106352897A (en) | Silicon MEMS (micro-electromechanical system) gyroscope error estimating and correcting method based on monocular visual sensor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |