Active landing method of UAV based on robotic arm assistance

Abstract

The present invention discloses a method for active landing of unmanned aerial vehicles based on mechanical arm assistance, which belongs to the technical field of unmanned aerial vehicle landing, including: modeling the dynamic models of unmanned aerial vehicles and mechanical arms respectively, wherein the state of the end effector of the mechanical arm is modeled as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the attitude; the optimization objective function of the optimal control of the active landing process of the unmanned aerial vehicle based on the assistance of the mechanical arm is set to minimize the time functional and energy functional of the state trajectory, and the trajectory end constraint and trajectory process constraint of the landing are given; the state trajectory with constraints in the optimization objective function is converted into an unconstrained optimization objective function, and the unconstrained optimization objective function is solved by the L‑BFGS algorithm to realize the active landing control of the unmanned aerial vehicle based on the assistance of the mechanical arm. This method can be used for stronger anti-disturbance capability, and can realize faster and more energy-efficient active landing, and improve the landing success rate.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle landing, and in particular relates to an active unmanned aerial vehicle landing method based on the assistance of a mechanical arm. Background Art

[0002] Drones are widely used in many fields due to their advantages such as light weight, high agility and strong maneuverability. However, their limited payload and flight time seriously hinder the realization of the above advantages. Many studies have explored ways to cooperate with ground systems that can support the takeoff, landing, maintenance and storage of drones, thereby ensuring continuous mission execution and expanding the time and space range in which drones can be used. To achieve such cooperation with ground systems, successful landing is the first step.

[0003] There have been many works that have made great progress in the landing of drones, achieving landing on static or moving targets. Drone landing can be generally divided into passive landing and active landing. Passive landing allows the drone to complete all landing tasks by itself, while active landing uses a second robot such as a robotic arm to assist in landing, thereby enhancing the robustness of landing and improving the accuracy and success rate of landing. For passive landing, although the existing technology provides a variety of efficient planning methods, the limitations of the system itself make it easy to fail to re-plan when disturbed, reducing the accuracy and success rate of passive landing.

[0004] In view of the defects that the accuracy and success rate of passive landing are greatly affected by disturbances, an active landing system for UAVs with robot-assisted landing is proposed, and theoretical analysis and simulation experiments are carried out. The control method of robot-assisted landing in the touchdown phase is also further studied. A system that uses a mobile robot to complete the take-off and landing mission cycle of the UAV is also proposed. It has good compatibility with different types of UAVs and can work under conditions that are unfavorable to passive landing without specially designed landing devices. The above active landing work generally adopts the vertical take-off and landing (VTOL) method, and only moves the robot arm in the final touchdown phase to cooperate with the UAV to complete the landing. There is a lack of planning methods for the entire landing process. To do this, the UAV needs to move above the robot arm first, and only use the movement of the UAV to counter disturbances in most stages of landing. The movement capacity of the robot arm is not fully utilized, making the entire landing process inefficient.

[0005] In addition, active landing achieved using only control methods cannot take into account some complex task constraints, such as maintaining the accuracy of the visual relative positioning of the drone and the robotic arm, the existence of the inverse kinematics solution of the robotic arm, and no self-collision. If such constraints are not considered, the success rate of the system during actual operation will be affected. Summary of the invention

[0006] In view of the above, the purpose of the present invention is to provide an active landing method for a UAV based on the assistance of a robotic arm, which method is used for stronger anti-disturbance capability, and can achieve faster and more energy-efficient active landing, and can ensure that both the robotic arm and the UAV meet the task constraints necessary for active landing, thereby improving the success rate of landing.

[0007] To achieve the above-mentioned purpose of the invention, an embodiment provides a method for actively landing a UAV based on the assistance of a mechanical arm, comprising the following steps:

[0008] The UAV and the robotic arm are modeled with dynamic models respectively, where the state of the end effector of the robotic arm is modeled as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the posture;

[0009] The optimization objective function of the optimal control of the active landing process of the UAV assisted by the manipulator is set to minimize the time functional and energy functional of the state trajectory, and the trajectory end constraint and trajectory process constraint of the landing are given, where the trajectory process constraint includes the UAV feasibility constraint, the manipulator feasibility constraint, and the task-related joint constraint;

