Flying robot visual servo control method based on complex environment obstacle avoidance tracking

Abstract

The invention provides a flying robot visual servo control method based on complex environment obstacle avoidance tracking. The flying robot visual servo control method comprises the steps that S1, a four-rotor unmanned aerial vehicle system model is constructed; s2, constructing pixel points of a dynamic target in an imaging plane of the flying platform, considering an image dynamic model of a moving target, and constructing a virtual image moment so as to realize tracking control of the flying robot on the dynamic target; s3, designing a constrained boundary function framework based on image features, and adjusting a constrained boundary function through local obstacle information acquired by a depth camera; s4, designing a position controller under the conditions that the target is visible and invisible, and achieving the position control of the flight platform through the state estimation of the final boundary of the target; and S5, constructing a generalized error vector based on attitude angle information acquired by the inertial measurement unit and the constructed virtual yaw image moment, and designing a sliding mode attitude controller to perform attitude control on the flying robot according to the generalized error vector.

Description

Technical Field

[0001] The present invention proposes a visual servo control method for a flying robot based on obstacle avoidance and tracking in complex environments, and particularly relates to the technical field of unmanned aerial vehicles. Background Art

[0002] With the advancement of technologies such as artificial intelligence, computers, and mechatronics, robotics has been developing at an accelerating pace, and its scope of application has continued to expand. Multipurpose collaborative robots, logistics robots, and flying robots have emerged, playing an important role in industries such as industrial manufacturing, healthcare, and catering. Flying robots, due to their ability to move freely in three-dimensional space and their excellent maneuverability and flexibility, are widely used in fields such as aerial photography, surveying and mapping, and inspection, greatly improving operational efficiency and safety. However, the introduction of flying robots also brings new challenges. The operating environment of flying robots is often unstructured, with complex and ever-changing scenes. Traditional positioning methods (such as GPS and visual odometry) cannot meet the requirements of high-precision local positioning. Therefore, flying robots typically adopt visual servoing control methods to achieve precise positioning and tracking of targets through image feedback.

[0003] Furthermore, when performing dynamic target tracking tasks, flying robots must consider obstacle avoidance. Since flying robots may encounter static or dynamic obstacles (such as buildings, trees, and other aircraft) during flight, these obstacles can cause the robot to deviate from its intended trajectory or even collide, resulting in equipment damage or mission failure. Therefore, designing an efficient and reliable obstacle avoidance system to ensure that the flying robot can safely avoid obstacles and quickly return to the desired trajectory is a key issue in dynamic target tracking systems for flying robots.

[0004] At the same time, when the flying robot is performing a dynamic target tracking task, the target may temporarily leave the flying robot's field of view due to problems such as obstacles and light changes. This not only requires the flying robot to have a highly accurate positioning system to compensate for the inaccuracy of GPS in complex environments (such as urban canyons and indoor areas), but also a powerful visual servo control system to cope with the sudden disappearance and reappearance of the target. The Global Positioning System (GPS) has high latency and large errors in complex urban environments, closed indoor areas or low-altitude areas, and cannot provide sufficiently accurate position information. Obtaining accurate speed information through image information is a difficult problem in the field of visual servoing. Inaccurate relative speed information will affect the stability of the controller. Existing image-based visual servo controllers are mainly used to track stationary targets, while the tracking of dynamic targets by flying robots needs to consider the target's movement speed and trajectory, which increases the control difficulty of flying robots in actual environment tracking. Summary of the Invention

[0005] To overcome these challenges and enable the flying robot to successfully track a moving target, the flying robot needs to improve its ability to continuously and stably track a moving target in a complex environment. Therefore, in-depth research is required to develop a constraint boundary framework that can achieve obstacle avoidance and a robust visual servo tracking control method when the moving target is briefly out of the field of view.

[0006] Through in-depth research on the visual servo tracking control method of the flying robot, not only can the obstacle avoidance tracking function in a complex environment be significantly improved, but also the safety, stability, and tracking accuracy of the visual servo control technology can be promoted to a higher level, providing strong technical support for future high-difficulty and high-precision aerial operation tasks.

[0007] The purpose of the present invention is to provide a visual servo control method for a flying robot based on obstacle avoidance tracking in a complex environment, which can effectively solve the problems that during the tracking flight, the flying robot cannot accurately track due to being hindered by obstacles during the flight process, and the moving target is briefly out of the field of view due to obstacle occlusion. When it reappears, it is difficult to ensure that it is within the field of view of the aircraft, the attitude error is significant after the dynamic target reappears, and the tracking accuracy of the end effector of the robotic arm is insufficient, and improve the visual tracking performance and stability of the flying operation robot system.

[0008] To achieve the above object, the technical solution of the present invention is: a visual servo control method for a flying robot based on obstacle avoidance tracking in a complex environment, including the following contents:

[0009] A visual servo control method for a flying robot based on obstacle avoidance tracking in a complex environment, characterized by including the following steps:

[0010] Step S1: Construct a quadrotor UAV system model;

