Trajectory planning method, device and simulation system for multi-UAV obstacle avoidance simulation control

Abstract

The present invention relates to a trajectory planning method, device and simulation system for multi-UAV obstacle avoidance simulation control; wherein the method includes generating an obstacle avoidance perception map representing the feasible state space of each simulated UAV according to the received location scene depth map output by each simulated UAV; performing path planning according to the obstacle avoidance perception map of each simulated UAV, generating a continuous expected trajectory of each simulated UAV; performing collision judgment between UAVs according to the continuous expected trajectory of each simulated robot in the same time period; performing secondary path planning for two simulated UAVs that are judged to have collided, generating a continuous expected trajectory to avoid UAV collision; and outputting the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation. The present invention realizes safe and efficient obstacle avoidance simulation of multiple UAVs, and provides a complete simulation platform for obstacle avoidance perception, algorithm verification, and trajectory tracking response.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) intelligent control, and in particular relates to a trajectory planning method, a device and a simulation system for multi-UAV obstacle avoidance simulation control. Background Art

[0002] The main obstacle avoidance methods for existing multi-UAV clusters against internal neighboring aircraft and external obstacles are the speed obstacle method and the artificial potential field method. The speed obstacle method uses the relative speed between the UAV and its neighboring aircraft and obstacles to construct a triangular area in the speed space, and selects the achievable speed of the UAV outside the triangular area to achieve cluster obstacle avoidance. The artificial potential field method regards the outside of the UAV to be controlled as an artificial potential field, which consists of a gravitational field and a repulsive field. Obstacles and neighboring aircraft produce repulsion, and target points produce gravitation. Reasonable adjustment of the weights of different objects can enable each single machine in the cluster to achieve inter-machine and external obstacle avoidance. However, these two methods only consider the state of the cluster at a certain moment, and the state of the cluster is constantly changing, so they do not have constraints on the stability, dynamic stability and control continuity of the UAV, which can easily cause the cluster to fall into a local minimum or out-of-control state.

[0003] At the same time, the existing multi-UAV simulation platforms are mostly concentrated in MATLAB / SIMULINK and other systems with poor visualization, modularity and portability. And they only focus on the simulation verification of the algorithm itself, ignoring the constraints of the real environment space complexity, sensor perception ability, data communication ability and flight control ability. If the multi-UAV obstacle avoidance algorithm is verified without considering the above multiple capability constraints, then the verified obstacle avoidance algorithm will inevitably fail when it is verified on multiple machines in the actual environment. . Summary of the invention

[0004] In view of the above analysis, the present invention aims to disclose a trajectory planning method, device and simulation system for multi-UAV obstacle avoidance simulation control, which is used to ensure that multiple UAVs can achieve safe and efficient obstacle avoidance effects, while ensuring the stability and smoothness of the operation of multiple UAVs.

[0005] The present invention discloses a trajectory planning method for multi-UAV obstacle avoidance simulation control, comprising:

[0006] Step S1: generating an obstacle avoidance perception map representing the feasible state space of each simulated drone according to the received scene depth map output by each simulated drone;

[0007] Step S2: performing path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV;

[0008] Step S3, judging the collision between drones according to the continuous expected trajectories of each simulated robot in the same time period; performing secondary path planning on the two simulated drones that are judged to have collided to generate a continuous expected trajectory to avoid the collision of the drones;

[0009] Step S4: outputting the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation.

[0010] Furthermore, the step S2 includes:

[0011] 1) Performing a one-time planning based on global A-star path planning on the obstacle avoidance perception map to generate a discrete path;

[0012] 2) Down-sampling is performed at the set path distance to generate sampled path points;

[0013] 3) Using the downsampled path points as Bezier curve control points, a parameterized trajectory curve equation with respect to time is established;

[0014] 4) Generate the continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

[0015] Furthermore, the step S3 includes:

[0016] 1) The trajectory curves in the same time period are transmitted between multiple UAVs, and the time period is downsampled to obtain multiple sampling moments;

[0017] 2) For the continuous expected trajectory of all UAVs, the sampling position points of each UAV are calculated at each sampling time;

[0018] 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3;

[0019] 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in the direction away from the other party's expected trajectory, so that the distance between the position points on the connection line at the same time exceeds the safe distance;

[0020] 5) Reconstruct the Bezier trajectory curve based on the updated control points to obtain the trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0021] Furthermore, a sampling time set consisting of a plurality of sampling time points is T={0,Δt,2Δt,3Δt,…,nΔt}; n is the number of sampling points, Δt is the sampling interval;