[0010] The constrained state trajectory in the optimization objective function is transformed into a differential flat trajectory without system dynamics constraints. At the same time, the penalty functional method is used to transform the trajectory process constraints into a part of the objective function to be optimized. Then, the trajectory parameterization method is used to transform the continuous differential flat trajectory into a discrete space-time parameter representation, thereby converting the constrained optimization objective function into an unconstrained optimization objective function. The L-BFGS algorithm is used to solve the unconstrained optimization objective function to realize active landing control of UAV based on the assistance of robotic arm.

[0011] Preferably, when the UAV is modeled as a dynamic model, the state of the UAV is modeled as position ,speed and rotation , subscript represents the state variables belonging to the drone, represents a three-dimensional Euclidean space, Represents a three-dimensional spherical space. The position and movement of the drone depend on the acceleration of gravity. and thrust , the angular velocity of the rotational motion in the body coordinate system is As input, the dynamic model of the UAV is expressed as:

[0012] ;

[0013] in, Indicates location The first derivative of Indicates speed The first derivative of Represents the identity matrix No. List, represents the net thrust, Indicates the quality of the drone, Indicates rotation The first derivative of Indicates angular velocity Antisymmetric matrix form of the cross product.

[0014] Preferably, when the dynamic model of the robot arm is modeled, the state of the end effector of the robot arm is modeled as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the posture, including:

[0015] The position of the end effector of the robot arm is expressed as , represents three-dimensional Euclidean space, subscript Represents the state variables belonging to the robot arm. The posture of the end effector of the robot arm uses the azimuth of the spherical coordinate system and polar angle To indicate, and , Representing a two-dimensional spherical space, the dynamic model of the robot arm is expressed as:

[0016] ;

[0017] in, Indicates location The first derivative of represents the speed of the end effector of the robot arm, Indicates polar angle The first derivative of represents the angular velocity of the polar angle, Indicates azimuth The first derivative of Indicates the angular velocity of the azimuth, according to the azimuth and polar angle The z-axis direction component of the end effector of the robot arm can be obtained.

[0018] Preferably, the optimization objective function of the optimal control of the active landing process of the UAV assisted by the manipulator is set to minimize the time functional and energy functional of the state trajectory, and the trajectory end constraint and trajectory process constraint of the landing are given, including:

[0019] The optimization objective function is expressed as:

[0020] ;

[0021] in, represents the energy functional of the state trajectory, represents the time functional, represents the total duration of the trajectory, Indicates the robot-assisted UAV system at time status, It is a robotic arm-assisted drone system that The control input, according to the system dynamics equation, the state of the system includes , the control input of the system includes , The dynamic model of the robot-assisted UAV system that represents the trajectory that needs to be satisfied, represents the trajectory process constraints that need to be satisfied, represents the initial state at time 0, Indicates the end constraint of the trajectory, symbol Indicates less than or equal to.

[0022] Preferably, the trajectory end constraint indicates that the position of the drone needs to coincide with the position of the end effector of the robotic arm, and the z-axis of the drone coincides with the z-axis of the end effector of the robotic arm, which is expressed as:

[0023] ;

[0024] in, Represents the z-axis direction component of the robot end effector at time T, using the azimuth angle of the robot end at time T Calculated, and They represent the position of the drone and the position of the end effector of the robotic arm at time T respectively. express T The rotation of the drone at the moment, Represents the identity matrix No. List.

[0025] Preferably, the feasibility constraints of the drone include:

[0026] First, the control input angular velocity of the drone and thrust It needs to be within a reasonable and feasible range and meet the following requirements:

[0027] ;

[0028] in, Indicates angular velocity The second norm of is the maximum possible angular velocity, and They represent the minimum and maximum feasible pulling forces respectively;

[0029] Secondly, the speed of the drone It should not be too high and should meet the following requirements:

[0030] ;

[0031] in, Indicates speed The second norm of Indicates the maximum feasible speed;

[0032] Finally, the drone must be at a certain height above the ground, satisfying the following:

[0033] ;

[0034] in, Represents the identity matrix No. List, express The transpose of Indicates the minimum allowed altitude.

[0035] Preferably, the feasibility constraints of the robot arm include:

[0036] Based on the dynamic model of the robot arm, the self-collision-free local extension of the robot arm is defined as:

[0037] ;

[0038] in, Indicates the position of the end effector of the robot arm The three-dimensional position component of and Indicates the minimum and maximum values ​​of the radius, and Represents the position component The minimum and maximum values ​​of and Indicates polar angle The minimum and maximum values ​​of and Represents the values ​​of two azimuth angles, Indicates the modification of the variable, which means the difference is modified;

