An optimal control method for UAV visual perception

Abstract

The present invention discloses a control method for optimal visual perception of unmanned aerial vehicles (UAVs). This method addresses the key issues of UAV mission execution—the stability and efficiency of UAV control—and provides an optimal perception control method for UAVs based on the demand for perception stability during UAV flight. The method includes the design of a UAV visual perception algorithm and a control algorithm. Visual perception positioning uses ORB-SLAM3 to obtain the UAV's position and attitude information. The control algorithm uses a nonlinear model predictive control algorithm to obtain the UAV control instruction sequence based on the UAV's current position and attitude, positioning information, and the desired trajectory, thereby completing the control of the UAV and ensuring that the UAV's perception quality meets the control requirements.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous control of unmanned aerial vehicles (UAVs), and in particular relates to a control method for optimizing the visual perception of UAVs. Background Art

[0002] A drone is an unmanned aerial vehicle (UAV) that uses a ground control station and a remote controller, controlled via wireless communication equipment, and capable of autonomous flight. UAVs first appeared in the 1920s and were used in warfare. In 1956, the first true quadcopter, the Convertawings Model "A," successfully completed a test flight, but it fell short of the military's altitude and speed requirements. In the early 1990s, rapid advances in electronics and computing technology led to breakthroughs in hardware integration and computing power. MEMS-based inertial navigation systems began to be used in multi-rotor UAVs, enabling them to achieve relatively stable flight. However, due to the continued presence of significant sensor noise, truly stable multi-rotor aircraft did not emerge until 2004, after the maturity of noise reduction and attitude control algorithms. In 2004, the US company Spectrolutions launched the Dragonflyer series of multi-rotor UAVs and, in 2006, a preliminary version of its aerial photography gimbal. In 2010, the French company Parrot launched the AR.Drone quad-rotor drone, ushering in the era of consumer-grade multi-rotor drones.

[0003] Quadrotors (UAVs) achieve a variety of motion modes, including hovering, vertical takeoff and landing, pitching, and rolling, by adjusting the rotational state of their rotor motors. Multirotor UAVs, due to their simple structure, small size, ease of operation, and excellent spatial flexibility, are widely used in a variety of civilian and military tasks, including aerial photography and 3D reconstruction. However, the highly coupled and highly flexible postures of UAVs make it difficult for most methods to handle the combined effects of agile flight, such as nonlinear dynamics and aerodynamics. These challenges make controlling UAVs significantly more challenging than conventional control systems (such as unloaded motor speed control systems). In recent years, increases in hardware computing power and optimization of basic algorithm complexity have enabled the implementation of demanding control algorithms based on modern control theory on airborne computing devices. Among these, model predictive control (MPC) has garnered considerable attention due to its precise performance in high-speed trajectory tracking.

[0004] Furthermore, in most research on pure multirotor UAV control, perception and localization are achieved using motion capture systems, which is virtually impossible to achieve in practical applications. Because camera motion can negatively impact positioning and introduce noise into the acquisition of physical information such as the robot's position, velocity, and acceleration, onboard vision systems cannot always be equated with motion capture systems. Currently, thanks to the massive development of perception algorithms and the significant advantages of visual sensing equipment in terms of weight, cost, size, and power consumption, vision-based perception and localization is the mainstream approach for practical UAV applications. However, this mainstream approach is significantly affected by the environment and the position and motion of the onboard equipment, resulting in significant limitations. This demonstrates that high-precision and high-stability perception and localization for multirotor UAVs can significantly enhance control. However, the quality of control results and the rationality of decision-making can in turn affect the effectiveness of perception and localization. For example, to navigate a narrow gap while simultaneously using an onboard camera to localize the UAV, it is necessary to ensure that the gap is visible at all times. Therefore, how to reasonably control, ensure that the visual sensing equipment always points to the area with rich texture, give the controller stable positioning feedback, and make full use of the dynamic flexibility of the quadrotor drone is very important for the overall scheduling of drones based on indoor positioning.