[0022] The set of sampling position points on the n-segment Bezier curve of the k-th UAV is:

[0023]

[0024] On the m-segment Bezier curve, the distance between the two sampling positions of drones i and j at the same time is expressed as:

[0025]

[0026] exist When f is the safe distance; select two control points {Q m ,Q m+1};

[0027] For control points {Q m ,Q m+1} Move the distance distance along the sampling position difference vector dir; the updated control point obtains the updated control point {Q m,new ,Q m+1,new};in,

[0028]

[0029]

[0030]

[0031]

[0032] In the formula, λ m , m+1 ≥1 is the pull-off coefficient;

[0033] According to the updated control points {Q0,Q1,…,Q m,new ,Q m+1,new ,…,Q n}Reconstruct the Bezier trajectory curve to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0034] The present invention also discloses a trajectory planning device for obstacle avoidance control of multiple UAVs, comprising: a perception map generation module, a primary trajectory planning module, a secondary trajectory planning module and a trajectory output module;

[0035] The perception map generation module is used to perform path planning according to the obstacle avoidance perception map of each simulated UAV and generate a continuous expected trajectory for each simulated UAV;

[0036] The primary trajectory planning module is used to perform path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV;

[0037] The secondary trajectory planning module is used to determine the collision between drones based on the continuous expected trajectories of each simulated robot in the same time period; for the two simulated drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of the drones;

[0038] The trajectory output module is used to output the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation.

[0039] Furthermore, the primary trajectory planning module includes a global A-star path planning module, a path sampling module, a Bessel module, and a trajectory generation module;

[0040] A global A-star path planning module, used for performing one-time planning based on the global A-star path planning on the obstacle avoidance perception map to generate a discrete path;

[0041] A path sampling module is used to perform downsampling at a set path distance and generate sampled path points;

[0042] Bezier module, used to use the downsampled path points as Bezier curve control points to establish a parameterized trajectory curve equation about time;

[0043] The trajectory generation module is used to generate a continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

[0044] Furthermore, the secondary trajectory planning module includes a time sampling module, a sampling position point calculation module, a collision judgment module, a planning module and a trajectory output module;

[0045] The time sampling module is used for multiple drones to transmit trajectory curves in the same time period to each other, sample the time period, and obtain multiple sampling moments;

[0046] The sampling position point calculation module is used to calculate the sampling position point of each UAV at each sampling time for the continuous expected trajectory of all UAVs;

[0047] The collision judgment module is used to judge whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, the expected trajectory of the UAV is input into the planning module;

[0048] The quadratic planning module is used to determine two Bezier curve control points with optimized positions in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in a direction away from the expected trajectory of the other party, so that the distance between the position points on the connection line at the same time exceeds the safe distance;

[0049] The trajectory output module is used to reconstruct the Bezier trajectory curve according to the updated control points to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0050] Further, in the time sampling module, a sampling time set consisting of a plurality of sampling times is T={0,Δt,2Δt,3Δt,…,nΔt}; n is the number of sampling points, and Δt is the sampling interval;

[0051] In the sampling position point calculation module, the set of sampling position points on the n-segment Bezier curve of the k-th UAV is:

[0052]

[0053] In the collision judgment module, judge and safety distance f ; is the distance between the two sampling positions of drone i and j at the same time on the m-segment Bezier curve;

[0054] exist When, in the quadratic programming module, two control points {Q m ,Q m+1}; for control point {Q m ,Q m+1} Move the distance distance along the sampling position difference vector dir; the updated control point obtains the updated control point {Q m,new ,Q m+1,new};in,

[0055]

[0056]

[0057]

[0058]

[0059] In the formula, λ m , m+1 ≥1 is the pull-off coefficient;

[0060] In the trajectory output module, according to the updated control points {Q0,Q1,…,Q m,new ,Q m+1,new ,…,Q n}Reconstruct the Bezier trajectory curve to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0061] The present invention also discloses a multi-UAV flight obstacle avoidance control simulation system, comprising a plurality of simulated UAVs and a plurality of trajectory planning devices corresponding one to one with the simulated UAVs;

[0062] The trajectory planning device is the trajectory planning device for multi-UAV obstacle avoidance control as described above;

[0063] The simulated UAVs respectively output the scene depth map of their respective locations to the corresponding trajectory planning devices, and receive the continuous expected trajectory output by the trajectory planning devices to perform flight control simulation, thereby achieving obstacle avoidance flight of the UAV.

