CN115903899A - Trajectory planning method, device and simulation system for multi-unmanned aerial vehicle obstacle avoidance simulation control - Google Patents

Abstract

The invention relates to a track planning method, a device and a simulation system for multi-unmanned aerial vehicle obstacle avoidance simulation control; the method comprises the steps of generating an obstacle avoidance perception map representing the feasible state space of each simulation unmanned aerial vehicle according to a received position scene depth map output by each simulation unmanned aerial vehicle; carrying out path planning according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles to generate a continuous expected track of each simulated unmanned aerial vehicle; carrying out collision judgment between the unmanned aerial vehicles according to the continuous expected tracks of the simulation robots in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles with the collision to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles; and respectively outputting the continuous expected track of each simulated unmanned aerial vehicle to a flight controller of each unmanned aerial vehicle for flight simulation. The invention realizes safe and efficient obstacle avoidance simulation of multiple unmanned aerial vehicles and provides a complete simulation platform for obstacle avoidance perception, algorithm verification and track tracking response.

Description

Trajectory planning method, device and simulation system for multi-unmanned aerial vehicle obstacle avoidance simulation control

Technical Field

The invention belongs to the technical field of intelligent control of unmanned aerial vehicles, and particularly relates to a track planning method, a device and a simulation system for obstacle avoidance simulation control of multiple unmanned aerial vehicles.

Background

The main obstacle avoidance methods of the existing cluster multi-unmanned aerial vehicle aiming at internal neighboring machines and external obstacles are a speed obstacle method and an artificial potential field method. The speed obstacle method is characterized in that a triangular area is constructed in a speed space by utilizing the relative speed between the unmanned aerial vehicle and an adjacent aircraft and an obstacle, and the cluster obstacle avoidance can be realized by selecting the reachable speed of the unmanned aerial vehicle outside the triangular area. The artificial potential field method regards the exterior of the unmanned aerial vehicle to be controlled as an artificial potential field, the potential field is composed of a gravitational field and a repulsive force field, the obstacle and the adjacent machines generate repulsive force, the target point generates attractive force, and the obstacle avoidance between the machines and the exterior of each single machine in the cluster can be realized by reasonably adjusting the weights of different objects. However, the two methods only consider the state of the cluster at a certain moment, and the state of the cluster changes from moment to moment, so that the method does not have constraints on the stationarity, the dynamic stability and the control continuity of the unmanned aerial vehicle, and the cluster is easily caused to fall into a local minimum or an out-of-control state.

Meanwhile, the existing multi-unmanned aerial vehicle simulation platform is mostly concentrated on systems such as MATLAB/SIMULINK and the like with poor visualization, modularization and portability for operation. And only the simulation verification of the algorithm is paid attention to, and the restrictions of the space complexity, the sensor sensing capability, the data communication capability and the flight control capability of the real environment are ignored. If the multi-unmanned-plane obstacle avoidance algorithm is verified without considering the various capacity restriction factors, the verified obstacle avoidance algorithm inevitably fails when multi-plane verification is carried out in the actual environment. .

Disclosure of Invention

In view of the above analysis, the present invention aims to disclose a trajectory planning method, device and simulation system for multi-drone obstacle avoidance simulation control, which are used to ensure that multi-drone achieve safe and efficient obstacle avoidance effect and ensure stability and smoothness of multi-drone operation.

The invention discloses a track planning method for multi-unmanned aerial vehicle obstacle avoidance simulation control, which comprises the following steps:

s1, generating an obstacle avoidance perception map representing feasible state space of each simulation unmanned aerial vehicle according to received position scene depth maps output by each simulation unmanned aerial vehicle;

s2, planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles to generate a continuous expected track of each simulated unmanned aerial vehicle;

s3, judging collision between the unmanned aerial vehicles according to the expected continuous track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles which collide to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and S4, respectively outputting the continuous expected tracks of the simulated unmanned aerial vehicles to the flight controllers of the unmanned aerial vehicles for flight simulation.

Further, the step S2 includes:

1) Performing primary planning on the obstacle avoidance perception map based on global A star path planning to generate a discrete path;

2) Performing down-sampling according to the set path distance to generate sampled path points;

3) Taking the down-sampled path points as Bezier curve control points, and establishing a parameterized trajectory curve equation related to time;

4) And generating a continuous expected track at each moment according to a parameterized track curve equation.

Further, the step S3 includes:

1) Mutually transmitting the track curves in the same time period among the multiple unmanned aerial vehicles, and performing down-sampling on the time period to obtain multiple sampling moments;

2) Calculating sampling position points of all unmanned aerial vehicles at each sampling moment aiming at the continuous expected tracks of all the unmanned aerial vehicles;

3) Judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, entering the secondary trajectory planning of the step 4), otherwise, ending the step S3;

4) In secondary trajectory planning, two Bezier curve control points with optimized positions are determined in respective expected trajectories of two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

