UAV nonlinear simulation controller, simulation UAV and simulation system

Abstract

The present invention relates to a nonlinear simulation controller for unmanned aerial vehicles, a simulated unmanned aerial vehicle and a simulation system; wherein the simulation controller comprises: a trajectory solver converts inputted simulation data of the desired trajectory of the unmanned aerial vehicle into the desired state quantity of the system at the current moment; a position loop controller performs position loop control to output the total error of position-speed control; a nonlinear angle converter performs spatial angle conversion on the total error of position-speed control to obtain the desired rotation matrix in space; a nonlinear attitude mapper performs nonlinear attitude mapping on the current desired rotation matrix in space and the measured rotation matrix to output the attitude control error; a hybrid controller performs motor speed control according to the attitude control error and the total error of position-speed control to output the motor speed to the simulated motor to simulate the stress condition of the unmanned aerial vehicle. The present invention provides a nonlinear simulation controller, a simulated unmanned aerial vehicle and a simulation system for realizing the bottom-level simulation of high-maneuverability flight trajectory in the field of autonomous control of unmanned aerial vehicles, so as to facilitate the research on the upper-level planning of unmanned aerial vehicles.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle intelligent control, and in particular relates to a nonlinear simulation controller for an unmanned aerial vehicle, a simulation unmanned aerial vehicle and a simulation system. Background Art

[0002] Conventional flight controllers such as PX4 have two controllers: position loop controller and attitude loop. The position loop controller controls the error of the drone to the desired position and resolves the error into the desired attitude. The attitude loop controls the error of the drone's angle and angular velocity and adjusts the current attitude to the desired attitude transmitted by the position loop controller. However, since the PX4 position loop controller only uses linear PID control for position deviation and velocity deviation, there is no constraint on the time to reach the desired state, and The desired posture is resolved in the state space, causing it to become a discrete quantity, which makes the controller unable to maneuver and track trajectories with time constraints.

[0003] In addition, this controller is designed with the consideration that most multi-rotor drones only need to adjust their posture in the hovering state. At this time, their angular deviation is usually small and due to the small weight of such drones, the rotation of the drone is usually linearized using Euler angles, thus ignoring the manifold structure of the drone in the configuration space. This leads to separate control of the rotation. On the one hand, such separate control cannot form a continuous and smooth trajectory in SE(3). On the other hand, the universal lock problem of Euler angles will cause attitude loss of control. In addition to the linearization of the rotation error calculation, the dual-loop conversion part is also linearized. Such control methods will limit the maneuverability of the drone. Complex drone maneuver control such as attitude tracking and path planning cannot be stably implemented without proper processing on SE(3).

[0004] At present, the UAV flight control simulation system (SITL) mainly focuses on the PID parameter debugging, flight stability test, and remote control capability test of mainstream flight controllers such as PX4, APM, or custom flight controllers. The main simulation and test object is the flight control algorithm itself, and the source of the flight control command is not considered. Autonomous control of UAVs requires that the UAV generates an appropriate planning trajectory based on environmental perception, and responds to the trajectory through the flight control system to achieve the entire autonomous control. This requires the simulation system platform to provide both a visual simulation world environment and a flight control simulator that can track trajectories. Simulation systems that can provide a visual simulation world environment include X-Plane, FlightGear, Gazebo, JMavSim, AirSim, etc., but the built-in flight controllers of these simulation systems are mostly SITLs of mainstream flight controllers such as PX4 or APM. These flight controllers lack the ability to track high-speed and maneuvering trajectories and cannot provide the flight control simulator required for intelligent autonomous control of UAVs. Summary of the invention

[0005] In view of the above analysis, the present invention aims to disclose a UAV nonlinear simulation controller, a simulated UAV and a simulation system, which are used to realize the simulation of the UAV's rapid tracking of the space-time trajectory, and provide a convenient and stable simulation verification platform for the UAV's intelligent autonomous control.

[0006] The invention discloses a nonlinear simulation controller for a UAV, which is used for simulation control of a quad-rotor UAV, and comprises a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

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