[0064] Furthermore, the simulated UAV is a quad-rotor UAV, and the flight controller used is a nonlinear simulation controller.

[0065] The present invention can achieve one of the following beneficial effects:

[0066] The trajectory planning method, device and simulation system of multi-UAV obstacle avoidance simulation control of the present invention realizes safe and efficient obstacle avoidance of multiple UAVs, and at the same time ensures the stability and smoothness of the operation of multiple UAVs. And the external obstacle avoidance of multiple UAVs is realized through global A-star planning, and the internal obstacle avoidance of multiple UAVs is realized by pulling away from the collision point between the UAVs through down-sampling trajectories;

[0067] In addition, the present invention provides a complete simulation platform that can realize obstacle avoidance perception, algorithm verification, and trajectory tracking response, solving the problem that the influence of other module factors cannot be considered in the verification of multi-UAV intelligent control algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0069] Figure 1 Flowchart of the trajectory planning method for multi-UAV obstacle avoidance simulation control in Example 1

[0070] Figure 2 This is a principle block diagram of the trajectory planning device for multi-UAV obstacle avoidance simulation control in Example 2;

[0071] Figure 3 This is a principle block diagram of the multi-UAV flight obstacle avoidance control simulation system in Example 3. DETAILED DESCRIPTION

[0072] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used to illustrate the principles of the present invention together with the embodiments of the present invention.

[0073] Embodiment 1

[0074] One embodiment of the present invention discloses a trajectory planning method for multi-UAV obstacle avoidance simulation control, such as Figure 1 As shown, including:

[0075] Step S1: generating an obstacle avoidance perception map representing the feasible state space of each simulated drone according to the received scene depth map output by each simulated drone;

[0076] Step S2: performing path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV;

[0077] Step S3, judging the collision between drones according to the continuous expected trajectories of each simulated robot in the same time period; performing secondary path planning on the two simulated drones that are judged to have collided to generate a continuous expected trajectory to avoid the collision of the drones;

[0078] Step S4: outputting the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation.

[0079] Specifically, in step S1, it includes:

[0080] 1) Based on the pinhole imaging model of the depth camera, the pixels in the depth map of the drone depth camera are converted into coordinates in the global coordinate system.

[0081] The intrinsic parameter matrix of the drone depth camera is expressed as:

[0082]

[0083] f x 、f y 、c x 、c y is the internal parameter of the camera;

[0084] The external parameter matrix of the drone depth camera is:

[0085]

[0086] R is the rotation matrix of the camera's extrinsic parameters, and t is the translation vector; it is determined by the camera's installation position on the drone.

[0087] According to the camera's intrinsic parameters, the pixel plane is mapped to the camera coordinate system as follows:

[0088]

[0089] Among them, u and v are the two-dimensional pixel coordinates of the image; X, Y, and Z are the coordinates of the camera coordinate system;

[0090] According to the external parameters of the camera, the coordinates in the camera coordinate system are transferred to the global coordinate system as follows:

[0091]

[0092] Among them, Xw, Yw, and Zw are the coordinates of the global coordinate system;

[0093] In this way, the two-dimensional pixel coordinates and depth information in the depth map can be converted into an obstacle space point cloud in the global coordinate system.

[0094] 2) Construct an obstacle perception map based on the obstacle space point cloud in the global coordinate system;

[0095] In the obstacle space point cloud of the continuously captured depth map, whether an obstacle is captured in the unit voxel corresponding to each pixel is dynamic;

[0096] Thus, the obstacle perception map is composed of a dynamic array, in which double-precision floating-point numbers are stored, indicating that the array unit represents the obstacle observation probability of a unit voxel, and the observation probability is updated by the obstacle space point cloud.

[0097] Specifically, the step S2 includes:

[0098] 1) Performing a one-time planning based on global A-star path planning on the obstacle avoidance perception map to generate a discrete path;

[0099] The global A-star algorithm is used to find a discrete safe path from the current position to the target position in the current obstacle perception map. The minimum node of the path is the minimum volume unit of the map. In the A-star algorithm, the total cost of each node in the map is expressed as the sum of the cost from the starting point and the cost from the end point:

[0100] f(n)=g(n)+h(n);

[0101] Starting from the starting point, the A-star algorithm searches for the adjacent nodes with the minimum total cost in turn until the end point is found. The total cost of the node is stored in a priority queue structure to ensure that the algorithm can quickly extract the node with the minimum total cost. In the end, a safe path with the minimum unit of map resolution is obtained, which is represented by path points of consecutive adjacent map unit voxels.