5) And reconstructing a Bezier track curve according to the updated control points to obtain tracks of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

Further, the set of sampling instants consisting of the plurality of sampling instants is T = {0, Δ T,2 Δ T,3 Δ T, \8230;, n Δ T }; n is the number of sampling points, and delta t is the sampling interval;

the sampling position point set on n sections of Bessel curves of the k unmanned aerial vehicle is as follows:

on the m-segment Bessel curve, the distance between every two sampling position points of the unmanned aerial vehicles i and j at the same moment is represented as follows:

in that

When s is _f Is a safe distance; selecting two control points (Q) on m sections of Bezier curves where sampling position points are located _m ,Q _m+1 }；

For control point { Q _m ,Q _m+1 Respectively moving a distance along the sampling position point difference vector dir; the updated control point gets an updated control point { Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula, λ _m 、λ _m+1 A pull-off coefficient of not less than 1;

according to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And (4) reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

The invention also discloses a track planning device for obstacle avoidance control of multiple unmanned aerial vehicles, which comprises: the system comprises a perception map generation module, a primary track planning module, a secondary track planning module and a track output module;

the perception map generation module is used for planning paths according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles and generating continuous expected tracks of the simulated unmanned aerial vehicles;

the primary track planning module is used for planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles and generating a continuous expected track of each simulated unmanned aerial vehicle;

the secondary track planning module is used for judging collision between the unmanned aerial vehicles according to the continuous expected track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles with the collision to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and the track output module is used for outputting the continuous expected track of each simulated unmanned aerial vehicle to the flight controller of each unmanned aerial vehicle for flight simulation.

Further, the primary trajectory planning module comprises a global A star path planning module, a path sampling module, a Bessel module and a trajectory generation module;

the global A star path planning module is used for carrying out primary planning based on global A star path planning on the obstacle avoidance perception map to generate a discrete path;

the path sampling module is used for carrying out downsampling according to the set path distance to generate sampled path points;

the Bezier module is used for establishing a parameterized trajectory curve equation related to time by taking the down-sampled path points as Bezier curve control points;

and the track generation module is used for generating the continuous expected track at each moment according to the parameterized track curve equation.

Furthermore, the secondary trajectory planning module comprises a time sampling module, a sampling position point calculating module, a collision judging module, a planning module and a trajectory output module;

the time sampling module is used for mutually transmitting track curves in the same time period among the multiple unmanned aerial vehicles, sampling the time period and acquiring multiple sampling moments;

the sampling position point calculation module is used for calculating sampling position points of all unmanned aerial vehicles at each sampling moment according to the continuous expected tracks of all the unmanned aerial vehicles;

the collision judgment module is used for judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, inputting the expected track of the unmanned aerial vehicle into a planning module;

the quadratic programming module is used for determining two Bezier curve control points with optimized positions in respective expected tracks of the two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

and the track output module is used for reconstructing a Bezier track curve according to the updated control points to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

Further, in the time sampling module, a set of sampling moments formed by a plurality of sampling moments is T = {0, Δ T,2 Δ T,3 Δ T, \8230, n Δ T }; n is the number of sampling points, and delta t is the sampling interval;

in the sampling position point calculation module, the sampling position point set on the n-section Bessel curve of the k unmanned aerial vehicle is as follows:

in the collision judging module, judgment is performed

And a safety distance s _f ；/>

The distance between two sampling position points of the unmanned aerial vehicles i and j at the same moment on the m-section Bessel curve is shown;

in that

In the quadratic programming module, two control points { Q) on the m-section Bezier curve where the sampling position points are located are selected _m ,Q _m+1 }; for is toControl Point { Q _m ,Q _m+1 Respectively moving a distance along the sampling position point difference vector dir; the updated control point gets an updated control point { Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula, λ _m 、λ _m+1 A pull-off coefficient of not less than 1;

in the track output module, according to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And (4) reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

The invention also discloses a multi-unmanned aerial vehicle flight obstacle avoidance control simulation system, which comprises a plurality of simulated unmanned aerial vehicles and a plurality of track planning devices in one-to-one correspondence with the simulated unmanned aerial vehicles;

the track planning device is the track planning device for obstacle avoidance control of the multiple unmanned aerial vehicles;

the simulation unmanned aerial vehicle respectively outputs the scene depth maps of the positions to the corresponding track planning devices, and receives the continuous expected track output by the track planning devices to carry out flight control simulation; realize unmanned aerial vehicle's obstacle avoidance flight.

Further, emulation unmanned aerial vehicle is four rotor unmanned aerial vehicle, and the flight controller who adopts is nonlinear simulation controller.

The invention can realize one of the following beneficial effects:

the track planning method, the device and the simulation system for the multi-unmanned aerial vehicle obstacle avoidance simulation control realize safe and efficient obstacle avoidance of the multi-unmanned aerial vehicle, and simultaneously ensure stability and smoothness of the multi-unmanned aerial vehicle in operation. The external obstacle avoidance of the multiple unmanned aerial vehicles is realized through global A-star planning, and the internal obstacle avoidance of the multiple unmanned aerial vehicles is realized through pulling a down-sampling track away from a collision point between the machines;

in addition, the invention provides a complete simulation platform capable of realizing obstacle avoidance perception, algorithm verification and track tracking response, and solves the problem that the influence of other module factors cannot be considered in multi-unmanned aerial vehicle intelligent control algorithm verification.

Drawings

The drawings, in which like reference numerals refer to like parts throughout, are for the purpose of illustrating particular embodiments only and are not to be considered limiting of the invention.

Fig. 1 is a flowchart of a trajectory planning method for obstacle avoidance simulation control of multiple unmanned aerial vehicles in the first embodiment

Fig. 2 is a schematic block diagram of a trajectory planning device for obstacle avoidance simulation control of multiple unmanned aerial vehicles in the second embodiment;

fig. 3 is a schematic block diagram of a multi-drone flight obstacle avoidance control simulation system in the third embodiment.

Detailed Description

The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form a part hereof, and which together with the embodiments of the invention serve to explain the principles of the invention.

Example one

One embodiment of the present invention discloses a trajectory planning method for obstacle avoidance simulation control of multiple unmanned aerial vehicles, as shown in fig. 1, including:

s1, generating an obstacle avoidance perception map representing feasible state space of each simulation unmanned aerial vehicle according to received position scene depth maps output by each simulation unmanned aerial vehicle;

s2, planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles to generate a continuous expected track of each simulated unmanned aerial vehicle;

s3, judging collision between the unmanned aerial vehicles according to the expected continuous track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles with the collision to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and S4, respectively outputting the continuous expected tracks of the simulated unmanned aerial vehicles to the flight controllers of the unmanned aerial vehicles for flight simulation.

Specifically, step S1 includes:

1) And based on the small hole imaging model of the depth camera, converting pixels in the depth image of the unmanned aerial vehicle depth camera into coordinates under a global coordinate system.

The internal parameter matrix of the depth camera of the unmanned aerial vehicle is expressed as:

f _x 、f _y 、c _x 、c _y is an internal parameter of the camera;

the external parameter matrix of the depth camera of the unmanned aerial vehicle is as follows:

r is a rotation matrix of the camera external parameter, and t is a translation vector; determined by the camera in the installation position of the drone.

Mapping the pixel plane to a camera coordinate system according to the camera's internal parameters:

wherein u and v are two-dimensional pixel coordinates of the image; x, Y and Z are coordinates of a camera coordinate system;

according to the external parameters of the camera, the coordinate under the camera coordinate system is transferred to the global coordinate system as follows:

wherein Xw, yw and Zw are global coordinate system coordinates;

two-dimensional pixel coordinates and depth information within the depth map may thus be converted to an obstacle space point cloud under a global coordinate system.

2) Constructing an obstacle perception map according to the obstacle space point cloud under the global coordinate system;

whether the obstacles are shot in the unit voxels corresponding to each pixel in the obstacle space point cloud of the continuously shot depth map is dynamic;

therefore, the obstacle perception map is composed of a dynamic array, double-precision floating point numbers are stored in the array, the observation probability of the obstacles of unit voxels is represented by the array unit, and the observation probability is updated by the obstacle space point cloud.

Specifically, the step S2 includes:

1) Performing primary planning on the obstacle avoidance perception map based on global A star path planning to generate a discrete path;

