Multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud

Abstract

The application discloses a multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point clouds, comprising the following steps: step one, obtaining local two-dimensional point clouds of multiple unmanned vehicles and preprocessing; step two, constructing a global point cloud map; step three, constructing a global three-dimensional occupancy grid map; step four, obtaining obstacle avoidance path points; step five, fitting the obstacle avoidance path points; step six, obtaining an optimal trajectory for each unmanned vehicle to travel; and step seven, tracking the optimal trajectory to realize formation obstacle avoidance operation of the multiple unmanned vehicles. It can be seen that the application improves the centralized formation method of the previous unmanned vehicles, adopts distributed formation and relies on two-dimensional laser radars to realize the method, is low in cost, simple in implementation, and is particularly suitable for unmanned vehicle formation obstacle avoidance in complex scenes.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of multi-unmanned vehicle formation planning, and particularly relates to a multi-unmanned vehicle formation obstacle avoidance method based on a two-dimensional laser radar point cloud. BACKGROUND

[0002] Multi-UAV formation coordination is a very important research field. In the early stage of UAV development, research mainly focused on the technology of single UAV, such as the basic principles of autonomous driving, sensor technology, etc. With the advancement of technology, people began to focus on the coordination between multiple UAVs, trying to make multiple UAVs work together in certain scenarios. In recent years, with the rapid development of artificial intelligence, sensor technology, communication technology, etc., multi-UAV formation coordination technology has made significant progress, and has begun to be applied in some specific scenarios such as logistics parks, closed roads, etc. Various high-precision sensors such as laser radar, camera, millimeter wave radar, etc. are used to perceive the environment around the vehicle in real time, including obstacle, other vehicle position, speed, etc. information, providing data basis for the decision and control of UAV. In terms of communication technology, efficient and stable communication system is the key to multi-UAV formation coordination, ensuring real-time and accurate transmission of information between vehicles and between vehicles and control center, such as position, speed, travel intention, etc. Common communication technologies include Dedicated Short Range Communication (DSRC), 5G communication, etc. At the same time, high-precision positioning systems such as Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and sensor-based positioning technology, etc. ensure that each UAV can accurately know its own position, which is the basis for realizing formation coordination. In terms of decision and control algorithms, including path planning algorithm, formation control algorithm, etc. Path planning algorithm is responsible for planning a safe and efficient driving path for UAV; formation control algorithm controls the distance and formation between vehicles according to the position and state information of the vehicle, such as behavior-based method, virtual structure method, leader-follower method, etc. In the field of logistics transportation, in the scenarios of logistics parks, warehouses, etc., multi-UAV formation coordination can realize efficient handling and transportation of goods, improve logistics efficiency, and reduce labor costs. In the military field, it can be used for reconnaissance, patrol, combat, etc. tasks, improving combat effectiveness and survivability. How to make UAV formation work stably and safely in complex urban roads, bad weather, etc. conditions is one of the current research hotspots. This involves further improvement of environmental perception technology, and optimization of decision and control algorithms, so as to cope with various emergencies and uncertainties. With the increase of the number of UAVs, how to ensure the real-time and reliability of communication, and how to realize more efficient coordination decision, so that the whole formation can quickly respond to changes and improve operation efficiency, are also the focus of research. In terms of safety, it is crucial to ensure the safety and reliability of multi-UAV formation coordination. This includes the reliability design of vehicle system, research on fault detection and diagnosis technology, and the development of perfect safety strategy and emergency handling mechanism to prevent accidents and take timely measures when problems occur.

[0003] In the existing multi-unmanned vehicle formation planning method, most of them are through the virtual structure method and the leader-follower method to form the formation of the multi-unmanned vehicle fleet. From the existing method, the following problems exist: first, the existing leader-follower algorithm for unmanned vehicle formation task will stop once the leader is paralyzed or fails, and the communication delay between the leader and the follower will affect the control accuracy of the formation. Second, the centralized communication multi-vehicle formation planning algorithm is poor for local planning and dynamic scene, and the flexibility is poor compared with the distributed multi-vehicle cluster algorithm. Third, for the scene of multi-vehicle passing through complex obstacles, there is a lack of obstacle avoidance or formation constraint to further optimize the formation, and the formation will be disordered when passing through, which will cause the instability of the formation and the inability to avoid obstacles in the dense scene. SUMMARY

[0004] Therefore, the purpose of the present application is to provide a multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud, which can better complete the formation and obstacle avoidance tasks of the robot, avoid obstacles while maintaining the formation, reduce the cost and positioning demand power, and facilitate the stable operation of the unmanned vehicle system and the normal realization of the formation function.

[0005] To achieve the above technical purposes, the present application will adopt the following technical solutions:

[0006] A multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud, comprising the following steps:

[0007] Placing a plurality of unmanned vehicles in a preset cluster formation, and configuring a two-dimensional laser radar on each unmanned vehicle to perceive the environmental information around the corresponding unmanned vehicle, obtain the corresponding obstacle point cloud of each two-dimensional laser radar within the scanning range, and obtain the local two-dimensional point cloud of the corresponding unmanned vehicle after preprocessing;

[0008] Convert the obtained local two-dimensional point cloud into a local three-dimensional point cloud, and perform inflation in the three-dimensional direction on the obtained local three-dimensional point cloud to form a global three-dimensional point cloud map;

