A method and device for estimating the state of a target drone based on monocular vision

Abstract

The present invention discloses a method and device for estimating the state of a target UAV based on monocular vision, comprising: (1) obtaining the state information of the UAV by using a sensor on the tracking UAV; (2) calculating the wind speed in combination with the force analysis of the UAV; (3) obtaining an image of the target UAV by using a pan / tilt camera on the tracking UAV, obtaining the position and attitude of the target UAV in the camera coordinate system by using a posture detection algorithm, and converting the position and attitude to a global coordinate system; (4) constructing a target UAV state estimation model including wind disturbance based on the calculated wind speed; predicting the state of the target UAV by using the state estimation model, and correcting and updating the state of the target UAV by using the position and attitude; (5) tracking the target UAV by using the obtained state of the target UAV. The present invention can realize the state estimation of a highly maneuverable target UAV in a single UAV wind disturbance environment, thereby improving the UAV tracking control performance.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) tracking and control, and in particular to a method and device for estimating the state of a target UAV in a wind disturbance environment based on monocular vision. Background Art

[0002] As an emerging research field, drone swarms have demonstrated strong mission execution capabilities and are being applied in various fields. Formation flying and swarm control are cutting-edge research areas.

[0003] Currently, coordinated control of drone swarms primarily relies on communication systems. For example, Chinese patent publication CN112180975A discloses an intelligent swarm drone control system and method. This system uses wireless ad hoc communication radios to establish communication channels between drones and between drones and a control center. However, communication limits the upper limit of swarming.

[0004] Methods based on the detection of known drones can obtain the relative position information of adjacent drones without communication. For example, Chinese patent publication CN115951716A discloses a method for adaptively swarming fixed-wing drones using monocular vision. This method, which does not rely on communication but relies entirely on small-field monocular vision, achieves fixed-wing drone swarming. However, this tracking control method suffers from slow response and tail-swinging.

[0005] Therefore, many studies at home and abroad have focused on posture-based target drone detection, aiming to predict the target's motion trend by detecting its posture. However, this method also has its drawbacks. In outdoor environments, wind interference is very severe, resulting in posture uncertainty, making it impossible to achieve posture tracking control of drones in outdoor wind-disturbed scenarios.

[0006] Therefore, a three-dimensional spatial state estimation algorithm for highly maneuverable target UAVs based on machine vision is urgently needed to solve the problems existing in the existing technology, especially the three-dimensional spatial state estimation algorithm for target UAVs in a wind disturbance environment based on the known full state information of the observing UAV itself and the relative position and attitude information of the target UAV. Summary of the Invention

[0007] The present invention provides a method and device for estimating the state of a target UAV based on monocular vision, which can realize the state estimation of a highly maneuverable target UAV in an outdoor wind disturbance environment with a single UAV, thereby improving the tracking and control performance of the UAV.

[0008] A method for estimating the state of a target UAV based on monocular vision, characterized by comprising:

[0009] (1) Using the sensors on the tracking drone to measure its own state information, including the tracking drone's position, speed, acceleration, attitude, and thrust;

[0010] (2) Based on the tracking drone's own status information and combined with the drone's force analysis, the wind speed is further calculated;

[0011] (3) Obtain an image of the target UAV by tracking the gimbal camera on the UAV, obtain the position and attitude of the target UAV in the camera coordinate system using the pose detection algorithm, and convert the position and attitude of the target UAV to the global coordinate system;

[0012] (4) Based on the calculated wind speed, a state estimation model of the target UAV including wind speed disturbance is constructed; the state estimation model is used to predict the state of the target UAV, and the state of the target UAV is corrected and updated according to the position and attitude of the target UAV;

[0013] Among them, the state estimation model predicts the state of the target UAV, including the position, velocity, thrust acceleration increment, attitude angle, and angular velocity of the target UAV in three-dimensional space;

[0014] (5) Using the obtained state of the target UAV, track the target UAV.

