Control method for optimal visual perception of unmanned aerial vehicle

Abstract

The invention discloses a control method for optimal visual perception of an unmanned aerial vehicle, and provides an optimal perception control method for the unmanned aerial vehicle for the key problem of task execution of the unmanned aerial vehicle, namely the stability and the high efficiency of unmanned aerial vehicle control according to the requirement of the unmanned aerial vehicle for the perception stability in the flight process. The method comprises the steps of designing an unmanned aerial vehicle visual perception algorithm and a control algorithm. The visual perception positioning uses ORB-SLAM3 to obtain the position and attitude information of the unmanned aerial vehicle; the control algorithm uses a non-linear model prediction control algorithm, an unmanned aerial vehicle control instruction sequence is solved according to the current position attitude and positioning information of the unmanned aerial vehicle and an expected track, the unmanned aerial vehicle is controlled, and meanwhile it is ensured that the perception quality of the unmanned aerial vehicle meets the control requirement.

Description

Technical Field

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

[0002] A drone is an unmanned aircraft that uses a ground control station and a flight remote controller as a carrier and is controlled by wireless communication equipment to achieve autonomous flight. Drones first appeared in the 1920s and were used in wars. In 1956, the first true quadcopter Convertawings Model "A" successfully completed its test flight, but it did not meet the military's requirements for height and speed. In the early 1990s, due to the vigorous development of electronic technology and computer technology, hardware integration capabilities and computer computing capabilities achieved breakthrough progress. MEMS-based inertial navigation systems began to be used in multi-rotor drones, so multi-rotor aircraft began to use such systems to achieve relatively stable flight. However, due to the fact that there is still a lot of noise in the sensor, it was not until 2004, after the noise reduction and attitude control algorithms were relatively mature, that truly stable multi-rotor aircraft really appeared. In 2004, the American Spectrolutions company launched the Dragonflyer series of multi-rotor drones, and in 2006 launched the primary version of the 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] Quadcopter drones can achieve multiple motion modes such as hovering in the air, vertical take-off and landing, pitching and rolling motions by adjusting the rotation state of the motors on the rotors. Multi-rotor drones are widely used in many tasks in civil and military fields such as aerial photography and three-dimensional reconstruction due to their simple structure, small size, easy operation, and excellent spatial flexibility. However, the posture of drones has high coupling and high degrees of freedom, and most methods find it difficult to deal with the joint effects in agile flight, such as nonlinear dynamics and aerodynamic effects. The existence of these problems makes it much more difficult to control drones than general control systems (such as motor speed control systems without loads). In recent years, the improvement of hardware computing power and the optimization of basic algorithm complexity have enabled control algorithms based on modern control theory and with high demand to be implemented on airborne computing devices. Among them, model predictive control has become a drone control algorithm that has received much attention due to its accurate performance in handling high-speed trajectory tracking.

[0004] In addition, in most studies on pure control of multi-rotor drones, perception and positioning are based on motion capture systems, which is almost impossible to meet in practical applications. Since camera movement will have a negative impact on the effect of its positioning and cause noise interference to the acquisition of physical information such as robot posture, velocity, acceleration, etc., people cannot always equate airborne vision with motion capture systems. At present, thanks to the large-scale development of perception algorithms and the great advantages of weight, cost, size, and power consumption of visual sensing equipment, vision-based perception and positioning is the mainstream solution for perception and positioning in practical applications of drones. Even so, this mainstream solution is greatly affected by the environment and the posture and motion of airborne equipment, and has great limitations. This shows that the high-precision and high-stability perception and positioning of multi-rotor drones can provide more help for control, while the quality of control results and the rationality of decision-making will in turn affect the effect of perception and positioning. For example, in order to pass through a narrow gap and use an airborne camera to locate the drone at the same time, it is necessary to ensure that such a gap is visible at all times. Therefore, how to reasonably control and 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] How to reasonably control the obtained positioning data to ensure that the visual sensor device always points to the area with rich texture, give the controller stable positioning feedback, and make full use of the dynamic flexibility of the quadcopter is very important for the overall scheduling of drones based on indoor positioning. For this problem, for example, in some recent studies, model predictive control can enable drones to track pre-given nonlinear trajectories at speeds of up to 20m / s. In the relatively classical control field, the most advanced non-predictive method, differential-flatness-based controller (DFBC), also shows strong performance in autonomous tracking of agile trajectories. When tracking a trajectory with a speed of 12.9m / s and an acceleration of 2.1g, the differential flatness controller only has an average position tracking error of 6cm.