[0011] Step S2: By analyzing the dynamic target, construct the pixel points of the dynamic target in the imaging plane of the flight platform, and construct a virtual image moment according to the image dynamics model of the moving target to realize the tracking control of the flying robot for the dynamic target;

[0012] Step S3: Design a constraint boundary function framework based on image features, and design a constraint boundary function for the flying robot to achieve obstacle avoidance by using the local obstacle information obtained by the depth camera;

[0013] Step S4: Design a position controller in the case of target visibility and target invisibility, and prove the global tracking stability based on the average dwell time theorem, and realize the position control of the flight platform through the state estimation of the final boundary of the target;

[0014] Step S5: Based on the attitude angle information obtained by the inertial measurement unit and the constructed virtual yaw image moments, construct a generalized error vector, and design a sliding mode attitude controller according to the generalized error vector to perform attitude control on the flying robot.

[0015] Furthermore, step S1 includes the following contents:

[0016] Step S11: Model the flying robot platform according to the momentum and moment of momentum theorem, including the following contents:

[0017] The inertial coordinate system and the body coordinate system are defined as Σ w and Σ b , the position and velocity of the flying robot in the inertial coordinate system are respectively represented by and The attitude and angular velocity of the flying robot are respectively represented by and The dynamic model of the flying robot dynamics is described by the following equation:

[0018]

[0019] where m and J are respectively the mass and inertia matrix of the flying robot, and respectively represent the thrust and torque generated by the quadrotor UAV; e3 = [0, 0, 1] T represents the unit vector in the Z-axis direction, and g is the acceleration due to gravity; and respectively represent the rotation matrix and transformation matrix of the flying robot; R = R ψ R θ R φ , here and respectively represent the unit rotation matrices corresponding to the yaw angle, pitch angle, and roll angle of the flying platform; T is expressed as:

[0020]

[0021] where θ and φ respectively represent the pitch angle and roll angle of the flying platform.

[0022] Furthermore, step S2 includes the following contents:

[0023] Define a virtual camera coordinate system Σ v , the virtual camera coordinate system Σ v shares the same coordinate origin and yaw angle with the actual camera coordinate system Σ c , while the pitch angle and roll angle of the virtual camera coordinate system are the same as those of the inertial coordinate system Σ w ;

[0024] The target coordinate system Σ is defined. t ;

[0025] It is defined that the virtual image plane of the UAV is always parallel to the target plane.

[0026] Define R c as the rotation matrix of the body coordinate system Σ b relative to the camera coordinate system Σ c . R v is the rotation matrix of the virtual camera coordinate system Σ v relative to the inertial coordinate system Σ w ; Let P c = [u c , v c , 1] T and P v = [u v , v v , 1] T respectively represent the normalized image pixel coordinates captured by the actual camera and the virtual camera, then P v is calculated by the following formula:

[0027]

[0028] where K = diag(λ , λ, 1) represents the camera focal length matrix of the camera, and λ represents the focal length value of the airborne camera; c R v = R θ R φ represents the rotation matrix of the camera coordinate system Σ c relative to the virtual camera coordinate system Σ v ;

