Polarization / inertial navigation integrated navigation method of embedded unmanned aerial vehicle kinetic model

Abstract

The invention relates to a polarization / inertial navigation integrated navigation method of an embedded unmanned aerial vehicle kinetic model, which comprises the following steps of: expanding a three-dimensional attitude error angle, a speed error, a position error, an angular speed error and a thrust coefficient error of the unmanned aerial vehicle kinetic model into inertial navigation state parameters as state quantities to form system state quantities; establishing a system state equation; taking the course angle calculated by the polarization sensor and the attitude, the speed and the position of the unmanned aerial vehicle kinetic model as quantity measurement, and establishing a system measurement equation; establishing a credibility discrimination function of polarization course measurement based on course angle innovation, and designing a combined navigation mode switching and thrust coefficient error feedback strategy; a Kalman filtering method is used for estimating system state quantities, namely an inertial navigation attitude misalignment angle, a speed error and a position error, and an attitude error angle, a speed error, a position error and a thrust coefficient error of an unmanned aerial vehicle dynamic model, so that correction of the attitude and the position of the unmanned aerial vehicle is realized.

Description

Technical Field

[0001] The present invention belongs to the field of combined navigation of unmanned aerial vehicles, and specifically relates to a polarization / inertial navigation combined navigation method with an embedded unmanned aerial vehicle dynamics model. The method utilizes a polarization sensor and a unmanned aerial vehicle dynamics model to enhance the constraints on inertial navigation attitude and position errors, realizes online estimation and correction of the thrust coefficient error of the unmanned aerial vehicle, improves the combined navigation accuracy of polarization / dynamics / inertial navigation, and provides an effective technical means for autonomous attitude determination and positioning of unmanned aerial vehicles in a satellite denial environment. Background Art

[0002] With the development of drone technology, it has demonstrated an irreplaceable role in all areas of human life and has become an important equipment in the fields of national defense and economy. UAV navigation technology is the core key technology of drone systems, which can provide them with information such as position and attitude. At present, the most commonly used navigation method for drones is still inertial / satellite combined navigation. However, in an interference denial environment, the performance of satellite navigation is attenuated, and the accumulated error of inertial navigation cannot be effectively corrected, which seriously affects the working efficiency of drones. Therefore, how to improve the autonomous navigation accuracy of drones under satellite signal denial conditions needs to be studied urgently.

[0003] Monarch butterflies, sand ants and other organisms can use polarized light for navigation, which provides ideas for solving the problem of autonomous navigation of drones under conditions of satellite signal denial. The patent "An airborne inertial / polarized light / optical flow / vision combined navigation device" (application number: CN202123204111.9) uses inertial / polarized light / optical flow / vision combined navigation to solve the problem of poor navigation accuracy of drones. The patent "A drone pose estimation method based on visual inertial polarized light fusion" (application number: CN202010623718.1) uses a drone pose estimation method based on visual inertial polarized light fusion to overcome the defect that the heading error accumulates over time during drone pose estimation. Although the above patents have achieved certain results, polarized light is mainly used to correct the cumulative heading error of inertial navigation, and it is difficult to achieve effective correction of the drone position error. Therefore, how to improve the positioning accuracy of drones under conditions of satellite signal denial needs to be solved urgently.

[0004] In order to improve the positioning accuracy of UAVs under satellite signal denial conditions, many researchers have conducted extensive research on the navigation method of embedded dynamic models. The patent "A quadrotor aircraft dynamic model / airborne sensor combined navigation method" (application number: CN201810244190.X) uses a satellite / geomagnetic / rotation speed sensor combined navigation method to achieve the combined navigation of a quadrotor aircraft based on the consideration of dynamic model assistance. The patent "A combined navigation and positioning method based on dynamic model assistance" (application number: CN202010410189.7) uses a satellite / inertial combined navigation method to achieve the navigation and positioning of an underwater autonomous submersible based on the consideration of dynamic model assistance. The above method only uses the UAV dynamic model as a measurement to assist in correcting the inertial navigation, and does not consider the correction of the UAV dynamic model. In the actual environment, the UAV dynamic model has parameter errors, which affects the recursive accuracy of the dynamic model for attitude, velocity and position. Therefore, how to combine polarization navigation with the UAV dynamics model, while correcting the accumulated inertial navigation error, use the polarization sensor to estimate and compensate the parameters of the UAV dynamics model, improve the accuracy of the dynamics model, further enhance the constraints of polarization / dynamics on the inertial navigation error, and realize the effective fusion of the three information. Summary of the invention

