Vision-based unmanned aerial vehicle synchronous positioning tracking and control system

Abstract

The invention discloses an unmanned aerial vehicle synchronous positioning tracking and control system based on vision, and belongs to the field of unmanned aerial vehicle positioning tracking, the system comprises a target state estimation unit, a target positioning unit, an unmanned aerial vehicle state planning unit and a trajectory tracking and control unit, the state of a to-be-tracked target is estimated, and the estimated target state is obtained; presetting the initial estimation state of the moving target or obtaining the first two estimation positions of the static target, and filtering the estimation position or estimation state of the target at the current moment by using the next observation image and the estimated target state to obtain the next estimation position or estimation state of the target; obtaining the next expected position of the unmanned aerial vehicle; and finally, according to the next expected position, controlling the unmanned aerial vehicle to fly towards the expected position. The planned path is more beneficial for the controller to stably execute the control process.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle target positioning and tracking, and in particular relates to a vision-based unmanned aerial vehicle synchronous positioning, tracking and control system. Background Art

[0002] The problem of synchronous target positioning, tracking and control of unmanned aerial vehicles is to locate and track the target using one or more unmanned aerial vehicles. These unmanned aerial vehicles act as mobile sensors and use certain information about the target measured to find the state of the unknown target. The state of the target usually includes the position, velocity and acceleration of the target, depending on the target model adopted. The information about the target can be the distance, direction or both, depending on the sensors carried by the unmanned aerial vehicles.

[0003] Drone simultaneous positioning, tracking and control systems are usually used in scenarios where precise positioning and tracking of targets are required, such as construction site monitoring, crop monitoring in the agricultural field, environmental monitoring and other fields. In these scenarios, real-time and accurate positioning and tracking of targets are usually required for effective monitoring and management.

[0004] Existing positioning methods can be divided into active positioning and passive positioning. Active positioning and tracking systems refer to positioning methods that require the use of active sensors such as radar to radiate high-power electromagnetic waves into the air. Active positioning is mainly based on laser ranging positioning models. Its advantages are simple methods and high positioning accuracy, but it needs to be equipped with a laser rangefinder, has a large load, and needs to actively emit electromagnetic waves, which is not conducive to its own concealment. Passive positioning and tracking systems need to use passive sensors such as infrared sensors and sonar sensors to detect targets. They do not emit electromagnetic waves, so they have the advantages of good concealment and are not easy to be detected by the enemy. Passive positioning methods include target positioning based on image matching patterns and target positioning based on imaging models. The image matching method has poor real-time image matching and has certain limitations in obtaining reference images. Visual sensors are small in size and low in power consumption, and are suitable for being carried on drones for positioning. The disadvantage is that they are greatly affected by system errors and have insufficient positioning accuracy.

[0005] There are still some problems with the existing UAV synchronous positioning, tracking and control systems. For example, patent document CN116382350A discloses a UAV target tracking method and a UAV target tracking system, which locate and track targets by identifying sound signals around the UAV. However, the system can only track several types of sounds trained by neural networks, and the types of trackable targets are limited, and the sound signals are not suitable for long-distance tracking; patent document CN 117406770 A discloses an omnidirectional UAV autonomous identification and tracking system and method, which only tracks targets by controlling the rotation of the gimbal, has a limited tracking range, lacks flexibility, and uses a binocular camera for ranging. The binocular camera has higher requirements for the accuracy of calibration parameters, so the maintenance cost is very high in actual operation. Summary of the invention

[0006] In order to solve the problems in the prior art, the present invention provides a vision-based UAV synchronous positioning, tracking and control system.

[0007] The technical solution adopted by the present invention is as follows:

[0008] The present invention discloses a vision-based UAV synchronous positioning tracking and control system, comprising a target state estimation unit, a target positioning unit, a UAV state planning unit and a trajectory tracking and control unit;

[0009] The target state estimation unit estimates the state of the target to be tracked and transmits the estimated target state to the target positioning unit;

[0010] When the target is in a static state, the target positioning unit obtains the observation image taken by the onboard camera, collects the position of the target in the observation image, and uses the initial two observation images to locate the target according to the pinhole camera model to obtain the first two estimated positions of the target. The target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated position of the target at the current moment, obtain the next estimated position of the target, and send the next estimated position of the target to the drone state planning unit;

[0011] When the target is in motion, an initial estimated state of the moving target is preset, the initial estimated state of the moving target includes the initial estimated position and speed of the moving target, the target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated state of the target at the current moment, and obtains the next estimated state of the target, the next estimated state of the target includes the next estimated position and speed of the target, and the target positioning unit sends the next estimated position of the target to the drone state planning unit;

[0012] The UAV state planning unit obtains the next expected position of the UAV based on the next estimated position of the target combined with the preset UAV operation trajectory, and sends the next expected position of the UAV to the trajectory tracking and control unit;

[0013] The trajectory tracking and control unit controls the drone to fly toward the next expected position according to the received next expected position of the drone.

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

[0015] (1) A systematic approach is proposed to solve the problem of simultaneous target localization and tracking, which can ensure real-time tracking of the target while satisfying observability conditions. In this problem, the motion planning, control, and target state estimation subunits must be coupled, while existing methods often only consider part of the problem.

