A Bezier curve-based path planning method for heavy-duty trucks in ports to avoid obstacles

Abstract

The invention relates to the technical field of automatic driving path planning, and discloses a method for planning an obstacle bypassing path for a heavy-duty truck for a port based on a Bezier curve, comprising the following steps: S1: the vehicle tracks along a reference route, and continuously calculates the distance between the vehicle and the obstacle; S2: determines whether the distance between the vehicle and the obstacle is less than a safety threshold; S3: samples obstacle bypassing control points, and generates an obstacle bypassing route based on a Bezier curve equation; S4: the vehicle tracks along the obstacle bypassing route, and continuously calculates the remaining distance of the obstacle bypassing route; S5: determines whether the obstacle bypassing route is complete, and determines whether the remaining distance is greater than the remaining threshold; S6: the vehicle tracks along the obstacle bypassing route until the obstacle bypassing is completed, and continues to track along the reference route; the invention can plan a smooth and comfortable obstacle bypassing route, taking into account both the stability and obstacle bypassing efficiency of the vehicle during driving.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic driving path planning, and in particular to a method for planning an obstacle avoidance path for a heavy truck used in a port based on a Bezier curve. Background Art

[0002] Closed and semi-closed structured road scenes are currently becoming the main scenes for the rapid implementation of autonomous driving technology, including ports, mines, industrial parks, etc. At present, autonomous driving on structured roads mostly adopts the reference route tracking method. Due to road sharing, there may be obstacles on the route to be driven by the vehicle, such as other operating vehicles, environmental maintenance vehicles such as sprinkler trucks, and other social vehicles. In order to ensure that the operation of the vehicle is not interrupted, improve the vehicle's operating efficiency, and prevent collision accidents, it is necessary to plan a reasonable detour route based on environmental perception.

[0003] Currently, obstacle avoidance curves mostly adopt a single-stage planning solution. For large obstacles, such as a long-axle truck parked longitudinally in the middle of the road, the obstacle cannot be fully sensed at one time due to the obstruction of the obstacle itself, resulting in the failure of single-stage planning and direct interruption of driving. It is necessary to wait for the obstacle to be removed or manually take over and drive past the obstacle before resuming operations, which seriously affects the efficiency of port operations.

[0004] For example, a Chinese patent with authorization announcement number CN110045727B discloses a path planning and control method for an unmanned vehicle in a park. The unmanned vehicle first collects path location information in fixed places such as industrial parks, campuses, tourist attractions, and port terminals through satellite navigation. The controller plans the main route and establishes a path tracking algorithm to control the unmanned vehicle to run along the previously planned path, and obtains surrounding environment information in real time through a TOF three-dimensional camera to detect obstacles during the journey, so as to avoid and re-plan the route, effectively improving the stability of the unmanned vehicle's operation.

[0005] For example, the patent application with publication number CN117826796A discloses a method for planning the path of unmanned container trucks in ports, including: collecting port road information, generating SHP map data, extracting the coordinates of the vertex of the lane rectangle and the lane orientation information; generating road spectrum data according to the road information and the first preset rule; and planning the path of unmanned container trucks according to the second preset rule with the unmanned container truck as the starting point and the port machinery at the destination of the operation as the end point according to the generated road spectrum data. Compared with the prior art, this invention can save labor costs and improve work efficiency.

[0006] The above patents all have the problem raised by this background technology: for large obstacles that cannot be sensed at one time, the ability to circumvent obstacles is limited.

[0007] The information disclosed in this background technology section is only intended to enhance the understanding of the overall background of the invention and should not be regarded as an acknowledgement or any form of suggestion that the information constitutes the prior art already known to ordinary technicians in this field. Summary of the invention

[0008] The technical problem to be solved by the present invention is to overcome the defects of the prior art and provide a Bezier curve-based obstacle avoidance path planning method for heavy trucks used in ports, so as to plan a smooth and comfortable obstacle avoidance route, taking into account the stability and obstacle avoidance efficiency of the vehicle during driving.

[0009] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0010] A Bezier curve-based path planning method for a heavy truck for a port to avoid obstacles comprises the following steps:

[0011] S1: The vehicle follows the reference route and continuously calculates the distance between the vehicle and obstacles;

