Low-speed Ackerman steering small unmanned vehicle local path planning method
Through improved quadratic planning and spline interpolation algorithms, the global trajectory is smoothed and local trajectory optimization is combined with state machines, the path planning problem of low-speed Ackerman steering small unmanned vehicles in complex scenarios is solved, and the vehicle is safe and efficiently driven.
Patent Information
- Application Number
- CN202411943963.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-27
- Publication Date
- 2025-04-08
AI Technical Summary
There is a lack of effective local path planning methods for low-speed Ackermann to small unmanned vehicles in the prior art, especially in scenarios with large flow of people and complex environments, and it is difficult to ensure the safety and efficiency of the vehicle.
The improved quadratic planning and spline interpolation algorithm are used to smooth the global trajectory, and local trajectory optimization is performed in combination with the state machine. Through the calculation of tracking state, obstacle avoidance state and error state, available vehicle driving trajectories are generated.
The smooth processing of the global trajectory and the optimization of the local trajectory are achieved, and a vehicle driving trajectory that meets the safety and efficiency requirements is generated, which is suitable for scenarios with large flow of people and complex environments.
Smart Images
Figure CN119779335A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of unmanned vehicles, and in particular to a local path planning method for a small unmanned vehicle with low-speed Ackerman steering. Background Art
[0002] With the rapid development of unmanned driving technology, especially in the field of small unmanned vehicles (such as unmanned patrol cars, unmanned delivery robots, etc.), the importance of path planning has become increasingly prominent. Path planning not only involves how to find an effective route in a given environment, but also concerns whether the vehicle can complete its target task safely and efficiently. Among the many steering methods, Ackerman steering is widely used in multiple unmanned driving systems due to its good controllability and stability.
[0003] Currently, there are few planning methods for low-speed Ackerman steering small vehicles. The reason is that this type of vehicle is between passenger cars and differential cars, and the application scenarios are quite different. Passenger cars have the task of carrying people and the vehicle model has many restrictions, so the planning algorithm needs to consider comfort. In addition, due to their high speed, the planned trajectory is often long (more than 50 meters), and they also have high requirements for safety. Differential cars have strong maneuverability, and the planning algorithm prioritizes efficiency, and does not have high requirements for safety and comfort. Summary of the invention
[0004] In view of the above problems, the present invention provides a local path planning method for a small unmanned vehicle with low-speed Ackerman steering. The method not only smoothes the global trajectory, but also clips the smoothed trajectory as a reference trajectory for local planning, enters a state machine for calculation, and generates a usable vehicle driving trajectory through calculations of the tracking state, obstacle avoidance state and error state. The method also meets the planning requirements of scenes with large passenger flow and complex environment.
[0005] In order to achieve the above-mentioned purpose and other related purposes, the technical solution provided by the present invention is as follows: a local path planning method for a small unmanned vehicle with low-speed Ackerman steering, the method comprising:
[0006] U1. Initialization: The system is initialized to ensure that all necessary parameters and states are set to initial values, and the state machine is initialized;
[0007] U2. Global trajectory smoothing: After receiving the global trajectory information, the improved quadratic programming and spline interpolation algorithm are used for smoothing;
[0008] U3. If the global trajectory smoothing return fails, then end, otherwise proceed to the next step;
[0009] U4. Trajectory clipping: Generate a reference trajectory for local planning, and consider the back-up scenario. Based on the current position of the vehicle, clip L meters in the forward direction and L-3 meters in the back-up direction;
[0010] U5. State machine: The entire state machine has three states, namely tracking state, obstacle avoidance state and error state;
[0011] U6. Determine whether an error occurs in the state machine. If so, send an error message. Otherwise, send the calculated optimal trajectory.
[0012] Furthermore, in step U2, the smoothing using improved quadratic programming and spline interpolation algorithm includes:
[0013] U21. Path point sampling: extract global trajectory information and retain path information;
[0014] U22. Path point smoothing: Normalize the range of s of each segment of the polynomial to the range of [0,1], convert the coordinates of the original reference point on UTU into relative coordinates based on the first reference point, and use two quintic polynomials to represent each segment.
[0015]
[0016] A i =[a 0 ,a i1 ,a i2 ,a i3 ,a i4 ,a i5 ] T ,
[0017] B i =[b 0 ,b i1 ,b i2 ,b i3 ,b i4 ,b i5 ] T ,
[0018] S=[1,s,s 2 ,s 3 ,s 4 ,s 5 ] T ,
[0019] Among them, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression of the fifth-order polynomial of the i-th trajectory, a 0 、a i1 、a i2 、a i3 、ai4 and a i5 is the constant coefficient of the expression of the fifth-order polynomial of the i-th trajectory, b 0 、b i1 、b i2 、b i3 、b i4 and b i5 is the constant coefficient of the expression of the quintic polynomial of the i-th trajectory, s is the variable parameter of the trajectory, A i is the constant coefficient matrix of the expression of the fifth-order polynomial of the i-th trajectory, B i is the constant coefficient matrix of the expression of the quintic polynomial of the i-th trajectory, S is the variable parameter matrix of the trajectory, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is a function of the expression of the quintic polynomial of the i-th trajectory.
[0020] Further, in step U22, the cost function of the trajectory optimization problem is the minimum curvature and curvature change rate of the polynomial, and the cost function J is,
[0021]
[0022] Among them, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is the function of the expression of the quintic polynomial of the i-th trajectory, N is the sample size, and s is the variable parameter of the trajectory.
[0023] Furthermore, the boundary constraint condition of the cost function J is:
[0024]
[0025] Among them, P i is the i-th point of the reference line, θ is point P i The heading angle, P i A is point P i The heading angle direction, P i M is point P i The direction of the normal vector, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression 2 of the quintic polynomial of the i-th trajectory.
[0026] Further, in step U5, the state machine includes:
[0027] U51. The default state is tracking state. The current position is obtained through positioning, the reference trajectory is matched, and the tracking trajectory is intercepted from the matching point to the end point of the reference trajectory. The trajectory collision detection is performed in combination with the obstacle information. If there is no collision in the tracking trajectory, it stays in the tracking state, otherwise it jumps to the obstacle avoidance state.
[0028] U52. Obstacle avoidance state, obstacle avoidance is achieved based on local planning algorithm.
[0029] Furthermore, in step U52, the local planning algorithm includes:
[0030] U521. Forward trajectory sampling. All operations in this step are performed in the Frenet coordinate system, where d is the lateral distance, d′ is the lateral velocity, d" is the lateral acceleration, l is the longitudinal distance, l′ is the longitudinal velocity, and l" is the longitudinal acceleration. First, the current reference line matching point is converted to the Frenet coordinate system coordinate as the sampling starting point {d 0 , d 0 ′,d 0 ",(l 0 , l 0 ′,l 0 ")}, read the configuration parameters to confirm the end point mileage value, match the generated sampling end point {d n , d n ′,d n ",(l n , l n ′,l n ")}, the starting point and the end point are fitted with a polynomial, the function is as follows:
[0031]
[0032] Where f(x) is a polynomial, x is the fitting parameter of the polynomial f(x), and c 0 、c 1 、c 2 、c 3 、c 4 and c 5 is the constant coefficient of the polynomial f(x). Since the starting point and the end point are known, we can get:
[0033]
[0034] Finally, the Frenet coordinate system coordinates are converted into UTM coordinate system coordinates to generate sampling trajectory clusters;
[0035] U522. Trajectory selection. Trajectory selection adopts cost sorting. The smaller the cost value, the higher the priority. First, traverse the trajectory points to determine whether the curvature exceeds the limit. The maximum curvature is tanδ / L, δ is the maximum turning angle of the vehicle, and L is the wheelbase. If yes, the trajectory is eliminated. If not, calculate the curvature cost cost1=q1*k, q1 is the first weight, and k is the absolute value of the maximum curvature of the trajectory. Then determine whether there is a collision. If yes, the trajectory is eliminated. If not, calculate the collision cost cost2, and then calculate the lateral distance cost cost3. Next, sort by cost cost=cost1+cost2+cost3 to select the trajectory. The trajectory with the smallest cost is the optimal trajectory.
[0036] U523. Determine whether there is a valid forward trajectory. If so, enter the forward mode. In the forward mode, the lateral error and angular error are determined in real time. If the lateral error is less than 0.2 meters and the angular error is less than 3 degrees, the vehicle is on the reference trajectory and returns to the tracking state. Otherwise, it stays in the obstacle avoidance state. If there is no valid forward trajectory, the vehicle stops and waits for 3 seconds to enter the reverse trajectory sampling. This step is the same as the forward trajectory sampling. The only difference is that the reverse trajectory samples a trajectory with a lateral distance d=0. The trajectory selection is the same as step U522. If there is a valid reverse trajectory, the reverse mode is entered. At this time, the matching point S of the reverse starting point on the reference trajectory is recorded. 0 ,During the reversing process, the reference trajectory matching point S of the real-time position needs to be calculated 1 , if |S 0 -S 1 |>3 meters, the reversing distance exceeds the limit and the system immediately enters the error state. If the limit is not exceeded, the system stays in the obstacle avoidance state.
[0037] Furthermore, the collision cost cost2 is,
[0038]
[0039] Among them, q2 is the second weight, l is the shortest distance between the trajectory and the obstacle, and l max The maximum distance from the obstacle is set to 0.5 meters. min The minimum distance from obstacles, also called the protection distance, is set to 0.2 meters.
[0040] Furthermore, the lateral distance cost cost3 is,
[0041]
[0042] Among them, q3 is the third weight, and d is the lateral distance from the end point of the trajectory to the reference line.
[0043] In order to achieve the above-mentioned purpose and other related purposes, the present invention also provides a local path planning system for a small unmanned vehicle with low-speed Ackerman steering, including a computer device, which is programmed or configured to execute the steps of any one of the local path planning methods for a small unmanned vehicle with low-speed Ackerman steering.
[0044] In order to achieve the above-mentioned purpose and other related purposes, the present invention also provides a computer-readable storage medium, which stores a computer program programmed or configured to execute any one of the local path planning methods for low-speed Ackerman steering small unmanned vehicle.
[0045] The present invention has the following positive effects:
[0046] 1. The present invention smoothes the global trajectory of the vehicle by using an improved quadratic programming and spline interpolation algorithm, and optimizes the local trajectory of the vehicle by combining an optimal trajectory optimization algorithm based on a state machine. Not only is the global trajectory smoothed, but the smoothed trajectory is clipped as a reference trajectory for local planning and enters the state machine for calculation. After calculating the tracking state, obstacle avoidance state and error state, an available vehicle driving trajectory is generated, and the planning requirements of scenes with large passenger flow and complex environment are met.
[0047] 2. In view of the fact that the received global planning trajectory has uneven curvature, the present invention segments the trajectory, effectively solving the problem of global trajectory smoothness of U-shaped bends and S-shaped bends. At the same time, for scenes with large passenger flow and complex environment, the local driving path of the vehicle is adaptively adjusted and optimized, thereby improving the safety of vehicle driving. BRIEF DESCRIPTION OF THE DRAWINGS
[0048] Figure 1 It is a schematic diagram of the method flow of the present invention;
[0049] Figure 2 is a schematic diagram of a state machine of the present invention;
[0050] Figure 3 It is a schematic diagram of the trajectory selection process of the present invention. DETAILED DESCRIPTION
[0051] The following is a description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, including various details of the embodiments of the present disclosure to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope and spirit of the present disclosure. Similarly, for the sake of clarity and conciseness, descriptions of well-known functions and structures are omitted in the following description.
[0052] Example: Figure 1 As shown, a local path planning method for a small unmanned vehicle with low-speed Ackerman steering is provided, the method comprising:
[0053] U1. Initialization: The system is initialized to ensure that all necessary parameters and states are set to initial values, and the state machine is initialized;
[0054] U2. Global trajectory smoothing: After receiving the global trajectory information, the improved quadratic programming and spline interpolation algorithm are used for smoothing;
[0055] U3. If the global trajectory smoothing return fails, then end, otherwise proceed to the next step;
[0056] U4. Trajectory clipping: Generate a reference trajectory for local planning, and consider the back-up scenario. Based on the current position of the vehicle, clip L meters in the forward direction and L-3 meters in the back-up direction;
[0057] U5. State machine: The entire state machine has three states, namely tracking state, obstacle avoidance state and error state;
[0058] U6. Determine whether an error occurs in the state machine. If so, send an error message. Otherwise, send the calculated optimal trajectory.
[0059] In this embodiment, in step U2, the smoothing using improved quadratic programming and spline interpolation algorithm includes:
[0060] U21. Path point sampling: extract global trajectory information and retain path information;
[0061] U22. Path point smoothing: Normalize the range of s of each segment of the polynomial to the range of [0,1], convert the coordinates of the original reference point on UTU into relative coordinates based on the first reference point, and use two quintic polynomials to represent each segment.
[0062]
[0063] A i =[a 0 ,a i1 ,a i2 ,a i3 ,a i4 ,a i5 ] T ,
[0064] B i =[b 0 ,b i1 ,b i2 ,b i3 ,b i4 ,b i5 ]T ,
[0065] S=[1,s,s 2 ,s 3 ,s 4 ,s 5 ] T ,
[0066] Among them, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression of the fifth-order polynomial of the i-th trajectory, a 0 、a i1 、a i2 、a i3 、a i4 and a i5 is the constant coefficient of the expression of the fifth-order polynomial of the i-th trajectory, b 0 、b i1 、b i2 、b i3 、b i4 and b i5 is the constant coefficient of the expression of the quintic polynomial of the i-th trajectory, s is the variable parameter of the trajectory, A i is the constant coefficient matrix of the expression of the fifth-order polynomial of the i-th trajectory, B i is the constant coefficient matrix of the expression of the quintic polynomial of the i-th trajectory, S is the variable parameter matrix of the trajectory, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is a function of the expression of the quintic polynomial of the i-th trajectory.
[0067] In this embodiment, in step U22, the cost function of the trajectory optimization problem is the curvature of the polynomial and the curvature change rate is minimized, and the cost function J is,
[0068]
[0069] Among them, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is the function of the expression of the quintic polynomial of the i-th trajectory, N is the sample size, and s is the variable parameter of the trajectory.
[0070] In this embodiment, the boundary constraint condition of the cost function J is:
[0071]
[0072] Among them, P i is the i-th point of the reference line, θ is point P i The heading angle, Pi A is point P i The heading angle direction, P i M is point P i The direction of the normal vector, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression 2 of the quintic polynomial of the i-th trajectory.
[0073] In this embodiment, if Figure 2 As shown, in step U5, the state machine includes:
[0074] U51. The default state is tracking state. The current position is obtained through positioning, the reference trajectory is matched, and the tracking trajectory is intercepted from the matching point to the end point of the reference trajectory. The trajectory collision detection is performed in combination with the obstacle information. If there is no collision in the tracking trajectory, it stays in the tracking state, otherwise it jumps to the obstacle avoidance state.
[0075] U52. Obstacle avoidance state, obstacle avoidance is achieved based on local planning algorithm.
[0076] In this embodiment, if Figure 3 As shown, in step U52, the local planning algorithm includes:
[0077] U521. Forward trajectory sampling. All operations in this step are performed in the Frenet coordinate system, where d is the lateral distance, d′ is the lateral velocity, d" is the lateral acceleration, l is the longitudinal distance, l′ is the longitudinal velocity, and l" is the longitudinal acceleration. First, the current reference line matching point is converted to the Frenet coordinate system coordinate as the sampling starting point {d 0 , d 0 ′,d 0 ",(l 0 , l 0 ′,l 0 ")}, read the configuration parameters to confirm the end point mileage value, match the generated sampling end point {d n , d n ′,d n ",(l n , l n ′,l n ")}, the starting point and the end point are fitted with a polynomial, the function is as follows:
[0078]
[0079] Where f(x) is a polynomial, x is the fitting parameter of the polynomial f(x), and c 0 、c 1 、c 2 、c 3 、c 4 and c 5is the constant coefficient of the polynomial f(x). Since the starting point and the end point are known, we can get:
[0080]
[0081] Finally, the Frenet coordinate system coordinates are converted into UTM coordinate system coordinates to generate sampling trajectory clusters;
[0082] U522. Trajectory selection. Trajectory selection adopts cost sorting. The smaller the cost value, the higher the priority. First, traverse the trajectory points to determine whether the curvature exceeds the limit. The maximum curvature is tanδ / L, δ is the maximum turning angle of the vehicle, and L is the wheelbase. If yes, the trajectory is eliminated. If not, calculate the curvature cost cost1=q1*k, q1 is the first weight, and k is the absolute value of the maximum curvature of the trajectory. Then determine whether there is a collision. If yes, the trajectory is eliminated. If not, calculate the collision cost cost2, and then calculate the lateral distance cost cost3. Next, sort by cost cost=cost1+cost2+cost3 to select the trajectory. The trajectory with the smallest cost is the optimal trajectory.
[0083] U523. Determine whether there is a valid forward trajectory. If so, enter the forward mode. In the forward mode, the lateral error and angular error are determined in real time. If the lateral error is less than 0.2 meters and the angular error is less than 3 degrees, the vehicle is on the reference trajectory and returns to the tracking state. Otherwise, it stays in the obstacle avoidance state. If there is no valid forward trajectory, the vehicle stops and waits for 3 seconds to enter the reverse trajectory sampling. This step is the same as the forward trajectory sampling. The only difference is that the reverse trajectory samples a trajectory with a lateral distance d=0. The trajectory selection is the same as step U522. If there is a valid reverse trajectory, the reverse mode is entered. At this time, the matching point S of the reverse starting point on the reference trajectory is recorded. 0 ,During the reversing process, the reference trajectory matching point S of the real-time position needs to be calculated 1 , if |S 0 -S 1 |>3 meters, the reversing distance exceeds the limit and the system immediately enters the error state. If the limit is not exceeded, the system stays in the obstacle avoidance state.
[0084] In this embodiment, the collision cost cost2 is:
[0085]
[0086] Among them, q2 is the second weight, l is the shortest distance between the trajectory and the obstacle, and l max The maximum distance from the obstacle is set to 0.5 meters. min The minimum distance from obstacles, also called the protection distance, is set to 0.2 meters.
[0087] In this embodiment, the lateral distance cost cost3 is:
[0088]
[0089] Among them, q3 is the third weight, and d is the lateral distance from the end point of the trajectory to the reference line.
[0090] In this embodiment, the present invention provides a local path planning system for a small unmanned vehicle with low-speed Ackerman steering, including a computer device that is programmed or configured to execute any one of the steps of the local path planning method for a small unmanned vehicle with low-speed Ackerman steering.
[0091] In summary, the present invention not only smoothes the global trajectory, but also clips the smoothed trajectory as a reference trajectory for local planning, enters the state machine for calculation, and generates an available vehicle driving trajectory through calculations of the tracking state, obstacle avoidance state, and error state, and also meets the planning requirements of scenes with large passenger flow and complex environment.
[0092] The above specific implementations do not constitute a limitation on the protection scope of the present disclosure. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure.
Claims
1. A local path planning method for a small unmanned vehicle with low-speed Ackerman steering, characterized in that: The method comprises: U1. Initialization: The system is initialized to ensure that all necessary parameters and states are set to initial values, and the state machine is initialized; U2. Global trajectory smoothing: After receiving the global trajectory information, the improved quadratic programming and spline interpolation algorithm are used for smoothing; U3. If the global trajectory smoothing return fails, then end, otherwise proceed to the next step; U4. Trajectory clipping: Generate a reference trajectory for local planning, and consider the back-up scenario. Based on the current position of the vehicle, clip L meters in the forward direction and L-3 meters in the back-up direction; U5. State machine: The entire state machine has three states, namely tracking state, obstacle avoidance state and error state; U6. Determine whether an error occurs in the state machine. If so, send an error message. Otherwise, send the calculated optimal trajectory.
2. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 1 is characterized in that: In step U2, the smoothing using improved quadratic programming and spline interpolation algorithm includes: U21. Path point sampling: extract global trajectory information and retain path information; U22. Path point smoothing: Normalize the range of s of each segment of the polynomial to the range of [0,1], convert the coordinates of the original reference point on UTU into relative coordinates based on the first reference point, and use two quintic polynomials to represent each segment. THE i =[a0,a i1 ,the i2 ,the i3 ,the i4 ,the i5 ] T , B i =[b0,b i1 ,b i2 ,b i3 ,b i4 ,b i5 ] T , S=[1,s,s 2 ,s 3 ,s 4 ,s 5 ] T , Among them, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression of the fifth-order polynomial of the i-th trajectory, a0, a i1 、a i2 、a i3 、a i4 and a i5 is the constant coefficient of the expression of the fifth-order polynomial of the i-th trajectory, b0, b i1 , b i2 , b i3 , b i4 and b i5 is the constant coefficient of the expression of the quintic polynomial of the i-th trajectory, s is the variable parameter of the trajectory, A i is the constant coefficient matrix of the expression of the fifth-order polynomial of the i-th trajectory, B i is the constant coefficient matrix of the expression of the quintic polynomial of the i-th trajectory, S is the variable parameter matrix of the trajectory, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is a function of the expression of the quintic polynomial of the i-th trajectory.
3. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 2 is characterized in that: In step U22, the cost function of the trajectory optimization problem is the curvature of the polynomial and the curvature change rate is minimized. The cost function J is, Among them, f i (s) is the function of the fifth-order polynomial expression of the i-th trajectory, g i (s) is the function of the expression of the quintic polynomial of the i-th trajectory, N is the sample size, and s is the variable parameter of the trajectory.
4. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 3 is characterized in that: The boundary constraints of the cost function J are: Among them, P i is the i-th point of the reference line, θ is point P i The heading angle, P i A is point P i The heading angle direction, P i M is point P i The direction of the normal vector, x i is the expression of the fifth-order polynomial of the i-th trajectory, y i is the expression 2 of the quintic polynomial of the i-th trajectory.
5. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 1 is characterized in that: In step U5, the state machine includes: U51. The default state is tracking state. The current position is obtained through positioning, the reference trajectory is matched, and the tracking trajectory is intercepted from the matching point to the end point of the reference trajectory. The trajectory collision detection is performed in combination with the obstacle information. If there is no collision in the tracking trajectory, it stays in the tracking state, otherwise it jumps to the obstacle avoidance state. U52. Obstacle avoidance state, obstacle avoidance is achieved based on local planning algorithm.
6. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 5 is characterized in that: In step U52, the local planning algorithm includes: U521. Forward trajectory sampling. All operations in this step are performed in the Frenet coordinate system, where d is the lateral distance, d′ is the lateral speed, d" is the lateral acceleration, l is the longitudinal distance, l′ is the longitudinal speed, and l" is the longitudinal acceleration. First, the current reference line matching point is converted to the Frenet coordinate system coordinates as the sampling starting point {d0, d0′, d0", (l0, l0′, l0")}, the configuration parameters are read to confirm the end point mileage value, and the matching generates the sampling end point {d n , d n ′,d n ",(l n , l n ′,l n ")}, the starting point and the end point are fitted with a polynomial, the function is as follows: Among them, f(x) is a polynomial, x is the fitting parameter of the polynomial f(x), c0, c1, c2, c3, c4 and c5 are the constant coefficients of the polynomial f(x). Since the starting point and the end point states are known, we can get: Finally, the Frenet coordinate system coordinates are converted into UTM coordinate system coordinates to generate sampling trajectory clusters; U522. Trajectory selection. Trajectory selection adopts cost sorting. The smaller the cost value, the higher the priority. First, traverse the trajectory points to determine whether the curvature exceeds the limit. The maximum curvature is tanδ / L, δ is the maximum turning angle of the vehicle, and L is the wheelbase. If yes, the trajectory is eliminated. If not, calculate the curvature cost cost1=q1*k, q1 is the first weight, and k is the absolute value of the maximum curvature of the trajectory. Then determine whether there is a collision. If yes, the trajectory is eliminated. If not, calculate the collision cost cost2, and then calculate the lateral distance cost cost3. Next, sort by cost cost=cost1+cost2+cost3 to select the trajectory. The trajectory with the smallest cost is the optimal trajectory. U523. Determine whether there is a valid forward trajectory. If so, enter the forward mode. In the forward mode, the lateral error and angular error are determined in real time. If the lateral error is less than 0.2 meters and the angular error is less than 3 degrees, the vehicle is on the reference trajectory and returns to the tracking state. Otherwise, it stays in the obstacle avoidance state. If there is no valid forward trajectory, the vehicle stops, waits for 3 seconds, and enters the reverse trajectory sampling. This step is consistent with the forward trajectory sampling. The only difference is that the reverse trajectory samples a trajectory with a lateral distance of d=0. The trajectory selection is the same as step U522. If there is a valid reverse trajectory, the vehicle enters the reverse mode. At this time, the matching point S0 of the reverse starting point on the reference trajectory is recorded. During the reverse process, the reference trajectory matching point S1 of the real-time position needs to be calculated. If |S0-S1|>3 meters, the reverse distance exceeds the limit and the vehicle immediately enters the error state. If it does not exceed the limit, the vehicle stays in the obstacle avoidance state.
7. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 6 is characterized by: The collision cost cost2 is, Among them, q2 is the second weight, l is the shortest distance between the trajectory and the obstacle, and l max The maximum distance from the obstacle is set to 0.5 meters. min The minimum distance from obstacles, also called the protection distance, is set to 0.2 meters.
8. The local path planning method for a small unmanned vehicle with low-speed Ackerman steering according to claim 6 is characterized by: The lateral distance cost cost3 is, Among them, q3 is the third weight, and d is the lateral distance from the end point of the trajectory to the reference line.
9. A low-speed Ackerman steering small unmanned vehicle local path planning system, including a computer device, characterized in that: The computer device is programmed or configured to execute the steps of the method for local path planning of a small unmanned vehicle with low-speed Ackerman steering as described in any one of claims 1 to 8.
10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program that is programmed or configured to execute the local path planning method for a small unmanned vehicle with low-speed Ackerman steering as described in any one of claims 1 to 8.