and searching a discrete safe path from the current position to the target position in the current obstacle perception map by using a global A star algorithm, wherein 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 within the map is represented as the sum of the cost from the start point and the cost from the end point:

f(n)＝g(n)+h(n)；

starting from the starting point, the A star algorithm sequentially searches for the adjacent node with the minimum total cost until the end point is found. The total cost of the nodes is stored in the priority queue structure, and the algorithm can be ensured to quickly extract the nodes with the minimum total cost. And finally obtaining a safe path with the minimum unit of map resolution, wherein the path is represented by path points of the voxel size of the continuous adjacent map units.

2) Performing down-sampling according to the set path distance to generate sampled path points;

and (4) downsampling the discrete path calculated by the global A star according to the proper path distance, and sampling the continuous adjacent path voxel points at proper distance intervals to form more dispersed path point data.

3) Taking the path points after down-sampling as Bezier curve control points, and establishing a parameterized track curve equation related to time;

establishing a parameterized trajectory profile equation with respect to time:

wherein, w _i The weight of the ith term is represented by,

the number of combinations is indicated.

4) And generating a continuous expected track at each moment according to a parameterized track curve equation.

The safety discrete path is represented as a continuous expected trajectory with respect to time by using a parameterized bezier curve, and the expected state of the drone at each moment can be obtained.

