Dynamic target grasping method for aerial operating robot based on image visual servoing

Abstract

The present application relates to the technical field of unmanned aerial vehicle control, in particular to a dynamic target grasping method for aerial operation robot based on image visual servoing, comprising: regarding the force and torque of a mechanical arm on an unmanned aerial vehicle as coupled interference to the unmanned aerial vehicle; establishing a coupled interference model based on variable inertia parameters; constructing a virtual camera of the unmanned aerial vehicle, calculating the translation motion characteristics and yaw motion characteristics of a dynamic target by motion analysis on the dynamic target; designing a robust filter observer to estimate the translation velocity and yaw angular velocity of the dynamic target; designing an IBVS controller and an attitude controller of the unmanned aerial vehicle based on the observer; and designing an IBVS controller of the mechanical arm based on the virtual camera to realize accurate grasping of the mechanical arm. The present application can realize tracking of unknown dynamic targets by the unmanned aerial vehicle and accurate grasping of the mechanical arm.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle control, in particular to a dynamic target grasping method for aerial operation robot based on image visual servoing. BACKGROUND

[0002] An unmanned aerial vehicle (UAV) is a kind of simple structure and powerful aerial vehicle, which is mainly divided into fixed-wing unmanned aerial vehicle and multi-rotor unmanned aerial vehicle from the wing mechanism. The multi-rotor unmanned aerial vehicle has been widely used in the fields of aerial photography, plant protection, monitoring and inspection, exploration and remote sensing, etc. due to its low cost, simple operation, flexible maneuvering and the functions of vertical take-off and landing and fixed-point hovering compared with the fixed-wing unmanned aerial vehicle.

[0003] However, in the face of the scene of "active interaction" with target objects or target environment, only the ability to "look" cannot meet the real needs of the multi-rotor unmanned aerial vehicle interactive operation in the current complex scene. Therefore, researchers install a multi-joint mechanical arm on the multi-rotor unmanned aerial vehicle to form an aerial operation robot (also known as a flying mechanical arm or an unmanned aerial vehicle with an arm), so that it can not only maneuver at high speed and hover at a fixed point, but also can replace manual operation in various complex scenes to complete tasks such as material transportation, object grasping and pollution sampling, thereby significantly expanding the application field of unmanned aerial vehicles.

[0004] Although the aerial operation robot technology has made great progress, most of the existing researches are concentrated on the operation in static environment. When the target is in a motion state (such as the goods on the conveyor belt, the goods on the moving vehicle, the fruits swinging in the wind, etc.), the time-varying and uncertainty introduced by the dynamic target are intertwined with the inherent strong coupling, nonlinearity and under-actuation of the aerial operation robot, forming an extremely complex control problem, which makes the complexity of the operation task of the aerial operation robot increase exponentially. The grasping control method designed based on the static environment cannot effectively realize the tracking of the dynamic target, resulting in the reduction of the precision and efficiency of grasping. SUMMARY

[0005] Therefore, the technical problem to be solved by the present application is to overcome the problem that the existing technology cannot effectively realize the tracking of the dynamic target, resulting in the reduction of the precision and efficiency of grasping.

[0006] To solve the above technical problems, the present application provides a dynamic target grasping method for aerial operation robot based on image visual servoing, which comprises:

[0007] The force and torque of the mechanical arm on the unmanned aerial vehicle are regarded as the coupling disturbance of the unmanned aerial vehicle; a coupling disturbance model is established based on variable inertia parameters to calculate the coupling disturbance force vector and the coupling disturbance torque vector;

[0008] A virtual camera for a drone is constructed based on images of dynamic targets. The translational motion features and yaw motion features of the dynamic targets, as well as the translational motion feature errors and yaw motion feature errors of the dynamic targets, are calculated using the image moments of the virtual camera.

[0009] Based on the translational motion characteristic error of the dynamic target, the estimated translational velocity of the dynamic target is calculated using a translational motion robust filter observer; based on the yaw motion characteristic error of the dynamic target, the estimated yaw angular velocity of the dynamic target is calculated using a yaw motion robust filter observer.

[0010] The estimated translational velocity and coupled disturbance vector of the dynamic target are input into the UAV IBVS controller to obtain the control output of the UAV IBVS controller;

[0011] The estimated yaw rate and coupled disturbance torque vector of the dynamic target are input into the attitude controller to obtain the control output of the attitude controller;

[0012] The drone is controlled by the control outputs of the IBVS controller and the attitude controller.

[0013] Based on dynamic target images, a virtual camera for the robotic arm is constructed at the end of the robotic arm. The image moments of the virtual camera are used as the image features of the robotic arm to obtain the image feature error of the robotic arm.

[0014] The image feature error of the robotic arm and the estimated translation speed of the dynamic target are input into the robotic arm IBVS controller to obtain the control output of the robotic arm IBVS controller, thereby realizing the control of the robotic arm.

[0015] Preferably, based on variable inertia parameters, a coupled disturbance model is established, and the coupled disturbance force vector and coupled disturbance moment vector are calculated. The formulas include:

[0016] ;

[0017] ;

[0018] in, For the coupled perturbation force vector, The total mass of the aerial robot. Let be the rotation matrix from the body coordinate system to the inertial coordinate system. This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let the center of mass of the aerial robot be in the body coordinate system. for The first derivative, for The second derivative; For the coupled disturbance moment vector, The origin of the robotic arm relative to the body coordinate system. The moment of inertia matrix, for The first derivative, It is the acceleration due to gravity. It is a unit vector in the vertical direction. The velocity of the UAV in the inertial coordinate system The first derivative, The mass of the robotic arm; and These are variable inertial parameters.

[0019] Preferably, the translational and yaw motion characteristics of the dynamic target are calculated using image moments from the UAV virtual camera, and the formulas include:

[0020] ;

[0021] ;