[0005] In order to solve the above problems and overcome the shortcomings of the prior art, the present invention proposes a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model, and expands the three-dimensional attitude error angle, velocity error, position error, angular velocity error and thrust coefficient error of the UAV dynamics model as state quantities to the inertial navigation state parameters to form the system state quantity, and establishes the system state equation. The heading angle solved by the polarization sensor, the attitude, velocity and position of the UAV dynamics model are used as measurements to establish the system measurement equation. Based on the heading angle update information, a credibility discriminant function of the polarization heading vector measurement is established, and a combined navigation mode switching and thrust coefficient error feedback strategy are designed. Finally, the Kalman filtering method is used to estimate the system state quantities, namely the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model, so as to realize the correction of the UAV attitude and position.

[0006] The technical solution of the present invention is: a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model, the implementation steps are as follows:

[0007] Step (1), the three-dimensional attitude error angle δθ of the UAV dynamics model is UDM =[δγ, δα, δψ], speed error Position error δP UDM =[δL UDM δλ UDM δh UDM], angular velocity error δω UDM =[δω x δω y δω z ] and thrust coefficient error Δk F As the state quantity, it is expanded to the inertial navigation state parameter to form the system state quantity x = [x INS x UDM ] T . Where x UDM =[δθ UDM δV UDM δP UDM δω UDM Δk F ], φ INS =[φ x φ y φ z ]、 δP INS =[δL INS δλ INS δh INS ]、ε=[ε x ε y ε z ]and The state equation of the filter is established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation. Where f(x) represents the state equation related to x.

[0008] Step (2): Calculate the heading angle ψ of the drone based on the polarization sensor pol , and ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, and ψ pol As a measurement to establish the heading angle error δψ and thrust coefficient error Δk of the dynamic model F The related measurement equation Z2=H2x, the attitude angle θ of the UAV dynamics model UDM =[γαψ] T ,speed and position P UDM =[L UDM λ UDM h UDM ] T As a measurement, the measurement equation Z3=H3x related to the three-dimensional attitude error angle, velocity error, position error of the UAV dynamics model and the three-dimensional attitude misalignment angle, velocity error, and position error of the inertial navigation is established. Finally, the measurement equation Z=[Z1 Z2 Z3] of the system is established. T =Hx, where H represents the system measurement matrix.

[0009] Step (3): Based on the system state equation and measurement equation of step (1) and step (2), calculate the heading angle update Δψ of the polarization sensor. pol The reliability discriminant function P(Δψpol) of polarization heading vector measurement is designed using the heading angle information. pol ) When the set threshold condition is met, the system measurement equation Z = [Z1 Z2 Z3] is established according to step (2) T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. When P(Δψ pol ) does not meet the threshold condition, Z3=H3x is used as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback correction of UAV dynamics model.

[0010] Step (4), combined with step (1), step (2) and step (3), according to the characteristics of the system state equation and the measurement equation, the Kalman filter method is used to estimate the system state quantity, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model. At the same time, by feedback compensating the thrust coefficient error, the accuracy of the UAV dynamics model is improved, and the integrated navigation performance of the UAV is further improved.

[0011] Furthermore, in step (1), the three-dimensional attitude error angle δθ of the UAV dynamics model is UDM =[δγ, δα, δψ], speed error Position error δP UDM =[δL UDM δλ UDM δh UDM ], angular velocity error δω UDM =[δω x δω y δω z ] and thrust coefficient error Δk F As the state quantity, it is expanded to the inertial navigation state parameter to form the system state quantity x = [x INS x UDM ] T . Where x UDM =[δθ UDM δV UDM δP UDM δω UDM Δk F ], φ INS =[φ x φ y φz ]、 δP INS =[δL INS δλ INS δh INS ]、ε=[ε x ε y ε z ]and The state equation of the filter is established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation. Where f(x) represents the state equation related to x, the specific process is as follows:

[0012] The error state of the inertial navigation can be expressed as

[0013]

[0014] where φ x ,φ y and φ z is the 3D attitude misalignment angle of the inertial navigation, and is the east, north and celestial velocity error of the inertial navigation in the navigation coordinate system, δL INS ,δλ INS and δh INS are latitude error, longitude error and altitude error respectively, ε x , ε y and ε z is the gyroscope three-axis drift, and is the accelerometer three-axis drift.

[0015] The error state of the UAV dynamics model can be expressed as

[0016]

[0017] Among them, δγ, δα and δψ are the three-dimensional attitude error angles of the UAV dynamics model, and is the east, north and celestial velocity error of the UAV dynamics model in the navigation coordinate system, δL UDM ,δλ UDM and δh UDM are latitude error, longitude error and altitude error, Δk F is the model thrust coefficient error.

[0018] The system state equation is:

[0019]

[0020] Where x = [xINS x UDM ] T , F is the system matrix, and w is the system noise.

[0021] In step (2), the heading angle ψ of the drone is obtained by calculating based on the polarization sensor. pol , and ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, and ψ pol As a measurement to establish the heading angle error δψ and thrust coefficient error Δk of the dynamic model F The related measurement equation Z2=H2x, the attitude angle θ of the UAV dynamics model UDM =[γαψ] T ,speed and position P UDM =[L UDM λ UDM h UDM ] T As a measurement, the measurement equation Z3=H3x related to the three-dimensional attitude error angle, velocity error, position error of the UAV dynamics model and the three-dimensional attitude misalignment angle, velocity error, and position error of the inertial navigation is established. Finally, the measurement equation Z=[Z1 Z2 Z3] of the system is established. T =Hx, where H represents the system measurement matrix. The specific process is as follows:

[0022] Define the polarization vector under the carrier system as p b , the solar vector in the geographic system is s n Based on the vertical relationship between the polarization vector and the sun vector and combined with the horizontal attitude information, the heading angle ψ calculated by the polarization sensor can be obtained. pol . pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, where

[0023]

[0024] ψ pol As a measurement to establish the heading angle error δψ of the dynamic model UDM , thrust coefficient error Δk F The relevant measurement equation is Z2=H2x, where

[0025]

[0026] The position P obtained by the UAV dynamics model UDM =[L UDM λ UDM h UDM ]T 、Posture Θ UDM =[γαψ] T and speed As a measurement, the three-dimensional attitude error angle δΘ with the UAV dynamics model is established UDM , speed error δV UDM 、Position error δP UDM and the inertial navigation three-dimensional attitude misalignment angle φ INS , speed error δV INS 、Position error δP INS The relevant measurement equation is Z3=H3x, where

[0027]

[0028] Finally, the measurement equation of the system is established: Z = [Z1 Z2 Z3] T =h(x), where

[0029]

[0030] ν, ν1, ν2 and ν3 represent measurement noise.

[0031] In step (3), based on the system state equation and measurement equation of step (1) and step (2), the heading angle update Δψ of the polarization sensor is calculated. pol Using the heading angle information to design the credibility discriminant function P(Δψ pol ). When P(Δψ pol ) When the set threshold condition is met, the system measurement equation Z = [Z1 Z2 Z3] is established according to step (2) T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. When P(Δψ pol ) does not meet the threshold condition, Z3=H3x is used as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback correction dynamics model, the specific process is as follows:

[0032] Based on the system state equation in step (1), the predicted UAV heading angle at the kth moment is obtained: The difference between the predicted UAV heading angle and the heading angle measured by the polarization sensor is the heading angle update of the polarization sensor.

[0033]

[0034] where ψ pol (k) is the heading angle measurement value of the polarization sensor at the kth moment.

[0035] On this basis, a credibility discrimination function based on the heading information of the polarization sensor is established.

[0036]

