Shape adaptive planning and control method for deformable unmanned aerial vehicle

Abstract

The invention discloses a shape adaptive planning and control method for a deformable unmanned aerial vehicle, and the method comprises the steps: carrying out the searching based on an occupied grid map of a scene through employing a variable-size kinematics A * path planning algorithm, and obtaining an initial path; by taking the initial path as an initial condition, shape-adaptive trajectory optimization in a continuous space-time space is carried out to obtain an optimized trajectory, and in the trajectory optimization process, the mass center position and deformation parameters of the unmanned aerial vehicle are optimized at the same time; and based on the optimized trajectory, on the basis of a nonlinear model predictive controller and by combining an incremental nonlinear dynamic inverse algorithm, calculating and compensating external force disturbance and external torque disturbance caused by deformation of the unmanned aerial vehicle and a load, so as to realize shape adaptive control of the deformable unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) navigation, and in particular relates to a shape adaptive planning and control method for a deformable UAV. Background Art

[0002] Drones are playing an increasingly important role in a wide range of applications, including dynamic cave exploration, aerial photography, and complex tunnel inspection. However, traditional quadrotors, constrained by their fixed physical size, have inherent limitations. Large drones face significant challenges in maneuvering within confined spaces, making it difficult to navigate narrow passages. Smaller drones, while offering improved reach, often sacrifice flight endurance and payload capacity, and their resilience to external disturbances such as wind is reduced due to propeller overlap and reduced torque control. To overcome these limitations, the concept of deformable drones has emerged as a promising paradigm shift. These unique platforms possess active shapeshifting capabilities, enabling real-time adjustments to their physical dimensions. This dynamic adaptability not only significantly enhances navigation capabilities in complex and spatially confined environments but also unlocks unprecedented operational potential beyond traditional drones, including object grasping and transport, thereby expanding their applications in various human-robot interaction areas, such as indoor logistics.

[0003] Deformable drones offer a more natural and efficient method for navigating confined spaces by dynamically reducing their size. However, during deformation, deformable drones face control challenges such as deformation interference, model mismatch, and external disturbances, which can degrade trajectory tracking performance. This issue is particularly critical in high-precision tracking scenarios, such as gap drilling, where precise control is crucial. In existing studies, “Falanga D, Kleber K, Mintchev S, et al. The foldable drone: A morphing quadrotor that can squeeze and fly [J]. IEEE Robotics and Automation Letters, 2018, 4 (2): 209-216.” uses a linear quadratic regulator (LQR) controller for hovering and grasping tasks, while “Derrouaoui SH, Bouzid Y, Guiatni M. PSO based optimal gain scheduling backstepping flight controller design for a transformable quadrotor [J]. Journal of Intelligent & Robotic Systems, 2021, 102 (3): 67.” and “Kim C, Lee H, Jeong M, et al. A morphing quadrotor that can optimize morphology for transportation [C] / / 2021 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2021: 9683-9689. A linear PID (proportion integration differentiation) controller was used for flight or grasping. Hu D, Pei Z, Shi J, et al. Design, modeling and control of a novel morphing quadrotor [J]. IEEE Robotics and Automation Letters, 2021, 6(4): 8013-8020. Reinforcement learning was used to achieve stable hovering during the deformation process. However, these studies did not consider aerodynamic disturbances.To address this issue, a nonlinear attitude controller with aerodynamic drag compensation was proposed in Cui G, Xia R, Jin X, et al. Motion planning and control of a morphing quadrotor in restricted scenarios [J]. IEEE Robotics and Automation Letters, 2024., and a nonlinear multi-processor (NMPC) algorithm was developed in Wu Y, Yang F, Wang Z, et al. Ring-rotor: A novel retractable ring-shaped quadrotor with aerial grasping and transportation capability [J]. IEEE Robotics and Automation Letters, 2023, 8(4): 2126-2133. to improve control performance. However, these methods do not effectively address the problems of model mismatch and additional force and torque disturbances. Summary of the Invention

[0004] In response to the problems existing in the prior art, the purpose of the embodiments of the present application is to provide a shape adaptive planning and control method for a deformable UAV.

[0005] According to a first aspect of an embodiment of the present application, a shape adaptive planning and control method for a deformable UAV is provided, comprising:

[0006] Based on the scene's occupancy grid map, a resizable kinematic A* path planning algorithm is used to search and obtain the initial path.

[0007] Taking the initial path as an initial condition, performing shape-adaptive trajectory optimization in a continuous spatiotemporal space to obtain an optimized trajectory, wherein during the trajectory optimization process, the center of mass position and deformation parameters of the UAV are simultaneously optimized;

[0008] Based on the optimized trajectory and taking the nonlinear model predictive controller as the basis, combined with the incremental nonlinear dynamic inverse algorithm, the external force disturbance and external torque disturbance caused by the deformation and load of the UAV are calculated and compensated, thereby realizing the shape adaptive control of the deformable UAV.

[0009] Furthermore, in the resizable kinematic A* path search, the comprehensive cost metric of each motion primitive generated during node expansion is used as a heuristic function, and each node in the initial path satisfies safety constraints based on the geometry of the drone.

[0010] Furthermore, in the resizable kinematic A* path planning algorithm, the cost metric of each motion primitive generated during node expansion is used as a heuristic function, and the cost metric g c is a weighted combination of flight energy consumption, path smoothness, and first-order temporal regularization:

[0011]

[0012] Among them, r max is the maximum radius of the drone, a and w T is a user-defined weight, r represents the radius of the deformable drone, u x,y,z represents the control input, u r is the path smoothness cost, and ΔT represents the time interval allocated for each node expansion during the search process.