[0016] (2) A target initial state estimation method based on the least squares method is proposed. The target initial value is reasonably set by combining the first sampling image with the planned trajectory of the UAV to ensure that the UAV flies level, provide a more favorable observation angle for the camera, and accelerate the convergence of positioning error.

[0017] (3) Considering various errors in the target observation process, unscented Kalman filtering is performed using multiple images to improve positioning accuracy;

[0018] (4) A space-time curve of the UAV flight with the optimal observation geometry is planned, so that the UAV hovers around the target and sets the trajectory radial velocity v r With the UAV radial position error d xoy The planned path is more conducive to the controller to stably execute the control process.

[0019] (5) Accurately modeling the position and attitude dynamics of the UAV and using PID control can achieve stable control of the quadrotor UAV, which is easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a system block diagram of a vision-based UAV synchronous positioning, tracking and control system provided by the present invention;

[0021] Figure 2 It is a schematic diagram of the observation of the pinhole camera model and a coordinate transformation diagram;

[0022] Figure 3 It is the relationship between the second estimated position of the target and the camera observation axis;

[0023] Figure 4 It is a schematic diagram of target positioning using the least squares method;

[0024] Figure 5 It is the three-dimensional positioning error map of the UAV to the stationary target;

[0025] Figure 6 It is the three-dimensional positioning error map of the UAV to the moving target;

[0026] Figure 7 is the UAV trajectory tracking control error graph when locating a stationary target;

[0027] Figure 8 It is the three-dimensional positioning and trajectory planning map of the UAV for the moving target;

[0028] Fig. 9 It is the simulink simulation diagram of the control unit;

[0029] Fig.10 It is the three-dimensional positioning and trajectory planning map of the UAV for the stationary target;

[0030] Fig.11 It is the trajectory tracking control error diagram of the UAV when tracking a moving target. DETAILED DESCRIPTION

[0031] The present invention is further described and illustrated below in conjunction with specific embodiments. The embodiments are merely exemplary of the present disclosure and do not define the scope of limitation. The technical features of each embodiment of the present invention may be combined accordingly without conflicting with each other.

[0032] This embodiment provides a vision-based UAV synchronous positioning tracking and control system. The problem of UAV synchronous target positioning tracking and control is to use a UAV to locate and track the target, and the UAV synchronous positioning tracking and control system can be installed on the UAV. The block diagram of the system is as follows Figure 1 As shown, it includes a target state estimation unit, a target positioning unit, a UAV state planning unit and a trajectory tracking and control unit;

[0033] The target state estimation unit is used to estimate the target state, predict the position and speed information of the target, and transmit the estimated target state to the target positioning unit;

[0034] When the target is stationary, the target positioning unit obtains the observation image taken by the onboard camera, collects the position of the target in the observation image, and uses the initial two observation images to locate the target according to the pinhole camera model to obtain the first two estimated positions of the target. The target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated position of the target at the current moment, obtain the next estimated position of the target, and send the next estimated position of the target to the drone state planning unit;

[0035] When the target is in motion, an initial estimated state of the moving target is preset, the initial estimated state of the moving target includes the initial estimated position and speed of the moving target, the target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated state of the target at the current moment, and obtains the next estimated state of the target, the next estimated state of the target includes the next estimated position and speed of the target, and the target positioning unit sends the next estimated position of the target to the drone state planning unit;

[0036] The drone state planning unit receives the information sent by the target positioning unit, obtains the next expected position of the drone based on the next estimated position of the target combined with the preset drone operation trajectory, and sends the next expected position of the drone to the trajectory tracking and control unit;

[0037] The trajectory tracking and control unit controls the drone to fly toward the next expected position according to the received next expected position of the drone.

[0038] The target state estimation unit first calculates the target state transfer model: according to different target types, different target state transfer models are used to estimate the state of the target at the next moment. The target state transfer model of the present invention is:

[0039] q k+1 =Fq k +η,η~N(0,Q)

[0040] Among them, q k is the state of the target at the current moment; k represents discrete time, F is the target state transfer matrix, η is the state transfer noise vector of the normal distribution with a mean of 0 and a covariance matrix of Q; q k+1 Represents the state of the target at the next moment, that is, the state of the target estimated by the target state transition model.

[0041] For a stationary target, its state is the three-dimensional position of the target, that is, q = [x q ,y q ,z q ] T , then F is the three-dimensional unit matrix and Q is the three-dimensional diagonal matrix.

[0042] For a steadily moving target, its state is the three-dimensional position and three-dimensional velocity of the target, that is, q = [x q ,y q ,z q ,v qx ,v qy ,v qz ] T, the state of the target at the next moment can be estimated using the three-dimensional constant velocity model in a stable motion state, that is, Among them, T s represents the sampling time, and Q is a six-dimensional diagonal matrix.

