CN113777645A - High-precision pose estimation algorithm in GPS rejection environment - Google Patents

High-precision pose estimation algorithm in GPS rejection environment Download PDF

Info

Publication number
CN113777645A
CN113777645A CN202111060355.6A CN202111060355A CN113777645A CN 113777645 A CN113777645 A CN 113777645A CN 202111060355 A CN202111060355 A CN 202111060355A CN 113777645 A CN113777645 A CN 113777645A
Authority
CN
China
Prior art keywords
gps
adaptive
neural network
layer
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.)
Granted
Application number
CN202111060355.6A
Other languages
Chinese (zh)
Other versions
CN113777645B (en
Inventor
雷旭升
王锐
付发
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202111060355.6A priority Critical patent/CN113777645B/en
Publication of CN113777645A publication Critical patent/CN113777645A/en
Application granted granted Critical
Publication of CN113777645B publication Critical patent/CN113777645B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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

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 high-precision pose estimation algorithm under a GPS rejection environment relates to a self-adaptive complementary filtering attitude algorithm based on a PI regulator and a high-precision position estimation algorithm based on a self-adaptive fuzzy neural network. Firstly, designing an addition inhibition function on the basis of a gyro attitude calculation equation, and constructing a PI feedback correction loop to improve the attitude measurement precision of a system under high dynamic conditions; and secondly, constructing an adaptive fuzzy neural network, and optimizing weight parameters of the fuzzy neural network based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined 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 real-time performance, strong disturbance resistance, high learning speed, high prediction precision and the like, and can be used for high-precision navigation of small and medium-sized unmanned aerial vehicles in a GPS rejection environment.

Description

High-precision pose estimation algorithm in GPS rejection environment
Technical Field
The invention relates to a high-precision pose estimation algorithm in a GPS rejection environment, which is suitable for the field of high-precision navigation of a moving carrier in a GPS rejection area.
Background
In recent years, small and medium-sized unmanned aerial vehicles are widely applied to military and civil fields due to the advantages of small size, light weight, good concealment, high cost performance and the like, and navigation systems of the small and medium-sized unmanned aerial vehicles are generally researched on the basis of GPS/SINS combined navigation due to the limitation of cost and volume.
With the wide application of unmanned aerial vehicles, in actual flight, a GPS rejection area may occur through complex environments such as dense forests, high buildings and tunnels, and the GPS cannot provide continuous and effective navigation information to assist the INS system. At this time, the problems of error drift, scale factor instability and the like generated by the inertial device are accumulated continuously because of no correction, so that the overall performance of the integrated navigation system is reduced. Therefore, the research on the high-precision pose estimation navigation algorithm under the GPS rejection environment has very important significance.
Strapdown inertial navigation algorithms, Kalman filter algorithms, complementary filter algorithms, and the like are often used to provide navigation information resolution for the system. The strapdown inertial navigation algorithm has high data updating speed and good stability when the operation time is short, but when navigation information is calculated for a long time, the error of the strapdown inertial navigation algorithm increases along with the time, the pose data drifts and the accuracy diverges, and the navigation requirements of long time and high accuracy are difficult to meet. The Kalman filtering algorithm depends on a GPS as a measurement value, when the GPS signal is abnormal, the measurement updating in the Kalman filtering process cannot be carried out, and at the moment, the pose resolving result is no longer reliable because the error accumulation diverges along with time. The complementary filtering algorithm is used for iteratively updating the attitude information, data are subjected to fusion processing according to the frequency domain characteristics of the sensor, the drift of the attitude angle can be effectively restrained, the complementary filtering algorithm is only suitable for a static state or a low-speed state, a large acceleration error is introduced in a high maneuvering state, and the adaptability of the system is poor.
Disclosure of Invention
The technical problem solved by the invention is as follows: a GPS/SINS combined navigation system solves the problem of attitude and position data divergence when GPS signals are interfered or shielded, provides a high-precision attitude estimation algorithm under a GPS rejection environment, introduces a PI feedback correction loop to realize self-adaptive compensation on gyro output based on an added measurement value as an observed quantity so as to correct attitude information of the system, constructs a self-adaptive fuzzy neural network, adopts a combined algorithm to predict and estimate system position information without a GPS, and realizes high-precision navigation under the GPS rejection environment.
The technical solution of the invention is as follows: firstly, on the basis of a gyro attitude calculation equation, a PI feedback correction loop is constructed, and self-adaptive compensation of an accelerometer to a gyro is introduced to solve the problem of attitude information divergence; and secondly, constructing an adaptive fuzzy neural network, optimizing parameters based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined 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 method comprises the following implementation steps:
(1) based on a quaternion equation solved by the gyro attitude,
Figure BDA0003256262450000021
wherein Q ═ Q0 q1 q2 q3]TIs composed of a scalar q0And vector q1i+q2j+q3k constitutes a quaternion for describing the rotation of the carrier, t is the current moment, t-1 is the previous moment, delta t is the algorithm resolving period,
Figure BDA0003256262450000022
projecting vectors for gyro angular velocities from the geographic system to the carrier system;
constructing a PI feedback correction loop, taking a measured value of an accelerometer as an observed quantity, introducing an accelerometer inhibition function, judging the acceleration state of the system, providing self-adaptive compensation for the angular velocity of the gyroscope, reducing the interference of the error of the accelerometer on the correction quantity of the gyroscope in a high maneuvering state, and realizing the inhibition of the angular drift of the gyroscope;
the attitude solution equation in the form of the compensated quaternion is as follows:
Figure BDA0003256262450000031
in which delta epsilon is R3×1And the filtering result generated by the PI feedback regulator is used for compensating the rotation error of the measured vector and the predicted vector, and the expression is as follows:
Figure BDA0003256262450000032
error of accelerometerVector e (ω) is e R3×1The measured value and the estimated value of the gravity vector are realized by means of vector product, and represent the pitch and roll errors in the attitude quaternion obtained by the gyroscope integration, and the expression is as follows:
Figure BDA0003256262450000033
in the formula (f)b∈R3×1Normalized triaxial acceleration, g, for accelerometer measurementsn∈R3×1Is the acceleration of gravity under geographic system, gb∈R3×1Is the gravity acceleration under the carrier system,
Figure BDA0003256262450000034
converting the attitude of the geographic system to the carrier system;
kpis a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adderiFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:
Figure BDA0003256262450000035
by introducing an accelerometer inhibition function p (a), the interference of the error of the accelerometer to the correction quantity of the gyro in a high maneuvering state is reduced, so that the self-adaptive adjustment of the feedback correction quantity delta is realized, and the calculation precision of the horizontal attitude angle fusion algorithm is improved.
(2) Constructing a neural network for training the position under the GPS normal state, selecting a weight parameter based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined algorithm to optimize a self-adaptive fuzzy neural network, and realizing the correction of the calculated position error of the SINS system under the GPS-free environment;
when the GPS is effective, the Kalman filtering algorithm is used for correcting the error of the SINS system, the position information solved by the SINS is compensated, and simultaneously, the three-axis output of the addition meter is used
Figure BDA0003256262450000041
Three-axis output of gyroscope
Figure BDA0003256262450000042
Three-axis velocity v of the systemiThe heading angle theta is used as input, and the Kalman filtering solution calculates position errors delta L and delta lambda which are used as output to construct a sample set to train the self-adaptive fuzzy neural network for predicting the position of the system;
the self-adaptive fuzzy neural network is a multilayer self-adaptive network based on an If-Then rule of a T-S type fuzzy logic system, and sequentially comprises a fuzzy layer, a fuzzy set operation layer, a normalization layer, an inverse fuzzy layer and a total output layer, wherein weight parameters of the neural network are respectively contained in the fuzzy layer and the inverse fuzzy layer;
in the forward channel, the weight parameter of the fuzzification layer is fixed, the weight parameter of the defuzzification layer is identified by adopting a least square method, in the backward channel, the weight parameter of the defuzzification layer is fixed, and the weight parameter of the fuzzification layer is identified by adopting a Fletcher-Reeves conjugate gradient method until the precision requirement is met or the maximum iteration number is 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:
Figure BDA0003256262450000043
when searching along the negative gradient direction, the single-step operation formula is as follows:
Xk+1＝Xk+akPk
wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, akFor a learning rate determined according to a line search criterion, PkRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:
Figure BDA0003256262450000044
in the formula, gkAn iterative gradient vector at the current moment;
the search can continue with the direction of the negative gradient, PkThe expression is as follows:
Figure BDA0003256262450000045
in the formula, betakThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:
Figure BDA0003256262450000051
and judging whether the GPS is effective or not based on the GPS satellite number, when the GPS is ineffective, predicting the position error of the system by using a parameter set of a trained neural network, and eliminating the error of the part from the navigation system to correct the position estimation of the system so as to correct the position information of the system.
Compared with the prior art, the invention has the advantages that:
(1) aiming at the problem that the output noise signal of an inertial sensor is increased in a complex environment without a GPS, the invention provides an attitude calculation method based on the adaptive complementary filtering of a PI regulator, designs a suppression function of harmful acceleration, and reduces the influence of the measurement error of an accelerometer on the attitude measurement precision in a high maneuvering state, thereby improving the disturbance resistance of a system and avoiding the error accumulation under the condition of no GPS auxiliary correction;
(2) aiming at the problem of position error accumulation in SINS system resolving under the GPS-free environment, the invention provides a high-precision position estimation algorithm based on a self-adaptive fuzzy neural network, and weight parameters of the neural network are optimized through a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined algorithm, so that optimization can be carried out at a faster convergence speed, 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 inhibited.
Drawings
FIG. 1 is a flow chart of an adaptive complementary filtering attitude 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 variation of course angle error of a self-grinding system in a sports car experiment;
FIG. 4 is a diagram of the variation of the roll angle error of the self-grinding system in a sports car experiment;
FIG. 5 is a diagram of variation of pitch angle error of a self-grinding system in a sports car experiment;
FIG. 6 is a graph showing the variation of longitude position deviation of the self-research system in a sports car experiment;
FIG. 7 is a diagram of variation of latitude position deviation of the self-research system in a sports car experiment;
Detailed Description
The invention provides attitude and position information based on GPS/SINS integrated navigation by considering volume and cost and using small-volume low-precision MEMS inertial navigation equipment. When GPS signals are interfered or lost, the invention adopts an inertial device to carry out complementary filtering and resolving attitude, and carries out self-adaptive compensation on an angle obtained by integrating the gyroscope by an accelerometer according to an acceleration state; and moreover, the position information of the system is predicted based on the neural network trained under the normal state of the GPS.
As shown in FIGS. 1 and 2, the present invention is embodied as follows
(1) Establishing attitude solution equation relationship
Establishing an attitude differential equation in a quaternion form as follows:
Figure BDA0003256262450000061
wherein Q is [ Q ]0 q1 q2 q3]TIs composed of a scalar q0And vector q1i+q2j+q3k constitutes a quaternion to describe the rotation of the carrier,
Figure BDA0003256262450000062
angular velocity output by a gyroscope for projecting vectors of angular velocity from a geographic system to a carrier system
Figure BDA0003256262450000063
Calculated, the expression is as follows:
Figure BDA0003256262450000064
in the formula (I), the compound is shown in the specification,
Figure BDA0003256262450000065
is the attitude transformation matrix from the geographic system to the carrier system,
Figure BDA0003256262450000066
is the representation of the rotation angular speed of the geographic system relative to the inertial system under the carrier system,
Figure BDA0003256262450000067
the angular velocity of rotation of the geographic system relative to the earth system caused by the change in position,
Figure BDA0003256262450000068
the expressions of the rotational angular velocity of the earth in the geographic system are respectively as follows:
Figure BDA0003256262450000069
in the formula, vxIs the east velocity of the vector, vyIs the north velocity of the carrier, RM、RNThe radius of the prime circle and the radius of the prime circle are respectively, L is the local latitude, and h is the carrier height;
solving the differential equation by adopting a first-order Runge-Kutta method, and obtaining the quaternion Q of the current momenttCan be expressed as:
Figure BDA0003256262450000071
wherein Qt-1The quaternion is the quaternion of the previous moment, and delta t is an algorithm resolving period;
updating attitude transformation matrix by using quaternion
Figure BDA0003256262450000072
The expression is as follows:
Figure BDA0003256262450000073
attitude angle (course angle theta, pitch angle)
Figure BDA0003256262450000074
Roll angle γ) from attitude transformation matrix
Figure BDA0003256262450000075
And (4) calculating, wherein the expressions are respectively as follows:
Figure BDA0003256262450000076
Figure BDA0003256262450000077
Figure BDA0003256262450000078
Figure BDA0003256262450000079
Figure BDA00032562624500000710
in the formula (I), the compound is shown in the specification,
Figure BDA00032562624500000711
is composed of
Figure BDA00032562624500000712
The element of the (i + 1) th row and the (j + 1) th column of the matrix;
in long-time measurement, the attitude angle of the system can deviate, and because the angular velocity error of the gyroscope is continuously accumulated in the integration process, the rotation angle obtained by the angular velocity integration alone generates drift error;
(2) constructing a PI feedback correction loop
In order to correct the accumulated error of the gyroscope angle and improve the measurement precision of the horizontal attitude angle, the measurement value based on the accelerometer is used as the observed quantity, and the self-adaptive feedback correction is realized through a PI feedback controller;
the corrected quaternion differential equation is updated as follows:
Figure BDA0003256262450000081
in which delta epsilon is R3×1And the filtering result generated by the PI feedback regulator is used for compensating the rotation error of the measured vector and the predicted vector, and the expression is as follows:
Figure BDA0003256262450000082
wherein the error vector e (ω) E ∈ R of the accelerometer3×1The measured value and the estimated value of the gravity vector are realized by means of vector product, and represent the pitch and roll errors in the attitude quaternion obtained by the gyroscope integration, and the expression is as follows:
Figure BDA0003256262450000083
in the formula (f)b∈R3×1Normalized triaxial acceleration, g, for accelerometer measurementsn∈R3×1Is the acceleration of gravity under geographic system, gb∈R3×1Is the gravitational acceleration under the carrier system;
kpis a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adderiFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:
Figure BDA0003256262450000084
by introducing an accelerometer inhibition function p (a), the interference of the error of the accelerometer to the correction quantity of the gyro in a high maneuvering state is reduced, so that the self-adaptive adjustment of the feedback correction quantity delta is realized, and the calculation precision of the horizontal attitude angle fusion algorithm is improved.
(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 solved by the SINS, simultaneously constructing training sample data, and training a self-adaptive fuzzy neural network for predicting the position of the system;
establishing a system model, and taking a 15-dimensional state variable X, wherein the expression is as follows:
Figure BDA0003256262450000085
in the formula, phix、φy、φzFor three-axis attitude misalignment angle, δ vx、δvy、δvzIs the three-axis speed error in the geographic system, δ L and δ λ are longitude and latitude errors, δ h is altitude error,
Figure BDA0003256262450000091
in order to estimate the drift of the gyro,
Figure BDA0003256262450000092
Figure BDA0003256262450000093
zero offset estimation is carried out on the accelerometer;
according to the Kalman filtering model, the state equation of the system is constructed as follows:
Figure BDA0003256262450000094
wherein G (t) e R15×6For the noise coefficient matrix of the system, w (t) e R6×1For the systematic process noise vector, F (t) e R15×15The system state transition matrix is expressed as follows:
Figure BDA0003256262450000095
in the formula, FN(t)∈R9×9Error matrix for three-axis attitude, velocity and position, FS(t)∈R9×6Transfer matrix for inertial device error drift, FM(t)∈R6×6A system error matrix corresponding to the gyroscope and the 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) e R6×1For system quantity measurement, selecting the difference value of the position and speed information given by GPS and the carrier position and speed information obtained by inertial navigation solution, H (t) epsilon R6×15To measure the noise figure matrix, v (t) is e R6×1Is the measured noise vector of the system;
training input settings for adaptive fuzzy neural networks as an additive triaxial output
Figure BDA0003256262450000096
Three-axis output of gyroscope
Figure BDA0003256262450000097
Three-axis velocity v of the systemiAnd the position errors delta L and delta lambda calculated by heading angle theta and Kalman filtering solution are used as output to construct a sample set pairTraining a self-adaptive neural network;
the adaptive fuzzy neural network is a multilayer adaptive network based on an If-Then rule of a T-S type fuzzy logic system, wherein the If-Then rule expression is as follows:
1、if x isA1and y is B1then h1＝p1x+q1y+r1
2、if x isA2and y is B2then h2＝p2x+q2y+r2
in the formula, x and y are input variables, Ai、BiFor fuzzy sets, pi、qi、riThe weight parameters of the neural network are continuously adjusted in the learning process;
the first fuzzy layer fuzzifies the input variables to obtain the membership degree of the corresponding fuzzy set, and the expression is as follows:
Figure BDA0003256262450000101
in the formula (I), the compound is shown in the specification,
Figure BDA0003256262450000102
are respectively Ai、BiThe membership function of (2) comprises weight parameters of the adaptive fuzzy neural network,
Figure BDA0003256262450000103
respectively corresponding membership function values;
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 ruleiThe expression is as follows:
Figure BDA0003256262450000104
the third layer normalizes the excitation intensity, the expression is as follows:
Figure BDA0003256262450000105
the fourth layer of the inverse fuzzy layer calculates the result of fuzzy rule self-adaptive reasoning, and the expression is as follows:
Figure BDA0003256262450000106
the fifth layer total output layer defuzzifies the output of the previous layer, calculates the weighted sum of all the outputs and outputs the network operation result;
because the bell-shaped function adopted by the fuzzy layer contains nonlinear parameters, the inverse fuzzy layer is linear parameters, the weight parameters of the adaptive fuzzy neural network are optimized based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined algorithm, and high-precision optimization is realized under the requirement of ensuring precision and rapidity.
In the forward channel, the weight parameter of the fuzzification layer is fixed, the weight parameter of the defuzzification layer is identified by adopting a least square method, in the backward channel, the weight parameter of the defuzzification layer is fixed, and the weight parameter of the fuzzification layer is identified by adopting a Fletcher-Reeves conjugate gradient method until the precision requirement is met or the maximum iteration number is 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:
Figure BDA0003256262450000111
the single step operation formula of the inverse gradient descent is as follows:
Xk+1＝Xk-akgk
wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, akFor a learning rate determined according to a line search criterion, gkFor the iterative gradient vector at the current time, when searching in the negative gradient direction,the single step equation can be expressed as:
Xk+1＝Xk+akPk
in the formula, PkRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:
Figure BDA0003256262450000112
the search can continue with the direction of the negative gradient, PkThe expression is as follows:
Figure BDA0003256262450000113
in the formula, betakThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:
Figure BDA0003256262450000114
by introducing a Fletcher-Reeves conjugate gradient method, parameters of the self-adaptive fuzzy neural network are optimized, and the convergence speed of the neural network and the prediction accuracy of position information are improved;
and judging whether the GPS is effective or not based on the GPS satellite number, when the GPS is ineffective, predicting the position error of the system by using a parameter set of a trained neural network, and eliminating the error of the part from the navigation system to correct the position estimation of the system so as to correct the position information of the system.
(4) Sports car example
And fixedly installing the self-researched navigation system and the high-precision fiber-optic gyroscope system in the car, and carrying out sports car experimental verification. The high-precision fiber-optic gyroscope 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 fiber-optic gyroscope system as a reference. The data of the experimental process is stored in the recorder, the posture data results of the sports car of a certain experiment are shown in fig. 3, fig. 4 and fig. 5, and the position data results are shown in fig. 6 and fig. 7.
The self-developed navigation system realizes low-cost and high-precision attitude calculation and position calculation, and can adapt to navigation in a high maneuvering state without a GPS. When the GPS satellite loss time is 600s, the root mean square error of the heading 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 loss 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.39 m.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Claims (3)

