CN116184993A - Path planning method based on Hybrid Astar and space corridor - Google Patents

Abstract

The invention discloses a path planning method based on Hybrid Astar and a space corridor, which comprises the steps of constructing a global occupation grid map in a multi-obstacle environment and setting coordinates of a starting point and a terminal point of a task; performing path planning based on Hybrid Astar according to the constructed global occupation grid map to obtain a planned initial path; expanding the road points to the barriers according to the obtained initial paths to obtain a plurality of rectangular spaces; the number of the obtained rectangular spaces is simplified, and a space corridor is formed; and performing secondary planning on the initial path in the space corridor through the optimal control model to obtain an optimized path. According to the invention, path planning is carried out under the multi-obstacle scene, a space corridor is introduced when an initial path is optimized, an optimal control model is established to obtain an optimal path which is positioned in the barrier-free space corridor after optimization, the vehicle kinematic constraint is met, the safety of the vehicle is ensured to the greatest extent, and the danger of collision with a nearby obstacle caused by the fluctuation of the optimal path is effectively avoided.

Description

Path planning method based on Hybrid Astar and space corridor

Technical Field

The invention relates to the technical field of path planning, in particular to a path planning method based on Hybrid Astar and a space corridor.

Background

Unmanned vehicles are commonly used in agriculture, transportation, military industry or daily traffic, and in the actual use process, a multi-obstacle environment is unavoidable. The drone classical path planning algorithm includes a series of RRTs based on sampling, a series such as a based on searching, a polynomial based and an optimal control based. In practical application, the path requirements are not only safe and free of barriers, but also smooth and comfortable, so that the initial result path is smoothed by adopting a polynomial or spline interpolation method and the like after the initial planning result is obtained. However, in a multi-obstacle environment, even if the initial result is to ensure that no obstacle is encountered, the smoothed curve may hit an obstacle at a corner or a stenosis due to the fluctuation. Or when designing the initial path under the obstacle avoidance situation, in order to obtain the result with lower path cost, planning a path closer to the obstacle, smoothing the initial path by Bezier at the moment, and if the result with larger fluctuation is generated, increasing the possibility of obstacle collision. And for Bezier, a new road point is selected, and the optimization of the paths at the front and rear sections is difficult to splice.

Disclosure of Invention

The invention aims to overcome the defects in the prior art, and adopts a path planning method based on Hybrid Astar and a space corridor to solve the problems in the background art.

A path planning method based on Hybrid Astar and space corridor comprises the following steps:

constructing a global occupation grid map in a multi-obstacle environment, and setting coordinates of a starting point and a terminal point of a task;

performing path planning based on Hybrid Astar according to the constructed global occupation grid map to obtain a planned initial path;

expanding the road points to the barriers according to the obtained initial paths to obtain a plurality of rectangular spaces;

the number of the obtained rectangular spaces is simplified, and a space corridor is formed;

and performing secondary planning on the initial path in the space corridor through the optimal control model to obtain an optimized path.

As a further aspect of the invention: the specific steps of constructing a global occupation grid map in a multi-obstacle environment and setting coordinates of a start point and an end point of a task include:

self-positioning is carried out by utilizing positioning equipment GPS to obtain a starting point coordinate, and a task end point GPS coordinate is obtained;

simultaneously, the obstacle around the current position of the vehicle is acquired through the environment sensing system of the autonomous unmanned vehicle, so that a local occupied grid map is obtained, and the local occupied grid map is updated continuously;

the local occupancy grid map is mapped into the constructed global occupancy grid map by coordinate transformation.

As a further aspect of the invention: the specific steps of carrying out path planning based on Hybrid Astar according to the constructed global occupation grid map to obtain a planned initial path include:

acquiring a starting point and an end point of a task and constructing a global occupation grid map;

path planning is performed based on Hybrid Astar:

when the distance between the expansion node and the terminal point is nearest, the expansion node is considered to reach the terminal point;

when the Reeds_Sheep is utilized to hit the end point, the Reeds_Sheep road points are contained in the global occupation grid map, and the obstacle cannot be touched after crossing the boundary, otherwise, the Hybrid Astar is continuously used for path searching.

As a further aspect of the invention: the specific steps of expanding the road points to the barriers according to the obtained initial paths to obtain a plurality of rectangular spaces comprise the following steps:

sequentially traversing the road points of the whole initial path, and initializing four corner points P1, P2, P3 and P4 of the rectangular space box_now according to the coordinates of each road point;