[0043] The target positioning unit uses the pinhole camera model to obtain the observation equation of the target: the Central Earth-fixed Frame is selected as the reference coordinate system, and the coordinates of the target in the reference coordinate system are assumed to be It can be converted to the drone body coordinate system through the coordinate transformation matrix. Then transform it into the camera coordinate system in, is the x-axis coordinate value of the target q in the reference coordinate system; is the y-axis coordinate value of the target q in the reference coordinate system; is the coordinate value of the target q on the z-axis in the reference coordinate system; is the x-axis coordinate value of the target q in the UAV body coordinate system; is the y-axis coordinate value of the target q in the UAV body coordinate system; is the coordinate value of the z-axis of the target q in the UAV body coordinate system; is the x-axis coordinate value of the target q in the camera coordinate system; is the y-axis coordinate value of the target q in the camera coordinate system; is the coordinate value of the target q on the z-axis in the camera coordinate system; is the transformation matrix from the reference coordinate system to the drone body coordinate system; It is the transformation matrix from the drone body coordinate system to the camera coordinate system.

[0044] Using the pinhole camera model It can be transformed into the image coordinate system, where f Represents the focal length of the pinhole camera, and obtains the coordinates of the target in the image coordinate system is the x-axis coordinate value of the target q in the image coordinate system; is the y-axis coordinate value of the target q in the image coordinate system; the pixel size of the known image is (d x , d y ), d x is the length of the pixel of the observation image taken by the airborne camera in the x-axis direction, d y is the length of the pixel of the observation image taken by the airborne camera in the y-axis direction, and the pixel coordinate of the center point of the image is is the x-axis coordinate value of the pixel at the center point of the observation image taken by the airborne camera. The y-axis coordinate value of the pixel at the center point of the observation image taken by the airborne camera; convert it to the pixel coordinate system to obtain the coordinate of the target in the pixel coordinate system is the x-axis coordinate value of the target q in the pixel coordinate system; is the y-axis coordinate value of the target q in the pixel coordinate system; thus, the observation equation of the target can be obtained

[0045] Pinhole camera model observation diagram and coordinate transformation relationship as follows Figure 2 As shown in the figure, O R -X R Y R Z R represents the reference coordinate system, O B -X B Y B Z B Represents the coordinate system of the drone body, where the origin of the coordinate system is O B Located in the center of the drone’s quadcopter, X B The Z axis is in the symmetry plane of the drone and parallel to the design axis of the drone, pointing to the front of the nose. B Axis through O B Point and X B O B Y B The plane is vertical, pointing upwards to the drone, X B Axis, Y B Axis, Z B The axis satisfies the right-hand coordinate system, O C -X c Y C Z C Represents the camera coordinate system, where the origin of the coordinate system is O C Located at the optical center of the camera, assuming that the optical center of the camera coincides with the origin of the drone's coordinate system, Z C The axis coincides with the camera optical axis, X c Axis, Y C Axis and image coordinate system X I Axis, Y I The axes are parallel and in the same direction, O I -X I Y I Represents the image coordinate system, where the origin of the coordinate system is O I Located in the center of the image, X I Axis, Y I The axes are parallel to the edges of the image, O P -X P Y P Represents the pixel coordinate system, where the origin of the coordinate system is O PLocated in the lower right corner of the image, X P Axis, Y P Axis and image X I Axis, Y I The axes are parallel and in the same direction. The observation model of the pinhole camera is the projection transmission relationship of the object from the camera coordinate system to the image coordinate system.

[0046] For stationary targets, the position of the target in the pixel coordinate system obtained from the initial two images can be used to make a second estimate of the target position using the least squares method, so that the target converges to the true value more quickly.

[0047] When the drone obtains the first observation image of the target, the target pixel coordinates are obtained through the target detection algorithm. The orientation of the target can be determined by the image and converted into the image coordinate system to obtain Use the pinhole camera model to obtain the coordinates of the target in the camera coordinate system at this time

[0048]

[0049] Convert it to the reference coordinate system and get the coordinates of the target in the first frame of observation image in the reference coordinate system. Since the transformation from image to target coordinates in the reference system is a two-dimensional to three-dimensional transformation, it is impossible to directly obtain q R (1), we need to assume a The value is used as an estimate. Since the planned UAV tracking trajectory gradually converges to a height of r above the target z , a circle with a radius of r. Assuming that the origin of the camera coordinate system coincides with the origin of the drone body coordinate system, the coordinates of the drone in the reference coordinate system are Therefore, the z-axis coordinate of the target in the reference system is the z-axis coordinate of the drone in the reference system minus r z ,Right now This ensures that the drone is above the target. z Flying at a high altitude instead of diving directly towards the target avoids the overlap or parallelism of the first two observation axes, providing favorable conditions for obtaining the second estimated position of the target, and then obtaining the initial coordinate estimate of the target in the reference coordinate system.

[0050] Based on the initial estimated position of the target using the least squares method Get the second estimated position of the target, specifically:

[0051] Assume that the position of the drone when acquiring the bth image is The coordinates of the target in the pixel coordinate system in the bth image Get the coordinates of the target in the image coordinate system b∈(1,2); obtain the unit observation vector pointing from the optical center of the airborne camera to the target and

[0052]