[0039] The control input of the end effector of the robot arm also needs to be within a feasible and reasonable range, satisfying the following:

[0040] ;

[0041] in, represents the maximum feasible speed of the robot end effector, represents the maximum polar angular velocity feasible for the end effector of the robot arm, It represents the maximum angular velocity of the end effector of the robot arm. represents the two-norm of a vector, represents the speed of the end effector of the robot arm, represents the polar angular velocity, represents the azimuth angular velocity.

[0042] Preferably, task-related joint constraints include:

[0043] The bottom surface of the drone is modeled as a three-dimensional space with a radius of The center of the disk is located at the position of the drone. , the normal vector of the disk and the z-axis of the drone The airport of the end effector of the robot arm is modeled as a plane, and the position of the plane is determined by the position of the end effector of the robot arm. Determine the normal vector of the plane and the z-axis of the end effector of the robot arm To prevent the bottom of the drone from colliding with the robotic arm, the following constraints must be met:

[0044] ;

[0045] And shrink constraint 1 to:

[0046] ;

[0047] When the drone successfully satisfies the landing constraint at the end of the trajectory, the above constraint 1 just takes the equal sign;

[0048] During the active landing process of the UAV assisted by the robotic arm, the UAV needs to keep the end of the robotic arm within the camera field of view and estimate the state of the robotic arm through visual relative positioning. The camera field of view is modeled as an open cone in three-dimensional space with a vertex angle of , determined by the camera parameters, the direction is along the drone's axis, the apex of the cone is at the position of the drone , the position of the end effector of the robot arm must be inside the cone, then:

[0049] .

[0050] Preferably, the state trajectory with constraints in the optimization objective function is transformed into a differential flat trajectory without system dynamics constraints, and the simplified optimization objective function is:

[0051] ;

[0052] in, Differentiable flat trajectory represented by discrete space-time parameters The abbreviation of represents the differentially flat trajectory, express The first-order derivative of express The b-order derivative of Represents trajectory process constraints and the end-of-trajectory constraint At this time, differential flat space coordinates are used, represents the initial state of the system after transformation to differentially flat space, T Indicates the total duration of the trajectory;

[0053] The trajectory process is constrained by using the penalty functional method. Transformed into a part of the objective function to be optimized, the optimization objective function is:

[0054] ;

[0055] in, represents an arbitrary penalty function of second order smoothness;

[0056] The continuous differential flat trajectory is then transformed into a discrete spatiotemporal parameter representation using the trajectory parameterization method, including:

[0057] Will Represented as a set of space-time parameters The determined trajectory , while parameterizing the trajectory, let the trajectory satisfy the constraints , and by adding the augmentation parameter Let the trajectory satisfy the constraints , then the unconstrained optimization objective function is expressed as:

[0058] ;

[0059] in, Energy functional representing the parameterized state trajectory;

[0060] The L-BFGS algorithm is used to solve the unconstrained optimization objective function and obtain the parameters to be optimized. ,h, according to these parameters ,h determines the landing trajectory.

[0061] Preferably, an arbitrary penalty function of second order smoothness ,express

[0062] ;

[0063] in, Represents the smoothing parameter, set to a constant less than 1.

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

[0065] Compared with the passive landing method, the method of the present invention has stronger anti-disturbance ability and higher landing success rate due to the active cooperation of the robotic arm to assist the UAV. Compared with the existing active landing vertical take-off and landing method, the present invention has a complete robotic arm to assist the UAV's collaborative trajectory planning, which can achieve faster and more energy-saving active landing, and can ensure that both the robotic arm and the UAV meet the task constraints necessary for active landing, thereby improving the landing success rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0067] Figure 1 It is a flow chart of a method for active landing of a UAV based on mechanical arm assistance provided in an embodiment. DETAILED DESCRIPTION

[0068] To make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific implementation methods described herein are only used to explain the present invention and do not limit the scope of protection of the present invention.

[0069] The inventive concept of the present invention is: since the motion characteristics of the robotic arm and the UAV are very different, the motion planning algorithms of the two are usually also different. It is generally difficult to achieve collaborative planning so that the two can work together to complete the task. For this reason, an embodiment of the present invention provides an active landing method for a UAV based on the assistance of a robotic arm, which solves the collaborative planning problem of the two in the active landing scenario to improve the success rate of landing.