[0022] in, The translational motion characteristics of dynamic targets, , and These are the x-direction, y-direction, and z-direction features of translational motion, respectively. The yaw motion characteristics of a dynamic target. Let be the desired height of the UAV virtual camera coordinate system from the dynamic target plane. Geometric properties of the feature point distribution. , and For the second-order image moments of the drone's virtual camera, for Expected value Let x be the mean of the x-coordinates of all image feature points in the UAV virtual camera coordinate system. Let be the mean of the y-coordinates of all image feature points in the UAV virtual camera coordinate system. This refers to the camera's focal length.

[0023] Preferably, based on the translational motion characteristic error of the dynamic target, the estimated translational velocity of the dynamic target is calculated using a translational motion robust filter observer, as follows:

[0024] A first-order low-pass filter is used to smooth the translational motion characteristic error of a dynamic target, resulting in the filtered translational motion characteristic error. ;

[0025] The image feature change rate caused by the UAV translation motion is smoothed by using a first-order low-pass filter to obtain a filtered image feature change rate caused by the UAV translation motion ;

[0026] A translation motion robust filter observer is constructed, and based on the translation motion feature error of the dynamic target, the filtered translation motion feature error, and the filtered image feature change rate caused by the UAV translation motion, an estimated translation velocity of the dynamic target is obtained, and the formula is:

[0027] ;

[0028] wherein, is the estimated translation velocity of the dynamic target, is an output of the translation motion robust filter observer at time t, is the filtered image feature change rate caused by the UAV translation motion at time t, is the filtered translation motion feature error at time t, is a first time constant, and t is time, is the translation motion feature error of the dynamic target at time t, is the translation motion feature error of the dynamic target at an initial time.

[0029] Preferably, based on the yaw motion feature error of the dynamic target, an estimated yaw angular velocity of the dynamic target is calculated by using a yaw motion robust filter observer, and the method is:

[0030] The yaw motion feature error of the dynamic target is smoothed by using a first-order low-pass filter to obtain a filtered yaw motion feature error ;

[0031] The image feature change rate caused by the UAV yaw motion is smoothed by using a first-order low-pass filter to obtain a filtered image feature change rate caused by the UAV yaw motion ;

[0032] A yaw motion robust filter observer is constructed, and based on the yaw motion feature error of the dynamic target, the filtered yaw motion feature error, and the filtered image feature change rate caused by the UAV yaw motion, an estimated yaw angular velocity of the dynamic target is obtained, and the formula is:

[0033] ;

[0034] wherein, is the estimated yaw angular velocity of the dynamic target, is an output of the yaw motion robust filter observer at time t, is the filtered image feature rate of change caused by the UAV yaw motion at time t, is the filtered yaw motion feature error at time t, is a second time constant, t is time, is the yaw motion feature error of the dynamic target at time t, is the yaw motion feature error of the dynamic target at initial time.

[0035] Preferably, the estimated translational velocity of the dynamic target and the coupled disturbance force vector are input into the UAV IBVS controller to obtain the control output of the UAV IBVS controller, which is given by:

[0036] ;

[0037] wherein, is the control output of the UAV IBVS controller, , is the transpose of the rotation matrix of the yaw angle of the UAV, is the coupled disturbance force vector, is the total mass of the aerial operating robot, and are control parameters of the UAV IBVS controller, is used to control the velocity of the UAV to reach the expectation, is the velocity of the UAV, is the translational motion feature error of the dynamic target, is the anti-symmetric matrix operation, is the yaw angular velocity of the UAV, is the unit vector in the vertical direction, is the estimated translational velocity of the dynamic target.

[0038] Preferably, the constraint conditions of the UAV IBVS controller and the translational motion robust filter observer include:

[0039] ;

[0040] ;

[0041] ;

[0042] wherein, and are control parameters of the UAV IBVS controller, is a first time constant.

[0043] Preferably, the estimated yaw angular velocity of the dynamic target and the coupled disturbance torque vector are input into the attitude controller to obtain the control output of the attitude controller, which is given by:

[0044] ;

[0045] wherein, is a control output of the attitude controller, is an angular velocity of the UAV, is an inertia matrix of the UAV, is a coupled disturbance torque vector, is a rotation matrix converting the angular velocity of the UAV into the attitude angular velocity, is a first derivative of , is a second derivative of the desired attitude of the UAV, and are control parameters of the attitude controller, for controlling the attitude error of the UAV to reach a desired value, is an attitude error of the UAV, is a first derivative of the desired attitude of the UAV, is an estimated yaw angular velocity of the dynamic target.

[0046] Preferably, the constraint conditions of the attitude controller and the yaw motion robust filter observer include:

[0047] ;

[0048] ;

[0049] ;

[0050] wherein, and are control parameters of the attitude controller, is a second time constant.

[0051] Preferably, the image feature error of the manipulator and the estimated translational velocity of the dynamic target are input into the manipulator IBVS controller to obtain a control output of the manipulator IBVS controller, and the formula is:

[0052] ;

[0053] wherein, is a control output of the manipulator IBVS controller, is an angle of a joint of the manipulator, is a Jacobian matrix of the manipulator, is a product of a rotation matrix of a pitch angle and a roll angle of the UAV, is a control parameter of the manipulator IBVS controller, is an image feature error of the manipulator, is a velocity of the UAV, an estimated translational velocity of the dynamic target, a first derivative of a first derivative of a position of a virtual camera coordinate system of the robot arm relative to a real camera coordinate system.

[0054] The above technical solutions of the present application have the following beneficial effects compared with the prior art:

[0055] The dynamic target grasping method of the aerial operation robot based on image visual servoing has the beneficial effects that: the variable inertia parameter is used to describe the coupling interference of the robot arm to the unmanned aerial vehicle, the coupling interference model is constructed, and the first derivative and the second derivative of the variable inertia parameter are included in the constructed coupling interference model, so that the model is applicable to the case of fast movement of the robot arm; secondly, the virtual camera of the unmanned aerial vehicle is constructed, the translational motion feature error and the yaw motion feature error of the dynamic target are calculated by using the collected dynamic target image, and the translational motion robust filter observer and the yaw motion robust filter observer are designed to estimate the translational velocity and the yaw angular velocity of the dynamic target respectively, so that the real-time tracking of the dynamic target by the unmanned aerial vehicle is realized; thirdly, the unmanned aerial vehicle IBVS controller and the attitude controller are used to calculate the control output of the unmanned aerial vehicle tracking the dynamic target, so that the tracking speed is improved while the tracking accuracy is ensured, and the aerial operation robot is ensured to track the dynamic target quickly; finally, the virtual camera of the robot arm is constructed at the end of the robot arm, the image feature error of the robot arm is calculated, and the control output of the robot arm IBVS controller is calculated and obtained, so that when the unmanned aerial vehicle deviates from the expected position, the control output of the robot arm IBVS controller can also be used to ensure that the robot arm realizes accurate grasping of the dynamic target. BRIEF DESCRIPTION OF DRAWINGS

[0056] In order to make the content of the present application more easily understood, the present application will be further described in detail below according to specific embodiments of the present application and in conjunction with the drawings, in which:

[0057] Figure 1 is a flowchart of the dynamic target grasping method of the aerial operation robot based on image visual servoing of the present application;

[0058] Figure 2 is a reference coordinate system definition diagram of the present application;

[0059] Figure 3 is a relationship diagram between the image plane and the virtual image plane of the present application;

[0060] Figure 4 is a control structure diagram of the present application. DETAILED DESCRIPTION

[0061] The application will be further described below in conjunction with the drawings and specific embodiments so that those skilled in the art can better understand the application and implement it, but the embodiments are not intended to limit the application.

[0062] Referring to Figure 1 As shown in the figure, the application proposes an aerial operating robot dynamic target grabbing method based on image visual servoing, comprising:

[0063] S1: The force and torque of the mechanical arm on the UAV are regarded as the coupling disturbance of the UAV; a coupling disturbance model is established based on variable inertia parameters, and a coupling disturbance force vector and a coupling disturbance torque vector are calculated;

[0064] S2: A UAV virtual camera is constructed based on the dynamic target image, and the image matrix of the UAV virtual camera is used to calculate the translation motion feature and the yaw motion feature of the dynamic target, as well as the translation motion feature error and the yaw motion feature error of the dynamic target;

[0065] S3: The estimated translation velocity of the dynamic target is calculated by using a translation motion robust filter observer based on the translation motion feature error of the dynamic target, and the estimated yaw angular velocity of the dynamic target is calculated by using a yaw motion robust filter observer based on the yaw motion feature error of the dynamic target;

[0066] S4: The estimated translation velocity of the dynamic target and the coupling disturbance force vector are input into the UAV IBVS controller to obtain the control output of the UAV IBVS visual servoing (Image-Based Visual Servoing) controller;

[0067] S5: The estimated yaw angular velocity of the dynamic target and the coupling disturbance torque vector are input into the attitude controller to obtain the control output of the attitude controller;

[0068] S6: The control of the UAV is realized by using the control output of the UAV IBVS controller and the control output of the attitude controller;

[0069] S7: A mechanical arm virtual camera is constructed at the end of the mechanical arm based on the dynamic target image, and the image matrix of the mechanical arm virtual camera is used as the image feature of the mechanical arm to obtain the image feature error of the mechanical arm;

[0070] S8: The image feature error of the mechanical arm and the estimated translation velocity of the dynamic target are input into the mechanical arm IBVS controller to obtain the control output of the mechanical arm IBVS controller, and the control of the mechanical arm is realized.

[0071] The aerial operating robot used in this embodiment is composed of a four-rotor UAV and a three-degree-of-freedom mechanical arm, and its coordinate system is as shown in Figure 2The third degree of freedom in the mechanical arm only controls the opening and closing of the end gripper, and will not cause changes in the motion state of the mechanical arm. Therefore, the three-degree-of-freedom mechanical arm is abstracted into a two-degree-of-freedom mechanical arm in this embodiment. In this embodiment, the and respectively represent the inertial coordinate system and the body coordinate system, the origin of is the center of mass of the quadrotor unmanned aerial vehicle, points to the direction of the head of the quadrotor unmanned aerial vehicle, points to the ground. The n-th link coordinate system of the mechanical arm is represented by , which can be established according to the MDH parameter table.

[0072] Specifically, in S1, the force and torque of the mechanical arm on the unmanned aerial vehicle are regarded as the coupled disturbance of the unmanned aerial vehicle; based on the variable inertia parameters (i.e., the changes in the center of mass and the moment of inertia), a coupled disturbance model is established to calculate the coupled disturbance force vector and the coupled disturbance torque vector, and the specific derivation steps include:

[0073] The center of mass of the n-th link of the mechanical arm , the velocity and the angular velocity in the body coordinate system are as follows:

[0074] ;

[0075] wherein, represents the joint angle of the mechanical arm, represents the joint angular velocity of the mechanical arm, represents the conversion matrix from the n-th link coordinate system to the body coordinate system, represents the Jacobian matrix of the center of mass of the n-th link of the mechanical arm relative to the body coordinate system, represents the center of mass of the n-th link of the mechanical arm in the n-th link coordinate system.

[0076] The expression of the center of mass of the entire aerial operation robot system in the body coordinate system is as follows:

[0077] ;

[0078] wherein, represents the center of mass of the aerial operation robot in the body coordinate system, is the first derivative of , and is the total mass of the aerial operation robot, is the mass of the n-th link of the mechanical arm, is the index of the link of the mechanical arm.

[0079] The expression of the system moment of inertia in the body coordinate system is as follows:

[0080] ;

[0081] in, The origin of the robotic arm relative to the body coordinate system. The moment of inertia matrix, for The first derivative, Let be the rotation matrix from the coordinate system of the nth link of the robotic arm to the coordinate system of the body. Let be the inertia matrix of the nth link of the robotic arm in the nth link coordinate system. for The identity matrix, For antisymmetric matrix operations, Let be the angular velocity of the nth link in the body coordinate system.