[0053] O C (b) Convert to the reference coordinate system and obtain the unit observation vector in the reference coordinate system

[0054]

[0055] Assume that the second estimated position of the target in the reference coordinate system is The projection of the second estimated position of the target on the observation axis of the onboard camera when acquiring the bth observation image is:

[0056] m R (b) = p R (b)+l(b)O R (b)

[0057] Where l(b) is the distance from the drone to m when acquiring the bth observation image. R (b) Distance:

[0058]

[0059] The relationship between the second estimated position of the target in the reference coordinate system and the camera observation axis is as follows: Figure 3 As shown, the slash dots represent the position of the UAV in the reference coordinate system when acquiring the b-th observation image, the square dots represent the second estimated position of the target in the reference coordinate system, and the solid dots represent the projection of the second estimated position of the target on the observation axis of the onboard camera when acquiring the b-th observation image.

[0060] Calculate the sum of the squares of the distances D from the second estimated position of the target to the projection of the second estimated coordinates of the target on the observation axis of the onboard camera when acquiring the first two observation images:

[0061]

[0062] In order to find the target estimated position value that minimizes the sum of squares of distances, D is used for Find the partial derivative and set it equal to 0, and then sort it out to get:

[0063]

[0064] As long as the observation axes do not overlap or are parallel in the b observations, we can always get The unique solution of is taken as the second estimated position of the target. The schematic diagram of target positioning using the least squares method is as follows Figure 4 As shown in the figure, the five-pointed star represents the actual position of the target, the dot represents the second estimated position of the target calculated by the least squares method, the solid arrow represents the observation axis of the camera at two moments, the dotted arrow represents the flight trajectory of the UAV, and the vertical line from the black dot to the camera observation axis represents the distance from the second estimated position of the target to the observation axis of the onboard camera when the first two observation images are obtained.

[0065] The above method is used for the first two state estimations of stationary target positioning. For moving targets, since the target state changes in real time, a single UAV cannot observe the target twice at the same time. Therefore, when there is only one UAV, the least squares method is not considered for target state estimation. An initial value can be directly set based on experience as the initial estimated state of the target.

[0066] Positioning result optimization:

[0067] After obtaining the initial estimated position of the target, the position of the target is not accurate due to sensor measurement errors, camera installation errors, image position recognition errors, etc., so the unscented Kalman filter is used to optimize the positioning result. At this time, the image obtained by the i-th (i≥3) observation is predicted, updated and estimated respectively.

[0068] The filtering is an unscented Kalman filter; performing an unscented Kalman filter can obtain multiple estimated positions or estimated states of the target in multiple subsequent observation images; wherein, the estimated position of the target at the current moment during the initial filtering is the second estimated position of the stationary target; and the estimated state of the target at the current moment during the initial filtering is the initial estimated state of the moving target.

[0069] The prediction steps include:

[0070] Step S1: The estimated target state after the second observation is known and its covariance matrix P 2 , as well as the covariance matrix Q of the state transition noise vector and the initial covariance matrix R of the observation noise, first Perform an unscented transformation and sample j points according to the following rules to obtain the sigma point set: in, is a point in the sampled sigma point set, n is the dimension of the state quantity, λ=α 2 (n+κ)-n, where α and κ are proportional parameters, set to α=1 and κ=3-n respectively;

[0071] Step S2: Substitute the points in the sigma point set into the state transfer equation to obtain an estimate of the target state: Wherein, f′ represents the state transition model of the target, which is consistent with the description of the target state estimation unit when estimating the state of the target to be tracked;

[0072] Step S 3: Calculate the prior expectation and covariance matrix of the state: in

[0073]

[0074] The update steps include:

[0075] Step S4: Perform an unscented transformation and sample the sigma point set according to the following rules:

[0076] Step S5: Substitute the points in the sigma point set into the observation equation of the target obtained by using the pinhole camera model to obtain the observation value of the target in the pixel coordinate system under ideal conditions: Where y represents the measurement value calculated by the observation equation, which is a two-dimensional vector;

[0077] Step S6: Calculate the expected and covariance matrices of the observations:

[0078] Step S7: Calculate the covariance matrix of states and observations:

[0079] Step S8: Calculate the Kalman gain:

[0080] The estimation steps include:

[0081] Step S9: Calculate the posterior expectation and covariance matrix of the state: Among them, y m Indicates the actual coordinates of the target in the pixel coordinate system in the current observation.

[0082] The above optimization method is used to estimate the states of stationary targets and moving targets respectively. The three-dimensional positioning error of the UAV to the stationary target is as follows: Figure 5 As shown in the figure, the x-axis, y-axis, z-axis positioning errors and the total positioning error of the three axes are shown respectively. It can be seen that with the increase of the number of observations, the positioning error gradually decreases to close to 0 meters; the three-dimensional positioning error of the drone for the moving target is shown in Figure 6As shown in the figure, the x-axis, y-axis, z-axis positioning errors and the total positioning error of the three axes are respectively shown. It can be seen that compared with the positioning results of stationary targets, there are fluctuations in the positioning error of moving targets, but with the increase in the number of observations, the positioning error gradually decreases to close to 0 meters.