[0006] Currently, the commonly used UAV control algorithms are divided into predictive methods and non-predictive methods. The former only focuses on the trajectory at the current moment for control, and the control effect is poor in extreme cases; while the latter takes into account the trajectories of multiple future times in the controller, which consumes more computing resources.

[0007] Different solutions are proposed for the advantages and disadvantages listed above. For drone control algorithms and positioning solutions, the existing solutions are as follows:

[0008] Controller scheme 1: Differential 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 differential flat controller can still meet the required tracking performance under relatively high speed requirements. As people's 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 UAVs and was only used for position control of UAVs. In recent years, thanks to the development of hardware and nonlinear optimization solvers, it has become feasible to run NMPC algorithms for 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 quadrotor UAVs.

[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 the feature trajectory in the image plane and utilize the invalid space of the visual servoing task. In addition, the under-actuated characteristics of the robot are not taken into account in the trajectory planning. Due to the conflict between the robot motion and the perception target, this will greatly affect the performance of the robot's overall motion. Summary of the invention

[0012] In view of this, the present invention aims at the key issue of drone mission execution - the stability and efficiency of drone control, and provides an optimal perception control method for drones based on the demand for perception stability during drone flight. Compared with previous control methods, the present invention can provide a stable, efficient and reliable solution for drone control while maintaining the reliability of positioning data.

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

[0014] The first step is to analyze the UAV system and solve the full-state kinematic equation and dynamic equation of the UAV based on the UAV parameters and the angular velocity inner loop of the flight controller.

[0015] The second step is to perform visual positioning and mapping of the surrounding environment, so as to obtain the current position information and attitude information as the state quantity of the UAV system to participate in the control solution; and obtain the position and speed information of each feature point used to describe the current visual positioning quality; add the visual positioning quality Z of the UAV as a new state quantity to the full-state kinematic equation of the UAV, construct the cost function, and comprehensively calculate the control quantity required to control the UAV, thereby completing the tracking of the planner trajectory by the UAV;

[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, and 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 perceived 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 between the body coordinate system and the world coordinate system;

[0025] Among them, ω x ,ω y ,ω z Indicates the three-axis angular velocity of the drone; 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 UAV control. Compared with the more mainstream control algorithms before, the method provided by the present invention can effectively avoid the loss of control of the UAV under extreme working conditions for the UAV state at multiple moments in the future, so that the UAV 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 UAV 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 It 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 drone control while ensuring the visual positioning perception positioning effect, the method comprising the steps of:

[0039] The first step is to analyze the UAV system. Based on the mass, size and other parameters of the UAV and the angular velocity inner loop of the flight controller, 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 the surrounding environment, thereby obtaining the current position information and attitude information as the state quantity of the drone system to participate in the control solution. In addition, the position and speed information of each feature point used to describe the current visual positioning quality are obtained; the visual positioning quality Z of the drone is added as a new state quantity to the full-state kinematic equation of the drone, a new cost function is constructed, and the control quantity required to control the drone is comprehensively calculated, thereby completing the drone's tracking of the planner's trajectory.

[0041] The algorithm of the present 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] The visual positioning algorithm is used to extract ORB features from the two images. The corresponding epipolar line of each left feature in the right image is searched to solve the initial position of the spatial map point, and then the current position and posture of the drone are calculated.

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

[0044] The visual positioning algorithm uses IMU pose prior to help binocular positioning for 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, several frames will observe the same map points. The back-end optimization constructs the least squares problem according to the bundle adjustment method as follows, uses the G2O optimization library to optimize and obtain accurate pose information and generates messages in the nav_msgs::Odometry format through ROS for publication.

[0045] The UAV control algorithm uses a 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: 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}, which is consistent with the body coordinate system posture, but has a fixed offset in 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 the actual use requirements, the quantities that need to be controlled are mainly the position, speed and attitude of the drone, which can be represented by three-axis position, three-axis speed 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 obtained first, 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 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 The force of 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] In this way, the thrust required by the drone can be calculated.