Specifically, the step S3 includes:

1) Mutually transmitting the track curves in the same time period among the multiple unmanned aerial vehicles, and performing down-sampling on the time period to obtain multiple sampling moments;

the set of sampling instants consisting of a plurality of sampling instants is T = {0, Δ T,2 Δ T,3 Δ T, \8230;, n Δ T }; n is the number of sampling points, and delta t is the sampling interval;

2) Calculating sampling position points of all unmanned aerial vehicles at each sampling moment aiming at the continuous expected tracks of all the unmanned aerial vehicles;

after sampling, the sampling position point set on n sections of Bezier curves of the k unmanned aerial vehicle is as follows:

3) Judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, entering the secondary trajectory planning of the step 4), otherwise, ending the step S3;

4) In secondary trajectory planning, two Bezier curve control points with optimized positions are determined in respective expected trajectories of two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

specifically, on the m-segment bezier curve, the distance between two sampling position points of the unmanned aerial vehicles i and j at the same moment is represented as:

in that

When s is _f Is a safe distance; in the secondary trajectory planning, two control points { Q ] on m sections of Bezier curves where sampling position points are located are selected _m ,Q _m+1 }；

For control point { Q _m ,Q _m+1 Respectively moving a distance along the sampling position point difference vector dir; the updated control point gets an updated control point { Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula, λ _m 、λ _m+1 The pull-off coefficient is more than or equal to 1.

5) And reconstructing a Bezier track curve according to the updated control points to obtain tracks of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

According to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

And respectively outputting the continuous expected track of each simulated unmanned aerial vehicle to a flight controller of each unmanned aerial vehicle for flight simulation.

In summary, the trajectory planning method for multi-unmanned aerial vehicle obstacle avoidance simulation control in the embodiment realizes safe and efficient obstacle avoidance of multiple unmanned aerial vehicles, and simultaneously ensures stability and smoothness of multiple unmanned aerial vehicles during operation. And the external obstacle avoidance of the multiple unmanned aerial vehicles is realized through global A-star planning, and the internal obstacle avoidance of the multiple unmanned aerial vehicles is realized through the pulling of down-sampling tracks from collision points between the machines.

Example two

One embodiment of the present invention discloses a trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles, as shown in fig. 2, including: the system comprises a perception map generation module, a primary track planning module, a secondary track planning module and a track output module;

the perception map generation module is used for generating an obstacle avoidance perception map representing the feasible state space of each simulation unmanned aerial vehicle according to the received scene depth map of the position output by each simulation unmanned aerial vehicle;

the primary track planning module is used for planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles and generating a continuous expected track of each simulated unmanned aerial vehicle;

the secondary track planning module is used for judging collision between the unmanned aerial vehicles according to the continuous expected track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles which collide to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and the track output module is used for outputting the continuous expected track of each simulated unmanned aerial vehicle to the flight controller of each unmanned aerial vehicle for flight simulation.

Specifically, the primary trajectory planning module comprises a global A star path planning module, a path sampling module, a Bessel module and a trajectory generation module;

the global A star path planning module is used for carrying out primary planning on the obstacle avoidance perception map based on global A star path planning to generate a discrete path;

the path sampling module is used for carrying out downsampling according to a set path distance to generate sampled path points;

the Bezier module is used for establishing a parameterized track curve equation related to time by taking the down-sampled path points as Bezier curve control points;

and the track generation module is used for generating the continuous expected track at each moment according to the parameterized track curve equation.

Specifically, the secondary trajectory planning module comprises a time sampling module, a sampling position point calculating module, a collision judging module, a planning module and a trajectory output module;

the time sampling module is used for mutually transmitting track curves in the same time period among the multiple unmanned aerial vehicles, sampling the time period and acquiring multiple sampling moments;

in the time sampling module, a sampling time set formed by a plurality of sampling times is T = {0, delta T,2 delta T,3 delta T, \8230;, n delta T }; n is the number of sampling points, and delta t is the sampling interval;

the sampling position point calculation module is used for calculating sampling position points of all unmanned aerial vehicles at each sampling moment according to the continuous expected tracks of all the unmanned aerial vehicles;

in the sampling position point calculation module, the sampling position point set on the n-section Bezier curve of the first k unmanned aerial vehicle is as follows:

the collision judgment module is used for judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, inputting the expected track of the unmanned aerial vehicle into a planning module;

in the collision judging module, judgment is performed

And a safety distance s _f ；/>

The distance between two sampling position points of the unmanned aerial vehicles i and j at the same moment on the m-section Bessel curve is shown;

the quadratic programming module is used for determining two Bezier curve control points with optimized positions in respective expected tracks of the two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

in that

In the quadratic programming module, two control points { Q) on m sections of Bezier curves where sampling position points are located are selected _m ,Q _m+1 }; for control point { Q _m ,Q _m+1 Along the sample position point difference vectordir respectively moves a distance; the updated control point gets the updated control point Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula of lambda _m 、λ _m+1 The pull-off coefficient is more than or equal to 1.

And the track output module is used for reconstructing a Bezier track curve according to the updated control points to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

In the track output module, according to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

The specific technical details and corresponding technical effects in this embodiment are the same as those in the previous embodiment. Please refer to the previous embodiment, which is not repeated herein.

EXAMPLE III

One embodiment of the invention discloses a multi-unmanned aerial vehicle flight obstacle avoidance control simulation system, which comprises a plurality of simulation unmanned aerial vehicles and a plurality of track planning devices in one-to-one correspondence with the simulation unmanned aerial vehicles, as shown in fig. 3;

the trajectory planning device is the trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles as described in the second embodiment;

the simulation unmanned aerial vehicle respectively outputs the scene depth maps of the positions to the corresponding track planning devices, and receives the continuous expected track output by the track planning devices to perform flight control simulation; realize unmanned aerial vehicle's obstacle avoidance flight.

Specifically, the simulation unmanned aerial vehicle is a quad-rotor unmanned aerial vehicle, and the adopted flight controller is a nonlinear simulation controller; the nonlinear simulation controller includes: the system comprises a track resolver, a position ring controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

the trajectory solver is used for converting the input simulation data of the unmanned aerial vehicle expected trajectory into the system expected state quantity at the current moment;

the position loop controller is used for carrying out position loop PID control according to the expected state quantity of the system and outputting a total error of the speed control;

the nonlinear angle converter is used for carrying out nonlinear SE (3) space angle conversion on the total error of the speed control to obtain a space expected rotation matrix;

the nonlinear attitude mapper is used for performing nonlinear attitude SO (3) space mapping on the current space expected rotation matrix and the measurement rotation matrix and outputting an attitude control error;

and the hybrid controller is used for controlling the rotating speed of the motor according to the attitude control error and the total bit speed control error and outputting the rotating speed of the motor to the simulation motor of the simulation unmanned aerial vehicle.

Specifically, the input signal of the trajectory solver is that the unmanned aerial vehicle expects the trajectory to be a polynomial equation with respect to time t:

wherein the content of the first and second substances,

indicating the position that the drone should reach at time t, C _3×5 A 5 th order coefficient matrix representing a three-dimensional polynomial, [ t [ ⁴ ,t ³ ,t ² ,t,1] ^T Representing a higher order argument with respect to time t.

Specifically, the track solver converts the expected track into the current time t _k System state quantity of (2):

wherein, P _des Represents the time t _k Desired position of

V _des Represents the time t _k Desired speed of

A _des Represents the time t _k Is desired acceleration->

Respectively at time t _k And taking 0-order derivation, 1-order derivation and 2-order derivation for the expected track respectively:

P _des ＝F ⁽⁰⁾ (t _k )

V _des ＝F ⁽¹⁾ (t _k )

A _des ＝F ⁽²⁾ (t _k )

at the same time, the trajectory solver will expect the yaw angle attitude ψ in view of the possibility of an obstacle appearing on the course of travel _des Adjusted to be forward along the trajectory:

[Δx,Δy] ^T ＝F ⁽⁰⁾ (t _k )-F ⁽⁰⁾ (t _k-1 )

thus, the trajectory solver solves the desired trajectory into the desired state quantity of the input flight control

Specifically, in the position loop controller, a desired state quantity output from a trajectory resolver

The input position ring PID is converted into the total error A of bit rate control _input 。

e _P ＝P _des -P _now

e _V ＝V _des -V _now

A _input ＝K _P e _P +K _V e _V +K _Vi ∫e _V +A _des +g

Wherein e is _P 、e _V An error value representing the desired position and the desired velocity, g represents the acceleration of gravity, A _input Representing the total error of the bit rate control; k _P Proportional gain, K, representing position error _V Proportional gain, K, representing speed error _Vi Integral gain, P, representing velocity error _now Representing the current moment t of the simulation unmanned aerial vehicle _k Position of (A), V _now Representing the current moment t of the simulation unmanned aerial vehicle _k The speed of (2).

Specifically, in the nonlinear angle converter, the total error A is controlled for the input bit rate _input Carrying out nonlinear SE (3) space angle conversion to obtain a space expected rotation matrix R _des . Spatial expectation rotation matrix R _des By [ x ] _B,des ,y _B,des ,z _B,des ]It is shown that, among others,

z _B,des vector direction representing acceleration PID value:

/>

y _B,des denotes z _B,des And vector [ cos psi _des ,sinψ _des ,0] ^T Normal vector of the component plane:

x _B,des denotes y _B,des And z _B,des Normal vector of the component plane:

x _B,des ＝y _B,des ×z _B,des

thereby, a desired rotation matrix R is obtained _des 。

In particular, the spatial expectation is rotated by a matrix R in a nonlinear attitude mapper _des And the measured rotation matrix R _B Performing nonlinear attitude SO (3) space mapping and outputting attitude control error [ e ] _p ,e _q ,e _r ] ^T ；

