Unmanned aerial vehicle formation cooperative navigation method based on line-of-sight starlight angle distance auxiliary constraint

Abstract

The application discloses a kind of unmanned plane formation cooperative navigation method based on line-of-sight starlight angle distance auxiliary constraint, each unmanned plane in unmanned plane formation is equipped with inertial navigation equipment, barometer and communication ranging link equipment, wherein a certain unmanned plane in unmanned plane formation is arranged with servo control optical sensor as observation unmanned plane;Observation unmanned plane tracks observation and extracts the mass center coordinates of observed unmanned plane, and then carries out star map matching on the background stars in field of view and identifies and extracts mass center coordinates, calculates the line-of-sight starlight angle distance observation information between observation unmanned plane and observed unmanned plane and background stars in optical sensor coordinate system, to assist in improving the overall navigation positioning accuracy of formation unmanned plane.The application can effectively correct the tendency that navigation positioning error of formation unmanned plane assisted by inter-aircraft communication ranging is rotated around a point in space, thereby further improving the overall autonomous navigation positioning accuracy of formation unmanned plane in GNSS denial environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle autonomous navigation, in particular to a method for cooperative navigation of unmanned aerial vehicle formation based on line-of-sight starlight angle distance auxiliary constraint. BACKGROUND

[0002] In a global navigation satellite system (GNSS) signal interference environment, in a traditional cooperative navigation scheme of unmanned aerial vehicle formation, the unmanned aerial vehicles in the formation network cooperatively measure the distance and direction of each other through the communication link, and the information is interactively utilized to correct the navigation information of the unmanned aerial vehicles in the formation, such as the position, velocity and attitude, so as to improve the overall navigation accuracy of the unmanned aerial vehicle formation. In this scheme, the overall navigation and positioning information of the unmanned aerial vehicle formation is observed and corrected only by the communication distance information between the unmanned aerial vehicles, which can suppress the relative position error of the unmanned aerial vehicle formation and improve the overall absolute positioning accuracy of the unmanned aerial vehicle formation to a certain extent, but still cannot suppress the tendency of the overall absolute positioning error of the unmanned aerial vehicle formation to rotate around a certain point in space and the tendency of the overall absolute positioning error to drift in a certain direction. SUMMARY

[0003] In order to solve the problem that the traditional cooperative autonomous navigation method of unmanned aerial vehicle formation based on inter-aircraft communication distance assistance cannot suppress the tendency of the overall absolute positioning error of the unmanned aerial vehicle formation to rotate around a certain point in space and the tendency of the overall absolute positioning error to drift in a certain direction in a GNSS denial environment, the present application proposes a method for cooperative navigation of unmanned aerial vehicle formation based on line-of-sight starlight angle distance auxiliary constraint. On the basis of the overall navigation and positioning of the unmanned aerial vehicle formation assisted by the inter-aircraft communication distance, the line-of-sight starlight angle distance between any two unmanned aerial vehicles in the formation is introduced as auxiliary observation information for the overall navigation and positioning. The introduction of starlight information can provide an inertial space reference for the unmanned aerial vehicle formation, and this method can effectively eliminate the tendency of the overall absolute positioning error of the unmanned aerial vehicle formation to rotate around a certain point in space, thereby further improving the overall absolute navigation and positioning accuracy of the unmanned aerial vehicle formation.

[0004] A method for cooperative navigation of unmanned aerial vehicle formation based on line-of-sight starlight angle distance auxiliary constraint, each unmanned aerial vehicle in the formation is provided with an inertial navigation device, an air pressure gauge and a communication distance link device, and a certain unmanned aerial vehicle in the formation is provided with a servo-controlled optical sensor as an observation unmanned aerial vehicle; the observation unmanned aerial vehicle tracks and observes the observed unmanned aerial vehicle and extracts the centroid coordinates of the observed unmanned aerial vehicle, matches the star map and identifies and extracts the centroid coordinates of the background stars in the field of view, and calculates the line-of-sight starlight angle distance observation information between the observation unmanned aerial vehicle and the observed unmanned aerial vehicle and the background stars in the optical sensor coordinate system, which is used to assist in improving the overall navigation and positioning accuracy of the unmanned aerial vehicle formation.