judging whether an obstacle exists on the current road point coordinate, if so, skipping the point, initializing the maximum expansion times, and continuously expanding the P2 and P3 transverse and longitudinal coordinates to the obstacle boundary within the expansion times to obtain the P2 and P3 transverse and longitudinal coordinates;

and assigning the obtained corresponding abscissa of P2 and P3 to P1 and P4 to form a rectangular space corresponding to the current road point.

As a further aspect of the invention: the specific steps of simplifying the number of the obtained rectangular spaces and forming the space corridor comprise:

presetting four corner points as rectangular space box_last of (0, 0);

when the rectangular space box_last and the initialized rectangular space box_now have the same waypoint in the intersection part, but the non-intersection part has no unique waypoint, the covered part is deleted.

As a further aspect of the invention: the method comprises the specific steps of performing secondary planning on an initial path in a space corridor through an optimal control model to obtain an optimized path, wherein the specific steps comprise:

step one, establishing a kinematic model:

vehicle kinematics are first modeled based on a bicycle model:

wherein, the rear axle of the vehicle is terminated by P ₀ (x ₀ ,y ₀ )，θ ₀ Representing heading angle, v ₀ Representing P ₀ Speed of a) ₀ A linear acceleration is indicated and a linear acceleration is indicated,

is the rotation angle of the front wheel, w ₀ For the angular velocity of the front wheel, L _w0 Representing the length of the rear axle of the vehicle;

when t is E [0, t _f ]The state variable boundaries at the time are:

|a ₀ (t)|≤a _max ；

|v ₀ (t)|≤v _max ；

|w ₀ (t)|≤Ω _max ；

wherein a is _max 、v _max 、

Ω _max Representing the upper bound of each variable separately;

step two, setting boundary limit:

according to the sensing system of the vehicle, the moment x is acquired at the initial state t=0 ₀ (0)、y ₀ (0)、v ₀ (0)、

θ ₀ (0)、a ₀ (0) And w ₀ (0) Information for each variable;

at termination time t=t _f The whole vehicle system is stably stopped, and the formula is obtained:

[v ₀ (t _f ),a ₀ (t _f ),w ₀ (t _f )]＝[0,0,0]；

step three, setting space corridor limit:

the initial path contains N _fe Each road point P _c (x _c (t)，y _c (t)) should be confined within a spatial corridor, the formula:

wherein, therein

Abscissa indicating that the waypoint corresponds to the rectangular space P3 point, +.>

Represents the abscissa of the rectangular space P2 point corresponding to the waypoint,/->

Indicates that the road point corresponds to the ordinate of the P3 point of the rectangular space,>

representing the ordinate of the corresponding rectangular space P2 point of the road point;

step four, obtaining a cost function:

the cost function is:

wherein w is ₁ ,w ₂ Equal to or greater than 0 as a cost function, t _f The time from the start point to the end point of travel of the vehicle is represented, the second item represents the change in acceleration of the vehicle, and the third item represents the change in angular velocity;

step five, obtaining an optimal control model according to the combination of the steps one to four;

and solving the model based on the first-order explicit range-Kutta and the interior point method to obtain a safe and barrier-free optimal path limited in the space corridor.

Compared with the prior art, the invention has the following technical effects:

by adopting the technical scheme, the path planning is carried out under the multi-obstacle scene, the space corridor is introduced when the initial path is optimized, the optimal control model is built to obtain the optimal path in the barrier-free space corridor after the optimization, the vehicle kinematic constraint is met, the safety of the vehicle is ensured to the greatest extent, and the danger of collision with the near obstacle caused by the large fluctuation of the optimal path is effectively avoided.

Drawings

The following detailed description of specific embodiments of the invention refers to the accompanying drawings, in which:

FIG. 1 is a schematic diagram of a structure of an embodiment disclosed herein;

FIG. 2 is a 6-way schematic diagram of a Hybrid Astar extension of an embodiment of the present disclosure;

FIG. 3 is a schematic diagram of a distribution of four corner nodes of a rectangular space in accordance with an embodiment of the disclosure;

fig. 4 is an experimental diagram of a multi-obstacle environment according to an embodiment of the present disclosure.

Detailed Description

The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

Referring to fig. 1, in an embodiment of the present invention, a path planning method based on Hybrid Astar and a space corridor includes the following specific steps:

s1, constructing a global occupation grid map in a multi-obstacle environment, and setting coordinates of a starting point and an end point of a task;

in this embodiment, the environment needs to be modeled before path planning is performed, and a global occupancy grid map is established. The accurate real-time occupation of the grid map is the premise of effective planning and is the guarantee of planning the safety path.

And acquiring the current position of the vehicle and surrounding obstacle information by using an environment sensing system of the unmanned vehicle, modeling the surrounding environment, and giving a global occupation grid map.

As shown in fig. 4, an experimental diagram in a multi-obstacle environment is shown, in which a global occupancy grid map of X Y is constructed with the lower left corner as the origin of coordinates, the right corner as the X-axis direction, and the upward direction as the Y-axis direction, where X is the length of the map and Y is the width of the map. Resolution is d x d, d being the side length of the grid. The position, the size and the resolution of the global occupied grid map are preset, and the course angle takes the transverse axis as the starting point and takes the anticlockwise direction as the positive point. And remains unchanged throughout the planning process.

Because the global occupation grid map range exceeds the range of the vehicle self-perception system, the real-time local occupation grid map within a certain range around the vehicle is constructed, and the real-time local occupation grid map is projected to the global occupation grid map and is updated continuously;

the whole environment sensing is to obtain obstacle information around the vehicle through sensors such as a laser radar, a millimeter wave radar, a camera, a GPS (global positioning system) and an IMU (inertial measurement unit) mounted on the unmanned vehicle, and to establish a local occupation grid map with the size of Xp in real time by using a laser SLAM (sequential scanning method), wherein Xp is the length of the local occupation grid map in the advancing direction of the vehicle, and Yp is the length of the local occupation grid map in the direction perpendicular to Xp. The local occupancy grid map is mapped into the global occupancy grid map by coordinate transformation after the matching correction.

S2, carrying out path planning based on Hybrid Astar according to the constructed global occupation grid map to obtain a planned initial path;

in this embodiment, the known task start point s, the known task end point e and the global occupation grid map are obtained, and the Hybrid a is used for path planning. When the extension node is relatively close to the end point distance, the end point is considered to be reached. When the reeds_view is used to hit the end point, the present embodiment requires that the reeds_view waypoints must also be contained in the global occupancy grid map, and cannot cross the boundary and touch the obstacle, otherwise, the Hybrid a is used to perform the path search.

The Hybrid a algorithm flow is generally as follows:

marking the starting point as an open state, and adding the starting point into an open set;

when the open set is not empty, continuing to execute the next step, otherwise, returning to the starting point;

the point nPred with the minimum F value is taken out from the open set to start processing;

if nppred is in open state, indicating that it is a point that can be expanded, then continue, otherwise return to that point;

marking nppred status as close and removing from open set;

checking whether the point is a target point, if so, directly returning to the node nppred to indicate the end of the search; otherwise, the search is continued.

Priority is given to hit the target point with reeds_quench. If the Reeds_Sheep method can hit directly and the Reeds_Sheep waypoints do not cross the boundary and have no obstacle, i.e. the search does not need to be performed in Hybrid A, the result nSucc is directly returned.

If the Reeds_Shell misses, creating the next expansion node, and under the condition that reversing is possible, carrying out iterative query in sequence by 6 possible ways, as shown in FIG. 2, wherein the diagram is an expanded 6-direction schematic diagram;

according to the number of the iterative query direction, calculating the attitude value of the extensible node nSucc according to the model, namely presetting the variation of X, Y and course angles on each maneuver according to the turning radius, so as to obtain the specific pose of nSucc;

if the extended node is a node which is in the global occupation grid map range and does not generate collision, the F, G and H values of the extended node are calculated, and the extended node is added into the open set. If the parent node is updated, the G and F values of the child nodes need to be recalculated. Otherwise discard nSucc;

and backtracking the path, and connecting the parent nodes of each node from the end point to obtain an initial path planned by Hybrid A.

The preset turning radius is 6, the front wheel rotation angle is 6.67 Degrees (DEG), and under each maneuvering condition, the changes of transverse, longitudinal and aviation directions are as follows:

dx＝{0.7068582,0.705224,0.705224}；

dy＝{0,-0.0415893,0.0415893}；

dt＝{0,0.1178097,-0.11780097}。

the specific pose calculation model is as follows:

advancing:

xsucc=x+dx [ i ]. Cos (t) -dy [ i ]. Sin (t); ySucc=y+dx [ i ]. Sin (t) -dy [ i ]. Cos (t); tsucc=t+dt [ i ]; i e {1,2,3}, specifically 3 forward maneuver directions in FIG. 2.

And (3) retreating:

xsucc=x-dx [ i-3 ]. Times.cos (t) -dy [ i-3 ]. Times.sin (t); ySucc=y-dx [ i-3 ]. Sin (t) +dy [ i-3 ]. Cos (t); tsucc=t-dt [ i-3]; i e {4,5,6}, specifically 3 reverse maneuver directions in FIG. 2. Wherein x, y and t are pose information of nppred, and the pose information is converted into radian to continue calculation after tSucc is obtained.

The H value consists of three terms:

1. kinematic constrained barrier-free heuristic values. Denoted by max (reeds_sheep distance, euclidean distance);

2. and (3) carrying out planning by using A to obtain a G value when the U-shaped turning and dead road are found. Taking the larger values of 1 and 2 as the H value of the point;

g value has punishment penalty turning when the direction of the vehicle changes, punishment penalty reverse when the direction is opposite, and both can be satisfied simultaneously.

The specific model is g=g+k×dx [0];

wherein dx [0] is a preset value, k×pendytylturn (1.05) when there is a turn, k×pendytylturn (2.0) when there is an opposite direction, and k=1 when there is no change in direction. The F value is the sum of the H value and the G value, and the cost value is the F value.

S3, expanding the road points to the barriers according to the obtained initial paths to obtain a plurality of rectangular spaces;

in this embodiment, the specific steps of obtaining a plurality of rectangular spaces include:

s31, traversing the whole initial path point in sequence:

four corners P1, P2, P3 and P4 of the rectangular space box_now are initialized according to the coordinates of each road point, and as shown in FIG. 3, a distribution diagram of four corner nodes of the rectangular space is illustrated. Judging whether the open circuit point is in box_last, if so, skipping the point, otherwise, executing the next step;

s31, expanding an initialization rectangular space box_now:

first, whether an obstacle exists on the current coordinates of the road point is judged, and if so, the road point is skipped. The maximum expansion number max_flag_iter is initialized to 1000, P2 is extended to the upper right, and P3 is extended to the lower left. Every unit of rise of the P2 ordinate in the expansion times, whether the collision obstacle exists or not or the global occupation grid map range is exceeded is checked from the P3 abscissa to the P2 abscissa. If any, the P2 ordinate drops by one unit and the P2 ordinate expands and pauses. Every time the abscissa of P2 moves by one unit to the right, the ordinate of P2 is checked from the ordinate of P3 to see whether the obstacle is bumped or out of range, if so, the abscissa of P2 moves by one unit to the left, and the expansion of the abscissa of P2 is paused. Every time the ordinate of P3 falls by one unit, the abscissa of P2 is checked from the abscissa of P3, if out of range or obstacle is generated, the ordinate of P3 rises by one unit, and the expansion of the ordinate of P3 is suspended. Every time the P3 abscissa is shifted one unit to the left, the range from the P3 ordinate to the P2 ordinate is checked, wherein if a grid point crosses a border or bumps, the P3 abscissa is shifted one unit to the right and the P3 abscissa expansion is paused. And correspondingly assigning the obtained abscissas P2 and P3 to the abscissas P1 and P4 to form rectangular spaces corresponding to the road points.

S4, simplifying the number of the obtained rectangular spaces and forming a space corridor, wherein the method specifically comprises the following steps:

s41, presetting box_last before iteratively expanding box_now in step S3, all four corner points P1, P2, P3, P4 are initialized to (0, 0). If box_last and box_now have the same waypoints only in the intersection portion and not the intersection portion has no unique waypoints, then it is indicated that a certain rectangular space covers another, and the covered portion may be deleted.

I.e. a flag is set, and the flag is set to 0 if a certain point or a certain segment of point is within the box_last and outside the box_now, and a certain point or a certain segment of point is within the box_now and outside the box_last. It is described that both box_last and box_now have unique waypoints in the respective rectangular spaces, and therefore both need to be preserved;

if a certain point or a certain segment of points are in the box_last and the box_now and no other special points are in the box_now, the flag is set to 1, and the box_now is not added into the box_list at this time; if a certain point or a certain segment of points are in the box_last and the box_now, and the box_last has no specific waypoint, the flag is set to 2, and the box_last is deleted at this time, and the box_now is reserved. After the end of S4, the box_last is updated as required, and if the box_now is reserved, the box_last is updated to box_now.