[0013] Furthermore, the resizable kinematic A* path planning algorithm adheres to a safety constraint during the path search process. The safety constraint is to discretize the complete geometry of the UAV into sampling points, and require that the obstacle clearance metric of all sampling points be above a predetermined distance threshold:

[0014]

[0015] in, Indicates that the complete geometric shape of the drone is considered The obstacle clearance metric, p W represents the center of mass of the UAV, r represents the radius of the deformable UAV, D margin is the predetermined distance threshold.

[0016] Furthermore, trajectory optimization is achieved by minimizing the position control torque, radius control torque, second-order radius regularization term, flight time consumption and complying with obstacle avoidance constraints and dynamic feasibility requirements.

[0017] The trajectory optimization problem is modeled as:

[0018]

[0019] Among them, the trajectory uses The segmented polynomial function is expressed as follows: c and T are the polynomial coefficients and time intervals of the trajectory, s is the order of the control torque, and v max 、a max 、ω max , α max are the maximum speed, maximum acceleration, maximum deformation speed and maximum deformation acceleration of the UAV, r (1) (t) and r (2) (t) are the first and second derivatives of the radius r(t), is the i-th trajectory σ i of Derivative, σ [s-1] (0) is the value of the first-order derivative of the trajectory at t = 0, σ f is the value of the first-order derivative of the trajectory at t = T, where T is the total time.

[0020] Furthermore, the obstacle avoidance constraint requires that all surface points on the drone body are guaranteed to be collision-free. B To approximate enforcement, that is:

[0021]

[0022] Among them, R represents the direction of the drone, N θ and N l They represent the user-specified angular discretization resolution and the axial discretization resolution along the Z axis, respectively.

[0023] Furthermore, the control input of the nonlinear model predictive controller is

[0024]

[0025] Among them, i represents the current time step; x i,r and x N,r are the reference state vectors of the current time step and the Nth time step respectively; u i,r is the reference input vector; Q = diag(Q p ,Q v ,Q q ,Q ω ) represents the diagonal matrix composed of weights corresponding to position, velocity, attitude and angular velocity, Q N and R is a positive definite weight matrix; u min and u max Indicates the minimum and maximum thrust values ​​provided by the motor.

[0026] Furthermore, based on the nonlinear model predictive controller and combined with the incremental nonlinear dynamic inverse algorithm, the external force disturbance and external torque disturbance caused by the deformation and load of the UAV are calculated and compensated. Specifically:

[0027] Based on the translational dynamics of the system, the external force disturbance caused by the deformation and load of the drone is calculated as:

[0028]

[0029] By compensating for external disturbances in thrust control, the required thrust is:

[0030] F des =||Fz B -F ext||

[0031] Based on the rotational dynamics equation of the deformable UAV, the external torque disturbance caused by the deformation and load of the UAV is calculated:

[0032]

[0033] Among them, τ f is the control torque in the body coordinate system, ω B,f and denote the measured body angular velocity and angular acceleration, respectively;

[0034] Substituting the above external torque disturbance calculation formula into the UAV rotation dynamics equation, we get:

[0035]

[0036] Among them, ω B is the angular velocity of the UAV;

[0037] By deriving the desired total thrust and angular acceleration from the speed commands of the four brushless motors and reversing the above equation, the desired control torque command τ is obtained. des The increment expression is as follows:

[0038]

[0039] The total thrust and torque input of the UAV is obtained as follows:

[0040]

[0041] Among them, H k Assign a matrix to the control of a quadrotor drone.

[0042] According to a second aspect of an embodiment of the present application, there is provided an electronic device, including:

[0043] one or more processors;

[0044] a memory for storing one or more programs;

[0045] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in the first aspect.

[0046] According to a third aspect of an embodiment of the present application, a computer-readable storage medium is provided, on which computer instructions are stored. When the instructions are executed by a processor, the steps of the method described in the first aspect are implemented.

[0047] The technical solutions provided by the embodiments of the present application may have the following beneficial effects:

[0048] As can be seen from the above embodiments, this application addresses the problem that traditional motion planning algorithms cannot be effectively applied to deformable multimodal drones, and proposes a variable-size kinematic A* path planning algorithm that explicitly incorporates radius deformation into the state space representation, thereby enhancing the flexibility of obstacle avoidance during motion; proposes a trajectory optimization framework that operates in a continuous spatiotemporal space, while optimizing the drone's center of mass motion and deformation parameters, making full use of morphological adaptability to enhance environmental adaptability; in response to the challenges of aerodynamic forces, thrust coefficient changes, and model mismatches encountered during deformation and grasping transportation, an enhanced control method based on force and torque compensation is proposed, which improves control accuracy and robustness. In summary, this application proposes a shape adaptive planning and control method for deformable drones, giving deformable drones the ability to autonomously deform through narrow gaps and autonomously carry objects on their entire fuselage for transportation.

[0049] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0051] Figure 1 Schematic diagram of the structure of a deformable UAV according to an exemplary embodiment, wherein (a) is a conventional (maximum) size three-dimensional mechanical structure; (b) is a conventional (maximum) size two-dimensional mechanical schematic; (c) is a compact (minimum) size three-dimensional mechanical structure after deformation; and (d) is a compact (minimum) size two-dimensional mechanical schematic after deformation.

[0052] Figure 2 The present invention is a flowchart of a shape adaptive planning and control method for a deformable UAV according to an exemplary embodiment.

[0053] Figure 3 FIG. 4 is a schematic diagram of a variable-size kinematic A* path search according to an exemplary embodiment.