[0009] Based on the formed global three-dimensional point cloud map, a global three-dimensional occupancy grid map is constructed;

[0010] Based on the constructed global three-dimensional occupancy grid map, an obstacle avoidance path point is obtained;

[0011] Curve fitting is performed on the obstacle avoidance path point to obtain an obstacle avoidance path fitting curve;

[0012] Based on the obtained obstacle avoidance path fitting curve, an optimization item is used to construct a trajectory optimization problem; the optimization item includes curve smoothness, speed, acceleration and obstacle avoidance distance;

[0013] For the formation problem of unmanned vehicle cluster, a distributed shared network is used to broadcast the real-time positioning coordinates of the vehicle and other vehicles in real time, and a method of undirected graph optimization is used to construct formation constraints in the trajectory optimization problem. The optimal trajectory of each unmanned vehicle can be obtained by solving the above trajectory optimization problem.

[0014] The LMPC trajectory tracking algorithm is used to track the obtained optimal trajectory. The system model of the unmanned vehicle is designed as a linear time-invariant system, and the speed constraint and acceleration constraint of the linear time-invariant system are designed. According to the obtained optimal trajectory, an optimal cost function is designed for optimization and solution, and finally the optimal control output linear velocity and angular velocity are obtained to realize the formation obstacle avoidance operation of the unmanned vehicle in complex scenes.

[0015] Preferably, the construction of the global point cloud map specifically includes the following steps:

[0016] For the start radian angle_min and the end radian angle_max of the local two-dimensional point cloud LaserScan, the two-dimensional coordinate points in the above range are extracted;

[0017] Each two-dimensional coordinate point is converted into a three-dimensional coordinate point to form a local three-dimensional point cloud. The conversion formula is:

[0018]

[0019] where angle is the calculated angle information of the two-dimensional laser, range is the distance information obtained for each measurement point, and h represents the height information of the three-dimensional point cloud defined by the user. x , p y , p z represent the local three-dimensional point cloud position information converted from the two-dimensional coordinate points.

[0020] For the local three-dimensional point cloud position information, the corresponding x and y coordinates of each three-dimensional point cloud are kept unchanged in the z-axis direction, and are expanded upwards and downwards, and finally the expanded three-dimensional point cloud is obtained to form a global three-dimensional point cloud.

[0021] The global three-dimensional point cloud obtained is converted from the robot local coordinate system to the world coordinate system to obtain the point cloud in the world coordinate system. The transformation formula is as follows:

[0022] P ti =R i P i +T i (2)

[0023] In the formula: P i represents the global three-dimensional point cloud of the i-th unmanned vehicle; P tiP represents a global three-dimensional point cloud i P represents a point cloud in a world coordinate system after coordinate system transformation; R i R represents a rotation matrix of coordinate system transformation; T i T represents a translation matrix of coordinate system transformation.

[0024] P represents a point cloud in a world coordinate system ti The region formed is a three-dimensional point cloud map.

[0025] Preferably, the construction of the global three-dimensional occupancy grid map specifically comprises the following steps:

[0026] Step 3.1, for the obtained point cloud P in the world coordinate system ti , the length l, the width w, and the height h of the three-dimensional point cloud map are calculated, and the calculation formula is as follows:

[0027]

[0028] In the formula: x min represents the minimum boundary value of the three-dimensional point cloud map in the x direction, y min represents the minimum boundary value of the three-dimensional point cloud map in the y direction, z min represents the minimum boundary value of the three-dimensional point cloud map in the z direction, x max represents the maximum boundary value of the three-dimensional point cloud map in the x direction, y max represents the maximum boundary value of the three-dimensional point cloud map in the y direction, z max represents the maximum boundary value of the three-dimensional point cloud map in the z direction; r represents the resolution of the three-dimensional point cloud map.

[0029] Step 3.2, for the i-th three-dimensional point cloud position coordinates x i , y i , z i , the grid position ix i , iy i , iz i of the i-th three-dimensional point cloud is calculated, and the calculation formula is as follows:

[0030]

[0031] Step 3.3, the grid index value index i of the i-th three-dimensional point cloud position is calculated, and the calculation formula is as follows:

[0032] index i = ix i wh+iy i h+h(5)

[0033] The above grid is set to be occupied, and the grid not traversed is set to be idle. According to the above formula, a global three-dimensional occupancy grid map can be constructed.

[0034] Preferably, the constructed global three-dimensional occupancy grid map is sampled by using an Informed-RRT* algorithm to obtain an obstacle-avoiding path point, specifically:

[0035] The Informed-RRT* algorithm uses a standard elliptic equation as a boundary definition, with a starting point x start and an ending point x goal . The standard elliptic equation is:

[0036]

[0037] The distance between the starting point x start and the ending point x goal is set as the left and right focal distance c of the standard elliptic equation, the distance value is set as c min , and the long axis distance a of the ellipse is set as c best .

[0038] Sampling is performed in the elliptic sampling region and iterated continuously, and the sampling range is gradually shortened by gradually reducing the distance value c best to obtain an obstacle-avoiding path point.

[0039] Preferably, MINCO curves are used for fitting of the obstacle-avoiding path point, specifically:

[0040] The ith trajectory is divided into m i curves according to the same time interval T i , and each curve is expressed by an n-order polynomial, where the expression of n is:

[0041] n=2s-1(7)