Measured rotation matrix R _B ＝[x _B y _B z _B ]From the orthogonal three-axis direction x in the measured coordinate system of the body _B ,y _B ,z _B And (6) performing characterization.

Will expect to rotate the matrix R _des And the current rotation matrix R _B By vee mapping to SO (3) space, attitude angle error e _R Expressed as:

the superscript "" represents the vee mapping of SO (3); r _B ＝[x _B y _B z _B ]From orthogonal three-axis directions x on the coordinate system of the body _B ,y _B ,z _B Carrying out characterization;

in the present embodiment, the error e is calculated by correcting the attitude angle _R Performs PID control to output an attitude control error e _p ,e _q ,e _r ] ^T ：

[e _p ,e _q ,e _r ] ^T ＝K _R e _R +K _Ri ∫e _R

K _R Proportional gain, K, representing attitude angle error _Ri Gesture displayIntegral gain of attitude angle error.

Preferably, the simulation unmanned aerial vehicle is at the current moment t _k Position P of _now Velocity V _now And the measured rotation matrix R _B ＝[x _B y _B z _B ]The pose measurement sensor for simulating the IMU, the GPS module, the barometer and the magnetometer included in the unmanned aerial vehicle is obtained by sensing natural environment simulation data including a simulation force field, an atmospheric field and a magnetic field in a virtual environment and measuring the data.