[0008] 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;

[0009] 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;

[0010] 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;

[0011] 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 simulation motor, so that the simulation motor simulates the thrust, air resistance, rotational torque and air resistance torque generated by the brushless DC motor of the drone during the propeller rotation process.

[0012] Furthermore, the desired trajectory of the drone input by the trajectory solver is F(t) = C 3×5 [t 4 ,t 3 ,t 2 ,t,1] 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 high-order independent variable with respect to time t;

[0013] The current time t output by the trajectory solver k The system state quantity

[0014] 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 Take the 0th, 1st, and 2nd derivatives of the expected trajectory:

[0015] P des =F (0) (t k );V des =F (1) (t k );A des =F (2) (t k );

[0016] Expected yaw attitude [Δx,Δy] T =F (0) (t k )-F (0) (t k-1 ).

[0017] Furthermore, the total position-speed control error output by the position loop controller is:

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

[0019] Among them, e P =P des -P now ;e V =V des -V now They represent the error values ​​of the expected position and expected velocity respectively; g represents the acceleration due to gravity; 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 k The position of V now Indicates the current time t of the simulated drone k speed.

[0020] Furthermore, the attitude control error output by the nonlinear attitude mapper is:

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

[0022] Among them, K R Represents the proportional gain of the attitude angle error, K Ri Indicates the integral gain of attitude angle error; e R The attitude angle error is obtained by mapping the current desired rotation matrix and the measured rotation matrix to the SO(3) space through vee.

[0023] Furthermore, the simulated drone at the current time t k Position P now , speed V now The measured rotation matrix is ​​obtained by measuring the posture measurement sensors including the simulated IMU, GPS module, barometer and magnetometer in the simulated drone through sensing the natural environment simulation data including the simulated force field, atmospheric field and magnetic field in the virtual environment.

[0024] Furthermore, the speeds of the four motors of the simulated quad-rotor drone are:

[0025]

[0026] Among them, k F is the motor thrust coefficient; k Mis the motor torque coefficient; L is the distance from each motor of the quadrotor drone to the center of mass of the drone; m is the mass of the drone.

[0027] The present invention also discloses a simulation UAV, comprising a first type of simulation sensor, a second type of simulation sensor, a nonlinear simulation controller and a simulation motor;

[0028] The first type of simulated sensors includes simulated laser radars and cameras, which are used to obtain front-end sensor data including RGB images, depth images and object point cloud data from scene information of the virtual environment in which the simulated drone is located, so as to provide input data for the trajectory planning of the drone;

[0029] The second type of simulation sensors include posture measurement sensors including simulated IMU, GPS module, barometer and magnetometer, which are used to perceive the posture information of the simulated drone from the natural environment simulation data of the virtual environment in which the simulated drone is located;

[0030] The nonlinear simulation controller adopts the nonlinear simulation controller as described above;

[0031] The nonlinear simulation controller performs trajectory planning based on the front-end sensing data of the first type of simulation sensor to obtain the desired trajectory and the posture information obtained by the second type of simulation sensor, performs nonlinear control, and outputs the motor speed to the simulation motor.

[0032] The simulation motor is used to simulate the thrust, air resistance, rotational torque and air resistance torque generated by the brushless DC motor of the drone during the rotation of the propeller according to the motor speed output by the nonlinear simulation controller; and obtain the resultant force and torque exerted on the simulated four-rotor drone in the simulation environment under the action of four simulation motors.

[0033] Furthermore, the simulation motor is used to simulate the thrust F generated by the brushless DC motor of the actual drone during the propeller rotation process according to the motor speed ω output by the nonlinear simulation controller. i , air resistance Rotation torque M i , air resistance moment

[0034]

[0035]

[0036]

[0037]

[0038] 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;

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

[0040]

[0041]

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

[0043] The present invention also discloses a UAV simulation system, including a simulated UAV, a trajectory planning module, and a virtual simulation environment module; wherein:

[0044] The simulated drone adopts the simulated drone as described above;

[0045] The trajectory planning module is connected to the simulated UAV, receives front-end sensor data including RGB images, depth images and object point cloud data from the first type of simulated sensor of the simulated UAV; performs trajectory planning to obtain the desired trajectory of the UAV and outputs it to the flight controller of the simulated UAV;

[0046] The virtual simulation environment module communicates data with the simulated drone; outputs the simulated virtual scene information to the first type of simulation sensor of the simulated drone, so that the first type of simulation sensor can perceive the scene information and obtain front-end sensor data including RGB images, depth images and object point cloud data; according to the resultant force and torque exerted on the four-rotor drone in the simulation environment by the simulated motor output, the simulated natural environment simulation data including the simulated force field, atmospheric field and magnetic field are output to the second type of simulation sensor of the simulated drone, so that the second type of simulation sensor can perceive the posture information of the drone according to the natural environment simulation data.

[0047] Furthermore, in the trajectory planning module, the desired trajectory of the UAV is generated based on the discrete path planning and continuous trajectory of the global A star;

[0048] The discrete path calculated by the global A star is downsampled with a set path distance; the downsampled path points are used as Bezier curve control points to establish a parameterized trajectory curve equation with respect to time; and the expected trajectory simulation data of the UAV at each moment is obtained according to the parameterized trajectory curve equation.

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

[0050] The present invention 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; it 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 analog sensor interfaces and flight control debugging interfaces to facilitate their research on upper-level planning of UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] 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.

[0052] Figure 1 This is a schematic block diagram of the composition of a nonlinear simulation controller for a UAV according to Embodiment 1 of the present invention;

[0053] Figure 2 This is a schematic block diagram of the composition of a simulated drone according to Embodiment 1 of the present invention;

[0054] Figure 3 The figure is a schematic block diagram of the composition of the UAV simulation system according to the third embodiment of the present invention. DETAILED DESCRIPTION

[0055] 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.

[0056] Embodiment 1

[0057] One embodiment of the present invention discloses a nonlinear simulation controller for a UAV, wherein the controller is used for simulation control of a quad-rotor UAV, such as Figure 1 As shown, it includes a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

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

[0059] 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;

[0060] 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;

[0061] 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;

[0062] 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 simulation motor, so that the simulation motor simulates the thrust, air resistance, rotational torque and air resistance torque generated by the brushless DC motor of the drone during the propeller rotation process.

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

[0064]

[0065] 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.

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

[0067]

[0068] 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:

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

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

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

[0072] 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:

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

[0074]

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

[0076] Among them, the input data of the trajectory solver can be the target position that the drone should reach, based on the discrete path planning and continuous trajectory of the global A star to generate the simulation data of the expected trajectory of the drone.

[0077] Specifically, 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, and 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:

[0078] f(n)=g(n)+h(n)

[0079] 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.

[0080] The discrete path calculated by the global A star is downsampled at an appropriate path distance, and the continuous adjacent path voxel points are sampled at appropriate distance intervals to form more dispersed path point data. Using the Bezier curve principle, the downsampled path points are used as Bezier curve control points to establish the parameterized trajectory curve equation about time:

[0081]

[0082]

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

[0084] By using parameterized Bezier curves to represent safe discrete paths as continuous trajectory equations about time, the simulation data of the expected trajectory of the UAV at each moment can be obtained.

[0085] 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 .

[0086] e P =P des -P now

[0087] e V =V des -V now

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

[0089] 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 k The position of V now Indicates the current time t of the simulated drone k speed.

[0090] 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,

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

[0092]

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

[0094]

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

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

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

[0098] 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 ;

[0099] 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 ,z B To characterize.

[0100] 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:

[0101]

[0102] 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;

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

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

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

[0106] 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 pose 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.

[0107] In addition, the simulated drone has a current time t k Position P now , speed V now 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.

[0108] 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.

[0109] 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;

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

[0111]

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

[0113]

[0114] 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.