[0015] In step (1), the thrust of the tracking drone is obtained by measuring the speed of each motor through a sensor and then calculating the corresponding relationship between the speed of the motor and the thrust.

[0016] The correspondence between motor speed and thrust is constructed through a fully connected neural network, which consists of an input layer, two hidden layers and an output layer. The input of the network is the speed of each motor, and the output is the thrust.

[0017] In step (2), the wind speed is calculated as follows:

[0018]

[0019] Where, v wind is the wind speed, m is the mass of the tracking UAV, a is the acceleration of the tracking UAV, and T is the thrust of the tracking UAV; is the rotation matrix from the body to the world coordinate system, obtained according to the posture ρ of the tracking drone; e3 = [0,0,1] T , g is the acceleration due to gravity, C D is the air drag coefficient, ρ is the air density, S is the wind-exposed area of the tracking UAV, and v is the flight speed of the tracking UAV in the world coordinate system.

[0020] In step (3), the specific process of the posture detection algorithm is as follows:

[0021] First, eight 3D key points are preset for the target drone in the drone coordinate system. Then, a neural network is constructed to determine the projected positions of these key points in the image, namely the 2D key points. Finally, the EPnP algorithm is used to combine the 2D-3D key point pairs to determine the position and attitude of the target drone relative to the observation camera.

[0022] The neural network takes an input image of size 640x480, and after feature extraction, feature fusion, and feature processing, it outputs eight heat maps of size 40x40; each heat map is responsible for predicting the position of one key point.

[0023] In step (4), the position and attitude of the target UAV are converted to the global coordinate system as follows:

[0024] According to the posture of the gimbal camera ρ camera , get the rotation matrix from the camera coordinate system to the world coordinate system and its position p in the global coordinate system c,world , and then transform the position and attitude of the target drone into the global coordinate system:

[0025]

[0026] Where p t,world represents the position of the target UAV in the global coordinate system, p t,camera represents the position of the target UAV in the camera coordinate system, ρ t,world represents the posture of the target UAV in the global coordinate system, ρ t,camera Indicates the pose of the target drone in the camera coordinate system.

[0027] The target UAV state estimation model including wind speed disturbance is constructed as follows:

[0028]

[0029] Where, They are the target position p, velocity v, and thrust acceleration increment w respectively T , attitude angle R and first-order derivative of angular velocity ω; is the rotation matrix from the body to the world coordinate system; e3 = [0,0,1] T , g is the acceleration due to gravity; a D is the drag of the target UAV, which is calculated by the current wind speed and the speed of the target UAV; [ω] × Represents the conversion operation from three-dimensional vector space to three-dimensional rotation space, J represents the moment of inertia vector of the drone, ∈ p ,∈ q ,∈ r They represent the angular velocity noise of the body coordinate system respectively.

[0030] A target UAV state estimation device based on monocular vision includes a UAV body and an onboard computer, a flight control system, a sensor, and a pan-tilt camera arranged on the UAV body;

[0031] The onboard computer stores a computer program and is respectively in communication with the flight control system, the sensor, and the gimbal camera; wherein the computer program includes program code for controlling a process to execute a process, wherein the process includes the above-mentioned monocular vision-based target UAV state estimation method;

[0032] The sensor is used to obtain and track the status information of the drone itself;

[0033] The pan-tilt camera is used to obtain images of the target drone;

[0034] The flight control system is used to control the UAV body to track the target UAV according to the control instructions output by the onboard computer.

[0035] Compared with the prior art, the present invention has the following beneficial effects:

[0036] 1. The present invention fully utilizes the characteristics of known traditional rotorcraft UAV models to establish a system model of the target UAV. On this basis, it adds thrust disturbance estimation and fully utilizes all the information that can be obtained by its own sensors in outdoor environments to estimate the ambient wind speed disturbance, thereby calculating the air resistance acting on the target UAV, thereby improving the target state estimation accuracy and estimation response speed.