[0083] Research has shown that the optimal geometric configuration for drone observation is when the drone hovers around the target. The drone state planning unit plans a space-time curve of drone flight with the optimal observation geometry based on this:

[0084] After performing unscented Kalman filtering on the image obtained from the i-th observation, the target position estimate is obtained: At this time, it is expected to plan a space-time curve circling above the target. Suppose the space-time curve to be planned is of radius r and of height r from the target. z The maximum flight speed of the drone is v p =6(m / s), which can be decomposed into radial velocity v r and the tangential velocity v t , adjust the radial velocity and tangential velocity according to the distance between the drone and the estimated target position; the current drone position is known to be That is, the coordinates of the drone in the reference coordinate system when taking the i-th observation image, is the x-axis coordinate of the drone in the reference coordinate system when taking the i-th observation image, is the y-axis coordinate of the drone in the reference coordinate system when taking the i-th observation image; When taking the i-th observation image, the coordinate of the z-axis of the drone in the reference coordinate system; the position of the target is estimated to be is the x-axis coordinate of the i-th estimated position of the target in the i-th observation image; is the y-axis coordinate of the i-th estimated position of the target in the i-th observation image; is the z-axis coordinate of the i-th estimated position of the target in the i-th observation image; calculates the radial distance between the drone and the target on the XOY plane of the reference coordinate system Design a UAV with radial velocity varying with d xoy Function of change: v r (i) = v p (-tanh(d xoy (i)-r)), v p is the maximum flight speed of the drone, d t is time; δ(i) is d t The angle that the drone needs to turn within this time, δ(i)=ω)i)d t, ω(i) is the angular velocity of the line connecting the i-th estimated position of the UAV and the target rotating around the target, v t (i) is the tangential velocity of the UAV, φ(i) is the phase of the UAV, The desired position of the drone at this time for:

[0085]

[0086]

[0087] Among them, The expected position of the UAV after obtaining the i-th estimated position of the target; The x-axis coordinate of the expected position of the UAV after obtaining the i-th estimated position of the target; The y-axis coordinate of the expected position of the UAV after obtaining the i-th estimated position of the target; The z-axis coordinate of the expected position of the drone after obtaining the i-th estimated position of the target.

[0088] When the UAV is estimated to be far away from the target on the XOY plane of the reference coordinate system, the radial velocity of the UAV is large, causing the UAV to fly as quickly and straight as possible to a position with a radius of r from the target. When the UAV is estimated to be close to the target on the XOY plane of the reference coordinate system, the radial velocity of the UAV is small and the tangential velocity is large, and the UAV will circle around the target.

[0089] The above-mentioned UAV state planning method is used to simulate the scenes of positioning stationary targets and moving targets respectively. The three-dimensional positioning scenes of stationary targets and the trajectory planning results of the UAV are shown in the figure. Figure 7 As shown in the figure, the five-pointed star represents the real position of the target, the hollow circle represents the estimated position of the target, and the surrounding gray area represents the 73% confidence interval of the target estimated position. The target estimated position obtained by the aforementioned first target estimation method and the second least squares estimation method is marked with text in the figure. The solid line represents the actual trajectory of the UAV during flight, the dotted line represents the UAV trajectory planned using the above trajectory planning method, and the dotted line represents the expected trajectory of the UAV, that is, the trajectory circling around the real position of the target. It can be seen from the figure that the actual trajectory of the UAV converges to the planned trajectory and eventually circles around the target. The estimated position of the target gradually approaches the real target and the confidence interval gradually decreases. The three-dimensional positioning scene of the UAV for the moving target and the trajectory planning results are shown in the figure. Figure 8As shown in the figure, the five-pointed star represents the end point of the real trajectory of the moving target, the hollow circle represents the estimated position of the target, the surrounding gray area represents the 73% confidence interval of the target estimate, the solid line represents the actual trajectory of the UAV during flight, the dotted line represents the UAV trajectory planned using the above trajectory planning method, and the dotted line represents the expected trajectory of the UAV, that is, the trajectory circling around the target. It can be seen from the figure that the actual trajectory of the UAV converges to the planned trajectory, and finally circles around the target, and the estimated position of the target gradually approaches the real target.

[0090] The trajectory tracking and control unit first constructs a UAV position dynamics model and a UAV attitude dynamics model, sets the UAV control amount and updates the UAV position dynamics model and the UAV attitude dynamics model, determines the UAV's quad-rotor position control loop based on the PID principle, calculates the UAV's desired attitude and determines the UAV's quad-rotor attitude control loop based on the PID principle; finally, updates the UAV's control amount, calculates the UAV's actual position and attitude based on the updated UAV's control amount, and controls the UAV to fly toward the desired position.

[0091] The trajectory tracking and control unit first constructs a dynamic model for describing the position and attitude of the drone based on Newton's second law:

[0092] Constructing the UAV position dynamics model:

[0093]