[0042] In the formula, s represents the minimum order that needs to be ensured for the continuity of the trajectory.

[0043] The jth curve of the ith trajectory is expressed by an N-order polynomial based on time t as:

[0044]

[0045] Wherein, the calculation formula of the polynomial coefficient is as follows:

[0046]

[0047] In the formula: is a real number, T i represents the total time of the ith trajectory, and t represents the discrete time.

[0048] Computing the discrete positions p of the trajectory i The expression of (t) is:

[0049]

[0050] Preferably, the optimal trajectory of each unmanned vehicle is obtained, specifically comprising:

[0051] The design cost equation is designed, and its expression is as follows:

[0052]

[0053] Wherein, J(c i,j ,T i ) is the design cost function, μ i (t) represents the i-th fitted curve equation, T i represents the total time of the i-th fitted curve equation, and w T represents the time optimization weight;

[0054] The gradient of the cost equation with respect to the decision variable c i,j ,T i is calculated:

[0055]

[0056] The constraint condition is designed as follows:

[0057]

[0058] In the above formula: m i represents the number of curves, and n represents the polynomial order; μ i (t) represents the i-th fitted curve equation; represents the i-th trajectory equation; represents the starting point of the 0-th trajectory, denoted as P0, represents the end point of the n-th trajectory, denoted as P f ; represents the position point of each i-th trajectory calculated at T i , denoted as P i ; represents the position point of the i+1-th trajectory at the initial position; represents the position point of the j-th of each i-th trajectory at time δT i ; represents the starting position point of the j+1-th of each i-th trajectory;

[0059] Acceleration constraint is added in the constraint condition

[0060]

[0061] where v m denotes the maximum velocity, denotes the velocity profile; a first order function expression as follows:

[0062]

[0063] The acceleration is split into tangential acceleration a t and normal acceleration a n are constrained separately, the tangential acceleration constraint and the normal acceleration constraint are defined as follows:

[0064]

[0065] where a tm , a nm denote the maximum tangential and normal acceleration, respectively, denotes the velocity profile; a first order function expression denotes the acceleration profile; B denotes a constant matrix;

[0066] The first order derivative of the tangential acceleration constraint with respect to velocity The first order derivative of the normal acceleration constraint with respect to velocity The second order derivative of the tangential acceleration constraint with respect to velocity The second order derivative of the normal acceleration constraint with respect to velocity The expressions are as follows:

[0067]

[0068] A curvature constraint is added to the constraint conditions

[0069]

[0070] where K m represents the maximum curvature, where B is a constant matrix as in equation 16, the expression is as follows:

[0071]

[0072] The first order derivative of the curvature with respect to velocity expression and the first order derivative of the curvature with respect to acceleration expression are as follows:

[0073]

[0074] where, transposed matrix of velocity curve, velocity curve, transposed matrix of acceleration curve, acceleration curve;

[0075] Designing hyperplane constraint P H , the expression is as follows:

[0076]

[0077] Where p represents the position of the unmanned vehicle, A and b represent the hyperplane coefficients, n represents the number of hyperplanes, R, I e respectively represent the rotation matrix and the identity matrix;

[0078] The obstacle avoidance constraint of the unmanned vehicle is defined as:

[0079] G ζ (p)=A i (p+RI e )-b i , i∈[0,n] (22)

[0080] In the formula: A i represents the first-order derivative of the obstacle avoidance constraint of the unmanned vehicle with respect to the position That is:

[0081]

[0082] The constructed undirected graph model: let the weight of the ith unmanned vehicle and the adjacent jth unmanned vehicle be w ij , get the weighted undirected graph, design the adjacency matrix A, degree matrix D and Laplacian matrix L of the undirected graph; the calculation formula of the Laplacian matrix L is:

[0083] L=D-A (24)

[0084] The symmetric standard Laplacian matrix F is obtained by normalizing the Laplacian matrix L through the degree matrix D, and the expression is designed as follows:

[0085] F=I-D -1 / 2 AD -1 / 2 (25)

[0086] Where, for the undirected graph constraint G f , according to the existing formation F c and the expected formation F r , the formation constraint is designed as:

[0087] G f (p)=||F c -F r || 2 (26)

[0088] Where, the first-order derivative of the formation with respect to the position is expressed as:

[0089]

[0090] The optimal trajectory is obtained by solving the optimization problem J and the corresponding constraint conditions by using an unconstrained optimization method, and a series of position and speed information is obtained for use by the path tracking algorithm.

[0091] Preferably, the obtained optimal trajectory is tracked by using an LMPC trajectory tracking algorithm to realize the formation obstacle avoidance operation of the unmanned vehicle in a complex scene, and the specific process is as follows:

[0092] The kinematics analysis of the unmanned vehicle is performed by using a two-wheel differential model, and the expression form is as follows:

[0093]

[0094] The motion model needs to be linearized, and the system state is set as z, and the specific expression is as follows:

[0095]

[0096] In which, A, B, C, u expressions are as follows:

[0097]

[0098] In which, v is the longitudinal speed of the vehicle, ω is the angular speed of the turning, and φ is the heading angle of the vehicle;

[0099] The prediction step number of the LMPC controller is set as N, the control period of the unmanned vehicle is set as T, and the control sequence U={u k ,u k+1 ,...,u k+N-1} in the future N periods is taken as the decision variable, in which k is the current control period of the robot, and the cost function J(x k ,u k ) is designed, and the expression is as follows:

[0100]

[0101] In which, Q is the control constraint, R1 is the constraint of the control change; x k is the horizontal coordinate of the current position of the unmanned vehicle; r k is the horizontal coordinate of the expected position of the unmanned vehicle; u k is the speed control amount of the unmanned vehicle; x N is the horizontal coordinate of the position of the unmanned vehicle after N steps; r N is the horizontal coordinate of the expected position of the unmanned vehicle after N steps;

[0102] The optimal problem is solved to obtain the longitudinal speed and angular speed of the unmanned vehicle.

[0103] Another technical purpose of the present application is to provide an electronic device comprising a memory, a processor and a computer program stored on the memory and running on the processor, which computer program runs to perform the above-mentioned multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud.

[0104] Based on the above technical solutions, the present application has the following advantages compared with the prior art:

[0105] The multi-unmanned vehicle formation obstacle avoidance method proposed by the present application has good real-time performance, high efficiency and simple deployment method, and is particularly suitable for formation obstacle avoidance of multiple unmanned vehicles in complex scenes. Compared with the prior art, the present application has the following advantages:

[0106] Firstly, compared with the existing leader-follower algorithm, the present application adopts a distributed communication method for position sharing among multiple vehicles before formation advancement, wherein any vehicle failure can still ensure stable operation of other vehicles, thereby ensuring stable operation of the formation system.

[0107] Secondly, compared with the previous three-dimensional laser point cloud as an obstacle avoidance sensor, the method of converting two-dimensional laser point cloud to three-dimensional point cloud can greatly reduce the cost, and the computing power required for unmanned vehicle positioning is reduced, thereby facilitating stable operation of the unmanned vehicle system and normal implementation of the formation function.

[0108] Thirdly, compared with the previous formation algorithm, the method of constructing formation constraints and obstacle avoidance constraints can better complete the formation and obstacle avoidance tasks of the robot, and can avoid obstacles while maintaining the formation. BRIEF DESCRIPTION OF DRAWINGS

[0109] Figure 1 The flowchart of the present application.

[0110] Figure 2 The simulation scene diagram of the multi-unmanned vehicle formation obstacle avoidance scene in the present application.

[0111] Figure 3 The diagram of the present application for obstacle perception of the multi-unmanned vehicle using two-dimensional laser radar.

[0112] Figure 4 The diagram of the present application for obstacle perception of the multi-unmanned vehicle using the extended three-dimensional laser point cloud converted from the two-dimensional laser radar.

[0113] Figure 5 The diagram of the present application for path point search using Informed RRT* algorithm under the three-dimensional occupancy grid map.

[0114] Figure 6 Trajectory optimization generation curve for obstacle avoidance for multiple unmanned vehicles in the present application

[0115] Figure 7 Historical trajectory diagram obtained by path tracking using the LMPC algorithm in the present application DETAILED DESCRIPTION

[0116] The present application will be further described below by way of examples in conjunction with the accompanying drawings.

[0117] The multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud provided by the present application comprises: data preprocessing of two-dimensional laser radar point clouds of multiple unmanned vehicles, converting two-dimensional point clouds LaserScan into three-dimensional point clouds PointCloud2, inflating the obtained point clouds in three-dimensional directions, and simultaneously performing coordinate conversion of the three-dimensional point clouds according to the positioning of each unmanned vehicle in the world coordinate system to obtain global point clouds in the world coordinate system, and simultaneously constructing a global three-dimensional occupancy grid map of the obtained global point clouds. According to the grid map constructed by the global point cloud map, multiple unmanned vehicles respectively acquire obstacle avoidance path points by using the Informed RRT* algorithm, curve fitting of the above obstacle avoidance path points is performed by using the MINCO (Minimum Control) curve, and an optimal problem is constructed according to the curve smoothness, speed, acceleration, obstacle avoidance distance and the like. In the unmanned vehicle cluster formation problem, a distributed shared network is used to broadcast the real-time positioning coordinates of the ego vehicle and other vehicles in real time, and a method of undirected graph optimization is used to model the multi-unmanned vehicle formation problem. The above optimization problem is solved, and finally the optimal trajectory of each unmanned vehicle is obtained. Finally, the LMPC method is used to track the target optimal trajectory, and finally the unmanned vehicle is realized in the complex scene. Formation obstacle avoidance operation.

[0118] A, two-dimensional point cloud is converted and expanded to obtain three-dimensional point cloud

[0119] First, the end angle of the two-dimensional laser point cloud data LaserScan scanning point cloud starting radian angle_min and scanning point cloud termination radian angle_max is extracted. The two-dimensional coordinate points in the above range are converted to three-dimensional point clouds according to the index label i of the three-dimensional coordinate points, the scanning point cloud starting radian angle_min and the distance angle_increment between the measured angles. The conversion formula is as follows:

[0120]

[0121] wherein angle is the angle information of the calculated two-dimensional laser, range is the distance information obtained at each measurement point, and h represents the height information defined by the three-dimensional point cloud. x , p y , p z The local three-dimensional point cloud position information converted from the two-dimensional laser points is represented, and the corresponding x and y coordinates of the three-dimensional point cloud in the z-axis direction are kept unchanged, and the point cloud is expanded upward and downward, and finally the expanded three-dimensional point cloud is obtained. The obtained point cloud is converted from the robot local coordinate system to the world coordinate system, and the rotation matrix R i and the translation matrix T i are calculated according to the three-dimensional coordinates of the i-th unmanned vehicle in the world coordinate system and the corresponding attitude. The local point cloud set of the i-th unmanned vehicle is P i , the point cloud in the world coordinate system after coordinate conversion is P ti , and the transformation formula is as follows:

[0122] P ti = R i P i + T i (2)

[0123] The minimum and maximum boundary values of x, y, and z, i.e., x min , y min , z min , x max , y max , z max , are calculated for the obtained three-dimensional point cloud P ti in the world coordinate system. The resolution of the map is r, and the length l, width w, and height h of the three-dimensional point cloud map can be calculated, and the calculation formula is as follows:

[0124]

[0125] The grid positions ix i , iy i , iz i of the i-th three-dimensional point cloud are calculated according to the position coordinates x i , y i , z i of the i-th three-dimensional point cloud, and the calculation formula is as follows:

[0126]

[0127] The corresponding grid index value index i is calculated, and the calculation formula is as follows:

[0128] index i = ix i wh+iyi h+h(5)

[0129] The above grid is set to be occupied, and the grid not traversed is set to be idle. A three-dimensional occupancy grid map is constructed according to the above formula.

[0130] B, search for obstacle avoidance path points by using InformedRRT* algorithm

[0131] The Informed-RRT* algorithm is an algorithm obtained by optimizing the sampling process of RRT*. It uses an elliptical sampling method to replace global uniform sampling. A standard elliptic equation is used as a boundary definition, and the start point x start and the end point x goal The standard elliptic equation is:

[0132]

[0133] Let the distance between the start point x start and the end point x goal be the distance between the left and right foci c, the distance value be c min , and the distance of the major axis of the ellipse a be c best Then sample in the elliptical sampling area and iterate continuously. By limiting the sampling range, the speed of progressive optimization is increased. That is, we limit the sampling range (ellipse) after finding the path, and gradually reduce c best to shorten the sampling range and obtain the optimal path point.

[0134] C, adopt MINCO curve for curve fitting and construct unconstrained optimization problem

[0135] (1) The optimal path points obtained above are fitted by using MINCO curve. The obtained obstacle avoidance path points and time are used as parameters for curve fitting. The trajectory parameters have spatial and temporal degrees of freedom, and a trajectory fitting scheme for reducing linear complexity is realized. The specific expression of each trajectory is divided into m i segments of curve according to the same time interval T i for the i-th trajectory, and each curve is expressed by an n-order polynomial, where the expression of n is:

[0136] n=2s-1(7)

[0137] Where s represents the minimum order that needs to be ensured for the continuity of the trajectory, generally taken as 3 to ensure the continuity of the jerk. The N-order expression of the j-th curve of the i-th trajectory based on time t is:

[0138]

[0139] Where the polynomial coefficients The calculation formula is as follows:

[0140]

[0141] For a discrete time t, the trajectory discrete position p i The expression of (t) is:

[0142]

[0143] (2) The design cost equation is expressed as follows:

[0144]

[0145] For the above optimization problem, the gradient of the cost equation with respect to the decision variable c i,j ,T i is calculated:

[0146]

[0147] Where J(c,T) is the design cost function, μ i (t) is the i-th piece of fitting curve equation, T i is the total time of the fitting curve equation, w T is the time optimization weight, and the constraint condition is as follows:

[0148]

[0149] The constraint condition means that, first, the i-th piece of fitting curve equation μ i (t) is expressed as Second, the starting point of the 0-th trajectory is P0, and the end point of the n-th trajectory is P f ; Third, the position point calculated at T i for each i-th trajectory and the position point of the initial position of the i+1-th trajectory are equal, and both are equal to P i . Fourth, represents the position point of the j-th piece of the i-th trajectory at time δT i , and the starting position point of the j+1-th piece of the i-th trajectory is P i . At the same time, the speed constraint

[0150]

[0151] Where v m represents the maximum speed, and the first-order function expression is as follows:

[0152]

[0153] For acceleration, it needs to be split into tangential acceleration a t and normal acceleration a n respectively, tangential acceleration constraint and normal acceleration constraint are defined as follows:

[0154]

[0155] where a tm , a nm represent the maximum tangential acceleration and maximum normal acceleration respectively, the first order derivative of tangential acceleration constraint with respect to velocity the first order derivative of normal acceleration constraint with respect to velocity and the second order derivative of tangential acceleration constraint with respect to velocity the second order derivative of normal acceleration constraint with respect to velocity The expressions are as follows:

[0156]

[0157] At the same time, curvature constraint is added

[0158]

[0159] κ m represents the maximum curvature, where B is a constant matrix, and the expression is as follows:

[0160]

[0161] The expression of the first order derivative of curvature with respect to velocity and the first order derivative of curvature with respect to acceleration are as follows:

[0162]

[0163] where, represents the transpose matrix of velocity curve, velocity curve, transpose matrix of acceleration curve, acceleration curve. The design hyperplane constraint P H is expressed as follows:

[0164]

[0165] where p represents the position of the robot, A and b represent the hyperplane coefficients, n represents the number of hyperplanes, R, I e represent the rotation matrix and identity matrix respectively. The obstacle avoidance constraint of the unmanned vehicle can be defined as:

[0166] Gζ (p) = A i (p + RI e )-b i , i e [0, n] (22)

[0167] The first-order derivative with respect to position is represented as:

[0168]

[0169] For the constructed undirected graph model, let the weight of the ith unmanned vehicle and the adjacent jth unmanned vehicle be w ij , to obtain a weighted undirected graph, design the adjacency matrix A, degree matrix D and Laplacian matrix L of the undirected graph; the calculation formula of the Laplacian matrix L is:

[0170] L = D - A (24)

[0171] The symmetric standard Laplacian matrix F is obtained by normalizing the Laplacian matrix L through the degree matrix D, and the expression is as follows:

[0172] F = I - D -1 / 2 AD -1 / 2 (25)

[0173] Among them, for the undirected graph constraint G f , according to the existing formation F c and the expected formation F r , design the formation constraint:

[0174] G f (p) = ||F c -F r || 2 (26)

[0175] Among them, the first-order derivative constraint is:

[0176]

[0177] The optimization problem J and the corresponding constraint conditions are solved by using an unconstrained optimization method to obtain the optimal trajectory, and a series of position and velocity information are obtained for the path tracking algorithm.

[0178] D, LMPC tracks the optimal trajectory obtained

[0179] Kinematics analysis is performed on the two-wheel differential model, and the expression is as follows:

[0180]

[0181] The motion model needs to be linearized, and the system state z is set as follows:

[0182]

[0183] where A, B, C, u expressions are respectively:

[0184]

[0185] where v is the longitudinal speed of the vehicle, ω is the angular speed of the turn, and φ is the vehicle heading angle. Let the prediction step number of the LMPC controller be N, and the control period of the robot be T. The control sequence U = {u k ,u k+1 ,...,u k+N-1} in the future N periods is taken as the decision variable, where k is the current control period of the robot, and the cost function J(x k ,u k ) is designed as follows:

[0186]

[0187] where Q is the control constraint, and R1 is the constraint of control change. The control amount of the unmanned vehicle longitudinal speed and the unmanned vehicle angular speed are obtained by solving the above optimal problem.

[0188] A multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud, as shown in Figure 1 , includes the following steps:

[0189] Firstly, the environment is built in the Gazebo simulation, and four differential wheeled unmanned vehicles as shown in Figure 2 are obtained, and their position coordinates are (0, 1.5), (-1.5, 0), (0, -1.5), and (1.5, 0) respectively, which are placed in space according to the above formation.

[0190] Secondly, the environment information is perceived according to the two-dimensional laser radar carried by the multi-unmanned vehicle, and the result as shown in Figure 3 is obtained, and the obstacle point cloud is only in the two-dimensional plane, which is converted and extended to obtain the three-dimensional point cloud as shown in Figure 4 .

[0191] For the obtained three-dimensional point cloud environment, the InformedRRT* algorithm is used to obtain the obstacle avoidance path points according to the global map point cloud, and the algorithm for obstacle avoidance in the three-dimensional occupancy grid map is as shown in Figure 5As shown, the extension point of the algorithm is constrained within an ellipse, the obstacle avoidance path points are fitted with MINCO curves, the optimization problem is constructed according to the curve smoothness, speed, acceleration, obstacle avoidance distance and other optimization items, in the unmanned vehicle formation problem, the real-time positioning coordinates of the self vehicle and other vehicles are broadcast in real time through a distributed shared network, the unmanned vehicle formation problem is modeled by using the undirected graph optimization method, the optimal trajectory of each unmanned vehicle is obtained by solving the optimization problem, and the optimal trajectory of each unmanned vehicle is obtained as shown in the figure. Figure 6 As shown, the multiple unmanned vehicles obtain the optimal curve for obstacle avoidance in the occupancy grid map constituted by three-dimensional point clouds, and finally the LMPC method is used to complete the tracking of the target optimal trajectory, and finally the unmanned vehicle is realized in the complex scene Formation obstacle operation as shown in the figure. Figure 7 As shown, four unmanned vehicles travel to obtain four historical trajectories.

[0192] The application also provides a storage medium, which stores a program that, when executed, performs any of the above-described two-dimensional laser radar point cloud multi-unmanned vehicle formation obstacle avoidance methods.

[0193] The application also provides an electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor executes the above-described two-dimensional laser radar point cloud multi-unmanned vehicle formation obstacle avoidance method through the computer program.

[0194] In the above embodiments of the application, the description of each embodiment has its own focus, and the parts not described in detail in a certain embodiment can be referred to the related description of other embodiments.

[0195] In the several embodiments provided in the present application, it should be understood that the disclosed technology can be implemented in other ways. Of course, the device embodiment described above is only schematic. For example, the division of the units can be a logical function division, and there can be another division manner in actual implementation, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some interface, unit or module, and can be electrical or other forms.

[0196] The units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, that is, they can be located in one place, or they can be distributed on multiple units. According to actual needs, part or all of the units can be selected to achieve the purpose of the embodiment scheme.

[0197] In addition, each function unit in each embodiment of the present application can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software function unit.

[0198] When the integrated unit is realized in the form of a software function unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application or the entire or part of the technical solutions that essentially contribute to the prior art can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes a U disk, a read-only memory (ROM), a random access memory (RAM), a mobile hard disk, a magnetic disk or an optical disk, and various media that can store program codes.

[0199] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of each embodiment of the present application.

Claims