[0029] Define the virtual image moment q = [q x , q y , q z T and the yaw angle q ψ of the flying robot relative to the moving target are used to characterize the movement of the flying operation robot:

[0030]

[0031] where N is the number of selected feature points, and respectively characterize the horizontal and vertical positions of the dynamic target on the imaging plane; u k and n k are the horizontal and vertical coordinates of the k-th pixel point respectively; a = μ 20 + μ 02 , where the image central moment a* The value of a corresponding to the UAV at the desired position, representing the desired image feature moment.

[0032] Further, the step S2 further includes the following content:

[0033] Define the desired depth as z * , z * and a * are both constants, satisfying the following equation:

[0034]

[0035] In the virtual image plane where the flying robot tracks a dynamic target, the image feature dynamics on the virtual imaging plane are represented as:

[0036]

[0037] Among them, is the skew-symmetric matrix corresponding to the vector This skew-symmetric matrix satisfies for any direction vector a. t represents the relative velocity between the flying robot and the target in the virtual image plane; v and v and respectively represent the translational velocities of the flying robot and the dynamic target in the inertial coordinate system;

[0038] Further, it is stipulated that the dynamic target moves on the xy plane of the inertial coordinate system, and the height z of this plane is known, and the speed of this dynamic target is bounded, obtaining the relational expression:

[0039] Further, the step S3 includes the following content:

[0040] Step S31: Set the desired image moment during the tracking process as q d , and the actual image moment captured by the on-board camera of the flying robot is q; then the image moment error during the tracking process is defined as e = q - q d ; To solve the obstacle avoidance problem, boundary constraints are imposed on the image moment error:

[0041] ζ n < e < ζ x (Formula VIII)

[0042] Among them, ζ n =(ζ x,n , ζ y,n , ζ z,n ), ζ x =(ζx,x ,ζ y,x ,ζ z,x ) are the specified maximum and minimum bounds; both the field of view constraint and the time-varying bounds constraint are considered:

[0043] q min -q d ≤ζ n <e<ζ x ≤q max -q d (Formula 9)

[0044] where q min and q max It is a constant parameter determined by the pixel resolution of the camera;

[0045] Step S32: Design the time derivative of the constraint boundary function by Lipschitz continuous projection:

[0046]

[0047] Among them *∈{xn,yn,zn,xx,yx,zx}:

[0048]

[0049] where κ x ,κ y ,κ z ,k1,k2,k3,k4, is a designed normal number, ι is a very small normal number; under the Lipschitz continuous projection, the constant and ζ * They are ζ * The upper and lower bounds of

[0050] Among them, ρ1, ρ2, ρ3, r1, r2, d, a1, a2, h are related parameters obtained by the depth camera;

[0051] Furthermore, the constraint function of the flying robot is obtained:

[0052]

[0053] where γ 11 ,γ 12 ,γ 21 ,γ 22 ,γ d is used to adjust the designed positive constant,

[0054] l1,l2,l 12 ,l∈(0,1) is the corresponding starting coefficient.

[0055] Furthermore, step S3 further includes the following content:

[0056] Step S33: To design a constraint boundary function suitable for obstacle avoidance, it is necessary to consider whether there are obstacles in the flight trajectory of the flying robot; when there are no obstacles, l1 = l2 = l = 0, l 12 = 1, and the flying robot can converge quickly; when there are obstacles, the obstacle avoidance task is carried out by the depth camera feedback of the local information of the obstacle: the distance ρ between the boundary of the flying robot and the boundary of the obstacle, the horizontal distance d between the center of the depth camera and the boundary of the obstacle, the vertical distance a between the center of the depth camera and the boundary of the obstacle, and the distance h between the center of the depth camera and the bottom surface of the flying robot;

[0057] Step S34: Use an obstacle function to constrain the image moment error:

[0058]

[0059] Among them, η represents the obstacle function, and e is the image moment error.

[0060] Furthermore, step S4 includes the following content:

[0061] Step S41: Define the target disappearance time and the number of switches during this period as T u (τ, t) and N σ (τ, t), where 0 < τ < t; if there are constants and τ a > 0, then the average dwell time τ a and the number of switches N σ (t, τ) satisfy the following conditions:

[0062]

[0063]

[0064] Among them, ɑ represents: the Lyapunov energy function in the visible case; β represents: the Lyapunov energy function in the invisible case;

[0065] Step S42: Construct a sliding mode surface s:

[0066]

[0067] where r is the sliding mode surface gain parameter and is a positive constant; η represents the obstacle function;

[0068] Step S43: Design a position controller based on the average time theorem during the tracking process as:

[0069]

[0070] where a is a positive constant, and:

[0071]

[0072] Step S44: For the dynamic model designed in Step S1, if the dynamic target is within the camera's field of view and the position controller is given by Equation (XVIII) in Step S43, then the virtual image moment tracking error e in this process converges at an exponential rate;

[0073] Step S45: Prove Step S44. To prove the convergence of the tracking error of the proposed position controller, define the following Lyapunov energy function:

[0074]

[0075] where s represents the constructed sliding surface;

[0076] Step S46: Take the derivative of V on :

[0077]

[0078] Step S47: Substitute Equation (XVIII) into Equation (XXIV), and it becomes:

[0079]

[0080] Step S48: For the dynamic model designed in Step S1, if the dynamic target temporarily disappears from the camera's field of view, update Equations (IV) and (XIV) accordingly, then the growth rate of the virtual image moment tracking error e p is limited within an exponential range:

[0081]

[0082] where e p = q new - q d , δ2 = R ψ T (v - v max ).

[0083] Furthermore, Step S4 also includes the following content:

[0084] Step S49: Prove Step S48. To prove the convergence of the tracking error of the proposed position controller, define the following Lyapunov energy function:

[0085]

[0086] where η off (0) = ln(-ζ n (0) / ζ x (0)),

[0087] Step S410: Since ||η|| ≤ Me -βt ||η off (0)||, then:

[0088]

[0089] where M and β are known positive constants;

[0090] Step S411: Take the derivative of V off :

[0091]

[0092] Step S412: Prove Step S41. Combining Step S47 and Step S411 gives:

[0093]

[0094] where, represents the time at the nth moment when the target enters the camera FOV, represents the time at the nth instant when the target leaves the camera FOV.

[0095] Furthermore, Step S5 includes the following:

[0096] Step S51: Define the generalized attitude tracking error as e Φ = Φ η - Φ d , where, Φ η = [φ, θ, -q ψ T ,

[0097] Step S52: Based on the generalized attitude tracking error obtained in S51, define the sliding mode surface as:

[0098]

[0099] where, r Φ is the sliding mode surface gain parameter and is a positive constant;

[0100] Step S53: Take the derivative of the sliding mode variable s Φ to obtain:

[0101] ​

[0102] Step S54: Design a sliding mode attitude controller as follows:

[0103]

[0104] where a Φ is a positive constant:

[0105] Step S55: Prove the convergence of the tracking error of the proposed global fast terminal sliding mode controller, and define the following Lyapunov energy function:

[0106]

[0107] Step S56: Take the derivative of V Φ to obtain:

[0108]

[0109] Step S57: Combine (Equation 34) and (Equation 35), and (Equation 37) is transformed into:

[0110]

[0111] where the sliding mode variable s Φ and the generalized attitude tracking error e Φ will converge to the constant 0.

[0112] In summary, a flying robot, characterized in that the flying robot is used to implement the steps of any one of the control methods of the vision servo control method of a flying robot for obstacle avoidance and tracking based on a complex environment involved in the present invention.

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

[0114] (1). The present invention designs a position controller based on the average dwell time theorem, and based on the average dwell time theorem, proves the stability of the system during the entire continuous tracking process. Ensure that when the flying robot executes the task of tracking a moving target, the dynamic target will not escape from the camera's field of view during the short disappearance process caused by obstacle occlusion, and continuously and stably track.

[0115] (2). The present invention designs a constraint boundary function framework based on image features, and adjusts the constraint boundary function through the local obstacle information obtained by the depth camera to ensure that the position feature error converges rapidly when there are no obstacles in the flight trajectory of the flying robot, so as to ensure that the flying robot efficiently and stably tracks the moving target, and realizes the obstacle avoidance task of the flying robot through error offset in the case of obstacle occlusion.

[0116] (3) Based on the sliding mode surface, the present invention designs a vision servo attitude controller for the flight platform. A terminal sliding mode surface is constructed by using the generalized attitude error variable, and a vision servo attitude controller for the flight platform is designed based on this sliding mode surface to achieve the rapid convergence of the attitude feature error, so as to ensure the efficient and rapid response performance of the flight operation robot to the attitude change of the dynamic target. Description of the Drawings

[0117] Figure 1 It is a schematic diagram of the relationship between the image plane and the virtual image plane when tracking a dynamic target in an embodiment of the present invention.

[0118] Figure 2 It is an overall control flow framework diagram of the vision servo obstacle avoidance tracking control scheme of the flying robot in an embodiment of the present invention.

[0119] Figure 3 It is a schematic diagram of the tracking error of the X-axis component of the position controller of the flying robot in an embodiment of the present invention.

[0120] Figure 4 It is a schematic diagram of the tracking error of the Y-axis component of the position controller of the flying robot in an embodiment of the present invention.

[0121] Figure 5 It is a schematic diagram of the tracking error of the Z-axis component of the position controller of the flying robot in an embodiment of the present invention. Detailed Embodiments

[0122] The technical solution of the present invention will be specifically described below with reference to the drawings.

[0123] It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present application belongs.

[0124] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular forms are also intended to include the plural forms. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0125] A vision servo control method for a flying robot based on obstacle avoidance tracking in a complex environment, characterized by comprising the following steps:

[0126] Step S1: Construct a quadrotor UAV system model;

[0127] Step S2: By analyzing the dynamic target, construct the pixel points of the dynamic target in the imaging plane of the flight platform, and construct virtual image moments according to the image dynamics model of the moving target to achieve the tracking control of the flying robot for the dynamic target;

[0128] Step S3: Design a constraint boundary function framework based on image features, and design a constraint boundary function for obstacle avoidance of the flying robot by using the local obstacle information obtained by the depth camera;

[0129] Step S4: Design position controllers in the cases of target visibility and non-visibility, prove the global tracking stability based on the average dwell time theorem, and achieve the position control of the flight platform through the state estimation of the final boundary of the target;

[0130] Step S5: Based on the attitude angle information obtained by the inertial measurement unit and the constructed virtual yaw image moments, construct a generalized error vector, and design a sliding mode attitude controller according to the generalized error vector to perform attitude control on the flying robot.

[0131] Furthermore, Step S1 includes the following contents:

[0132] Step S11: Model the flying robot platform according to the momentum and moment of momentum theorems, including the following contents:

[0133] The inertial coordinate system and the body coordinate system are defined as Σ w and Σ b , the position and velocity of the flying robot in the inertial coordinate system are represented by and The attitude and angular velocity of the flying robot are represented by and The dynamic model of the flying robot dynamics is described by the following equation:

[0134]

[0135] where m and J are the mass and inertia matrix of the flying robot respectively, and represent the thrust and torque generated by the quadrotor UAV respectively; e3 = [0, 0, 1] T represents the unit vector in the Z-axis direction, and g is the acceleration due to gravity; and represent the rotation matrix and transformation matrix of the flying robot respectively; R = R ψ R θ R φ , here and represent the unit rotation matrices corresponding to the yaw angle, pitch angle and roll angle of the flight platform respectively; T is expressed as:

[0136]

[0137] Among them, θ and φ respectively represent the pitch angle and roll angle of the flight platform.

[0138] Furthermore, step S2 includes the following content:

[0139] Define a virtual camera coordinate system Σ v , the virtual camera coordinate system Σ v shares the same coordinate origin and yaw angle with the actual camera coordinate system Σ c , while the pitch angle and roll angle of the virtual camera coordinate system are the same as those of the inertial coordinate system Σ w ;

[0140] Define the target coordinate system Σ t ;

[0141] Define that the virtual image plane of the UAV is always parallel to the target plane;

[0142] Define R c as the rotation matrix of the body coordinate system Σ b relative to the camera coordinate system Σ c . R v is the rotation matrix of the virtual camera coordinate system Σ v relative to the inertial coordinate system Σ w ; Let P c = [u c , v c , 1] T and P v = [u v , v v , 1] T respectively represent the normalized image pixel coordinates captured by the actual camera and the virtual camera, then P v is calculated by the following formula:

[0143]

[0144] Among them, K = diag(λ , λ, 1) represents the camera focal length matrix of the camera, and λ represents the focal length value of the airborne camera; c R v = R θ R φ represents the rotation matrix of the camera coordinate system Σ c relative to the virtual camera coordinate system Σ v ;

[0145] Define the virtual image moment q = [q x , q y , qz T and the yaw angle q of the flying robot relative to the moving target ψ used to characterize the movement of the flying operation robot:

[0146]

[0147] where N is the number of selected feature points, and respectively characterize the horizontal and vertical positions of the dynamic target on the imaging plane; u k and n k are respectively the horizontal and vertical coordinates of the k-th pixel point; a = μ 20 + μ 02 , where the image central moment a * is the value of a corresponding to when the UAV is at the desired position, representing the desired image feature moment.

[0148] Furthermore, the step S2 further includes the following content:

[0149] Define the desired depth as z * , z * and a * are both constants, satisfying the following equation:

[0150]

[0151] In the virtual image plane where the flying robot tracks the dynamic target, the image feature dynamics on the virtual imaging plane are represented as:

[0152]

[0153] where, is the skew-symmetric matrix corresponding to the vector , and this skew-symmetric matrix satisfies for any direction vector a. t represents the relative velocity between the flying robot and the target in the virtual image plane; v and v and respectively represent the translational velocities of the flying platform and the dynamic target in the inertial coordinate system;

[0154] Furthermore, it is stipulated that the dynamic target moves on the xy plane of the inertial coordinate system, and the height z of this plane is known, and the speed of this dynamic target is bounded, obtaining the relational expression as:

[0155] Furthermore, the step S3 includes the following content: ​

[0156] Step S31: Set the desired image moment during tracking as q d , the actual image moment captured by the onboard camera of the flying robot is q; then the image moment error in the tracking process is defined as e = qq d ; In order to solve the obstacle avoidance problem, a boundary constraint is imposed on the image moment error:

[0157] ζ n <e<ζ x (Formula 8)

[0158] Among them, n =(ζ x,n ,ζ y,n ,ζ z,n ),ζ x =(ζ x,x ,ζ y,x ,ζ z,x ) are the specified maximum and minimum bounds; both the field of view constraint and the time-varying bounds constraint are considered:

[0159] q min -q d ≤ζ n <e<ζ x ≤q max -q d (Formula 9)

[0160] where q min and q max It is a constant parameter determined by the pixel resolution of the camera;

[0161] Step S32: Design the time derivative of the constraint boundary function by Lipschitz continuous projection:

[0162]

[0163] Among them *∈{xn,yn,zn,xx,yx,zx}:

[0164]

[0165] where κ x ,κ y ,κ z ,k1,k2,k3,k4, is a designed normal number, ι is a very small normal number; under the Lipschitz continuous projection, the constant and ζ * They are ζ * The upper and lower bounds of

[0166] Among them, ρ1, ρ2, ρ3, r1, r2, d, a1, a2, and h are relevant parameters obtained by the depth camera;

[0167] Furthermore, the constraint function of the flying robot is obtained:

[0168]

[0169] where γ 11 , γ 12 , γ 21 , γ 22 , γ d are positive constants used for adjustment in the design,

[0170] l1, l2, l 12 , l ∈ (0, 1) are the corresponding starting coefficients.

[0171] Step S33: To design a constraint boundary function suitable for obstacle avoidance, it is necessary to consider whether there are obstacles in the flight trajectory of the flying robot; when there are no obstacles, l1 = l2 = l = 0, l 12 = 1, and the flying robot can converge quickly; when there are obstacles, the local information of the obstacles is fed back by the depth camera to carry out the obstacle avoidance task: the distance ρ between the boundary of the flying robot and the obstacle boundary, the horizontal distance d between the center of the depth camera and the obstacle boundary, the vertical distance a between the center of the depth camera and the obstacle boundary, and the distance h between the center of the depth camera and the bottom surface of the flying robot;

[0172] Step S34: Use the obstacle function to constrain the image moment error:

[0173]

[0174] where η represents the obstacle function and e is the image moment error.

[0175] Furthermore, step S4 includes the following content:

[0176] Step S41: Define the target disappearance time and the number of switches during this period as T u (τ, t) and N σ (τ, t), where 0 < τ < t; if there are constants and τ a > 0, then the average dwell time τ a and the number of switches N σ (t, τ) satisfy the following conditions:

[0177]

[0178] Among them, ɑ represents the Lyapunov energy function in the visible case; β represents the Lyapunov energy function in the invisible case;

[0179] Step S42: Construct the sliding mode surface s:

[0180]

[0181] where r is the sliding mode surface gain parameter and is a positive constant; η represents the obstacle function;

[0182] Step S43: Design the position controller based on the average time theorem during the tracking process as:

[0183]

[0184] where a is a positive constant, and:

[0185]

[0186] Step S44: For the dynamic model designed in Step S1, if the dynamic target is within the camera's field of view and the position controller is given by Equation (XVIII) in Step S43, then the virtual image moment tracking error e in this process converges at an exponential rate;

[0187] Step S45: Prove Step S44. To prove the convergence of the tracking error of the proposed position controller, define the following Lyapunov energy function:

[0188]

[0189] where s represents the constructed sliding mode surface;

[0190] Step S46: Take the derivative of V on :

[0191]

[0192] Step S47: Substitute Equation (XVIII) into Equation (XXIV), and it becomes:

[0193]

[0194] Step S48: For the dynamic model designed in Step S1, if the dynamic target temporarily disappears from the camera's field of view, update Equations (IV) and (XIV) accordingly, then the growth rate of the virtual image moment tracking error e p is restricted within an exponential range:

[0195]

[0196] where e p = q new - q d , δ2 = R ψ T (v - v max ).

[0197] Furthermore, step S4 also includes the following content:

[0198] Step S49: Prove step S48. To prove the convergence of the tracking error of the proposed position controller, define the following Lyapunov energy function:

[0199]

[0200] where η off (0) = ln(-ζ n (0) / ζ x (0)),

[0201] Step S410: Since ||η|| ≤ Me -βt ||η off (0)||, then:

[0202]

[0203] where M and β are known positive constants;

[0204] Step S411: Take the derivative of V off :

[0205]

[0206] Step S412: Prove step S41. Combining step S47 and step S411 gives:

[0207]

[0208] where, represents the time at the nth moment when the target enters the camera FOV, represents the time at the nth instant when the target leaves the camera FOV.

[0209] Furthermore, step S5 includes the following content:

[0210] Step S51: Define the generalized attitude tracking error as e Φ = Φ η - Φ d , where, Φ η = [φ, θ, -q ψ T ,​

[0211] Step S52: Based on the generalized attitude tracking error obtained in S51, define the sliding mode surface as:

[0212]

[0213] where r Φ is the sliding mode surface gain parameter and is a positive constant;

[0214] Step S53: Differentiate the sliding mode variable s Φ to obtain:

[0215]

[0216] Step S54: Design the sliding mode attitude controller as follows:

[0217]

[0218] where a Φ is a positive constant:

[0219] Step S55: Prove the convergence of the tracking error of the proposed global fast terminal sliding mode controller. Define the following Lyapunov energy function:

[0220]

[0221] Step S56: Differentiate V Φ to obtain:

[0222]

[0223] Step S57: Combining (Equation 34) and (Equation 35), (Equation 37) is transformed into:

[0224]

[0225] where the sliding mode variable s Φ and the generalized attitude tracking error e Φ will converge to the constant 0.

[0226] In summary, a flying robot, characterized in that the flying robot is used to implement the steps of any one of the control methods of the vision servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment involved in the present invention.

[0227] In this embodiment, referring to Figures 1 - 5, a specific application example is used to elaborate on the operation of the present invention in detail. According to the present invention, a vision servo tracking control method for a flying robot based on obstacle avoidance and tracking in a complex environment is proposed, mainly studying obstacle avoidance when there are obstacles in the flight trajectory of the flying robot and tracking the target when the speed information of the dynamic target is uncertain and the target disappears briefly due to obstacle occlusion. The specific settings are as follows:

[0228] 1): Set the dynamic target to move at an unknown speed and observe the position tracking of the target by the flying robot.

[0229] 2): Set the dynamic target to move at an unknown speed and observe the position tracking of the target by the flying robot when there are obstacles occluding the flight trajectory.

[0230] 3): Set the dynamic target to move at an unknown speed and observe the tracking of the dynamic target by the flying robot when the target disappears briefly due to obstacle occlusion.