[0094] in, is the first derivative of the x-axis coordinate of the drone in the reference coordinate system; is the first derivative of the y-axis coordinate of the drone in the reference coordinate system; is the first derivative of the coordinate of the z-axis of the UAV in the reference coordinate system; ρ is the lift coefficient of the UAV rotor; ω c is the rotation speed of the UAV’s rotor c, c∈(1,2,3,4); m is the mass of the UAV; is the roll angle of the UAV; θ is the pitch angle of the UAV; ψ is the yaw angle of the UAV; k is the air resistance coefficient of the UAV; g is the acceleration of gravity.

[0095] Constructing the UAV attitude dynamics model:

[0096]

[0097] Among them, I xx I is the moment of inertia of the UAV rotating around the x-axis in the UAV body coordinate system; yy I is the moment of inertia of the UAV rotating around the y-axis in the UAV body coordinate system; zzis the moment of inertia of the drone rotating around the z-axis in the drone body coordinate system; L is the distance from any rotor to the center of gravity of the drone, and the distance from each rotor to the center of gravity of the drone is equal; F 1 、F 2 、F 3 、F 4 The lift generated by the motors of the four rotors; I r is the moment of inertia of any rotor, and the moment of inertia of each rotor is equal; ω r is the residual speed, ω r =ω 1 -ω 2 +ω 3 -ω 4 ;γ is the rotational air resistance coefficient;

[0098] Based on the position and attitude dynamics model of the quadrotor drone, the expressions of the control force u1 and control torque u2, u3, u4, u1, u2, u3, u4 are designed as follows:

[0099]

[0100] Among them, u1 controls the position of the drone, and u2, u3, and u4 control the roll angle, pitch angle, and yaw angle of the drone respectively.

[0101] Substituting the control force u1 and the control torque u2, u3, and u4 into the UAV position dynamics model and the UAV attitude dynamics model, we obtain:

[0102]

[0103] Design of quadrotor position control loop based on PID principle:

[0104]

[0105]

[0106]

[0107] Among them, Kp x 、Ki x , Kd x They represent the scale, integral, and differential parameters of the x-axis coordinate of the UAV in the reference coordinate system; Kp y 、Ki y , Kd y They represent the scale, integral, and differential parameters of the y-axis coordinate of the UAV in the reference coordinate system; Kp z 、Ki z , Kd zThey represent the scale, integral, and differential parameters of the z-axis coordinate of the drone in the reference coordinate system; pd ,y pd 、z pd They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the desired position of the UAV in the reference coordinate system; t is the sampling time; They are the x-axis, y-axis, and z-axis accelerations required by the UAV in the reference coordinate system calculated by the PID method.

[0108] Will Substitution In the updated

[0109] Calculate the desired pose of the drone θ d

[0110] Due to the drone’s posture θ is coupled to the position of the drone, so the desired attitude of the drone can be calculated using the quadrotor drone position dynamics model θ d , and ψ d There is no coupling with the drone position and it can be set as needed. The desired attitude of the drone θ d , ψ d They are:

[0111]

[0112]

[0113] ψ d =B

[0114] Among them, B is the preset value.

[0115] According to the calculated desired attitude, the quadrotor attitude control loop is designed by the PID principle:

[0116]

[0117]

[0118]

[0119] in, For drones Proportional, integral, and differential parameters of posture; Kp θ 、Ki θ, Kd θ are the proportional, integral and differential parameters of the θ attitude of the UAV respectively; Kp ψ 、Ki ψ , Kd ψ are the proportional, integral, and differential parameters of the UAV’s ψ attitude, respectively; They are the roll angle, pitch angle, and yaw angle acceleration required by the UAV calculated by the PID method.

[0120] According to the quadrotor position control loop of the UAV, the control torques u2, u3, and u4 are updated, and the nonlinear terms in the attitude dynamics model of the UAV are ignored, and the following is obtained:

[0121]

[0122] Among them, u2', u3', and u4' are the updated control torques respectively; the updated control torques are substituted into the rewritten UAV attitude dynamics model to obtain the updated θ and ψ, substitute the updated control force u1' into the rewritten UAV position dynamics model to obtain the updated x p ,y p and z p , and finally get the actual position and attitude of the UAV to control the UAV to fly towards the desired position.

[0123] In this way, the drone can be controlled. The entire control process is built in the simulink software. The block diagram structure is as follows Fig. 9 As shown, the current position of the drone is known to be x p ,y p 、z p and posture θ, ψ, update the control force u1 and control torque u2, u3, u4 through the speed control loop and attitude control loop, input the quadcopter UAV dynamics model, and get the updated UAV position and attitude. Use the above control method to control the UAV, and the UAV trajectory tracking control error when locating a stationary target is as follows: Fig.10 As shown in the figure, it can be seen that the UAV control error fluctuates at the beginning and finally stabilizes at about 0.4m. The UAV trajectory tracking control error when locating the moving target is as follows Fig.11 As shown, it can be seen that the UAV control error fluctuated at the beginning and finally stabilized at about 0.4m.