[0005] The unmanned aerial vehicle formation cooperative navigation method based on the line-of-sight starlight angle distance auxiliary constraint takes the navigation parameters of each unmanned aerial vehicle in the formation, including position, speed and attitude, as the state to be estimated, takes the navigation parameter strapdown solution process based on the measurement data of the inertial navigation equipment of each unmanned aerial vehicle as the state estimation process of the KF algorithm, takes the communication ranging information of any two unmanned aerial vehicles in the formation as the first group of observation information of the KF, and takes the line-of-sight starlight angle distance observation information as the second group of observation information of the KF, so as to complete the overall navigation positioning solution of the formation unmanned aerial vehicles through the KF algorithm.

[0006] The unmanned aerial vehicle formation cooperative navigation method based on the line-of-sight starlight angle distance auxiliary constraint has the following steps:

[0007] Step 1: The on-board inertial navigation module of each unmanned aerial vehicle in the formation completes the integral recursion of the position, speed and attitude navigation parameters of each unmanned aerial vehicle based on the gyro measurement data and the accelerometer measurement data, the height data measured by the on-board barometer is used to correct the error of the position of the unmanned aerial vehicle in the height direction, and the navigation parameter estimation data is sent to the center host. The unmanned aerial vehicle responsible for the fusion and update of the navigation parameters of all unmanned aerial vehicles in the formation is defined as the center unmanned aerial vehicle.

[0008] Step 2: Each unmanned aerial vehicle in the formation completes communication ranging through the data link, sends the inter-aircraft ranging information to the center host, and the center host completes the overall navigation positioning error correction of the formation unmanned aerial vehicles based on the inter-aircraft ranging observation information.

[0009] Step 3: The high-precision servo-controlled optical sensor arranged on the observation unmanned aerial vehicle tracks and observes a certain unmanned aerial vehicle in the formation and extracts the centroid coordinates of the unmanned aerial vehicle. The optical sensor matches the star map in the field of view and identifies and extracts the centroid coordinates of the background stars. The line-of-sight starlight angle distance observation information between the observation unmanned aerial vehicle and the observed unmanned aerial vehicle and the background stars is calculated in the optical sensor coordinate system and sent to the center host to complete the overall navigation positioning error correction of the formation unmanned aerial vehicles based on the line-of-sight starlight angle distance observation information constraint.

[0010] The line-of-sight starlight angle distance algorithm model of any unmanned aerial vehicle observing another unmanned aerial vehicle in the formation is as follows:

[0011]

[0012] Wherein, α ij is the line-of-sight starlight angle distance obtained by the on-board optical sensor of the i unmanned aerial vehicle in the formation observing the j unmanned aerial vehicle and the background stars in the field of view, x i ,y i ,z i is the position coordinates of the i unmanned aerial vehicle, x j ,y j ,z jThe j unmanned aerial vehicle position coordinates.

[0013] The beneficial effects and advantages of the present application: the GNSS denial environment unmanned aerial vehicle formation cooperative autonomous navigation method based on the line-of-sight starlight angular distance auxiliary constraint can effectively eliminate the rotation divergence trend of the overall absolute positioning error of the formation unmanned aerial vehicle around a certain point in space, thereby further improving the overall absolute navigation positioning accuracy of the formation unmanned aerial vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 The line-of-sight starlight angular distance schematic diagram.

[0015] Figure 2 The formation unmanned aerial vehicle cooperative autonomous navigation schematic diagram based on the line-of-sight starlight angular distance auxiliary constraint. DETAILED DESCRIPTION

[0016] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application will be further described in detail below according to each figure combined with specific embodiments.

