CN115406437A - Cooperative navigation method for UAV formation based on auxiliary constraints of line-of-sight starlight angle distance - Google Patents

Abstract

The invention discloses a UAV formation cooperative navigation method based on the auxiliary constraint of line-of-sight starlight angular distance. Each UAV in the UAV formation has its own inertial navigation equipment, barometer and communication ranging link equipment. A UAV in the UAV formation is equipped with a servo-controlled optical sensor as an observation UAV; the observation UAV tracks and observes the observed UAV and extracts the coordinates of the center of mass of the observed UAV. The background stars in the field perform star map matching and identify and extract the coordinates of the center of mass, and calculate the line-of-sight starlight angular distance observation information between the observation UAV, the observed UAV and the background stars in the optical sensor coordinate system, which is used to assist in the improvement of formation wireless Human-machine overall navigation and positioning accuracy. The present invention can effectively correct the tendency of navigation and positioning errors of formation UAVs based solely on inter-machine communication and ranging to assist formation divergence around a certain point in space, thereby further improving the overall autonomous navigation and positioning accuracy of formation UAVs in a GNSS-denied environment.

Description

Cooperative navigation method for UAV formation based on auxiliary constraint of line-of-sight starlight angle distance

technical field

The invention relates to the autonomous navigation technology of unmanned aerial vehicles, in particular to an unmanned aerial vehicle formation cooperative navigation method based on the auxiliary constraint of line-of-sight starlight angular distance.

Background technique

When the global satellite navigation (GNSS) signal is interfered and unavailable, in the traditional UAV formation cooperative navigation scheme, the formation UAVs in the cooperative network communicate with each other through data link ranging, direction finding and other information, interactive use, real-time correction Navigation information such as the position, speed, and attitude of the UAVs in the formation can be used to improve the overall navigation accuracy of the UAVs in the formation. In this scheme, the UAVs only use the inter-machine communication and ranging information to observe and correct the overall navigation and positioning information of the formation UAVs. Although it can curb the relative position error of the formation UAVs and improve the formation The overall absolute positioning accuracy of UAVs, but it is still unable to curb the tendency of the overall absolute positioning error of formation UAVs to rotate and diverge around a certain point in space and the trend of overall drift and divergence along a certain direction.

Contents of the invention

For the GNSS-denied environment, the cooperative autonomous navigation method based on inter-machine communication and ranging assistance of traditional formation UAVs cannot curb the tendency of the overall absolute positioning error of formation UAVs to rotate and diverge around a certain point in space and the overall drift and divergence along a certain direction. The present invention proposes a UAV formation collaborative navigation method based on the auxiliary constraint of line-of-sight starlight angular distance. The line-of-sight starlight angular distance between man and machine is used as the auxiliary observation information for overall navigation and positioning. The introduction of stellar starlight information can provide an inertial space reference for the formation of UAVs. This method can effectively eliminate the tendency of the overall absolute positioning error of the formation UAVs to rotate and diverge around a certain point in space, thereby further improving the overall absolute navigation and positioning accuracy of the formation UAVs. .

A UAV formation cooperative navigation method based on the auxiliary constraint of line-of-sight starlight angle distance. Each UAV in the UAV formation has its own inertial navigation equipment, barometer and communication ranging link equipment. The UAV formation One of the UAVs is equipped with a servo-controlled optical sensor as an observation UAV; the observation UAV tracks and observes the observed UAV and extracts the coordinates of the center of mass of the observed UAV, and then detects the background stars in the field of view. Perform star map matching and identify and extract the coordinates of the center of mass, and calculate the line-of-sight starlight angular distance observation information between the observation UAV, the observed UAV and the background stars in the optical sensor coordinate system, which is used to assist in improving the overall navigation of the formation UAV positioning accuracy.

Said a UAV formation cooperative navigation method based on the auxiliary constraints of line-of-sight and starlight angular distance, uses the navigation parameters of each UAV in the formation, including position, speed, and attitude, as the state to be estimated, based on the inertia of each UAV The navigation parameter strapdown calculation process of the navigation equipment measurement data is used as the state estimation process of the KF algorithm. The communication ranging information of any two UAVs in the formation is used as the first group of KF observation information, and the line-of-sight starlight angular distance observation information is used as the KF algorithm. The second set of observation information is used to complete the overall navigation and positioning calculation of the formation UAV through the KF algorithm.

Described a kind of unmanned aerial vehicle formation cooperative navigation method based on line-of-sight starlight angular distance auxiliary constraint, the steps are as follows:

Step 1, each UAV airborne inertial navigation module in the formation completes the integral recursion of each UAV position, speed, and attitude navigation parameters based on the gyro measurement data and accelerometer measurement data, and the height data measured by the airborne barometer is used for Correct the error of the UAV position in the height direction, and send the estimated navigation parameter data to the central host, and define the UAV responsible for the fusion and update of all UAV navigation parameters in the formation as the central UAV;

Step 2, each UAV in the formation completes the communication ranging through the data link, and sends the inter-machine ranging information to the central host, and the central host completes the overall navigation and positioning error correction of the formation UAV based on the inter-machine ranging observation information;

Step 3: The high-precision servo control optical sensor arranged on the observation UAV tracks and observes a certain UAV in the formation and extracts the coordinates of the center of mass of the UAV, and then performs star map matching on the background stars in the field of view and identifies and extracts them. The center of mass coordinates, in the coordinate system of the optical sensor, calculate the line-of-sight starlight angular distance observation information between the observation UAV, the observed UAV and the background stars, and send it to the central host to complete the formation unmanned based on the line-of-sight starlight angular distance observation information constraints Machine overall navigation and positioning error correction.

According to the UAV formation cooperative navigation method based on the auxiliary constraint of line-of-sight starlight angular distance, any UAV in the formation observes the line-of-sight starlight angular distance algorithm model of another UAV as follows:

Among them, α _ij is the line-of-sight starlight angular distance obtained by UAV i in the formation through the onboard optical sensor to observe UAV j and the background stars in the field of view, x _i , y _i , z _i are UAV i Position coordinates, x _j , y _j , z _j are the position coordinates of j UAV.

Beneficial effects and advantages of the present invention: the GNSS-denied environment UAV formation cooperative autonomous navigation method based on the auxiliary constraint of line-of-sight starlight angular distance can effectively eliminate the tendency of the overall absolute positioning error of the formation UAV to rotate and diverge around a certain point in space, thereby Further improve the overall absolute navigation and positioning accuracy of formation drones.

Description of drawings

Figure 1 is a schematic diagram of the line-of-sight starlight angular distance.

Figure 2 is a schematic diagram of cooperative autonomous navigation of formation UAVs based on the constraint of line-of-sight starlight angular distance assistance.

Detailed ways

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in combination with specific embodiments according to the figures.

A UAV formation cooperative navigation method based on the auxiliary constraint of line-of-sight and starlight angular distance. The relevant hardware components include the inertial navigation equipment, barometer, communication ranging link equipment and A high-precision servo-controlled optical sensor for a drone. The high-precision servo-controlled optical sensor arranged in a UAV in the formation tracks and observes another UAV in the formation and extracts the coordinates of the center of mass of the UAV, and then performs star map matching on the background stars in the field of view and identifies and extracts them. The center of mass coordinates can be used to calculate the line-of-sight starlight angular distance observation information between the observing UAV, the observed UAV and the background stars in the optical sensor coordinate system, as shown in Figure 1, which is used to assist in improving the overall formation of UAVs. Navigation positioning accuracy.

The basic principle involved in this method is that under the framework of the Kalman filter algorithm (KF), the error of the navigation state (including position, velocity, and attitude) of each UAV in the formation is used as the state to be estimated, based on the inertia of each UAV The error state variance of the navigation equipment strapdown solution process is used as the state estimation process of the KF algorithm, and the difference between the communication ranging measurement information of any two UAVs in the formation and the inertial navigation estimated ranging information is used as the first group of KF observation information 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 used as the second group of KF observation information, and the KF algorithm is used to complete the overall navigation and positioning solution of the formation UAV. The principle block diagram involved in this method is shown in Figure 2 As shown, the algorithm involves the following formulas:

1) The strapdown solution and the KF state equation based on the data of the inertial navigation equipment of the formation UAV are as follows:

为第i架无人机加速度计三轴零偏，

为第i架无人机加速度计三轴零偏在导航坐标系分量，ω_Ni,ω_Ui为第i架无人机地球自转角速率在北向和天向分量，f_Ei,f_Ni,f_Ui为第i架无人机加速度计数据在导航坐标系分量，R_Mhi,R_Nhi为第i架无人机含高度信息的当地曲率半径。Among them, φ _Ei , φ _Ni , φ _Ui are the three-axis attitude angle error of the i-th UAV, δv _Ei , δv _Ni , δv _Ui are the three-axis velocity error of the i-th UAV, δL _i , δλ _i , δh _i is the position error of the i-th UAV, ε _xi , ε _yi , ε _zi are the three-axis zero bias of the gyro of the i-th UAV, ε _Ei , ε _Ni , ε _Ui are the three-axis gyro of the i-th UAV Zero offset in the navigation coordinate system component,