[0070] like Figure 1 As shown, an embodiment provides a method for active landing of a UAV based on the assistance of a mechanical arm, comprising the following steps:

[0071] S1, the dynamic models of the UAV and the robotic arm are modeled respectively, where the state of the end effector of the robotic arm is modeled as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the posture.

[0072] In the embodiment, the dynamics model of the power system in the trajectory planning of the UAV is modeled by adopting a general modeling method and using a quad-rotor UAV, specifically: the state of the UAV is modeled as position ,speed and rotation , subscript represents the state variables belonging to the drone, represents a three-dimensional Euclidean space, Represents a three-dimensional spherical space. The position and movement of the drone depend on the acceleration of gravity. and thrust , the angular velocity of the rotational motion in the body coordinate system is As input, the dynamic model of the UAV is expressed as:

[0073] ;

[0074] in, Indicates location The first derivative of Indicates speed The first derivative of Represents the identity matrix No. List, represents the net thrust, Indicates the quality of the drone, Indicates rotation The first derivative of Indicates angular velocity Antisymmetric matrix form of the cross product.

[0075] There are two general modeling methods for modeling the dynamics model of the power system of the robot arm, namely joint space modeling and end-task space modeling. The joint space modeling takes the joint angle of the robot arm as the state, but the forward and inverse kinematics of the robot arm needs to be introduced in the trajectory planning process, which will bring difficulties to the joint trajectory planning of the drone and the robot arm. The end-task space modeling is generally based on the position of the end effector of the robot arm. and posture In this way, the trajectory of the end effector can be directly planned. However, the trajectory of the end effector planned in this way is not necessarily feasible, and may cause problems such as no inverse kinematics solution and self-collision of the robot arm.

[0076] In order to solve the problems existing in the existing end task space modeling, the idea provided by the embodiment of the present invention is to avoid these situations by applying additional end task space constraints. However, in order to make the additional constraints simple and optimizable, a more geometrically meaningful state representation method of the end effector of the manipulator is required. The representation of represents the state variables belonging to the robot arm. For the attitude component, it is noted that during the landing process, only the end effector The axis direction has an impact, and the azimuth of the spherical coordinate system can be used and polar angle to represent the posture of the end effector of the robot arm, and , Representing a two-dimensional spherical space, we can get a state of the end effector of the robotic arm with more geometric meaning, that is, modeling the state of the end effector of the robotic arm as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the attitude. In this way, when considering the active landing mission, the modeling of the robotic arm has a clear geometric meaning and satisfies the property of differential flatness, so as to better adapt to our active landing mission and realize collaborative planning with drones.

[0077] Then the dynamic model of the robot arm can be expressed as:

[0078] ;

[0079] in, Indicates location The first derivative of represents the speed of the end effector of the robot arm, Indicates polar angle The first derivative of represents the angular velocity of the polar angle, Indicates azimuth The first derivative of Indicates the angular velocity of the azimuth, according to the azimuth and polar angle The z-axis direction component of the end effector of the robot arm can be obtained.

[0080] Since the robotic arm and the UAV do not come into contact until the landing is completed, the dynamic model of the entire system is the parallel connection of the dynamic models of the UAV and the robotic arm.

[0081] S2, the optimization objective function of the optimal control of the active landing process of the UAV assisted by the robotic arm is set to minimize the time functional and energy functional of the state trajectory, and the landing trajectory end constraints and trajectory process constraints are given.

[0082] In the embodiment, the framework of the optimal control problem is used to describe the active landing process of the UAV assisted by the robot arm. Specifically, the optimization objective function of the optimal control is set to minimize the time functional and energy functional of the state trajectory, and the landing trajectory end constraints and trajectory process constraints are given to ensure that the active landing can be completed safely.

[0083] Specifically, the optimization objective function is expressed as:

[0084] ;

[0085] in, represents the energy functional of the state trajectory, represents the time functional, Representation function The domain of , the value range is ,arrow Represented by the domain Inferred value range , represents the total duration of the trajectory, Indicates the robot-assisted UAV system at time status, It is a robotic arm-assisted drone system that The control input, according to the system dynamics equation, the state of the system includes , the control input of the system includes , The dynamic model of the robot-assisted UAV system that represents the trajectory that needs to be satisfied, represents the trajectory process constraints that need to be satisfied, represents the initial state at time 0, Indicates the end constraint of the trajectory, symbol Indicates less than or equal to.