[0012] S2: Determine whether the distance between the vehicle and the obstacle is less than the safety threshold. If so, execute step S3; if not, return to step S1;

[0013] S3: Sampling obstacle avoidance control points and generating obstacle avoidance routes based on Bezier curve equations;

[0014] S4: The vehicle follows the obstacle avoidance route and continuously calculates the remaining distance of the obstacle avoidance route;

[0015] S5: Determine whether the obstacle avoidance route is complete, and determine whether the remaining distance is greater than the remaining threshold;

[0016] If the obstacle avoidance route is incomplete and the remaining distance is greater than the remaining threshold, return to step S4; if the obstacle avoidance route is incomplete and the remaining distance is not greater than the remaining threshold, return to step S3; if the obstacle avoidance route is complete, execute step S6;

[0017] S6: The vehicle tracks along the obstacle avoidance route until the obstacle avoidance is completed, and then continues to track along the reference route.

[0018] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks in ports of the present invention, the method for sampling obstacle avoidance control points is as follows:

[0019] S311: Establishing a plane rectangular coordinate system;

[0020] The plane rectangular coordinate system takes the current position of the vehicle as the origin, the direction in which the vehicle travels along the reference route for obstacle avoidance is the positive direction of the y-axis, and the direction perpendicular to the y-axis and to the right is the positive direction of the x-axis;

[0021] S312: Sampling obstacle avoidance control points within the current observation range of the vehicle;

[0022] S313: Screening the sampled obstacle avoidance control points to remove the sampled points that do not meet the collision condition and the boundary condition.

[0023] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks in ports of the present invention, the method for sampling obstacle avoidance control points is as follows:

[0024] Select the distance directly in front of the vehicle's current position The position at is taken as the initial control point, and the distances are set to the left and right along the initial control point. Sampling of obstacle avoidance control points is performed until the boundary of the road where the vehicle is currently located is reached; each obstacle avoidance control point is sampled in the positive direction of the y-axis at an interval distance Sampling of obstacle avoidance control points is performed until the maximum observation range of the vehicle is reached, sampling is completed, and the coordinates of each obstacle avoidance control point are recorded.

[0025] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy-duty trucks for ports described in the present invention, the collision condition is that the distance between the obstacle avoidance control point and the obstacle is not less than a preset collision threshold distance; the boundary condition is that the distance between the obstacle avoidance control point and the boundary on either side of the road where the vehicle is currently located is not less than a preset boundary threshold distance.

[0026] As a preferred solution of the obstacle bypassing path planning method for heavy trucks for ports based on Bezier curves according to the present invention, the obstacle bypassing route is composed of at least one item of obstacle bypassing unit routes; the obstacle bypassing unit routes include obstacle bypassing start route, obstacle bypassing middle route, and obstacle bypassing completion route; wherein the obstacle bypassing start route, obstacle bypassing middle route, and obstacle bypassing completion route are all calculated and generated by at least m obstacle bypassing control points, and m is a positive integer; let the first obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the first control point of the obstacle bypassing unit route, let the last obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the last control point of the obstacle bypassing unit route, then the first control point of any obstacle bypassing start route is located in the reference route, and the last control point is located outside the reference route; the first control point and the last control point of any obstacle bypassing middle route are both located outside the reference route; the first control point of any obstacle bypassing completion route is located outside the reference route, and the last control point is located in the reference route;

[0027] The method for determining whether the obstacle circumvention route is complete is as follows: if the obstacle circumvention route includes a completed obstacle circumvention route, the obstacle circumvention route is complete; if the obstacle circumvention route does not include a completed obstacle circumvention route, the obstacle circumvention route is incomplete.

[0028] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks in ports of the present invention, the method for generating an obstacle avoidance route based on the Bezier curve equation is as follows:

[0029] S321: dividing the obstacle avoidance control point into one or more obstacle avoidance unit routes based on the obstacle avoidance control point;

[0030] S322: generating a curve equation of each obstacle avoidance unit route based on the coordinates of the obstacle avoidance control point to obtain a segmented obstacle avoidance route;

[0031] S323: Repeat steps S321-S322 to generate M obstacle avoidance routes as candidate obstacle avoidance routes, and eliminate the candidate obstacle avoidance routes with collision risks;