1. A multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional laser radar point cloud, characterized in that, The method comprises the following steps: A plurality of unmanned vehicles are arranged in a preset cluster formation, and two-dimensional laser radars are configured on the unmanned vehicles to perceive the environment information around the corresponding unmanned vehicles, obtain corresponding obstacle point clouds in the scanning range of each two-dimensional laser radar, and obtain local two-dimensional point clouds of the corresponding unmanned vehicles after preprocessing; The obtained local two-dimensional point clouds are converted into local three-dimensional point clouds, and the obtained local three-dimensional point clouds are expanded in the three-dimensional direction to form a global three-dimensional point cloud map; Based on the formed global three-dimensional point cloud map, a global three-dimensional occupancy grid map is constructed; Based on the constructed global three-dimensional occupancy grid map, an obstacle avoidance path point is obtained; The obstacle avoidance path point is curve-fitted to obtain an obstacle avoidance path fitting curve; Based on the obtained obstacle avoidance path fitting curve, an optimization item is used to construct a trajectory optimization problem; the optimization item includes curve smoothness, speed, acceleration and obstacle avoidance distance; For the cluster formation problem of the unmanned vehicle, a distributed shared network is used to broadcast the real-time positioning coordinates of the vehicle and other vehicles in real time, and a method of undirected graph optimization is used to construct formation constraints in the trajectory optimization problem, so that the trajectory optimization problem is solved, and the optimal trajectory of each unmanned vehicle is obtained; The LMPC trajectory tracking algorithm is used to track the obtained optimal trajectory: the system model of the unmanned vehicle is designed as a linear time-invariant system, and the speed constraint condition and the acceleration constraint condition satisfying the linear time-invariant system are designed; the optimal cost function is designed according to the obtained optimal trajectory to optimize and solve, and finally the optimal control output linear velocity and angular velocity are obtained to realize the formation obstacle avoidance operation of the unmanned vehicle in a complex scene; Let the prediction step number of the LMPC controller be N, the control period of the unmanned vehicle be T, and the control sequence in the future N periods be , where k is the current control period of the robot, and a cost function is designed , and the expression is as shown in the following ； wherein Q is a control constraint, is a control constraint for the change; is a horizontal coordinate of a current position of the unmanned vehicle; is a horizontal coordinate of a desired position of the unmanned vehicle; is a speed control of the unmanned vehicle; is a horizontal coordinate of a position of the unmanned vehicle after N steps; is a horizontal coordinate of a desired position of the unmanned vehicle after N steps; The optimal problem is solved to obtain the vehicle longitudinal velocity and angular velocity of the unmanned vehicle.

2. The multi-unmanned vehicle formation obstacle avoidance method based on two-dimensional lidar point cloud according to claim 1, characterized in that, The construction of the global point cloud map specifically comprises the following steps: For the starting radian angle_min and the ending radian angle_max of the scanning point cloud of the local two-dimensional point cloud LaserScan, the two-dimensional coordinate points in the above range are extracted; Each two-dimensional coordinate point is converted into a three-dimensional coordinate point to form a local three-dimensional point cloud; the conversion formula is: （1） Wherein, angle is the calculated angle information of two-dimensional laser, range is the distance information obtained by each measuring point, and h represents the height information defined by the three-dimensional point cloud; , , represent the local three-dimensional point cloud position information converted by the two-dimensional coordinate points; The position information of the local three-dimensional point cloud is kept unchanged in the x and y coordinates of the corresponding three-dimensional point cloud in the z-axis direction, and is expanded upward and downward, and finally the expanded three-dimensional point cloud is obtained to form a global three-dimensional point cloud; The global three-dimensional point cloud obtained is converted from the robot local coordinate system to the world coordinate system to obtain the point cloud in the world coordinate system, and the transformation formula is as follows: （2） In the formula: represents the global three-dimensional point cloud of the ith unmanned vehicle; represents the global three-dimensional point cloud the point cloud in the world coordinate system after coordinate system transformation; represents the rotation matrix of coordinate system transformation; represents the translation matrix of coordinate system transformation; Point cloud in a world coordinate system The region formed is a three-dimensional point cloud map.

3. The method of claim 1, wherein, The construction of the global three-dimensional occupancy grid map specifically comprises the following steps: Step 3.1, for the obtained point cloud in the world coordinate system , the length of the three-dimensional point cloud map is calculated , the width of the three-dimensional point cloud map is calculated , the height of the three-dimensional point cloud map is calculated , the calculation formula is as follows: （3） wherein: represents a minimum boundary value of the three-dimensional point cloud map in the x direction, represents a minimum boundary value of the three-dimensional point cloud map in the y direction, represents a minimum boundary value of the three-dimensional point cloud map in the z direction, represents a maximum boundary value of the three-dimensional point cloud map in the x direction, represents a maximum boundary value of the three-dimensional point cloud map in the y direction, represents a maximum boundary value of the three-dimensional point cloud map in the z direction; represents a resolution of the three-dimensional point cloud map; Step 3.2, compute the grid position for the i-th three-dimensional point cloud position coordinate , , , compute the grid position for the i-th three-dimensional point cloud , , , the formula is as follows: （4） Step 3.3, calculating the grid index value of the i-th three-dimensional point cloud position The calculation formula is as follows: （5） The above grid is set as occupied, and the grid not traversed is set as idle, and the global three-dimensional occupancy grid map can be constructed according to the above formula.