[0115] The speeds of the four motors are input into the simulation motor so that the simulation motor can simulate the thrust, air resistance, rotational torque and air resistance torque generated by the drone using the brushless DC motor during the propeller rotation, as well as the resultant force and torque acting on the quad-rotor drone.

[0116] The force and torque output by the simulated motor are input into the ODE virtual physics engine for force field simulation to obtain gravity information, and dynamics simulation is performed to obtain state increments; then, according to the state increments and the current time t k The state of the simulated UAV is iteratively updated to obtain the state of the UAV at the next moment t k+1 Status S k+1,real =[P k+1,real ,V k+1,real ,A k+1,real ]; and this state information can be output as iterative data to the simulation controller as measurement data;

[0117] In addition, the ODE virtual physics engine also simulates the natural environment data of the simulated robot's location, including the magnetic field, atmospheric field and geographical location, based on the state data, so that the posture measurement sensors including the simulated IMU, GPS module, barometer and magnetometer included in the simulated drone can perceive the natural environment simulation data to obtain the position, speed and attitude data of the simulated drone, which are output as iterative data to the simulation controller as measurement data.

[0118] In summary, the nonlinear simulation controller of the UAV in this embodiment can realize the simulation of rapid tracking of the space-time trajectory; enable the UAV to quickly track the space-time trajectory that needs to be responded to, ensure that the state of the UAV at each moment reaches the state at the corresponding moment on the expected trajectory, and verify the rationality of the generated trajectory and the flight control response capability.

[0119] Embodiment 2

[0120] One embodiment of the present invention discloses a simulated drone, such as Figure 2As shown, it includes a first type of simulation sensor, a second type of simulation sensor, a nonlinear simulation controller and a simulation motor;

[0121] The first type of simulated sensors includes simulated laser radars and cameras; the first type of simulated sensors are used to meet the requirements of the UAV for environmental perception, obtain front-end sensor data including RGB images, depth images and object point cloud data, and provide input for trajectory planning;

[0122] The second type of simulated sensors include simulated IMU, GPS module, barometer and magnetometer, and other posture measurement sensors, through which the drone can perceive the posture information;

[0123] The nonlinear simulation controller is the nonlinear simulation controller described in Example 1. It performs trajectory planning based on the front-end sensor data of the first type of simulation sensor to obtain the desired trajectory and the posture information obtained by the second type of simulation sensor, performs nonlinear control, and outputs the motor speed to the simulation motor.

[0124] The simulation motor is used to simulate the thrust F generated by the actual UAV using the brushless DC motor during the propeller rotation process according to the motor speed ω output by the nonlinear simulation controller. i , air resistance Rotation torque M i , air resistance moment

[0125]

[0126]

[0127]

[0128]

[0129] 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;

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

[0131]

[0132]

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

[0134] The force and torque output by the simulated motor are input into the ODE virtual physics engine for force field simulation to obtain gravity information, and dynamics simulation is performed to obtain state increments; then, according to the state increments and the current time t k The state of the simulated UAV is iteratively updated to obtain the state of the UAV at the next moment t k+1 Status S k+1,real =[P k+1,real ,V k+1,real ,A k+1,real ]; and this status information can be output as iterative data to the simulation controller as measurement data.

[0135] In addition, the ODE virtual physics engine also simulates the natural environment data of the simulated robot's location, including the magnetic field, atmospheric field and geographical location, based on the state data, so that the second type of simulation sensor in the simulated drone can perceive the natural environment simulation data to obtain the position, speed and attitude data of the simulated drone, and output it as iterative data to the simulation controller as measurement data.

[0136] 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.

[0137] Embodiment 3

[0138] One embodiment of the present invention discloses a drone simulation system, such as Figure 3 As shown, it includes a simulated drone, a trajectory planning module, and a virtual simulation environment module; wherein,

[0139] The simulated drone adopts the simulated drone described in the second embodiment;

