Online two-dimensional obstacle avoidance trajectory planning system and method thereof

Abstract

The invention discloses an online two-dimensional obstacle avoidance trajectory planning system and method. The trajectory planning system comprises a kinematic model, a two-dimensional laser radar, a state quantity constraint, a control quantity constraint and a motion trajectory safety area. The motion trail safety region is composed of a convex feasible region, a dynamic linear obstacle avoidance constraint, a dynamic optimization index and a dynamic prediction time domain; the method comprises the following steps: S1, constructing an unmanned autonomous platform kinematics model; s2, unknown surrounding environment information is detected through a two-dimensional laser radar, obstacle point cloud data is obtained, and a motion trail safety area is constructed; s3, constructing a state quantity constraint and a control quantity constraint according to the kinematic model; s4, according to the dynamic linear obstacle avoidance constraint, the state quantity constraint, the control quantity constraint, the dynamic optimization index and the dynamic prediction time domain, rolling optimization is adopted to output a real-time obstacle avoidance planning track of the online unmanned autonomous platform; according to the invention, online obstacle avoidance and trajectory planning of the unmanned autonomous platform can be rapidly and safely realized.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned autonomous platform trajectory planning, and in particular relates to an online two-dimensional obstacle avoidance trajectory planning system and method thereof. Background Art

[0002] With the rapid development of the digital economy, unmanned autonomous platforms have shown broad application prospects in military and civilian fields due to their high mobility, scalability and multifunctionality. Among many research directions, online trajectory planning of unmanned autonomous platforms in unknown environments is both a basic problem and a hot issue. Generally speaking, the movement of unmanned autonomous platforms to target points in unknown environments mainly involves three technical points: one is the perception and processing of unknown environments, the second is obstacle avoidance based on environmental information, and the third is real-time trajectory planning.

[0003] For the perception of unknown environments, the mainstream solutions include visual perception, lidar perception, ultrasonic perception, etc. Vision-based perception often uses sensors such as infrared cameras, optical flow cameras, and depth cameras to obtain environmental point cloud information, and processes the point cloud information based on SLAM technology. This solution has the advantages of lightweight sensors, mature visual algorithms, and low equipment costs, but factors such as short detection distance, limited viewing angle, and large detection errors also limit the application of visual cameras. With the continuous expansion of demand, high-precision and high-efficiency lidars are gradually coming into people's view. Due to the large size, heavy weight, and high price of three-dimensional lidars, most unmanned autonomous platforms are equipped with two-dimensional lidars as sensors.

[0004] For obstacle avoidance of unmanned autonomous platforms, the classic solutions include artificial potential field method and dynamic window method. The artificial potential field method guides the movement of the unmanned autonomous platform through the combined force field formed by the gravitational field of the desired target point and the repulsive field of the obstacle, thereby achieving obstacle avoidance. Its characteristics are simple principle, easy implementation, and low computational cost, but it will cause problems such as force balance, uneven trajectory, and target point oscillation. The dynamic window method generates a set of possible trajectories within the current position and speed range of the unmanned autonomous platform and selects the optimal trajectory to avoid obstacles. The algorithm is simple to calculate and has strong real-time performance, but it depends on the accuracy of the environmental model. Both of the above methods use obstacles as objects to obtain obstacle avoidance constraints. Some scholars use unmanned autonomous platforms as objects to generate safe areas around unmanned autonomous platforms to avoid obstacles. This scheme represents the obstacle avoidance task as a set of linear constraints by making a convex approximation to the feasible space, which simplifies the solution of the optimization problem, but reduces the success rate of obstacle avoidance. In addition, current research on obstacle avoidance of unmanned autonomous platforms mostly focuses on convex obstacles, while weakening the processing of more complex non-convex obstacles.

[0005] For unmanned autonomous platform trajectory planning, the A* algorithm and the RRT algorithm are both highly flexible, but both have the disadvantages of high computational complexity and poor real-time performance, making them difficult to apply to online trajectory planning for unmanned autonomous platforms. Deep reinforcement learning can learn obstacle avoidance strategies and trajectory optimization in complex environments, but the training time is long and the data cost is high. In recent years, model predictive control has been increasingly applied to online trajectory planning for unmanned autonomous platforms. The algorithm solves an optimization problem at each control moment, predicts future trajectories, and selects the optimal control input to guide the movement of the unmanned autonomous platform, but the large amount of computation also restricts its development. Summary of the invention

[0006] The present invention proposes an online two-dimensional obstacle avoidance trajectory planning system and method thereof, and adopts the following technical solutions:

[0007] An online two-dimensional obstacle avoidance trajectory planning system, the trajectory planning system includes a kinematic model, a two-dimensional laser radar, a state quantity constraint, a control quantity constraint and a motion trajectory safety area; the motion trajectory safety area is composed of a convex feasible domain, a dynamic linear obstacle avoidance constraint, a dynamic optimization index, and a dynamic prediction time domain; wherein:

[0008] Step 1: Obstacle point cloud data is obtained by detecting the surrounding unknown environment information through a two-dimensional laser radar;

[0009] Step 2: Construct a safe area for the motion trajectory, including:

[0010] 201. Based on the obstacle point cloud data, the convex feasible domain is generated according to the expansion rule with the unmanned autonomous platform as the center;