[0037] where |Δψ pol (k)| represents Δψ pol The absolute value of (k), N th represents the threshold. pol When (k)| is less than the threshold, the system measurement equation Z=[Z1 Z2 Z3] is established according to step (2). T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. When |Δψ pol When (k)| is greater than the threshold, ψ pol (k) is discarded, Z3=H3x is taken as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback-corrected kinetic model.

[0038] In step (4), step (1), step (2) and step (3) are combined, and according to the characteristics of the system state equation and the measurement equation, the Kalman filter method is used to estimate the system state quantity, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model. At the same time, the thrust coefficient error is compensated by feedback to improve the accuracy of the UAV dynamics model and further improve the integrated navigation performance of the UAV. The specific process is as follows:

[0039] Combining formulas (3) to (9), the polarization / inertial navigation integrated navigation equation of the embedded UAV dynamics model is:

[0040]

[0041]

[0042] Use Kalman filtering to implement x INS and x UDM At the same time, the inference coefficient error Δk is compensated by feedback F , improve the accuracy of the UAV dynamics model, and further enhance the combined navigation performance of UAVs in environments such as satellite denial and polarization occlusion.

[0043] Compared with the prior art, the advantages of the present invention are as follows: for the first time, a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model is proposed, a polarization / dynamics / inertial navigation combined navigation equation considering the UAV thrust coefficient error is established, online estimation and correction of the UAV thrust coefficient error is achieved, the accuracy of the UAV dynamics model is improved, and the combined navigation performance of polarization / dynamics / inertial navigation is enhanced in environments such as satellite denial and polarization occlusion. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 The present invention is a schematic flow chart of a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model. DETAILED DESCRIPTION

[0045] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the protection scope of the present invention.

[0046] The present invention provides a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model. The three-dimensional attitude error angle, velocity error, position error, angular velocity error and thrust coefficient error of the UAV dynamics model are used as state quantities to expand the dimension into the inertial navigation state parameters to form the system state quantity, and establish the system state equation. The heading angle solved by the polarization sensor, the attitude, velocity and position of the UAV dynamics model are used as the measurement quantity to establish the system measurement equation. Based on the heading angle update information, a credibility discriminant function of the polarization heading vector measurement is established, and a combined navigation mode switching and thrust coefficient error feedback strategy are designed. The Kalman filtering method is used to estimate the system state quantity, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model, so as to correct the attitude and position of the UAV.

[0047] The specific implementation steps of the present invention are as follows:

[0048] Step 1: The three-dimensional attitude error angle δθ of the UAV dynamics model UDM =[δγ, δα, δψ], speed error Position error δP UDM =[δL UDM δλ UDM δh UDM ], angular velocity error δω UDM =[δω x δω y δω z ] and thrust coefficient error ΔkF As the state quantity, it is expanded to the inertial navigation state parameter to form the system state quantity x = [x INS x UDM ] T . Where x UDM =[δθ UDM δV UDM δP UDM δω UDM Δk F ], φ INS =[φ x φ y φ z ]、 δP INS =[δL INS δλ INS δh INS ]、ε=[ε x ε y ε z ]and The state equation of the filter is established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation. Where f(x) represents the state equation related to x, the specific process is as follows:

[0049] The error state of the inertial navigation can be expressed as

[0050]

[0051] where φ x ,φ y and φ z is the 3D attitude misalignment angle of the inertial navigation, and is the east, north and celestial velocity error of the inertial navigation in the navigation coordinate system, δL INS ,δλ INS and δh INS are latitude error, longitude error and altitude error respectively, ε x , ε y and ε z is the gyroscope three-axis drift, and is the accelerometer three-axis drift.

[0052] The error state of the UAV dynamics model can be expressed as

[0053]

[0054] Among them, δγ, δα and δψ are the three-dimensional attitude error angles of the UAV dynamics model, and is the east, north and celestial velocity error of the UAV dynamics model in the navigation coordinate system, δL UDM ,δλ UDM and δh UDM are latitude error, longitude error and altitude error, Δk F is the model thrust coefficient error.

[0055] The system state equation is:

[0056]

[0057] Where x = [x INS x UDM ] T , F is the system matrix, and w is the system noise.