[0102] 2) Down-sampling is performed at the set path distance to generate sampled path points;

[0103] The discrete path calculated by the global A star is downsampled at an appropriate path distance, and continuous adjacent path voxel points are sampled at appropriate distance intervals to form more dispersed path point data.

[0104] 3) Using the downsampled path points as Bezier curve control points, a parameterized trajectory curve equation with respect to time is established;

[0105] Create a parametric trajectory curve equation with respect to time:

[0106]

[0107]

[0108] Among them, w i represents the weight of the i-th item, Indicates the number of combinations.

[0109] 4) Generate the continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

[0110] By using parameterized Bezier curves to represent safe discrete paths as continuous expected trajectories over time, the expected state of the UAV at each moment can be obtained.

[0111] Specifically, the step S3 includes:

[0112] 1) The trajectory curves in the same time period are transmitted between multiple UAVs, and the time period is downsampled to obtain multiple sampling moments;

[0113] The sampling time set composed of multiple sampling time is T = {0, Δt, 2Δt, 3Δt, ..., nΔt}; n is the number of sampling points, Δt is the sampling interval;

[0114] 2) For the continuous expected trajectory of all UAVs, the sampling position points of each UAV are calculated at each sampling time;

[0115] After sampling, the set of sampling position points on the n-segment Bezier curve of the k-th drone is:

[0116]

[0117] 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3;

[0118] 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in the direction away from the other party's expected trajectory, so that the distance between the position points on the connection line at the same time exceeds the safe distance;

[0119] Specifically, on the m-segment Bezier curve, the distance between the two sampling position points of drones i and j at the same time is expressed as:

[0120]

[0121] exist When f is the safe distance; in the secondary trajectory planning, two control points {Q m ,Q m+1};

[0122] For control points {Q m ,Q m+1} Move the distance distance along the sampling position difference vector dir; the updated control point obtains the updated control point {Q m,new ,Q m+1,new};in,

[0123]

[0124]

[0125]

[0126]

[0127] In the formula, λ m , m+1 ≥1 is the pull-off coefficient.

[0128] 5) Reconstruct the Bezier trajectory curve based on the updated control points to obtain the trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0129] According to the updated control points {Q0,Q1,…,Q m,new ,Q m+1,new ,…,Q n}Reconstruct the Bezier trajectory curve to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0130] The continuous expected trajectory of each simulated UAV is output to the flight controller of each UAV for flight simulation.

[0131] In summary, the trajectory planning method of multi-UAV obstacle avoidance simulation control in this embodiment realizes safe and efficient obstacle avoidance of multiple UAVs, and at the same time ensures the stability and smoothness of the operation of multiple UAVs. And the external obstacle avoidance of multiple UAVs is realized through global A-star planning, and the internal obstacle avoidance of multiple UAVs is realized by pulling away from the collision point between the UAVs through down-sampling trajectories.

[0132] Embodiment 2

[0133] One embodiment of the present invention discloses a trajectory planning device for obstacle avoidance control of multiple UAVs, such as Figure 2 As shown, it includes: a perception map generation module, a primary trajectory planning module, a secondary trajectory planning module and a trajectory output module;

[0134] The perception map generation module is used to generate an obstacle avoidance perception map representing the feasible state space of each simulated drone based on the received location scene depth map output by each simulated drone;

[0135] The primary trajectory planning module is used to perform path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV;

[0136] The secondary trajectory planning module is used to determine the collision between drones based on the continuous expected trajectories of each simulated robot in the same time period; for the two simulated drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of the drones;

[0137] The trajectory output module is used to output the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation.

[0138] Specifically, the primary trajectory planning module includes a global A-star path planning module, a path sampling module, a Bessel module, and a trajectory generation module;

[0139] A global A-star path planning module, used for performing one-time planning based on the global A-star path planning on the obstacle avoidance perception map to generate a discrete path;

[0140] A path sampling module is used to perform downsampling at a set path distance and generate sampled path points;

[0141] Bezier module, used to use the downsampled path points as Bezier curve control points to establish a parameterized trajectory curve equation about time;

[0142] The trajectory generation module is used to generate a continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

[0143] Specifically, the secondary trajectory planning module includes a time sampling module, a sampling position point calculation module, a collision judgment module, a planning module and a trajectory output module;

[0144] The time sampling module is used for multiple drones to transmit trajectory curves in the same time period to each other, sample the time period, and obtain multiple sampling moments;

[0145] In the time sampling module, a sampling time set consisting of multiple sampling times is T = {0, Δt, 2Δt, 3Δt, ..., nΔt}; n is the number of sampling points, and Δt is the sampling interval;

