CN116839591A - Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle - Google Patents
Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle Download PDFInfo
- Publication number
- CN116839591A CN116839591A CN202310853754.0A CN202310853754A CN116839591A CN 116839591 A CN116839591 A CN 116839591A CN 202310853754 A CN202310853754 A CN 202310853754A CN 116839591 A CN116839591 A CN 116839591A
- Authority
- CN
- China
- Prior art keywords
- navigation
- vector
- unmanned aerial
- aerial vehicle
- beidou
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000001914 filtration Methods 0.000 title claims abstract description 31
- 230000004927 fusion Effects 0.000 title claims abstract description 21
- 239000013598 vector Substances 0.000 claims abstract description 47
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 230000009466 transformation Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000005516 engineering process Methods 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000002360 preparation method Methods 0.000 abstract description 2
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000033001 locomotion Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Classifications
-
- 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/20—Instruments for performing navigational calculations
-
- 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
-
- 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
A track tracking positioning filtering system and a fusion navigation method of a rescue unmanned aerial vehicle belong to the field of navigation of rescue unmanned aerial vehicles. The track tracking module and the automatic tracking return module of the track tracking positioning filtering system are fixedly arranged on the navigation platform of the rescue unmanned aerial vehicle, the track tracking module is in signal connection with the automatic return module, and the automatic return module is in signal connection with the navigation platform of the rescue unmanned aerial vehicle. The method comprises the following steps: calculating the rotation angle of the gyroscope; calculating the rotation angle of the navigation platform; speed and position descriptions; defining a state vector of an inertial navigation system and a Beidou combined system; constructing a fusion measurement position vector by using the measurement of the Beidou receiver; a filtering method based on unscented Kalman filtering uses unscented transformation techniques to approximate the probability distribution of nonlinear system states. The invention can track and record the path of the unmanned aerial vehicle in search and rescue, provide simple dangerous environment and condition positioning for staff, and provide expected preparation for further search and rescue work.
Description
Technical Field
The invention belongs to the field of navigation of rescue unmanned aerial vehicles, and particularly relates to a track tracking and positioning filtering system and a fusion navigation method of the rescue unmanned aerial vehicle.
Background
Dangerous accidents often occur unexpectedly, and searching and rescuing without familiarity with terrain and unknown terrain risk factors often wastes a significant amount of time. The existing rescue platform has various problems, such as a crawler type rescue platform, is developed on the basis of the traditional wheeled robot to meet the requirements of military reconnaissance, dangerous object dismantling and the like. Its movement is fast, but when facing severe environments, it is inconvenient to enter a narrow area due to its relatively large size, which can cause interruption of the rescue process. In the aspect of space, most of design researches of rescue platforms are based on satellite positioning to perform track simulation, and in a real environment, satellite navigation has certain limitation. For example, in closed environments such as buildings, tunnels, caverns, etc., signal attenuation is very severe, so in most cases accurate positioning information cannot be provided. The inertial navigation system is currently used for autonomous navigation positioning, and the accuracy is poor, so that a fusion navigation method for constructing the rescue unmanned aerial vehicle under the condition of communication degradation by combining the inertial navigation system with the Beidou navigation system is provided.
Disclosure of Invention
The invention aims to solve the problems in the background art and provides a track tracking and positioning filtering system and a rescue unmanned aerial vehicle fusion navigation method.
The technical scheme adopted by the invention is as follows:
the track tracking and positioning filtering system consists of a track tracking module and an automatic tracking and returning module, wherein the track tracking module and the automatic tracking and returning module are fixedly arranged on a navigation platform of the rescue unmanned aerial vehicle, the track tracking module is in signal connection with the automatic returning module, and the automatic returning module is in signal connection with the navigation platform of the rescue unmanned aerial vehicle;
the track tracking module is used for calculating a system state vector of an Inertial Navigation System (INS) and a Beidou combined system by using a complementary algorithm; the measurement of the Beidou receiver is utilized to construct a fusion measurement position vector, and the speed error and the position error of the corresponding Beidou receiver can be measured; the filtering method based on unscented Kalman filtering uses unscented transformation technology to approximate the probability distribution of nonlinear system state, and predicts mean and covariance;
and the automatic tracking return module is used for storing the track data transmitted by the track tracking module, and when the signal is interrupted and an external person cannot control, the navigation platform starts to read the track data so as to realize the automatic return of the navigation platform along the original path track.
A fusion navigation method of a rescue unmanned aerial vehicle, the method comprising the steps of:
step 1: calculating the rotation angle sigma of the gyroscope; according to the coriolis force principle of the gyroscope, outputting the angular velocity omega (t) of the gyroscope to obtain the rotation angle sigma of the gyroscope, as shown in formula (2);
σ＝∫ω(t)dt (2)
step 2: calculating the rotation angle of the navigation platform; collecting the rotation angle of a navigation platform at a certain time interval to obtain a plane acceleration direction, combining a magnetometer to compensate the correction direction in real time, and obtaining an initial value of the attitude angle of the navigation platform by solving a quaternion rotation matrix:
wherein ,ψ0 、θ 0 、γ 0 Initial values of yaw angle, pitch angle and roll angle of the navigation platform are respectively obtained;
by yaw angle ψ of the navigation platform 0 Pitch angle theta 0 And a roll angle gamma 0 The initial values of the attitude angles of the navigation platform are formed;
wherein ,εb ＝(ε x ,ε y ,ε z ) T The continuous drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system is shown;
the component of the white noise corresponding to the continuous drift of the gyroscope in the x, y and z coordinate directions;
psi, theta and gamma are respectively yaw angle, pitch angle and roll angle of the navigation platform, and the angles are combined to form a rotation angle of the navigation platform;
ω gb represents the angular velocity conversion from g-coordinate system to b-coordinate system, ω gb The projection in the b coordinate system is:
wherein ,respectively is +.>Values in x, y, z coordinate directions;
ε x 、ε y 、ε z respectively the constant drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system;
step 3: speed and position descriptions; the velocity and position equations of an Inertial Navigation System (INS) are:
wherein ,is a position expressed in terms of latitude L, longitude λ, and altitude h, respectively;
velocity components in x, y, z-axis direction, respectively, +.>Measurement of specific force in x, y and z axis directions, ω ie G is gravity acceleration, R is the rotation angular velocity of the earth M and RN The radius of curvature in the meridian and plumb line directions respectively,component of constant bias error generated for accelerometer in x, y, z axis direction, +.>For the gesture matrix from the b coordinate system to the g coordinate system, w is Gaussian white noise, constant drift epsilon of a gyroscope and constant bias error of an accelerometer +.>Setting zero;
step 4: defining a system state vector of an Inertial Navigation System (INS) and a Beidou combined system as follows:
according to the states of an Inertial Navigation System (INS) and a Beidou combined system, a combined state vector of the Inertial Navigation System (INS) and the Beidou combined system is obtained:
wherein f (·) is a nonlinear function, G is a distribution matrix of process white noise vectors, u is a set of additional 10% modeling uncertainty; discretizing the above system by using an Euler discretization formula to obtain a discrete vector of the combined state of the two systems;
x k ＝f(x k-1 )+Gu；
wherein ,xk As the state of the x (t) function at time k, x k-1 Is the state at a moment in time on the x (t) function;
step 5: constructing a fusion measurement position vector by using the measurement of the Beidou receiver; high-precision output of the Beidou receiver in terms of speed and position is adopted to obtain a position vector z to be measured k ；
z k ＝[v x ,v y ,v z ,L,λ,h] T ；
Based on the determined system state vector and measurement vector, a fusion measurement vector z of an Inertial Navigation System (INS) and a Beidou combined system is constructed k ；
wherein ,vv and vp Measuring noise, namely respectively corresponding to the speed error and the position error of the Beidou receiver; h k Is a matrix, H k ＝[0 6×3 ,I 6×6 ,0 6×6 ]；x k The state of the x (t) function at the moment k;
since the INS/Beidou is a closed-loop system, the navigation parameters estimated by the filter are used for compensating the drift of an Inertial Measurement Unit (IMU) of the inertial navigation system, so that a more accurate combined navigation solution of the Inertial Navigation System (INS) and the Beidou combined system can be obtained;
step 6: a filtering method based on unscented Kalman filtering, which uses unscented transformation technology to approximate the probability distribution of nonlinear system states; first, a set of very small sigma points is deterministically selected from a priori mean and covariance of the states; then, an unscented transformation technique is applied to the points sequentially to generate a transformed sample, and a predicted mean and covariance are calculated using the weighted mean and covariance of the transformed samples.
Further, the specific steps of the step 6 are as follows:
step 6.1: preparing; assuming that a state estimate is givenAnd error covariance matrix->
Step 6.2: predicting; converting each sigma point through a navigation platform state vector to generate a group of new samples, and updating the predicted mean value and covariance;
ξ i,k/k-1 ＝f(ξ i,k-1 ),i＝0,1,…,2n
wherein n is the number of samples sampled;
wherein ,
ξ i,k-1 an x (t) value at any time estimated from k-1 sample values of x (t);
ξ i,k/k-1 ＝f(ξ i,k-1 )，ξ i,k/k-1 new sample values generated for the first k-1 samples;
is the mean value;
f (·) is some nonlinear function;
ω i is the angular velocity at time i;is covariance; q (Q) k Covariance of white noise; a is a tuning parameter, which is a constant;
step 6.3: updating; because the measurement vector is linear, the same equation as the traditional Kalman filtering is adopted for measurement updating;
step 6.4: transmission and storage; the measurement vector is transmitted as a state vector to an auto-tracking return module.
Compared with the prior art, the invention has the beneficial effects that: inertial Navigation Systems (INS) are an important positioning method in the current navigation field. The method has the advantages of high autonomy, full-parameter navigation, high short-term navigation precision and the like. Based on the above, the invention designs a track tracking and positioning filtering system based on inertial navigation, which is used for acquiring the motion path of the unmanned aerial vehicle. The track tracking positioning filtering system is arranged on the unmanned aerial vehicle navigation platform, tracks and records the path of the unmanned aerial vehicle in search and rescue, provides simple dangerous environment and condition positioning for staff, and makes expected preparation for further search and rescue work. The method combines the advantages of good autonomy of the inertial navigation system, high precision of the satellite navigation system and the like, and has higher short-term precision and stability.
Drawings
FIG. 1 is a schematic diagram of a dead reckoning algorithm.
Detailed Description
The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the invention, but not all embodiments, and all other embodiments obtained by those skilled in the art without making creative efforts based on the embodiments of the present invention are all within the protection scope of the present invention.
The first embodiment is as follows: the embodiment discloses a track tracking and positioning filtering system, which consists of a track tracking module and an automatic tracking and returning module, wherein the track tracking module and the automatic tracking and returning module are fixedly arranged on a navigation platform of a rescue unmanned aerial vehicle, the track tracking module is in signal connection with the automatic returning module, and the automatic returning module is in signal connection with the navigation platform of the rescue unmanned aerial vehicle;
the track tracking module is used for calculating a system state vector (embodied in step 4 in the method) of an Inertial Navigation System (INS) and a Beidou combined system by using a complementary algorithm; the measurement of the Beidou receiver is utilized to construct a fusion measurement position vector, and the speed error and the position error (reflected in step 5 of the method) of the corresponding Beidou receiver can be measured; a filtering method based on unscented Kalman filtering, which uses unscented transformation technology to approximate the probability distribution of nonlinear system state, and predicts mean and covariance (embodied in step 6 of the method);
and the automatic tracking return module is used for storing the track data transmitted by the track tracking module, and when the signal is interrupted and an external person cannot control, the navigation platform starts to read the track data (attitude angle and coordinate value) so as to realize that the navigation platform automatically returns along the original path track.
The second embodiment is as follows: the embodiment discloses a fusion navigation method for realizing a rescue unmanned aerial vehicle based on the track tracking and positioning filtering system of the specific embodiment, wherein the method comprises the following steps:
step 1: calculating the rotation angle sigma of the gyroscope; according to the coriolis force principle of the gyroscope, outputting the angular velocity omega (t) of the gyroscope to obtain the rotation angle sigma of the gyroscope, as shown in formula (2);
σ＝∫ω(t)dt (2)
step 2: calculating the rotation angle of the navigation platform; collecting the rotation angle of a navigation platform at a certain time interval (usually 1 second), obtaining a plane acceleration direction, combining a magnetometer, compensating the correction direction in real time, and obtaining an initial value of the attitude angle of the navigation platform by solving a quaternion rotation matrix:
wherein ,ψ0 、θ 0 、γ 0 Initial values of yaw angle, pitch angle and roll angle of the navigation platform are respectively obtained;
by yaw angle ψ of the navigation platform 0 Pitch angle theta 0 And a roll angle gamma 0 The initial values of the attitude angles of the navigation platform are formed;
wherein ,εb ＝(ε x ,ε y ,ε z ) T The continuous drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system is shown;
the component of the white noise corresponding to the continuous drift of the gyroscope in the x, y and z coordinate directions;
psi, theta and gamma are respectively yaw angle, pitch angle and roll angle of the navigation platform, and the angles are combined to form a rotation angle of the navigation platform;
ω gb represents the angular velocity conversion from the g-coordinate system (geographical coordinate system) to the b-coordinate system (carrier coordinate system), ω gb The projection in the b coordinate system is:
wherein ,respectively is +.>Values in x, y, z coordinate directions;
ε x 、ε y 、ε z respectively the constant drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system;
step 3: speed and position descriptions; the velocity and position equations of an Inertial Navigation System (INS) are:
wherein ,is a position expressed in terms of latitude L, longitude λ, and altitude h, respectively;
velocity components in x, y, z-axis direction, respectively, +.>Measurement of specific force in x, y and z axis directions, ω ie G is gravity acceleration, R is the rotation angular velocity of the earth M and RN The radius of curvature in the meridian and plumb line directions respectively,component of constant bias error generated for accelerometer in x, y, z axis direction, +.>For the gesture matrix from the b coordinate system to the g coordinate system, w is Gaussian white noise, constant drift epsilon of a gyroscope and constant bias error of an accelerometer +.>Setting zero;
step 4: defining a system state vector of an Inertial Navigation System (INS) and a Beidou combined system as follows:
according to the states of an Inertial Navigation System (INS) and a Beidou combined system, a combined state vector of the Inertial Navigation System (INS) and the Beidou combined system is obtained:
wherein f (·) is a nonlinear function, G is a distribution matrix of process white noise vectors, u is a set of additional 10% modeling uncertainty; discretizing the above system by using an Euler discretization formula to obtain a discrete vector of the combined state of the two systems;
x k ＝f(x k-1 )+Gu；
wherein ,xk The state of the x (t) function at time k (i.e. the current position of the positioned object), x k- 1 is the state (also the position) at one time on the x (t) function;
step 5: constructing a fusion measurement position vector by using the measurement of the Beidou receiver; high-precision output of the Beidou receiver in terms of speed and position is adopted to obtain a position vector z to be measured k ；
z k ＝[v x ,v y ,v z ,L,λ,h] T ；
Based on the determined system state vector and measurement vector, a fusion measurement vector z of an Inertial Navigation System (INS) and a Beidou combined system is constructed k ；
wherein ,vv and vp Measuring noise, namely respectively corresponding to the speed error and the position error of the Beidou receiver; h k Is a matrix, H k ＝[0 6×3 ,I 6×6 ,0 6×6 ]；x k The state of the x (t) function at the moment k;
since the INS/Beidou is a closed-loop system, the navigation parameters estimated by the filter are used for compensating the drift of an Inertial Measurement Unit (IMU) of the inertial navigation system, so that a more accurate combined navigation solution of the Inertial Navigation System (INS) and the Beidou combined system can be obtained;
step 6: a filtering method based on unscented Kalman filtering, which uses unscented transformation technology to approximate the probability distribution of nonlinear system states; first, a set of very small sigma points (state points) are deterministically selected from a priori means and covariance of the states (locations); then, an unscented transformation technique is applied to the points sequentially to generate a transformed sample, and a predicted mean and covariance are calculated using the weighted mean and covariance of the transformed samples.
And a third specific embodiment: this embodiment is further described in the second embodiment, and the specific steps in step 6 are as follows:
step 6.1: preparing; assuming that a state estimate is givenAnd error covariance matrix->
Step 6.2: predicting; converting each sigma point through a navigation platform state vector to generate a group of new samples, and updating the predicted mean value and covariance;
ξ i,kk-1 ＝f(ξ i,k-1 ),i＝0,1,…,2n
wherein n is the number of samples sampled;
wherein ,
ξ i,k-1 an x (t) value at any time estimated from k-1 sample values of x (t);
ξ i,k/k-1 ＝f(ξ i,k-1 )，ξ i,k/k-1 new sample values generated for the first k-1 samples;
is the mean value;
f (·) is some nonlinear function;
ω i is the angular velocity at time i;is covariance; q (Q) k Covariance of white noise; a is a tuning parameter, which is a constant;
step 6.3: updating; because the measurement vector is linear, the same equation as the traditional Kalman filtering is adopted for measurement updating;
step 6.4: transmission and storage; the measurement vector is transmitted as a state vector to an auto-tracking return module.
Inertial navigation is essentially a dead reckoning system, as shown in fig. 1, which is a schematic diagram of a dead reckoning algorithm. The rescue unmanned aerial vehicle navigation platform adopts passive inertial navigation, acquires inertial element original data (initial values of yaw angle, pitch angle and roll angle) of the remote control platform in an inertial reference system, and calculates unmanned aerial vehicle speed, attitude angle and yaw angle information in a navigation coordinate system through data fusion and attitude calculation. And establishing mathematical models of each attitude angle and each position according to the working principle of the inertial navigation system.
In fig. 1, x0 and y0 are initial positions of nodes, Δ01 and Δ02 are estimated positions, and S0, S1, and S2 are estimated tracks.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential characteristics thereof. The present embodiments are, therefore, to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned.
Furthermore, it should be understood that although the present disclosure describes embodiments, not every embodiment is provided with a separate embodiment, and that this description is provided for clarity only, and that the disclosure is not limited to the embodiments described in detail below, and that the embodiments described in the examples may be combined as appropriate to form other embodiments that will be apparent to those skilled in the art.
Claims (3)
1. A track tracking and positioning filtering system is characterized in that: the track tracking, positioning and filtering system consists of a track tracking module and an automatic tracking and returning module, wherein the track tracking module and the automatic tracking and returning module are fixedly arranged on the navigation platform of the rescue unmanned aerial vehicle, the track tracking module is in signal connection with the automatic returning module, and the automatic returning module is in signal connection with the navigation platform of the rescue unmanned aerial vehicle;
the track tracking module is used for calculating a system state vector of the inertial navigation system and the Beidou combined system by using a complementary algorithm; the measurement of the Beidou receiver is utilized to construct a fusion measurement position vector, and the speed error and the position error of the corresponding Beidou receiver can be measured; the filtering method based on unscented Kalman filtering uses unscented transformation technology to approximate the probability distribution of nonlinear system state, and predicts mean and covariance;
and the automatic tracking return module is used for storing the track data transmitted by the track tracking module, and when the signal is interrupted and an external person cannot control, the navigation platform starts to read the track data so as to realize the automatic return of the navigation platform along the original path track.
2. A fusion navigation method for realizing a rescue unmanned aerial vehicle based on the track tracking and positioning filtering system of claim 1, the method comprising the following steps:
step 1: calculating the rotation angle sigma of the gyroscope; according to the coriolis force principle of the gyroscope, outputting the angular velocity omega (t) of the gyroscope to obtain the rotation angle sigma of the gyroscope, as shown in formula (2);
σ＝∫ω(t)dt (2)
step 2: calculating the rotation angle of the navigation platform; collecting the rotation angle of a navigation platform at a certain time interval to obtain a plane acceleration direction, combining a magnetometer to compensate the correction direction in real time, and obtaining an initial value of the attitude angle of the navigation platform by solving a quaternion rotation matrix:
wherein ,ψ0 、θ 0 、γ 0 Initial values of yaw angle, pitch angle and roll angle of the navigation platform are respectively obtained;
by yaw angle ψ of the navigation platform 0 Pitch angle theta 0 And a roll angle gamma 0 The initial values of the attitude angles of the navigation platform are formed;
wherein ,εb ＝(ε x ,ε y ,ε z ) T The continuous drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system is shown;
the component of the white noise corresponding to the continuous drift of the gyroscope in the x, y and z coordinate directions;
psi, theta and gamma are respectively yaw angle, pitch angle and roll angle of the navigation platform, and the angles are combined to form a rotation angle of the navigation platform;
ω gb represents the angular velocity conversion from g-coordinate system to b-coordinate system, ω gb The projection in the b coordinate system is:
wherein ,respectively is +.>Values in x, y, z coordinate directions;
ε x 、ε y 、ε z respectively the constant drift of the gyroscope in the x, y and z coordinate directions in the b coordinate system;
step 3: speed and position descriptions; the speed and position equations of the inertial navigation system are:
wherein ,is a position expressed in terms of latitude L, longitude λ, and altitude h, respectively;
velocity components in x, y, z-axis direction, respectively, +.>Measurement of specific force in x, y and z axis directions, ω ie G is gravity acceleration, R is the rotation angular velocity of the earth M and RN Radius of curvature in meridian and principal perpendicular directions, respectively, +.>Component of constant bias error generated for accelerometer in x, y, z axis direction, +.>For the gesture matrix from the b coordinate system to the g coordinate system, w is Gaussian white noise, constant drift epsilon of a gyroscope and constant bias error of an accelerometer +.>Setting zero;
step 4: the system state vector defining the inertial navigation system and the Beidou combination system is as follows:
according to the states of the inertial navigation system and the Beidou combined system, a combined state vector of the inertial navigation system and the Beidou combined system is obtained:
wherein f (·) is a nonlinear function, G is a distribution matrix of process white noise vectors, u is a set of additional 10% modeling uncertainty; discretizing the above system by using an Euler discretization formula to obtain a discrete vector of the combined state of the two systems;
x k ＝f(x k-1 )+Gu；
wherein ,xk As the state of the x (t) function at time k, x k-1 Is the state at a moment in time on the x (t) function;
step 5: constructing a fusion measurement position vector by using the measurement of the Beidou receiver; high-precision output of the Beidou receiver in terms of speed and position is adopted to obtain a position vector z to be measured k ；
z k ＝[v x ,v y ,v z ,L,λ,h] T ；
Based on the determined system state vector and measurement vector, a fusion measurement vector z of the inertial navigation system and the Beidou combined system is constructed k ；
wherein ,vv and vp Measuring noise, namely respectively corresponding to the speed error and the position error of the Beidou receiver; h k Is a matrix, H k ＝[0 6×3 ,I 6×6 ,0 6×6 ]；x k The state of the x (t) function at the moment k;
step 6: a filtering method based on unscented Kalman filtering, which uses unscented transformation technology to approximate the probability distribution of nonlinear system states; first, a set of very small sigma points is deterministically selected from a priori mean and covariance of the states; then, an unscented transformation technique is applied to the points sequentially to generate a transformed sample, and a predicted mean and covariance are calculated using the weighted mean and covariance of the transformed samples.
3. The fusion navigation method of a rescue unmanned aerial vehicle according to claim 2, wherein: the specific steps of the step 6 are as follows:
step 6.1: preparing; assuming that a state estimate is givenAnd error covariance matrix->
Step 6.2: predicting; converting each sigma point through a navigation platform state vector to generate a group of new samples, and updating the predicted mean value and covariance;
ξ i,k/k-1 ＝f(ξ i,k-1 ),i＝0,1,…,2n
wherein n is the number of samples sampled;
wherein ,
ξ i,k-1 an x (t) value at any time estimated from k-1 sample values of x (t);
ξ i,k/k-1 ＝f(ξ i,k-1 )，ξ i,k/k-1 new sample values generated for the first k-1 samples;
is the mean value;
f (·) is some nonlinear function;
ω i is the angular velocity at time i;is covariance; q (Q) k Covariance of white noise; a is a tuning parameter, which is a constant;
step 6.3: updating; because the measurement vector is linear, the same equation as the traditional Kalman filtering is adopted for measurement updating;
step 6.4: transmission and storage; the measurement vector is transmitted as a state vector to an auto-tracking return module.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310853754.0A CN116839591B (en) | 2023-07-12 | 2023-07-12 | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310853754.0A CN116839591B (en) | 2023-07-12 | 2023-07-12 | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116839591A true CN116839591A (en) | 2023-10-03 |
| CN116839591B CN116839591B (en) | 2024-05-28 |
Family
ID=88164975
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310853754.0A Active CN116839591B (en) | 2023-07-12 | 2023-07-12 | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116839591B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117346794A (en) * | 2023-12-05 | 2024-01-05 | 山东省科学院海洋仪器仪表研究所 | Unmanned ship integrated navigation system and navigation method for enteromorpha tracking |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106291645A (en) * | 2016-07-19 | 2017-01-04 | 东南大学 | Be suitable to the volume kalman filter method that higher-dimension GNSS/INS couples deeply |
| CN107610157A (en) * | 2016-07-12 | 2018-01-19 | 深圳雷柏科技股份有限公司 | A kind of unmanned plane target method for tracing and system |
| CN107831776A (en) * | 2017-09-14 | 2018-03-23 | 湖南优象科技有限公司 | Unmanned plane based on nine axle inertial sensors independently makes a return voyage method |
| FR3070773A1 (en) * | 2017-09-05 | 2019-03-08 | Thales | DRONE TRACKING SYSTEM, METHOD AND COMPUTER PROGRAM |
| US20200271749A1 (en) * | 2015-07-17 | 2020-08-27 | Origin Wireless, Inc. | Method, apparatus, and system for wireless tracking with graph-based particle filtering |
| CN111722257A (en) * | 2020-06-29 | 2020-09-29 | 河南工业大学 | Remote rescue robot system based on GPS and INS combined guidance |
| US20210080287A1 (en) * | 2019-09-18 | 2021-03-18 | Harbin Engineering University | Method for initial alignment of radar assisted airborne strapdown inertial navigation system |
| CN112630813A (en) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system |
| WO2021212462A1 (en) * | 2020-04-24 | 2021-10-28 | 深圳市大疆创新科技有限公司 | Movement control method, movement apparatus, and movement platform |
| CN115540860A (en) * | 2022-09-26 | 2022-12-30 | 福建（泉州）哈工大工程技术研究院 | Multi-sensor fusion pose estimation algorithm |
| CN116203600A (en) * | 2023-02-22 | 2023-06-02 | 中急管(北京)网络科技有限公司 | Method for tracking motion trail with power after communication signal of unmanned aerial vehicle is lost |
-
2023
- 2023-07-12 CN CN202310853754.0A patent/CN116839591B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200271749A1 (en) * | 2015-07-17 | 2020-08-27 | Origin Wireless, Inc. | Method, apparatus, and system for wireless tracking with graph-based particle filtering |
| CN107610157A (en) * | 2016-07-12 | 2018-01-19 | 深圳雷柏科技股份有限公司 | A kind of unmanned plane target method for tracing and system |
| CN106291645A (en) * | 2016-07-19 | 2017-01-04 | 东南大学 | Be suitable to the volume kalman filter method that higher-dimension GNSS/INS couples deeply |
| FR3070773A1 (en) * | 2017-09-05 | 2019-03-08 | Thales | DRONE TRACKING SYSTEM, METHOD AND COMPUTER PROGRAM |
| CN107831776A (en) * | 2017-09-14 | 2018-03-23 | 湖南优象科技有限公司 | Unmanned plane based on nine axle inertial sensors independently makes a return voyage method |
| US20210080287A1 (en) * | 2019-09-18 | 2021-03-18 | Harbin Engineering University | Method for initial alignment of radar assisted airborne strapdown inertial navigation system |
| WO2021212462A1 (en) * | 2020-04-24 | 2021-10-28 | 深圳市大疆创新科技有限公司 | Movement control method, movement apparatus, and movement platform |
| CN111722257A (en) * | 2020-06-29 | 2020-09-29 | 河南工业大学 | Remote rescue robot system based on GPS and INS combined guidance |
| CN112630813A (en) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system |
| CN115540860A (en) * | 2022-09-26 | 2022-12-30 | 福建（泉州）哈工大工程技术研究院 | Multi-sensor fusion pose estimation algorithm |
| CN116203600A (en) * | 2023-02-22 | 2023-06-02 | 中急管(北京)网络科技有限公司 | Method for tracking motion trail with power after communication signal of unmanned aerial vehicle is lost |
Non-Patent Citations (5)
| Title |
|---|
| LIU F, SUN X, XIONG Y, ET AL.: "Combination of iterated cubature Kalman filter and neural networks for GPS/INS during GPS outages", REV SCI INSTRUM, 31 December 2019 (2019-12-31) * |
| YINGHONG MA; XUNAN LI; YI JIAO; LIN GUO; SUPING REN; QI ZHANG: "A Fast Multi-UAV Cooperative Reconnaissance Method Exploiting Payload Diversity", 2022 IEEE 96TH VEHICULAR TECHNOLOGY CONFERENCE, 31 December 2022 (2022-12-31) * |
| 李俊芳;李峰;吉月辉;高强;: "四旋翼无人机轨迹稳定跟踪控制", 控制与决策, no. 02, 29 February 2020 (2020-02-29) * |
| 李文超;袁冬莉;: "基于机动目标模型的无人机视场跟踪仿真研究", 计算机测量与控制, no. 02, 28 February 2011 (2011-02-28) * |
| 李斌飞;崔世钢;施国英;祖林禄;: "基于无迹卡尔曼滤波的农用无人机定位研究", 中国农机化学报, no. 09, 30 September 2020 (2020-09-30) * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117346794A (en) * | 2023-12-05 | 2024-01-05 | 山东省科学院海洋仪器仪表研究所 | Unmanned ship integrated navigation system and navigation method for enteromorpha tracking |
| CN117346794B (en) * | 2023-12-05 | 2024-02-23 | 山东省科学院海洋仪器仪表研究所 | Unmanned ship integrated navigation system and navigation method for enteromorpha tracking |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116839591B (en) | 2024-05-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Bian et al. | Inertial navigation | |
| Huang et al. | Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS | |
| CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
| CN1322311C (en) | vehicle-carrying quick positioning and orienting method | |
| CN104655131B (en) | Inertial navigation Initial Alignment Method based on ISTSSRCKF | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN101858748A (en) | Fault-tolerance autonomous navigation method of multi-sensor of high-altitude long-endurance unmanned plane | |
| CN102519470A (en) | Multi-level embedded integrated navigation system and navigation method | |
| CN116839591B (en) | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle | |
| CN105928515A (en) | Navigation system for unmanned plane | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| Iqbal et al. | Experimental results on an integrated GPS and multisensor system for land vehicle positioning | |
| CN113503892A (en) | Inertial navigation system moving base initial alignment method based on odometer and backtracking navigation | |
| CN111964675A (en) | Intelligent aircraft navigation method for blackout area | |
| Yang et al. | Multilayer low-cost sensor local-global filtering fusion integrated navigation of small UAV | |
| CN113375664B (en) | Autonomous mobile device positioning method based on dynamic loading of point cloud map | |
| Al-Jlailaty et al. | Efficient attitude estimators: A tutorial and survey | |
| CN105928519A (en) | Navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer | |
| CN112325878A (en) | Ground carrier combined navigation method based on UKF and air unmanned aerial vehicle node assistance | |
| CN115202383B (en) | Unmanned aerial vehicle multidimensional track expression and generation method | |
| Atia et al. | A novel systems integration approach for multi-sensor integrated navigation systems | |
| Gao et al. | Gyroscope drift estimation in tightly-coupled INS/GPS navigation system | |
| Fang et al. | Integrating SINS sensors with odometer measurements for land vehicle navigation system | |
| Shien et al. | Integrated navigation accuracy improvement algorithm based on multi-sensor fusion | |
| Raković et al. | UAV Positioning and Navigation-Review |
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 |