[0231] 4) The system parameters are shown in Table 1:

[0232]

[0233] Table 1 System parameters of the flight operation robot

[0234] As Figure 3 、 Figure 4 、 Figure 5 shown, according to a vision servo tracking method for a flying robot based on obstacle avoidance and tracking in a complex environment of this embodiment, the flying robot can avoid obstacles when there are obstacles in the flight trajectory in a complex environment and stably track a dynamic target that disappears briefly. When the flying robot stably tracks the dynamic target, it can achieve fast and stable tracking, smoothly avoid obstacles when there are obstacles, and converge at an exponential rate after completing the obstacle avoidance task. When the target disappears briefly due to obstacle occlusion, even if the image moment error increases, globally, the image moment error shows a decreasing trend at an exponential rate, ensuring that the target does not escape from the field of view of the on-board camera of the flying robot. Figures 3 - 5 Proved the feasibility and superiority of the present invention.

[0235] The above is a preferred embodiment of the present invention. All changes made according to the technical solution of the present invention and whose functional effects do not exceed the scope of the technical solution of the present invention belong to the protection scope of the present invention.

Claims

1. A vision servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment, characterized in that It includes the following steps: Step S1: Construct a quadrotor UAV system model; Step S2: By analyzing dynamic targets, construct pixel points of dynamic targets in the imaging plane of the flight platform, and construct virtual image moments according to the image dynamics model of moving targets to achieve the tracking control of the flight robot for dynamic targets; Step S3: Design a constraint boundary function framework based on image features, and design a constraint boundary function for obstacle avoidance of the flight robot by using the local obstacle information obtained by the depth camera; Step S4: Design position controllers in the cases of target visibility and invisibility, prove the global tracking stability based on the average dwell time theorem, and achieve the position control of the flight platform through the state estimation of the final boundary of the target; Step S5: Based on the attitude angle information obtained by the inertial measurement unit and the constructed virtual yaw image moments, construct a generalized error vector, and design a sliding mode attitude controller according to the generalized error vector to perform attitude control on the flight robot.