[0017] A line-of-sight starlight angular distance auxiliary constraint based unmanned aerial vehicle formation cooperative navigation method, the related hardware components include the inertial navigation equipment, the barometer, the communication ranging link equipment of each unmanned aerial vehicle in the formation, and the high-precision servo control optical sensor arranged on a certain unmanned aerial vehicle in the formation. The high-precision servo control optical sensor arranged on a certain unmanned aerial vehicle in the formation tracks and observes another unmanned aerial vehicle in the formation and extracts the mass center coordinates of the unmanned aerial vehicle, and then matches the star map of the background stars in the field of view and identifies and extracts the mass center coordinates, so as to calculate the line-of-sight starlight angular distance observation information between the observed unmanned aerial vehicle and the observed unmanned aerial vehicle and the background stars in the optical sensor coordinate system, as shown in the figure, for assisting in improving the overall navigation positioning accuracy of the formation unmanned aerial vehicle. Figure 1

[0018] The basic principle involved in the method is that in the Kalman filter algorithm (KF) framework, the error of the navigation state (including position, velocity, and attitude) of each unmanned aerial vehicle in the formation is taken as the state to be estimated, the error state variance of the strapdown solution process of the inertial navigation equipment carried by each unmanned aerial vehicle is taken as the state estimation process of the KF algorithm, the difference between the communication ranging measurement information and the inertial navigation estimated ranging information of any two unmanned aerial vehicles in the formation is taken as the first group of observation information of KF, and the difference between the line-of-sight starlight angular distance observation information and the inertial navigation estimated line-of-sight starlight angular distance information is taken as the second group of observation information of KF. The overall navigation positioning of the formation unmanned aerial vehicle is completed through the KF algorithm, and the principle block diagram involved in the method is shown in the figure. Figure 2 The algorithm involves the following formulas:

[0019] 1) The strapdown solution based on the data of the inertial navigation equipment carried by the formation unmanned aerial vehicle and the KF state equation are as follows:

[0020] ​

[0021]

[0022]

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029]

[0030]

[0031] where φ Ei ,φ Ni ,φ Ui is the three-axis attitude angle error of the ith UAV, δv Ei ,δv Ni ,δv Ui is the three-axis velocity error of the ith UAV, δL i ,δλ i ,δh i is the position error of the ith UAV, ε xi ,ε yi ,ε zi is the three-axis gyro zero bias of the ith UAV, ε Ei ,ε Ni ,ε Ui is the three-axis gyro zero bias of the ith UAV in the navigation coordinate system, ω is the three-axis accelerometer zero bias of the ith UAV, is the three-axis accelerometer zero bias of the ith UAV in the navigation coordinate system, ω Ni ,ω Ui is the north and sky components of the earth rotation rate of the ith UAV, f Ei ,f Ni ,f Ui is the accelerometer data of the ith UAV in the navigation coordinate system, R Mhi ,R Nhi is the local radius of curvature of the ith UAV containing height information.

[0032] 2) KF observation equation based on inter-vehicle ranging data of formation UAVs is as follows:

[0033] Suppose inter-vehicle ranging measurement value of i UAV and j UAV in the formation is:

[0034]

[0035] Inter-vehicle ranging based on inertial navigation estimation of i UAV and j UAV is:

[0036]

[0037] Then, the inter-vehicle ranging error observation equation is:

[0038]

[0039] Wherein,

[0040]

[0041]

[0042]

[0043]

[0044] Wherein, x Ii ,y Ii ,z Ii is the position estimation value of the i UAV based on inertial navigation recursion, r Iij In numerically, ρ Iij , D ij is the conversion matrix between the earth coordinate system and the latitude and longitude coordinate.

[0045] 3) KF observation equation based on line-of-sight starlight angle distance data of formation UAVs is as follows:

[0046] Suppose i UAV in the formation observes j UAV and background stars in the field of view through the on-board optical sensor, then the corresponding line-of-sight starlight angle distance measurement value is:

[0047]

[0048] Line-of-sight / starlight angle distance based on inertial navigation estimation of i UAV and j UAV is:

[0049]

[0050] Then, the line-of-sight starlight angle distance error observation equation is:

[0051]

[0052] wherein,

[0053]

[0054]

[0055]

[0056]

[0057]

[0058] wherein, ap i = s x ·X Bi + s y ·Y Bi + s z ·Z Bi , X Bi = x i -x j , Y Bi = y i -y j , Z Bi = z i -z j , s x , s y , s z is a star vector.

[0059] 4) The KF algorithm process is as follows:

[0060] In the multi-UAV formation cooperative autonomous navigation scheme, a distributed sequential Kalman filter is used as the fusion filtering algorithm framework, and the specific process is as follows by taking the single sequential filtering of the ith UAV and the jth UAV as an example:

[0061] First, the time update of the navigation state parameters is completed based on the respective inertial measurement unit data:

[0062] The time update process of the ith UAV is as follows:

[0063] X i,k+1 / k = Φ i,k+1 / k ·X i,k

[0064]

[0065] The time update process of the jth UAV is as follows:

[0066] X j,k+1 / k = Φ j,k+1 / k ·X j,k

[0067]

[0068] The sequential measurement update process corresponding to the observation associated with the i-th unmanned aerial vehicle and the j-th unmanned aerial vehicle is as follows:

[0069]

[0070] X ij,k+1 =X ij,k+1 / k +K ij,k+1 ·(Z ij,k+1 -H ij,k+1 ·X ij,k+1 / k )

[0071] P ij,k+1 =(I-K ij,k+1 ·H ij,k+1 )·P ij,k+1 / k

[0072] wherein,

[0073] X ij,k+1 / k =[X i,k+1 / k ,X j,k+1 / k ] T

[0074]

[0075] Φ i,k+1 / k ,Φ j,k+1 / k are state update system matrices of the i-th unmanned aerial vehicle and the j-th unmanned aerial vehicle, Γ i,k , Γ j,k are noise driving matrices, Q i,k , Q j,k are noise matrices, Z ij,k+1 are observations associated with the i-th unmanned aerial vehicle and the j-th unmanned aerial vehicle, H ij,k+1 is a measurement matrix associated with the observation of the i-th unmanned aerial vehicle and the j-th unmanned aerial vehicle, and R ij,k+1 is a measurement noise matrix.

[0076] The method specifically comprises the following steps:

[0077] Step 1, each unmanned aerial vehicle in the formation performs self-navigation parameter estimation based on an airborne inertial navigation device and a barometer, and sends the navigation parameter estimation data to the central host. The specific process is as follows: each unmanned aerial vehicle in the formation completes integral recursion of position, velocity, attitude and other navigation parameters of each unmanned aerial vehicle based on gyro measurement data and accelerometer measurement data through the airborne inertial navigation module, and height data measured by the airborne barometer is used to correct the error of the unmanned aerial vehicle position in the height direction.

[0078] Step 2, each unmanned aerial vehicle in the formation completes communication ranging through a data link, sends inter-vehicle ranging information to the central host, and the central host completes overall navigation and positioning error correction of the formation unmanned aerial vehicles based on inter-vehicle ranging observation information.

[0079] Step 3, a high-precision servo control optical sensor arranged in the host or any deputy machine performs tracking observation on a certain unmanned aerial vehicle in the formation and extracts the mass center coordinates of the unmanned aerial vehicle, performs star map matching on background stars in a field of view and identifies and extracts the mass center coordinates of the background stars, calculates line-of-sight starlight angular distance observation information between the observed unmanned aerial vehicle and the observed unmanned aerial vehicle and the background stars in the optical sensor coordinate system, and sends the information to the central host to complete overall navigation and positioning error correction of the formation unmanned aerial vehicles based on line-of-sight starlight angular distance observation information constraints.