In addition, the current moment t of the simulation unmanned aerial vehicle _k Position P of _now Velocity V _now And the measured rotation matrix R _B ＝[x _B y _B z _B ]Or the pose data of the simulated unmanned aerial vehicle can be obtained by the current virtual physical engine.

Specifically, in the hybrid controller, an attitude control error [ e ] is output according to the nonlinear attitude mapper _p ,e _q ,e _r ] ^T And the bit rate control total error A output by the position loop controller _input And controlling the rotating speed of the motor to output the rotating speed omega of the motor to the simulation motor.

Wherein, the thrust generated by the ith simulation motor meets the requirement

The generated moment satisfies->

k _F Is the thrust coefficient; k is a radical of _M Is a moment coefficient;

for drones in the simulation system:

the four motor speeds of the four-rotor simulation unmanned aerial vehicle are as follows:

wherein L is the distance length from each motor of the quad-rotor unmanned aerial vehicle to the center of mass of the unmanned aerial vehicle; and m is the mass of the unmanned aerial vehicle.

The four motor rotating speeds are input into the simulation motor, so that the simulation motor simulates thrust F generated in the rotating process of the propeller by using the brushless direct current motor of the unmanned aerial vehicle _i Air resistance

Rotational moment M _i Air resistance moment->

Wherein k is _D Is the coefficient of air resistance, mu _D Is the air moment coefficient of resistance;

which represents the air flow velocity at the surface of the propeller,

dir _turn representing positive and negative of an emulated machineTurning direction, z _B Is a propeller plane vertical normal vector;

under the action of the four simulation motors, resultant force and resultant moment borne by the quad-rotor unmanned aerial vehicle are expressed as follows:

wherein m is the unmanned aerial vehicle mass, and g is acceleration of gravity.

The multi-unmanned aerial vehicle flight obstacle avoidance control simulation system also comprises a virtual simulation environment module; the virtual simulation environment module is in data communication with each simulation unmanned aerial vehicle; outputting the simulated virtual scene information to a simulated depth camera of the simulated unmanned aerial vehicle, so that the simulated depth camera can sense the scene information to obtain front-end sensing data comprising a scene depth map of the position; and outputting the simulated natural environment simulation data such as the simulated force field, the simulated atmospheric field, the simulated magnetic field and the like to the simulated IMU, the GPS module, the barometer and the magnetometer in the unmanned aerial vehicle, so that the simulated IMU, the GPS module, the barometer and the magnetometer can sense the pose information of the unmanned aerial vehicle according to the natural environment simulation data.

Specifically, in the environment building process of the virtual simulation environment module, the main steps include:

the method mainly comprises the following steps:

1) Modeling a simulated obstacle;

by modeling in a Gazebo simulation platform, obstacles with different shapes and sizes are constructed, and typical structures in various scenes such as forests, buildings, rooms and the like are simulated. In order to verify the adaptability of the obstacle avoidance capacity of the multiple unmanned aerial vehicles in different environments, obstacle scenes with different densities are constructed, and various test environments are provided for obstacle avoidance.

2) Virtual physical engine configuration;

