A polarization / inertial navigation integrated navigation method with embedded UAV dynamics model

Abstract

The invention relates to a polarization / inertial navigation combined navigation method with an embedded unmanned aerial vehicle (UAV) dynamics model. The method comprises the following steps: taking 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 and expanding them into inertial navigation state parameters to form system state quantities, and establishing a system state equation; taking the heading angle solved by a polarization sensor and the attitude, velocity and position of the UAV dynamics model as quantity measurements to establish a system measurement equation; establishing a credibility discriminant function of the polarization heading vector measurement based on heading angle innovation information, and designing a combined navigation mode switching and thrust coefficient error feedback strategy; and using a Kalman filtering method to estimate the system state quantities, namely the inertial navigation attitude misalignment angle, velocity error and position error, and the attitude error angle, velocity error, position error and thrust coefficient error of the UAV dynamics model, to achieve correction of the UAV attitude and position.

Description

Technical Field

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

[0002] With the development of drone technology, it has demonstrated an irreplaceable role in all areas of human life and has become a vital piece of equipment in fields such as national defense and the economy. Drone navigation technology is a key core technology for drone systems, providing information such as their position and attitude. Currently, the most commonly used navigation method for drones is still inertial / satellite combined navigation. However, in interference-denied environments, satellite navigation performance degrades, and the accumulated inertial navigation errors cannot be effectively corrected, seriously affecting the drone's operational effectiveness. Therefore, research is urgently needed to improve the accuracy of autonomous drone navigation in satellite signal denial conditions.

[0003] Creatures such as monarch butterflies and sand ants 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 / visual combined navigation device" (application number: CN202123204111.9) uses inertial / polarized light / optical flow / visual combined navigation to solve the problem of poor drone navigation accuracy. 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] To improve the positioning accuracy of drones in conditions where satellite signals are denied, many researchers have conducted extensive research on navigation methods that embed dynamic models. The patent "A Quadrotor Aircraft Dynamic Model / Airborne Sensor Combined Navigation Method" (Application Number: CN201810244190.X) utilizes a satellite / geomagnetic / rotational speed sensor combined navigation method, aided by a dynamic model, to achieve integrated navigation for a quadrotor. The patent "A Combined Navigation and Positioning Method Based on Dynamic Model Assistance" (Application Number: CN202010410189.7) also utilizes a satellite / inertial combined navigation method, aided by a dynamic model, to achieve navigation and positioning for an autonomous underwater vehicle. These methods only use the drone's dynamic model as a measurement aid to correct the inertial navigation system and do not consider correcting the drone's dynamic model. In real-world environments, drone dynamic models contain parameter errors, which affect the accuracy of the dynamic model's recursive inferences of attitude, velocity, and position. Therefore, how to combine polarization navigation with the UAV dynamics model, while correcting the accumulated error of inertial navigation, use polarization sensors 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 inertial navigation errors, 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 existing technology, the present invention proposes a polarization / inertial navigation combined navigation method with an embedded UAV dynamics model, which 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 is designed. Finally, the Kalman filter 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, to achieve the correction of the UAV attitude and position.

[0006] The technical solution of the present invention is: a polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model, which is implemented in the following steps:

[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 into 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 equations of the filter are established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation respectively. Where f(x) represents the state equation related to x.

[0008] Step (2): Calculate the heading angle ψ of the UAV based on the polarization sensor pol , change ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation Z1=H1x, ψ 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): Calculate the heading angle update Δψ of the polarization sensor based on the system state equation and measurement equation of steps (1) and (2). pol The reliability judgment function P(Δψpol) of polarization heading vector measurement is designed using the heading angle information. pol ) When the set threshold conditions are 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 last estimated Δk when the threshold condition is met F Feedback correction of UAV dynamics model.