[0124] The above-mentioned embodiments only express several implementation methods of the present invention, and the description is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. For ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A vision-based UAV synchronous positioning, tracking and control system, characterized in that: It includes a target state estimation unit, a target positioning unit, a UAV state planning unit and a trajectory tracking and control unit; The target state estimation unit estimates the state of the target to be tracked and transmits the estimated target state to the target positioning unit; When the target is stationary, the target positioning unit obtains the observation image taken by the onboard camera, collects the position of the target in the observation image, and uses the initial two observation images to locate the target according to the pinhole camera model to obtain the first two estimated positions of the target. The target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated position of the target at the current moment, obtain the next estimated position of the target, and send the next estimated position of the target to the drone state planning unit; When the target is in motion, an initial estimated state of the moving target is preset, the initial estimated state of the moving target includes the initial estimated position and speed of the moving target, the target positioning unit uses the next observation image and the target state estimated by the target state estimation unit to filter the estimated state of the target at the current moment, and obtains the next estimated state of the target, the next estimated state of the target includes the next estimated position and speed of the target, and the target positioning unit sends the next estimated position of the target to the drone state planning unit; The UAV state planning unit obtains the next expected position of the UAV based on the next estimated position of the target combined with the preset UAV operation trajectory, and sends the next expected position of the UAV to the trajectory tracking and control unit; The trajectory tracking and control unit controls the drone to fly toward the next expected position according to the received next expected position of the drone.

2. The system according to claim 1, characterized in that The target state estimation unit estimates the target state through a target state transition model; the target state transition model is: q k+1 =Fq k +η, η~N(0,Q) Among them, q k is the target state at the current moment; F is the target state transfer matrix; η is the state transfer noise vector of the normal distribution with a mean of 0 and a covariance matrix of Q; q k+1 is the target state at the next moment, that is, the estimated target state.

3. The system according to claim 1, characterized in that The target positioning unit obtains the observation equation of the target using the pinhole camera model, and then obtains the estimated coordinate value of the target in the reference coordinate system based on the position of the target on the observation image taken by the airborne camera; the observation equation of the target is: Among them, q P is the coordinate of the target in the pixel coordinate system, that is, the pixel position of the target on the observation image taken by the airborne camera, is the x-axis coordinate of the target in the pixel coordinate system, is the y-axis coordinate of the target in the pixel coordinate system; q R is the coordinate of the target in the reference coordinate system, is the x-axis coordinate of the target in the reference coordinate system, is the y-axis coordinate of the target in the reference coordinate system, is the z-axis coordinate of the target in the reference coordinate system; is the transformation matrix from the reference coordinate system to the drone body coordinate system, q B is the coordinate of the target in the UAV body coordinate system; is the transformation matrix from the drone body coordinate system to the camera coordinate system, q C is the coordinate of the target in the camera coordinate system, is the x-axis coordinate of the target in the camera coordinate system, is the y-axis coordinate of the target in the camera coordinate system, is the coordinate of the z-axis of the target in the camera coordinate system; h(*) is the conversion relationship from the reference coordinate system to the pixel coordinate system, f is the focal length of the onboard camera, d x is the length of the pixel of the observation image taken by the airborne camera in the x-axis direction, d y is the length of the pixel of the observation image taken by the airborne camera in the y-axis direction, is the x-axis coordinate of the pixel at the center point of the observation image taken by the airborne camera, It is the y-axis coordinate of the pixel at the center point of the observation image taken by the airborne camera.

4. The system according to claim 3, characterized in that The first two estimated positions of the target are obtained as follows: Based on the first observation image taken by the UAV, the coordinates q of the target in the pixel coordinate system at the current moment are collected. P (1), assuming that the coordinates of the target in the reference coordinate system are R (1) The tracking trajectory of the preset drone gradually converges to a height r above the target. z The radius of the circle is r. At this time, the coordinate of the drone in the reference coordinate system is p R (1) make The initial estimated position of the target is Get the coordinates of the drone in the reference coordinate system when taking the bth observation image And obtain the coordinates of the target in the pixel coordinate system in the bth observation image Convert it to the image coordinate system to get the coordinates of the target in the image coordinate system Get the unit observation vector from the optical center of the airborne camera to the target and O C (b) Convert to the reference coordinate system and obtain the unit observation vector in the reference coordinate system Assume the second estimated position of the target is The second coordinate estimate of the target is the projection of the observation axis of the onboard camera when acquiring the bth observation image. m R (b)=p R (b)+l(b)O R (b) Where l(b) is the distance from the drone to m when acquiring the bth observation image. R (b) Distance: Calculate the sum of the squares of the distances D from the second estimated position of the target to the projection of the second estimated coordinates of the target on the observation axis of the onboard camera when acquiring the first two observation images. Respectively Taking partial derivative, we get get The unique solution is taken as the second estimated position of the target.

5. The system according to claim 1, characterized in that The filtering is an unscented Kalman filter; performing an unscented Kalman filter can obtain multiple estimated positions or estimated states of the target in multiple subsequent observation images; wherein, the estimated position of the target at the current moment during the initial filtering is the second estimated position of the stationary target; and the estimated state of the target at the current moment during the initial filtering is the initial estimated state of the moving target.

