CN113777645B - High-precision pose estimation algorithm under GPS refused environment - Google Patents
High-precision pose estimation algorithm under GPS refused environment Download PDFInfo
- Publication number
- CN113777645B CN113777645B CN202111060355.6A CN202111060355A CN113777645B CN 113777645 B CN113777645 B CN 113777645B CN 202111060355 A CN202111060355 A CN 202111060355A CN 113777645 B CN113777645 B CN 113777645B
- Authority
- CN
- China
- Prior art keywords
- gps
- neural network
- adaptive
- algorithm
- precision
- 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
- 238000013528 artificial neural network Methods 0.000 claims abstract description 36
- 238000000034 method Methods 0.000 claims abstract description 26
- 238000001914 filtration Methods 0.000 claims abstract description 23
- 230000006870 function Effects 0.000 claims abstract description 19
- 238000005259 measurement Methods 0.000 claims abstract description 16
- 230000003044 adaptive effect Effects 0.000 claims abstract description 15
- 238000012937 correction Methods 0.000 claims abstract description 13
- 230000000295 complement effect Effects 0.000 claims abstract description 10
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 239000013598 vector Substances 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000005457 optimization Methods 0.000 claims description 8
- 238000002939 conjugate gradient method Methods 0.000 claims description 7
- 230000001629 suppression Effects 0.000 claims description 7
- 238000012549 training Methods 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 2
- 230000005764 inhibitory process Effects 0.000 abstract description 3
- 239000011159 matrix material Substances 0.000 description 11
- 230000008569 process Effects 0.000 description 8
- 238000002474 experimental method Methods 0.000 description 7
- 238000009825 accumulation Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 239000013307 optical fiber Substances 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000005284 excitation Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
-
- 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/18—Stabilised platforms, e.g. by gyroscope
Abstract
A high-precision pose estimation algorithm under a GPS rejection environment relates to an adaptive complementary filtering pose algorithm based on a PI regulator and a high-precision position estimation algorithm based on an adaptive fuzzy neural network. Firstly, designing an accelerometer inhibition function on the basis of a gyro attitude calculation equation, and constructing a feedback correction loop based on a PI regulator to improve the attitude measurement precision of the system under high dynamic state; and secondly, constructing a self-adaptive fuzzy neural network, and optimizing the weight parameters of the fuzzy neural network based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm when the GPS is effective so as to realize high-precision position estimation in a GPS rejection environment. The method has the advantages of good instantaneity, strong disturbance rejection capability, high learning speed, high prediction precision and the like, and can be used for high-precision navigation of the small and medium unmanned aerial vehicle in the GPS rejection environment.
Description
Technical Field
The invention relates to a high-precision pose estimation algorithm under a GPS refusing environment, which is suitable for the field of high-precision navigation of a movable carrier in a GPS refusing area.
Background
In recent years, small and medium-sized unmanned aerial vehicles are widely applied in the military and civil fields due to the advantages of small size, light weight, good concealment, high cost performance and the like, and navigation systems thereof are generally researched on the basis of GPS/SINS integrated navigation due to the limitation of cost and volume.
With the wide application of unmanned aerial vehicle, possibly through complex environments such as dense forest, high building, tunnel, etc. in actual flight, GPS refuses the district, and GPS can't provide continuous effective navigation information auxiliary INS system. At this time, the problems of error drift, unstable scale factors and the like generated by the inertial device are accumulated continuously because the problems are not corrected, so that the overall performance of the integrated navigation system is reduced. Therefore, the research of the high-precision pose estimation navigation algorithm under the GPS rejection environment has very important significance.
Strapdown inertial navigation algorithms, kalman filtering algorithms, complementary filtering algorithms, etc. are often used to provide navigation information solutions for the system. The strapdown inertial navigation algorithm has the advantages that the data updating speed is high, the stability is good when the operation time is short, but when the navigation information is resolved for a long time, the error is increased along with the time, the pose data drift and the precision divergence are generated, and the long-time and high-precision navigation requirements are difficult to meet. The Kalman filtering algorithm depends on GPS as a measurement value, when the GPS signal is abnormal, measurement update in the Kalman filtering process cannot be performed, and the pose solving result is not reliable any more because of error accumulation and divergence along with time. The complementary filtering algorithm is used for iteratively updating the attitude information, and fusion processing is carried out on the data according to the frequency domain characteristics of the sensor, so that the drift of an attitude angle can be effectively restrained, but the complementary filtering algorithm is only suitable for a static or low-speed state, and larger acceleration errors are introduced in a high maneuver state, so that the adaptability of the system is poor.
Disclosure of Invention
The invention solves the technical problems that: the GPS/SINS integrated navigation system solves the problem that the attitude and position data diverge when GPS signals are interfered or blocked, provides a high-precision attitude estimation algorithm under the GPS rejection environment, introduces a PI regulator to realize the self-adaptive compensation of gyro output based on the measured value of an accelerometer as an observed quantity so as to correct the attitude information of the system, constructs a self-adaptive fuzzy neural network, adopts a combined algorithm to predict and estimate the system position information under the GPS-free environment, and realizes the high-precision navigation under the GPS rejection environment.
The technical scheme of the invention is as follows: firstly, on the basis of a gyroscope attitude solving equation, a feedback correction loop is constructed based on a PI regulator, and the self-adaptive compensation of an accelerometer to a gyroscope is introduced to solve the problem of attitude information divergence; and secondly, constructing a self-adaptive fuzzy neural network, optimizing parameters based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm, and training in a GPS effective state to provide real-time high-precision position information for the system when the GPS is invalid. The implementation steps are as follows:
(1) Based on the quaternion equation of the gyroscope attitude solution,
wherein q= [ Q ] 0 q 1 q 2 q 3 ] T Is formed by scalar q 0 Sum vector q 1 i+q 2 j+q 3 k forms a quaternion used for describing the rotation of the carrier, t is the current moment, t-1 is the previous moment, deltat is the algorithm calculation period,projected vectors for gyro angular velocities from geographic to carrier systems;
constructing a feedback correction loop, taking a measured value of an accelerometer as an observed quantity, introducing an accelerometer suppression function, judging a system acceleration state, providing self-adaptive compensation for the angular speed of the gyroscope, reducing the interference of the error of the accelerometer on the correction quantity of the gyroscope in a high maneuver state, and realizing the suppression of the angular drift of the gyroscope;
the gesture solution equation of the compensated quaternion form is as follows:
in delta epsilon R 3×1 The filter result generated by the PI regulator is used for compensating the rotation error of the actual measurement vector and the prediction vector, and the expression is as follows:
where the error vector e (ω) εR of the accelerometer 3×1 From measurements and estimates of gravity vectorsThe values are realized in a vector product way and represent pitch and roll errors in the attitude quaternion obtained by integrating a gyroscope, and the expression is as follows:
wherein f b ∈R 3×1 Triaxial acceleration normalized for accelerometer measurements, g n ∈R 3×1 G is the gravitational acceleration under the geographic system b ∈R 3×1 For the acceleration of gravity under the carrier system,a pose transformation matrix from geographic system to carrier system;
k p is a proportionality coefficient for adjusting the crossover frequency, k, between gyroscope and accelerometer i For the integral coefficient, to correct drift error of gyro output angular velocity, p (a) is an accelerometer suppression function, whose expression is as follows:
by introducing the accelerometer inhibition function p (a), the interference of the error of the accelerometer on the gyro correction amount under the high maneuver state is reduced, thereby realizing the self-adaptive adjustment of the feedback correction amount delta and improving the resolving precision of the horizontal attitude angle fusion algorithm.
(2) Constructing a neural network for position training under a GPS normal state, selecting a weight parameter based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm to optimize a self-adaptive fuzzy neural network, and correcting a SINS system resolving position error under a GPS-free environment;
when GPS is effective, the Kalman filtering algorithm is used for correcting the error of the SINS system, compensating the position information of SINS solution and simultaneously outputting by the three axes of the accelerometerTriaxial output of gyroscope->Triaxial speed v of the system i The heading angle theta is used as input, and the position errors delta L and delta lambda calculated by Kalman filtering are used as output to construct a sample set to train the self-adaptive fuzzy neural network for predicting the system position;
the self-adaptive fuzzy neural network is a multi-layer self-adaptive network based on an If-ten rule of a T-S type fuzzy logic system, and sequentially comprises a fuzzification layer, a fuzzy set operation layer, a normalization layer, an anti-fuzzification layer and a total output layer, wherein weight parameters of the neural network are respectively contained in the fuzzification layer and the anti-fuzzification layer;
in the forward channel, the weight parameters of the fuzzification layer are fixed, the least square method is adopted to identify the weight parameters of the anti-fuzzification layer, in the backward channel, the weight parameters of the anti-fuzzification layer are fixed, and the Fletcher-Reeves conjugate gradient method is adopted to identify the weight parameters of the fuzzification layer until the accuracy requirement is met or the maximum iteration times are reached;
the Fletcher-Reeves conjugate gradient method converts the optimization problem into an objective function minimization problem, and the objective function expression is as follows:
when searching along the negative gradient direction, the single-step operation formula is as follows:
X k+1 ＝X k +a k P k
wherein k is the current time, k+1 is the next time, X is the parameter to be adjusted, a k P for learning rate determined from line search criteria k The conjugate gradient vector of the current moment represents the searching direction of the current moment, and when the following conditions are satisfied:
in the formula g k An iteration gradient vector at the current moment;
the search can be continued with a negative gradient direction, P k The expression is as follows:
wherein beta is k For conjugate parameters, fletcher-Reeves method was chosen for calculation, and the expression is as follows:
judging whether the GPS is effective or not based on the satellite number of the GPS, when the GPS is ineffective, predicting the position error of the system by using the trained parameter set of the neural network, and eliminating the error of the part from the navigation system to correct the position estimation of the system, thereby correcting the position information of the system.
Compared with the prior art, the invention has the advantages that:
(1) Aiming at the problem that the noise signal output by an inertial sensor is increased in a complex environment without a GPS, the invention provides a self-adaptive complementary filtering gesture resolving method based on a PI regulator, which designs a suppression function of harmful acceleration, reduces the influence of the measurement error of an accelerometer on gesture measurement precision in a high maneuver state, thereby improving the disturbance rejection capability of the system and avoiding error accumulation without GPS auxiliary correction;
(2) Aiming at the problem of SINS system resolving position error accumulation under GPS-free environment, the invention provides a high-precision position estimation algorithm based on a self-adaptive fuzzy neural network, and the weight parameters of the neural network are optimized through a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm, so that the optimization can be realized at a higher convergence rate, the local optimization is avoided, the learning efficiency and the prediction precision of the neural network are improved, and the position information divergence in the horizontal direction when the GPS fails is effectively restrained.
Drawings
FIG. 1 is a flow chart of an adaptive complementary filtering gesture algorithm based on a PI regulator;
FIG. 2 is a flow chart of a high-precision position estimation algorithm based on an adaptive fuzzy neural network;
FIG. 3 is a diagram of course angle error change of the self-grinding system in a sports car experiment;
FIG. 4 is a graph showing the roll angle error of the self-grinding system in the sports car experiment;
FIG. 5 is a graph showing the pitch angle error of the self-grinding system in a sports car experiment;
FIG. 6 is a graph showing the variation of the longitude position deviation of the self-grinding system in the sports car experiment;
FIG. 7 is a graph showing the deviation of the latitude position of the self-grinding system in the sports car experiment;
Detailed Description
In the invention, the small-size low-precision MEMS inertial navigation equipment is used in consideration of the volume and the cost, and the attitude and position information is provided based on GPS/SINS integrated navigation. When GPS signals are interfered or lost, the invention adopts an inertial device to carry out complementary filtering and resolving gesture, and carries out self-adaptive compensation on the angle obtained by gyro integration by an accelerometer according to the acceleration state; in addition, the position information of the system is predicted based on the neural network trained in the normal state of the GPS.
As shown in fig. 1 and 2, the present invention is embodied as follows
(1) Establishing an equation relation for solving the gesture
The pose differential equation in the form of quaternion is established as follows:
wherein Q= [ Q ] 0 q 1 q 2 q 3 ] T Is formed by scalar q 0 Sum vector q 1 i+q 2 j+q 3 k constitutes a quaternion describing the rotation of the carrier,for projection of angular velocity from geographical to carrierVector, angular velocity outputted by gyroscope +.>The expression is calculated as follows:
in the method, in the process of the invention,posture transformation matrix for geographical system to carrier system, < >>For the representation of the rotational angular velocity of the geographical system relative to the inertial system under the carrier system, +.>For the rotational angular velocity of the geographical system relative to the earth system caused by the change of position +.>The expression of the rotation angular velocity of the earth under the geographic system is as follows:
in the formula, v x For the eastern speed of the carrier, v y For carrier north speed, R M 、R N The radius of each mortise circle and each noon circle is respectively, L is the local latitude, and h is the carrier height;
solving differential equation by first-order Dragon-Kutta method, and then the quaternion Q at the current moment t Can be expressed as:
wherein Q is t-1 As the quaternion of the previous moment, deltat is the algorithm resolving period;
updating a gesture conversion matrix using the quaternionThe expression is as follows:
attitude angle (heading angle θ, pitch angle)Roll angle γ) is converted by the posture conversion matrix +.>The expression is calculated as follows:
in the method, in the process of the invention,is->Elements of the i+1th row and j+1th column of the matrix;
in long-time measurement, the attitude angle of the system can deviate, because the angular speed error of the gyroscope is continuously accumulated in the integration process, the rotation angle obtained by the angular speed integration alone generates drift error;
(2) Constructing a feedback correction loop
In order to correct the accumulated error of the gyroscope angle, the measurement accuracy of the horizontal attitude angle is improved, the measurement value based on the accelerometer is used as an observed quantity, and the self-adaptive feedback correction is realized through a PI regulator;
the corrected quaternion differential equation is updated as follows:
in delta epsilon R 3×1 The filter result generated by the PI regulator is used for compensating the rotation error of the actual measurement vector and the prediction vector, and the expression is as follows:
where the error vector e (ω) εR of the accelerometer 3×1 The measured value and the estimated value of the gravity vector are realized in a vector product mode, and represent the pitch and roll errors in the attitude quaternion obtained by integrating a gyroscope, wherein the expression is as follows:
wherein f b ∈R 3×1 Triaxial acceleration normalized for accelerometer measurements, g n ∈R 3×1 G is the gravitational acceleration under the geographic system b ∈R 3×1 Is the gravitational acceleration under the carrier system;
k p is a proportionality coefficient for adjusting the crossover frequency, k, between gyroscope and accelerometer i For the integral coefficient, to correct drift error of gyro output angular velocity, p (a) is an accelerometer suppression function, whose expression is as follows:
by introducing the accelerometer inhibition function p (a), the interference of the error of the accelerometer on the gyro correction amount under the high maneuver state is reduced, thereby realizing the self-adaptive adjustment of the feedback correction amount delta and improving the resolving precision of the horizontal attitude angle fusion algorithm.
(3) High-precision position estimation algorithm based on self-adaptive fuzzy neural network
When the GPS is effective, correcting the error of the SINS by using a Kalman filtering algorithm, compensating the position information calculated by the SINS, constructing training sample data, and training an adaptive fuzzy neural network for predicting the position of the system;
a system model is built, a 15-dimensional state variable X is taken, and the expression is as follows:
in phi x 、φ y 、φ z Is the three-axis attitude misalignment angle δv x 、δv y 、δv z Is a triaxial speed error under a geographic system, δl, δλ are longitude and latitude errors, δh is a altitude error,for gyro drift estimation, +.>Zero offset estimation for the accelerometer;
according to a Kalman filtering model, the state equation of the system is constructed as follows:
wherein G (t) ∈R 15×6 For the noise figure matrix of the system, w (t) εR 6×1 F (t) εR, the process noise vector for the system 15×15 The expression of the system state transition matrix is as follows:
wherein F is N (t)∈R 9×9 Error matrix of three-axis gesture, speed and position, F S (t)∈R 9×6 Transfer matrix for inertial device error drift, F M (t)∈R 6×6 The system error matrix corresponds to a gyroscope and an accelerometer;
according to the Kalman filtering model, a measurement equation of the system is constructed as follows:
Z(t)＝H(t)X(t)+v(t)
wherein Z (t) ∈R 6×1 For measuring system quantity, selecting the difference value of position and speed information given by GPS and carrier position and speed information obtained by inertial navigation solution, H (t) E R 6×15 To measure the noise figure matrix, v (t) ∈R 6×1 Measuring noise vector for the system;
the training input of the adaptive fuzzy neural network is set as the tri-axial output of the accelerometerTriaxial output of gyroscope->Triaxial speed v of the system i And the position errors delta L and delta lambda calculated by Kalman filtering are used as output construction sample sets to train the self-adaptive neural network;
the adaptive fuzzy neural network is a multi-layer adaptive network based on an 'If-Then' rule of a T-S type fuzzy logic system, and the expression of the 'If-Then' rule is as follows:
1、if x isA 1 and y is B 1 then h 1 ＝p 1 x+q 1 y+r 1
2、if x isA 2 and y is B 2 then h 2 ＝p 2 x+q 2 y+r 2
wherein x and y are input variables, A i 、B i For fuzzy set, p i 、q i 、r i The weight parameters of the neural network are continuously adjusted in the learning process;
the first fuzzification layer fuzzifies the input variable to obtain membership of a corresponding fuzzy set, and the expression is as follows:
in the method, in the process of the invention,respectively A i 、B i Wherein the membership function of (1) comprises the weight parameter of the adaptive fuzzy neural network, < ->The membership function values are respectively corresponding;
the second fuzzy set operation layer multiplies the membership function value in the first layer to obtain the excitation intensity w of the ith fuzzy rule i The expression is as follows:
the third layer normalizes excitation intensity as follows:
the fourth layer of anti-fuzzification layer calculates the result of fuzzy rule self-adaptive reasoning, and the expression is as follows:
the fifth layer total output layer defuzzifies the output of the upper layer, calculates the weighted sum of all the outputs and outputs the network operation result;
the bell-shaped function adopted by the blurring layer comprises nonlinear parameters, the anti-blurring layer is a linear parameter, the weight parameters of the self-adaptive fuzzy neural network are optimized based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm, and high-precision optimization is realized under the requirements of ensuring precision and rapidity.
In the forward channel, the weight parameters of the fuzzification layer are fixed, the least square method is adopted to identify the weight parameters of the anti-fuzzification layer, in the backward channel, the weight parameters of the anti-fuzzification layer are fixed, and the Fletcher-Reeves conjugate gradient method is adopted to identify the weight parameters of the fuzzification layer until the accuracy requirement is met or the maximum iteration times are reached;
the Fletcher-Reeves conjugate gradient method converts the optimization problem into an objective function minimization problem, and the objective function expression is as follows:
the single step formula for the reverse gradient descent is as follows:
X k+1 ＝X k -a k g k
wherein k is the current time, k+1 is the next time, X is the parameter to be adjusted, a k G for learning rate determined from line search criteria k For the iterative gradient vector at the current moment, when searching along the negative gradient direction, the single-step operation formula can be expressed as:
X k+1 ＝X k +a k P k
in the middle of，P k The conjugate gradient vector of the current moment represents the searching direction of the current moment, and when the following conditions are satisfied:
the search can be continued with a negative gradient direction, P k The expression is as follows:
wherein beta is k For conjugate parameters, fletcher-Reeves method was chosen for calculation, and the expression is as follows:
by introducing a Fletcher-Reeves conjugate gradient method, optimizing parameters of the self-adaptive fuzzy neural network, the convergence speed of the neural network and the prediction accuracy of position information are improved;
judging whether the GPS is effective or not based on the satellite number of the GPS, when the GPS is ineffective, predicting the position error of the system by using the trained parameter set of the neural network, and eliminating the error of the part from the navigation system to correct the position estimation of the system, thereby correcting the position information of the system.
(4) Sports car example
And fixedly installing the self-grinding navigation system and the high-precision optical fiber gyro system in a car, and carrying out the verification of the sports car experiment. And the high-precision optical fiber gyro system has the attitude precision of 0.02 degree and the position precision of 0.1m, and the self-grinding navigation system is tested by taking the output of the high-precision optical fiber gyro system as a reference. The data of the experimental process are stored in a recorder, the posture data result of the sports car of a certain experiment is shown in fig. 3, 4 and 5, and the position data result is shown in fig. 6 and 7.
The self-research navigation system realizes low-cost and high-precision gesture resolving and position resolving, and can adapt to navigation in a high maneuver state without GPS. When the GPS satellite-missing time is 600s, the root mean square error of the course angle of the system is 1.1828 degrees, the root mean square error of the roll angle is 0.1812 degrees, and the root mean square of the pitch angle is 0.6037 degrees. When the GPS satellite-missing time is 30s, the root mean square error of the longitude position is 1.83m, and the root mean square error of the latitude position is 1.39m.
What is not described in detail in the present specification belongs to the prior art known to those skilled in the art.
Claims (1)
1. A GPS refuses the high-accuracy pose estimation algorithm under the environment, characterized by that to realize the following steps:
(1) When the GPS is effective, the position and posture information of the carrier is calculated based on a strapdown Kalman filtering algorithm, an adaptive fuzzy neural network is designed, and weight optimization is carried out on the adaptive neural network by utilizing a strapdown Kalman filtering result;
(2) When the GPS is invalid, constructing an attitude information based on a PI regulator self-adaptive complementary filtering solving carrier, wherein an attitude solving equation in a quaternion form after compensation is as follows:
in delta epsilon R 3×1 The filtering result is generated by the PI regulator and used for compensating the rotation error of the actual measurement vector and the prediction vector; meanwhile, based on the trained self-adaptive neural network, the position error is estimated on line, so that real-time compensation of strapdown calculation positions is realized, and the navigation performance is improved;
the self-adaptive complementary filtering based on the PI regulator is characterized in that: based on a gesture algorithm of complementary filtering, the measured value of the accelerometer is effectively fused with the information of the gyroscope through a feedback correction loop based on a PI regulator, and meanwhile, an accelerometer suppression function is introduced to realize self-adaptive feedback correction, so that the anti-interference capability of the system in a high maneuver state is improved while the gesture angle error is effectively suppressed;
the filtering result δ of the PI regulator is as follows:
wherein e (ω) εR 3 × 1 For the error vector of the accelerometer, f b ∈R 3×1 Triaxial acceleration normalized for accelerometer measurements, g b ∈R 3×1 Is the gravitational acceleration under the carrier system, k p Is a proportionality coefficient for adjusting the crossover frequency, k, between gyroscope and accelerometer i For the integral coefficient, to correct drift error of gyro output angular velocity, p (a) is an accelerometer suppression function, whose expression is as follows:
the high-precision position estimation algorithm based on the self-adaptive fuzzy neural network is characterized in that: the optimization of weight parameters based on the adaptive fuzzy neural network prediction system position information without GPS adopts a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combination algorithm, has the advantage of high convergence rate, ensures that the algorithm has higher operation efficiency and stronger adaptability, and improves the prediction precision of the system position information;
when the GPS is effective, training an adaptive fuzzy neural network, wherein the adaptive fuzzy neural network is a multi-layer adaptive network based on an If-ten rule of a T-S type fuzzy logic system, and sequentially comprises a fuzzification layer, a fuzzy set operation layer, a normalization layer, an anti-fuzzification layer and a total output layer, and weight parameters of the neural network are respectively contained in the fuzzification layer and the anti-fuzzification layer;
in the forward channel, the weight parameters of the fuzzification layer are fixed, the least square method is adopted to identify the weight parameters of the anti-fuzzification layer, in the backward channel, the weight parameters of the anti-fuzzification layer are fixed, and the Fletcher-Reeves conjugate gradient method is adopted to identify the weight parameters of the fuzzification layer until the accuracy requirement is met or the maximum iteration times are reached;
the Fletcher-Reeves conjugate gradient method converts the optimization problem into an objective function minimization problem, and the objective function expression is as follows:
when searching along the negative gradient direction, the single-step operation formula is as follows:
X k+1 ＝X k +a k P k
wherein k is the current time, k+1 is the next time, X is the parameter to be adjusted, a k P for learning rate determined from line search criteria k The conjugate gradient vector of the current moment represents the searching direction of the current moment, and when the following conditions are satisfied:
in the formula g k An iteration gradient vector at the current moment;
the search can be continued with a negative gradient direction, P k The expression is as follows:
wherein beta is k For conjugate parameters, fletcher-Reeves method was chosen for calculation, and the expression is as follows:
judging whether the GPS is effective or not based on the GPS satellite receiving number, and when the GPS is ineffective, correcting the position error of the system by using the trained parameter set of the neural network, and predicting the position information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111060355.6A CN113777645B (en) | 2021-09-10 | 2021-09-10 | High-precision pose estimation algorithm under GPS refused environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111060355.6A CN113777645B (en) | 2021-09-10 | 2021-09-10 | High-precision pose estimation algorithm under GPS refused environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113777645A CN113777645A (en) | 2021-12-10 |
| CN113777645B true CN113777645B (en) | 2024-01-09 |
Family
ID=78842396
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111060355.6A Active CN113777645B (en) | 2021-09-10 | 2021-09-10 | High-precision pose estimation algorithm under GPS refused environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113777645B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN112066979A (en) * | 2020-08-27 | 2020-12-11 | 北京航空航天大学 | Polarization pose information coupling iteration autonomous navigation positioning method |
| CN112781588A (en) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | Navigation resolving method for while-drilling gyroscope positioning and orientation instrument |
| CN112945225A (en) * | 2021-01-19 | 2021-06-11 | 西安理工大学 | Attitude calculation system and method based on extended Kalman filtering |
-
2021
- 2021-09-10 CN CN202111060355.6A patent/CN113777645B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN112066979A (en) * | 2020-08-27 | 2020-12-11 | 北京航空航天大学 | Polarization pose information coupling iteration autonomous navigation positioning method |
| CN112781588A (en) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | Navigation resolving method for while-drilling gyroscope positioning and orientation instrument |
| CN112945225A (en) * | 2021-01-19 | 2021-06-11 | 西安理工大学 | Attitude calculation system and method based on extended Kalman filtering |
Non-Patent Citations (6)
| Title |
|---|
| Radar_Image-Based_Positioning_for_USV_Under_GPS_Denial_Environment;Hongjie Ma 等;《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》;第19卷(第1期);第72-80页 * |
| 国外提升卫星信号在拒止环境下导航定位能力的新技术;李冀;《导航定位学报》;第1卷(第2期);第55-59页 * |
| 基于多传感器融合的机器人位姿估计研究;罗耀耀 等;《实验技术与管理》;第37卷(第5期);第58-62页 * |
| 基于黎曼流形优化的数据降维表达及应用;陈浩然;《中国博士学位论文全文数据库 基础科学辑》(第03期);A002-26 * |
| 惯性技术计量领域若干问题的思考与展望;叶文 等;《计量科学与技术》;第65卷(第3期);第9-14页 * |
| 美陆军研究实验室( AＲL) 开发GPS 拒止环境下的导航定位新技术;《无线电工程》;第48卷(第12期);第1071页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113777645A (en) | 2021-12-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112629538B (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| Huang et al. | Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS | |
| CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
| CN109000640B (en) | Vehicle GNSS/INS integrated navigation method based on discrete grey neural network model | |
| CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN110207691B (en) | Multi-unmanned vehicle collaborative navigation method based on data link ranging | |
| CN109945859B (en) | Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering | |
| CN112504275B (en) | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN112945225A (en) | Attitude calculation system and method based on extended Kalman filtering | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN115265532A (en) | Auxiliary filtering method for marine integrated navigation | |
| Lei et al. | An adaptive method of attitude and position estimation during GPS outages | |
| CN113777645B (en) | High-precision pose estimation algorithm under GPS refused environment | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN112325878A (en) | Ground carrier combined navigation method based on UKF and air unmanned aerial vehicle node assistance | |
| CN110388942B (en) | Vehicle-mounted posture fine alignment system based on angle and speed increment | |
| CN111964675A (en) | Intelligent aircraft navigation method for blackout area | |
| CN115574817A (en) | Navigation method and navigation system based on three-axis rotation type inertial navigation system | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data |
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 |