A Safe Trajectory Cooperative Planning Method for Air-Ground Heterogeneous Multi-Robots

Abstract

The present invention relates to a safety trajectory collaborative planning method for air-ground heterogeneous multi-robots. Collaborative behavior modeling is carried out according to the motion characteristics of air-ground heterogeneous multi-robots. A guidance path planner for the unmanned vehicle in the spatial domain is constructed on the UAV platform, and the online path optimization problem of the unmanned vehicle is established. The influence factor of obstacle distribution density is introduced to reflect the coupling effect of multiple obstacles on the vehicle. The null space behavior control (NSBC) framework is used to solve the decision conflict problem of inconsistent sensing information caused by angle differences, ensure the complete execution of the obstacle avoidance task while performing fast path planning, and propose an event trigger manager when adjusting the output combination, eliminating the problem of frequent switching of the traditional manager and ensuring the reasonable use of computing resources.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent robots, and in particular to a safe trajectory collaborative planning method for air-ground heterogeneous multi-robots. Background Art

[0002] Due to the progress of automatic control and artificial intelligence, unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) have been successfully applied to various military and civilian fields. However, due to their incomplete perception, decision-making, and control capabilities, UAVs or UGVs still face many challenges. Therefore, it is necessary to combine UAVs and UGVs for collaborative perception, decision-making, and control to utilize their heterogeneous and complementary characteristics. This combination, namely the air-ground heterogeneous system, has been applied to many fields including positioning, agriculture, surveillance, transportation, and construction.

[0003] The trajectory planning of the air-ground system requires UAVs and UGVs to use environmental information to dynamically plan the trajectories of collaborative tasks in real time. Existing trajectory planning methods include planning methods based on numerical optimization such as evolution. These methods usually bring a large computational burden in order to consider the smoothness of the trajectory and the vehicle model. In recent years, model predictive control (MPC) using rolling horizon optimization has shown great potential in the field of trajectory planning because it can better handle dynamic environments and trajectory smoothness. In order to achieve rapid planning of obstacle avoidance trajectories, a potential function is usually introduced as a soft constraint into MPC. However, the non-differentiable characteristics of the potential field function lead to a high computational burden, and suboptimal paths and local minimum points often occur, and the soft constraint does not provide obstacle avoidance guarantees.

[0004] Different from a single UAV or UGV system, the air-ground heterogeneous system allows UAVs and UGVs to jointly plan trajectories and needs to address the problem of inconsistent sensing information. In current research on this problem, when the UGV cannot make a decision autonomously, the UAV is usually designated as the complete alternative decision maker for the UGV. However, the UAV also has sensing and energy limitations, so it is difficult to ensure that the UAV as the decision maker is completely reliable. Therefore, one of the challenges faced by the air-ground system is how to dynamically resolve decision conflicts to achieve collaborative trajectory planning while completing predefined tasks. Behavior control is one of the solutions to this challenge. In this case, the null-space-based behavior control method resolves trajectory planning conflicts by integrating the basic tasks of individual agents to perform collaborative trajectory planning of the air-ground system and ensure the full execution of high-priority tasks and partial execution of low-priority tasks. However, the traditional null-space projection behavior control method uses a finite state machine or a fuzzy task supervisor, which usually leads to frequent switching of task priorities, thereby reducing control performance and system stability. In response, the task supervisor based on model predictive control gives a more intelligent task priority switching by considering prediction information, but the nature of integer programming makes it difficult to achieve in real time when the number of tasks is large.

[0005] Therefore, it is necessary to consider a heterogeneous air-ground system composed of drones as auxiliary roles and unmanned vehicles as actuators. The unmanned vehicle combines the auxiliary decision-making of the drone with its own decision-making and performs real-time cooperative trajectory planning relying only on light computing resources. Summary of the Invention

[0006] The purpose of the present invention is to provide a safe trajectory cooperative planning method for heterogeneous air-ground multi-robots, which combines the model predictive control method with the behavior control method based on null space projection, adopts an event-triggered mechanism, and effectively integrates the decision-making information between drones and unmanned vehicles. First, the drone uses nonlinear model predictive control to generate the guidance trajectory of the unmanned vehicle, considering the coupling effect of multiple obstacles to avoid the local minimum problem; secondly, a behavior control framework based on the null space is used to merge the guidance trajectory into the tasks planned by the unmanned vehicle itself; finally, an event-triggered task supervisor is developed to determine the priority of all tasks. This method can effectively balance the consumed computing resources and improve the autonomous trajectory planning ability of the heterogeneous air-ground system for complex environments.