[0037] 2. When a highly maneuverable rotorcraft drone performs a maneuver, its attitude is the first thing to change. Traditional estimation models do not incorporate attitude information, resulting in significant deviations in velocity estimation when estimating the state of a maneuvering target. This invention fully considers the target's attitude, resulting in a more accurate model, faster convergence of target velocity estimation, and smaller state estimation errors. This provides more real-time and accurate tracking error information for tracking control, improving overall drone tracking control performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a flow chart of a method for estimating the state of a target drone based on monocular vision according to an embodiment of the present invention;

[0039] Figure 2 This is a flow chart of wind disturbance estimation in an embodiment of the present invention;

[0040] Figure 3 This is a flow chart of target UAV state estimation under wind disturbance in an embodiment of the present invention;

[0041] Figure 4This is a flow chart of the target drone pose detection algorithm in an embodiment of the present invention;

[0042] Figure 5 This is a schematic diagram of the error result of estimating the position of the target UAV in an embodiment of the present invention;

[0043] Figure 6 Schematic diagram of the target UAV speed error estimation result in an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The present invention will be described in further detail below with reference to the accompanying drawings and examples. It should be noted that the following examples are intended to facilitate understanding of the present invention and do not have any limiting effect on the present invention.

[0045] Example 1

[0046] This application proposes a method for estimating the state of a target drone based on monocular vision. One application scenario involves tracking a highly maneuverable target drone in an open outdoor environment, using only a monocular camera, in the presence of wind disturbances. This method, which requires no communication with the target drone, enables effective cluster expansion applications and has important implications for future research on decentralized and communication-free clustering.

[0047] The main process of the method of the present invention is as follows Figure 1 The entire process is performed on the tracking UAV, eliminating the need for information exchange with the target UAV. The method consists of two main parts: one is wind disturbance estimation based on its own sensor measurements; the other is target state estimation based on monocular vision.

[0048] like Figure 2 As shown in Figure 2, the wind disturbance estimation method based on the wind disturbance measured by the sensor itself is as follows:

[0049] S01, using the sensors on the tracking drone to measure and obtain its own status information.

[0050] First, we need to obtain the relationship between the motor speed and thrust of the tracking drone.

[0051] T i =f T (ω i ),i=1,2,3,4

[0052] Among them, T i ,ω i are the thrust and speed generated by the i-th motor respectively.

[0053] This relationship is generally linear, but in the current scenario, more accurate data is needed. A fully connected neural network is used to learn the model. The network consists of an input layer, two hidden layers, and an output layer. The input of the network is the motor speed, and the output is the thrust. The tracking drone can obtain the thrust T of the drone's four propellers through the trained model. i .

[0054] Then, the tracking drone needs to obtain the measurement information of its own sensors, including the inertial guidance unit and RTK positioning, and obtain the tracking drone's own state information including the target's position p, velocity v, acceleration a, attitude ρ and thrust T.

[0055] S02, based on the state information of the tracking drone itself and combined with the force analysis of the drone, the wind speed is further calculated.

[0056] According to the force analysis of the UAV, the following equation can be obtained:

[0057]

[0058] Where m is the mass of the tracking drone, The rotation matrix from the body to the world coordinate system can be obtained according to the posture ρ of the tracking drone, and g is the gravitational acceleration e3 = [0,0,1] T , D is resistance.

[0059] Since all other quantities except D in the equation are known, the value of resistance can be obtained as

[0060]

[0061] Furthermore, we know that the calculation formula of air resistance is

[0062]

[0063] Where C D is the air resistance coefficient, ρ is the air density, S is the wind-exposed area of the tracking drone, v a is the airspeed of the UAV in the airflow coordinate, which is equal to

[0064] v a =v+v wind

[0065] And v is the flight speed of the tracking drone in the world coordinate system, v wind Then, after sorting, we can get the formula for calculating wind speed as