is the three-axis zero bias of the i-th UAV accelerometer,

is the three-axis bias of the i-th UAV accelerometer in the navigation coordinate system, ω _Ni , ω _Ui are the north and celestial components of the i-th UAV’s earth rotation angular rate, f _Ei , f _Ni , f _Ui are The accelerometer data of the i-th UAV is in the navigation coordinate system component, R _Mhi , R _Nhi is the local curvature radius of the i-th UAV with height information.

2) The KF observation equation based on the ranging data between formation UAVs is as follows:

Assume that the inter-machine ranging measurement values of i-UAV and j-UAV in the formation are:

The estimated inter-machine ranging based on the inertial navigation of the i UAV and the j UAV is:

Then the inter-machine ranging error observation equation is:

in,

Among them, x _Ii , y _Ii , z _Ii are the position estimates of the i-th UAV based on inertial navigation recursion, r _Iij can be numerically ρ _Iij , and D _ij is the transformation matrix between the earth coordinate system and the latitude and longitude coordinates .

3) The KF observation equation based on the line-of-sight starlight angular distance data of formation UAVs is as follows:

Assuming that UAV i in the formation observes UAV j and the background stars in the field of view through the onboard optical sensor, the corresponding measurement value of the line-of-sight starlight angular distance is:

The line-of-sight/starlight angular distance estimated based on the built-in inertial navigation of i UAV and j UAV is:

Then the line-of-sight starlight angular distance error observation equation is:

in,

X_Bi＝x_i-x_j，Y_Bi＝y_i-y_j，Z_Bi＝z_i-z_j，s_x,s_y,s_z为恒星矢量。Among them, ap _i = s _x · X _Bi + s _y · Y _Bi + s _z · Z _Bi ,

X _Bi ＝ _xi -x _j , Y _Bi ＝y _i -y _j , Z _Bi ＝zi _{-z j} , s _x , s _y , s _z are stellar vectors.

4) The KF algorithm flow is as follows:

In the multi-UAV formation cooperative autonomous navigation scheme, the distributed sequential Kalman filter is used as the framework of the fusion filtering algorithm. Taking the single sequential filtering of the i UAV and the j UAV as an example, the specific process is as follows: :

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

The time update process of the i-th UAV is as follows:

X _i,k+1/k = Φ _i,k+1/k X _i,k

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

X _j,k+1/k ＝Φ _j,k+1/k X _j,k

The sequential measurement update process corresponding to the observations of the i-th UAV and the j-th UAV is as follows:

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 )

P _ij,k+1 ＝(IK _ij,k+1 H _ij,k+1 ) P _ij,k+1/k

in,

X _ij,k+1/k ＝[X _i,k+1/k ,X _j,k+1/k ] ^T

Φ _i,k+1/k , Φ _j,k+1/k is the state update system matrix of the i-th UAV and j UAV, Γ _i,k , Γ _j,k is the noise driving matrix, Q _{i, k} , Q _j,k is the noise matrix, Z _ij,k+1 is associated with i-UAV and j-UAV observations, H _ij,k+1 is the amount associated with i-UAV and j-UAV observations measurement matrix, R _ij,k+1 is the measurement noise matrix.

This method specifically comprises the following steps:

Step 1. Each UAV in the formation estimates its own navigation parameters based on the onboard inertial navigation equipment and barometer, and sends the navigation parameter estimation data to the central host. The specific process is: the airborne inertial navigation module of each UAV in the formation completes the integral recursion of navigation parameters such as the position, speed, and attitude of each UAV based on the gyro measurement data and the accelerometer measurement data, and the height data measured by the airborne barometer Used to correct the error of the drone's position in the altitude direction.

Step 2. Each UAV in the formation completes the communication ranging through the data link, and sends the inter-machine ranging information to the central host, and the central host completes the overall navigation and positioning error correction of the formation UAVs based on the inter-machine ranging observation information.

Step 3: The high-precision servo-controlled optical sensor arranged on the main engine or any wingman tracks and observes a UAV in the formation and extracts the coordinates of the center of mass of the UAV, and then performs star map matching and identification extraction on the background stars in the field of view Its center of mass coordinates are used to calculate the line-of-sight starlight angular distance observation information between the observation UAV, the observed UAV and the background stars in the optical sensor coordinate system, and send it to the central host to complete the formation based on the line-of-sight starlight angular distance observation information constraints. Man-machine overall navigation and positioning error correction.