[0146] The sampling position point calculation module is used to calculate the sampling position point of each UAV at each sampling time for the continuous expected trajectory of all UAVs;

[0147] In the sampling position point calculation module, the set of sampling position points on the n-segment Bezier curve of the k-th UAV is:

[0148]

[0149] The collision judgment module is used to judge whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, the expected trajectory of the UAV is input into the planning module;

[0150] In the collision judgment module, judge and safety distance f ; is the distance between the two sampling positions of drone i and j at the same time on the m-segment Bezier curve;

[0151] The quadratic planning module is used to determine two Bezier curve control points with optimized positions in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in a direction away from the expected trajectory of the other party, so that the distance between the position points on the connection line at the same time exceeds the safe distance;

[0152] exist When, in the quadratic programming module, two control points {Q m ,Q m+1}; for control point {Q m ,Q m+1} Move the distance distance along the sampling position difference vector dir; the updated control point obtains the updated control point {Q m,new ,Q m+1,new};in,

[0153]

[0154]

[0155]

[0156]

[0157] In the formula, λ m , m+1 ≥1 is the pull-off coefficient.

[0158] The trajectory output module is used to reconstruct the Bezier trajectory curve according to the updated control points to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0159] In the trajectory output module, according to the updated control points {Q0,Q1,…,Q m,new ,Q m+1,new ,…,Q n}Reconstruct the Bezier trajectory curve to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

[0160] The specific technical details and corresponding technical effects of this embodiment are the same as those of the previous embodiment. Please refer to the previous embodiment for details, and no further description will be given here.

[0161] Embodiment 3

[0162] One embodiment of the present invention discloses a multi-UAV flight obstacle avoidance control simulation system, such as Figure 3 As shown, it includes a plurality of simulated UAVs and a plurality of trajectory planning devices corresponding to the simulated UAVs one by one;

[0163] The trajectory planning device is the trajectory planning device for multi-UAV obstacle avoidance control as described in the second embodiment;

[0164] The simulated UAVs respectively output the scene depth map of their respective locations to the corresponding trajectory planning devices, and receive the continuous expected trajectory output by the trajectory planning devices to perform flight control simulation, thereby achieving obstacle avoidance flight of the UAV.

[0165] Specifically, the simulated UAV is a quad-rotor UAV, and the flight controller used is a nonlinear simulation controller; the nonlinear simulation controller includes: a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

[0166] The trajectory solver is used to convert the inputted UAV expected trajectory simulation data into the system expected state quantity at the current moment;

[0167] The position loop controller is used to perform position loop PID control according to the expected state quantity of the system and output the total position-speed control error;

[0168] The nonlinear angle converter is used to perform nonlinear SE (3) space angle conversion on the total error of the position and speed control to obtain the desired spatial rotation matrix;

[0169] The nonlinear attitude mapper is used to perform nonlinear attitude SO(3) space mapping on the current desired spatial rotation matrix and the measured rotation matrix, and output an attitude control error;

[0170] The hybrid controller is used to control the motor speed according to the attitude control error and the total position-speed control error, and output the motor speed to the simulated motor of the simulated UAV.

[0171] Specifically, the input signal of the trajectory solver, the desired trajectory of the drone, is a polynomial equation about time t:

[0172]

[0173] in, represents the position that the UAV should reach at time t, C 3×5 Represents the fifth-order coefficient matrix of a three-dimensional polynomial, [t 4 ,t 3 ,t 2 ,t,1] T represents a higher-order independent variable with respect to time t.

[0174] Specifically, the trajectory solver converts the desired trajectory into the current time t k The system state quantity:

[0175]

[0176] Among them, P des Indicates time t k Expected location V des Indicates time t k Expected speed A des Indicates time t k The expected acceleration At time t k At , take the 0th, 1st, and 2nd derivatives of the desired trajectory respectively:

[0177] P des =F (0) (t k )

[0178] V des =F(1) (t k )

[0179] A des =F (2) (t k )

[0180] At the same time, considering that there may be obstacles on the route, the trajectory solver will expect the yaw angle attitude ψ des Adjust to face forward along the track:

[0181] [Δx,Δy] T =F (0) (t k )-F (0) (t k-1 )

[0182]

[0183] Therefore, the trajectory solver solves the desired trajectory into the desired state quantity of the input flight control

[0184] Specifically, in the position loop controller, the desired state quantity output by the trajectory solver is The input position loop PID is converted into the total error A of position and speed control input .

