Simulation verification system for fast traversal and search control of UAV in complex space

Abstract

The present invention relates to a simulation verification system for fast traversal search control of complex space of unmanned aerial vehicles, comprising a space search module, a trajectory planning module and a simulation unmanned aerial vehicle; the space search module receives a dynamic scene depth map for space search; in the space search, the probability occupancy map is updated based on the dynamic scene depth map, a boundary set and the best observation point position corresponding to each boundary in the boundary set are found, and one is selected from multiple best observation point positions as the position of the next target point of the simulation unmanned aerial vehicle; the trajectory planning module is used to perform trajectory planning according to the searched target point to generate a continuous expected trajectory and output it to the simulation unmanned aerial vehicle; the simulation unmanned aerial vehicle is used to perform simulation flight according to the continuous expected trajectory, and verify the stability and safety of operation during the flight. The present invention provides a complete simulation platform that can realize environmental perception, algorithm verification, and maneuverable flight control.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) intelligent control, and in particular relates to a simulation verification system for fast traversal and search control of complex spaces of UAVs. Background Art

[0002] The main method for existing drones to search indoor spaces is the wall navigation method. This method uses distance sensors to sense the distance between the drone and the adjacent wall in the room, maintains a fixed distance between the drone and the wall, and enables the drone to traverse and search all the rooms in the room along the wall. However, this method is not suitable for indoor spaces with complex environments, because in this scenario, the distance sensor will not only sense the distance to the wall, but also sense the distance of other obstacles that are not part of the wall. This will cause great interference to the logic of the algorithm, resulting in the inability of the algorithm to run. In addition, even if there are only walls in the spatial environment, in the case of large-scale scenes, due to the limitation of the sensor's perception range, navigation along the wall may lead to a lack of exploration in the middle of the space, resulting in incomplete search. In addition, the wall navigation method controls the operation of the drone, mainly referring to the artificial potential field method to control the speed direction and size of the drone at a certain moment. This method cannot constrain the overall time and average speed of the drone's operation, and cannot guarantee the rapid completion of the drone's spatial search task.

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

[0004] In view of the above analysis, the present invention aims to disclose a simulation verification system for rapid traversal and search control of complex spaces of UAVs, which is used to solve the simulation verification problem of flight trajectory planning of UAVs in complex indoor environments.

[0005] The present invention discloses a simulation verification system for the rapid traversal search control of a complex space of an unmanned aerial vehicle, comprising a space search module, a trajectory planning module and a simulation unmanned aerial vehicle;

[0006] The spatial search module is connected to the simulated UAV and is used to perform spatial search according to the dynamic scene depth map output frame by frame by the received simulated UAV during simulated flight; in the spatial search, the probability occupancy map is updated based on the dynamic scene depth map, a boundary set for distinguishing unknown and known areas of the map and the best observation point position corresponding to each boundary in the boundary set are found, and one of the multiple best observation point positions is selected as the position of the next target point of the simulated UAV;

[0007] The trajectory planning module is connected to the space search module and is used to perform trajectory planning according to the searched target point and generate a continuous expected trajectory and output it to the simulated UAV;

[0008] The simulated UAV is used to perform simulated flight in a virtual simulation environment according to a continuous expected trajectory, and to verify the smoothness and safety of operation during the flight.

[0009] Further, the spatial search module includes a coordinate conversion module, a probability occupancy map update module, a boundary generation module, an observation point calculation module and a target point generation module;

[0010] The coordinate conversion module is used to convert the scene depth map output frame by frame by the simulated drone into a three-dimensional obstacle space point cloud;

[0011] The probability occupancy map update module is used to establish a probability occupancy map composed of a dynamic array, and update the current state of each unit element in the probability occupancy map according to the three-dimensional obstacle space point cloud acquired frame by frame, and the current state includes three states: unknown, unoccupied and occupied;

[0012] The boundary generation module is used to search the current state of each element in the probability occupation map to find a boundary set for distinguishing unknown and known areas of the map;

[0013] The observation point calculation module is used to determine the observation point position that maximizes the observation efficiency when observing each boundary in the boundary set;

[0014] The target point generation module is used to perform global cost sorting on the observation point positions of each boundary and select an observation point position as the next target point of path planning.

[0015] Further, the probabilistic occupancy map updating module includes a map initialization module, an updating module and a state determination module;