[0086] In order to completely model the optimal control problem of active landing of UAV based on robotic arm assistance, it is also necessary to consider the trajectory process constraints and trajectory end constraints that the trajectory must satisfy.

[0087] Among them, the trajectory process constraints It means that when the system completes the landing task at the end of the trajectory, the position of the drone needs to coincide with the position of the end effector of the robotic arm, and the z-axis of the drone coincides with the z-axis of the end effector of the robotic arm, which can be expressed as:

[0088] ;

[0089] in, Represents the z-axis direction component of the robot end effector at time T, using the azimuth angle of the robot end at time T Calculated, and They represent the position of the drone and the position of the end effector of the robotic arm at time T respectively. represents the rotation of the drone at time T, Represents the identity matrix No. List.

[0090] In the embodiment, the trajectory process constraints It is very complex, including UAV feasibility constraints, robotic arm feasibility constraints, and task-related joint constraints.

[0091] Specifically, the feasibility constraints of the UAV include: first, the control input angular velocity of the UAV and thrust It needs to be within a reasonable and feasible range and meet the following requirements:

[0092] ;

[0093] in, Indicates angular velocity The second norm of is the maximum possible angular velocity, and They represent the minimum and maximum feasible pulling forces respectively;

[0094] Secondly, in order to ensure the accuracy of control, the speed of the drone It should not be too high and should meet the following requirements:

[0095] ;

[0096] in, Indicates speed The second norm of Indicates the maximum feasible speed;

[0097] Finally, the drone must be at a certain height above the ground to ensure safety and avoid the proximity effect, as follows:

[0098] ;

[0099] in, Represents the identity matrix No. List, express The transpose of Indicates the minimum allowed altitude.

[0100] Specifically, for the feasibility constraints of the robot, the end-task space modeling can directly plan the trajectory of the end effector, but the end-effector trajectory planned in this way is not necessarily feasible, and may cause problems such as no inverse kinematics solution and self-collision of the robot. This solution first transforms the model of the end-task space to make it have good geometric meaning, and then uses the transformed model to define the self-collision-free local extension of the robot. Within the local extension, the end trajectory of the robot can always be mapped to a feasible and continuous robot joint angle trajectory without self-collision problems. Specifically, the local extension of the robot is defined as follows:

[0101] ;

[0102] in, Indicates the position of the end effector of the robot arm The three-dimensional position component of and Indicates the minimum and maximum values ​​of the radius, and Represents the position component The minimum and maximum values ​​of and Indicates polar angle The minimum and maximum values ​​of and Represents the values ​​of two azimuth angles, Indicates the modification of the variable, which means the difference is modified;

[0103] The control input of the end effector of the robot arm also needs to be within a feasible and reasonable range, satisfying the following:

[0104] ;

[0105] in, represents the maximum feasible speed of the robot end effector, represents the maximum polar angular velocity feasible for the end effector of the robot arm, It represents the maximum angular velocity of the end effector of the robot arm. represents the two-norm of a vector, represents the speed of the end effector of the robot arm, represents the polar angular velocity, represents the azimuth angular velocity.

[0106] Specifically, for task-related joint constraints, the drone and the robotic arm must not only have their own feasible trajectories, but also meet task-related joint constraints to ensure that they cooperate with each other without conflict. First, except for the final contact moment of landing, the drone and the robotic arm must avoid collision. Here, the bottom surface of the drone is modeled as a radius of 3D space. The center of the disk is located at the position of the drone. , the normal vector of the disk and the z-axis of the drone The airport of the end effector of the robot arm is modeled as a plane, and the position of the plane is determined by the position of the end effector of the robot arm. Determine the normal vector of the plane and the z-axis of the end effector of the robot arm To prevent the bottom of the drone from colliding with the robotic arm, the following constraints must be met:

[0107] ;

[0108] To reduce the nonlinearity of the constraints, constraint 1 is shrunk to:

[0109] ;

[0110] When the drone successfully satisfies the landing constraint at the end of the trajectory, the above constraint 1 just takes the equal sign;