[0058] Step 2: Obtain the heading angle ψ of the drone based on the polarization sensor pol , and ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, and ψ pol As a measurement to establish the heading angle error δψ and thrust coefficient error Δk of the dynamic model F The related measurement equation Z2=H2x, the attitude angle θ of the UAV dynamics model UDM =[γαψ] T ,speed and position P UDM =[L UDM λ UDM h UDM ] T As a measurement, the measurement equation Z3=H3x related to the three-dimensional attitude error angle, velocity error, position error of the UAV dynamics model and the three-dimensional attitude misalignment angle, velocity error, and position error of the inertial navigation is established. Finally, the measurement equation Z=[Z1 Z2 Z3] of the system is established. T =Hx, where H represents the system measurement matrix. The specific process is as follows:

[0059] Define the polarization vector under the carrier system as p b , the solar vector in the geographic system is s n Based on the vertical relationship between the polarization vector and the sun vector and combined with the horizontal attitude information, the heading angle ψ calculated by the polarization sensor can be obtained. pol . pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, where

[0060]

[0061] ψpol As a measurement to establish the heading angle error δψ of the dynamic model UDM , thrust coefficient error Δk F The relevant measurement equation is Z2=H2x, where

[0062]

[0063] The position P obtained by the UAV dynamics model UDM =[L UDM λ UDM h UDM ] T 、Posture Θ UDM =[γαψ] T and speed As a measurement, the three-dimensional attitude error angle δΘ with the UAV dynamics model is established UDM , speed error δV UDM 、Position error δP UDM and the inertial navigation three-dimensional attitude misalignment angle φ INS , speed error δV INS 、Position error δP INS The relevant measurement equation is Z3=H3x, where

[0064]

[0065] Finally, the measurement equation of the system is established: Z = [Z1 Z2 Z3] T =h(x), where

[0066]

[0067] ν, ν1, ν2 and ν3 represent measurement noise.

[0068] Step 3: Based on the system state equation and measurement equation of step (1) and step (2), calculate the heading angle innovation Δψpol of the polarization sensor. Use the heading angle innovation to design the credibility judgment function P(Δψpol) of polarization heading vector measurement. pol ). When P(Δψ pol ) When the set threshold condition is met, the system measurement equation Z = [Z1 Z2 Z3] is established according to step (2) T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. When P(Δψ pol ) does not meet the threshold condition, Z3=H3x is used as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback correction of the UAV dynamics model, the specific process is as follows:

[0069] Based on the system state equation in step (1), the predicted UAV heading angle at the kth moment is obtained: The difference between the predicted UAV heading angle and the heading angle measured by the polarization sensor is the heading angle update of the polarization sensor.

[0070]

[0071] where ψ pol (k) is the heading angle measurement value of the polarization sensor at the kth moment.

[0072] On this basis, a credibility discrimination function based on the heading information of the polarization sensor is established.

[0073]

[0074] where |Δψ pol (k)| represents Δψ pol The absolute value of (k), N th represents the threshold. pol When (k)| is less than the threshold, the system measurement equation Z=[Z1 Z2 Z3] is established according to step (2). T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. When |Δψ pol When (k)| is greater than the threshold, ψ pol (k) is discarded, Z3=H3x is used as the measurement equation of the system, and the Δk estimated at the last moment when P(Δψpol) satisfies the threshold condition is F Feedback-corrected kinetic model.

[0075] Step 4: Combine steps (1), (2) and (3), and use the Kalman filter method to estimate the system state quantities, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model according to the characteristics of the system state equation and the measurement equation. At the same time, the thrust coefficient error is compensated by feedback to improve the accuracy of the UAV dynamics model and further improve the integrated navigation performance of the UAV. The specific process is as follows:

[0076] Combining formulas (3) to (9), the polarization / inertial navigation integrated navigation equation of the embedded UAV dynamics model is:

[0077]

[0078] Use Kalman filtering to implement x INS and x UDM At the same time, the inference coefficient error Δk is compensated by feedback F, improve the accuracy of the UAV dynamics model, and further enhance the combined navigation performance of UAVs in environments such as satellite denial and polarization occlusion.