[0060] Given a reference trajectory, the cost function is the error in time between the predicted state at each prediction moment in the prediction time and the reference state at the corresponding prediction moment. This means that in each control cycle, multiple reference points of the reference trajectory will need to be used, and these reference points are needed as controller inputs to adjust the controller output.

[0061] In order to perform numerical optimization, solve the trajectory tracking problem faced by the above-mentioned UAV, and obtain reasonable control instructions, it is necessary to discretize the state X and input U of the UAV into N equal intervals in the time span τ∈[t,t+h], and the size of each interval dt=h / N. Among them, h represents the total prediction time, dt represents the prediction step size, and N represents the number of predictions. According to the full-state dynamics equation of the UAV, a restricted nonlinear optimization problem can be obtained.

[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 drone 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 (expressed in quaternion form) of the drone in the world coordinate system; and the control command 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 thrust restrictions and angular velocity restrictions of the drone, which are conducive to improving the stability of the drone. Q and R represent the penalty terms of the state quantity and input quantity 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 movement are two essential parts. Many works have studied these two separately, but rarely have they been considered as a unified problem.

[0069] In fact, the movement of the camera will strongly affect the quality of visual perception. On the one hand, if the camera leaves the viewing angle where the feature points are dense, it may not be able to extract enough feature points, thus reducing the positioning stability; on the other hand, if a series of requirements of 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 of motion and thus reducing camera jitter), 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. Let:

[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 sufficiently stable, the feature points C p fThe projection s should be as close to the center of the image as possible. On the one hand, this is to keep the feature points in the picture taken by the camera when the drone is disturbed by external interference and vibrates; on the other hand, it is because the periphery of the image usually has relatively large distortion characteristics, especially for cameras with a large field of view. Even now, there are many literatures that study many models of this distortion, and use techniques such as estimating its parameters and performing camera calibration to compensate for the distortion. However, this compensation is never perfect, and there will still be inaccurate estimates. Therefore, making the 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 also 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 yields:

[0079]

[0080] The distance of the feature point projection from the image center and the speed of the feature point projection on the image plane can describe the perceived quality. The variables are described as 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] From formula (15) and (16), we can know that 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 drone, which is also part of the state quantity X. The three-axis speed of the drone in the body coordinate system C v W It can be obtained by the first-order derivative of the state quantity X, so the perceived 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, such as the position and speed of each feature point in the image, which can describe 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 means that the invention expects that all feature points are located as close as possible to the center of the image plane captured by the camera and the velocity is kept at zero. The reference values ​​of state and input are the desired posture calculated by the trajectory planner.

[0093] Example:

[0094] The present invention provides a control method that ensures visual positioning perception and positioning effect while maintaining the stability and efficiency of UAV control. First, the current position and posture of the UAV are obtained through visual positioning information. Then, the control amount of the UAV is obtained by using a nonlinear model predictive control algorithm to control the UAV, combining the expected position and speed at the future moment given by the trajectory planner and the prediction of the feature point trajectory obtained by the UAV visual sensor.

[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, it can also be represented by the 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), take-off, 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 states at the next few moments are predicted based on the self-derived UAV full-state kinematic model. 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 is used to deploy the NMPC algorithm. At the same time, the stability of visual positioning is considered in the control, and the description and prediction of perception quality are added on the basis of the full-state kinematic model derived previously. The algorithm performance is 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 protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A control method for optimal visual perception of a drone, characterized in that: include: The first step is to analyze the UAV system and solve the full-state kinematic equation and dynamic equation of the UAV based on the UAV parameters and the angular velocity inner loop of the flight controller. The second step is to perform visual positioning and mapping of the surrounding environment, so as to obtain the current position information and attitude information as the state quantity of the UAV system to participate in the control solution; and obtain the position and speed 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 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. 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, and 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 perceived 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 between the body coordinate system and the world coordinate system; Among them, ω x ,ω y ,ω z Indicates the three-axis angular velocity of the drone; 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 as claimed in claim 1, characterized in that: 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 as claimed in claim 1, characterized in that: 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 as claimed in claim 1, characterized in that: 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