1. A high-precision pose estimation algorithm in a GPS rejection environment is characterized by comprising the following steps:
(1) constructing self-adaptive complementary filtering based on a PI regulator, wherein the attitude resolving equation of the compensated quaternion form is as follows:
Figure FDA0003256262440000011
in which delta epsilon is R3×1The filter result generated by the PI feedback regulator is used for compensating the rotation error of the measured vector and the predicted vector;
(2) designing an adaptive fuzzy neural network, and optimizing the weight of the adaptive neural network based on a strapdown Kalman filtering result when the GPS is effective; when the GPS is invalid, the position error is estimated on line based on the trained adaptive neural network, and the navigation performance is improved.
2. The PI regulator-based adaptive complementary filtering of claim 1, wherein: on the basis of an attitude algorithm of complementary filtering, the information of the added measurement value and the information of the gyroscope are effectively fused through a PI feedback correction loop, and meanwhile, an adding inhibition function is introduced to realize self-adaptive feedback correction, so that the anti-interference capability of the system in a high-maneuvering state is improved while attitude angle errors are effectively inhibited;
the filtering result δ of the PI feedback regulator is as follows:
Figure FDA0003256262440000012
wherein e (ω) ∈ R3×1Is the error vector of the accelerometer, fb∈R3×1Normalized triaxial acceleration, g, for accelerometer measurementsb∈R3×1Is the acceleration of gravity, k, under a carrier systempIs a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adderiFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:
Figure FDA0003256262440000013
3. the adaptive fuzzy neural network-based high precision position estimation algorithm of claim 1, wherein: the system position information under the GPS is predicted based on the self-adaptive fuzzy neural network, the Fletcher-Reeves conjugate gradient descent algorithm and the least square method combined algorithm are adopted for optimizing the weight parameters, the convergence speed is high, the algorithm has higher operation efficiency and stronger adaptability, and the prediction precision of the system position information is improved;
when the GPS is effective, training a self-adaptive fuzzy neural network, wherein the self-adaptive fuzzy neural network is a multi-layer self-adaptive network based on an If-Then rule of a T-S type fuzzy logic system, and sequentially comprises a fuzzy layer, a fuzzy set operation layer, a normalization layer, an inverse fuzzy layer and a total output layer, and weight parameters of the neural network are respectively contained in the fuzzy layer and the inverse fuzzy layer;
in the forward channel, the weight parameter of the fuzzification layer is fixed, the weight parameter of the defuzzification layer is identified by adopting a least square method, in the backward channel, the weight parameter of the defuzzification layer is fixed, and the weight parameter of the fuzzification layer is identified by adopting a Fletcher-Reeves conjugate gradient method until the precision requirement is met or the maximum iteration number is 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:
Figure FDA0003256262440000021
when searching along the negative gradient direction, the single-step operation formula is as follows:
Xk+1＝Xk+akPk
wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, akFor a learning rate determined according to a line search criterion, PkRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:
Figure FDA0003256262440000022
in the formula, gkAn iterative gradient vector at the current moment;
the search can continue with the direction of the negative gradient, PkThe expression is as follows:
Figure FDA0003256262440000031
in the formula, betakThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:
Figure FDA0003256262440000032
and judging whether the GPS is effective or not based on the GPS satellite number, and when the GPS is ineffective, correcting the position error of the system by using a trained parameter set of the neural network to predict the position information.
CN202111060355.6A 2021-09-10 2021-09-10 High-precision pose estimation algorithm under GPS refused environment Active CN113777645B (en)

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 true CN113777645A (en) 2021-12-10
CN113777645B 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)