[0011] 202. According to the position and shape of the convex feasible region at each moment, the dynamic linear obstacle avoidance constraint is obtained according to the following formula:

[0012] E k x(k)+F k y(k)+G k ≤0

[0013] Where E k 、F k , G k They are all N×1 vectors, which change dynamically with the different convex feasible regions;

[0014] 203. According to the area of ​​the convex feasible region, a dynamic prediction time domain is obtained by construction;

[0015] 204. Construct a dynamic optimization index through the positional relationship between the unmanned autonomous platform, the target point, and the centroid of the convex feasible domain;

[0016] Step 3: According to the kinematic model, the state quantity constraints and control quantity constraints are constructed according to the following formulas:

[0017]

[0018] Where: X min , X max They represent the maximum and minimum values ​​of the state quantity of the unmanned autonomous platform, U min , U max Respectively represent the maximum and minimum values ​​of the control quantity of the unmanned autonomous platform; N k For dynamic prediction time domain;

[0019] Step 4: According to the dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, dynamic optimization indicators and dynamic prediction time domain, rolling optimization is used to output the real-time obstacle avoidance planning trajectory of the online unmanned autonomous platform.

[0020] Furthermore, the process of generating a convex feasible region in step 201 includes:

[0021] Unmanned autonomous platform D k Generate a regular N-gon as the center And the distance between each endpoint and the unmanned autonomous platform is s, that is, D k P 1 1 The angle with the positive direction of the X axis is β; where: the current position of the unmanned autonomous platform is at D k , its coordinates D(k)=[x D (k),y D (k)] T , the number of polygon edges is N, and the maximum number of expansions is M;

[0022] Let the radius of the circumscribed circle of the unmanned autonomous platform be L 1 , the obstacle avoidance margin is L 2 , then the safe distance of the convex feasible region extension is L s =L 1 +L 2 ;

[0023] According to the principle of proportional expansion, let the expansion factor be k 1 , the expansion times are initialized to 1, and the expansion rules are as follows:

[0024] 210. D k P 1 1 Expand to D k P 1 2 , so that D k P 1 2 =k 1 D k P 1 1; Considering the safety distance of the unmanned autonomous platform, expand D k P 1 2 Distance makes Determine N-gon Do the following two conditions meet at the same time: (1) the polygon is convex; (2) the polygon does not contain obstacle point clouds; if both conditions are met

[0025] If all are satisfied, the extended P is retained. 1 2 ; If any of the conditions is not met, then let P 1 2 =P 1 1 ;

[0026] 211. Expand the remaining endpoints according to step 210 Get a new N-gon This expansion

[0027] The expansion is completed, and the number of expansions increases by 1;

[0028] 212. Determine whether the number of expansions is less than the maximum number of expansions M. If so, use a new N-gon

[0029] To expand the benchmark, repeat steps (210) and (211) to continue expanding; if the maximum number of expansions M is reached, the expansion is completed and the final convex polygon is obtained. That is the convex feasible region at the current moment.

[0030] Furthermore, the step 203 constructs a dynamic prediction time domain process, including:

[0031] The obstacle rate around the unmanned autonomous platform is measured according to the feasible area to determine the current prediction time domain;

[0032] Assume that the feasible area at the current moment is S k , the maximum feasible area when there are no obstacles is S max , area ratio r = S k / S max , the relationship between the predicted time domain and the feasible area ratio can be expressed as follows:

[0033]

[0034] The step 204 constructs a dynamic optimization index process, including:

[0035] Let the centroid of the convex feasible region be O k , coordinate O(k)=(x O (k),y O (k)), the target point position is T, the coordinate T = (xT ,y T ), The midpoint is V k , coordinate V(k)=(x V (k),y V (k)); D k is the current position of the unmanned autonomous platform. When there is no obstacle within the detection range of the laser radar, O k With D k coincides, it can be seen that when an obstacle is detected, O k With D k Separated and approaching the obstacle, and The angle is smaller than the angle when moving away from the obstacle;

[0036] make and The angle is λ, then:

[0037]

[0038] Where: Set the constant λ d ∈[0,π / 2], and use it as the basis for judging whether the unmanned autonomous platform is close to or away from obstacles; if λ is less than λ d , then the unmanned autonomous platform is considered to be approaching an obstacle; if λ is greater than λ d , it is considered that the unmanned autonomous platform is moving away from obstacles;

[0039] Let R 1 With R 2 are the mass center ranges of the unmanned autonomous platform approaching and moving away from obstacles, and R 1 <R 2 ; When the unmanned autonomous platform approaches an obstacle, if Less than R 1 , it is considered that the obstacle has not been completely approached, and the optimization index considers the best ability-time combination, that is:

[0040]

[0041] Q and P represent quadratic weight matrices, where:

[0042]

[0043] if Greater than R 1 , it is believed that the classical optimization index cannot complete the obstacle avoidance task. At this time, the center of mass effect is included in the objective function to help the unmanned autonomous platform avoid obstacles. In order to avoid the situation where the center of mass effect and the target point effect reach a balanced state and the unmanned autonomous platform wanders in place, the partial center of mass H is introduced k, coordinate H(k)=(x H (k),y H (k)), such that and k 2 is the deviation coefficient, which can be given according to the complexity of the obstacle; where:

[0044] The centroid deviation can be divided into left deviation and right deviation. The unmanned autonomous platform will traverse the obstacle point cloud at the current moment and divide the point cloud group according to the continuity of the point cloud and the sudden change of the radar distance. After traversing all the groups, the point cloud group with the smallest average distance from the unmanned autonomous platform is selected. Taking the current heading of the unmanned autonomous platform as the positive direction, the vector composed of the current position of the unmanned autonomous platform and the left and right boundary points of the point cloud group is calculated respectively, and the vector Angle; select the boundary point of the point cloud group with a smaller angle. If the point is on the left, select the left deviation as the centroid; if it is on the right, select the right deviation as the centroid;

[0045] After obtaining the eccentric centroid position, update the optimization index:

[0046]

[0047] Among them, W 1 is a quadratic weight matrix, and:

[0048]

[0049] When the unmanned autonomous platform is away from obstacles, if Less than R 2 , it is considered that the obstacle has been removed, and the optimization index considers the best combination of ability and time, that is:

[0050]

[0051] if Greater than R 2 , it is considered that the unmanned autonomous platform has not completely escaped from the obstacle. At this time, the center of mass effect and the target point effect will not reach a state of equilibrium. The center of mass effect is directly considered and the optimization index is updated:

[0052]

[0053] Where: W 2 is a quadratic weight matrix, and:

[0054]

[0055] Based on the above, the kinematic state constraints are:

[0056]

[0057] The present invention can also be implemented by using an online two-dimensional obstacle avoidance trajectory planning method, comprising the following steps:

[0058] S1, construct the kinematic model of the unmanned autonomous platform;

[0059] S2, detects the surrounding unknown environment information through two-dimensional laser radar, obtains obstacle point cloud data and constructs the motion trajectory safety area; including:

[0060] 201. Based on the obstacle point cloud data, with the unmanned autonomous platform as the center, a convex feasible domain is generated according to the given expansion rules;

[0061] 202. Obtain dynamic linear obstacle avoidance constraints according to the position and shape of the convex feasible region at each moment;

[0062] 203. Obtain the dynamic prediction time domain according to the convex feasible region area;

[0063] 204. Construct a dynamic optimization index through the positional relationship between the unmanned autonomous platform, the target point, and the centroid of the convex feasible domain;

[0064] S3, construct state quantity constraints and control quantity constraints according to the kinematic model;

[0065] S4, according to the dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, dynamic optimization indicators, and dynamic prediction time domain, rolling optimization is used to output the real-time obstacle avoidance planning trajectory of the online unmanned autonomous platform, that is:

[0066] minJ(k)

[0067] xT min ≤X(k+j|k)≤X max

[0068] U min ≤U(k+j|k)≤U max

[0069] E k x(k)+F k y(k)≤G k

[0070]

[0071] j=1,…,N k

[0072] Where: X min , X max They represent the maximum and minimum values ​​of the state quantity of the unmanned autonomous platform, U min , U max They represent the maximum and minimum values ​​of the control quantity of the unmanned autonomous platform, E k、F k , G k They are all N×1 vectors and change dynamically with the different convex feasible domains. is the current position sequence of the unmanned autonomous platform, is the target point position sequence, is the centroid position sequence of the convex feasible region, is the eccentric centroid sequence; N k is the dynamic prediction time domain; R 1 With R 2 are the mass center ranges of the unmanned autonomous platform approaching and moving away from obstacles, and R 1 <R 2 , r is the ratio of the convex feasible region area.

[0073] Beneficial Effects

[0074] 1. After the convex feasible domain is initialized, the convex feasible domain is expanded at a fixed ratio. As the number of expansions increases, the distance difference of each expansion increases, which ensures the safety of the convex feasible domain while improving the expansion speed. Considering the objective size and obstacle avoidance margin of the unmanned autonomous platform, the safety distance is added to the expansion of the convex feasible domain of the unmanned autonomous platform, further improving the safety of the convex feasible domain. The dynamic linear obstacle avoidance constraints of the unmanned autonomous platform obtained based on the convex feasible domain are not only applicable to convex obstacles, but also to non-convex obstacles within the radar detection range.

[0075] 2. According to the positional relationship between the unmanned autonomous platform, the centroid of the feasible domain, and the target point, determine whether the unmanned autonomous platform is close to or far away from the obstacle. When close to the obstacle, set a smaller centroid range to increase the success rate of obstacle avoidance. When far away from the obstacle, set a larger centroid range to accelerate the unmanned autonomous platform to reach the target point.

[0076] 3. Combine the prediction time domain with the obstacle rate of the surrounding environment. If an obstacle is detected and the obstacle rate is larger, the convex feasible domain area is smaller. In this case, increasing the prediction time domain can improve the obstacle avoidance success rate. If no obstacle is detected, reducing the prediction time domain can improve the algorithm running speed.

[0077] 4. In order to improve the shortcoming of low success rate of obstacle avoidance in the classic convex feasible domain, the role of the center of mass or eccentric center of mass is introduced into the index function, and the index function is dynamically updated according to different scenarios, so that the unmanned autonomous platform can approach the center of mass or eccentric center of mass of the feasible domain when approaching the target point, thus achieving a trade-off between obstacle avoidance and reaching the target point. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 The present invention provides an online two-dimensional obstacle avoidance trajectory planning system and a flow chart of its method.