[0185] e P =P des -P now

[0186] e V =V des -V now

[0187] A input =K P e P +K V e V +K Vi ∫e V +A des +g

[0188] Among them, e P 、e V represents the error between the expected position and the expected velocity, g represents the acceleration due to gravity, A input Indicates the total error of position and speed control; K P Represents the proportional gain of the position error, K V Represents the proportional gain of the speed error, K Vi Indicates the integral gain of the speed error, P now Indicates the current time t of the simulated drone kThe position of V now Indicates the current time t of the simulated drone k speed.

[0189] Specifically, in the nonlinear angle converter, the total error A of the input position speed control is input Perform nonlinear SE (3) space angle transformation to obtain the desired space rotation matrix R des The spatial expected rotation matrix R des By [x B,des ,y B,des ,z B,des ] indicates that, among them,

[0190] z B,des The vector direction representing the acceleration PID value:

[0191]

[0192] y B,des Indicates z B,des With the vector [cosψ des ,sinψ des ,0] T The normal vectors that make up the plane:

[0193]

[0194] x B,des Represents y B,des With z B,des The normal vectors that make up the plane are:

[0195] x B,des =y B,des × B,des

[0196] Thus, the desired rotation matrix R is obtained des .

[0197] Specifically, the spatial desired rotation matrix R is converted into des and the measured rotation matrix R B Perform nonlinear attitude SO(3) space mapping and output attitude control error [e p ,e q ,e r ] T ;

[0198] The measured rotation matrix R B =[x B y B z B ] The three orthogonal axes x in the measured body coordinate system B ,y B ,zB To characterize.

[0199] The desired rotation matrix R des and the current rotation matrix R B By mapping vee to SO(3) space, the attitude angle error e R It is expressed as:

[0200]

[0201] The superscript “∨” represents the vee mapping of SO(3); R B =[x B y B z B ] By the orthogonal three-axis directions x on the body coordinate system B ,y B ,z B To characterize;

[0202] In this embodiment, the attitude angle error e R PID control is performed to output the attitude control error [e p ,e q ,e r ] T :

[0203] [e p ,e q ,e r ] T =K R e R +K Ri ∫e R

[0204] K R Represents the proportional gain of the attitude angle error, K Ri Indicates the integral gain of the attitude angle error.

[0205] Preferably, the simulated drone at the current time t k Position P now , speed V now and the measured rotation matrix R B =[x B y B z B ] is the position measurement sensor including the simulated IMU, GPS module, barometer and magnetometer included in the simulated drone, which is obtained by sensing the simulated data of the natural environment including the simulated force field, atmospheric field and magnetic field in the virtual environment.

[0206] In addition, the simulated UAV current time t k Position P now , speed Vnow and the measured rotation matrix R B =[x B y B z B ] It can also be the current virtual physics engine that obtains the pose data of the simulated drone.

[0207] Specifically, in the hybrid controller, according to the attitude control error [e p ,e q ,e r ] T The total position-speed control error A output by the position loop controller input , control the motor speed and output the motor speed ω to the simulation motor.

[0208] Among them, the thrust generated by the i-th simulation motor satisfies The torque generated satisfies k F is the thrust coefficient; k M is the moment coefficient;

[0209] For the drones in the simulation system, there are:

[0210]

[0211] The obtained speeds of the four motors of the simulated quad-rotor drone are:

[0212]

[0213] Where L is the distance from each motor of the quadcopter to the center of mass of the drone; m is the mass of the drone.

[0214] The four motor speeds are input into the simulation motor so that the simulation motor can simulate the thrust F generated by the propeller rotation of the drone using the brushless DC motor. i , air resistance Rotational torque M i , air resistance moment

[0215]

[0216]

[0217]

[0218]

[0219] Among them, k D is the air resistance coefficient, μ D is the air resistance moment coefficient; represents the airflow speed on the propeller surface, dir turn Indicates the forward and reverse direction of the simulated motor, z B is the normal vector perpendicular to the propeller plane;

[0220] Under the action of four simulated motors, the resultant force and torque of the quad-rotor drone are expressed as:

[0221]

[0222]

[0223] Among them, m is the mass of the drone and g is the acceleration due to gravity.

[0224] The multi-UAV flight obstacle avoidance control simulation system also includes a virtual simulation environment module; the virtual simulation environment module communicates data with each simulated UAV; the simulated virtual scene information is output to the simulated depth camera of the simulated UAV, so that the simulated depth camera can perceive the scene information and obtain front-end sensor data including the scene depth map of the location; the simulated force field, atmospheric field, magnetic field and other natural environment simulation data are output to the simulated IMU, GPS module, barometer and magnetometer in the simulated UAV, so that the simulated IMU, GPS module, barometer and magnetometer can perceive the posture information of the UAV according to the natural environment simulation data.