[0066]

[0067] After the wind speed is calculated, the optimal estimated wind speed is obtained according to the Kalman filter algorithm, and the wind speed in the local wind field is output.

[0068] Next, the calculated wind speed will be combined to estimate the state of the target drone.

[0069] Because tracking drones require close-range tracking of the target drone (the tracked drone), it can be assumed that the two drones are in the same wind field, and therefore the target drone is also exposed to the same wind conditions. Based on this, a monocular vision-based state estimation method for the target drone under wind disturbance is constructed.

[0070] S03, obtain an image of the target drone by tracking the gimbal camera on the drone, use the object position and attitude estimation algorithm to obtain the position and attitude of the target drone in the camera coordinate system, and convert the position and attitude of the target drone to the global coordinate system.

[0071] like Figure 3 As shown, firstly, the image is obtained by the gimbal camera, and the position p of the target in the camera coordinate system is estimated by the known prior information. t,camera and posture ρ t,camera .

[0072] A large number of drone pose datasets were collected using the motion capture system Vicon and RGB cameras, which include images of target drones and their positions and poses relative to the camera. The position of the target drone is estimated by training the existing object position and pose estimation algorithm (drone detection algorithm). t,camera and pose estimation ρ t,camera .

[0073] In the present invention, eight 3D key points are first preset for the target UAV in the UAV coordinate system, and then a neural network is constructed to determine the projection positions of these key points in the image. Finally, the position and posture of the target UAV relative to the observation camera are calculated by combining the 2D-3D key point pairs through the EPnP algorithm.

[0074] In order to locate the 2D key points in the image, we construct Figure 4 The network takes an input image of size 640x480 and then outputs 8 heat maps of size 40x40 after feature extraction, feature fusion and feature processing. Each heat map is responsible for predicting the position of a key point. Represents weighted fusion of feature maps.

[0075] The network is trained using supervised learning, and the loss function is defined as follows:

[0076] L JS =JSD(φ(Φ),Φ* )

[0077] Among them, φ(*) represents the channel-level Softmax function, which is normalized to a probability distribution at each pixel of the feature map; Φ * represents the true value heat map; Φ represents the feature map output by the feature processing layer; JSD(*) represents the JS divergence, defined as

[0078]

[0079] Where KL(*) represents the KL divergence, defined as

[0080]

[0081] According to the posture of the gimbal camera ρ camera , you can get the rotation matrix from the camera coordinate system to the world coordinate system and its position p in the global coordinate system c,world , the position and attitude of the target drone can be transformed into the global coordinate system:

[0082]

[0083] The gimbal camera uses the gimbal angle control algorithm to achieve visual tracking of the target. First, the error of the target drone in the image from the image center is calculated.

[0084]

[0085] Among them [p x ,p y ] is the pixel position of the target drone in the image, [w,h] is the image pixel size, and the image coordinate system has the upper left corner as the origin, the x axis points to the right along the horizontal axis of the image, and the y axis points downward along the vertical direction of the image.

[0086] The control of the PTZ is obtained by the following formula

[0087]

[0088] in and are the control instructions for the pitch angular velocity and yaw angular velocity of the gimbal camera respectively. p and k i is the control gain coefficient.

[0089] S04, constructing a state estimation model of the target UAV including wind speed disturbance according to the calculated wind speed; using the state estimation model to predict the state of the target UAV, and correcting and updating the state of the target UAV according to the position and attitude of the target UAV.

[0090] Construct a state estimation model for the target UAV. Assume that the state of the target UAV is

[0091]

[0092] Where p is the position of the target UAV, v is the speed of the target UAV, and w is the target UAV’s velocity. T is the thrust acceleration increment of the target UAV, R is the attitude angle of the target UAV, and ω is the angular velocity of the target UAV.