[0016] The map initialization module is used to initialize the probability occupancy map and assign an initial value to each element in the map array used to store the observed state values ​​of each unit in the three-dimensional space; the initial value is the probability value of the unknown state;

[0017] The updating module is used to dynamically update the observed state value of the probability occupation map according to the three-dimensional obstacle point cloud obtained frame by frame; according to the three-dimensional obstacle point cloud of the current frame, if a unit element is judged to be in an occupied state, the observed state value of the unit element is updated according to the probability of the occupied state; if a unit element is judged to be in an unoccupied state, the observed state value of the unit element is updated according to the probability of the unoccupied state;

[0018] The state determination module is used to determine the current state of each unit element in the probability occupation map according to the current observed state value.

[0019] Furthermore, in the updating module, the observed state value of the map element is updated as follows:

[0020] map[x i ][y i ][z i ]=S old +l occ ;

[0021] map[x k ][y k ][z k ]=S old +l free ;

[0022]

[0023]

[0024] Among them, map[x i ][y i ][z i ] is the updated value of the map element currently observed to be occupied; map[x k ][y k ][z k ] is the updated value of the map element that is currently observed to be unoccupied; S old Indicates the observed state value of the map element last observed; l occ The current observation is the increment of the occupied state value, l free The current observation is the increment of the state value that is not occupied; p hit is the pre-set probability of observing an occupied state, p miss is the pre-set probability of observing an unoccupied state, and p hit +p miss =1, p hit >p miss .

[0025] Furthermore, in the observation point calculation module, the best observation point position VP of the boundary Fro is determined as:

[0026] VP=Mid+s·dir normal ;

[0027] in, and To start from the mean point of the boundary Fro, perform a traversal search to obtain the two spatial points farthest from each other at both ends of the boundary Fro; dir normal is the center normal of the boundary Fro; s is the distance between the best observation point VP on the center normal and the center point Mid.

[0028] Furthermore, in the target point generation module, a total observation cost consisting of a distance cost, a speed cost and a speed change cost is established for the observation point positions of each boundary and sorted, and the observation point position with the smallest total observation cost is used as the next target point of the drone.

[0029] Furthermore, the total observation cost is:

[0030] cost sum =cost reach_dist +cost vel_normal +cost vel_change :

[0031] Among them, cost reach_dist is the distance cost, which is the path length from the current position of the simulated drone to the observation point through the global A-star planning;

[0032] cost vel_normal is the speed cost, which is the angle between the vector direction of the path point at the end of the path planned by the global A star and the normal vector of the boundary center;

[0033] cost vel_change is the velocity change cost, which is the angle between the current velocity vector direction and the normal vector of the boundary center.

[0034] Furthermore, the simulated UAV is a simulated quad-rotor UAV, comprising a first type of simulated sensor, a second type of simulated sensor, a nonlinear simulated controller and four simulated motors;

[0035] The first type of simulated sensor includes a simulated depth camera, which is used to obtain front-end sensor data including dynamic scenes from scene information of the virtual environment in which the simulated drone is located, as input data for space search and trajectory planning;

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

[0037] The nonlinear simulation controller is used to perform spatial search and trajectory planning based on the front-end sensor data of the first type of simulation sensor to obtain the expected trajectory and the simulated drone posture information sensed by the second type of simulation sensor, perform nonlinear control, and output the motor speed corresponding to the simulated motor;

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

[0039] Further, the nonlinear simulation controller includes a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

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

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

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

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

[0044] The hybrid controller is used to control the motor speed and output the motor speed to the simulation motor according to the posture control error and the total position-speed control error.

[0045] Furthermore, it also includes a virtual simulation environment module; the virtual simulation environment module communicates data with the simulated drone; the simulated virtual scene information is output 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 dynamic scenes; 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 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.

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

[0047] The simulation system of the present invention overcomes the problem of the lack of a fast, complete, concise and clear complex space traversal search strategy and simulation verification environment in the field of intelligent control of UAVs; it provides researchers in this field with a complete simulation platform that can realize environmental perception, algorithm verification, and maneuverable flight control, and solves the problem that the influence of other module factors cannot be considered in the verification of UAV space search algorithms.