[0225] Specifically, in the process of setting up the virtual simulation environment module, the main steps include:

[0226] The main steps include:

[0227] 1) Simulation obstacle modeling;

[0228] By modeling in the Gazebo simulation platform, obstacles of different shapes and sizes are constructed to simulate typical structures in various scenes such as forests, buildings, and indoors. In order to verify the adaptability of multi-UAV obstacle avoidance capabilities in different environments, obstacle scenes of different densities are constructed to provide a variety of test environments for obstacle avoidance.

[0229] 2) Virtual physics engine configuration;

[0230] The virtual physics engine simulates the forces on the drone in the natural environment, applies external forces such as gravity, aerodynamics, and resistance to it, and updates its current kinematic state in each iteration through dynamic model calculation. At the same time, the physics engine applies simulated force fields, atmospheric fields, magnetic fields, etc. to the internal sensor modules of the flight control, such as IMU, barometer, and magnetometer, to provide the flight control with natural environment simulation data.

[0231] 3) Writing analog sensor interfaces;

[0232] In the simulation environment and virtual physics engine, build a drone model in the simulated drone. Configure obstacle avoidance perception sensors, write sensor data reading interfaces, the main type of obstacle avoidance sensor is depth camera, build a depth camera model, set the lens field of view, output image format and resolution, camera internal parameters and distortion parameters.

[0233] Using the motor dynamics model in the simulated drone: the thrust generated by a single motor meets The torque generated satisfies Simulate the aerodynamic force and torque generated by the quadrotor UAV motor and write the UAV motor speed input interface.

[0234] 4) Data communication interface writing.

[0235] Write a data communication interface to connect the data of sensors and motors in the drone model with the trajectory tracking flight control; based on the robot operating system ROS, set the UDP port of the flight control and simulation system, connect the data of the simulation sensors IMU, GPS, magnetometer and barometer to the flight control, and transmit the flight control output motor speed to the simulation motor through the topic mechanism to generate simulated aerodynamics.

[0236] In summary, the UAV simulation system of this embodiment overcomes the problem of lack of flight controller and simulation platform for trajectory tracking response testing and verification in the field of autonomous control of UAVs. The present invention provides researchers in the field of autonomous control of UAVs with a bottom-level simulation platform that can realize high-maneuverability flight trajectories, and provides a simulated sensor interface and a flight control debugging interface to facilitate their research on upper-level planning of UAVs.

[0237] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A trajectory planning method for multi-UAV obstacle avoidance simulation control, characterized in that: include: Step S1: generating an obstacle avoidance perception map representing the feasible state space of each simulated drone according to the received scene depth map output by each simulated drone; Step S2: performing path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV; Step S3, judging the collision between drones according to the continuous expected trajectories of each simulated robot in the same time period; performing secondary path planning on the two simulated drones that are judged to have collided to generate a continuous expected trajectory to avoid the collision of drones; Step S4, outputting the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation; The step S3 includes: 1) The trajectory curves in the same time period are transmitted between multiple drones, and the time period is downsampled to obtain multiple sampling moments; 2) For the continuous expected trajectory of all UAVs, the sampling position of each UAV is calculated at each sampling time; 3) Determine whether the distance between the position points of any two UAVs at the same sampling time is within the set safety distance; if yes, proceed to the secondary trajectory planning of step 4), otherwise end step S3; 4) In the secondary trajectory planning, two Bezier curve control points with optimized positions are determined in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in the direction away from the other party's expected trajectory, so that the distance between the position points on the connection line at the same time exceeds the safe distance; 5) Reconstruct the Bezier trajectory curve based on the updated control points to obtain the trajectory of multiple drones that meets the safety obstacle avoidance requirements; The sampling time set composed of multiple sampling times is ; is the number of sampling points, is the sampling interval; No. drones The set of sampling position points on the segment Bezier curve is: ； exist On a Bezier curve, the drone , The distance between two sampling points at the same time is expressed as: ； exist hour, is the safe distance; select the sampling location Two control points on a segment Bezier curve ; Control Points Difference vector along the sampling position Move distance ; Updated control points get updated control points ;in, ； ； ； ； Where, is the pull-off coefficient; Based on the updated control points The Bezier trajectory curve is reconstructed to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