[0111] During the active landing process of the UAV assisted by the robotic arm, the UAV needs to keep the end of the robotic arm within the camera field of view (FOV) and estimate the state of the robotic arm through visual relative positioning. The camera field of view is modeled as an open cone in three-dimensional space with a vertex angle of , determined by the camera parameters, the direction is along the drone's axis, the apex of the cone is at the position of the drone , the position of the end effector of the robot arm must be inside the cone, then:

[0112] .

[0113] S3, transforms the constrained state trajectory in the optimization objective function into a differential flat trajectory without system dynamics constraints, and uses the penalty functional method to transform the trajectory process constraints into a part of the objective function to be optimized. Then, the trajectory parameterization method is used to transform the continuous differential flat trajectory into a discrete space-time parameter representation, thereby transforming the constrained optimization objective function into an unconstrained optimization objective function. The L-BFGS algorithm is used to solve the unconstrained optimization objective function to realize active landing control of UAV based on robotic arm assistance.

[0114] In the embodiment, the above general form of the optimization objective function with constraints is often difficult to solve, so it is necessary to simplify its form first and finally use a suitable numerical algorithm to solve it. First, the joint state trajectory of the robot-assisted UAV system is transformed into Transformed into a trajectory in flat output space To briefly introduce differential flat transformations, consider the system dynamics of the following form:

[0115] ;

[0116] in , which represents the function The domain of the definition is n-dimensional Euclidean space , the range inferred based on the domain is n-dimensional Euclidean space , , which represents the mapping function The domain of the definition is n-dimensional Euclidean space , the range inferred based on the domain is the real number domain ,state , control input . Mapping function The rank of This system is called a differentially flat system if it satisfies the following conditions: The system has a differentially flat output Can be and is determined by the finite-order derivatives of and Can be is parameterized by the finite-order derivatives of , namely:

[0117] ;

[0118] in , which means transformation The domain of the definition is m(b-1)-dimensional Euclidean space , the inferred range is n-dimensional Euclidean space , , which means transformation The domain of is mb-dimensional Euclidean space, and the inferred range is m-dimensional Euclidean space ,Depend on and Induced by. Applying the properties of differential flatness, the trajectory can be directly output from the differential flatness And its finite-order derivatives are transformed algebraically to obtain the state trajectory , while in the differentially flat output space, The components of are independent, and there is no need to consider the differential constraints of the system dynamics model. It has been widely studied, and the dynamic model of the robot arm proposed in step S1 is already in the differentially flat space and does not require transformation.

[0119] The simplified optimization objective function is:

[0120] ;

[0121] in, Differentiable flat trajectory represented by discrete space-time parameters The abbreviation of represents the differentially flat trajectory, express The first-order derivative of express The b-order derivative of Represents trajectory process constraints and the end-of-trajectory constraint At this time, differential flat space coordinates are used, represents the initial state of the system after transformation to differentially flat space, T Indicates the total duration of the trajectory;

[0122] The trajectory process is constrained by using the penalty functional method. Transformed into a part of the objective function to be optimized, the optimization objective function is:

[0123] ;

[0124] in, An arbitrary penalty function representing second-order smoothness can be:

[0125] ;

[0126] in, Represents the smoothing parameter, set to a constant less than 1.

[0127] The continuous differential flat trajectory is then transformed into a discrete spatiotemporal parameter representation using the trajectory parameterization method, including:

[0128] You can select common trajectory types such as polynomial trajectory, B-spline trajectory, etc. Represented as a set of space-time parameters The determined trajectory , while parameterizing the trajectory, let the trajectory satisfy the constraints , and by adding the augmentation parameter Let the trajectory satisfy the constraints , then the unconstrained optimization objective function is expressed as:

[0129] ;

[0130] in, Energy functional representing the parameterized state trajectory.

[0131] Such finite-dimensional unconstrained optimization problems can be solved using any unconstrained gradient optimization numerical algorithm such as the L-BFGS algorithm to obtain the parameters to be optimized. ,h, according to these parameters ,h can determine the landing trajectory.

[0132] Compared with the passive landing method, the active landing method of the UAV assisted by the robotic arm provided in the embodiment of the present invention has stronger anti-disturbance capability and higher success rate.

[0133] Compared with the trajectory planned by the passive landing method, the trajectory planned by the active landing method of the UAV assisted by the robot arm provided in the embodiment of the present invention can better meet the task constraints required for landing, as shown in Table 1.