[0054] Figure 4 It is a schematic diagram showing a comparison of trajectories of the present method, the maximum size planning method, and the minimum size planning method according to an exemplary embodiment, wherein (a) is a schematic diagram of the trajectories planned by the three methods, and (b) is a partial enlarged view of (a).

[0055] Figure 5is a schematic diagram of the method for navigating through a narrow and complex space by autonomously changing the shape according to an exemplary embodiment, wherein (a) is the planned trajectory, (b) is the deformation state of the drone during flight, and (c) is a schematic diagram of the drone control error.

[0056] Figure 6 is a schematic diagram of a grabbing-transporting trajectory generated by the method according to an exemplary embodiment.

[0057] Figure 7 2 is a schematic diagram illustrating enhanced control performance for tracking a figure-8 trajectory under morphological changes according to an exemplary embodiment.

[0058] Figure 8 The present invention is a block diagram of a shape adaptive planning and control device for a deformable UAV according to an exemplary embodiment.

[0059] Figure 9 The figure is a schematic diagram of an electronic device according to an exemplary embodiment.

[0060] Figure numerals: 1. first imitation finger module; 2. second imitation finger module; 3. first imitation palm module; 4. second imitation palm module; 5. servo motor; 6. fixed pulley; 7. compression spring; 8. thin rope; 9. slide rail; 10. slider; 11. propeller; 12. brushless motor; 13. circumferential bearing; 14. torsion spring. DETAILED DESCRIPTION

[0061] Exemplary embodiments are described in detail herein, with examples illustrated in the accompanying drawings. When the following description refers to the drawings, identical numerals in different drawings represent identical or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all embodiments consistent with this application.

[0062] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. As used in this application and the appended claims, the singular forms "a," "an," "the," and "the" are intended to include the plural forms, unless the context clearly indicates otherwise. It should also be understood that the term "and / or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0063] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0064] The present invention belongs to the technical field of autonomous navigation of unmanned aerial vehicles (UAVs), and specifically relates to a multimodal UAV kinematic path planning method integrating dynamic deformation constraints, which is suitable for generating three-dimensional obstacle avoidance paths for deformable UAVs such as ring rotors.

[0065] This application is based on Figure 1 A bionic grasping and flight-integrated deformable drone is shown. The deformable drone includes a first finger-like module 1, a first palm-like module 3, a second finger-like module 2, and a second palm-like module 4. Each module is equipped with a propeller 11 and a corresponding driver. A brushless motor 12 can be used as the driver for each propeller 11. The first finger-like module 1 and the first palm-like module 3, the first palm-like module 3 and the second palm-like module 4, and the second palm-like module 4 and the second finger-like module 2 are all rotationally connected via a composite underactuated deformable structure. The composite underactuated deformable structure is linearly actuated and includes a passive telescopic mechanism and a passive rotation mechanism. The passive telescopic mechanism is used to adjust the distance between the modules, and the passive rotation mechanism is used to rotate the finger-like module relative to the palm-like module to achieve bionic flexion, extension, and grasping functions. Adjacent modules are connected by a passive telescopic mechanism, which includes a slider 10-slide rail 9 assembly and a compression spring 7. Preferably, two sets of sliders 10-slide rail 9 assemblies can be provided. Compared with a single set of sliders 10-slide rail 9 assemblies, this dual-structure design can improve the rigidity of the connection. The compression spring 7 is mounted on one of the slide rails 9 to achieve recovery after expansion and contraction between modules. A passive rotation mechanism connection structure is adopted, which includes a torsion spring 14 and a circumferential bearing 13. The circumferential bearing 13 is used to reduce friction during rotation, and the torsion spring 14 is used to provide a driving force for recovery after rotation, ensuring that the finger-like module can rotate like a finger joint. The installation position of the passive rotation mechanism is as follows: Figure 2 As shown, the circumferential bearing 13 reduces friction during the rotation of the simulated fingertip module, and the torsion spring 14 performs deformation and shape recovery functions similar to the compression spring 7 described above. The telescopic and rotational mechanisms together constitute the drone's composite biomimetic deformation structure, which has five degrees of freedom. Driven by a single servo motor 5, this deformation structure exhibits underactuated characteristics.

[0066] In one embodiment, each finger-like module is provided with a double-layer structure, which is rotatably connected to the finger-like module via a bolt. The sliders 10 of the two slider 10-rail 9 assemblies are respectively disposed on the upper and lower plates of the double-layer structure. One end of the rail 9 is correspondingly disposed on the top and bottom plates of the palm-like module. A circumferential bearing 13 and a torsion spring 14 are mounted on the bolts and separated by the upper plate of the double-layer structure. In a specific implementation, to prevent sliding from poking components within the finger-like module (such as the servo motor 5), the upper slider 10-rail 9 assembly disposed between the finger-like module and the palm-like module can be configured such that one end of the rail 9 is fixed to the upper plate of the double-layer structure, and the slider 10 is disposed at the other end of the rail 9 and fixedly connected to the top plate of the palm-like module. To achieve greater flexibility, the remaining slider 10-rail 9 assembly can be configured with two sliders 10, with a compression spring 7 disposed between the two sliders 10, and the two sliders 10 are respectively fixed to the module to be connected.