[0032] Calculating a minimum distance between an obstacle and the alternative obstacle avoidance route based on a curve equation of the obstacle avoidance unit route of the alternative obstacle avoidance route; if the minimum distance is less than a preset threshold, the alternative obstacle avoidance route has a collision risk;

[0033] S324: Calculate the obstacle avoidance cost of each candidate obstacle avoidance route, and select the candidate obstacle avoidance route with the lowest obstacle avoidance cost as the obstacle avoidance route.

[0034] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks for ports of the present invention, the method of dividing obstacle avoidance control points into one or more obstacle avoidance unit routes based on obstacle avoidance control points is as follows:

[0035] If the vehicle is located in the reference route, no less than m obstacle avoidance control points are selected from the obstacle avoidance control points to generate the obstacle avoidance start route, and no less than m obstacle avoidance control points are selected to generate the obstacle avoidance middle route; and if there are at least n obstacle avoidance control points in the reference route behind the obstacle, no less than m obstacle avoidance control points are selected to generate the obstacle avoidance completion route, otherwise, no obstacle avoidance control points are divided to generate the obstacle avoidance completion route;

[0036] If the vehicle is outside the reference route and there are less than n obstacle avoidance control points in the reference route behind the obstacle, then no less than m obstacle avoidance control points are selected to generate the obstacle avoidance route;

[0037] If the vehicle is outside the reference route and there are at least n obstacle avoidance control points in the reference route behind the obstacle, then at least m obstacle avoidance control points are selected to generate the obstacle avoidance completion route.

[0038] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks in ports of the present invention, the curve equation of any obstacle avoidance unit route is a Bezier curve equation, and the formula is as follows:

[0039] ;

[0040] Among them, t represents the relative position parameter, and its value range is ; Represents the coordinates of the point with relative position t on the obstacle avoidance unit route; represents the coordinates of the kth obstacle avoidance control point on the obstacle avoidance unit route. The value range of k is 1, 2, ..., N, where N is the total number of obstacle avoidance control points on the obstacle avoidance unit route. It represents the number of combinations of selecting k elements from N elements.

[0041] As a preferred solution of the Bezier curve-based obstacle avoidance path planning method for heavy trucks in ports of the present invention, the obstacle avoidance cost of the alternative obstacle avoidance route is the sum of the obstacle avoidance costs of all obstacle avoidance unit routes in the alternative obstacle avoidance route; the calculation formula of the obstacle avoidance cost of any obstacle avoidance unit route is as follows:

[0042] ;

[0043] Wherein, F represents the obstacle avoidance cost of the obstacle avoidance unit route; L represents the curve length of the obstacle avoidance unit route; C represents the average curvature of the obstacle avoidance unit route; represents the weight coefficient of the curve length, Represents the weight coefficient of the mean curvature.

[0044] As a preferred solution of the Bezier curve-based port heavy truck obstacle avoidance path planning method of the present invention, the calculation method of the average curvature of the obstacle avoidance unit route is as follows: uniformly select sampling points; calculate the curvature of the obstacle avoidance unit route at each sampling point and find the average value to obtain the average curvature of the obstacle avoidance unit route; the calculation formula for the curvature of any sampling point is as follows:

[0045] ;

[0046] in, Represents the curvature of the sampling point with relative position t on the obstacle avoidance unit route; express The first derivative with respect to t; express The second derivative with respect to t; Indicates taking the absolute value; Represents the determinant.

[0047] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0048] Through segmented planning and re-planning, complex obstacles can be circumvented without relying on complete perception of the obstacles; as the perception of obstacles improves, the obstacle avoidance routes are optimized and improved, which reduces the dependence on vehicle perception capabilities and reduces the investment cost of autonomous driving.

[0049] The path planning method using multi-order Bezier curves can plan a smooth and comfortable obstacle-avoiding route to ensure the smooth operation of heavy vehicles. Obstacle-avoiding route planning is performed under the constraints of road boundaries, vehicle size, and vehicle perception range, taking into account both the stability and obstacle-avoiding efficiency of the vehicle during driving. The route with the lowest cost is selected from the alternative obstacle-avoiding routes as the optimal route. On the premise of ensuring the safety and comfort of heavy vehicles, the length of the detour route is reduced, significantly improving the operating efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative labor. Among them:

[0051] Figure 1 A flow chart of a Bezier curve-based obstacle avoidance path planning method for heavy trucks used in ports provided by the present invention;