[0048] In addition, the simulation and verification space search and trajectory planning method in the present invention quantifies the unknown area in the space by constructing the boundary and its optimal observation point, and determines the spatial position where the search efficiency is maximized, thereby solving the problems of low efficiency, incomplete search, and complex logic of existing indoor search technology in complex spaces; and then determines the global optimal observation point as the next target point by calculating the observation point cost, thereby solving the problem of slow search speed inside complex spaces. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0050] Figure 1 A schematic block diagram showing the connection components of a simulation verification system in an embodiment of the present invention;

[0051] Figure 2 A schematic block diagram showing the connection of a spatial search module in an embodiment of the present invention;

[0052] Figure 3 A schematic block diagram showing the connection of the trajectory planning module in an embodiment of the present invention;

[0053] Figure 4 The figure is a schematic diagram of the composition and internal and external connections of the simulated drone in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0055] The embodiment of the present invention discloses a simulation verification system for fast traversal search control of a complex space of an unmanned aerial vehicle, such as Figure 1 As shown, it includes a space search module, a trajectory planning module, a simulated UAV and a virtual simulation environment module;

[0056] The spatial search module is connected to the simulated UAV and is used to perform spatial search according to the dynamic scene depth map output frame by frame by the received simulated UAV during simulated flight; in the spatial search, the probability occupancy map is updated based on the dynamic scene depth map, a boundary set for distinguishing unknown and known areas of the map and the best observation point position corresponding to each boundary in the boundary set are found, and one of the multiple best observation point positions is selected as the next target point position of the simulated UAV simulated flight;

[0057] The trajectory planning module is connected to the space search module and is used to perform trajectory planning according to the position of the next target point to be searched and generate a continuous expected trajectory to output to the simulated UAV;

[0058] The simulated UAV is used to perform simulated flight in a virtual simulation environment according to the continuous expected trajectory to verify the smoothness and safety of the operation during the flight;

[0059] The virtual simulation environment module communicates data with the simulated drone; outputs the simulated virtual scene information to the simulated depth camera of the simulated drone, so that the simulated depth camera can perceive the scene information and obtain front-end sensor data including the depth map of the scene at the location; outputs the simulated force field, atmospheric field, magnetic field and other natural environment simulation data to the simulated IMU, GPS module, barometer and magnetometer in the simulated drone, so that the simulated IMU, GPS module, barometer and magnetometer can perceive the posture information of the drone according to the natural environment simulation data.

[0060] Specifically, Figure 2 As shown, the spatial search module includes a coordinate conversion module, a probability occupancy map update module, a boundary generation module, an observation point calculation module and a target point generation module;

[0061] The coordinate conversion module is used to convert the scene depth map output frame by frame by the simulated drone into a three-dimensional obstacle space point cloud;

[0062] The probability occupancy map update module is used to establish a probability occupancy map composed of a dynamic array, and update the current state of each unit element in the probability occupancy map according to the three-dimensional obstacle space point cloud acquired frame by frame, and the current state includes three states: unknown, unoccupied and occupied;

[0063] The boundary generation module is used to search the current state of each element in the probability occupation map to find a boundary set for distinguishing unknown and known areas of the map;

[0064] The observation point calculation module is used to determine the observation point position that maximizes the observation efficiency when observing each boundary in the boundary set;

[0065] The target point generation module is used to perform global cost sorting on the observation point positions of each boundary and select an observation point position as the next target point of path planning.

[0066] More specifically, in the coordinate conversion module, based on the depth camera pinhole imaging model, the pixels in the depth map taken by the drone depth camera are converted into coordinates in the global coordinate system, so that the depth information corresponding to the pixel coordinates is converted into a three-dimensional obstacle space point cloud in the global coordinate system.

[0067] Among them, the intrinsic parameter matrix of the drone depth camera is expressed as:

[0068]

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

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

[0071]

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

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

[0074]

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

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

[0077]

[0078] Among them, X w , Y w , Z w is the three-dimensional coordinate in the global coordinate system;

[0079] Thus, the depth information corresponding to the two-dimensional pixel coordinates in the depth map can be converted into a three-dimensional obstacle space point cloud in the global coordinate system.

[0080] More specifically, the probabilistic occupancy map update module includes a map initialization module, an update module and a state determination module;

[0081] The map initialization module is used to initialize the probability occupancy map and assign an initial value to each element in the map array used to store the observed state values ​​of each unit in the three-dimensional space; the initial value is the probability value of the unknown state;

[0082] In the map initialization module, according to the maximum size x of the indoor map in the global coordinate system lenght ,y lenght 、z lenght And resolution res, build a map array representing the state:

[0083] map[x size ][y size ][z size ];

[0084] in,

[0085]

[0086]

[0087]

[0088] Each element in the map array stores the observed state value of the corresponding three-dimensional space unit represented by the element.

[0089] During initialization, each element in the array is initialized by the initial value S init Assignment, indicating the initial state value of the space where the element is located: map[x size ][y size ][z size ]={S init}.

[0090] The updating module is used to dynamically update the observed state value of the probability occupation map according to the three-dimensional obstacle point cloud obtained frame by frame; according to the three-dimensional obstacle point cloud of the current frame, if a unit element is judged to be in an occupied state, the observed state value of the unit element is updated according to the probability of the occupied state; if a unit element is judged to be in an unoccupied state, the observed state value of the unit element is updated according to the probability of the unoccupied state;

[0091] In the updating module, specifically, when a frame of obstacle point cloud is acquired from the map, the occupied points in the point cloud data are represented as the point set P of the obstacle in the three-dimensional space. obs ={pt0, pt1, ..., pt i , ..., pt n}, P obs Each obstacle in the point set represents an obstacle point in the global coordinate system The corresponding elements and collections in the map array are:

[0092] map[x i ][y i ][z i ]∈Set occ ;

[0093]

[0094] Among them, Set occ is the set of occupied elements, Represents the coordinates of the map starting element map[0][0][0] in the three-dimensional coordinate system.

[0095] Specifically, when a frame of obstacle point cloud is acquired from the map, the unoccupied points in the point cloud data are the spatial points in the global coordinate system through which the line of sight between the drone's own position and the obstacle point cloud passes;

[0096] The unoccupied points obtained The corresponding array elements of the collection on the map are:

[0097] {map[x k，1 ][y k，1 ][z k，1 ],map[x k，2 ][y k，2 ][z k，2 ], ..., map[x k，m ][y k，m ][z k，m ]}∈Set free ;

[0098] Among them, corresponding to a certain frame of obstacle point cloud, the probability of occupying the occupied element set in the map is Set occ In the map element map[x i ][y i ][z i ] is the element that is currently observed to be occupied; the set of unoccupied elements Set free In the map element map[x k ][y k ][z k ] is the element that is observed to be in an unoccupied state for the current time;

[0099] Then, the update of the observed state value of the map element is:

[0100] map[x i ][y i ][z i ]=S old +l occ ;

[0101] map[x k ][y k ][z k ]=S old +l free ;

[0102]

[0103]

[0104] Among them, S old Indicates the observed state value of the previous observation in the map element; l occ The current observation is the increment of the occupied state value, l free The current observation is the increment of the state value that is not occupied; p hit is the pre-set probability of observing an occupied state, p miss is the pre-set probability of observing an unoccupied state, and p hit +p miss =1, p hit >p miss .

[0105] The state determination module is used to determine the current state of each unit element in the probability occupation map according to the current observed state value.

[0106] In the state determination module, the current state of each element is determined according to the currently observed state value of any element in the map array:

[0107]

[0108] Where map[x][y][z] is the observed state value of any element in the map array, Threshold is the boundary threshold between the unoccupied state and the occupied state, which can be pre-set based on experience, S init The initial value of the elements in the map array.

[0109] More specifically, the boundary generation module searches for map elements that are in an unknown state and an unoccupied state in the map with a probability of occupying the map as the boundary elements map[x j ][y j ][z j ];

[0110] The map[x j ][y j ][z j ] is in an unoccupied state, and its adjacent elements are in an unknown state. The set of three-dimensional space points represented by such map elements constitutes the boundary There may be multiple Fro in a map, represented as a set of boundaries Fro∈Set Fro .

[0111] For the boundary Fro, it is necessary to select a reasonable observation point so that the drone can observe the boundary at this point to maximize the observation efficiency. Considering that in general, when observing an unknown area, observing inward along the normal direction of the center of the unknown area can maximize the observation efficiency.

[0112] More specifically, the process of determining the observation point position that maximizes the observation efficiency of each boundary Fro in the observation point calculation module includes:

[0113] 1) Obtaining the mean point of the boundary Fro;

[0114] Specifically, the boundary Fro mean point is expressed as:

[0115]

[0116] Among them, num Fro Indicates the number of spatial points contained in the boundary Fro, It represents the sum of all points on the boundary.