[0067] In the specific implementation, a single motor can be used to realize linear drive deformation, specifically: a number of light fixed pulleys 6 are installed on the inner side of each part of the fuselage, such as Figure 2 As shown, each palm-imitation module is provided with at least one fixed pulley 6, and the bottom plate and double-layer structure part of each finger-imitation module each include a fixed pulley 6. Then, one end of a nylon string 8 is fixed to the finger-imitation module, and the other end of the string 8 is wound in sequence along the groove direction of each fixed pulley 6, and finally connected to the turntable of the servo motor 5 of another finger-imitation module, forming the basic structure of the line drive. When the servo motor 5 of the drone starts to rotate, the string 8 is pulled to slide, causing the pulley to produce a compression movement. At this time, the spring will be compressed, and the body will shrink to reduce its own size. When there is no grabbing load, since the tension on the string 8 is equal everywhere, the deformation of the three compression springs 7 is the same, so the displacement of the telescopic mechanism between the modules is consistent with each other; similarly, the rotational deformation of the two compression springs 7 is the same, and the rotation angles of the two finger-imitation modules are also the same. In this way, the body can achieve centrally symmetrical deformation under the drive of the servo motor 5, as shown Figure 2As shown. Furthermore, addressing the need for multiple power sources for multi-degree-of-freedom mechanisms, this drone utilizes a drive method that maximizes utilization within an integrated configuration. It utilizes only a single servo motor 5 to drive the string 8 within the deforming mechanism, pulling the modules together to generate deformation motion, driving the body to undergo underactuated deformation. This achieves both the diverse grasping capabilities and highly flexible deformation of the bionic hand while significantly reducing the number of actuators and energy consumption. It should be noted that the spring constant of the compression spring 7 can be set to be smaller than that of the torsion spring 14. Under the same driving force from the servo motor 5, the deformation of the rotating mechanism is significantly smaller than that of the retracting mechanism. Therefore, when the servo motor 5 pulls the string 8 to gradually deform the body, the palm-like module does not significantly rotate and deform in the initial stages. This maintains a large gripping opening between the two finger-like modules, thus meeting the need to grasp a wider range of large objects. This deformable drone possesses full-body aerial grasping and transport capabilities, adapting to objects of various shapes and having broad applications in post-disaster rescue, package delivery, and other fields. In one embodiment, the maximum size of the drone is 42.2×42.2 cm, which can be reduced by a maximum of 37.8% to a minimum size of 26.2×26.2 cm.

[0068] Based on the above-mentioned deformable UAV, this application proposes a shape adaptive planning and control method for deformable UAV, such as Figure 2 As shown in the figure, this method collaboratively plans the flight trajectory and deformation state, allowing the UAV to deform, traverse, and grasp and transport in complex and narrow environments. It can include:

[0069] Step S1: Based on the scene's occupancy grid map, a resizable kinematic A* path planning algorithm is used to search and obtain an initial path.

[0070] Traditional kinematic A* algorithms usually simplify drones into point masses or fixed-size entities when performing path search, which is not suitable for multimodal deformable drones. Deformable drones have different drone shape models, so a path search front-end that can adapt to these different shapes and ensure accessibility in various modes is required. This application proposes a variable-size kinematic A* path planning algorithm based on a grid map of the scene and pose information such as the starting point, intermediate point (where the object is located), and end point. The algorithm explicitly incorporates radius deformation into the state space representation, thereby enhancing the flexibility of obstacle avoidance during motion, such as Figure 3 shown.

[0071] The state space X of the drone is formally defined as a composite vector:

[0072]

[0073] Among them, p Wrepresents the center of mass of the drone, r represents the radius of the deformable drone, which is a variable parameter, and v W represents the velocity of the center of mass, v r represents the radius deformation rate. The control input u is specified as the acceleration that controls the system dynamics. The state transition mechanism is mathematically modeled by the following discrete time propagation equation:

[0074] X k+1 =AX k +Bu,

[0075]

[0076] Among them, X k represents the kth state, and ΔT represents the time interval allocated for the expansion of each node during the search process.

[0077] In the path search process, the control input u∈[-u ax ,u max ] and time interval ΔT∈[ΔT min ,ΔT max ], systematically generate the successor state X k+1 , where -u max ,u max are the predetermined minimum control input and the predetermined maximum control input, ΔT min ,ΔT max The predetermined minimum time interval and the predetermined maximum time interval are respectively.

[0078] In order to strictly enforce safety constraints in the path exploration process, the Euclidean Signed Distance Field (ESDF) is introduced:

[0079]

[0080] in, Indicates that the complete geometric shape of the drone is considered The obstacle clearance metric is . Before grasping, the drone is modeled as a cylinder with radius r and fixed height h; after grasping, the geometry of the grasped object is also incorporated into the collision model, that is, the system is modeled as a cylinder (drone) + grasped object. In practice, safety constraints are enforced by discretizing the complete geometry of the drone into sampling points, requiring that all sampling points remain above a specified threshold D margin In addition to obstacle avoidance, nodes exceeding predefined velocity or deformation rate bounds are systematically constrained to ensure the physical feasibility of the generated path.

[0081] The energy consumption of airborne flight is inversely proportional to the square of the deformation radius r. Therefore, the comprehensive cost metric g for each motion primitive generated during node expansion is c It is formulated as a weighted combination of flight energy consumption, path smoothness, and first-order temporal regularization, defined as follows:

[0082]

[0083] Among them, r max is the maximum radius of the drone, a and w T is the user-defined weight, u r is the path smoothness cost. Following the traditional A* algorithm principle, a heuristic function is crucial to accelerate the search process. The present invention uses a cubic spline curve to analyze the connection between the current extended node state and the target state, and the above cost metric g of the spline curve is c Serves as a heuristic function.

[0084] Step S2: performing shape-adaptive trajectory optimization with the initial path as the initial condition to obtain an optimized trajectory, wherein during the trajectory optimization process, the center of mass position and deformation parameters of the UAV are simultaneously optimized;