S42, simplifying the box_list set. Initializing a space set temp as a box_list, recording the first rectangular space in the box_list as old, emptying the box_list, and traversing the whole temp set. If the rectangular space now and the last rectangular space old do not completely coincide, the last rectangular space old is added to the box_list set, and the space now is assigned to the last rectangular space old. When the rectangular space old intersects the last rectangular space in temp, the traversal is exited and the last rectangular space is inserted into box_list. Box_list is now the final usable space corridor.

S5, performing secondary planning on the initial path in the space corridor through an optimal control model to obtain an optimized path, wherein the method comprises the following specific steps of:

s51, establishing a kinematic model:

since tire sideslip is ignored in low speed environments, vehicle kinematics are modeled based on a bicycle model:

wherein, the rear axle of the vehicle is terminated by P ₀ (x ₀ ,y ₀ )，θ ₀ Representing heading angle, v ₀ Representing P ₀ Speed of a) ₀ A linear acceleration is indicated and a linear acceleration is indicated,

is the rotation angle of the front wheel, w ₀ For the angular velocity of the front wheel, L _w0 Indicating the length of the rear axle of the vehicle.

When t is E [0, t _f ]The state variable boundaries at the time are:

|a ₀ (t)|≤a _max ，

|v ₀ (t)|≤v _max ，

|w ₀ (t)|≤Ω _max ，

wherein a is _max 、v _max 、

Ω _max Representing the upper bound of each variable separately.

S52, boundary limitation:

at the initial state t=0 moment, x ₀ (0)、y ₀ (0)、v ₀ (0)、

θ ₀ (0)、a ₀ (0)、w ₀ (0) Each variable may be realistically obtained by the perception system of the vehicle. At the endTime t=t _f The entire vehicle system needs to be stopped stably, so it is necessary to:

[v ₀ (t _f ),a ₀ (t _f ),w ₀ (t _f )]＝[0,0,0]； (2)

s53, space corridor limitation:

the initial path has N _fe Each waypoint, then each waypoint P _c (x _c (t)，y _c (t)) should be within the corridor, i.e. limited:

/>

wherein, the liquid crystal display device comprises a liquid crystal display device,

abscissa indicating that the waypoint corresponds to the rectangular space P3 point, +.>

Abscissa indicating that the waypoint corresponds to a rectangular space P2 point, +.>

Ordinate representing the point of the road corresponding to the rectangular space P3 point,/, is given>

The ordinate of the road point corresponding to the rectangular space P2 point is indicated.

S54, cost function:

the cost function is:

wherein w is ₁ ,w ₂ Equal to or greater than 0 as a cost function, t _f The time from the start point to the end point of travel of the vehicle is represented, the second item describes the change in acceleration of the vehicle, and the third item describes the change in angular velocity. I.e. the planned path is required to be safe and consumedShort time and high comfort.

S55, an optimal control model:

obtaining an optimal control model for describing the path planning task of the whole vehicle system according to formulas (1), (2), (3), and (4);

finally, solving the NLP problem based on the first-order explicit Runge-Kutta and interior point method, and obtaining a safe and barrier-free optimized path limited in a space corridor.

Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the spirit and scope of the invention as defined by the appended claims and their equivalents.

Claims (6)

1. A path planning method based on Hybrid Astar and space corridor is characterized by comprising the following specific steps:

constructing a global occupation grid map in a multi-obstacle environment, and setting coordinates of a starting point and a terminal point of a task;

performing path planning based on Hybrid Astar according to the constructed global occupation grid map to obtain a planned initial path;

expanding the road points to the barriers according to the obtained initial paths to obtain a plurality of rectangular spaces;

the number of the obtained rectangular spaces is simplified, and a space corridor is formed;

and performing secondary planning on the initial path in the space corridor through the optimal control model to obtain an optimized path.

2. The path planning method based on Hybrid Astar and space corridor according to claim 1, wherein the specific steps of constructing a global occupancy grid map in a multi-obstacle environment and setting coordinates of a start point and an end point of a task comprise:

self-positioning is carried out by utilizing positioning equipment GPS to obtain a starting point coordinate, and a task end point GPS coordinate is obtained;

simultaneously, the obstacle around the current position of the vehicle is acquired through the environment sensing system of the autonomous unmanned vehicle, so that a local occupied grid map is obtained, and the local occupied grid map is updated continuously;

