A vision-based UAV synchronous positioning, tracking and control system

Abstract

The present invention discloses a vision-based synchronized positioning, tracking, and control system for unmanned aerial vehicles (UAVs), belonging to the field of UAV positioning and tracking. The system includes a target state estimation unit, a target positioning unit, a UAV state planning unit, and a trajectory tracking and control unit. The system estimates the state of a target to be tracked to obtain an estimated target state; presets an initial estimated state of a moving target or obtains the first two estimated positions of a stationary target; uses the next observed image and the estimated target state to filter the target's current estimated position or estimated state to obtain the target's next estimated position or estimated state; obtains the UAV's next desired position; and finally, controls the UAV to fly toward the desired position based on the next desired position. The path planned by the present invention is more conducive to the controller's stable execution of the control process.

Description

Technical Field

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

[0002] The problem of synchronized target positioning, tracking and control of UAVs is to locate and track a target using one or more UAVs. These UAVs act as mobile sensors and use certain information about the target to find the state of an 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 UAVs.

[0003] UAV synchronous 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 agriculture, 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 and passive positioning. Active positioning and tracking systems use active sensors such as radar to radiate high-power electromagnetic waves into the air. Active positioning is primarily based on laser ranging models. Its advantages are simplicity and high positioning accuracy, but it requires a laser rangefinder, a heavy payload, and the active emission of electromagnetic waves, which compromises its concealment. Passive positioning and tracking systems use passive sensors such as infrared and sonar sensors to detect targets. These do not emit electromagnetic waves, resulting in good concealment and resistance to enemy detection. Passive positioning methods include target positioning based on image matching patterns and target positioning based on imaging models. Image matching methods suffer from poor real-time image matching and limited access to reference images. Visual sensors, on the other hand, are small and low-power, making them suitable for use on drones for positioning. However, they are significantly affected by system errors and suffer from insufficient positioning accuracy.

[0005] Existing UAV synchronous positioning, tracking, and control systems still have some problems. 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 a few types of sounds trained by neural networks, and the types of targets that can be tracked are limited. In addition, 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. This system only tracks targets by controlling the rotation of the gimbal, resulting in a limited tracking range and lack of flexibility. In addition, it uses a binocular camera for ranging, which requires higher accuracy of calibration parameters, resulting in high maintenance costs 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 in the present invention is as follows:

[0008] The present invention discloses a vision-based UAV synchronous positioning tracking and control system, which includes 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 stationary state, the target positioning unit obtains an observation image taken by an 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 to obtain the next estimated position of the target, and sends 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, wherein the initial estimated state of the moving target includes an initial estimated position and speed of the moving target. The target positioning unit filters the estimated state of the target at the current moment using the next observation image and the target state estimated by the target state estimation unit to obtain a next estimated state of the target, wherein the next estimated state of the target includes a next estimated position and speed of the target. The target positioning unit sends the next estimated position of the target to the UAV 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 and 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 ensures 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 UAV planned trajectory 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 the various errors existing 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 Radial position error of the UAV 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 This 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 pinhole camera model observation diagram and coordinate transformation relationship 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] Figure 9 It is the simulink simulation diagram of the control unit;

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

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

[0031] The present invention will be further described and illustrated below in conjunction with specific embodiments. The embodiments are merely illustrative of the present disclosure and do not limit its scope. The technical features of the various embodiments of the present invention may be combined accordingly, provided that there is no conflict between them.