[0117] 2) Through the established objective function, a traversal search is performed starting from the mean point of the boundary Fro to obtain the positions of the two spatial points farthest from each other at the two ends of the boundary Fro;

[0118] The objective function is:

[0119]

[0120]

[0121]

[0122] Among them, dir PCA is the principal component direction of all spatial points in Fro.

[0123] The point that satisfies the objective function will be found in the boundary Fro by searching and Determined as the spatial point with the farthest distance between the two ends within the boundary Fro.

[0124] 3) Determine the center normal of the boundary Fro according to the positions of the two spatial points farthest from each other at the two ends of the boundary Fro;

[0125] The center normal direction is expressed as:

[0126]

[0127]

[0128] Among them, lenght represents the distance between the spatial points at both ends of the boundary, z w is the z-axis in the global coordinate system.

[0129] 4) The best observation point position is determined based on the center point position calculated from the two spatial points at both ends of the boundary and the perception field of view FOV of the depth camera.

[0130] The center point position calculated from the spatial points at both ends of the boundary is:

[0131]

[0132]

[0133] Among them, s represents the distance between the best observation point VP and the center point Mid on the center normal line, so the best observation point VP can be expressed as:

[0134] VP=Mid+s·dir normal .

[0135] In this embodiment, there are multiple boundaries Fro in a map, and the best observation point position corresponding to each boundary Fro can be obtained by searching each boundary Fro using the above method.

[0136] More specifically, in the target point generation module, the total observation cost consisting of distance cost, speed cost and speed change cost is established for the observation point positions of each boundary and sorted, and the observation point position with the smallest total observation cost is used as the next target point of the drone.

[0137] Among them, the observation costs for global cost sorting of the observation point positions of each boundary include:

[0138] 1) Distance cost: Consider the distance from the current position of the drone to the best observation point on each boundary. This distance is obtained by planning the path length from the current position of the drone to the point through the global A star:

[0139] cost reach_dist =length(A * path);

[0140] 2) Speed ​​cost: Consider the overlap between the speed at the end of the drone path and the center normal of the optimal observation point. This value is the angle between the path point vector direction at the end of the path planned by the global A star and the center normal vector of the boundary:

[0141] cost vel_normal =coS -1 (dir normal dir path_end );

[0142] 3) Speed ​​change cost: Consider the change in the current speed of the drone to reach this point. This value is the angle between the current speed vector direction of the drone and the normal vector of the boundary center:

[0143] cost vel_change =cos -1 (dir vel_current dir path_end );

[0144] The total observation cost of the observation point obtained by combining the above three observation costs is:

[0145] cost sum =cost reach_dist +cost vel_normal +cost vel_change .

[0146] By sorting the costs of all boundary observation points, the observation point with the minimum cost can be selected for observation. This observation point will be used as the target point of the UAV, and the continuous trajectory curve to reach this point will be solved through the trajectory planning method.

[0147] like Figure 3 As shown, the trajectory planning module includes: a discrete path planning module and a continuous trajectory generation module;

[0148] The discrete path planning module is used to perform global A-star path planning according to the target point position output by the spatial search module, and output discrete path nodes;

[0149] Specifically, the global A-star algorithm is used to find a discrete safe path from the current position to the target point position in the current map, and the minimum node of the path is the minimum volume unit of the map.

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

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

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

[0153] The continuous trajectory generation module is used to generate a continuous trajectory according to the discrete path nodes output by the discrete path planning module, and output a continuous expected trajectory to the simulated UAV.

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

[0155]

[0156]

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

[0158] By using parameterized Bezier curves to represent the safe discrete path as a continuous trajectory equation about time, the desired position of the drone at each moment can be obtained to generate a continuous desired trajectory. The continuous desired trajectory is output to the flight controller of the simulated drone, so that the simulated drone performs simulated flight along the desired trajectory.

[0159] Specifically, Figure 4 As shown, the simulated UAV is a simulated quad-rotor UAV, including a first type of simulated sensor, a second type of simulated sensor, a nonlinear simulated controller and four simulated motors;

[0160] The first type of simulated sensor includes a simulated depth camera, which is used to obtain front-end sensor data including dynamic scenes from scene information of the virtual environment in which the simulated drone is located, as input data for space search and trajectory planning;

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