the local occupancy grid map is mapped into the constructed global occupancy grid map by coordinate transformation.

3. The method for planning a path based on Hybrid Astar and space corridor according to claim 1, wherein the specific step of planning a path based on Hybrid Astar according to the constructed global occupation grid map to obtain the planned initial path comprises the following steps:

acquiring a starting point and an end point of a task and constructing a global occupation grid map;

path planning is performed based on Hybrid Astar:

when the distance between the expansion node and the terminal point is nearest, the expansion node is considered to reach the terminal point;

when the Reeds_Sheep is utilized to hit the end point, the Reeds_Sheep road points are contained in the global occupation grid map, and the obstacle cannot be touched after crossing the boundary, otherwise, the Hybrid Astar is continuously used for path searching.

4. The path planning method based on Hybrid Astar and space corridor according to claim 1, wherein the specific step of obtaining a plurality of rectangular spaces according to the obtained initial path expansion waypoints to the obstacles comprises the following steps:

sequentially traversing the road points of the whole initial path, and initializing four corner points P1, P2, P3 and P4 of the rectangular space box_now according to the coordinates of each road point;

judging whether an obstacle exists on the current road point coordinate, if so, skipping the point, initializing the maximum expansion times, and continuously expanding the P2 and P3 transverse and longitudinal coordinates to the obstacle boundary within the expansion times to obtain the P2 and P3 transverse and longitudinal coordinates;

and assigning the obtained corresponding abscissa of P2 and P3 to P1 and P4 to form a rectangular space corresponding to the current road point.

5. The method for planning a path based on Hybrid Astar and space corridor according to claim 4, wherein the specific steps of performing quantity simplification on the obtained plurality of rectangular spaces and forming the space corridor comprise:

presetting four corner points as rectangular space box_last of (0, 0);

when the rectangular space box_last and the initialized rectangular space box_now have the same waypoint in the intersection part, but the non-intersection part has no unique waypoint, the covered part is deleted.

6. The path planning method based on Hybrid Astar and space corridor according to claim 1, wherein the specific step of performing quadratic programming on the initial path in the space corridor through the optimal control model to obtain the optimized path comprises the following steps:

step one, establishing a kinematic model:

vehicle kinematics are first modeled based on a bicycle model:

wherein, the rear axle of the vehicle is terminated by P ₀ (x ₀ ,y ₀ )，θ ₀ Representing heading angle, v ₀ Representing P ₀ Speed of a) ₀ A linear acceleration is indicated and a linear acceleration is indicated,

is the rotation angle of the front wheel, w ₀ For the angular velocity of the front wheel, L _w0 Representing the length of the rear axle of the vehicle;

when t is E [0, t _f ]The state variable boundaries at the time are:

|a ₀ (t)|≤a _max ；

|v ₀ (t)|≤v _max ；

|w ₀ (t)|≤Ω _max ；

wherein a is _max 、v _max 、

Ω _max Representing the upper bound of each variable separately;

step two, setting boundary limit:

according to the sensing system of the vehicle, the moment x is acquired at the initial state t=0 ₀ (0)、y ₀ (0)、v ₀ (0)、

θ ₀ (0)、a ₀ (0) And w ₀ (0) Information for each variable;

at termination time t=t _f The whole vehicle system is stably stopped, and the formula is obtained:

[v ₀ (t _f ),a ₀ (t _f ),w ₀ (t _f )]＝[0,0,0]；

step three, setting space corridor limit:

the initial path contains N _fe Each road point P _c (x _c (t)，y _c (t)) should be confined within a spatial corridor, the formula:

wherein, therein

Abscissa indicating that the waypoint corresponds to the rectangular space P3 point, +.>

Represents the abscissa of the rectangular space P2 point corresponding to the waypoint,/->

Indicates that the road point corresponds to the ordinate of the P3 point of the rectangular space,>

representing the ordinate of the corresponding rectangular space P2 point of the road point;

step four, obtaining a cost function:

the cost function is:

wherein w is ₁ ,w ₂ Equal to or greater than 0 as a cost function, t _f The time from the start point to the end point of travel of the vehicle is represented, the second item represents the change in acceleration of the vehicle, and the third item represents the change in angular velocity;

step five, obtaining an optimal control model according to the combination of the steps one to four;

and solving the model based on the first-order explicit range-Kutta and the interior point method to obtain a safe and barrier-free optimal path limited in the space corridor.

Images