[0079] Figure 2Schematic diagram of a two-dimensional radar in an online two-dimensional obstacle avoidance trajectory planning system and method of the present invention.

[0080] Figure 3 A schematic diagram of convex feasible domain generation in an online two-dimensional obstacle avoidance trajectory planning system and method thereof of the present invention.

[0081] Figure 4 A schematic diagram of the change in the area of ​​the convex feasible domain in an online two-dimensional obstacle avoidance trajectory planning system and method of the present invention.

[0082] Figure 5 A schematic diagram of the center of mass and eccentric center of mass in an online two-dimensional obstacle avoidance trajectory planning system and method thereof of the present invention.

[0083] Figure 6 The present invention discloses an online two-dimensional obstacle avoidance trajectory planning system and a simulation result in a method thereof. DETAILED DESCRIPTION

[0084] The following is combined with Figure 1 ~Attached Figure 6 The present invention is described in detail:

[0085] The present invention designs an online two-dimensional obstacle avoidance trajectory planning system and method thereof, such as Figure 1 As shown. The trajectory planning system includes a kinematic model, a two-dimensional laser radar, a state quantity constraint, a control quantity constraint and a motion trajectory safety area; the motion trajectory safety area is composed of a convex feasible domain, a dynamic linear obstacle avoidance constraint, a dynamic optimization index, and a dynamic prediction time domain; wherein: the unmanned autonomous platform is equipped with a two-dimensional laser radar to detect unknown obstacles in the surrounding area in real time, and combines the current state of the unmanned autonomous platform with the obstacle point cloud position, generates a convex feasible domain according to a given expansion rule, and obtains the dynamic linear obstacle avoidance constraint of the unmanned autonomous platform; in the expansion rule, proportional expansion is used as the principle to achieve the unity of safety and rapidity, and considers the objective size and obstacle avoidance margin of the unmanned autonomous platform; in order to balance the contradiction between obstacle avoidance and computational amount, the present invention combines the prediction time domain with the obstacle rate of the surrounding environment, and obtains the dynamic prediction time domain according to the size of the convex feasible domain area at the current moment. In order to improve the success rate of obstacle avoidance, according to the positional relationship between the centroid of the convex feasible domain, the unmanned autonomous platform, and the target point, the role of the centroid or eccentric centroid is taken into account, and the optimization index is dynamically updated. Considering dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, unmanned autonomous platform model constraints, etc., a rolling optimization problem is established and solved to obtain the real-time obstacle avoidance trajectory of the unmanned autonomous platform.

[0086] The kinematic model takes the center of the unmanned autonomous platform as the reference point and considers the two-dimensional plane to obtain the kinematic model of the unmanned autonomous platform:

[0087]

[0088] Among them, x and y are the two-dimensional positions of the unmanned autonomous platform. is the yaw angle, v is the total velocity of the unmanned autonomous platform, and ω is the yaw angular velocity. Control quantity U=[v,ω] T . Discretize the above model:

[0089]

[0090] Where △T is the sampling time. The above discrete model is further written as a discrete state space equation:

[0091] X(k+1)=A k X(k)+B k U(k) (3)

[0092] Among them A k , B k is the Jacobian matrix, expressed as:

[0093]

[0094] The two-dimensional laser radar detects the surrounding environment. Figure 2 As shown, when the unmanned autonomous platform moves, the two-dimensional laser radar emits radar rays in all directions at a fixed frequency. When it encounters an unknown obstacle, it is reflected and received by the radar receiver to obtain obstacle point cloud data.

[0095] The motion trajectory safety domain is constructed according to the current state of the unmanned autonomous platform and the position of the obstacle point cloud; the motion trajectory safety domain is a convex feasible domain, that is, with the unmanned autonomous platform as the center, a maximum convex polygon without obstacle point cloud is obtained by expansion according to given rules. This convex polygon is the convex feasible domain, and movement in this area is considered absolutely safe. In the expansion rule, proportional expansion is used as the principle to achieve the unity of safety and speed, and the safety distance of the unmanned autonomous platform is considered. Among them:

[0096] Assume that the current position of the unmanned autonomous platform is D k , its coordinates D(k)=[x D (k),y D (k)] T , the number of polygon edges is N, and the maximum number of expansions is M. First, initialize the regular polygon, that is, use the unmanned autonomous platform D k Generate a regular N-gon as the center And the distance between each endpoint and the unmanned autonomous platform is s, that is, D k P 1 1The angle with the positive direction of the X-axis is β. Considering the inertia factor of the unmanned autonomous platform during movement, a certain safety distance should be set for real-time obstacle avoidance. Assume that the radius of the circumscribed circle of the unmanned autonomous platform is L 1 , the obstacle avoidance margin is L 2 , then the safe distance of the convex feasible region extension is L s =L 1 +L 2 In the process of expanding the convex feasible domain, the first few expansions are directly related to whether the obstacle avoidance can be successful, so the expansion amplitude should be smaller; the subsequent expansions can increase the expansion amplitude and speed up the overall expansion speed. Therefore, the present invention adopts the principle of proportional expansion, and sets the expansion coefficient k 1 , the expansion times are initialized to 1, and the expansion rules are as follows:

[0097] 210. D k P 1 1 Expand to D k P 1 2 , so that D k P 1 2 =k 1 D k P 1 1 ; Considering the safety distance of the unmanned autonomous platform, expand D k P 1 2 Distance makes Determine N-gon Whether the following two conditions are met at the same time: (1) the polygon is convex; (2) the polygon does not contain obstacle point clouds. If both conditions are met, the expanded P is retained. 1 2 ; If any of the conditions is not met, then let P 1 2 =P 1 1 ;

[0098] 211. Expand the remaining endpoints according to step 210 Get a new N-gon This expansion is completed, and the number of expansions increases by 1;

[0099] 212. Determine whether the number of expansions is less than the maximum number of expansions M. If so, use a new N-gon To expand the benchmark, repeat steps (210) and (211) to continue expanding; if the maximum number of expansions M is reached, the expansion is completed and the final convex polygon is obtained. That is, the convex feasible region at the current moment.

[0100] Extension examples such as Figure 3 As shown, the number of edges in the convex feasible region is N = 7, the maximum number of expansions is M = 5, and the expansion coefficient is k 1 =1.65. Initialize the regular polygon to P 1 1 P 1 2 P 1 3 P 1 4 P 1 5 P 1 6 P 1 7 , and each endpoint is s = 1.5 away from the center of the unmanned autonomous platform, and the angle β = 0. After 4 expansions, P 1 3 With P 1 4 , P 1 5 Overlap, P 2 3 With P 2 4 , P 2 5 Overlap, P 3 2 With P 3 3 , P 3 4 , P 3 5 coincide, and Overlap, P 5 4 With P 5 5 coincides, the feasible domain is finally That is to say

[0101] After obtaining the convex feasible domain, in order to balance the contradiction between obstacle avoidance and computational complexity, the dynamic prediction time domain is obtained according to the size of the convex feasible domain. In order to improve the success rate of obstacle avoidance, the optimization index is dynamically updated according to the positional relationship between the centroid of the convex feasible domain, the unmanned autonomous platform, and the target point. Considering dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, and unmanned autonomous platform model constraints, a rolling optimization problem is established and solved to obtain the real-time obstacle avoidance trajectory of the unmanned autonomous platform.

[0102] The obtained convex feasible domain is an absolutely safe area for the operation of the unmanned autonomous platform. The unmanned autonomous platform can perform online trajectory planning in the convex feasible domain at the current moment to meet the task of real-time obstacle avoidance. According to the endpoint position of the convex feasible domain, the linear description of the area can be obtained, that is, the dynamic linear obstacle avoidance constraint:

[0103] E k x(k)+F k y(k)+G k ≤0 (4)

[0104] Where E k 、F k , G k They are all N×1 vectors, which change dynamically with the different convex feasible domains. Since the generation of the convex feasible domain is based on the unmanned autonomous platform, the obtained dynamic linear obstacle avoidance constraints are not only applicable to convex obstacles, but also to non-convex obstacles within the radar detection range.

[0105] The present invention is based on the classical model predictive control and implements rolling optimization of variable prediction time domain and variable index function. Assuming that the prediction time domain is the dynamic change amount N k , the control time domain is a constant N c , and N c <N k X(k+j|k) represents the state of the unmanned autonomous platform predicted at time k+j, and U(k+j|k) represents the control amount of the unmanned autonomous platform applied at time k. The initial state of the unmanned autonomous platform is X 0 , the target state is X s .

[0106] At time k, U(k-1) is known, and the control increment △U(k+j|k)=U(k+j|k)-U(k+j-1|k), then:

[0107]

[0108] Where j = 1, 2, ..., N c By iterating the discrete state space equation of the unmanned autonomous platform, the future prediction expression of the motion state of the unmanned autonomous platform can be obtained:

[0109]

[0110] Where j = 1, 2, ..., N k , when N c <j≤N k When U(k+j-1|k)=U(k+N c -1|k). Combining the above two sets of iterative equations, the total prediction equation can be obtained:

[0111]

[0112] The huge amount of calculation of rolling optimization limits its practical application on unmanned autonomous platforms. In order to reduce the real-time calculation burden, the present invention adopts a variable prediction time domain. When there are no obstacles around the unmanned autonomous platform, the environment is safe and the trajectory planning difficulty is small. A smaller prediction time domain can be selected to reduce the amount of calculation. When the laser radar detects an obstacle, the difficulty of trajectory planning increases. At this time, the number of prediction steps is appropriately increased according to the surrounding obstacle rate.

[0113] analyze Figure 4 From the obstacle avoidance process of the unmanned autonomous platform, we can see that the smaller the obstacle rate around the unmanned autonomous platform, the larger the feasible domain area; the larger the obstacle rate, the smaller the feasible domain area. Therefore, the obstacle rate around the unmanned autonomous platform can be measured according to the feasible domain area, and the prediction time domain at the current moment can be further determined. Assume that the feasible domain area at the current moment is S k , the maximum feasible area when there are no obstacles is S max , area ratio r = S k / S max , the relationship between the predicted time domain and the feasible area ratio can be expressed as follows:

[0114]