[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. 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 an 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 to obtain the next estimated position of the target, and sends 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, wherein the initial estimated state of the moving target includes an initial estimated position and speed of the moving target. The target positioning unit filters the estimated state of the target at the current moment using the next observation image and the target state estimated by the target state estimation unit to obtain a next estimated state of the target, wherein the next estimated state of the target includes a next estimated position and speed of the target. The target positioning unit sends the next estimated position of the target to the UAV state planning unit.

[0036] The UAV state planning unit receives the information sent by the target positioning unit, and 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;

[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 transition model: according to the different target types, different target state transition models are used to estimate the state of the target at the next moment. The target state transition 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 mean 0 and covariance matrix 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 identity matrix and Q is the three-dimensional diagonal matrix.

[0042] For a steady moving target, its state is the target's three-dimensional position and three-dimensional velocity, 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 steady 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 into 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 target q on the z axis 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; 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; from this, 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 is 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 pinhole camera observation model 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 As an estimate, 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 level 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] Among them, l(b) is the distance from the UAV 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 in the figure, the slashed dots represent the position of the UAV in the reference coordinate system when the b-th observation image is obtained, 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 the b-th observation image is obtained.

[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 coordinate estimate 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 to Find the partial derivative and set it equal to 0, and we can 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, it is impossible to observe the target twice at the same time using a single UAV. 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 target position may not be accurate due to sensor measurement errors, camera installation errors, image position recognition errors, etc. Therefore, 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 unscented Kalman filtering 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 P2, 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 unscented transformation and sample j points according to the following rules to obtain the sigma point set: in, is a point in the sigma point set obtained by sampling, 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 transition 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 S3: Calculate the prior expectation and covariance matrix of the state: in

[0073]

[0074] The update steps include:

[0075] Step S4: Perform 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 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 state and observation:

[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 UAV for the moving target is as follows 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, the positioning error of moving targets fluctuates, but with the increase of 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 is circling around the target. The drone state planning unit accordingly plans a space-time curve of the drone flight with the optimal observation geometry:

[0084] After performing unscented Kalman filtering on the image obtained by the i-th observation, the target position estimation is obtained At this time, it is expected to plan a space-time curve that circles above the target. Let the space-time curve to be planned be 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 tangential velocity v t , adjust the radial velocity and tangential velocity according to the distance between the UAV and the target estimated position; the current UAV 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 UAV 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 UAV, d t is time; δ(i) is d t The angle that the drone must 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 UAV's expected position after obtaining the i-th estimated position of the target; The z-axis coordinate of the expected position of the UAV 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 UAV's radial velocity is large, causing the UAV to fly as quickly and straight as possible to a position with a radius 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 UAV's radial velocity is small and the tangential velocity is large, causing the UAV to circle around the target.

[0089] The above-mentioned UAV state planning method is used to simulate the scenarios of positioning stationary targets and moving targets respectively. The three-dimensional positioning scenario of the stationary target 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 drone during flight, the dotted line represents the drone trajectory planned using the above trajectory planning method, and the dotted line represents the expected trajectory of the drone, 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 drone 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 moving target and the trajectory planning results of the drone 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 eventually circles around the target. 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 quantity and updates the UAV position dynamics model and the UAV attitude dynamics model, determines the UAV's quadrotor position control loop based on the PID principle, calculates the UAV's desired attitude and determines the UAV's quadrotor attitude control loop based on the PID principle; finally, updates the UAV's control quantity, calculates the UAV's actual position and attitude based on the updated UAV control quantity, and controls the UAV to fly toward the desired position.

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

[0092] Constructing a UAV position dynamics model:

[0093]

[0094] in, is the first derivative of the x-axis coordinate of the UAV in the reference coordinate system; is the first derivative of the y-axis coordinate of the UAV 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 due to 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; 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;

[0098] Based on the quadrotor drone position and attitude dynamics model, the control force u1 and control torque u2, u3, u4, u1, u2, u3, u4 expressions 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 torques 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 z They represent the scale, integral, and differential parameters of the z-axis coordinate of the UAV in the reference coordinate system; x 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 with 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's position and it can be set as needed. θ 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 using 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 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 UAV attitude dynamics model are ignored, resulting in:

[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 Figure 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 obtain 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: Figure 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 Figure 11 As shown in the figure, it can be seen that the drone control error fluctuates at the beginning and finally stabilizes at around 0.4m.

[0124] The above-described embodiments merely illustrate several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. Persons skilled in the art will readily appreciate that variations and modifications may be made without departing from the scope of the present invention, all of which fall within the scope of protection of the present invention.

Claims

1. A vision-based UAV synchronous positioning, tracking and control system, characterized in that: It includes target state estimation unit, target positioning unit, UAV state planning unit and 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 an 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 to obtain the next estimated position of the target, and sends 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, wherein the initial estimated state of the moving target includes an initial estimated position and speed of the moving target. The target positioning unit filters the estimated state of the target at the current moment using the next observation image and the target state estimated by the target state estimation unit to obtain a next estimated state of the target, wherein the next estimated state of the target includes a next estimated position and speed of the target. The target positioning unit sends the next estimated position of the target to the UAV 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 and 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 UAV to fly toward the next desired position according to the received UAV; The target positioning unit uses a pinhole camera model to obtain the observation equation of the target, and then obtains the estimated coordinate value of the target in the reference coordinate system based on the position of the target in 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 in the y-axis direction of the observation image taken by the airborne camera, is the x-axis coordinate of the pixel at the center point of the observation image taken by the airborne camera, The y-axis coordinate of the pixel at the center point of the observation image taken by the airborne camera.

2. The system according to claim 1, wherein: 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 with a normal distribution that satisfies the mean of 0 and the 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, wherein: 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 q R (1), The tracking trajectory of the preset drone is gradually converging to a height r above the target. z And the radius is r, the coordinate of the drone in the reference coordinate system is p R (1), make The initial estimated position of the target 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 b-th 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) Among them, l(b) is the distance from the UAV 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 coordinate estimate of the target on the observation axis of the onboard camera when acquiring the first two observation images. respectively Taking the partial derivative, we get get The unique solution is taken as the second estimated position of the target.

4. The system according to claim 1, wherein: The filtering is an unscented Kalman filter; performing unscented Kalman filtering 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.

5. The system according to claim 4, characterized in that The next desired position of the UAV is obtained as follows: in, To obtain the target's i-th estimated position, the expected position of the UAV is: 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 UAV, d t is time; δ(i) is d t The angle that the drone must 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, 6. The system according to claim 1, wherein: The trajectory tracking and control unit first constructs a UAV position dynamics model and a UAV attitude dynamics model, sets the UAV control quantity and updates the UAV position dynamics model and the UAV attitude dynamics model, determines the UAV's quadrotor position control loop based on the PID principle, calculates the UAV's desired attitude and determines the UAV's quadrotor attitude control loop based on the PID principle; finally, updates the UAV's control quantity, calculates the UAV's actual position and attitude based on the updated UAV control quantity, and controls the UAV to fly toward the desired position.

7. The system according to claim 6, characterized in that The UAV position dynamics model is: in, is the first derivative of the x-axis coordinate of the UAV in the reference coordinate system; is the first derivative of the y-axis coordinate of the UAV 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 due to gravity; 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, set the control force u1 and control torque u2, u3, u4, 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 torques u2, u3, and u4 into the UAV position dynamics model and the UAV attitude dynamics model, we obtain:

8. The system according to claim 7, 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 UAV in the reference coordinate system; x 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; The x-axis, y-axis, and z-axis accelerations required by the UAV in the reference coordinate system are calculated using 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 UAV d , ψ d =B Among them, B is the preset value; According to the desired attitude of the UAV obtained and based on the PID principle, the UAV's quadrotor attitude control loop 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 acceleration required by the UAV calculated by the PID method.

9. The system according to claim 8, 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 UAV attitude dynamics model are ignored. 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