[0085] Front-end search can generate collision-free paths, but the low-dimensionality and discretization of the search space often result in insufficient trajectory quality for direct UAV execution. To address this limitation, the present invention introduces a subsequent trajectory optimization framework that operates in a continuous spatiotemporal space and uses the initial path as an initial condition to generate refined trajectories by systematically considering higher-order kinematic constraints and smoothness requirements. Unlike traditional approaches that model UAVs as point masses or fixed-radius spheres, the present invention simultaneously optimizes the center of mass position and deformation parameters. This joint optimization fully exploits morphological adaptability to enhance environmental adaptability.

[0086] Given a control torque of order s, the composite trajectory in the flat space [0,T] (T is the end time) use The segmented polynomial function is represented by the order of N = 2s-1, which together represent the geometric deformation radius r and the center of mass position p W , thus providing a compact representation while structurally enforcing that Continuity.

[0087] Under this formulation, the trajectory is given by the polynomial coefficients and time interval Parameterized, where the time interval corresponds to each trajectory. Therefore, the i-th trajectory σ i It can be expressed as:

[0088]

[0089]

[0090] The complete trajectory representation σ(t):[0,T] is formulated as:

[0091]

[0092] The trajectory planning problem is formulated as a nonlinear constrained optimization problem that simultaneously minimizes the position control effort (PCE), radius control effort (RCE), the second-order radius regularization term (SORR), and flight time consumption, subject to obstacle avoidance constraints and dynamic feasibility requirements:

[0093]

[0094] Among them, v max 、a max 、ω max , α max are the maximum speed, maximum acceleration, maximum deformation speed and maximum deformation acceleration of the UAV, r (1) (t) and r (2) (t) are the first and second derivatives of the radius r(t), is the i-th trajectory σ i of Derivative, σ [s-1] (0) is the value of the first-order derivative of the trajectory at t = 0, σ f is the value of the first-order derivative of the trajectory at t = T, where T is the total time.

[0095] Formula σ [s-1] (0)=σ0,σ [s-1] (T)=σ J Boundary constraints are enforced by ensuring that the initial and terminal states of the trajectory strictly adhere to prescribed conditions, which are defined as the position of the UAV, the deformable radius and its (s-1)th-order time derivative.

[0096] formula and formula Dynamic feasibility constraints are enforced by limiting the UAV’s velocity, acceleration, deformation velocity, and deformation acceleration to within prescribed thresholds.

[0097] Considering the geometry of the drone In the case of , the obstacle avoidance constraint requires that all surface points on the drone body are guaranteed to be collision-free. Before grasping the object, the drone system is abstracted as a cylinder with a configurable radius, where the surface points q are discretely sampled. B to approximately enforce the full shape safety condition:

[0098]

[0099] Where R represents the direction of the drone. θ and N l They represent the user-specified angular discretization resolution and the axial discretization resolution along the Z axis, respectively.

[0100] After grasping the object, the deformation radius r becomes a fixed parameter and is no longer involved in the optimization, while the volume of the grasped object is integrated into the composite geometry This composite representation is still an analytically differentiable function of the optimization variables, thus facilitating gradient-based optimization.

[0101] During the grasping task, it is usually desirable for the drone to maintain a stable flight attitude. Therefore, the present invention makes a reasonable assumption that the roll and pitch angles of the drone attitude matrix R are both zero, while the yaw angle is aligned with the velocity direction.

[0102] formula Enforced at waypoints connecting adjacent polynomial segments Continuity.

[0103] To obtain a practical solution, the present invention utilizes the minimum energy trajectory principle (i.e., the control torque cost is minimized) and reformulates the optimization problem by reparameterizing the polynomial coefficients into interconnected waypoints and time intervals. This transformation inherently satisfies the equality constraint σ [s-1] (0)=σ0,σ [s-1] (T)=σ J and At the same time, the dimension of the solution space is reduced. In order to handle the inequality constraint formula and The present invention approximates the continuous-time constraints by uniformly discrete sampling along each polynomial segment, and then relaxes the constraints (i.e., converts hard constraints into soft constraints) through a penalty method. Ultimately, the original problem is converted into an analytically differentiable unconstrained optimization problem and solved efficiently by the L-BFGS solver.

[0104] Step S3: Based on the optimized trajectory, a nonlinear model predictive controller (NMPC) is used as the basis, combined with an incremental nonlinear dynamic inversion (INDI) algorithm, to calculate and compensate for the external force and torque disturbances caused by the UAV deformation and load, thereby achieving shape adaptive control of the deformable UAV.

[0105] Specifically, the optimized trajectory σ(t) obtained includes the center of mass position p of the UAV at all path points W Based on the calculated radius r, the servo motor controller controls the servo motor, thereby achieving deformation. Based on the calculated center of mass position, the NMPC algorithm, combined with the INDI algorithm, compensates for external force and torque disturbances caused by the drone's deformation and load, thereby controlling the brushless motor and achieving trajectory tracking control. This is further explained below.