[0093] Because most drones have an inner ring height self-stabilization during flight, it is assumed that the thrust acceleration is an additional deviation w after balancing the gravity acceleration. T , the deviation also changes with the balance amount.,This method can achieve the target UAV modeling to a great extent, ensuring the applicability and rapid response of the model.

[0094] Then the dynamic equation of the state estimation model is

[0095]

[0096] Its expanded form is as follows

[0097]

[0098] Linearize the dynamic equation and get the state transfer matrix:

[0099]

[0100] Where A, B, C and u are

[0101]

[0102] C k =[0 3×3 I 3×3 0 3×7 ]

[0103]

[0104] is the rotation matrix from the target UAV body to the world coordinate system, which is calculated based on the target UAV posture obtained by detection ρ = [φ, θ, ψ] T Get, calculated as follows

[0105]

[0106]

[0107] a Dis the drag of the target drone, which is calculated based on the estimated target speed and the current wind speed:

[0108]

[0109] Among them, S is the wind-exposed area of the target UAV, is the estimated target UAV speed, To track the wind speed estimated by the drone based on its own state.

[0110] Then, the spatiotemporal fusion algorithm is combined, and its iteration includes two parts: prediction and update:

[0111] Prediction section

[0112]

[0113] Among them F k =I+A k , I is the unit diagonal matrix.

[0114] Update section

[0115]

[0116] Among them, γ1, γ2, α1, α2, and c are adjustment parameters. p , z ρ are the measured target position and posture, H p , H ρ is the corresponding measurement matrix.

[0117] S05: Track the target UAV using the obtained state of the target UAV.

[0118] After obtaining the estimated state of the target UAV, better tracking effect can be achieved through traditional control algorithms.

[0119] Figure 5 : The error result of estimating the target drone position in an embodiment of the present invention, where the black solid line is the true target position, the dotted line is the estimated target state position without wind disturbance, and the dot-dash line is the estimated target state position with wind disturbance proposed by the present invention.

[0120] Figure 6 : The error result of estimating the target UAV speed in an embodiment of the present invention, where the black solid line is the target true speed, the dashed line is the target state estimated speed without wind disturbance, and the dotted line is the target state estimated speed with wind disturbance proposed by the present invention.

[0121] The wind speed of 4m / s was added in the simulation. From the results, it can be seen that under wind disturbance, the method proposed in the present invention can achieve faster speed tracking response and more accurate position tracking results.

[0122] This application also proposes a target drone state estimation device based on monocular vision, which includes a drone body and an onboard computer, a flight control system, a sensor and a gimbal camera arranged on the drone body.

[0123] The onboard computer stores a computer program and is respectively connected to the flight control system, the sensor, and the gimbal camera; wherein the computer program includes a program code for controlling a process to execute a process, and the process includes the target UAV state estimation method based on monocular vision of Example 1.

[0124] The sensor is used to obtain and track the status information of the drone itself;

[0125] The gimbal camera is used to obtain images of the target drone;

[0126] The flight control system is used to control the UAV body to track the target UAV according to the control instructions output by the onboard computer.