[0082] Assumption Let p be the position of any point mass in the aerial robot relative to the inertial coordinate system. The formula for calculating it is as follows:

[0083] ;

[0084] in, for The first derivative, Let be the position of the UAV's center of mass relative to the inertial coordinate system. for The first derivative, Let be the position vector from the center of mass of the UAV to the system mass point p in the inertial coordinate system. Let be the rotation matrix from the body coordinate system to the inertial coordinate system. Let be the position vector from the center of mass of the UAV to the mass point p of the aerial robot in the body coordinate system. for The first derivative, This is the angular velocity vector of the UAV in the body coordinate system.

[0085] Based on the definitions of momentum and angular momentum of a multi-rigid-body system, the momentum of the aerial robot is obtained. and angular momentum for:

[0086] ;

[0087] in, Let the system's center of mass be in the inertial coordinate system. For the rotational inertia matrix of the UAV, The mass of the robotic arm.

[0088] According to the momentum and the moment of momentum theorem, the momentum and the moment of momentum of the aerial manipulation robot are derived as follows:

[0089] ;

[0090] wherein, is the time, is the total external force acting on the aerial manipulation robot in the inertial coordinate system, is the total moment acting on the aerial manipulation robot in the inertial coordinate system.

[0091] For the aerial manipulation robot system of the rotor unmanned aerial vehicle, the following detailed expression can be obtained:

[0092] ;

[0093] wherein, is the lift generated by the unmanned aerial vehicle propeller, is the gravity acceleration, is the vertical unit vector, is the moment generated by the unmanned aerial vehicle propeller.

[0094] The momentum and the moment of momentum of the above-mentioned aerial manipulation robot are derived, and combined with the expressions of , , and , the dynamics model of the aerial manipulation robot can be derived as follows:

[0095] ;

[0096] wherein, is the second-order derivative of , is the second-order derivative of , is the first-order derivative of .

[0097] Different from the conventional dynamics model of the aerial manipulation robot, the dynamics model constructed by the present application contains the dynamically changing inertial parameters and the additional coupling disturbance items caused thereby, which are closely related to the states of the unmanned aerial vehicle and the manipulator and the derivatives thereof. In theory, the dynamics model describes the real dynamic characteristics of the aerial manipulation robot under the coupling disturbance.

[0098] Through the simplification of the dynamics model of the aerial manipulation robot, the coupling disturbance model based on the variable inertial parameters is obtained to calculate the coupling disturbance force vector and the coupling disturbance moment vector, and the expressions are as follows:

[0099] ;

[0100] in, For the coupled perturbation force vector, The total mass of the aerial robot. Let be the rotation matrix from the body coordinate system to the inertial coordinate system. This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let the center of mass of the aerial robot be in the body coordinate system. for The first derivative, for The second derivative; For the coupled disturbance moment vector, The origin of the robotic arm relative to the body coordinate system. The moment of inertia matrix, for The first derivative, It is the acceleration due to gravity. It is a unit vector in the vertical direction. The velocity of the UAV in the inertial coordinate system The first derivative, The mass of the robotic arm; and These are variable inertial parameters.

[0101] Combining the coupling interference model, the dynamic model of the aerial robot system can be represented in the following form:

[0102] ;

[0103] in, and These are the position and velocity vectors of the UAV in the inertial coordinate system, respectively. for The first derivative, These represent the attitude angles of the UAV in the inertial coordinate system, namely roll angle, pitch angle, and yaw angle. for The first derivative, To convert the angular velocity of the UAV into its attitude angular velocity, Let be the angular velocity of the UAV in the body coordinate system. Let be the inertial matrix of the UAV in the body coordinate system.

[0104] Specifically, in S2, a UAV virtual camera is constructed based on the dynamic target image, translation motion features and yaw motion features of the dynamic target, and translation motion feature errors and yaw motion feature errors of the dynamic target are calculated by using an image matrix of the UAV virtual camera, and specific derivation steps include:

[0105] The virtual camera is an ideal reference camera model constructed for the UAV, and the core purpose is to provide a constant image feature reference benchmark. Referring to Figure 3 , a real camera coordinate system and a UAV virtual camera coordinate system are defined as and respectively. By rotating the body coordinate system to align with the z-axis of the inertial coordinate system , can be obtained . Define to coincide with the body coordinate system (the body coordinate system is the same as the body coordinate system ). Define as the tracking target coordinate system.

[0106] Define as the pixel coordinates of the feature points of the image of the dynamic target in the real camera plane, as the pixel coordinates of the feature points of the virtual image formed by the dynamic target in the UAV virtual camera plane. Through the homography transformation of the image plane, can be calculated from :

[0107]

[0108] wherein, and respectively represent the image depth of the real camera and the UAV virtual camera, is the intrinsic matrix of the camera, is the focal length of the camera, and are respectively the rotation matrix of the pitch angle and the roll angle.

[0109] The image moments and are selected as the image features for adjusting the translation motion and the yaw motion respectively in this embodiment, and the formula definitions are as follows:

[0110]

[0111] wherein, is the translation motion feature of the dynamic target, , and ​​​x-direction feature, y-direction feature and z-direction feature of translational motion feature respectively, yaw motion feature of dynamic target, expected height of virtual camera coordinate system of UAV from dynamic target plane, geometric property of feature point distribution, 、 and second order image moment of virtual camera of UAV, expected value of , representing expected image feature moment, mean value of x coordinate of all image feature points in virtual camera coordinate system of UAV, mean value of y coordinate of all image feature points in virtual camera coordinate system of UAV, focal length of actual camera.

[0112] The formula of second order image moment is: wherein is total number of image feature points, and are x coordinate and y coordinate of kth image feature point in virtual camera coordinate system of UAV respectively.

[0113] Image feature kinematics in virtual image plane can be expressed as:

[0114] ;

[0115] ;

[0116] wherein, is first order derivative of , is first order derivative of , and are translational velocity of UAV and dynamic target in virtual camera coordinate system respectively. and are yaw angular velocity of UAV and dynamic target in virtual camera coordinate system respectively.