[0134] Compared with the active landing of the vertical take-off and landing (VTOL) mode without a planning module, the active landing method of the UAV assisted by the robotic arm provided in the embodiment of the present invention can complete the landing task more quickly and more energy-efficiently, as shown in Table 2.

[0135]

[0136] Compared with the active landing of the vertical take-off and landing (VTOL) mode without a planning module, the active landing method of the UAV assisted by the robotic arm provided in the embodiment of the present invention can theoretically consider some complex task constraints, such as the robotic arm's no self-collision constraint, relative positioning constraint, task safety constraint, etc., and the planned trajectory does meet these constraints. The active landing of the vertical take-off and landing (VTOL) mode without a planning module cannot consider these constraints at all.

[0137] The specific implementation methods described above provide a detailed description of the technical solutions and beneficial effects of the present invention. It should be understood that the above is only the most preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, supplements and equivalent substitutions made within the scope of the principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for active landing of a UAV based on mechanical arm assistance, characterized in that: The following steps are involved: The UAV and the robotic arm are modeled with dynamic models respectively, where the state of the robotic arm end effector is modeled as a three-dimensional Euclidean space representing the position and a two-dimensional spherical space representing the posture, including: Model the state of the drone as a position ,speed and rotation , subscript represents the state variables belonging to the drone, represents a three-dimensional Euclidean space, Represents a three-dimensional spherical space. The position and movement of the drone depend on the acceleration of gravity. and thrust , the angular velocity of the rotational motion in the body coordinate system is As input, the dynamic model of the UAV is expressed as: ； in, Indicates location The first derivative of Indicates speed The first derivative of Represents the identity matrix No. List, represents the net thrust, Indicates the quality of the drone, Indicates rotation The first derivative of Indicates angular velocity Antisymmetric matrix form of the cross product; The position of the end effector of the robot arm is expressed as , represents three-dimensional Euclidean space, subscript Represents the state variables belonging to the robot arm. The posture of the end effector of the robot arm uses the azimuth of the spherical coordinate system and polar angle To indicate, and , Representing a two-dimensional spherical space, the dynamic model of the robot arm is expressed as: ； in, Indicates location The first derivative of represents the speed of the end effector of the robot arm, Indicates polar angle The first derivative of represents the angular velocity of the polar angle, Indicates azimuth The first derivative of Indicates the angular velocity of the azimuth, according to the azimuth and polar angle The z-axis direction component of the end effector of the robot arm can be obtained; The optimization objective function of the optimal control of the active landing process of the UAV assisted by the manipulator is set to minimize the time functional and energy functional of the state trajectory, and the landing trajectory end constraints and trajectory process constraints are given, including: The optimization objective function is expressed as: ； in, represents the energy functional of the state trajectory, represents the time functional, represents the total duration of the trajectory, Indicates the robot-assisted UAV system at time status, It is a robotic arm-assisted drone system that The control input, according to the system dynamics equation, the state of the system includes The control input of the system includes , The dynamic model of the robot-assisted UAV system that represents the trajectory that needs to be satisfied, represents the trajectory process constraints that need to be satisfied, represents the initial state at time 0, Indicates the end constraint of the trajectory, symbol Indicates less than or equal to; The trajectory process constraints include UAV feasibility constraints, robotic arm feasibility constraints, and task-related joint constraints; The constrained state trajectory in the optimization objective function is transformed into a differential flat trajectory without system dynamics constraints. At the same time, the penalty functional method is used to transform the trajectory process constraints into a part of the objective function to be optimized. Then, the trajectory parameterization method is used to transform the continuous differential flat trajectory into a discrete space-time parameter representation, thereby converting the constrained optimization objective function into an unconstrained optimization objective function. The L-BFGS algorithm is used to solve the unconstrained optimization objective function to realize active landing control of UAV based on the assistance of robotic arm.

2. The method for active landing of a UAV based on mechanical arm assistance according to claim 1, characterized in that: The trajectory end constraint means that the position of the drone needs to coincide with the position of the end effector of the robotic arm, and the z-axis of the drone coincides with the z-axis of the end effector of the robotic arm, which is expressed as: ； in, Represents the z-axis direction component of the robot end effector at time T, using the azimuth angle of the robot end at time T Calculated, and They represent the position of the drone and the position of the end effector of the robotic arm at time T respectively. express T The rotation of the drone at the moment, Represents the identity matrix No. List.