[0010] Step (4) combines step (1), step (2) and step (3), and uses 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, based on the characteristics of the system state equation and the measurement equation. At the same time, by feedback compensating for 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 into 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 equations of the filter are established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation respectively. Where f(x) represents the state equation related to x, and 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 eastward, northward 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 respectively, Δ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 UAV is obtained by calculating the polarization sensor. pol , change ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation Z1=H1x, ψ 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 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, establish the three-dimensional attitude error angle δΘ with the UAV dynamics model 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), the heading angle update Δψ of the polarization sensor is calculated based on the system state equation and measurement equation of step (1) and step (2). pol Using the heading angle information to design the credibility judgment function P(Δψ pol ). When P(Δψ pol ) When the set threshold conditions are 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 last estimated Δk when the threshold condition is met F Feedback correction dynamic 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 discriminant 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. When |Δψ pol When (k)| is less than the threshold, establish the system measurement equation Z=[Z1 Z2 Z3] according to step (2). T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. 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 last estimated Δk when the threshold condition is met F Feedback-corrected dynamic model.

[0038] In step (4), 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 are estimated by combining step (1), step (2), and step (3) 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 enhance the combined 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 the UAV in environments such as satellite denial and polarization occlusion.

[0043] Compared with the existing technology, the advantages of the present invention are: 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, and the online estimation and correction of the UAV thrust coefficient error is realized, thereby improving the accuracy of the UAV dynamics model and enhancing the combined navigation performance of polarization / dynamics / inertial navigation in environments such as satellite denial and polarization occlusion. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0045] The following will clearly and completely describe the technical solutions in the embodiments of the present invention 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 scope of protection 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 expanded as state quantities into inertial navigation state parameters to form system state quantities, and a system state equation is established. The heading angle solved by the polarization sensor, the attitude, velocity and position of the UAV dynamics model are used as measurements to establish a system measurement equation. Based on the heading angle update, a credibility discriminant function of the polarization heading vector measurement is established, and a combined navigation mode switching and thrust coefficient error feedback strategy is designed. 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, 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 into 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 equations of the filter are established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation respectively. Where f(x) represents the state equation related to x, and 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 eastward, northward 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 respectively, Δ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: Calculate the heading angle ψ of the drone based on the polarization sensor pol , change ψ pol As a measurement to establish the misalignment angle φ with the inertial navigation z The relevant measurement equation Z1=H1x, ψ 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 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, establish the three-dimensional attitude error angle δΘ with the UAV dynamics model 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 the polarization heading vector measurement. pol ). When P(Δψ pol ) When the set threshold conditions are 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 last estimated Δk 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 discriminant 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, establish the system measurement equation Z=[Z1 Z2 Z3] according to step (2). T =Hx, and real-time feedback of thrust coefficient error Δk F Modify the UAV dynamics model. 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) meets the threshold condition is used as F Feedback-corrected dynamic 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, based on the characteristics of the system state equation and the measurement equation. At the same time, by feedback compensating for the thrust coefficient error, the accuracy of the UAV dynamics model is improved, and the integrated navigation performance of the UAV is further improved. 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 the UAV in environments such as satellite denial and polarization occlusion.

[0079] Although the above describes the illustrative specific embodiments of the present invention to facilitate understanding of the present invention by those skilled in the art, and it should be clear that the present invention is not limited to the scope of the specific embodiments, it is obvious to those skilled in the art that as long as various changes are within the spirit and scope of the present invention as defined and determined by the appended claims, these changes are obvious, and all inventions and creations utilizing the concepts 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 , speed error , position error , angular velocity error and thrust coefficient error As a state quantity, it is expanded into the inertial navigation state parameter to form the system state quantity ;in ; , 、 、 、 、 The state equations of the filter are established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation respectively. ,in Represents The relevant equation of state; (2) Obtain the heading angle of the drone based on the polarization sensor ,Will As a measurement to establish the misalignment angle with the inertial navigation Related measurement equations ,Will As a measurement to establish the heading angle error of the dynamic model , thrust coefficient error Related measurement equations , the attitude angle of the UAV dynamics model ,speed and location As a measurement, establish the measurement equation 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, position error of the inertial navigation. ;Finally, establish the measurement equation of the system ,in represents the system measurement matrix; (3) Based on the system state equation and measurement equation in steps (1) and (2), calculate the heading angle information of the polarization sensor ; Designing a Credibility Discriminant Function for Polarization Heading Vector Measurement Using Heading Angle Innovation ;when When the set threshold conditions are met, the measurement equation of the system is established according to step (2) , and real-time feedback of thrust coefficient error Modify the UAV dynamics model; when If the threshold condition is not met, As the measurement equation of the system, The estimated value at the last moment when the threshold condition is met 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, 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; 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, characterized in that: In the step (1), the three-dimensional attitude error angle of the UAV dynamics model is , speed error , position error , angular velocity error and thrust coefficient error As a state quantity, it is expanded into the inertial navigation state parameter to form the system state quantity ;in , , 、 、 、 and The state equations of the filter are established for the three-dimensional attitude misalignment angle, velocity error, position error, gyroscope drift and accelerometer drift of the inertial navigation respectively. ,in Represents The relevant state equation is as follows: The error state of the inertial navigation can be expressed as (1) in , and 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, , and are latitude error, longitude error and altitude error respectively, , and 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 (2) in , and is the three-dimensional attitude error angle of the UAV dynamics model, , and are the east, north and sky velocity errors of the UAV dynamics model in the navigation coordinate system, , and are latitude error, longitude error and altitude error respectively, is the model thrust coefficient error; The system state equation is: (3) in, , is the system matrix, is the system noise.

3. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1 is characterized by: In step (2), the heading angle of the UAV is obtained by calculating the polarization sensor. ,Will As a measurement to establish the misalignment angle with the inertial navigation Related measurement equations ,Will As a measurement to establish the heading angle error of the dynamic model , thrust coefficient error Related measurement equations , the attitude angle of the UAV dynamics model ,speed and location As a measurement, establish the measurement equation 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, position error of the inertial navigation. ;Finally, establish the measurement equation of the system ,in Represents the system measurement matrix. The specific process is as follows: The polarization vector under the carrier system is defined as , the sun vector in the geographic system is Based on the vertical relationship between the polarization vector and the sun vector, combined with the horizontal attitude information, the heading angle calculated by the polarization sensor is obtained. ;Will As a measurement to establish the misalignment angle with the inertial navigation Related measurement equations ,in (4) Will As a measurement, establish the heading angle error with the dynamic model , thrust coefficient error Related measurement equations ,in (5) The position of the UAV dynamics model ,attitude and speed As a measurement, establish the three-dimensional attitude error angle with the UAV dynamics model , speed error , position error and inertial navigation 3D attitude misalignment angle , speed error , position error Related measurement equations ,in (6) Finally, the measurement equation of the system is established ,in (7) 、 、 and Indicates the measurement noise.

4. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1, characterized in that: In step (3), based on the system state equation and measurement equation of step (1) and step (2), the heading angle information of the polarization sensor is calculated. ; Designing a credibility judgment function for polarization heading vector measurement using heading angle innovation ;when When the set threshold conditions are met, the measurement equation of the system is established according to step (2) , and real-time feedback of thrust coefficient error Modify the UAV dynamics model; when If the threshold condition is not met, As the measurement equation of the system, The estimated value at the last moment when the threshold condition is met Feedback correction dynamic model, the specific process is as follows: Based on the system state equation in step (1), we can get Predicted UAV heading angle at all times 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. (8) in For the The heading angle measurement value of the polarization sensor at each moment; On this basis, a credibility discriminant function based on the heading information of the polarization sensor is established. (9) in express The absolute value of Indicates the threshold; when When it is less than the threshold, establish the measurement equation of the system according to step (2) , and real-time feedback of thrust coefficient error Modify the UAV dynamics model; when When it is greater than the threshold, Abandoned, will As the measurement equation of the system, The estimated value at the last moment when the threshold condition is met Feedback-corrected dynamic model.

5. The polarization / inertial navigation integrated navigation method with an embedded UAV dynamics model according to claim 1, characterized in that: 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, so as to improve the accuracy of the UAV dynamics model and further enhance the combined 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: (10) (11) Use Kalman filtering method to achieve and At the same time, the thrust coefficient error is compensated by feedback , improve the accuracy of the UAV dynamics model and further enhance the combined navigation performance of the UAV in satellite denial and polarization occlusion environments.

Images