[0052] Figure 2 A flow chart of the method for sampling obstacle avoidance control points provided by the present invention;

[0053] Figure 3 A flow chart of a method for generating an obstacle avoidance route based on a Bezier curve equation provided by the present invention;

[0054] Figure 4 A schematic diagram of an alternative obstacle circumvention route provided by the present invention. DETAILED DESCRIPTION

[0055] The technical solution of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations on the technical solution of the present invention. The embodiments of the present invention and the technical features in the embodiments may be combined with each other unless there is a conflict.

[0056] This embodiment introduces a method for planning a path around obstacles for heavy trucks in ports based on Bezier curves. Figure 1 , the method comprises the following steps:

[0057] S1: The vehicle follows the reference route and continuously calculates the distance between the vehicle and obstacles;

[0058] The reference route is a predetermined driving route for the vehicle. If the vehicle encounters no obstacles during driving, it will travel along the reference route from the starting point to the end point.

[0059] The method for calculating the distance between the vehicle and the obstacle can be ultrasonic ranging, lidar ranging, or stereo vision technology, that is, using two or more cameras to capture different perspectives of the same obstacle to calculate the depth map, thereby calculating the distance of the obstacle.

[0060] S2: Determine whether the distance between the vehicle and the obstacle is less than the safety threshold. If so, execute step S3; if not, return to step S1;

[0061] The safety threshold is a reserved safety distance for obstacle avoidance path planning, which is set by technicians in this field according to actual needs. When the distance between the vehicle and the obstacle is not less than the safety threshold, there is no need to plan the obstacle avoidance route temporarily, and the vehicle continues to drive along the reference route and approach the obstacle. Otherwise, the obstacle avoidance route is too long and the obstacle avoidance cost is high. Once it is detected that the distance between the vehicle and the obstacle is less than the safety threshold, the obstacle avoidance path is planned and the obstacle avoidance is prepared.

[0062] S3: Sampling obstacle avoidance control points and generating obstacle avoidance routes based on Bezier curve equations;

[0063] Reference Figure 2 , the method for sampling obstacle avoidance control points is as follows:

[0064] S311: Establishing a plane rectangular coordinate system;

[0065] The plane rectangular coordinate system takes the current position of the vehicle as the origin, the direction in which the vehicle travels along the reference route for obstacle avoidance is the positive direction of the y-axis, and the direction perpendicular to the y-axis and located to the right of the positive direction of the y-axis is the positive direction of the x-axis;

[0066] S312: Sampling obstacle avoidance control points within the current observation range of the vehicle; the method is as follows:

[0067] Select the distance directly in front of the vehicle's current position The position at is taken as the initial control point, and the distances are set to the left and right along the direction of movement of the initial control point. Sampling of obstacle avoidance control points is performed until the boundary of the road where the vehicle is currently located is reached; each obstacle avoidance control point is sampled in the positive direction of the y-axis at an interval distance Sampling of obstacle avoidance control points is performed until the maximum observation range of the vehicle is reached, sampling is completed, and the coordinates of each obstacle avoidance control point are recorded; and It is set by technicians in this field based on actual needs;

[0068] The observation range within which the vehicle can accurately observe the obstacle avoidance control points (and record their coordinates) is a fixed value. Here, obstacle avoidance observation points are sampled only within the observation range. Due to the limitations of distance and angle, the position behind the obstacle before obstacle avoidance may not be observed. As the obstacle avoidance proceeds, the vehicle can gradually sample a sufficient number of obstacle avoidance control points behind the obstacle.

[0069] S313: Screening the sampled obstacle avoidance control points to remove the sampled points that do not meet the collision condition and the boundary condition.

[0070] The collision condition is that the distance between the obstacle avoidance control point and the obstacle is not less than the preset collision threshold distance; the boundary condition is that the distance between the obstacle avoidance control point and the boundary of either side of the road where the vehicle is currently located is not less than the preset boundary threshold distance. Considering the limitations of the vehicle body width and road boundaries, only when the vehicle maintains a certain distance from the road boundary can it safely avoid obstacles.