6. The system according to claim 5, characterized in that The next desired position of the drone is obtained as follows: in, To obtain the expected position of the drone after the i-th estimated position of the target, is the i-th estimated position of the target in the i-th observation image, d xoy (i) is the radial distance between the UAV and the target on the XOY plane of the reference coordinate system, is the x-axis coordinate of the drone in the reference coordinate system when taking the i-th observation image, v is the y-axis coordinate of the drone in the reference coordinate system when taking the i-th observation image; r (i) is the radial velocity of the UAV, v r (i) = v p (-tanh(d xoy (i)-r)), v p is the maximum flight speed of the drone, d t is time; δ(i) is d t The angle that the drone needs to turn within this time, δ(i) = ω(i)d t , ω(i) is the angular velocity of the line connecting the i-th estimated position of the UAV and the target rotating around the target, v t (i) is the tangential velocity of the UAV, φ(i) is the phase of the UAV, 7. The system according to claim 1, characterized in that The trajectory tracking and control unit first constructs a UAV position dynamics model and a UAV attitude dynamics model, sets the UAV control amount and updates the UAV position dynamics model and the UAV attitude dynamics model, determines the UAV's quad-rotor position control loop based on the PID principle, calculates the UAV's desired attitude and determines the UAV's quad-rotor attitude control loop based on the PID principle; finally, updates the UAV's control amount, calculates the UAV's actual position and attitude based on the updated UAV's control amount, and controls the UAV to fly toward the desired position.

8. The system according to claim 7, characterized in that The UAV position dynamics model is: in, is the first derivative of the x-axis coordinate of the drone in the reference coordinate system; is the first derivative of the y-axis coordinate of the drone in the reference coordinate system; is the first derivative of the coordinate of the z-axis of the UAV in the reference coordinate system; ρ is the lift coefficient of the UAV rotor; ω c is the rotation speed of the UAV’s rotor c, c∈(1,2,3,4); m is the mass of the UAV; is the roll angle of the drone; θ is the pitch angle of the drone; ψ is the yaw angle of the drone; k is the air resistance coefficient of the drone; g is the gravitational acceleration; The UAV attitude dynamics model is: Among them, I xx I is the moment of inertia of the UAV rotating around the x-axis in the UAV body coordinate system; yy I is the moment of inertia of the UAV rotating around the y-axis in the UAV body coordinate system; zz is the moment of inertia of the drone rotating around the z-axis in the drone body coordinate system; L is the distance from any rotor to the center of gravity of the drone, and the distance from each rotor to the center of gravity of the drone is equal; F1, F2, F3, and F4 are the lifts generated by the motors of the four rotors respectively; I r is the moment of inertia of any rotor, and the moment of inertia of each rotor is equal; ω r is the residual speed, ω r =ω1-ω2+ω3-ω4; γ is the rotational air resistance coefficient; According to the UAV position dynamics model and the UAV attitude dynamics model, the control force u1 and the control torque u2, u3, and u4 are set. Among them, u1 controls the position of the drone, u2 controls the roll angle of the drone, u3 controls the pitch angle of the drone, and u4 controls the yaw angle of the drone; Substituting the control force u1 and the control torque u2, u3, and u4 into the UAV position dynamics model and the UAV attitude dynamics model, we obtain:

9. The system according to claim 8, characterized in that The quadrotor position control loop of the UAV is: Among them, Kp x 、Ki x , Kd x They represent the scale, integral, and differential parameters of the x-axis coordinate of the UAV in the reference coordinate system; Kp y 、Ki y , Kd y They represent the scale, integral, and differential parameters of the y-axis coordinate of the UAV in the reference coordinate system; Kp z 、Ki z , Kd z They represent the scale, integral, and differential parameters of the z-axis coordinate of the drone in the reference coordinate system; pd ,y pd 、z pd They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the desired position of the UAV in the reference coordinate system; t is the sampling time; They are the x-axis, y-axis, and z-axis accelerations required by the drone in the reference coordinate system calculated by the PID method; Will Substitution In the updated The desired attitude of the drone includes the desired roll angle of the drone The desired pitch angle θ of the drone d and the desired yaw angle ψ of the drone d , Among them, B is the preset value; According to the desired attitude of the UAV obtained and based on the PID principle, the attitude control loop of the UAV's quadrotor is determined. in, For drones Proportional, integral, and differential parameters of posture; Kp θ 、Ki θ , Kd θ are the proportional, integral and differential parameters of the θ attitude of the UAV respectively; Kp ψ 、Ki ψ , Kd ψ are the proportional, integral, and differential parameters of the UAV’s ψ attitude, respectively; They are the roll angle, pitch angle, and yaw angle acceleration required by the UAV calculated by the PID method.

10. The system according to claim 9, characterized in that According to the quadrotor position control loop of the UAV, the control torques u2, u3, and u4 are updated, and the nonlinear terms in the attitude dynamics model of the UAV are ignored, and we get Among them, u2', u3', and u4' are the updated control torques respectively; the updated control torques are substituted into the rewritten UAV attitude dynamics model to obtain the updated θ and ψ; Substitute the updated control force u1′ into the rewritten UAV position dynamics model to obtain the updated x p ,y p and z p , and finally get the actual position and attitude of the UAV to control the UAV to fly towards the desired position.

Images