[0106] First, the dynamic model of the deformable UAV is established. Figure 1 As shown, define the world coordinate system {x W ,y W ,z W} and the body coordinate system {x B ,y B ,z B The location of the quadcopter speed attitude and angular velocity The speed of the four propellers is given by Ω=[Ω1,Ω2,Ω3,Ω4] T express.

[0107] The quadrotor model is based on the kinematics and dynamics equations of a 6-DOF rigid body. The translational dynamics equations of the drone are:

[0108]

[0109] Where F is the total thrust applied to the Z axis of the body, B is the Z axis of the body coordinate system in the world coordinate system, m is the body mass, g W =[0,0,-g] T is the gravity vector, g is the acceleration due to gravity, and f ext Represents the force residual term caused by external disturbance force or model mismatch in the world coordinate system, including the influence of gravity of the grasped object;

[0110] UAV rotation dynamics equation:

[0111]

[0112] in represents the quaternion multiplication operator, τ is the total torque, J t is the deformation-dependent inertia matrix that changes with the deformation process, τ ext It is the torque residual term caused by external disturbance torque or model mismatch in the body coordinate system.

[0113] The thrust and torque of the drone are provided by the propeller and can be expressed as:

[0114]

[0115] H t It is the time-varying control allocation matrix when the quadrotor deforms, which will be updated in real time according to the deformation. t is the vector composed of the thrust of the four motors of the deformable drone.

[0116] The deformable UAV can use nonlinear model predictive control to adjust the rotor. Nonlinear model predictive control can apply thrust saturation constraints and achieve more accurate angular velocity control in this system. The present invention considers the position of the quadrotor. speed attitude and angular velocity Composition of motion state vector UAV thrust F and torque τ T Composition system input u NMPC =[Fτ T ] T The discrete dynamic equations of the quadrotor are x k+1 =f(x k ,u k ) can be obtained by discretizing the translational dynamics equation and rotational dynamics equation of the UAV.

[0117] The nonlinear model predictive controller discretizes the state and input into D equal time intervals within the time range [t, t + Δt], where dt = l / D and l is the length of the time range. A cost function is then formulated based on the state and input errors, and the system dynamics constraints and input boundaries are used as initial conditions to solve for the optimal control input sequence for the NMPC controller:

[0118]

[0119] Among them, i represents the current time step; x i,r and x N,r are the reference state vectors of the current time step and the Nth time step respectively; u i,r is the reference input vector; Q = diag(Q p,Q v ,Q q ,Q ω ) represents the diagonal matrix composed of weights corresponding to position, velocity, attitude and angular velocity, Q N and R is a positive definite weight matrix; u min and u max Indicates the minimum and maximum thrust values ​​that the motor can provide, ensuring that the required thrust input remains within the feasible range.

[0120] In real-world scenarios, deformable drones are also subject to motion disturbances caused by load and deformation, external forces, and propeller aerodynamic effects. To improve mission control performance, such as aerial grasping, this paper considers incorporating these uncertainties into the system's state space representation. Based on the system's translational dynamics, the system's external force disturbance can be observed as:

[0121]

[0122] By compensating for external disturbances in thrust control, the required thrust can be formulated as:

[0123] F des =||Fz B -F ext ||

[0124] Deformable drones can experience torque disturbances due to deformation and load, which can adversely affect control performance. To address this issue, the present invention employs a control strategy based on motor speed and inertial measurement unit feedback—incremental nonlinear dynamic inverse torque compensation—that enables rapid response to input commands and exhibits robustness to model uncertainties and external disturbances. For the rotational dynamics equations of a deformable drone, the external torque disturbance can be calculated as follows:

[0125]

[0126] Among them, τ f is the control torque in the body coordinate system. B,f and denote the measured body angular velocity and angular acceleration, respectively. Assuming that the difference between the gyroscopic torque term and its filtered counterpart is negligible, the above external torque disturbance calculation formula is substituted into the UAV rotation dynamics equation to obtain:

[0127]

[0128] By deriving the desired total thrust and angular acceleration from the speed commands of the four motors and reversing the above formula, the desired control torque command τ can be obtained. des The increment expression is as follows:

[0129]

[0130] Finally, the total thrust and torque input of the quadrotor is:

[0131]

[0132] Among them, H k Assign a matrix to the control of a quadrotor drone.

[0133] In addition, for the deformation process of the drone, the servo motor system can be approximated as a first-order system with a time constant σ. The present invention designs a proportional controller as shown below:

[0134]

[0135] Where K(r) represents the desired servo motor angle mapped to the drone radius, θest is the current servo motor angle, and Ω servo is the desired servo speed.

[0136] The desired servo speed can be calculated by the above formula, so that deformation control can be achieved using the servo motor deformation controller.

[0137] In summary, this paper proposes a novel shape-adaptive motion planner that generates trajectories that intrinsically account for shape deformation in response to the environment, following a hierarchical framework. At the front end, a scalable kinematic dynamics A* path search expands the drone's state space to include deformation parameters and performs a discrete search to construct a search tree from the initial state, ultimately producing a path with a shape transformation. This path then initializes the back-end trajectory optimization module, which formulates a nonlinear optimization problem using compact piecewise polynomial representations in Cartesian and deformation spaces to ensure robust convergence to an optimal smooth trajectory in continuous spacetime. Notably, the back-end trajectory optimization component is inherently scalable and unified, easily incorporating the volume of the grasped object for obstacle avoidance. Furthermore, to improve trajectory tracking accuracy during shape-shifting maneuvers, this paper proposes an enhanced control strategy. This strategy uses NMPC as its foundation and integrates real-time external force estimation into the thrust controller for compensation, combined with the INDI algorithm to effectively counteract external torque disturbances. Experimental evaluation demonstrates that this enhanced control strategy achieves a 37.3% reduction in trajectory tracking error compared to the NMPC algorithm.

[0138] In order to verify the effectiveness of the motion planning algorithm proposed in this invention, experiments and comparisons were carried out in both simulation and real environment experiments.

[0139] 1. Simulation Experiment

[0140] In the simulation, a complex environment containing narrow gaps and dense obstacles is constructed to evaluate the effectiveness of our shape-adaptive motion planning and compared with the traditional trajectory planning algorithm (《Wang Z, Zhou X, Xu C, et al. Geometrically constrained trajectory optimization for multicopters[J]. IEEE Transactions on Robotics, 2022, 38(5): 3259-3278.》) using fixed maximum and minimum sizes (hereinafter referred to as maximum size planning and minimum size planning) to demonstrate its advantages, such as Figure 4 shown.

[0141] In addition, the total energy consumption of the flight was calculated based on a power model, which takes the form of a weighted time integral of the 1.5th power of the UAV's total thrust and the quadratic term of the fuselage radius. Furthermore, the average metric costs were summarized over hundreds of planning runs, as shown in Table 1 below. The data show that the maximum-size plan fails to fully exploit the maneuverability benefits of shape adaptation and performs poorly in terms of time, energy, and trajectory position control. While the minimum-size plan maximizes maneuverability, continuous flight at the minimum size increases energy consumption and is detrimental to actual flight interference rejection. In contrast, the proposed method dynamically adjusts the size of the UAV based on environmental requirements, reducing it only when obstacle avoidance is required. This morphological adaptation strategy achieves a good balance between various cost metrics, maintaining a large size as much as possible to reduce energy consumption while minimizing time consumption and approaching the minimum size. Therefore, the proposed method demonstrates significant superiority in terms of total cost.

[0142] Table 1. Comparison of trajectory performance for maximum size, minimum size, and this method in simulation

[0143] 2. Physical Experiment Verification

[0144] (1) Autonomous deformation navigation and traversal experiment

[0145] The effectiveness of the morphologically adaptive motion planning algorithm was also verified through actual experiments. A complex environment was set up, containing multiple narrow gaps (40cm in width, smaller than the size of the deformable drone) and dense obstacle areas, which posed a significant challenge to autonomous morphological change traversal. Figure 5 As shown in (a) in the figure, the deformable UAV can generate a variable scale trajectory and deformation state series based on the constructed environmental point cloud map and the optimized comprehensive cost. Figure 5As shown in (b) of the figure, the drone undergoes five morphological changes in sequence, which not only enables it to pass through narrow gaps with higher passability, but also allows it to avoid obstacles more efficiently through shorter paths. The proposed enhanced controller can estimate external disturbance forces, such as deformation disturbance forces, and ultimately achieves an average control error of less than 5 cm ( Figure 5 (c) in the figure.

[0146] (2) Autonomous deformation grasping-traversing experiment

[0147] The unique characteristics of the deformable drone not only enable it to avoid obstacles efficiently during flight, but also enable full-body grasping and transportation. It is worth mentioning that this method can be easily extended to grasping and transportation tasks, incorporating the grasped objects into the obstacle avoidance calculation and generating a full-morphology trajectory that takes into account the existence of the objects. To verify this capability, a complex scene was designed, which contains a narrow cross-shaped gap (the width is only 8cm larger than the size of the drone) and multiple obstacles. Figure 6 As shown in the figure, after grasping an object measuring 20 cm in length and width, 40 cm in height, and weighing 160 grams, this method successfully generated a grasp-and-transport trajectory that traversed an irregular cross-shaped gap. This scenario also placed high demands on the drone's precise control. Using an enhanced controller, the drone accurately estimated the external force disturbance, primarily caused by the payload, and ultimately successfully utilized its grasp-and-transport capabilities to move the object through the obstacle.

[0148] (3) Enhanced adaptive control performance

[0149] In order to evaluate the performance of the enhanced adaptive controller, experiments were conducted on morphological deformation and tracking of a figure-eight trajectory, and compared with the reference trajectory optimized by this method and the trajectory formed by the control method provided by Wu Y, Yang F, Wang Z, et al. Ring-rotor: A novel retractable ring-shaped quadrotor with aerial grasping and transportation capability [J]. IEEE Robotics and Automation Letters, 2023, 8(4): 2126-2133. The tracking results are shown in Figure 2. Figure 7 As shown in Figure 2, the enhanced controller achieves more accurate trajectory tracking. Data analysis shows that the root mean square error of the enhanced controller is 0.0352m, which is 37.3% less than that of the previous controller. This improvement comes from the implementation of a torque compensation strategy that estimates external disturbances (such as Figure 7This strategy effectively addresses issues such as model mismatch, deformation interference, and changes in thrust coefficient during morphological changes. The aforementioned morphological deformation and grasping-transport experiments also demonstrate the effectiveness of this approach in multimodal deformation tasks.

[0150] Corresponding to the aforementioned embodiment of the shape adaptive planning and control method for a deformable UAV, the present application also provides an embodiment of a shape adaptive planning and control device for a deformable UAV.

[0151] Figure 8 This is a block diagram of a shape adaptive planning and control device for a deformable UAV according to an exemplary embodiment. Figure 8 , the apparatus may include:

[0152] A path search module 21 is configured to search based on the scene's occupancy grid map using a resizable kinematic A* path planning algorithm to obtain an initial path;

[0153] a trajectory optimization module 22 for performing shape-adaptive trajectory optimization in a continuous spatiotemporal space using the initial path as an initial condition to obtain an optimized trajectory, wherein during the trajectory optimization process, the center of mass position and deformation parameters of the UAV are simultaneously optimized;

[0154] The control module 23 is used to calculate and compensate for the external force disturbance and external torque disturbance caused by the deformation and load of the UAV based on the optimized trajectory and the nonlinear model predictive controller, combined with the incremental nonlinear dynamic inverse algorithm, to achieve shape adaptive control of the deformable UAV.

[0155] Regarding the apparatus in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0156] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0157] Accordingly, the present application also provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the shape adaptive planning and control method for a deformable drone as described above.

[0158] Accordingly, the present application also provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned shape adaptive planning and control method for deformable drones. Figure 9 As shown in the figure, a hardware structure diagram of a shape adaptive planning and control device for a deformable UAV provided by an embodiment of the present invention is provided in any device with data processing capability, except Figure 9 In addition to the processor, memory, and network interface shown, any device with data processing capabilities in which the apparatus in the embodiment is located may also include other hardware, generally based on the actual functions of the device with data processing capabilities, which will not be described in detail.

[0159] Accordingly, the present application also provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the shape adaptive planning and control method for a deformable drone as described above. The computer-readable storage medium can be an internal storage unit of any device with data processing capabilities described in any of the aforementioned embodiments, such as a hard disk or memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), an SD card, a flash card, etc. equipped on the device. Furthermore, the computer-readable storage medium can also include both an internal storage unit and an external storage device of any device with data processing capabilities. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and can also be used to temporarily store data that has been output or is to be output.