[0007] To achieve the above object, the technical solution of the present invention is: a safe trajectory cooperative planning method for heterogeneous air-ground multi-robots, including the construction of a guidance trajectory planner, cooperative task design, and dynamic switching of composite tasks. The construction of the guidance trajectory planner based on nonlinear model predictive control described in step S1 is characterized by specifically including the following steps:

[0008] Step S1: Obtain the sensing and detection information of the drone, use the nonlinear model predictive control method to construct an online local guidance trajectory planner, analyze the coupling information of the obstacle distribution from the perspective of the drone to the unmanned vehicle, and generate the intervention trajectory of the drone for the high-altitude perspective of the unmanned vehicle.

[0009] Step S2: Based on the functional characteristics of the drone and the unmanned vehicle, design basic tasks such as cooperation and intervention for the heterogeneous air-ground system based on the behavior control method.

[0010] Step S3: Apply the event-triggered and null space projection mechanisms, design task fusion rules, effectively synthesize the decision-making information from multiple perspectives, and output the trajectory results of cooperative planning for both the drone and the unmanned vehicle.

[0011] Furthermore, the specific steps of step S1 include the following steps:

[0012] Step S11: Reconstruct the time-domain model space of the unmanned vehicle:

[0013] The expression of the nonlinear kinematic model of the unmanned vehicle is, and the state vector including the horizontal and vertical coordinates, height, and heading angle is defined as ξ g =[x g , yg , z g , ψ g , the input vector containing speed and angular velocity is u g = [v g , ω g . The time-domain prediction model of the driverless vehicle is as follows:

[0014]

[0015] where is the derivative of the state with respect to time, and f g is the driverless vehicle system model. Since it is difficult to define the prediction range in the time domain, a nonlinear model with the independent variable converted from time to space is used. The arc length along the trajectory to be tracked is defined as s. Further, the geodetic coordinate system is projected onto the center line of the trajectory to be tracked. Using the differential chain transformation rule, the above time-domain prediction model can be spatially reconstructed as:

[0016]

[0017] where assuming The expression of is as follows:

[0018]

[0019] where ν g B，x and v g B，y represent the longitudinal and lateral speeds of the driverless vehicle relative to the body-fixed coordinate, p g is the position vector of the driverless vehicle, is the desired trajectory, is the desired heading angle, and are the displacement and yaw angle errors of the driverless vehicle respectively. And to ensure the uniqueness of the projection on the center line, the constant μ must satisfy μ > 0 and

[0020] Furthermore, the reconstructed spatial model formula is obtained as:

[0021]

[0022] Step S12: Construct a trajectory generator based on nonlinear model predictive control:

[0023] The trajectory generator based on nonlinear model predictive control is used to plan an optimal trajectory from the current position to a future spatial prediction range. Define N p= L / Δs step lengths as the prediction range, where L is the surveillance range and Δs is the discrete unit step length. This planning process requires discretizing the continuous and infinite-dimensional optimal planning problem within the surveillance range of the UAV, thereby transforming it into a discrete finite-dimensional nonlinear planning process. The optimization problem is as follows:

[0024]

[0025]

[0026]

[0027]

[0028]

[0029] where l(·) and l f (·) are the stage and terminal costs respectively, and and are the predicted state and input vectors of the unmanned vehicle, represents the state and input of the unmanned vehicle at the (k + 1)-th step predicted at the k-th step, represents its current state. It is assumed that the input set u and the state set χ are compact sets.

[0030] Step S13: Construct the objective function of the optimization problem, specifically, construct the obstacle avoidance penalty function of the optimization objective:

[0031] The trajectory generator avoids obstacles by considering the penalty term as a soft constraint in the objective function. This penalty function takes into account the coupling effect of multiple obstacles on the unmanned vehicle and only takes effect when the obstacle is close to the safety distance of the unmanned vehicle to avoid unnecessary penalty effects. The obstacle avoidance penalty function is designed as:

[0032]

[0033]

[0034] where D(k, n) represents the Euclidean distance between the unmanned vehicle and the n-th obstacle at the k-th sampling step, D o is the safety distance between the unmanned vehicle and the obstacle, and K is the number of obstacles within the sensing range. ε r →0 + and c o ≥ 1 is the relaxation variable of the obstacle sensing range, represents the density of obstacles within the safety distance of the predicted sampling points.

[0035] The proposed penalty function amplifies the obstacle avoidance cost at local minimum points, enabling the coupling points of multiple obstacles to be regarded as a virtual obstacle. In the trade-off between tracking and obstacle avoidance costs, when facing local minima, an outward repulsive force is generated to prevent the planned trajectory from approaching it.

[0036] The obstacle avoidance penalty function for the optimization objective described above is:

[0037]

[0038] Furthermore, summarize the prediction range N p The cost function l(·) for each stage within it is:

[0039]

[0040] Among them, is the matrix norm x T Q * x, Q ξ 、Q u 、Q t are the positive definite weight matrices for the state, input, and maneuver time respectively, Q o is the obstacle avoidance weight, and are the desired state and control input quantities.

[0041] Among them, l t is used to minimize the maneuver time required for the unmanned vehicle to reach the end of the prediction range and is defined as:

[0042]

[0043] Among them, is the desired time for the unmanned vehicle to cover each sampling point in the prediction range, is the change rate of s at time k.

[0044] Furthermore, the specific steps of step S2 include the following steps:

[0045] The guidance trajectory generator constructed by nonlinear model predictive control optimizes the guidance state set from the environmental information extracted from the auxiliary perspective It should be noted that the calculated may violate the safety constraints and cannot be directly applied to the unmanned vehicle, which is caused by the deviation of environmental information from different role perspectives of the unmanned aerial vehicle and the unmanned vehicle or the soft constraints that may be violated.

[0046] The above-mentioned design of basic tasks such as cooperation and intervention of the air-ground heterogeneous system is carried out based on the behavior control method.

[0047] By integrating the information from the subjective perspective of the driverless vehicle, guiding the trajectory of the drone is regarded as one of the basic tasks. Meanwhile, other basic tasks such as autonomous trajectory tracking and obstacle avoidance are considered, and then a composite task is formed through the fusion of null space projection based on the event-triggered mechanism.

[0048] Define the task variable as ε, and the corresponding task function can be expressed as:

[0049] ε = f(p) ∈ R M

[0050] where p is the system state vector, and the superscript M ∈ {2, 3} depends on whether the considered robot object is a drone or a driverless vehicle. Its corresponding partial derivative is:

[0051]

[0052] where v is the speed vector of the robot, and J(p) represents the Jacobian matrix of the task. Therefore, the speed output command for a single task is:

[0053]

[0054] where Λ is a positive definite constant gain matrix, ε d is the desired task, and the error of the task is And represents the pseudo-inverse of the Jacobian matrix J(p).

[0055] Furthermore, the specific steps of step S3 include the following steps:

[0056] By means of null space projection, the output command of the final composite task is realized by combining the prioritization of basic tasks through the task supervisor. Among them, the low-priority tasks are projected into the null space of the high-priority tasks to eliminate task conflicts, and the combined output of multiple tasks The formula is as follows:

[0057]

[0058] where the superscript η ∈ {a, g} can represent either a drone or a driverless vehicle, is the speed output of task p, is the Jacobian matrix of task p, and the priority of the task with subscript p is higher than p + 1. However, fixed task priorities may lead to a decline in the performance of the robot's behavior decision-making in dynamic and unknown environments, and frequent adjustment of task priorities will result in an uneven planned trajectory.

[0059] Furthermore, a method based on the event-triggered mechanism dynamically adjusts task priorities and balances computational resource consumption through a task supervisor.

[0060] Define the following two priority switching metrics:

[0061] Evaluate the deviation between the actual or planned system state and the desired state through the tracking error metric r e which is defined as: Define the real-time impact r of evaluating the detected obstacle

[0062]

[0063] Considering the perception accuracy of the UAV or the computational burden of the guidance trajectory calculation, the trajectory tracking task switches between the autonomous tracking task and the guidance trajectory tracking task according to the tracking error metric r o :

[0064]

[0065] This invention also provides a safe trajectory collaborative planning system for air-ground heterogeneous multi-robots, which includes a processor, a memory, and a computer program stored on the memory. When the processor runs the program instructions, it can implement the method steps as described above. e This invention also provides a computer-readable storage medium with computer program instructions stored thereon. When the instructions are loaded and executed by a processor, they can implement the method as described above.

[0066] Compared with the prior art, this invention has the following beneficial effects:

[0067] The UAV uses nonlinear model predictive control to generate a guidance trajectory for the unmanned vehicle, and the penalty function for obstacle avoidance is designed as a soft constraint. Different from the previous solutions that also regarded obstacle avoidance as a soft constraint, the penalty function considers the obstacle distribution density to reflect the nonlinear coupling effect of multiple obstacles around the unmanned vehicle. It can effectively avoid the local minimum problem without increasing the online computational burden. The collaborative framework based on null space projection solves the decision-making conflict problem caused by inconsistent perspective perception information between the UAV and the unmanned vehicle. Compared with the nonlinear model predictive control method using obstacle avoidance as a hard constraint, the collaborative framework generates the trajectory scheme much faster, and at the same time avoids the adjustment of the weight parameters of different tasks in the decision-making fusion process, and can also avoid the problem that the UAV-assisted trajectory may violate the constraints. The task manager based on the event-triggered mechanism greatly reduces the task switching frequency and improves the smoothness of the planned trajectory compared with the traditional task supervisor.

[0068] Brief Description of the Drawings

[0069]

[0070] ​​The accompanying drawings, which form a part of this application, are used to provide a further understanding of the present invention. The schematic embodiments and descriptions thereof of the present invention are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0071] Figure 1 It is a schematic block diagram of the method principle of an embodiment of the present invention.

[0072] Figure 2 It is a scene diagram of the aerial-ground heterogeneous multi-robot trajectory planning experiment of an embodiment of the present invention.

[0073] Figure 3 It is a trajectory diagram of the aerial-ground heterogeneous multi-robot collaborative planning method and the traditional method of an embodiment of the present invention.

[0074] Figure 4 It is a diagram of the task priority switching situation of the event-triggered supervisor and the traditional method of an embodiment of the present invention. Detailed implementation manners

[0075] Next, in conjunction with the accompanying drawings, the technical solutions of the present invention will be specifically described.

[0076] This embodiment provides a safe trajectory collaborative planning method for aerial-ground heterogeneous multi-robots. In this embodiment, a drone, a self-driving vehicle, and several obstacles are selected for illustration, and the specific steps are as follows:

[0077] Obtain the sensing detection information of the drone, use the nonlinear model predictive control method to construct an online local guidance trajectory planner, analyze the coupling information of the obstacle distribution from the drone's perspective to the self-driving vehicle, and generate an intervention trajectory of the drone for the high-altitude perspective of the self-driving vehicle.

[0078] For the functional characteristics of the drone and the self-driving vehicle, design basic tasks such as collaboration and intervention of the aerial-ground heterogeneous system based on the behavior control method.

[0079] Apply the event-triggered and null-space projection mechanisms, design task fusion rules, effectively synthesize decision-making information from multiple perspectives, and output the trajectory results of collaborative planning for both the drone and the self-driving vehicle at the same time.

[0080] In this embodiment, the main goal of the aerial-ground system to execute tasks is to make the self-driving vehicle track the pre-planned trajectory as much as possible, while the drone generates a guidance trajectory to ensure that the self-driving vehicle avoids obstacles and local minima when moving along the pre-planned trajectory, and the drone needs to maintain the required formation structure with the self-driving vehicle.

[0081] In this embodiment, a typical heterogeneous air-ground system is considered, where the unmanned vehicle avoids multiple obstacles that may be encountered during the process of reaching the target position, and the unmanned aerial vehicle can obtain information about the obstacles within the surveillance range. The state vector of the air-ground system in the global coordinate system \(W_{xyz}\), which includes the horizontal and vertical coordinates, altitude, and heading angle, is defined as \(\xi\) η (t)=[x η (t), y η (t), z η (t), \(\psi\) η (t)], and the symbols \(\eta\in\{a, g\}\) represent the unmanned aerial vehicle and the unmanned vehicle respectively. The Cartesian coordinates \(x\) η , \(y\) η and \(z\) η define the positions of the unmanned aerial vehicle and the unmanned vehicle \(\psi\) η represents the yaw angle.

[0082] This air-ground system adopts a virtual structure as the formation strategy in three-dimensional space and tracks a pre-planned parametric trajectory relative to the unmanned vehicle where is always zero. The relative formation position of the unmanned aerial vehicle in the inertial system can be described as

[0083]

[0084] where \(\gamma\) is the relative distance between the unmanned aerial vehicle and the unmanned vehicle, and its projection on the XY plane is the straight line \(\gamma'\) connecting the unmanned vehicle and the target point. The angle between \(\gamma'\) and the X-axis is \(\beta\), is defined as the angle between \(\gamma'\) and the plane XY. Note that \(\gamma\in[0, \gamma\) M is restricted by the maximum communication distance \(\gamma\) M between the unmanned vehicle and the unmanned aerial vehicle, and is restricted by the minimum field of view angle of the unmanned aerial vehicle to ensure that the unmanned vehicle is visible relative to the unmanned aerial vehicle.

[0085] Step S11, spatial reconstruction of the unmanned vehicle time-domain model:

[0086] The expression of the nonlinear kinematic model of the unmanned vehicle is:

[0087]

[0088] where \(v\) g represents the speed of the unmanned vehicle, \(\omega\) g represents the angular velocity, \(\psi\) g is the yaw angle, and are the longitudinal speed and the lateral speed of the unmanned vehicle in the geodetic coordinate system respectively.

[0089] Furthermore, the state vector including the horizontal and vertical coordinates, height, and heading angle is defined as ξ g = [x g , y g , z g , ψ g , and the input vector including the velocity and angular velocity is u g = [ν g , ω g . The time-domain prediction model of the unmanned vehicle is as follows:

[0090]

[0091] where is the derivative of the state with respect to time, and f g is the unmanned vehicle system model. Furthermore, by projecting the geodetic coordinate system onto the center line of the trajectory to be tracked and using the differential chain transformation rule, the above time-domain prediction model can be spatially reconstructed as:

[0092]

[0093] where assuming that is expressed as follows:

[0094]

[0095] where v g B，x and v g B，y represent the longitudinal and lateral velocities of the unmanned vehicle relative to the body-fixed coordinate, p g is the position vector of the unmanned vehicle, is the desired trajectory, is the desired heading angle, and are the displacement and yaw angle errors of the unmanned vehicle respectively. And to ensure the uniqueness of the projection on the center line, the constant μ must satisfy μ > 0 and

[0096] Step S12: Construct a trajectory generator based on nonlinear model predictive control:

[0097] The trajectory generator based on nonlinear model predictive control is used to plan an optimal trajectory from the current position to a future spatial prediction range. Define N p = L / Δs steps as the prediction range, where Δs is the discrete unit step, and the monitoring range The optimization problem is as follows:

[0098]

[0099]

[0100]

[0101]

[0102]

[0103] where \(l(\cdot)\) and \(l f (\cdot)\) are the stage and terminal costs, respectively, and and are the predicted state and input vector of the autonomous vehicle, representing the state and input of the autonomous vehicle at the \((k + 1)\)-th step predicted at the \(k\)-th step, representing its current state, assuming that the input set \(u\) and the state set \(\chi\) are compact sets.

[0104] Step S13: Construct the objective function of the optimization problem

[0105] Furthermore, construct an obstacle avoidance penalty function for the optimization objective.

[0106] Consider the coupling effect of multiple obstacles on the autonomous vehicle and only take effect when the obstacles are close to the safety distance of the autonomous vehicle to avoid unnecessary penalty effects. The obstacle avoidance penalty function is designed as:

[0107]

[0108]

[0109] where \(D(k,n)\) represents the Euclidean distance between the autonomous vehicle and the \(n\)-th obstacle at the \(k\)-th sampling step, \(D o is the safety distance between the autonomous vehicle and the obstacle, \(K\) is the number of obstacles within the sensing range. \(\epsilon r \to 0 + , \(c o \geq 1\) is the relaxation variable of the obstacle sensing range, representing the density of obstacles within the safety distance of the predicted sampling points.

[0110] The obstacle avoidance penalty function of the optimization objective is:

[0111]

[0112] Furthermore, summarize the cost function \(l(\cdot)\) of each stage within the prediction range \(N p \) as:

[0113]

[0114] Among them, is the matrix norm x T Q * x, Q ξ , Q u , Q t are the positive definite weight matrices of the state, input, and maneuver time respectively, and Q o is the obstacle avoidance weight. and are the desired state and control input quantities.

[0115] Among them, l t is used to minimize the maneuver time required for the unmanned vehicle to reach the end of the prediction range and is defined as:

[0116]

[0117] Among them, is the desired time at each sampling point of the prediction range covered by the unmanned vehicle at the k-th step, is the change rate of s at the k-th step.

[0118] Furthermore, the specific steps of step S2 include the following steps:

[0119] In order to fuse the information from the subjective perspective of the unmanned vehicle, the trajectory guidance of the unmanned aerial vehicle is taken as one of the basic tasks. At the same time, other basic tasks such as autonomous trajectory tracking and obstacle avoidance are considered, and then a composite task is formed through zero-space projection fusion based on the event-triggered mechanism.

[0120] Step S21, Design of basic tasks:

[0121] Furthermore, we define two basic task functions for the air-ground system, namely the trajectory tracking task and the obstacle avoidance task Let be the desired task function, is the task error, represents the Jacobian matrix of the task.

[0122] The trajectory tracking task function is defined as:

[0123]

[0124] Among them, p η is the position coordinate of the unmanned aerial vehicle or unmanned vehicle. The unmanned vehicle moves along the space arc length given by the unmanned aerial vehicle as the independent variable for the guiding trajectory and and are the longitudinal and lateral coordinate components along the trajectory, which can be converted back to the trajectory with time as the independent variable and is expressed as:

[0125]

[0126] The output of the trajectory tracking task corresponding to the autonomous vehicle based on closed-loop inverse kinematics is:

[0127]

[0128] The output signal of the trajectory tracking task function for the unmanned aerial vehicle to maintain the formation geometry is:

[0129]

[0130] Among them, is the Jacobian matrix and gain matrix of the UAV formation task, and the desired velocity of this task is defined as:

[0131]

[0132] Among them, is the task error formed by the UAV deviating from the required virtual structure, is the coordinate of the final target point of the autonomous vehicle.

[0133] Considering in the UAV trajectory generator, obstacle avoidance is achieved by considering the soft constraints in the objective function. Due to the inherent nature of the soft constraints and the inconsistent sensor information of the heterogeneous air-ground system, the generated trajectory may still violate the safety constraints. Here, strict obstacle avoidance is introduced as a basic task to compensate for this inconsistency.

[0134] When an obstacle appears within the safety range D o of the autonomous vehicle, the obstacle avoidance task can drive the autonomous vehicle away from the obstacle. The influence of the nth obstacle on the autonomous vehicle is defined as:

[0135]

[0136] Among them, is the position of the nth obstacle, In a complex environment, the influence of multiple obstacles around the autonomous vehicle must be analyzed simultaneously. The task function considering all obstacles within the safety distance of the autonomous vehicle is written as:

[0137]

[0138] The Jacobian matrix of the obstacle avoidance task is:

[0139]

[0140] The output of the obstacle avoidance task is obtained by the following formula:

[0141]

[0142] where and is the task error, is the error gain matrix of the obstacle avoidance task.

[0143] Step S22, Multi-task conflict resolution

[0144] Furthermore, by means of null space projection, the output instruction of the final composite task is realized by combining the basic tasks with priority sorting through the task supervisor, where the low-priority tasks are projected into the null space of the high-priority tasks to eliminate task conflicts. The combined output of multiple tasks The formula is as follows:

[0145]

[0146] The superscript η ∈ {a, g} can represent either the unmanned aerial vehicle or the unmanned vehicle, is the speed output of task p, is the Jacobian matrix of task p, where the priority of the task with subscript p is higher than p + 1. The task supervisor based on the event-trigger mechanism can dynamically adjust the task priority and balance the computational resource consumption.

[0147] Furthermore, define the following two priority switching metrics:

[0148] Evaluate the deviation between the actual or planned system state and the desired state through the tracking error metric r e as defined as:

[0149]

[0150] Define to evaluate the real-time impact r of the detected obstacle o as:

[0151]

[0152] Considering the perception accuracy of the unmanned aerial vehicle or the computational burden of the guidance trajectory, the trajectory tracking task switches between the autonomous tracking task and the guidance trajectory tracking task according to the tracking error metric re. Define the following triggering rules:

[0153]

[0154]

[0155] where is τ the guidance trajectory tracking task triggered by the reference state condition T1 of the unmanned vehicle at time, and T2 is the task priority of to resume the autonomous trajectory tracking task of the unmanned vehicle itself. Among them τ is the earliest time that meets specific conditions. is the task output of autonomous trajectory tracking, where is the Jacobian matrix of the unmanned vehicle tracking task, is the desired task, is the error gain matrix of the autonomous tracking task. is the bound of the state deviation, is the tracking task priority switching bound difference, which is used to prevent high-frequency switching of tasks within a limited time.

[0156] Multiple obstacles usually lead to continuous violation of the obstacle avoidance constraint. To solve this problem, the following trigger rules are defined:

[0157]

[0158]

[0159] Condition T3 triggers the obstacle avoidance task, and its task priority is T4 restores the task priority to execute the trajectory task, where q ∈ {m, t} is determined by T1 and T2. ε o is the obstacle avoidance task priority switching bound difference, which is used to avoid frequent task priority switching.

[0160] In this embodiment, the proposed cooperative trajectory planning method is compared with the nonlinear model predictive control method that takes obstacle avoidance as a hard constraint, and the event-triggered task supervisor deployed in the air-ground system is compared with the traditional finite state machine-based task supervisor only deployed on the unmanned vehicle platform without UAV assistance, and the results are statistically analyzed.

[0161] The parameter settings of the cooperative trajectory planning method are shown in Table 1.

[0162] Table 1 Parameter settings of the cooperative trajectory planning method

[0163]

[0164]

[0165] Considering a scenario where multiple obstacles form local minimum points as Figure 2 shown, the trajectory planning result is as Figure 3As shown. At t1, the air-ground system forms the required formation. At t2, the unmanned vehicle encounters a local minimum point. When the trigger event thresholds T3 and T4 are triggered, the drone generates a guidance trajectory for the unmanned vehicle. At t3, the unmanned vehicle returns to the reference trajectory under guidance and finally reaches the target position at t4. In contrast, the method using a fuzzy logic mission supervisor (FMS) and a standard nonlinear model predictive control (NMPC) fails to help the unmanned vehicle escape from the local minimum point. As Figure 4 shown, the planner using the FMS mission manager gets stuck at t = 3 s. On the other hand, although the guidance trajectory from the drone violates the safety constraint, the unmanned vehicle can still avoid obstacles due to its own obstacle avoidance task.

[0166] As described above, it is only the preferred embodiment of the present invention, and it is not a limitation to the present invention in other forms. Any person skilled in the art may use the disclosed technical content to make changes or modifications into equivalent embodiments with equivalent changes. However, any simple modification, equivalent change and modification made to the above embodiments based on the technical essence of the present invention without departing from the technical solution content of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A safety trajectory collaborative planning method for air-ground heterogeneous multi-robots. The air-ground heterogeneous robots consist of unmanned aerial vehicles and unmanned ground vehicles, and are characterized in that It includes the following steps: Step S1: Obtain the sensing and detection information of the drone. Use the nonlinear model predictive control method to construct an online local guidance trajectory planner, analyze the coupling information of the obstacle distribution from the drone's perspective on the unmanned vehicle, and generate an intervention trajectory of the drone for the high-altitude perspective of the unmanned vehicle; Step S2: For the functional characteristics of the drone and the unmanned vehicle, design the basic tasks including the cooperation and intervention of the air-ground heterogeneous system based on the behavior control method; Step S3: Apply the event-triggering and null-space projection mechanism, design the task fusion rule, synthesize the decision-making information from multiple perspectives, and output the trajectory results of the cooperative planning for both the drone and the unmanned vehicle at the same time; The specific steps of Step S2 include the following steps: The guidance trajectory generator constructed by non-linear model predictive control, for the set of guidance states optimized according to the environmental information extracted from the auxiliary perspective The design of the basic tasks including the cooperation and intervention of the air-ground heterogeneous system based on the behavior control method, that is, by fusing the information from the subjective perspective of the unmanned vehicle, taking the trajectory guidance of the drone as one of the basic tasks, and at the same time considering other basic tasks such as autonomous trajectory tracking and obstacle avoidance, and then forming a composite task through the null-space projection fusion based on the event-triggering mechanism; Define the task variable as ε, and the corresponding task function is expressed as: ε = f(p) ∈ R M Among them, p is the system state vector, and the superscript M∈{2,3} depends on whether the considered robot object is a drone or an unmanned vehicle, and its corresponding partial derivative is: Among them, v is the speed vector of the robot, and J(p) represents the Jacobian matrix of the task. Therefore, the speed output instruction of a single task is: where Λ is a positive definite constant gain matrix, and ε d is the desired task, and the error of the task is and denotes the pseudo-inverse of the Jacobian matrix J(p); The specific steps of Step S3 include the following steps: The output instruction of the final composite task is formed by the method of null space projection, which is achieved by combining the prioritization of basic tasks through a task supervisor, where low-priority tasks are projected into the null space of high-priority tasks to eliminate task conflicts, and the combined output of multiple tasks The formula is as follows: where the superscript η ∈ {a, g} can represent either a drone or an unmanned vehicle, is the speed output for task p, is the Jacobian matrix of task p, and the priority of the task with subscript p is higher than that of p + 1; The event-triggering mechanism dynamically adjusts the task priority and balances the consumption of computing resources through the task supervisor; Define the following two priority switching indicators: By tracking the error metric r e Evaluate the deviation between the actual or planned system state and the desired state Defined as: Define the real-time impact r of evaluating the detected obstacle o : Considering the perception accuracy of the UAV or the computational burden of calculating the guidance trajectory, the trajectory tracking task is based on the tracking error index r e Switch between the autonomous tracking task and the guided trajectory tracking task.

2. The safety trajectory collaborative planning method for air-ground heterogeneous multi-robots according to claim 1, wherein The specific steps of Step S1 include the following steps: Step S11: Spatial reconstruction of the time-domain prediction model of the unmanned vehicle; The expression of the non - linear kinematic model of the unmanned vehicle is: Define the state vector including the abscissa, ordinate, altitude, and heading angle as ξ g =[x g , y g , z g , ψ g , and the input vector including the speed and angular velocity is u g =[v g , ω g . The time - domain prediction model of the unmanned vehicle is as follows: where is the derivative of the state with respect to time, and f g is the unmanned vehicle system model, which is transformed from a time-dependent independent variable to a space-dependent independent variable non-linear model. The arc length along the trajectory to be tracked is defined as S, and the geodetic coordinate system is projected onto the center line of the trajectory to be tracked. Using the differential chain transformation rule, the time-domain prediction model of the unmanned vehicle is spatially reconstructed as: Among them, Suppose The expression is as follows: Among them, v g B，x and v g B，y represent the longitudinal and lateral speeds of the driverless vehicle relative to the body-fixed coordinates, p g is the position vector of the driverless vehicle, is the desired trajectory, is the desired heading angle, and are the displacement and yaw angle errors of the driverless vehicle respectively. To ensure the uniqueness of the projection on the center line, the constant μ must satisfy μ > 0 and The reconstructed spatial model formula is obtained as: Step S12: Construct a trajectory generator based on the nonlinear model predictive control; The trajectory generator based on nonlinear model predictive control is used to plan an optimized trajectory from the current position to a future spatial prediction range. Define N p = L / Δs step lengths as the prediction range, where L is the monitoring range and Δs is the discrete unit step length. This planning process needs to discretize the continuous and infinite-dimensional optimal planning problem within the monitoring range of the UAV, thus transforming it into a discrete finite-dimensional nonlinear programming process. The optimization problem is as follows: where \(l(\cdot)\) and \(l f (\cdot)\) are the stage and terminal costs respectively, and and are the predicted state and input vector of the autonomous vehicle, representing the state and input of the autonomous vehicle at the \((k + 1)\)-th step predicted at the \(k\)-th step, representing its current state, assuming the input set and the state set are compact sets; Step S13: Construct an obstacle avoidance penalty function for the optimization objective; The avoidance of obstacles by the trajectory generator is achieved by considering the penalty term as a soft constraint in the objective function. This penalty function takes into account the coupling effect of multiple obstacles on the unmanned vehicle and only takes effect when the obstacle is close to the safe distance of the unmanned vehicle to avoid unnecessary penalty effects. The obstacle avoidance penalty function is designed as: Among them, D(k, n) represents the Euclidean distance between the autonomous vehicle and the nth obstacle at the kth sampling step, D o is the safety distance between the autonomous vehicle and the obstacle, K is the number of obstacles within the sensing range, ε r →0 + , c o ≥1 is the slack variable of the obstacle sensing range, represents the density of obstacles within the safety distance of the predicted sampling point; The obstacle avoidance penalty function for the optimization objective is: Summary prediction range N p The cost function l(·) for each stage within it is as follows: where, is the matrix norm x T Q * x, Q ξ and Q u and Q t are positive definite weight matrices of the state, input, and maneuvering time respectively, and Q o is the obstacle avoidance weight, and are the desired state and control input quantities; where l t is used to minimize the maneuvering time required for the driverless vehicle to reach the end of the prediction range, and is defined as: where, is the expected time at each sampling point within the predicted range covered by the driverless vehicle at the k-th step, is the change rate of s at the k-th step.

3. A safety trajectory collaborative planning system for air-ground heterogeneous multi-robots, characterized in that, It includes a processor, a memory, and a computer program stored on the memory. When the processor runs the program, it can implement the method according to any one of claims 1-2.

4. A computer-readable storage medium, characterized in that, There are computer program instructions stored thereon. When the instructions are loaded and executed by the processor, it can implement the method according to any one of claims 1-2.

Images