2. The trajectory planning method for multi-UAV obstacle avoidance simulation control according to claim 1 is characterized in that: The step S2 includes: 1) Performing one-time planning based on global A-star path planning on the obstacle avoidance perception map to generate a discrete path; 2) Downsample at the set path distance to generate sampled path points; 3) Use the downsampled path points as Bezier curve control points to establish a parameterized trajectory curve equation about time; 4) Generate the continuous expected trajectory at each moment based on the parameterized trajectory curve equation.

3. A trajectory planning device for obstacle avoidance control of multiple UAVs; characterized in that: include: Perception map generation module, primary trajectory planning module, secondary trajectory planning module and trajectory output module; The perception map generation module is used to perform path planning according to the obstacle avoidance perception map of each simulated UAV and generate a continuous expected trajectory for each simulated UAV; The primary trajectory planning module is used to perform path planning according to the obstacle avoidance perception map of each simulated UAV to generate a continuous expected trajectory for each simulated UAV; The secondary trajectory planning module is used to determine the collision between drones based on the continuous expected trajectories of each simulated robot in the same time period; for the two simulated drones that are judged to have collided, secondary path planning is performed to generate a continuous expected trajectory to avoid the collision of the drones; The trajectory output module is used to output the continuous expected trajectory of each simulated UAV to the flight controller of each UAV for flight simulation; The secondary trajectory planning module includes a time sampling module, a sampling position point calculation module, a collision judgment module, a planning module and a trajectory output module; The time sampling module is used for multiple drones to transmit trajectory curves in the same time period to each other, sample the time period, and obtain multiple sampling moments; The sampling position point calculation module is used to calculate the sampling position point of each UAV at each sampling time for the continuous expected trajectory of all UAVs; The collision judgment module is used to judge whether the distance between the position points of any two drones at the same sampling time is within the set safety distance; If yes, the desired trajectory of the UAV is input into the planning module; The quadratic planning module is used to determine two Bezier curve control points with optimized positions in the respective expected trajectories of the two UAVs; the two Bezier curve control points are the two Bezier curve control points closest to the position point within the safe distance; the two Bezier curve control points are updated, and the connection line between the two Bezier curve control points is extended in a direction away from the expected trajectory of the other party, so that the distance between the position points on the connection line at the same time exceeds the safe distance; The trajectory output module is used to reconstruct the Bezier trajectory curve according to the updated control points to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements; In the time sampling module, the sampling time set consisting of multiple sampling times is ; is the number of sampling points, is the sampling interval; In the sampling position calculation module, drones The set of sampling position points on the segment Bezier curve is: ； In the collision judgment module, judge and safe distance ; For On a Bezier curve, the drone , The distance between two sampling positions at the same time; exist When the sampling location is selected in the quadratic planning module Two control points on a segment Bezier curve ; For control points Difference vector along the sampling position Move distance ; Updated control points get updated control points ;in, ； ； ； ； In the formula, is the pull-off coefficient; In the trajectory output module, according to the updated control point The Bezier trajectory curve is reconstructed to obtain the continuous expected trajectory of multiple UAVs that meets the safety obstacle avoidance requirements.

4. The trajectory planning device for obstacle avoidance control of multiple UAVs according to claim 3 is characterized in that: The primary trajectory planning module includes a global A-star path planning module, a path sampling module, a Bessel module, and a trajectory generation module; A global A-star path planning module, used for performing one-time planning based on the global A-star path planning on the obstacle avoidance perception map to generate a discrete path; A path sampling module is used to perform downsampling at a set path distance and generate sampled path points; Bezier module, used to use the downsampled path points as Bezier curve control points to establish a parameterized trajectory curve equation about time; The trajectory generation module is used to generate a continuous expected trajectory at each moment according to the parameterized trajectory curve equation.

5. A multi-UAV flight obstacle avoidance control simulation system, characterized in that: It includes a plurality of simulated UAVs and a plurality of trajectory planning devices corresponding to the simulated UAVs one by one; The trajectory planning device is a trajectory planning device for multi-UAV obstacle avoidance control as described in any one of claims 3-4; The simulated UAVs respectively output the scene depth map of their respective locations to the corresponding trajectory planning devices, and receive the continuous expected trajectory output by the trajectory planning devices to perform flight control simulation, thereby achieving obstacle avoidance flight of the UAV.

6. The multi-UAV flight obstacle avoidance control simulation system according to claim 5 is characterized in that: The simulated UAV is a four-rotor UAV, and the flight controller used is a nonlinear simulation controller.

Images