[0160] Those skilled in the art will readily conceive of other embodiments of the present application after considering the specification and practicing the contents disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present application that follow the general principles of this application and include common knowledge or customary techniques in the art that are not disclosed in this application.

Claims

1. A shape adaptive planning and control method for a deformable UAV, characterized by: include: Based on the scene's occupancy grid map, a resizable kinematic A* path planning algorithm is used to search and obtain the initial path. Taking the initial path as an initial condition, performing shape-adaptive trajectory optimization in a continuous spatiotemporal space to obtain an optimized trajectory, wherein during the trajectory optimization process, the center of mass position and deformation parameters of the UAV are simultaneously optimized; Based on the optimized trajectory and taking the nonlinear model predictive controller as the basis, combined with the incremental nonlinear dynamic inverse algorithm, the external force disturbance and external torque disturbance caused by the deformation and load of the UAV are calculated and compensated, thereby realizing the shape adaptive control of the deformable UAV.

2. The method according to claim 1, characterized in that In the resizable kinematic A* path search, the combined cost metric of each motion primitive generated during node expansion is used as a heuristic function, and each node in the initial path satisfies safety constraints based on the geometry of the drone.

3. The method according to claim 1, characterized in that In the resizable kinematic A* path planning algorithm, the cost metric of each motion primitive generated during node expansion is used as a heuristic function. The cost metric g c is a weighted combination of flight energy consumption, path smoothness, and first-order temporal regularization: Among them, r max is the maximum radius of the drone, a and w T is a user-defined weight, r represents the radius of the deformable drone, u x,y,z represents the control input, u r is the path smoothness cost, and ΔT represents the time interval allocated for each node expansion during the search process.