the virtual physical engine simulates the stress of the unmanned aerial vehicle in the natural environment, applies external forces such as gravity, aerodynamic force, resistance and the like to the unmanned aerial vehicle, calculates through a dynamic model, and updates the current kinematic state of the unmanned aerial vehicle in each iteration step. Meanwhile, the physical engine applies a simulation force field, an atmospheric field, a magnetic field and the like to sensor modules in the flight control such as an IMU (inertial measurement Unit), a barometer and a magnetometer, and natural environment simulation data are provided for the flight control.

3) Compiling an analog sensor interface;

and under a simulation environment and a virtual physical engine, constructing an unmanned aerial vehicle model in the simulation unmanned aerial vehicle. Configuring an obstacle avoidance sensor, compiling a sensing data reading interface, wherein the type of the obstacle avoidance sensor is mainly a depth camera, constructing a depth camera model, and setting a lens field angle, an output image format and resolution, camera internal parameters and distortion parameters.

Utilizing a motor dynamics model in the simulated unmanned aerial vehicle: the thrust generated by a single motor is satisfied

The torque generated satisfies >>

The aerodynamic force and the moment that four rotor unmanned aerial vehicle motors produced are simulated, and unmanned aerial vehicle motor speed input interface is compiled.

4) And compiling a data communication interface.

Compiling a data communication interface, and communicating data of a sensor and a motor in the unmanned aerial vehicle model with a trajectory tracking flight control; on the basis of a robot operating system ROS, a UDP port of a flight control and simulation system is arranged, data of a simulation sensor IMU, a GPS, a magnetometer and a barometer are accessed into flight control, the rotating speed of a flight control output motor is transmitted into a simulation motor through a topic mechanism, and simulated aerodynamic force is generated.

In conclusion, the unmanned aerial vehicle simulation system of the embodiment overcomes the problem that the unmanned aerial vehicle lacks a flight controller and a simulation platform for trajectory tracking response test and verification in the field of autonomous control. The invention provides a bottom layer simulation platform capable of realizing high maneuvering flight tracks for researchers in the field of autonomous control of the unmanned aerial vehicle, and provides a simulation sensor interface and a flight control debugging interface, so that the research on upper-layer planning of the unmanned aerial vehicle is facilitated.

The above description is only for the preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention.

Claims (10)

1. A track planning method for multi-unmanned aerial vehicle obstacle avoidance simulation control is characterized by comprising the following steps:

s1, generating an obstacle avoidance perception map representing feasible state space of each simulation unmanned aerial vehicle according to received position scene depth maps output by each simulation unmanned aerial vehicle;

s2, planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles to generate a continuous expected track of each simulated unmanned aerial vehicle;

s3, judging collision between the unmanned aerial vehicles according to the expected continuous track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles with the collision to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and S4, respectively outputting the continuous expected tracks of the simulated unmanned aerial vehicles to the flight controllers of the unmanned aerial vehicles for flight simulation.

2. The trajectory planning method for multi-UAV obstacle avoidance simulation control according to claim 1, wherein,

the step S2 includes:

1) Performing primary planning on the obstacle avoidance perception map based on global A star path planning to generate a discrete path;

2) Performing down-sampling according to the set path distance to generate sampled path points;

3) Taking the down-sampled path points as Bezier curve control points, and establishing a parameterized trajectory curve equation related to time;

4) And generating a continuous expected track at each moment according to a parameterized track curve equation.

3. The trajectory planning method for multi-UAV obstacle avoidance simulation control according to claim 2,

the step S3 includes:

1) Mutually transmitting the trajectory curves in the same time period among the unmanned aerial vehicles, and performing down-sampling on the time period to obtain a plurality of sampling moments;

2) Calculating sampling position points of all unmanned aerial vehicles at each sampling moment aiming at the continuous expected tracks of all the unmanned aerial vehicles;

3) Judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, entering the secondary trajectory planning of the step 4), otherwise, ending the step S3;

4) In secondary trajectory planning, two Bezier curve control points with optimized positions are determined in respective expected trajectories of two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

5) And reconstructing a Bezier track curve according to the updated control points to obtain tracks of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

4. The trajectory planning method for multi-UAV obstacle avoidance simulation control according to claim 3,

the set of sampling instants consisting of a plurality of sampling instants is T = {0, Δ T,2 Δ T,3 Δ T, \8230;, n Δ T }; n is the number of sampling points, and delta t is the sampling interval;

the sampling position point set on n sections of Bessel curves of the k unmanned aerial vehicle is as follows:

on the m-segment Bessel curve, the distance between every two sampling position points of the unmanned aerial vehicles i and j at the same moment is represented as follows:

/>

in that

When s is _f Is a safe distance; selecting two control points (Q) on the m-section Bezier curve where the sampling position point is located _m ,Q _m+1 }；