[0162] The nonlinear simulation controller is used to perform spatial search and trajectory planning based on the front-end sensor data of the first type of simulation sensor to obtain the expected trajectory and the simulated drone posture information sensed by the second type of simulation sensor, perform nonlinear control, and output the motor speed corresponding to the simulated motor;

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

[0164] Specifically, the nonlinear simulation controller includes a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a hybrid controller;

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

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

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

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

[0169] The hybrid controller is used to control the motor speed and output the motor speed to the simulation motor according to the posture control error and the total position-speed control error.

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

[0171]

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

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

[0174]

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

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

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

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

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

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

[0181]

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

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

[0184] e P =P des -P now

[0185] e V =V des -V now

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

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

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

[0189] z B，des The vector direction representing the acceleration PID value:

[0190]

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

[0192]

[0193] x B，des Represents y B，des With z B，des The normal vectors that make up the plane:

[0194] x B，des =y B，des × B，des

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

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

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

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

[0199]

[0200] The superscript “V” 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;

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

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

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

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

[0205] In addition, the simulated UAV 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.

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

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

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

[0209]

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

[0211]

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

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

[0214]

[0215]

[0216]

[0217]

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

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

[0220]

[0221]

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

[0223] Specifically, the virtual simulation environment module communicates data with the simulated UAV; outputs the simulated virtual scene information to the first type of simulation sensor of the simulated UAV, so that the first type of simulation sensor can perceive the scene information and obtain front-end sensor data including dynamic scenes; according to the resultant force and torque exerted on the four-rotor UAV in the simulated environment by the simulated motor output, the simulated natural environment simulation data including simulated force field, atmospheric field and magnetic field are output to the second type of simulation sensor of the simulated UAV, so that the second type of simulation sensor can perceive the position information of the UAV according to the natural environment simulation data.

[0224] Preferably, the Gazebo simulation platform and the virtual physics engine are used to build a simulation scene environment in the virtual simulation environment module.

[0225] Optionally, during the construction of the simulation scene environment, the main steps include:

[0226] 1) Simulation obstacle modeling;

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

[0228] 2) Virtual physics engine configuration;

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

[0230] 3) Writing analog sensor interfaces;

[0231] Build a drone model in the simulated drone under the simulation environment and virtual physics engine.

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

[0233] 4) Writing of data communication interface.

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

[0235] In summary, the UAV simulation system of this embodiment overcomes the problem of the lack of a fast, complete, concise and clear complex space traversal search strategy and simulation verification environment in the field of intelligent control of UAVs; it provides researchers in this field with a complete simulation platform that can realize environmental perception, algorithm verification, and maneuverable flight control, and solves the problem that the influence of other module factors cannot be considered in the verification of UAV space search algorithms.

[0236] In addition, the simulation verification space search and trajectory planning method in the present invention solves the problems of low efficiency, incomplete search, and complex logic of existing indoor search technology in complex spaces by constructing boundaries and their optimal observation points, quantifying unknown areas in space, and determining the spatial position that maximizes search efficiency; and then determining the global optimal observation point as the next target point by calculating the observation point cost, solving the problem of slow search speed inside complex spaces. The above is only a preferred specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of 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 simulation verification system for the rapid traversal and search control of a UAV in a complex space, characterized in that: Includes space search module, trajectory planning module and simulated drone; The spatial search module is connected to the simulated UAV and is used to perform spatial search according to the dynamic scene depth map output frame by frame by the received simulated UAV during simulated flight; in the spatial search, the probability occupancy map is updated based on the dynamic scene depth map, a boundary set for distinguishing unknown and known areas of the map and the best observation point position corresponding to each boundary in the boundary set are found, and one of the multiple best observation point positions is selected as the position of the next target point of the simulated UAV; The trajectory planning module is connected to the space search module and is used to perform trajectory planning according to the searched target point to generate a continuous expected trajectory and output it to the simulated UAV; The simulated UAV is used to perform simulated flight in a virtual simulation environment according to the continuous expected trajectory to verify the smoothness and safety of the operation during the flight; The spatial search module includes a coordinate conversion module, a probability occupancy map update module, a boundary generation module, an observation point calculation module and a target point generation module; The coordinate conversion module is used to convert the scene depth map output frame by frame by the simulated drone into a three-dimensional obstacle space point cloud; The probability occupancy map update module is used to establish a probability occupancy map composed of a dynamic array, and update the current state of each unit element in the probability occupancy map according to the three-dimensional obstacle space point cloud acquired frame by frame, and the current state includes three states: unknown, unoccupied and occupied; The boundary generation module is used to search the current state of each element in the probability occupation map to find a boundary set for distinguishing unknown and known areas of the map; The observation point calculation module is used to determine the observation point position that maximizes the observation efficiency when observing each boundary in the boundary set; The target point generation module is used to perform global cost sorting on the observation point positions of each boundary and select an observation point position as the next target point of path planning; The probability occupancy map update module includes a map initialization module, an update module and a state determination module; The map initialization module is used to initialize the probability occupancy map and assign an initial value to each element in the map array used to store the observed state values ​​of each unit in the three-dimensional space; the initial value is the probability value of the unknown state; The updating module is used to dynamically update the observed state value of the probability occupation map according to the three-dimensional obstacle point cloud obtained frame by frame; according to the three-dimensional obstacle point cloud of the current frame, if a unit element is judged to be in an occupied state, the observed state value of the unit element is updated according to the probability of the occupied state; If a unit element is judged to be in an unoccupied state, the observed state value of the unit element is updated according to the probability of the unoccupied state; The state determination module is used to determine the current state of each unit element in the probability occupation map according to the current observed state value; In the update module, the observed state value of the map element is updated as follows: ； ； ； ； in, The updated value of the map element currently observed to be occupied; The updated value of the map element that is currently observed to be unoccupied; Indicates the observed state value of the map element last observed; When the current observation is the occupied state value increment, The current observation is the increment of the state value that is not occupied; is the pre-set probability of observing an occupied state, is the pre-set probability of observing an unoccupied state, and .