[0071] The obstacle bypassing route is composed of at least one item of the obstacle bypassing unit routes; the obstacle bypassing unit routes include an obstacle bypassing start route, an obstacle bypassing middle route, and an obstacle bypassing completion route; wherein the obstacle bypassing start route, the obstacle bypassing middle route, and the obstacle bypassing completion route are all calculated and generated by at least m obstacle bypassing control points, where m is a positive integer; let the first obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the first control point of the obstacle bypassing unit route, let the last obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the last control point of the obstacle bypassing unit route, then the first control point of any obstacle bypassing start route is located in the reference route, and the last control point is located outside the reference route; the first control point and the last control point of any obstacle bypassing middle route are both located outside the reference route; the first control point of any obstacle bypassing completion route is located outside the reference route, and the last control point is located in the reference route.

[0072] Reference Figure 3 , the method for generating an obstacle avoidance route based on the Bezier curve equation is as follows:

[0073] S321: Based on the obstacle avoidance control point, the obstacle avoidance control point is divided into one or more obstacle avoidance unit routes; the method is as follows:

[0074] If the vehicle is located in the reference route, no less than m obstacle avoidance control points are selected from the obstacle avoidance control points to generate the obstacle avoidance start route, and no less than m obstacle avoidance control points are selected to generate the obstacle avoidance middle route; and if there are at least n obstacle avoidance control points in the reference route behind the obstacle, no less than m obstacle avoidance control points are selected to generate the obstacle avoidance completion route, otherwise, no obstacle avoidance control points are divided for generating the obstacle avoidance completion route; here, “behind the obstacle” refers to the following: the vehicle is located in front of the obstacle before avoiding the obstacle, and the vehicle is located behind the obstacle after avoiding the obstacle;

[0075] If the vehicle is outside the reference route and there are less than n obstacle avoidance control points in the reference route behind the obstacle, then no less than m obstacle avoidance control points are selected to generate the obstacle avoidance route;

[0076] If the vehicle is outside the reference route and there are at least n obstacle avoidance control points in the reference route behind the obstacle, then at least m obstacle avoidance control points are selected to generate the obstacle avoidance completion route;

[0077] Since the vehicle's perception range of the environment is limited, it may not be able to perceive the tail of a deep obstacle. Therefore, the obstacle avoidance route is planned only when the vehicle can observe at least n traversable obstacle avoidance control points behind the obstacle in the reference route.

[0078] S322: generating a curve equation of each obstacle avoidance unit route based on the coordinates of the obstacle avoidance control point to obtain a segmented obstacle avoidance route;

[0079] The curve equation of any obstacle avoidance unit route is a Bezier curve equation, and the formula is as follows:

[0080] ;

[0081] Among them, t represents the relative position parameter, and its value range is ; Represents the coordinates of the point with relative position t on the obstacle bypass unit route. The relative position t on the obstacle bypass unit route means that the ratio of the curve length between the point and the first control point to the total curve length of the obstacle bypass unit route is t. The curve length between the first control point and the last control point is the total curve length of the obstacle bypass unit route. Represents the coordinates of the kth obstacle avoidance control point on the obstacle avoidance unit route. The value range of k is 1, 2, ..., N, where N is the total number of obstacle avoidance control points on the obstacle avoidance unit route. It represents the number of combinations of selecting k elements from N elements. The formula is as follows:

[0082] ;

[0083] In order to avoid the difficulty of fitting the obstacle avoidance control points located directly in front of and behind the obstacle in the obstacle avoidance route into a curve, the present invention adopts a segmented curve generation method, and each segment of the obstacle avoidance unit route is independently generated by a Bezier curve. Figure 4 shown. Figure 4 When the vehicle encounters an obstacle, the vertical dotted line represents the reference route, the red dots represent the obstacle avoidance control points used to generate the obstacle avoidance start route, the yellow dots represent the obstacle avoidance control points used to generate the obstacle avoidance middle route, and the blue dots represent the obstacle avoidance control points used to generate the obstacle avoidance completion route.

[0084] S323: Repeat steps S321-S322 to generate M obstacle avoidance routes as candidate obstacle avoidance routes, and eliminate the candidate obstacle avoidance routes with collision risks;

[0085] Calculating a minimum distance between an obstacle and the alternative obstacle avoidance route based on a curve equation of the obstacle avoidance unit route of the alternative obstacle avoidance route; if the minimum distance is less than a preset threshold, the alternative obstacle avoidance route has a collision risk;