[0117] Combining , and introducing coupling interference model, the first order derivative of is:

[0118] ;

[0119] wherein, is representation of thrust vector in virtual camera coordinate system of UAV, ​a lift force generated by a propeller of the UAV, , a transpose of a rotation matrix of a yaw angle of the UAV.

[0120] Assuming that the translational velocity and the yaw angular velocity of the dynamic target are bounded, they are expressed as follows:

[0121] ;

[0122] wherein is a finite constant.

[0123] Defining the desired translational motion feature and the desired yaw motion feature of the dynamic target in the virtual image plane as and respectively, the translational motion feature error of the dynamic target is , and the yaw motion feature error of the dynamic target is .

[0124] Derivatives of the translational motion feature error and the yaw motion feature error of the dynamic target are as follows:

[0125] ;

[0126] .

[0127] In the face of the unmeasurable dynamic target velocity, the present application designs a robust filtering observer to estimate the translational velocity and the yaw angular velocity of the dynamic target. The UAV controls the dynamic target to keep in the center of the virtual image plane while keeping the desired height and yaw angle.

[0128] Specifically, in S3, based on the translational motion feature error of the dynamic target, the estimated translational velocity of the dynamic target is calculated by using the translational motion robust filtering observer, and the method is as follows:

[0129] S31: A first-order low-pass filter is used to smooth the translational motion feature error of the dynamic target, and the filtered translational motion feature error is obtained.

[0130] The expression of the first-order low-pass filter is as follows: wherein is a first-order derivative of , and is the filtered translational motion feature error at the initial time.

[0131] S32: Smooth the image feature change rate caused by the UAV translation motion using a first-order low-pass filter to obtain a filtered image feature change rate caused by the UAV translation motion .

[0132] The expression of the first-order low-pass filter is: wherein is a first-order derivative of , and is the filtered image feature change rate caused by the UAV translation motion at the initial moment.

[0133] S33: Construct a translation motion robust filter observer, and define as the output of the translation motion robust filter observer, and the expression of the translation motion robust filter observer is: wherein is a first-order derivative of , and is the output of the translation motion robust filter observer at the initial moment.

[0134] The output of the translation motion robust filter observer can be expressed based on the translation motion feature error of the dynamic target, the filtered translation motion feature error, and the filtered image feature change rate caused by the UAV translation motion as:

[0135] ;

[0136] wherein is the output of the translation motion robust filter observer at t, is the filtered image feature change rate caused by the UAV translation motion at t, is the filtered translation motion feature error at t, is a first time constant, t is time, is the translation motion feature error of the dynamic target at t, is the translation motion feature error of the dynamic target at the initial moment.

[0137] Therefore, based on the translation motion feature error of the dynamic target, the filtered translation motion feature error, and the filtered image feature change rate caused by the UAV translation motion, the estimated translation velocity of the dynamic target is:

[0138] ;

[0139] wherein is the estimated translation velocity of the dynamic target.

[0140] The estimation error of the translation velocity of the dynamic target is . The derivative is: ,in, for The first derivative.

[0141] Similarly, in S3, based on the yaw motion characteristic error of the dynamic target, the estimated yaw angular velocity of the dynamic target is calculated using a yaw motion robust filter observer. The method is as follows:

[0142] S34: Using a first-order low-pass filter, the yaw motion characteristic error of the dynamic target is smoothed to obtain the filtered yaw motion characteristic error. .

[0143] The expression for this first-order low-pass filter is: ,in for The first derivative, The yaw motion characteristic error is the filtered value at the initial moment.

[0144] S35: Using a first-order low-pass filter, the rate of change of image features caused by the yaw motion of the UAV is smoothed to obtain the filtered rate of change of image features caused by the yaw motion of the UAV. .

[0145] The expression for this first-order low-pass filter is: ,in for The first derivative, The rate of change of image features caused by the yaw motion of the UAV after filtering at the initial moment.

[0146] S36: Construct a yaw motion robust filter observer, define This is the output of the yaw motion robust filter observer, whose expression is: ,in, for The first derivative, This is the output of the robust filter observer for yaw motion at the initial moment.

[0147] Output of yaw motion robust filter observer It can be expressed based on the yaw motion feature error of the dynamic target, the filtered yaw motion feature error, and the rate of change of image features caused by the filtered UAV yaw motion:

[0148] ;

[0149] in, The output of the yaw motion robust filter observer at time t. Let be the rate of change of image features caused by the yaw motion of the UAV after filtering at time t. Let be the yaw motion characteristic error after filtering at time t. The second time constant, Let be the yaw motion characteristic error of the dynamic target at time t. This represents the yaw motion characteristic error of the dynamic target at the initial moment.

[0150] Therefore, based on the yaw motion feature error of the dynamic target, the filtered yaw motion feature error, and the rate of change of image features caused by the filtered UAV yaw motion, the estimated yaw angular velocity of the dynamic target is obtained as follows:

[0151] ;

[0152] in, The estimated yaw rate for a dynamic target.

[0153] The estimation error of the translational velocity of a dynamic target is . The derivative is: ,in, for The first derivative.

[0154] In S4, the design method of the UAV IBVS controller is as follows: based on the estimated translational velocity of the dynamic target, the UAV IBVS controller is designed using the backstepping method.

[0155] The first Lyapunov function is defined as follows: :

[0156] .

[0157] according to The formula can be obtained first derivative for:

[0158] .

[0159] In the first step of the anti-stepping design, the speed of the drone is taken into account. As a control input, define This is to ensure that the translational motion characteristic error of the dynamic target approaches zero. These are the control parameters for the UAV's IBVS controller. In the second step of the backstepping design, the second error term is defined. It is used to control the speed of the drone to achieve the desired value.

[0160] pass renew and The expression of

[0161] ;

[0162] .

[0163] According to the formula of , and the updated expression of and , the first derivative of is obtained as follows:

[0164] .

