阅读说明：本技术 一种gps拒止环境下高精度位姿估计算法 (High-precision pose estimation algorithm in GPS rejection environment ) 是由 雷旭升 王锐 付发 于 2021-09-10 设计创作，主要内容包括：一种GPS拒止环境下高精度位姿估计算法,涉及基于PI调节器的自适应互补滤波姿态算法和基于自适应模糊神经网络的高精度位置估计算法。首先在陀螺姿态解算方程的基础上,设计加计抑制函数,构建PI反馈修正回路来提升系统在高动态下的姿态测量精度；其次构建自适应模糊神经网络,在GPS有效时基于Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化模糊神经网络权值参数,以实现在GPS拒止环境下的高精度位置估计。本发明具有实时性好、抗扰动能力强、学习速度快、预测精度高等优点,可用于中小型无人机在GPS拒止环境下的高精度导航。(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.)

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:

in which delta epsilon is R^3×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:

wherein e (ω) ∈ R^3×1Is the error vector of the accelerometer, f^b∈R^3×1Normalized triaxial acceleration, g, for accelerometer measurements^b∈R^3×1Is the acceleration of gravity, k, under a carrier system_pIs a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adder_iFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:

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:

when searching along the negative gradient direction, the single-step operation formula is as follows:

X_k+1＝X_k+a_kP_k

wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, a_kFor a learning rate determined according to a line search criterion, P_kRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:

in the formula, g_kAn iterative gradient vector at the current moment;

the search can continue with the direction of the negative gradient, P_kThe expression is as follows:

in the formula, beta_kThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:

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.

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,

wherein Q ═ Q₀ q₁ q₂ q₃]^TIs composed of a scalar q₀And vector q₁i+q₂j+q₃k 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,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:

in which delta epsilon is R^3×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:

error of accelerometerVector e (ω) is e R^3×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:

in the formula (f)^b∈R^3×1Normalized triaxial acceleration, g, for accelerometer measurementsⁿ∈R^3×1Is the acceleration of gravity under geographic system, g^b∈R^3×1Is the gravity acceleration under the carrier system,converting the attitude of the geographic system to the carrier system;

k_pis a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adder_iFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:

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 usedThree-axis output of gyroscopeThree-axis velocity v of the system_iThe 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:

when searching along the negative gradient direction, the single-step operation formula is as follows:

X_k+1＝X_k+a_kP_k

wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, a_kFor a learning rate determined according to a line search criterion, P_kRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:

in the formula, g_kAn iterative gradient vector at the current moment;

the search can continue with the direction of the negative gradient, P_kThe expression is as follows:

in the formula, beta_kThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:

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:

wherein Q is [ Q ]₀ q₁ q₂ q₃]^TIs composed of a scalar q₀And vector q₁i+q₂j+q₃k constitutes a quaternion to describe the rotation of the carrier,angular velocity output by a gyroscope for projecting vectors of angular velocity from a geographic system to a carrier systemCalculated, the expression is as follows:

in the formula (I), the compound is shown in the specification,is the attitude transformation matrix from the geographic system to the carrier system,is the representation of the rotation angular speed of the geographic system relative to the inertial system under the carrier system,the angular velocity of rotation of the geographic system relative to the earth system caused by the change in position,the expressions of the rotational angular velocity of the earth in the geographic system are respectively as follows:

in the formula, v_xIs the east velocity of the vector, v_yIs the north velocity of the carrier, R_M、R_NThe 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 moment_tCan be expressed as:

wherein Q_t-1The quaternion is the quaternion of the previous moment, and delta t is an algorithm resolving period;

updating attitude transformation matrix by using quaternionThe expression is as follows:

attitude angle (course angle theta, pitch angle)Roll angle γ) from attitude transformation matrixAnd (4) calculating, wherein the expressions are respectively as follows:

in the formula (I), the compound is shown in the specification,is composed ofThe 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:

in which delta epsilon is R^3×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:

wherein the error vector e (ω) E ∈ R of the accelerometer^3×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:

in the formula (f)^b∈R^3×1Normalized triaxial acceleration, g, for accelerometer measurementsⁿ∈R^3×1Is the acceleration of gravity under geographic system, g^b∈R^3×1Is the gravitational acceleration under the carrier system;

k_pis a scale factor for adjusting the cross-over frequency, k, between the gyroscope and the adder_iFor the integral coefficient, to correct for drift error in gyro output angular velocity, p (a) is an accelerometer rejection function, expressed as:

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:

in the formula, phi_x、φ_y、φ_zFor three-axis attitude misalignment angle, δ v_x、δv_y、δv_zIs the three-axis speed error in the geographic system, δ L and δ λ are longitude and latitude errors, δ h is altitude error,in order to estimate the drift of the gyro, 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:

wherein G (t) e R^15×6For the noise coefficient matrix of the system, w (t) e R^6×1For the systematic process noise vector, F (t) e R^15×15The system state transition matrix is expressed as follows:

in the formula, F_N(t)∈R^9×9Error matrix for three-axis attitude, velocity and position, F_S(t)∈R^9×6Transfer matrix for inertial device error drift, F_M(t)∈R^6×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 R^6×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 R^6×15To measure the noise figure matrix, v (t) is e R^6×1Is the measured noise vector of the system;

training input settings for adaptive fuzzy neural networks as an additive triaxial outputThree-axis output of gyroscopeThree-axis velocity v of the system_iAnd 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 isA₁and y is B₁then h₁＝p₁x+q₁y+r₁

2、if x isA₂and y is B₂then h₂＝p₂x+q₂y+r₂

in the formula, x and y are input variables, A_i、B_iFor fuzzy sets, p_i、q_i、r_iThe 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:

in the formula (I), the compound is shown in the specification,are respectively A_i、B_iThe membership function of (2) comprises weight parameters of the adaptive fuzzy neural network,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 rule_iThe expression is as follows:

the third layer normalizes the excitation intensity, the expression is as follows:

the fourth layer of the inverse fuzzy 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 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:

the single step operation formula of the inverse gradient descent is as follows:

X_k+1＝X_k-a_kg_k

wherein k is the current time, k +1 is the next time, X is the parameter to be adjusted, a_kFor a learning rate determined according to a line search criterion, g_kFor the iterative gradient vector at the current time, when searching in the negative gradient direction,the single step equation can be expressed as:

X_k+1＝X_k+a_kP_k

in the formula, P_kRepresenting the searching direction of the current moment as the conjugate gradient vector of the current moment, and meeting the following conditions:

the search can continue with the direction of the negative gradient, P_kThe expression is as follows:

in the formula, beta_kThe conjugate parameter is calculated by a Fletcher-Reeves method, and the expression is as follows:

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.