For control point { Q _m ,Q _m+1 Respectively moving a distance along the sampling position point difference vector dir; the updated control point gets an updated control point { Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula of lambda _m 、λ _m+1 A pull-off coefficient of not less than 1;

according to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And (4) reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

5. A track planning device for obstacle avoidance control of multiple unmanned aerial vehicles; it is characterized by comprising: the system comprises a perception map generation module, a primary track planning module, a secondary track planning module and a track output module;

the perception map generation module is used for planning paths according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles and generating continuous expected tracks of the simulated unmanned aerial vehicles;

the primary track planning module is used for planning a path according to the obstacle avoidance perception maps of the simulated unmanned aerial vehicles and generating a continuous expected track of each simulated unmanned aerial vehicle;

the secondary track planning module is used for judging collision between the unmanned aerial vehicles according to the continuous expected track of each simulation robot in the same time period; performing secondary path planning on the two judged simulated unmanned aerial vehicles with the collision to generate a continuous expected track for avoiding collision of the unmanned aerial vehicles;

and the track output module is used for respectively outputting the continuous expected track of each simulated unmanned aerial vehicle to the flight controller of each unmanned aerial vehicle for flight simulation.

6. The trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles according to claim 5, wherein the primary trajectory planning module comprises a global A star path planning module, a path sampling module, a Bessel module, and a trajectory generation module;

the global A star path planning module is used for carrying out primary planning based on global A star path planning on the obstacle avoidance perception map to generate a discrete path;

the path sampling module is used for carrying out downsampling according to the set path distance to generate sampled path points;

the Bezier module is used for establishing a parameterized track curve equation related to time by taking the down-sampled path points as Bezier curve control points;

and the track generation module is used for generating the continuous expected track at each moment according to the parameterized track curve equation.

7. The trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles according to claim 5, wherein the secondary trajectory planning module comprises a time sampling module, a sampling position point calculating module, a collision judging module, a planning module and a trajectory output module;

the time sampling module is used for mutually transmitting the track curves in the same time period among the multiple unmanned aerial vehicles, sampling the time period and acquiring multiple sampling moments;

the sampling position point calculation module is used for calculating sampling position points of all unmanned aerial vehicles at each sampling moment according to the continuous expected tracks of all the unmanned aerial vehicles;

the collision judgment module is used for judging whether the distance between the position points of any two unmanned aerial vehicles at the same sampling moment is within a set safety distance; if yes, inputting the expected track of the unmanned aerial vehicle into a planning module;

the quadratic programming module is used for determining two Bezier curve control points with optimized positions in respective expected tracks of the two unmanned aerial vehicles; the two Bezier curve control points are two front and back Bezier curve control points which are closest to a position point within a safe distance; updating the two Sehr curve control points, and expanding a connecting line between the two Sehr curve control points to a direction away from an expected track of the opposite side so that the distance of position points on the connecting line at the same moment exceeds a safety distance;

and the track output module is used for reconstructing a Bezier track curve according to the updated control points to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

8. The trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles according to claim 5,

in the time sampling module, a sampling time set formed by a plurality of sampling times is T = {0, delta T,2 delta T,3 delta T, \8230;, n delta T }; n is the number of sampling points, and delta t is the sampling interval;

in the sampling position point calculation module, the sampling position point set on the n-section Bezier curve of the first k unmanned aerial vehicle is as follows:

in the collision judging module, judgment is performed

And a safety distance s _f ；/>

The distance between two sampling position points of the unmanned aerial vehicles i and j at the same moment on the m-section Bessel curve is shown;

in that

In the quadratic programming module, two control points { Q) on the m-section Bezier curve where the sampling position points are located are selected _m ,Q _m+1 }; for control point { Q _m ,Q _m+1 Respectively moving distances along the difference vector dir of the sampling position points; the updated control point gets an updated control point { Q _m,new ,Q _m+1,new }; wherein the content of the first and second substances,

in the formula, λ _m 、λ _m+1 A pull-off coefficient of not less than 1;

in the track output module, according to the updated control point { Q ₀ ,Q ₁ ,…,Q _m,new ,Q _m+1,new ,…,Q _n And (4) reconstructing a Bessel track curve to obtain a continuous expected track of the multiple unmanned aerial vehicles meeting the safety obstacle avoidance requirement.

9. A multi-unmanned aerial vehicle flight obstacle avoidance control simulation system is characterized by comprising a plurality of simulation unmanned aerial vehicles and a plurality of track planning devices which correspond to the simulation unmanned aerial vehicles one by one;

the trajectory planning device is the trajectory planning device for obstacle avoidance control of multiple unmanned aerial vehicles according to any one of claims 5 to 8;

the simulation unmanned aerial vehicle respectively outputs the scene depth maps of the positions to the corresponding track planning devices, and receives the continuous expected track output by the track planning devices to carry out flight control simulation; realize unmanned aerial vehicle's obstacle avoidance flight.

10. The multi-unmanned aerial vehicle flight obstacle avoidance control simulation system of claim 9, wherein the simulated unmanned aerial vehicle is a quad-rotor unmanned aerial vehicle and the employed flight controller is a nonlinear simulation controller.

Images