[0005] Given the acquired positioning data, how to rationally control it, ensuring that the visual sensor always points to a textured area, providing the controller with stable positioning feedback, and fully utilizing the quadrotor's dynamic flexibility, is crucial for the overall scheduling of UAVs based on indoor positioning. To address this issue, for example, recent research has demonstrated that model predictive control enables UAVs to track pre-defined nonlinear trajectories at speeds up to 20 m / s. In the relatively classical field of control, the most advanced non-predictive method, the differential-flatness-based controller (DFBC), has also demonstrated strong performance in autonomously tracking agile trajectories. When tracking a trajectory with a speed of 12.9 m / s and an acceleration of 2.1 g, the DFBC exhibits an average position tracking error of only 6 cm.

[0006] Currently, common drone control algorithms are categorized into predictive and non-predictive methods. The former focuses solely on the current trajectory for control, resulting in poor control effectiveness in extreme situations. The latter, on the other hand, incorporates multiple future trajectories into the controller, consuming significant computational resources.

[0007] In view of the advantages and disadvantages listed above, different solutions are proposed. For drone control algorithms and positioning solutions, the existing solutions are as follows:

[0008] Controller solution 1: differentially flat controller: The literature (Mellinger D, Kumar V. Minimum snaptrajectory generation and control for quadrotors [C] / / 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 2520-2525.) reveals that quadrotors are differentially flat systems. Based on this characteristic, given a time-parameterized three-dimensional path, the reference attitude, angular acceleration, and linear acceleration can be obtained. The differentially flat controller can still meet the required tracking performance under relatively high speed requirements. As the expected flight speed increases further, quadrotors need to overcome aerodynamic effects, including drag, aerodynamic torque

[16] , and thrust variation. Since aerodynamics does not affect differential flatness, scholars use a first-order aerodynamic model with a feedforward term from the reference trajectory to improve trajectory tracking performance.

[0009] Controller solution 2: The literature (Bangura M, Mahony R. Real-time Model Predictive Control for Quadrotors [J]. IFAC Proceedings Volumes, 2014, 47 (3): 11773-11780.) first used linear MPC to control the UAV, and was only used for the position control of the UAV. In recent years, thanks to the development of hardware and nonlinear optimization solvers, it has become feasible to run the NMPC algorithm of nonlinear full dynamic models on airborne computing devices. Therefore, in recent years, some studies have begun to use full nonlinear dynamic models. The literature (Foehn P, Romero A, Scaramuzza D. Time-optimal planning for quadrotor waypoint flight [J]. Science Robotics, 2021, 6 (56): eabh1221.) uses single rotor thrust as the input of NMPC, which can make full use of the power system of the quadrotor UAV.

[0010] UAV positioning solution: The literature (Mur-Artal R, Tardos J D. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras [J]. IEEE Transactions on Robotics, 2017, 33 (5): 1255-1262.) proposed ORB-SLAM3, which is the first system that can use monocular, binocular and RGB-D cameras for vision, visual inertial navigation fusion and multi-map SLAM.

[0011] UAV perception optimization solution: The literature (Spica R, Robuffo Giordano P, Chaumette F. Coupling Active Depth Estimation and Visual Servoing Via a Large Projection Operator [J]. The International Journal of Robotics Research, 2017, 36(11): 1177-1194.) combines visual servoing with active motion mechanisms and proposes a solution to modify the camera trajectory to improve the quality of reconstruction. However, to achieve this effect, it is necessary to track feature trajectories within the image plane and utilize the ineffective space of the visual servoing task. In addition, the robot's underactuated characteristics are not taken into account in trajectory planning. Due to the conflict between the robot's motion and the perception target, this will greatly affect the performance of the robot's overall motion. Summary of the Invention