4. The method according to claim 1, wherein The resizable kinematic A* path planning algorithm adheres to safety constraints during the path search process. The safety constraints are to discretize the complete geometry of the UAV into sampling points, and require that the obstacle clearance metric of all sampling points be above a predetermined distance threshold: in, Indicates that the complete geometric shape of the drone is considered The obstacle clearance metric, p W represents the center of mass of the UAV, r represents the radius of the deformable UAV, D margin is the predetermined distance threshold.

5. The method according to claim 1, wherein The trajectory optimization is achieved by minimizing the position control torque, radius control torque, second-order radius regularization term, flight time consumption and complying with the obstacle avoidance constraints and dynamic feasibility requirements. The trajectory optimization problem is modeled as: Among them, the trajectory uses The segmented polynomial function is expressed as follows: c and T are the polynomial coefficients and time intervals of the trajectory, s is the order of the control torque, and v max 、a max 、ω max , α max are the maximum speed, maximum acceleration, maximum deformation speed and maximum deformation acceleration of the UAV, r (1) (t) and r (2) (t) are the first and second derivatives of the radius r(t), is the i-th trajectory σ i of Derivative, σ [s-1] (0) is the value of the first-order derivative of the trajectory at t = 0, σ f is the value of the first-order derivative of the trajectory at t = T, where T is the total time.

6. The method according to claim 5, characterized in that The obstacle avoidance constraint requires that all surface points on the drone body are guaranteed to be collision-free. B To approximate enforcement, that is: Among them, R represents the direction of the drone, N θ and N l They represent the user-specified angular discretization resolution and the axial discretization resolution along the Z axis, respectively.

7. The method according to claim 1, characterized in that The control input of the nonlinear model predictive controller is Among them, i represents the current time step; x i,r and x N,r are the reference state vectors of the current time step and the Nth time step respectively; u i,r is the reference input vector; Q = diag(Q p ,Q v ,Q q ,Q ω ) represents the diagonal matrix composed of weights corresponding to position, velocity, attitude and angular velocity, Q N and R is a positive definite weight matrix; u min and u max Indicates the minimum and maximum thrust values ​​provided by the motor.

8. The method according to claim 1, characterized in that Based on the nonlinear model predictive controller and combined with the incremental nonlinear dynamic inverse algorithm, the external force disturbance and external torque disturbance caused by the deformation and load of the UAV are calculated and compensated. Specifically: Based on the translational dynamics of the system, the external force disturbance caused by the deformation and load of the drone is calculated as: By compensating for external disturbances in thrust control, the required thrust is: F des =||Fz B -F ext || Based on the rotational dynamics equation of the deformable UAV, the external torque disturbance caused by the deformation and load of the UAV is calculated: Among them, τ f is the control torque in the body coordinate system, ω B,f and denote the measured body angular velocity and angular acceleration, respectively; Substituting the above external torque disturbance calculation formula into the UAV rotation dynamics equation, we get: Among them, ω B is the angular velocity of the UAV; By deriving the desired total thrust and angular acceleration from the speed commands of the four brushless motors and reversing the above equation, the desired control torque command τ is obtained. des The increment expression is as follows: The total thrust and torque input of the UAV is obtained as follows: Among them, H k Assign a matrix to the control of a quadrotor drone.

9. An electronic device, characterized in that: include: one or more processors; a memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images