4. The method of claim 1, wherein, The global three-dimensional occupancy grid map constructed is sampled by using the Informed-RRT* algorithm to obtain an obstacle avoidance path point, specifically: The Informed-RRT* algorithm uses a standard ellipse equation as a boundary definition for the start and end points, the standard ellipse equation being: （6） The distance between the start point and the end point is set as the distance between the left and right focal points of a standard ellipse equation , the distance value is set as , and the distance of the long axis of the ellipse a is set as ; Sampling in the elliptical sampling region and constantly iterating, through the distance value Gradually reduce the sampling range to shorten the sampling range to get the obstacle avoidance path point.

5. The method of claim 1, wherein, The MINCO curve is used for fitting the obstacle avoidance path point, specifically: The segments are divided into segments of equal time intervals segments of equal time intervals wherein the expression of​​ （7） In the formula, represents the minimum order that needs to be guaranteed for trajectory continuity; The first segment of the trajectory is a first curve of a polynomial function of time is expressed as: ​ （8） where the polynomial coefficients The formula for calculating the polynomial coefficients is as follows: （9） wherein: denotes a real number, denotes the total time of the i-th trajectory, denotes a discrete time; Computing the trajectory discrete position The expression for the trajectory discrete position is: （10）。 6. The method of claim 1, wherein, The optimal trajectory of each unmanned vehicle is obtained, specifically: The cost equation is designed, and its expression is as follows: （11） wherein, is a cost function of the design, represents the equation of the fitting curve for the i-th segment, represents the total time of the equation of the fitting curve for the i-th segment, represents the time optimization weight; Computing the cost equation for the gradient of the decision variables : （12） The constraint condition is designed as follows: （13） In the above formulae: represents the number of curves, represents the order of the polynomial; represents the equation of the i-th segment of the fitting curve; represents the equation of the i-th segment of the trajectory; represents the start point of the 0-th segment of the trajectory, denoted as , represents the end point of the n-th segment of the trajectory, denoted as ; represents the position point of each i-th segment of the trajectory at , denoted as ; the position point of the i+1-th segment of the trajectory at the initial position; represents the position point of the j-th segment of each i-th segment of the trajectory at time ; represents the start position point of the j+1-th segment of each i-th segment of the trajectory; Adding an acceleration constraint in the constraints : （14） wherein represents the maximum speed, represents the speed profile; a first order function expression as follows: （15） The acceleration is split into tangential acceleration and normal acceleration are constrained separately, the tangential acceleration constraint and the normal acceleration constraint are defined as follows: （16） wherein respectively denote the maximum tangential and normal acceleration, denotes the velocity profile; and denotes the acceleration profile; denotes a constant matrix; First derivative of tangential acceleration constraint with respect to velocity First derivative of normal acceleration constraint with respect to velocity Second derivative of tangential acceleration constraint with respect to velocity Second derivative of normal acceleration constraint with respect to velocity The expression is as follows: （17） Adding curvature constraints in constraints : （18） where represents the maximum curvature, where B is the same as in equation 16, a constant matrix, and is expressed as follows: （19） Curvature first derivative expression with respect to velocity and curvature first derivative expression with respect to acceleration as follows: （20） wherein , , , represent the transpose matrix of the velocity profile, the velocity profile, the transpose matrix of the acceleration profile, the acceleration profile; Designing hyperplane constraints The expression is as follows: （21） wherein, represents the position of the ego vehicle, and represents the hyperplane coefficients, represents the number of hyperplanes, respectively represent a rotation matrix and an identity matrix; The obstacle avoidance constraint of the unmanned vehicle is defined as follows: （22） wherein: denotes the first derivative of the obstacle avoidance constraint of the unmanned vehicle with respect to the position i.e.: （23） The constructed undirected graph model: the weight of the ith unmanned vehicle and the adjacent jth unmanned vehicle is , to obtain a weighted undirected graph, design the adjacency matrix A, the degree matrix D and the Laplacian matrix L of the undirected graph; the calculation formula of the Laplacian matrix L is: （24） The symmetric normalized Laplacian matrix F is obtained by normalizing the Laplacian matrix L through the degree matrix D, and the design expression is as follows: （25） where the undirected graph constraints , the existing formation and the desired formation are designed: （26） The first-order derivative of the formation with respect to the position is expressed as: （27） The optimal trajectory is obtained by solving the optimization problem J and the corresponding constraint conditions using an unconstrained optimization method, and a series of position and velocity information is obtained for the path tracking algorithm.

7. The method of claim 1, wherein, The LMPC trajectory tracking algorithm is used to track the obtained optimal trajectory to realize the formation obstacle avoidance operation of the unmanned vehicle in a complex scene, and the specific process is as follows: The kinematics analysis of the unmanned vehicle is performed using a two-wheel differential model, and the expression form is as follows: （28） The motion model needs to be linearized, and the system state is set as z, and the specific expression is as follows: （29） wherein The expressions are respectively: （30） wherein is the vehicle longitudinal speed, is the angular velocity of the turn, is the vehicle heading angle.

8. An electronic device comprising a memory, a processor, and a computer program stored on the memory and running on the processor, the computer program running to perform the multi-unmanned vehicle formation obstacle avoidance method based on a two-dimensional laser radar point cloud according to any one of claims 1 to 7.

Images