[0080] The GNSS denial environment unmanned aerial vehicle formation cooperative autonomous navigation method based on line-of-sight starlight angular distance auxiliary constraints can effectively eliminate the rotation divergence trend of the overall absolute positioning error of the formation unmanned aerial vehicles around a certain point in space, thereby further improving the overall absolute navigation and positioning accuracy of the formation unmanned aerial vehicles.

[0081] The above-mentioned embodiments only express several embodiments of the present application, and the description is more specific and detailed, but it cannot be understood as a limitation on the scope of the application. It should be noted that for ordinary skilled persons in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which belong to the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the appended claims.

Claims

1. A method for cooperative navigation of UAV formation based on line-of-sight starlight angular distance auxiliary constraint, characterized in that: Each unmanned aerial vehicle in the unmanned aerial vehicle formation is provided with an inertial navigation device, a barometer and a communication ranging link device, wherein a certain unmanned aerial vehicle in the unmanned aerial vehicle formation is provided with a servo-controlled optical sensor as an observation unmanned aerial vehicle; the observation unmanned aerial vehicle tracks and observes a tracked unmanned aerial vehicle, extracts the center of mass coordinates of the tracked unmanned aerial vehicle, performs star map matching on background stars in a field of view, identifies and extracts the center of mass coordinates, and calculates line-of-sight starlight angular distance observation information between the observation unmanned aerial vehicle and the tracked unmanned aerial vehicle and the background stars in an optical sensor coordinate system, to assist in improving the overall navigation and positioning accuracy of the unmanned aerial vehicle formation; The navigation parameters of each unmanned aerial vehicle in the formation, including position, velocity and attitude, are taken as the state to be estimated, the navigation parameter strapdown solution process based on the measurement data of the inertial navigation device of each unmanned aerial vehicle is taken as the state estimation process of the KF algorithm, the communication ranging information of any two unmanned aerial vehicles in the formation is taken as the first set of observation information of the KF algorithm, and the line-of-sight starlight angular distance observation information is taken as the second set of observation information of the KF algorithm, so as to complete the overall navigation and positioning solution of the unmanned aerial vehicle formation by the KF algorithm; The steps are as follows: Step 1: The on-board inertial navigation module of each unmanned aerial vehicle in the formation completes the integral recursion of the position, velocity and attitude navigation parameters of each unmanned aerial vehicle based on the gyro measurement data and the accelerometer measurement data, the height data measured by the on-board barometer is used to correct the error of the position of the unmanned aerial vehicle in the height direction, and the navigation parameter estimation data is sent to the central host computer; the unmanned aerial vehicle responsible for the fusion and update of the navigation parameters of all unmanned aerial vehicles in the formation is defined as the central unmanned aerial vehicle; Step 2: Each unmanned aerial vehicle in the formation completes communication ranging through the data link, sends the inter-aircraft ranging information to the central host computer, and the central host computer completes the overall navigation and positioning error correction of the unmanned aerial vehicle formation based on the inter-aircraft ranging observation information; Step 3: The high-precision servo-controlled optical sensor arranged on the observation unmanned aerial vehicle tracks and observes a certain unmanned aerial vehicle in the formation, extracts the center of mass coordinates of the certain unmanned aerial vehicle, performs star map matching on background stars in a field of view, identifies and extracts the center of mass coordinates, calculates line-of-sight starlight angular distance observation information between the observation unmanned aerial vehicle and the tracked unmanned aerial vehicle and the background stars in an optical sensor coordinate system, and sends the line-of-sight starlight angular distance observation information to the central host computer to complete the overall navigation and positioning error correction of the unmanned aerial vehicle formation based on the line-of-sight starlight angular distance observation information; The line-of-sight starlight angular distance algorithm model of any unmanned aerial vehicle observing another unmanned aerial vehicle is as follows: ； Wherein, a ij For the formation i The UAV obtains the line-of-sight starlight angle distance of the background star in the field of view through the optical sensor on the UAV, j The UAV and the line-of-sight starlight angle distance of the background star in the field of view, x i , y i , z i For i The position coordinates of the UAV, x j , y j , z j For j The position coordinates of the UAV.

Images