[0086] S324: Calculate the obstacle avoidance cost of each candidate obstacle avoidance route, and select the candidate obstacle avoidance route with the lowest obstacle avoidance cost as the obstacle avoidance route.

[0087] The obstacle avoidance cost of the alternative obstacle avoidance route is the sum of the obstacle avoidance costs of all obstacle avoidance unit routes in the alternative obstacle avoidance route; the calculation formula of the obstacle avoidance cost of any obstacle avoidance unit route is as follows:

[0088] ;

[0089] Wherein, F represents the obstacle avoidance cost of the obstacle avoidance unit route; L represents the curve length of the obstacle avoidance unit route; C represents the average curvature of the obstacle avoidance unit route; represents the weight coefficient of the curve length, represents the weight coefficient of the mean curvature, and All are set by technicians in this field according to actual needs.

[0090] The calculation method of the curve length of the obstacle bypassing unit route is as follows: the obstacle bypassing unit route is divided into a plurality of very short curve segments, the tangent length of each curve segment is calculated and summed up to obtain the total curve length;

[0091] The method for calculating the average curvature of the obstacle avoidance unit route is as follows: uniformly select sampling points, is a positive integer; the curvature of the obstacle avoidance unit route at each sampling point is calculated and the average is obtained to obtain the average curvature of the obstacle avoidance unit route; the calculation formula for the curvature of any sampling point is as follows:

[0092] ;

[0093] in, Represents the curvature of the sampling point with relative position t on the obstacle avoidance unit route; express The first derivative about t, i.e. the tangent vector of the curve, gives the tangent direction of the sampling point;

[0094] express The second derivative with respect to t describes the rate at which the tangent direction changes with t and is directly related to the curvature of the curve; Indicates taking the absolute value; Represents the determinant, which is used to calculate the and The determinant of the matrix formed by these two vectors is the absolute value of the determinant, which represents the area of ​​the parallelogram formed by these two vectors and is proportional to the curvature.

[0095] Since heavy trucks have the characteristics of heavy load and long wheelbase, in order to ensure the controllability of the chassis and driving comfort during the obstacle avoidance process, it is necessary to ensure that the obstacle avoidance route is smooth enough to avoid sharp turns. Therefore, a continuous and smooth multi-order Bezier curve is selected to generate the obstacle avoidance route.

[0096] S4: The vehicle follows the obstacle avoidance route and continuously calculates the remaining distance of the obstacle avoidance route;

[0097] The remaining distance is the portion of the obstacle avoidance route that the vehicle has not yet traveled. While the vehicle is tracking along the obstacle avoidance route, the remaining distance is continuously calculated to determine whether to re-plan the obstacle avoidance route later.

[0098] S5: Determine whether the obstacle avoidance route is complete, and determine whether the remaining distance is greater than the remaining threshold;

[0099] If the obstacle avoidance route is incomplete and the remaining distance is greater than the remaining threshold, return to step S4; if the obstacle avoidance route is incomplete and the remaining distance is not greater than the remaining threshold, return to step S3; if the obstacle avoidance route is complete, execute step S6;

[0100] The method for determining whether the obstacle bypass route is complete is as follows:

[0101] If the obstacle circumventing route includes the obstacle circumventing completion route, the obstacle circumventing route is complete; if the obstacle circumventing route does not include the obstacle circumventing completion route, the obstacle circumventing route is incomplete.

[0102] Due to the limited perception range of the vehicle, when planning the obstacle avoidance route, it is often impossible to observe the obstacle avoidance control points behind the obstacle on the original reference route. In this case, the obstacle avoidance route generated based on the observable obstacle avoidance control points cannot meet the vehicle's needs to complete the obstacle avoidance. As the vehicle drives and avoids obstacles, the control points that the vehicle can observe will also change. Only when the vehicle can observe the obstacle avoidance control points on the original reference route can a complete obstacle avoidance route be generated through re-planning, that is, after the vehicle completes the obstacle avoidance, it can return to the original reference route and continue driving.

[0103] The remaining threshold is the reserved safety distance for re-planning the obstacle avoidance route, which is set by technicians in this field according to actual needs. When the remaining distance is greater than the remaining threshold, there is no need to re-plan the obstacle avoidance route temporarily, and the vehicle continues to drive along the obstacle avoidance route. Otherwise, the vehicle will not observe enough new control points, and the re-planning effect will be poor. When the remaining distance is not greater than the safety threshold, under the premise that the obstacle avoidance route is incomplete,