2. The simulation verification system according to claim 1, characterized in that: In the observation point calculation module, the boundary determined The best observation point location for: ； in, ; and From the border The mean point starts to traverse and search to get the boundary The two spatial points farthest apart from each other; For the border Center Normal; is the best observation point position on the center normal Distance from center distance.

3. The simulation verification system according to claim 1, characterized in that: In the target point generation module, the total observation cost consisting of distance cost, speed cost and speed change cost is established for the observation point positions of each boundary and sorted, and the observation point position with the smallest total observation cost is used as the next target point of the drone.

4. The simulation verification system according to claim 3, characterized in that: The total observation cost is: ； in, is the distance cost, which is the path length from the current position of the simulated drone to the observation point through the global A-star planning; is the speed cost, which is the angle between the vector direction of the path point at the end of the path planned by the global A star and the normal vector of the boundary center; is the velocity change cost, which is the angle between the current velocity vector direction and the normal vector of the boundary center.

5. The simulation verification system according to claim 1, characterized in that: The simulated UAV is a simulated quad-rotor UAV, comprising a first type of simulated sensor, a second type of simulated sensor, a nonlinear simulated controller and four simulated motors; The first type of simulated sensors includes simulated depth cameras; Used to obtain front-end sensor data including dynamic scene depth map from the scene information of the virtual environment where the simulated drone is located as input data for space search and trajectory planning; 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; Used to perform spatial search and trajectory planning based on the front-end sensor data of the first type of simulation sensor to obtain the desired trajectory and the simulated drone posture information sensed by the second type of simulation sensor, perform nonlinear control, and output the motor speed corresponding to the simulated 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; In order to obtain the resultant force and torque exerted on the simulated quad-rotor drone in the simulation environment under the action of four simulated motors.

6. The simulation verification system according to claim 1, characterized in that: The nonlinear simulation controller includes a trajectory solver, a position loop controller, a nonlinear angle converter, a nonlinear attitude mapper and a 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 and output the motor speed to the simulation motor according to the posture control error and the total position-speed control error.

7. The simulation verification system according to claim 1, characterized in that: It also includes a virtual simulation environment module; the virtual simulation environment module communicates data with the simulated unmanned aerial vehicle; the simulated virtual scene information is output to the first type of simulation sensor of the simulated unmanned aerial vehicle, so that the first type of simulation sensor can perceive the scene information and obtain front-end sensor data including dynamic scenes; according to the resultant force and torque exerted on the four-rotor unmanned aerial vehicle in the simulation environment by the simulated motor output, the simulated natural environment simulation data including simulated force field, atmospheric field and magnetic field are output to the second type of simulation sensor of the simulated unmanned aerial vehicle, so that the second type of simulation sensor can perceive the posture information of the unmanned aerial vehicle according to the natural environment simulation data.

Images