The present invention is based on the GNSS denial environment UAV formation cooperative autonomous navigation method based on the auxiliary constraint of line-of-sight starlight angle distance, which can effectively eliminate the tendency of the overall absolute positioning error of the formation UAV to rotate and diverge around a certain point in space, thereby further improving the formation UAV Overall absolute navigation positioning accuracy.

The above-mentioned embodiments only express several implementation modes of the present invention, and the description thereof is relatively specific and detailed, but should not be construed as limiting the scope of the invention. It should be pointed out that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention should be determined by the appended claims.

Claims (4)

1. An unmanned aerial vehicle formation collaborative navigation method based on auxiliary constraint of sight starlight angular distance is characterized by comprising the following steps: each unmanned aerial vehicle in the formation of unmanned aerial vehicles is provided with an inertial navigation device, a barometer and a communication ranging link device, wherein a servo control optical sensor is arranged on one unmanned aerial vehicle in the formation of unmanned aerial vehicles to serve as an observation unmanned aerial vehicle; the observation unmanned aerial vehicle tracks and observes the observed unmanned aerial vehicle and extracts the centroid coordinate of the observed unmanned aerial vehicle, then carries out star map matching on the background fixed star in the visual field and identifies and extracts the centroid coordinate, calculates the observation information of the sight starlight angular distance between the observation unmanned aerial vehicle and the observed unmanned aerial vehicle and between the observation unmanned aerial vehicle and the background fixed star under the coordinate system of the optical sensor, and is used for assisting in improving the overall navigation positioning precision of the formation unmanned aerial vehicle.

2. The unmanned aerial vehicle formation collaborative navigation method based on the auxiliary constraint of sight line starlight angular distance as claimed in claim 1, wherein: with each unmanned aerial vehicle navigation parameter in the formation, including the position, the speed, the gesture, as waiting to estimate the state, navigation parameter strapdown calculation process based on each unmanned aerial vehicle is from taking inertial navigation equipment measured data predicts the process as KF algorithm state, regard as KF first group observation information with two arbitrary unmanned aerial vehicle's in the formation communication range finding information, regard as KF second group observation information with sight starlight angular distance observation information, accomplish formation unmanned aerial vehicle whole navigation positioning through the KF algorithm and solve.

3. The unmanned aerial vehicle formation collaborative navigation method based on sight line starlight angular distance auxiliary constraint is characterized in that: the method comprises the following steps:

step 1, integrating and recursion of position, speed and attitude navigation parameters of each unmanned aerial vehicle is completed by each unmanned aerial vehicle airborne inertial navigation module in a formation based on gyro measurement data and accelerometer measurement data, height data measured by an airborne barometer is used for correcting errors of the position of the unmanned aerial vehicle in the height direction, navigation parameter prediction data is sent to a central host, and the unmanned aerial vehicle in the formation, which is responsible for fusion and update of all unmanned aerial vehicle navigation parameters, is defined as the central unmanned aerial vehicle;

step 2, finishing communication ranging by all unmanned aerial vehicles in the formation through a data chain, sending inter-machine ranging information to a central host, and finishing the correction of the whole navigation positioning error of the formation unmanned aerial vehicles based on the inter-machine ranging observation information by the central host;

and 3, tracking and observing a certain unmanned aerial vehicle in the formation by a high-precision servo control optical sensor arranged on the observing unmanned aerial vehicle, extracting the centroid coordinate of the unmanned aerial vehicle, performing star map matching on a background fixed star in the visual field, identifying and extracting the centroid coordinate of the background fixed star, calculating the sight starlight angular distance observation information between the observing unmanned aerial vehicle, the observed unmanned aerial vehicle and the background fixed star under the coordinate system of the optical sensor, and sending the sight starlight angular distance observation information to the central host to finish the whole navigation and positioning error correction of the unmanned aerial vehicle in the formation based on the sight starlight angular distance observation information constraint.

4. The unmanned aerial vehicle formation collaborative navigation method based on the auxiliary constraint of sight line starlight angular distance as claimed in claim 1, wherein: the sight line starlight angular distance algorithm model for observing another unmanned aerial vehicle by any unmanned aerial vehicle in the formation is as follows:

wherein alpha is _ij Observing the j unmanned aerial vehicle and the background fixed star in the view field by the i unmanned aerial vehicle in the formation through the airborne optical sensor to obtain the sight starlight angular distance, x _i ,y _i ,z _i For i unmanned plane position coordinates, x _j ,y _j ,z _j J drone position coordinates.

Images