[0079] Although the above describes the illustrative specific embodiments of the present invention to facilitate those skilled in the art to understand the present invention, and it should be clear that the present invention is not limited to the scope of the specific embodiments, for those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations using the concept of the present invention are protected.

Claims

1. A polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model, characterized in that: The implementation steps are as follows: (1) The three-dimensional attitude error angle δθ of the UAV dynamics model UDM =[δγ, δα, δψ], speed error Position error δP UDM =[δL UDM δλ UDM δh UDM ], angular velocity error δω UDM =[δω x δω y δω z ] and thrust coefficient error Δk F As the state quantity, it is expanded to the inertial navigation state parameter to form the system state quantity x = [x INS x UDM ] T ; where x UDM =[δθ UDM δV UDM δP UDM δω UDM Δk F ]; φ INS =[φ x φ y φ z ]、 δP INS =[δL INS δλ INS δh INS ]、ε=[ε x ε y ε z ]、 The state equation of the filter is established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation. Where f(x) represents the state equation related to x; (2) The heading angle ψ of the UAV is calculated based on the polarization sensor pol , ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, and ψ pol As a measurement to establish the heading angle error δψ and thrust coefficient error Δk of the dynamic model F The related measurement equation Z2=H2x, the attitude angle θ of the UAV dynamics model UDM =[γ α ψ] T ,speed and position P UDM =[L UDM λ UDM h UDM ] T As a measurement, the measurement equation Z3=H3x related to the three-dimensional attitude error angle, velocity error, position error of the UAV dynamics model and the three-dimensional attitude misalignment angle, velocity error, and position error of the inertial navigation is established; finally, the system measurement equation Z=[Z1 Z2 Z3] is established T =Hx, where H represents the system measurement matrix; (3) Based on the system state equation and measurement equation in steps (1) and (2), calculate the heading angle update Δψ of the polarization sensor pol ; Design of credibility discriminant function P(Δψ) of polarization heading vector measurement using heading angle innovation pol ); when P(Δψ pol ) When the set threshold condition is met, the system measurement equation Z = [Z1 Z2 Z3] is established according to step (2) T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model; when P(Δψ pol ) does not meet the threshold condition, Z3=H3x is used as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback correction of UAV dynamics model; (4) Combining steps (1), (2) and (3), according to the characteristics of the system state equation and the measurement equation, the Kalman filter method is used to estimate the system state quantities, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model; at the same time, the thrust coefficient error is compensated by feedback to improve the accuracy of the UAV dynamics model and further enhance the combined navigation performance of the UAV.

2. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1 is characterized in that: In the step (1), the three-dimensional attitude error angle δθ of the UAV dynamics model is UDM =[δγ, δα, δψ], speed error Position error δP UDM =[δL UDM δλ UDM δh UDM ], angular velocity error δω UDM =[δω x δω y δω z ] and thrust coefficient error Δk F As the state quantity, it is expanded to the inertial navigation state parameter to form the system state quantity x = [x INS x UDM ] T ; where x UDM =[δθ UDM δV UDM δP UDM δω UDM Δk F ], φ INS =[φ x φ y φ z ]、 δP INS =[δL INS δλ INS δh INS ]、ε=[ε x ε y ε z ]and The state equation of the filter is established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation. Where f(x) represents the state equation related to x, the specific process is as follows: The error state of the inertial navigation can be expressed as where φ x ,φ y and φ z is the 3D attitude misalignment angle of the inertial navigation, and is the east, north and celestial velocity error of the inertial navigation in the navigation coordinate system, δL INS ,δλ INS and δh INS are latitude error, longitude error and altitude error respectively, ε x , ε y and ε z is the gyroscope three-axis drift, and is the accelerometer three-axis drift; The error state of the UAV dynamics model can be expressed as Among them, δγ, δα and δψ are the three-dimensional attitude error angles of the UAV dynamics model, and is the east, north and celestial velocity error of the UAV dynamics model in the navigation coordinate system, δL UDM ,δλ UDM and δh UDM are latitude error, longitude error and altitude error, Δk F is the model thrust coefficient error; The system state equation is: Where x = [x INS x UDM ] T , F is the system matrix, and w is the system noise.

3. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1 is characterized in that: In step (2), the heading angle ψ of the drone is obtained by calculating based on the polarization sensor. pol , ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, and ψ pol As a measurement to establish the heading angle error δψ and thrust coefficient error Δk of the dynamic model F The related measurement equation Z2=H2x, the attitude angle θ of the UAV dynamics model UDM =[γ αψ] T ,speed and position P UDM =[L UDM λ UDM h UDM ] T As a measurement, the measurement equation Z3=H3x related to the three-dimensional attitude error angle, velocity error, position error of the UAV dynamics model and the three-dimensional attitude misalignment angle, velocity error, and position error of the inertial navigation is established; finally, the system measurement equation Z=[Z1 Z2 Z3] is established T =Hx, where H represents the system measurement matrix. The specific process is as follows: Define the polarization vector under the carrier system as p b , the solar vector in the geographic system is s n Based on the vertical relationship between the polarization vector and the sun vector and the horizontal attitude information, the heading angle ψ calculated by the polarization sensor is obtained. pol ; pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation is Z1=H1x, where ψ pol As a measurement, the heading angle error δψ is established with the dynamic model UDM , thrust coefficient error Δk F The relevant measurement equation is Z2=H2x, where The position P of the UAV dynamics model UDM =[L UDM λ UDM h UDM ] T 、Posture Θ UDM =[γ α ψ] T and speed As a measurement, the three-dimensional attitude error angle δΘ with the UAV dynamics model is established UDM , speed error δV UDM 、Position error δP UDM and the inertial navigation three-dimensional attitude misalignment angle φ INS , speed error δV INS 、Position error δP INS The relevant measurement equation is Z3=H3x, where Finally, the measurement equation of the system is established: Z = [Z1 Z2 Z3] T =Hx, where ν, ν1, ν2 and ν3 represent measurement noise.

4. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1 is characterized in that: In step (3), based on the system state equation and measurement equation of step (1) and step (2), the heading angle update Δψ of the polarization sensor is calculated. pol ; Design of credibility discriminant function P(Δψ) of polarization heading vector measurement using heading angle innovation pol ); when P(Δψ pol ) When the set threshold condition is met, the system measurement equation Z = [Z1 Z2 Z3] is established according to step (2) T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model; when P(Δψ pol ) does not meet the threshold condition, Z3=H3x is used as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback correction dynamics model, the specific process is as follows: Based on the system state equation in step (1), the predicted UAV heading angle at the kth moment is obtained: The difference between the predicted UAV heading angle and the heading angle measured by the polarization sensor is the heading angle update of the polarization sensor. where ψ pol (k) is the heading angle measurement value of the polarization sensor at the kth moment; On this basis, a credibility discrimination function based on the heading information of the polarization sensor is established. where |Δψ pol (k)| represents Δψ pol The absolute value of (k), N th represents the threshold; when |Δψ pol When (k)| is less than the threshold, the system measurement equation Z=[Z1 Z2 Z3] is established according to step (2). T =Hx, and real-time feedback of thrust coefficient error Δk F Modified UAV dynamics model; when |Δψ pol When (k)| is greater than the threshold, ψ pol (k) is discarded, Z3=H3x is taken as the measurement equation of the system, and P(Δψ pol ) The estimated Δk at the last moment when the threshold condition is met F Feedback-corrected kinetic model.

5. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1 is characterized in that: In the step (4), the steps (1), (2) and (3) are combined, and according to the characteristics of the system state equation and the measurement equation, the Kalman filter method is used to estimate the system state quantity, i.e., the inertial navigation attitude misalignment angle, velocity error, position error and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model; at the same time, the thrust coefficient error is compensated by feedback to improve the accuracy of the UAV dynamics model and further improve the integrated navigation performance of the UAV. The specific process is as follows: Combining formulas (3) to (9), the polarization / inertial navigation integrated navigation equation of the embedded UAV dynamics model is: Use Kalman filtering to implement x INS and x UDM At the same time, the thrust coefficient error Δk is compensated by feedback F , improve the accuracy of the UAV dynamics model, and further enhance the combined navigation performance of UAVs in environments such as satellite denial and polarization occlusion.

Images