[0140] The trajectory planning module is connected to the simulated UAV, receives front-end sensor data of RGB images, depth images and object point cloud data from the first type of simulated sensor of the simulated UAV; performs trajectory planning to obtain the desired trajectory of the UAV and outputs it to the flight controller of the simulated UAV;

[0141] The virtual simulation environment module communicates data with the simulated drone; outputs the simulated virtual scene information to the first type of simulation sensor of the simulated drone, so that the first type of simulation sensor can perceive the scene information and obtain front-end sensor data including RGB images, depth images and object point cloud data; outputs the simulated force field, atmospheric field, magnetic field and other natural environment simulation data to the second type of simulation sensor of the simulated drone, so that the second type of simulation sensor can perceive the posture information of the drone according to the natural environment simulation data.

[0142] Specifically, in the trajectory planning module, the desired trajectory of the UAV is generated based on the discrete path planning and continuous trajectory of the global A star.

[0143] Specifically, 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, and 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:

[0144] f(n)=g(n)+h(n)

[0145] 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.

[0146] The discrete path calculated by the global A star is downsampled at an appropriate path distance, and the continuous adjacent path voxel points are sampled at appropriate distance intervals to form more dispersed path point data. Using the Bezier curve principle, the downsampled path points are used as Bezier curve control points to establish the parameterized trajectory curve equation about time:

[0147]

[0148]

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

[0150] By using parameterized Bezier curves to represent the safe discrete path as a continuous trajectory equation about time, the expected state of the UAV at each moment can be obtained.

[0151] The virtual simulation environment module is used to build a simulation scene environment using the Gazebo simulation platform and a virtual physics engine.

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

[0153] 1) Simulation obstacle modeling;

[0154] 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.

[0155] 2) Virtual physics engine configuration;

[0156] 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.

[0157] 3) Writing analog sensor interfaces;

[0158] Under the simulation environment and virtual physics engine, a drone model is constructed in the simulated drone. The simulated drone is the simulated drone described in the second embodiment;

[0159] 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.

[0160] 4) Writing of data communication interface.

[0161] 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.

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

[0163] 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.

[0164] 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 nonlinear simulation controller for a UAV, used for simulation control of a quad-rotor UAV, characterized in that: Includes trajectory solver, position loop controller, nonlinear angle converter, nonlinear attitude mapper and hybrid controller; The trajectory solver is used to convert the inputted UAV expected trajectory simulation data into the system expected state quantity at the current moment; 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; The nonlinear angle converter is used to perform nonlinear angle conversion on the total error of the position and speed control. SE 3. Transform the spatial angle to obtain the desired spatial rotation matrix; The nonlinear attitude mapper is used to transform the current space desired rotation matrix and the measured rotation matrix into a nonlinear attitude map. SO 3. Space mapping, outputting attitude control error; 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 simulation motor, so that the simulation motor simulates the thrust, air resistance, rotation torque and air resistance torque generated by the brushless DC motor of the drone during the propeller rotation process; The desired trajectory of the drone as input to the trajectory solver ; represents the 5th-order coefficient matrix of a three-dimensional polynomial, Indicates about time 's higher-order independent variables; The current time of the trajectory solver output The system state quantity ; in, Indicates time Expected location , Indicates time Expected speed , Indicates time The expected acceleration ; respectively at time Take the 0th, 1st, and 2nd derivatives of the expected trajectory: ； Expected yaw attitude ; ; The total error of position-speed control at the output of the position loop controller is: ； in, Represent the error values ​​of the expected position and expected velocity respectively; represents the acceleration due to gravity; represents the proportional gain of the position error, represents the proportional gain of the velocity error, represents the integral gain of the speed error, Indicates the current time of the simulated drone location, Indicates the current time of the simulated drone speed.

2. The UAV nonlinear simulation controller according to claim 1, characterized in that: Attitude control error output by the nonlinear attitude mapper: ； in, represents the proportional gain of the attitude angle error, Indicates the integral gain of attitude angle error; The current desired rotation matrix and the measured rotation matrix are passed Map to The attitude angle error obtained in space.