[0012] In light of this, the present invention addresses the key issues of drone mission execution—stable and efficient drone control. Based on the demand for perceptual stability during flight, it provides an optimized perceptual control method for drones. Compared to previous control methods, this invention provides a stable, efficient, and reliable solution for drone control while maintaining the reliability of positioning data.

[0013] A control method for optimizing visual perception of a UAV, comprising:

[0014] The first step is to analyze the UAV system. Based on the UAV parameters and the angular velocity inner loop of the flight controller, the full-state kinematic and dynamic equations of the UAV are solved.

[0015] The second step is to perform visual positioning and mapping of the surrounding environment, thereby obtaining the current position and attitude information as the state of the UAV system to participate in the control solution. In addition, the position and velocity information of each feature point used to describe the current visual positioning quality are obtained. The UAV's visual positioning quality Z is added as a new state quantity to the UAV's full-state kinematic equation, and a cost function is constructed to comprehensively calculate the control quantity required to control the UAV, thereby completing the UAV's tracking of the planner's trajectory.

[0016] Wherein, the cost function is expressed as:

[0017]

[0018] Among them, Q and R represent the state quantity and the penalty term for the system control input quantity respectively, X k represents the state quantity at time k, X k,r represents the expected state quantity at time k, U k represents the system control input at time k, U k,r represents the expected system control input at time k; Q p Represents the weight of the variable that describes the perceptual quality of each feature point in the image in the cost function;

[0019] Let s = [u, v] T Represents the coordinates of the feature points in the image coordinate system. According to the classic pinhole camera model, we have:

[0020]

[0021] In the above formula, f x , f x Respectively represent the product of the camera's zoom multiple on the u and v axes of the image and the focal length; C p f =[ C p fx , C p fy , C p fz ] T Represents the three-axis coordinates of the feature point in the camera coordinate system;

[0022]

[0023] C p f =( W q B B q C ) -1 ☉( W p f -(W q B ☉ B P C + W p B ))

[0024] Where ⊙ represents quaternion multiplication; W p B Indicates the position of the body coordinate system in the world coordinate system. B p C Indicates the position of the camera coordinate system in the body coordinate system. B q C and W q B The rotation posture between the camera coordinate system to the body coordinate system, and the body coordinate system to the world coordinate system;

[0025] Among them, ω x ,ω y ,ω z Indicates the three-axis angular velocity of the UAV; C v W Indicates the three-axis speed of the drone in the body coordinate system;

[0026] The visual positioning quality

[0027] Preferably, the ORB-SLAM algorithm is used to perform visual positioning and mapping of the surrounding environment.

[0028] Preferably, the state quantity includes the three-axis position of the drone in the world coordinate system W p x , W p y , W p z , three-axis speed W v x , W v y , W v z and the quaternion q w ,q x ,q y ,q z .

[0029] Preferably, the control output of the drone includes the acceleration in the z-axis direction B a Z and the three-axis angular velocity ω x ,ω y ,ω z .

[0030] The present invention has the following beneficial effects:

[0031] The present invention provides a control method that ensures the visual positioning perception and positioning effect while maintaining the stability and efficiency of drone control. Compared with the more mainstream control algorithms before, the method provided by the present invention can, on the one hand, effectively avoid the loss of control of the drone under extreme working conditions for the drone status at multiple moments in the future, so that the drone can perform tasks in a relatively stable posture; on the other hand, the present invention takes into account the quality of important feature point information during visual positioning, comprehensively considers the position and speed of the feature points in each frame, and incorporates them into the control method to greatly improve the quality of visual perception during the control process. The planned path and positioning information input by the upper-level controller can control the drone to achieve the effect of trajectory following while ensuring the quality of visual positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 This is the system coordinate system involved in the present invention.

[0033] Figure 2 Schematic diagram of the finite state machine for running the algorithm of the present invention.

[0034] Figure 3 This is a schematic diagram of the core algorithm principle of the present invention.