3. The method for active landing of a UAV based on mechanical arm assistance according to claim 1, characterized in that: UAV feasibility constraints, including: First, the control input angular velocity of the drone and thrust It needs to be within a reasonable and feasible range and meet the following requirements: ； in, Indicates angular velocity The second norm of is the maximum possible angular velocity, and They represent the minimum and maximum feasible pulling forces respectively; Secondly, the speed of the drone It should not be too high and should meet the following requirements: ； in, Indicates speed The second norm of Indicates the maximum feasible speed; Finally, the drone must be at a certain height above the ground, satisfying the following: ； in, Represents the identity matrix No. List, express The transpose of Indicates the minimum allowed altitude.

4. The method for active landing of a UAV based on mechanical arm assistance according to claim 1, characterized in that: Robot feasibility constraints, including: Based on the dynamic model of the robot arm, the self-collision-free local extension of the robot arm is defined as: ； in, Indicates the position of the end effector of the robot arm The three-dimensional position component of and Indicates the minimum and maximum values ​​of the radius, and Represents the position component The minimum and maximum values ​​of and Indicates polar angle The minimum and maximum values ​​of and Represents the values ​​of two azimuth angles, Indicates the modification of the variable, which means the difference is modified; The control input of the end effector of the robot arm also needs to be within a feasible and reasonable range, satisfying the following: ； in, represents the maximum feasible speed of the robot end effector, represents the maximum polar angular velocity feasible for the end effector of the robot arm, It represents the maximum angular velocity of the end effector of the robot arm. represents the two-norm of a vector, represents the speed of the end effector of the robot arm, represents the polar angular velocity, represents the azimuth angular velocity.

5. The method for active landing of a UAV based on mechanical arm assistance according to claim 1, characterized in that: Task-related joint constraints, including: The bottom surface of the drone is modeled as a three-dimensional space with a radius of The center of the disk is located at the position of the drone. , the normal vector of the disk and the z-axis of the drone The airport of the end effector of the robot arm is modeled as a plane, and the position of the plane is determined by the position of the end effector of the robot arm. Determine the normal vector of the plane and the z-axis of the end effector of the robot arm To prevent the bottom of the drone from colliding with the robotic arm, the following constraints must be met: ； And shrink constraint 1 to: ； When the drone successfully satisfies the landing constraint at the end of the trajectory, the above constraint 1 just takes the equal sign; During the active landing of the UAV based on the assistance of the robotic arm, the UAV needs to keep the end of the robotic arm within the camera field of view and estimate the state of the robotic arm through visual relative positioning. The camera field of view is modeled as an open cone in three-dimensional space with a vertex angle of , determined by the camera parameters, the direction is along the drone's axis, the apex of the cone is at the position of the drone , the position of the end effector of the robot arm must be inside the cone, then: 。 6. The method for active landing of a UAV based on mechanical arm assistance according to claim 1, characterized in that: The state trajectory with constraints in the optimization objective function is transformed into a differential flat trajectory without system dynamics constraints, and the simplified optimization objective function is: ； in, Differentiable flat trajectory represented by discrete space-time parameters The abbreviation of represents the differentially flat trajectory, express The first-order derivative of express The b-order derivative of Represents trajectory process constraints and trajectory end constraints At this time, differential flat space coordinates are used, represents the initial state of the system after transformation to differentially flat space, T Indicates the total duration of the trajectory; The trajectory process is constrained by using the penalty functional method. Transformed into a part of the objective function to be optimized, the optimization objective function is: ； in, represents an arbitrary penalty function of second order smoothness; The continuous differential flat trajectory is then transformed into a discrete spatiotemporal parameter representation using the trajectory parameterization method, including: Will Represented as a set of space-time parameters The determined trajectory , while parameterizing the trajectory, let the trajectory satisfy the constraints , and by adding the augmentation parameter Let the trajectory satisfy the constraints , then the unconstrained optimization objective function is expressed as: ； in, Energy functional representing the parameterized state trajectory; The L-BFGS algorithm is used to solve the unconstrained optimization objective function and obtain the parameters to be optimized. ,h, according to these parameters ,h determines the landing trajectory.

7. The method for active landing of a UAV based on mechanical arm assistance according to claim 6, characterized in that: Arbitrary penalty function of second order smoothness ,express ； in, Represents the smoothing parameter, set to a constant less than 1.

Images