3. The UAV nonlinear simulation controller according to claim 2, characterized in that: The current moment of the simulated drone Location ,speed The measured rotation matrix is ​​obtained by measuring the posture measurement sensors including the simulated IMU, GPS module, barometer and magnetometer in the simulated drone through sensing the natural environment simulation data including the simulated force field, atmospheric field and magnetic field in the virtual environment.

4. The UAV nonlinear simulation controller according to claim 3, characterized in that: The speeds of the four motors of the simulated quad-rotor drone are: ； in, is the motor thrust coefficient; is the motor torque coefficient; is the distance from each motor of the quadrotor drone to the center of mass of the drone; For drone quality.

5. A simulated drone, characterized in that: It includes a first type of simulation sensor, a second type of simulation sensor, a nonlinear simulation controller and a simulation motor; The first type of simulated sensors includes simulated laser radars and cameras; Used to obtain front-end sensor data including RGB images, depth images and object point cloud data from the scene information of the virtual environment where the simulated drone is located, so as to provide input data for the trajectory planning of the drone; The second type of simulation sensors include posture measurement sensors including simulated IMU, GPS module, barometer and magnetometer, which are used to perceive the posture information of the simulated drone from the natural environment simulation data of the virtual environment in which the simulated drone is located; The nonlinear simulation controller adopts the nonlinear simulation controller as claimed in any one of claims 1 to 4; The nonlinear simulation controller performs trajectory planning based on the front-end sensor data of the first type of simulation sensor and the posture information obtained by the second type of simulation sensor, performs nonlinear control, and outputs the motor speed to the simulation motor; The simulation motor is used to simulate the thrust, air resistance, rotational torque and air resistance torque generated by the brushless DC motor of the drone during the propeller rotation process according to the motor speed output by the nonlinear simulation controller; The resultant force and torque of the simulated quad-rotor drone in the simulation environment are obtained under the action of four simulated motors; The simulated motor is used to output the motor speed according to the nonlinear simulation controller. Simulate the thrust generated by the actual drone using a brushless DC motor during the propeller rotation , air resistance , rotation torque , air resistance moment ; ； ； ； ； in, is the air resistance coefficient, is the air resistance moment coefficient; represents the airflow speed on the propeller surface, ; Indicates the forward and reverse direction of the simulated motor. is the normal vector perpendicular to the propeller plane; Under the action of four simulated motors, the resultant force and torque of the quad-rotor drone are expressed as: ； ； in, For drone quality, is the acceleration due to gravity.

6. A drone simulation system, characterized in that: It includes simulated drone, trajectory planning module and virtual simulation environment module; among them, The simulated drone adopts the simulated drone as claimed in claim 5; The trajectory planning module is connected to the simulated UAV, receives front-end sensor data including RGB images, depth images and object point cloud data from the first type of simulated sensor of the simulated UAV; performs trajectory planning to obtain the desired trajectory of the UAV and outputs it to the flight controller of the simulated UAV; The virtual simulation environment module communicates data with the simulated drone; outputs the simulated virtual scene information to the first type of simulation sensor of the simulated drone, so that the first type of simulation sensor can perceive the scene information and obtain front-end sensor data including RGB images, depth images and object point cloud data; according to the resultant force and torque exerted on the four-rotor drone in the simulation environment by the simulated motor output, the simulated natural environment simulation data including the simulated force field, atmospheric field and magnetic field are output to the second type of simulation sensor of the simulated drone, so that the second type of simulation sensor can perceive the posture information of the drone according to the natural environment simulation data.

7. The unmanned aerial vehicle simulation system according to claim 6, characterized in that: In the trajectory planning module, the desired trajectory of the UAV is generated based on the discrete path planning and continuous trajectory of the global A star; The discrete path calculated by the global A star is downsampled with a set path distance; the downsampled path points are used as Bezier curve control points to establish a parameterized trajectory curve equation with respect to time; and the expected trajectory simulation data of the UAV at each moment is obtained according to the parameterized trajectory curve equation.

Images