[0115] The implementation of variable prediction time domain reduces the computational burden of the unmanned autonomous platform and increases the possibility of practical application of rolling optimization. When designing the index function, only considering the distance from the unmanned autonomous platform to the target point cannot meet the needs of real-time obstacle avoidance in an unknown environment. This simplifies the model establishment, but also makes it unable to meet the obstacle avoidance needs of multiple scenarios. The dynamic linear obstacle avoidance constraint formed by the convex feasible domain can only ensure that the unmanned autonomous platform does not collide with obstacles, and cannot ensure that the unmanned autonomous platform bypasses obstacles in complex situations. Therefore, it is necessary to improve the index function to improve the success rate of obstacle avoidance. Analysis shows that when the unmanned autonomous platform approaches an obstacle, the position of the unmanned autonomous platform will be separated from the centroid position of the convex feasible domain, and the closer to the obstacle, the greater the separation. Based on this, the index function can be dynamically updated according to the positional relationship between the unmanned autonomous platform, the centroid of the convex feasible domain, and the target point, so that the unmanned autonomous platform moves towards the target point while trying to get close to the centroid of the convex feasible domain.

[0116] Assume that the centroid of the convex feasible region is O k , coordinate O(k)=(x O (k),y O (k)), the target point position is T, the coordinate T = (x T ,y T ), The midpoint is V k , coordinate V(k)=(x V (k),y V (k)). When there is no obstacle within the laser radar detection range, Ok With D k When an obstacle is detected, it is known that O k With D k Separated and approaching the obstacle, and The angle is smaller than the angle when moving away from the obstacle. and The angle is λ, then:

[0117]

[0118] Set the constant λ d ∈[0,π / 2], and use it as the basis for judging whether the unmanned autonomous platform is close to or away from obstacles. d , then the unmanned autonomous platform is considered to be approaching an obstacle; if λ is greater than λ d , it is considered that the unmanned autonomous platform is moving away from obstacles. When approaching obstacles, setting a smaller center of mass range can increase the success rate of obstacle avoidance; when far away from obstacles, setting a larger center of mass range can accelerate the unmanned autonomous platform to reach the target point. Assume R 1 With R 2 are the mass center ranges of the unmanned autonomous platform approaching and moving away from obstacles, and R 1 <R 2 When the unmanned autonomous platform approaches an obstacle, if Less than R 1 , it is considered that the obstacle has not been completely approached, and the optimization index considers the best ability-time combination, that is:

[0119]

[0120] Q and P represent quadratic weight matrices, where:

[0121]

[0122] if Greater than R 1 , it is believed that the classical optimization index cannot complete the obstacle avoidance task. At this time, the center of mass effect is included in the objective function to help the unmanned autonomous platform avoid obstacles. In order to avoid the situation where the center of mass effect and the target point effect reach a state of equilibrium and the unmanned autonomous platform wanders in place, the eccentric center of mass H is introduced k , coordinate H(k)=(x H (k),y H (k)), such that and k 2 is the deviation coefficient, which can be given according to the complexity of the obstacle.

[0123] The centroid can be left or right, which directly affects the obstacle avoidance direction of the unmanned autonomous platform. It can be determined by the point cloud distribution of the obstacle at the current moment. Specifically, the unmanned autonomous platform will traverse the obstacle point cloud at the current moment and divide the point cloud groups according to the continuity of the point cloud and the sudden change of the radar distance. Traverse all groups and select the point cloud group with the smallest average distance from the unmanned autonomous platform. Take the current heading of the unmanned autonomous platform as the positive direction, calculate the vector composed of the current position of the unmanned autonomous platform and the left and right boundary points of the point cloud group, and add them to the vector Angle. Select the boundary point of the point cloud group with the smaller angle. If the point is on the left, select the left deviation for the centroid; if it is on the right, select the right deviation for the centroid.

[0124] After obtaining the eccentric centroid position, update the optimization index:

[0125]

[0126] Among them, W 1 is a quadratic weight matrix, and:

[0127]

[0128] When the unmanned autonomous platform is away from obstacles, if Less than R 2 , it is considered that the obstacle has been removed, and the optimization index considers the best combination of ability and time, that is:

[0129]

[0130] if Greater than R 2 , it is considered that the unmanned autonomous platform has not completely escaped from the obstacle, and the center of mass effect and the target point effect will not reach a state of equilibrium at this time. The center of mass effect is directly considered and the optimization index is updated:

[0131]

[0132] Where W 2 is a quadratic weight matrix, and:

[0133]

[0134] Based on the above, the dynamic optimization index is:

[0135]

[0136] Schematic diagram as Figure 5As shown in the figure, when approaching an obstacle, the position of the unmanned autonomous platform is separated from the centroid of the convex feasible domain. At this time, the eccentric centroid deviates to the right, and the unmanned autonomous platform moves under the joint action of the eccentric centroid and the target point. When moving away from the obstacle, the unmanned autonomous platform moves under the joint action of the centroid and the target point. After completely leaving the obstacle, the positions of the centroid, eccentric centroid, and unmanned autonomous platform coincide, and the unmanned autonomous platform moves under the action of the target point.

[0137] In the process of trajectory planning, the unmanned autonomous platform constructs state quantity constraints and control quantity constraints according to the following constraints to ensure the safety and smoothness of the generated trajectory:

[0138]

[0139] Where X min , X max They represent the maximum and minimum values ​​of the state quantity of the unmanned autonomous platform, U min , U max They represent the maximum and minimum values ​​of the control quantity of the unmanned autonomous platform respectively. Combining equations (4), (7), (8), (14), and (15), the following rolling optimization model is obtained:

[0140]

[0141] By solving the following rolling optimization model, we can obtain the trajectory of the unmanned autonomous platform to avoid obstacles in real time.

[0142] The feasibility of the proposed algorithm is verified by simulation experiments. In the simulation experiment, the initial state of the unmanned autonomous platform is X 0 =[0,-10,π / 2] T , the target state is X s =[0,40,π / 2] T , the number of edges of the convex feasible region is N = 7, the maximum number of expansions is M = 6, and the safety distance is L s =0.25,λ d =5π / 36, the range of the center of mass R 1 =1.2, R 2 =2.5. State quantity constraint X min =[-20,-50,-π] T , X max =[20,50,π] T , control quantity constraint U min =[-2,-3.5] T , U max =[2,3.5] T ,The gray area represents unknown obstacles.

[0143] The results are as follows Figure 6As shown in the figure, in an environment with multiple types of unknown obstacles, the unmanned autonomous platform plans a smooth trajectory from the starting point to the target point online based on the convex feasible domain and avoids obstacles, and the yaw angle, combined velocity, and yaw angular velocity all meet the constraints. Facing a non-convex obstacle group consisting of three regular obstacles, the unmanned autonomous platform can also detect the non-convex surface and avoid the non-convex obstacle group in time.

Claims

1. An online two-dimensional obstacle avoidance trajectory planning system, It is characterized in that The trajectory planning system includes a kinematic model, a two-dimensional laser radar, state quantity constraints, control quantity constraints and a motion trajectory safety area; the motion trajectory safety area is composed of a convex feasible domain, a dynamic linear obstacle avoidance constraint, a dynamic optimization index, and a dynamic prediction time domain; wherein: Step 1: Obstacle point cloud data is obtained by detecting the surrounding unknown environment information through a two-dimensional laser radar; Step 2: Construct a safe area for the motion trajectory, including:

201. Based on the obstacle point cloud data, the convex feasible domain is generated according to the expansion rule with the unmanned autonomous platform as the center; 202. According to the position and shape of the convex feasible region at each moment, the dynamic linear obstacle avoidance constraint is obtained according to the following formula: E k x(k)+F k y(k)+G k ≤0 Where E k 、F k , G k They are all N×1 vectors, which change dynamically with the different convex feasible regions; 203. According to the area of ​​the convex feasible region, a dynamic prediction time domain is obtained by construction; 204. Construct a dynamic optimization index through the positional relationship between the unmanned autonomous platform, the target point, and the centroid of the convex feasible domain; Step 3: According to the kinematic model, the state quantity constraints and control quantity constraints are constructed according to the following formulas: Where: X min , X max They represent the maximum and minimum values ​​of the state quantity of the unmanned autonomous platform, U min , U max Respectively represent the maximum and minimum values ​​of the control quantity of the unmanned autonomous platform; N k For dynamic prediction time domain; Step 4: According to the dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, dynamic optimization indicators and dynamic prediction time domain, rolling optimization is used to output the real-time obstacle avoidance planning trajectory of the online unmanned autonomous platform.

2. An online two-dimensional obstacle avoidance trajectory planning system according to claim 1, It is characterized in that The process of generating a convex feasible region in step 201 includes: Unmanned autonomous platform D k Generate a regular N-gon as the center And the distance between each endpoint and the unmanned autonomous platform is s, that is, D k P 1 1 The angle with the positive direction of the X axis is β; where: the current position of the unmanned autonomous platform is at D k , its coordinates D(k)=[x D (k),y D (k)] T , the number of polygon edges is N, and the maximum number of expansions is M; Let the radius of the circumscribed circle of the unmanned autonomous platform be L 1 , the obstacle avoidance margin is L 2 , then the safe distance of the convex feasible region extension is L s =L 1 +L 2 ; According to the principle of proportional expansion, let the expansion factor be k 1 , the expansion times are initialized to 1, and the expansion rules are as follows:

210. D k 1 1 Expand to D k P 1 2 , so that D k P 1 2 =k 1 D k P 1 1 ; Considering the safety distance of the unmanned autonomous platform, expand D k P 1 2 Distance makes Determine N-gon Whether the following two conditions are met at the same time: (1) the polygon is convex; (2) the polygon does not contain obstacle point clouds. If both conditions are met, the expanded P is retained. 1 2 ; If any of the conditions is not met, then let P 1 2 =P 1 1 ; 211. Expand the remaining endpoints according to step 210 Get a new N-gon This expansion is completed, and the number of expansions increases by 1; 212. Determine whether the number of expansions is less than the maximum number of expansions M. If so, use a new N-gon To expand the benchmark, repeat steps (210) and (211) to continue expanding; if the maximum number of expansions M is reached, the expansion is completed and the final convex polygon is obtained. That is, the convex feasible region at the current moment.

3. An online two-dimensional obstacle avoidance trajectory planning system according to claim 1, It is characterized in that The step 203 constructs a dynamic prediction time domain process, including: The obstacle rate around the unmanned autonomous platform is measured according to the feasible area to determine the current prediction time domain; Assume that the feasible region area at the current moment is S k , the maximum feasible area when there are no obstacles is S max , area ratio r = S k / S max , the relationship between the predicted time domain and the feasible area ratio can be expressed as follows:

4. The online two-dimensional obstacle avoidance trajectory planning system according to claim 1, It is characterized in that The step 204 constructs a dynamic optimization index process, including: Let the centroid of the convex feasible region be O k , coordinate O(k)=(x O (k),y O (k)), the target point position is T, the coordinate T = (x T ,y T ), The midpoint is V k , coordinate V(k)=(x V (k),y V (k)); D k is the current position of the unmanned autonomous platform. When there is no obstacle within the detection range of the laser radar, O k With D k coincides, it can be seen that when an obstacle is detected, O k With D k Separated and approaching the obstacle, and The angle is smaller than the angle when moving away from the obstacle; make and The angle is λ, then: Where: Set the constant λ d ∈[0,π / 2], and use it as the basis for judging whether the unmanned autonomous platform is close to or away from obstacles; if λ is less than λ d , then the unmanned autonomous platform is considered to be approaching an obstacle; if λ is greater than λ d , it is considered that the unmanned autonomous platform is moving away from obstacles; Let R 1 With R 2 are the mass center ranges of the unmanned autonomous platform approaching and moving away from obstacles, and R 1 <R 2 ; When the unmanned autonomous platform approaches an obstacle, if Less than R 1 , it is considered that the obstacle has not been completely approached, and the optimization index considers the best ability-time combination, that is: Q and P represent quadratic weight matrices, where: if Greater than R 1 , it is believed that the classical optimization index cannot complete the obstacle avoidance task. At this time, the center of mass effect is included in the objective function to help the unmanned autonomous platform avoid obstacles. In order to avoid the situation where the center of mass effect and the target point effect reach a balanced state and the unmanned autonomous platform wanders in place, the partial center of mass H is introduced k , coordinate H(k)=(x H (k),y H (k)), such that and k 2 is the deviation coefficient, which can be given according to the complexity of the obstacle; where: The centroid deviation can be divided into left deviation and right deviation. The unmanned autonomous platform will traverse the obstacle point cloud at the current moment and divide the point cloud group according to the continuity of the point cloud and the sudden change of the radar distance. After traversing all the groups, the point cloud group with the smallest average distance from the unmanned autonomous platform is selected. Taking the current heading of the unmanned autonomous platform as the positive direction, the vector composed of the current position of the unmanned autonomous platform and the left and right boundary points of the point cloud group is calculated respectively, and the vector Angle; select the boundary point of the point cloud group with a smaller angle. If the point is on the left, select the left deviation as the centroid; if it is on the right, select the right deviation as the centroid; After obtaining the eccentric centroid position, update the optimization index: Among them, W 1 is a quadratic weight matrix, and: When the unmanned autonomous platform is away from obstacles, if Less than R 2 , it is considered that the obstacle has been removed, and the optimization index considers the best combination of ability and time, that is: if Greater than R 2 , it is considered that the unmanned autonomous platform has not completely escaped from the obstacle. At this time, the center of mass effect and the target point effect will not reach a state of equilibrium. The center of mass effect is directly considered and the optimization index is updated: Where: W 2 is a quadratic weight matrix, and: Based on the above, the kinematic state constraints are:

5. An online two-dimensional obstacle avoidance trajectory planning method, It is characterized in that The method adopts the system of claims 1-4 to realize the real-time obstacle avoidance and trajectory planning of the unmanned autonomous platform, comprising the following steps: S1, construct the kinematic model of the unmanned autonomous platform; S2, detects the surrounding unknown environment information through two-dimensional laser radar, obtains obstacle point cloud data and constructs the motion trajectory safety area; including:

201. Based on the obstacle point cloud data, with the unmanned autonomous platform as the center, a convex feasible domain is generated according to the given expansion rules; 202. Obtain dynamic linear obstacle avoidance constraints according to the position and shape of the convex feasible region at each moment; 203. Obtain the dynamic prediction time domain according to the convex feasible region area; 204. Construct a dynamic optimization index through the positional relationship between the unmanned autonomous platform, the target point, and the centroid of the convex feasible domain; S3, construct state quantity constraints and control quantity constraints according to the kinematic model; S4, according to the dynamic linear obstacle avoidance constraints, state quantity constraints, control quantity constraints, dynamic optimization indicators, and dynamic prediction time domain, rolling optimization is used to output the real-time obstacle avoidance planning trajectory of the online unmanned autonomous platform, that is: minJ(k) s.t.X min ≤X(k+j|k)≤X max U min ≤U(k+j|k)≤U max E k x(k)+F k y(k)≤G k j=1,…,N k Where: X min , X max They represent the maximum and minimum values ​​of the state quantity of the unmanned autonomous platform, U min , U max They represent the maximum and minimum values ​​of the control quantity of the unmanned autonomous platform, E k 、F k , G k They are all N×1 vectors and change dynamically with the different convex feasible domains. is the current position sequence of the unmanned autonomous platform, is the target point position sequence, is the centroid position sequence of the convex feasible region, is the eccentric centroid sequence; N k is the dynamic prediction time domain; R 1 With R 2 are the mass center ranges of the unmanned autonomous platform approaching and moving away from obstacles, and R 1 <R 2 , r is the ratio of the convex feasible region area.

Images