[0035] Figure 4 This is a schematic diagram of the visual positioning process used in the present invention.

[0036] Figure 5 This is the interaction logic of each link in the simulation environment of the present invention. DETAILED DESCRIPTION

[0037] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0038] The present invention provides a control method for maintaining the stability and efficiency of UAV control while ensuring the visual positioning perception and positioning effect. The method comprises the following steps:

[0039] The first step is to analyze the UAV system. Based on the UAV's mass, size and other parameters, combined with the flight controller's angular velocity inner loop, the full-state kinematic equations and dynamic equations of the UAV are solved.

[0040] In the second step, the drone uses the ORB-SLAM algorithm to visually locate and map its surroundings. This provides the current position and attitude information, which are then used as state variables in the drone system's control solution. Furthermore, the position and velocity of each feature point are obtained to describe the current visual positioning quality. The drone's visual positioning quality, Z, is added as a new state variable to the drone's full-state kinematic equations, and a new cost function is constructed to comprehensively calculate the control variables required to control the drone, thereby enabling the drone to track the planner's trajectory.

[0041] The algorithm of this invention is applicable to Ubuntu 20.04, written in C++14, compiled with CMake 3.28.0 and GCC 8.1.0, uses ROS as a software tool to schedule various tasks, and uses CasADi as the main optimization solver. The algorithm includes:

[0042] Using the visual positioning algorithm, ORB features are extracted from the two images. For each left feature, the corresponding epipolar line in the right image is searched to solve the initial position of the spatial map point, and then the current position and attitude of the drone are calculated.

[0043] A drone control algorithm is adopted, and the drone speed, position, attitude, and visual perception quality are used as the drone's state quantities, and the three-axis angular velocity and acceleration are used as the drone's control quantities. An optimal problem is established for solution, and the drone control quantity is obtained by solving it, while trying to ensure the best quality of visual perception.

[0044] The visual positioning algorithm uses IMU pose prior to help binocular positioning perform data association, and the inertial positioning based on EKF realizes visual pose feedback and ensures the output frequency. At the same time, the present invention adopts a loosely coupled visual inertial fusion algorithm to solve the problem of discontinuous pose output. For VSLAM, the present invention adopts an adaptive algorithm to dynamically adjust the number of feature point extractions. During the operation of the system, the same map points will be observed in several frames. The back-end optimization constructs the least squares problem according to the bundle adjustment method as follows, uses the G2O optimization library for optimization to obtain accurate pose information and generates messages in the nav_msgs::Odometry format through ROS for publication.

[0045] The UAV control algorithm uses the nonlinear model predictive control algorithm (NMPC) to control the UAV. The present invention uses bold lowercase letters to represent vectors and bold uppercase letters to represent matrices. Otherwise, they represent scalars. The present invention involves three coordinate systems, namely: the world coordinate system F W :{x W ,y W ,zW}, where z W Pointing in the direction opposite to the acceleration due to gravity; body coordinate system F B :{x B ,y B ,z B}, where x B Pointing to the direction directly in front of the aircraft, z B Pointing to the direction of the body's thrust; camera coordinate system F C :{x C ,y C ,z C}, the posture is consistent with the body coordinate system, and there is a fixed offset in the position. B ,y B ,z B The rotation angles corresponding to the axes are the roll, pitch, and yaw angles φ, θ, ψ of the drone, respectively.

[0046] During the movement of the drone, according to the planner and actual use requirements, the quantities that need to be controlled are mainly the position, velocity and attitude of the drone, which can be represented by three-axis position, three-axis velocity and quaternion respectively. Therefore, the full state variables of the drone can be expressed as:

[0047] X=[ W p x , W p y , W p z , W v x , W v y , W v z ,q w ,q x ,q y ,q z ] T (1)

[0048] The controller uses thrust and three-axis angular velocity to control the drone. Since it is difficult to directly obtain the thrust of the drone during control, in most cases the ideal z-axis acceleration is first obtained, and then the thrust required by the drone is calculated based on the z-axis acceleration. Therefore, the control output of the drone can be expressed as:

[0049] U=[ B a Z ,ω x ,ω y ,ω z ] T (2)

[0050] In terms of translational kinematics, the equation can be simply expressed as:

[0051]

[0052]

[0053] In terms of rotational kinematics, it is mainly necessary to analyze the last four items of the state quantity X, that is, the attitude of the drone, and obtain its rate of change, which is:

[0054]

[0055] The full-state kinematic equation of the UAV can be obtained as follows:

[0056]

[0057] The position vector of the center of mass in the world coordinate system is expressed as W p represents that its second-order derivative is the acceleration of the center of mass in the world coordinate system B a Z The force on the system is W gravity in the z-direction, and B The sum of the forces on each rotor in the z direction is T. The Newtonian equation of motion governing the acceleration of the center of mass is:

[0058] m B a Z =-mg W z+T B z (7)

[0059] This will allow us to calculate the thrust required by the drone.

[0060] Given a reference trajectory, the cost function is the time-dependent error between the predicted state at each prediction moment and the reference state at the corresponding prediction moment. This means that within each control cycle, multiple reference points along the reference trajectory are used as controller inputs to adjust the controller output.

[0061] To perform numerical optimization to solve the trajectory tracking problem faced by the aforementioned drone and obtain appropriate control commands, the drone's state X and input U must be discretized into N equal intervals over the time span τ∈[t,t+h], with each interval size dt = h / N. Here, h represents the total prediction time, dt is the prediction step size, and N is the number of predictions. Based on the full state dynamics equations of the drone, a constrained nonlinear optimization problem can be derived.

[0062]

[0063] xT k+1 =f(X k ,U k ),X0=X init

[0064] B ω∈[ B ω min , B ω max ],U∈[U min ,U max ](8)

[0065] Among them, X N,r Represents the desired state of the UAV when predicting the final state. The state equation of the system is X k+1 =f(X k ,U k ) is a nonlinear state equation. The state variable X of the system is a ten-dimensional state variable consisting of the position, velocity, and attitude of the drone in the world coordinate system (expressed in quaternion form); and the control instruction U of the system is the acceleration of the drone in the z-axis direction of the body coordinate system. B a Z , a four-dimensional variable composed of the three-axis angular velocity in the body coordinate system. init Represents the current state estimate of the UAV when solving the finite time optimal control problem, U min ,U max They represent the input restrictions, including the drone thrust restrictions and the drone angular velocity restrictions, which are conducive to improving the stability of the drone. Q and R represent the penalty terms of state and input respectively. According to the representation of X and U, Q and R are:

[0066] Q = diag {Q px ,Q py ,Q pz ,Q vx ,Q vy ,Q vz ,Q qw ,Q qx ,Q qy ,Q qz}

[0067]

[0068] For autonomous UAV flight, the perception of the surrounding environment and the planning and execution of UAV motion are two essential parts. Many works have studied the two separately, but rarely have they been considered as a unified problem.

[0069] In fact, camera motion can strongly affect the quality of visual perception. On the one hand, if the camera leaves the viewing angle where the feature points are densely packed, it may not be able to extract enough feature points, thus reducing the positioning stability. On the other hand, if a series of requirements for visual perception positioning are taken into account (such as presenting as many high-texture areas as possible in the field of view, reducing high-frequency jitter caused by motion and thus reducing camera shake), the effect of visual perception positioning can be significantly improved.

[0070] In order to take the effect of perception into account in control, a state variable describing the current perception quality needs to be added to the state equation, as follows:

[0071] Z=f p (X,U,σ) (10)

[0072] Where σ represents a series of camera parameters, including the focal length and field of view of the camera. The present invention has defined a cost function L related to control a (X,U)=||X k -X k,r ||Q+||U k -U k,r ||R, similarly, we can construct a cost function L related to perception p (Z). Therefore, after adding the perception term, the control of the drone can be expressed as the following optimization problem:

[0073]

[0074] In the camera coordinate system, the feature points are C p f =[ C p fx , C p fy , C p fz ] T In the image captured by the camera, the feature point is projected to a point s=[u,v] on the image plane. T According to the classic pinhole camera model, we have:

[0075]

[0076] Among them, f x , f x They represent the product of the camera's zoom multiple on the u and v axes of the image and the focal length, respectively, and are camera intrinsic parameters.

[0077] First, in order to ensure that the visual perception is stable enough, the feature points C p fThe projection s should be as close to the center of the image as possible. This is necessary, firstly, to ensure that feature points remain in the camera image when the drone experiences jitter or oscillation due to external interference. Second, the periphery of the image often exhibits significant distortion, especially for cameras with a large field of view. While there is currently extensive literature on this distortion model, including techniques for estimating its parameters and performing camera calibration to compensate for it, this compensation is never perfect, and inaccuracies can still occur. Therefore, locating feature points as close to the center of the image as possible can theoretically improve the stability of visual perception.

[0078] In addition, it is necessary to reduce the speed of the feature points on the image plane to ensure the stability of visual perception. Assuming that the feature points in the image are stationary, in order to express the speed of the projected point on the image plane as a function L that can take the state X of the quadcopter and the control command U as input p (Z), and differentiating it with respect to time, we get:

[0079]

[0080] The distance between the feature point projection and the image center and the speed of the feature point projection on the image plane can describe the perceptual quality. Variables that describe the cost function of perceived quality.

[0081] It should be noted that Z is a function of the quadcopter state X and input U. Among them, s is mainly provided by the positioning algorithm and can be considered as a known quantity, while It can be expressed as:

[0082]

[0083] in, are all known quantities, and C p f , It can be expressed as:

[0084] c p f =( W q B B q C ) -1 ⊙( W p f -( W q B ⊙ B p C + W p B ) (15)

[0085]

[0086] According to formulas (15) and (16), B q C and W q B The rotation posture between the camera coordinate system to the body coordinate system and the body coordinate system to the world coordinate system is part of the state quantity X. W p B It represents the position information of the UAV, which is also part of the state quantity X. The three-axis speed of the UAV in the body coordinate system is C v W It can be obtained by the first-order derivative of the state quantity X, so the perception quality Z can be considered as a function of the quadrotor drone state X and input U.

[0087] In formula (11), L p (Z) is specifically expressed as:

[0088] L p (Z)=||Z||Q p (17)

[0089] Among them, Q p =diag{Q u ,Q v ,Q du ,Q dv}It can describe the weight of variables in the cost function that can describe the position and speed of each feature point in the image and the perceived quality.

[0090] Therefore, the final cost function can be expressed as:

[0091]

[0092] The reference value of Z is the zero vector, which represents the expectation that all feature points are located as close as possible to the center of the image plane captured by the camera, and that the velocity is zero. The reference values ​​of state and input are the desired pose calculated by the trajectory planner.

[0093] Example:

[0094] This invention provides a control method that maintains smooth and efficient drone control while ensuring visual positioning. First, the drone's current position and attitude are acquired using visual positioning information. Then, combining the desired future position and velocity, as determined by a trajectory planner, with the trajectory predictions of feature points obtained by the drone's visual sensors, a nonlinear model predictive control algorithm is used to determine the drone's control variables and control the drone.

[0095] 1. System Coordinate System Description

[0096] The rotation matrix from the world coordinate system W to the body coordinate system B can be expressed as R(q)=[x B ,y B ,z B ] T ∈SO, can also be represented by quaternion q=[q w ,q x ,q y ,q z ] T ∈S 3 Use the subscript {x, y, z} to represent the imaginary part of the quaternion, i.e. q x,y,z =[q x ,q y ,q z ] T The derivative of the quaternion with respect to time is expressed as The coordinate system diagram is as follows Figure 1 .

[0097] 2. System Operation Framework

[0098] The system uses finite state and operation. During the operation of the drone, there are mainly the following states: unlocking (idling), takeoff, landing, waiting, algorithm execution process, abnormal state, and locked state. Among them, there may be other sub-states within the algorithm execution process, which is the main part of this design work. The rest is the state machine required for each drone. In the drone of this design, the workflow and state transition conditions of the finite state machine are as follows Figure 2 shown.

[0099] 3. UAV control operation process

[0100] The present invention adopts NMPC algorithm (principle as Figure 3 As shown), the current motion state and the state of the next few moments are predicted based on the UAV full-state kinematic model derived by the present invention. The positioning of the present invention adopts the visual positioning of ORB-SLAM3 (the operation process is as follows Figure 4 As shown in the figure, the CasADi toolkit was used to deploy the NMPC algorithm. At the same time, the stability of visual positioning was considered in the control. Based on the previously derived full-state kinematic model, the description and prediction of perception quality were added. The algorithm performance was verified in the simulation environment and the actual system. The workflow of the simulation environment is as follows: Figure 5 shown.

[0101] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A control method for optimizing the visual perception of a drone, characterized in that: include: The first step is to analyze the UAV system. Based on the UAV parameters and the angular velocity inner loop of the flight controller, the full-state kinematic and dynamic equations of the UAV are solved. The second step is to perform visual positioning and mapping of the surrounding environment, thereby obtaining the current position and attitude information as the state of the UAV system to participate in the control solution; and obtaining the position and velocity information of each feature point used to describe the current visual positioning quality; The visual positioning quality Z of the UAV is added as a new state variable to the UAV's full-state kinematic equation. A cost function is constructed to comprehensively calculate the control variables required to control the UAV, thereby completing the UAV's tracking of the planner's trajectory. Wherein, the cost function is expressed as: Among them, Q and R represent the state quantity and the penalty term for the system control input quantity respectively, X k represents the state quantity at time k, X k,r represents the expected state quantity at time k, U k represents the system control input at time k, U k,r represents the expected system control input at time k; Q p Represents the weight of the variable that describes the perceptual quality of each feature point in the image in the cost function; Let s = [u, v] T Represents the coordinates of the feature points in the image coordinate system. According to the classic pinhole camera model, we have: In the above formula, f x , f x Respectively represent the product of the camera's zoom multiple on the u and v axes of the image and the focal length; C p f =[ C p fx , C p fy , C p fz ] T Represents the three-axis coordinates of the feature point in the camera coordinate system; c p f =( W q B B q C ) -1 ☉( W p f -( W q B ☉ B p C + W p B )) Where ⊙ represents quaternion multiplication; W p B Indicates the position of the body coordinate system in the world coordinate system. B p C Indicates the position of the camera coordinate system in the body coordinate system. B q C and W q B The rotation posture between the camera coordinate system to the body coordinate system, and the body coordinate system to the world coordinate system; Among them, ω x ,ω y ,ω z Indicates the three-axis angular velocity of the UAV; C v W Indicates the three-axis speed of the drone in the body coordinate system; The visual positioning quality 2. The method for controlling optimal visual perception of a drone according to claim 1, wherein: Use the ORB-SLAM algorithm to perform visual positioning and mapping of the surrounding environment.

3. The method for controlling optimal visual perception of a drone according to claim 1, wherein: The state quantity includes the three-axis position of the drone in the world coordinate system W p x , W p y , W p z , three-axis speed W v x , W v y , W v z and the quaternion q w ,q x ,q y ,q z .

4. The method for controlling optimal visual perception of a drone according to claim 1, wherein: The control output of the drone includes the acceleration in the z-axis direction B a Z and the three-axis angular velocity ω x ,ω y ,ω z .

Images