[0104] Re-planning is required, that is, returning to step S3, re-sampling the obstacle avoidance control points, and regenerating the obstacle avoidance route.

[0105] S6: The vehicle tracks along the obstacle avoidance route until the obstacle avoidance is completed, and then continues to track along the reference route.

[0106] When the obstacle avoidance route is complete, the vehicle follows the obstacle avoidance route until the remaining distance of the obstacle avoidance route is 0, and the obstacle avoidance is completed. At this time, the vehicle is already located on the original reference route and can continue to drive along the reference route.

[0107] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0108] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the purpose and scope of protection of the present invention, which are all within the protection of the present invention.

Claims

1. A Bezier curve-based path planning method for heavy trucks in ports, characterized in that: The following steps are involved: S1: The vehicle follows the reference route and continuously calculates the distance between the vehicle and obstacles; S2: Determine whether the distance between the vehicle and the obstacle is less than the safety threshold. If so, execute step S3; if not, return to step S1; S3: Sampling obstacle avoidance control points and generating obstacle avoidance routes based on Bezier curve equations; The method for generating an obstacle avoidance route based on the Bezier curve equation is as follows: S321: dividing the obstacle avoidance control point into one or more obstacle avoidance unit routes based on the obstacle avoidance control point; S322: generating a curve equation of each obstacle avoidance unit route based on the coordinates of the obstacle avoidance control point to obtain a segmented obstacle avoidance route; S323: Repeat steps S321-S322 to generate M obstacle avoidance routes as candidate obstacle avoidance routes, and eliminate the candidate obstacle avoidance routes with collision risks; Calculating a minimum distance between the obstacle and the alternative obstacle avoidance route based on the curve equation of the alternative obstacle avoidance route; if the minimum distance is less than a preset threshold, the alternative obstacle avoidance route has a collision risk; S324: Calculate the obstacle avoidance cost of each candidate obstacle avoidance route, and select the candidate obstacle avoidance route with the lowest obstacle avoidance cost as the obstacle avoidance route; The obstacle avoidance cost of the alternative obstacle avoidance route is the sum of the obstacle avoidance costs of all obstacle avoidance unit routes in the alternative obstacle avoidance route; the calculation formula for the obstacle avoidance cost of any obstacle avoidance unit route is as follows: ； Wherein, F represents the obstacle avoidance cost of the obstacle avoidance unit route; L represents the curve length of the obstacle avoidance unit route; C represents the average curvature of the obstacle avoidance unit route; represents the weight coefficient of the curve length, represents the weight coefficient of the mean curvature; S4: The vehicle follows the obstacle avoidance route and continuously calculates the remaining distance of the obstacle avoidance route; S5: Determine whether the obstacle avoidance route is complete, and determine whether the remaining distance is greater than the remaining threshold; If the obstacle avoidance route is incomplete and the remaining distance is greater than the remaining threshold, return to step S4; if the obstacle avoidance route is incomplete and the remaining distance is not greater than the remaining threshold, return to step S3; if the obstacle avoidance route is complete, execute step S6; S6: The vehicle tracks along the obstacle avoidance route until the obstacle avoidance is completed, and then continues to track along the reference route.

2. A Bezier curve-based path planning method for heavy trucks in ports according to claim 1, characterized in that: The method for sampling obstacle avoidance control points is as follows: S311: Establishing a plane rectangular coordinate system; The plane rectangular coordinate system takes the current position of the vehicle as the origin, the direction in which the vehicle travels along the reference route for obstacle avoidance is the positive direction of the y-axis, and the direction perpendicular to the y-axis and to the right is the positive direction of the x-axis; S312: Sampling obstacle avoidance control points within the current observation range of the vehicle; S313: Screening the sampled obstacle avoidance control points to remove the sampled points that do not meet the collision condition and the boundary condition.