[0165] Consider the second Lyapunov function :

[0166] .

[0167] According to the updated expression of , the expression of and the expression of , the derivative of is obtained as follows:

[0168] .

[0169] Define as the control input, and design the following expression of so that the UAV IBVS controller designed by the application can obtain the control output of the UAV IBVS controller with the estimated translational velocity of the dynamic target and the coupled disturbance force vector :

[0170] ;

[0171] wherein, is the control output of the UAV IBVS controller, , is the transpose of the rotation matrix of the UAV yaw angle, is the coupled disturbance force vector, is the total mass of the aerial operating robot, and are control parameters of the UAV IBVS controller, , is used to control the speed of the UAV to reach the expected value, is the speed of the UAV in the body coordinate system, is the translational motion characteristic error of the dynamic target, is the anti-symmetric matrix operation,​​ is the yaw angular velocity of the UAV in the UAV virtual camera coordinate system, is the unit vector in the vertical direction, is the estimated translational velocity of the dynamic target.

[0172] The expression of is substituted into the expression of , assuming that the translational velocity and the yaw angular velocity of the dynamic target are bounded, and Young's inequality is applied, so that can be rewritten as:

[0173] .

[0174] Therefore, the constraint conditions of the UAV IBVS controller and the translational motion robust filter observer can be obtained as:

[0175] ;

[0176] ;

[0177] ;

[0178] According to the principle of Uniformly Ultimately Bounded (UUB), under the constraint conditions, the image error , and the observer estimation error are uniformly ultimately bounded.

[0179] Let , , and be the components of in the x, y and z directions, respectively. By decoupling , the desired lift, roll angle and pitch angle of the UAV can be obtained as:

[0180] ;

[0181] where is the desired lift of the UAV, is the desired roll angle of the UAV, is the desired pitch angle of the UAV.

[0182] Since the yaw angle of the UAV relative to the dynamic target is unknown, the yaw motion feature of the dynamic target in the image feature is used to replace the yaw angle in the attitude controller. Let be the attitude of the UAV, and the desired attitude The attitude error of the UAV is defined as . The derivative of

[0183] ;

[0184] where is the first derivative of .

[0185] In S5, the design method of the attitude controller is as follows: based on the estimated yaw angular velocity and the coupled disturbance moment vector of the dynamic target, the attitude controller is designed by using the backstepping method.

[0186] A third Lyapunov function is defined as :

[0187] .

[0188] The derivative of is expressed as:

[0189] .

[0190] The attitude error of the UAV is taken as the error term designed in the first step of the backstepping method. In the second step of the backstepping method, a second error term is defined to control the attitude error of the UAV to reach the expected value , and

[0191] The derivative of

[0192] ;

[0193] where is the second derivative of the expected attitude of the UAV.

[0194] The derivative of

[0195] .

[0196] A fourth Lyapunov function is considered as :

[0197] .

[0198] According to the expression of , the derivative of is expressed as:​​

[0199] .

[0200] Let be the control input, the expression of is designed as follows, so that the attitude controller designed by the present application can obtain the control output of the attitude controller with the estimated translational velocity of the dynamic target and the coupled disturbance force vector :

[0201] ;

[0202] wherein, is the control output of the attitude controller, is the angular velocity of the UAV, is the inertia matrix of the UAV, is the coupled disturbance torque vector, is the rotation matrix for converting the angular velocity of the UAV into the attitude angular velocity, is the first-order derivative of , is the second-order derivative of the desired attitude of the UAV, and are the control parameters of the attitude controller, , for controlling the attitude error of the UAV to reach the desired value, is the attitude error of the UAV, is the first-order derivative of the desired attitude of the UAV, is the estimated yaw angular velocity of the dynamic target.

[0203] Substitute the expression of the above into the expression of , and apply the Young inequality to rewrite as:

[0204] .

[0205] Therefore, the constraint conditions of the attitude controller and the yaw motion robust filtering observer can be obtained as:

[0206] ;

[0207] ;

[0208] ;

[0209] According to the UUB principle, under the constraint conditions, the image errors , and the observer estimation errors Errors are consistent and eventually bounded.

[0210] To achieve precise grasping by the robotic arm, this invention also designs a robotic arm vision servo controller based on a virtual camera.

[0211] In S7, a virtual camera for the robotic arm is constructed at the end effector of the robotic arm based on a dynamic target image. The image moments of the virtual camera are used as image features of the robotic arm to obtain the image feature error of the robotic arm. The steps include:

[0212] A virtual camera for the robotic arm is constructed to compensate for tracking errors from the drone and ensure grasping accuracy. The virtual camera is built on the end effector and defined as follows: . Compared to Position defined This can be obtained through the forward kinematics of the robotic arm.

[0213] Through homography transformation between image planes. Through homography transformation between image planes, the following formula can be used to... Calculate the image of the dynamic target in the virtual camera plane of the robotic arm. :

[0214] ;

[0215] in, The pixel coordinates of feature points in the virtual image formed by the virtual camera plane of the robotic arm representing the dynamic target. Calculated by the drone's virtual camera; and These represent the image depths of the robotic arm's virtual camera and the drone's virtual camera, respectively. yes and Rotation matrix between them. yes Compared to Location, , and These are the rotation matrices for pitch and roll angles, respectively; It is the unit normal vector of the dynamic target plane in the UAV virtual camera coordinate system. It is the distance from the target image plane to the origin of the drone's virtual camera. The distance.

[0216] Using the image moments of the virtual camera of the robotic arm as the image features of the robotic arm The calculation method is the same as that for the translational motion characteristics of dynamic targets, and will not be elaborated here.

[0217] definition is the image feature error of the robot arm, and . is derived from the grasping task.

[0218] The first derivative of

[0219] ;

[0220] wherein is the velocity of the robot arm end-effector with respect to , which can be expressed as:

[0221] ;

[0222] wherein is the first derivative of , is the Jacobian matrix of the robot arm, is the joint angle of the robot arm, is the first derivative of , i.e., the joint velocity of the robot arm.