2. The visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 1, wherein Among them, step S1 includes the following content: Step S11: Model the flight robot platform according to the momentum and moment of momentum theorems, including the following content: The inertial coordinate system and the body coordinate system are defined as Σ w and Σ b , the position and velocity of the flying robot in the inertial coordinate system are represented by and The attitude and angular velocity of the flying robot are represented by and The dynamic model of the dynamics of the flying robot is described by the following equation: where m and J are the mass and inertia matrix of the flying robot, and represent the thrust and torque generated by the quadrotor UAV, respectively; e3 = [0, 0, 1] T represents the unit vector in the Z-axis direction, and g is the acceleration due to gravity; and represent the rotation matrix and transformation matrix of the flying robot, respectively; R = R ψ R θ R φ , where and represent the unit rotation matrices corresponding to the yaw angle, pitch angle and roll angle of the flying platform, respectively; T is expressed as: Where θ and φ respectively represent the pitch angle and roll angle of the flight platform.

3. A vision servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 2, characterized in that, Among them, step S2 includes the following content: Define a virtual camera coordinate system Σ v , the virtual camera coordinate system Σ v shares the same coordinate origin and yaw angle with the actual camera coordinate system Σ c , while the pitch angle and roll angle of the virtual camera coordinate system are the same as those of the inertial coordinate system Σ w ; Defines the target coordinate system Σ t ; Defines that the virtual image plane of the UAV is always parallel to the target plane; Define R c as the rotation matrix of the body coordinate system Σ b relative to the camera coordinate system Σ c ; R v is the rotation matrix of the virtual camera coordinate system Σ v relative to the inertial coordinate system Σ w ; Let P c = [u c , v c , 1] T and P v = [u v , v v , 1] T respectively represent the normalized image pixel coordinates captured by the actual camera and the virtual camera, then P v is calculated by the following formula: where \(K = \text{diag}(\lambda , \lambda, 1)\) represents the camera focal length matrix of the camera, and \(\lambda\) represents the focal length value of the airborne camera; \(cR v = R θ R φ represents the rotation matrix of the camera coordinate system \(\Sigma c with respect to the virtual camera coordinate system \(\Sigma v . Define the virtual image moment \(q = [q x ,q y ,q z T and the yaw angle \(q ψ of the flying robot relative to the moving target to characterize the motion of the flying operation robot:​ where N is the number of selected feature points, and respectively represent the horizontal and vertical positions of the dynamic target on the imaging plane; u k and n k are respectively the horizontal and vertical coordinates of the k-th pixel point; a = μ 20 + μ 02 , where the image central moment a * is the a value corresponding to when the UAV is at the desired position, representing the desired image feature moment.

4. A visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 3, characterized in that The said step S2 further includes the following content: Define the desired depth as z * , z * and a * are both constants and satisfy the following equation: In the virtual image plane where the flight robot tracks dynamic targets, the image feature dynamics representation on the virtual imaging plane: Among them, is the skew-symmetric matrix corresponding to the vector This skew-symmetric matrix satisfies, for any direction vector a, represents the relative velocity between the flying robot and the target in the virtual image plane; v and v t are respectively the translational velocities of the flying robot and the dynamic target in the inertial coordinate system; and represent respectively the yaw angular velocities of the flying platform and the dynamic target; Furthermore, it is stipulated that the dynamic target moves on the xy plane of the inertial coordinate system, the height z of this plane is known, and the speed of the dynamic target is bounded, and the relational expression is obtained as follows:

5. A vision servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 4, characterized in that, The said step S3 includes the following content: Step S31: Set the expected image moment during tracking as q d , and the actual image moment captured by the on-board camera of the flying robot is q; then the image moment error during tracking is defined as e = q - q d ; To solve the obstacle avoidance problem, boundary constraints are imposed on the image moment error: ζ n <e<ζ x (Formula VIII) where, ζ n =(ζ x,n , ζ y,n , ζ z,n ), ζ x =(ζ x,x , ζ y,x , ζ z,x ) are the specified maximum and minimum boundary values; meanwhile, the field of view angle constraint and the time-varying boundary constraint are considered: q min -q d ≤ζ n <e<ζ x ≤q max -q d (Formula Nine) where q min and q max are constant parameters determined by the pixel resolution of the camera; Step S32: Design the time derivative of the constraint boundary function through Lipschitz continuous projection: Where * ∈ {xn, yn, zn, xx, yx, zx}: where κ x , κ y , κ z , k1, k2, k3, k4, are positive constants of the design, and ι is a positive constant; under the Lipschitz continuous projection, the constants and ζ * are respectively the upper bound and the lower bound of ζ * , and satisfy Where ρ1, ρ2, ρ3, r1, r2, d, a1, a2, h are relevant parameters obtained by the depth camera; Furthermore, obtain the constraint function of the flight robot: where γ 11 , γ 12 , γ 21 , γ 22 , γ d are positive constants used for adjustment, and l1, l2, l 12 , l ∈ (0, 1) are the corresponding starting coefficients.

6. A visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 5, characterized in that, The said step S3 further includes the following content: Step S33: Consider according to the situation of whether there are obstacles in the flight trajectory of the flying robot; when there are no obstacles, l1 = l2 = l = 0, l 12 = 1, and the flying robot can converge quickly; when there are obstacles, the depth camera feeds back the local information of the obstacles to carry out the obstacle avoidance task: the distance ρ between the boundary of the flying robot and the boundary of the obstacle, the horizontal distance d between the center of the depth camera and the boundary of the obstacle, the vertical distance a between the center of the depth camera and the boundary of the obstacle, and the distance h between the center of the depth camera and the bottom surface of the flying robot; Step S34: Use the obstacle function to constrain the image moment error: Where η represents the obstacle function and e is the image moment error.

7. A visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 6, characterized in that, Among them, step S4 includes the following content: Step S41: Define the target disappearance time and the number of switches during this period as T u (τ,t) and N σ (τ,t), where 0 < τ < t; If there exist constants and τ a > 0, then the average residence time τ a and the number of switches N σ (t,τ) satisfy the following conditions: Where ɑ represents: the Lyapunov energy function in the visible case; β represents: the Lyapunov energy function in the invisible case; Step S42: Construct the sliding mode surface s: Where r is the sliding mode surface gain parameter and is a positive constant; η represents the obstacle function; Step S43: Design the position controller based on the average time theorem during the tracking process as: Where a is a positive constant, and: Step S44: For the dynamic model designed in step S1, if the dynamic target is within the camera's field of view and the position controller is given as (Equation XVIII) in step S43, then the virtual image moment tracking error e in this process converges at an exponential rate; Step S45: Prove the convergence of the tracking error of the position controller proposed in step S44, and define the following Lyapunov energy function: Where s represents the constructed sliding mode surface; Step S46: Differentiate V on with respect to: Step S47: Substitute (Equation XVIII) into (Equation XXIV), which is changed to: Step S48: For the kinetic model designed in step S1, if the dynamic target disappears from the camera's field of view and updates the relative (Equation 4) and (Equation 14), then the growth rate of the virtual image moment tracking error e p is limited within the exponential range: where e p = q new - q d , δ2 = R ψ T (v - v max ).

8. A visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 7, characterized in that Among them, step S4 further includes the following content: Step S49: Prove step S48. To prove the convergence of the tracking error of the proposed position controller, define the following Lyapunov energy function: where η off (0) = ln(-ζ n (0) / ζ x (0)), Step S410: Since ||η|| ≤ Me -βt ||η off (0)||, then: where M and β are known positive constants; Step S411: Differentiate V off : Step S412: Prove step S41. Combining step S47 and step S411, we can obtain: Among them, represents the time at the nth moment when the target enters the camera FOV, represents the time at the nth instant when the target leaves the camera FOV.

9. A visual servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment according to claim 8, characterized in that where step S5 includes the following content: Step S51: Define the generalized attitude tracking error as e Φ = Φ η - Φ d , where Φ η = [φ, θ, -q ψ T ,​ Step S52: Based on the generalized attitude tracking error obtained in S51, define the sliding mode surface as: where r Φ is the sliding mode surface gain parameter and is a positive constant; Step S53: Differentiate the sliding mode variable s Φ to obtain: Step S54: Design the sliding mode attitude controller as follows: where a Φ is a positive constant: Step S55: Prove the convergence of the tracking error of the proposed global fast terminal sliding mode controller. Define the following Lyapunov energy function: Step S56: Differentiate V Φ to obtain: Step S57: Combining (Equation 34) and (Equation 35), (Equation 37) is transformed into: where the sliding mode variable s Φ and the generalized attitude tracking error e Φ converge to the constant 0.

10. A flying robot, characterized in that, The described flying robot is used to implement the steps of a vision servo control method for a flying robot based on obstacle avoidance and tracking in a complex environment as described in any one of claims 1 to 9.

Images