3. A Bezier curve-based path planning method for heavy trucks in ports according to claim 2, characterized in that: The method for sampling obstacle avoidance control points also includes: Select the distance directly in front of the vehicle's current position The position at is taken as the initial control point, and the distances are set to the left and right along the initial control point. Sampling of obstacle avoidance control points is performed until the boundary of the road where the vehicle is currently located is reached; each obstacle avoidance control point is sampled in the positive direction of the y-axis at an interval distance Sampling of obstacle avoidance control points is performed until the maximum observation range of the vehicle is reached, sampling is completed, and the coordinates of each obstacle avoidance control point are recorded.

4. A Bezier curve-based path planning method for heavy trucks in ports according to claim 3, characterized in that: The collision condition is that the distance between the obstacle avoidance control point and the obstacle is not less than a preset collision threshold distance; the boundary condition is that the distance between the obstacle avoidance control point and the boundary on either side of the road where the vehicle is currently located is not less than a preset boundary threshold distance.

5. The method for planning a path for a heavy truck to avoid obstacles in a port based on Bezier curves as claimed in claim 4, characterized in that: The obstacle bypassing route is composed of at least one item of the obstacle bypassing unit routes; the obstacle bypassing unit routes include an obstacle bypassing start route, an obstacle bypassing middle route, and an obstacle bypassing completion route; wherein the obstacle bypassing start route, the obstacle bypassing middle route, and the obstacle bypassing completion route are all calculated and generated by at least m obstacle bypassing control points, where m is a positive integer; let the first obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the first control point of the obstacle bypassing unit route, let the last obstacle bypassing control point passed by the vehicle in any obstacle bypassing unit route be the last control point of the obstacle bypassing unit route, then the first control point of any obstacle bypassing start route is located in the reference route, and the last control point is located outside the reference route; the first control point and the last control point of any obstacle bypassing middle route are both located outside the reference route; the first control point of any obstacle bypassing completion route is located outside the reference route, and the last control point is located in the reference route; The method for determining whether the obstacle circumvention route is complete is as follows: if the obstacle circumvention route includes a completed obstacle circumvention route, the obstacle circumvention route is complete; if the obstacle circumvention route does not include a completed obstacle circumvention route, the obstacle circumvention route is incomplete.

6. A Bezier curve-based path planning method for heavy trucks in ports according to claim 5, characterized in that: The method for dividing the obstacle avoidance control point into one or more obstacle avoidance unit routes based on the obstacle avoidance control point is as follows: If the vehicle is located in the reference route, no less than m obstacle avoidance control points are selected from the obstacle avoidance control points to generate the obstacle avoidance start route, and no less than m obstacle avoidance control points are selected to generate the obstacle avoidance middle route; and if there are at least n obstacle avoidance control points in the reference route behind the obstacle, no less than m obstacle avoidance control points are selected to generate the obstacle avoidance completion route, otherwise, no obstacle avoidance control points are divided to generate the obstacle avoidance completion route; If the vehicle is outside the reference route and there are less than n obstacle avoidance control points in the reference route behind the obstacle, then no less than m obstacle avoidance control points are selected to generate the obstacle avoidance route; If the vehicle is outside the reference route and there are at least n obstacle avoidance control points in the reference route behind the obstacle, then at least m obstacle avoidance control points are selected to generate the obstacle avoidance completion route.

7. A Bezier curve-based path planning method for heavy trucks in ports according to claim 6, characterized in that: The curve equation of any obstacle avoidance unit route is the Bezier curve equation, and the formula is as follows: ； Among them, t represents the relative position parameter, and its value range is ; Represents the coordinates of the point with relative position t on the obstacle avoidance unit route; represents the coordinates of the kth obstacle avoidance control point on the obstacle avoidance unit route. The value range of k is 1, 2, ..., N, where N is the total number of obstacle avoidance control points on the obstacle avoidance unit route. It represents the number of combinations of selecting k elements from N elements.

8. The method for planning a path for a heavy truck to avoid obstacles in a port based on a Bezier curve as claimed in claim 7, characterized in that: The method for calculating the average curvature of the obstacle avoidance unit route is as follows: uniformly select sampling points; Calculate the curvature of the obstacle avoidance unit route at each sampling point and find the average value to obtain the average curvature of the obstacle avoidance unit route; the calculation formula for the curvature of any sampling point is as follows: ； in, Represents the curvature of the sampling point with relative position t on the obstacle avoidance unit route; express The first derivative with respect to t; express The second derivative with respect to t; Indicates taking the absolute value; Represents the determinant.

Images