[0223] A robot arm IBVS controller is designed based on the robot arm virtual camera to achieve accurate positioning of the end-effector. In the designed robot arm IBVS controller, the output of the robust filter observer is used to compensate for the unpredictable relative velocity between the UAV and the dynamic target. Therefore, the end-effector can achieve accurate grasping even if the UAV deviates from the required position.

[0224] In S8, the design method of the robot arm IBVS controller is: based on the image feature error of the robot arm and the estimated translation velocity of the dynamic target, a backstepping method is used to design the robot arm IBVS controller.

[0225] The image feature error of the robot arm is defined as the error term of the backstepping method as follows:

[0226] .

[0227] The derivative of can be expressed as:

[0228] .

[0229] In the design of the backstepping method, consider taking as the virtual control input, set , is the control parameter of the robot arm IBVS controller, combined with ​The expression of the image feature error of the manipulator can be deduced to the actual input of the controller .

[0230] Therefore, the image feature error of the manipulator and the estimated translational velocity of the dynamic target are input into the manipulator IBVS controller, and the control output of the manipulator IBVS controller is obtained as follows:

[0231] ;

[0232] Wherein, is the control output of the manipulator IBVS controller, is the angle of the joint of the manipulator, is the Jacobian matrix of the manipulator, is the product of the rotation matrix of the pitch angle and the roll angle of the unmanned aerial vehicle, is the control parameter of the manipulator IBVS controller, is the image feature error of the manipulator, is the velocity of the unmanned aerial vehicle, is the estimated translational velocity of the dynamic target, is the first derivative of , is the position of the virtual camera coordinate system of the manipulator relative to the real camera coordinate system.

[0233] Substituting the expression of into the expression of , the expression of is rewritten as:

[0234] .

[0235] The constraint condition of the manipulator IBVS controller and the observer is: , so that the feature error and the estimation error are uniformly ultimately bounded.

[0236] The control structure of the whole application is shown in Figure 4 .

[0237] In summary, the aerial operation robot dynamic target grabbing method based on image visual servoing has the advantages that: the variable inertia parameter is used to describe the coupling interference of the mechanical arm to the unmanned aerial vehicle, a coupling interference model is constructed, and the first order derivative and the second order derivative of the variable inertia parameter are included in the constructed coupling interference model, so that the model is applicable to the case of fast movement of the mechanical arm; secondly, the unmanned aerial vehicle virtual camera is constructed, the translation motion feature error and the yaw motion feature error of the dynamic target are calculated by using the collected dynamic target image, and the translation motion robust filter observer and the yaw motion robust filter observer are designed to estimate the translation speed and the yaw angular velocity of the dynamic target respectively, so that the real-time tracking of the dynamic target by the unmanned aerial vehicle is realized; thirdly, the unmanned aerial vehicle IBVS controller and the attitude controller are used to calculate the control output of the unmanned aerial vehicle tracking the dynamic target, so that the tracking speed is improved while the tracking accuracy is ensured, and the aerial operation robot is ensured to track the dynamic target quickly; finally, the mechanical arm virtual camera is constructed at the end of the mechanical arm, the image feature error of the mechanical arm is calculated, the control output of the mechanical arm IBVS controller is calculated and obtained, so that when the unmanned aerial vehicle deviates from the expected position, the control output of the mechanical arm IBVS controller can also be used to ensure that the dynamic target is accurately grabbed by the mechanical arm.

[0238] Those skilled in the art will understand that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer usable program code.

[0239] The present application is described with reference to the flowcharts and / or block diagrams according to the methods, devices (systems), and computer program products of the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to produce a machine, so that the instructions executed by the computer or other programmable data processing devices produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 The device that implements the functions specified in one flow or multiple flows and / or blocks. Figure 1 The device that implements the functions specified in one flow or multiple flows and / or blocks.

[0240] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the flow Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0241] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions that execute on the computer or other programmable apparatus provide steps for implementing the flow Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0242] Obviously, the above-described embodiments are only examples and are not intended to limit the present application. Based on the above description, one of ordinary skill in the art can make other various changes or modifications to the present application. Here, it is not necessary or possible to exhaust all the embodiments. The obvious changes or modifications derived from the present application are still within the scope of the present application.

Claims

1. A method for dynamic target grasping of an aerial robot based on image visual servoing, characterized in that, include: The force and torque exerted by the robotic arm on the drone are considered as coupled interference to the drone; Based on variable inertia parameters, a coupled disturbance model is established, and the coupled disturbance force vector and coupled disturbance moment vector are calculated; wherein, the expressions for the coupled disturbance force vector and the coupled disturbance moment vector are respectively: ； ； in, For the coupled perturbation force vector, The total mass of the aerial robot. Let be the rotation matrix from the body coordinate system to the inertial coordinate system. This is the angular velocity vector of the UAV in the body coordinate system. for The first derivative, Let the center of mass of the aerial robot be in the body coordinate system. for The first derivative, for The second derivative; For the coupled disturbance moment vector, The origin of the robotic arm relative to the body coordinate system. The moment of inertia matrix, for The first derivative, It is the acceleration due to gravity. It is a unit vector in the vertical direction. The velocity of the UAV in the inertial coordinate system The first derivative, The mass of the robotic arm; and These are variable inertial parameters; A virtual camera for a UAV is constructed based on images of dynamic targets. The translational and yaw motion features of the dynamic targets, as well as the translational and yaw motion feature errors, are calculated using the image moments from the virtual camera. The expressions for the translational and yaw motion features are as follows: ； ； in, The translational motion characteristics of dynamic targets, , and These are the x-direction, y-direction, and z-direction features of translational motion, respectively. The yaw motion characteristics of a dynamic target. Let be the desired height of the UAV virtual camera coordinate system from the dynamic target plane. Geometric properties of the feature point distribution. , and For the second-order image moments of the drone's virtual camera, for Expected value Let x be the mean of the x-coordinates of all image feature points in the UAV virtual camera coordinate system. Let be the mean of the y-coordinates of all image feature points in the UAV virtual camera coordinate system. The focal length of the camera; Based on the translational motion characteristic error of the dynamic target, the estimated translational velocity of the dynamic target is calculated using a translational motion robust filter observer; based on the yaw motion characteristic error of the dynamic target, the estimated yaw angular velocity of the dynamic target is calculated using a yaw motion robust filter observer. The estimated translational velocity and coupled disturbance vector of the dynamic target are input into the UAV IBVS controller to obtain the control output of the UAV IBVS controller; The estimated yaw rate and coupled disturbance torque vector of the dynamic target are input into the attitude controller to obtain the control output of the attitude controller; The drone is controlled by the control outputs of the IBVS controller and the attitude controller. Based on dynamic target images, a virtual camera for the robotic arm is constructed at the end of the robotic arm. The image moments of the virtual camera are used as the image features of the robotic arm to obtain the image feature error of the robotic arm. The image feature error of the robotic arm and the estimated translation speed of the dynamic target are input into the robotic arm IBVS controller to obtain the control output of the robotic arm IBVS controller, thereby realizing the control of the robotic arm.

2. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 1, characterized in that, Based on the translational motion characteristic error of a dynamic target, the estimated translational velocity of the dynamic target is calculated using a translational motion robust filter observer. The method is as follows: A first-order low-pass filter is used to smooth the translational motion characteristic error of a dynamic target, resulting in the filtered translational motion characteristic error. ; A first-order low-pass filter is used to smooth the rate of change of image features caused by the translational motion of the UAV, resulting in the filtered rate of change of image features caused by the UAV's translational motion. ; A robust translational motion filter observer is constructed. Based on the translational motion feature error of the dynamic target, the filtered translational motion feature error, and the rate of change of image features caused by the filtered UAV translational motion, the estimated translational velocity of the dynamic target is obtained, as shown in the formula: ； in, For estimating the translational velocity of a dynamic target, The output of the translational motion robust filter observer at time t. Let be the rate of change of image features caused by the translational motion of the UAV after filtering at time t. The translational motion characteristic error after filtering at time t. Let t be the first time constant, and t be time. Let be the translational motion characteristic error of the dynamic target at time t. This represents the translational motion characteristic error of the dynamic target at the initial moment.

3. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 1, characterized in that, Based on the yaw motion characteristic error of a dynamic target, the estimated yaw angular velocity of the dynamic target is calculated using a yaw motion robust filter observer. The method is as follows: A first-order low-pass filter is used to smooth the yaw motion characteristic error of a dynamic target, resulting in the filtered yaw motion characteristic error. ; A first-order low-pass filter is used to smooth the rate of change of image features caused by the yaw motion of the UAV, resulting in the filtered rate of change of image features caused by the yaw motion of the UAV. ; A robust yaw motion filter observer is constructed. Based on the yaw motion characteristic error of the dynamic target, the filtered yaw motion characteristic error, and the rate of change of image features caused by the filtered UAV yaw motion, the estimated yaw angular velocity of the dynamic target is obtained, as shown in the formula: ； in, For estimating the yaw rate of a dynamic target, The output of the yaw motion robust filter observer at time t. Let be the rate of change of image features caused by the yaw motion of the UAV after filtering at time t. Let be the yaw motion characteristic error after filtering at time t. The second time constant is t, where t is time. Let be the yaw motion characteristic error of the dynamic target at time t. This represents the yaw motion characteristic error of the dynamic target at the initial moment.

4. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 2, characterized in that, The estimated translational velocity and coupled disturbance vector of the dynamic target are input into the UAV IBVS controller to obtain the control output of the UAV IBVS controller, as shown in the formula: ； in, For the control output of the UAV's IBVS controller, , This is the transpose of the rotation matrix for the UAV's yaw angle. For the coupled perturbation force vector, The total mass of the aerial robot. and These are the control parameters for the UAV's IBVS controller. Used to control the speed of the drone to achieve the desired result. For the speed of the drone, The translational motion characteristic error of a dynamic target. For antisymmetric matrix operations, Let be the yaw rate of the drone. It is a unit vector in the vertical direction. This is the estimated translational velocity for a dynamic target.

5. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 4, characterized in that, The constraints for the UAV IBVS controller and translational motion robust filter observer include: ； ； ； in, and These are the control parameters for the UAV's IBVS controller. This is the first time constant.

6. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 3, characterized in that, The estimated yaw rate and coupled disturbance moment vector of the dynamic target are input into the attitude controller to obtain the control output of the attitude controller, as shown in the formula: ； in, The control output of the attitude controller. The angular velocity of the drone, For the inertial matrix of the UAV, For the coupled disturbance moment vector, To convert the angular velocity of the UAV into its attitude angular velocity, for The first derivative, Let be the second derivative of the desired attitude of the UAV. and These are the control parameters for the attitude controller. Used to control the attitude error of the drone to achieve the desired value. For the attitude error of the drone, Let be the first derivative of the desired attitude of the UAV. The estimated yaw rate for a dynamic target.

7. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 6, characterized in that, The constraints for the attitude controller and the yaw motion robust filter observer include: ； ； ； in, and These are the control parameters for the attitude controller. This is the second time constant.

8. The method for dynamic target grasping of an aerial robot based on image visual servoing according to claim 1, characterized in that, The image feature error of the robotic arm and the estimated translational velocity of the dynamic target are input into the robotic arm IBVS controller to obtain the control output of the robotic arm IBVS controller, as shown in the formula: ； in, For the control output of the robotic arm's IBVS controller, For the angle of the robotic arm joint, Let Jacobian matrix be the value of the robotic arm. This is the product of the rotation matrices for the UAV's pitch and roll angles. For the control parameters of the robotic arm's IBVS controller, Image feature error of the robotic arm, For the speed of the drone, For estimating the translational velocity of a dynamic target, for The first derivative, This refers to the position of the robotic arm's virtual camera coordinate system relative to the real camera coordinate system.

Images