* Cited by examiner, † Cited by third party
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

Patent Citations (4)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
"美陆军研究实验室( AＲL) 开发GPS 拒止环境下的导航定位新技术", 《无线电工程》, vol. 48, no. 12, pages 1071 *
HONGJIE MA 等: "Radar_Image-Based_Positioning_for_USV_Under_GPS_Denial_Environment", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》, vol. 19, no. 1, pages 72 - 80, XP055605390, DOI: 10.1109/TITS.2017.2690577 *
叶文 等: "惯性技术计量领域若干问题的思考与展望", 《计量科学与技术》, vol. 65, no. 3, pages 9 - 14 *
李冀: "国外提升卫星信号在拒止环境下导航定位能力的新技术", 《导航定位学报》, vol. 1, no. 2, pages 55 - 59 *
罗耀耀 等: "基于多传感器融合的机器人位姿估计研究", 《实验技术与管理》, vol. 37, no. 5, pages 58 - 62 *
陈浩然: "基于黎曼流形优化的数据降维表达及应用", 《中国博士学位论文全文数据库 基础科学辑》, no. 03, pages 002 - 26 *

Also Published As

Publication number Publication date
CN113777645B (en) 2024-01-09

Similar Documents

Publication Publication Date Title
CN112629538B (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN106767900B (en) Online calibration method of marine optical fiber strapdown inertial navigation system based on integrated navigation technology
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
Hong Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV)
CN112505737B (en) GNSS/INS integrated navigation method
CN112945225A (en) Attitude calculation system and method based on extended Kalman filtering
CN109945859B (en) Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering
CN110207691B (en) Multi-unmanned vehicle collaborative navigation method based on data link ranging
CN110285804B (en) Vehicle collaborative navigation method based on relative motion model constraint
CN112146655B (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
Wang et al. Event-triggered adaptive terminal sliding mode tracking control for drag-free spacecraft inner-formation with full state constraints
CN113295162A (en) Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information
Lei et al. An adaptive method of attitude and position estimation during GPS outages
CN111190207B (en) PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method
CN116147624A (en) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN113175926B (en) Self-adaptive horizontal attitude measurement method based on motion state monitoring
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
CN113777645B (en) High-precision pose estimation algorithm under GPS refused environment
Wang et al. Accurate Attitude Determination Based on Adaptive UKF and RBF Neural Network Using Fusion Methodology for Micro‐IMU Applied to Rotating Environment
Zhou et al. IMU dead-reckoning localization with RNN-IEKF algorithm

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