[0127] The embodiments described above provide a detailed description of the technical solutions and beneficial effects of the present invention. It should be understood that the above are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, supplements and equivalent substitutions made within the scope of the principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for estimating the state of a target drone based on monocular vision, characterized in that: include: (1) Using the sensors on the tracking drone to measure its own state information, including the tracking drone's position, speed, acceleration, attitude, and thrust; (2) Based on the tracking drone's own status information and the force analysis of the drone, the wind speed is further calculated; the wind speed calculation formula is: Where, v wind is the wind speed, m is the mass of the tracking UAV, a is the acceleration of the tracking UAV, and T is the thrust of the tracking UAV; is the rotation matrix from the body to the world coordinate system, obtained according to the posture ρ of the tracking drone; e3 = [0,0,1] T , g is the acceleration due to gravity, C D is the air resistance coefficient, ρ is the air density, S is the wind-exposed area of the tracking UAV, and v is the flight speed of the tracking UAV in the world coordinate system; (3) Obtain an image of the target UAV by tracking the gimbal camera on the UAV, obtain the position and attitude of the target UAV in the camera coordinate system using the pose detection algorithm, and convert the position and attitude of the target UAV to the global coordinate system; (4) Based on the calculated wind speed, a target UAV state estimation model including wind speed disturbance is constructed; the state estimation model is used to predict the state of the target UAV, and the state of the target UAV is corrected and updated according to the position and attitude of the target UAV; Among them, the state estimation model predicts the state of the target UAV, including the position, velocity, thrust acceleration increment, attitude angle, and angular velocity of the target UAV in three-dimensional space; The target UAV state estimation model including wind speed disturbance is constructed as follows: Where, They are the target position p, velocity v, and thrust acceleration increment w respectively T , attitude angle R and first-order derivative of angular velocity ω; is the rotation matrix from the body to the world coordinate system; e3 = [0,0,1] T , g is the acceleration due to gravity; a D is the drag of the target drone, which is calculated by the current wind speed and the speed of the target drone, φ represents the roll angle of the drone, and θ represents the pitch angle of the drone; [ω] × Represents the conversion operation from three-dimensional vector space to three-dimensional rotation space, J represents the moment of inertia vector of the drone, ∈ p ,∈ q ,∈ r Respectively represent the angular velocity noise of the body coordinate system; (5) Using the obtained state of the target UAV, track the target UAV.

2. The method for estimating the state of a target drone based on monocular vision according to claim 1, characterized in that: In step (1), the thrust of the tracking drone is obtained by measuring the speed of each motor through a sensor and then calculating the corresponding relationship between the speed of the motor and the thrust.

3. The method for estimating the state of a target drone based on monocular vision according to claim 2, characterized in that: The correspondence between motor speed and thrust is constructed through a fully connected neural network, which consists of an input layer, two hidden layers and an output layer. The input of the network is the speed of each motor, and the output is the thrust.

4. The method for estimating the state of a target drone based on monocular vision according to claim 1, characterized in that: In step (3), the specific process of the posture detection algorithm is as follows: First, eight 3D key points are preset for the target drone in the drone coordinate system. Then, a neural network is constructed to determine the projected positions of these key points in the image, namely the 2D key points. Finally, the EPnP algorithm is used to combine the 2D-3D key point pairs to determine the position and attitude of the target drone relative to the observation camera. The neural network takes an input image of size 640x480, and after feature extraction, feature fusion, and feature processing, it outputs eight heat maps of size 40x40; each heat map is responsible for predicting the position of one key point.

5. The method for estimating the state of a target drone based on monocular vision according to claim 1, characterized in that: In step (3), the position and attitude of the target UAV are converted to the global coordinate system as follows: According to the posture of the gimbal camera ρ camera , get the rotation matrix from the camera coordinate system to the world coordinate system and its position p in the global coordinate system c,world , and then transform the position and attitude of the target drone into the global coordinate system: Where p t,world represents the position of the target UAV in the global coordinate system, p t,camera represents the position of the target UAV in the camera coordinate system, ρ t,world represents the posture of the target UAV in the global coordinate system, ρ t,camera Indicates the pose of the target drone in the camera coordinate system.

6. A target drone state estimation device based on monocular vision, characterized in that: It includes the drone body and the onboard computer, flight control system, sensors and pan-tilt camera installed on the drone body; The onboard computer stores a computer program and is respectively in communication with the flight control system, the sensor, and the gimbal camera; wherein the computer program includes a program code for controlling a process to execute a process, wherein the process includes the method for estimating the state of a target UAV based on monocular vision according to any one of claims 1 to 5; The sensor is used to obtain and track the status information of the drone itself; The pan-tilt camera is used to obtain images of the target drone; The flight control system is used to control the UAV body to track the target UAV according to the control instructions output by the onboard computer.

Images