Obstacle avoidance detour method for low-speed unmanned vehicle

Abstract

The invention discloses a low-speed unmanned vehicle-oriented obstacle avoidance detouring method, which comprises the following steps of: inputting and acquiring vehicle surrounding sensing environment information, static map information, vehicle real-time pose information and vehicle real-time state information in real time, and preprocessing to generate a passable cost map; generating a triggering detour logic, and entering a detour planning logic when a condition is satisfied according to a preset detour logic; generating a target bypassing distance; acquiring a bypassing target point according to the target bypassing distance; and according to the detour target point and the passable cost map, planning a detour track according to a preset planning algorithm. According to the invention, a passable area cost map is generated according to input information processing, and finally an obstacle avoidance detouring track is planned. Through the above steps, the unmanned vehicle can generate and execute a safe and effective bypassing track so as to avoid obstacles and continue to execute tasks, and the scene trafficability of the unmanned vehicle is improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving, and in particular to an obstacle avoidance and detour method for a low-speed unmanned driving vehicle. Background Art

[0002] The driving safety and passability of driverless vehicles have always been a hot topic and challenging problem. The related technologies and methods are also constantly iterating and upgrading. At present, the method based on quadratic programming is widely used. This method has great advantages in solving non-convex problems on structured roads. It can quickly and in real time generate smooth detour trajectories and control the vehicle to pass obstacles safely and smoothly.

[0003] However, this method has great limitations. First, it is highly dependent on structured roads. Second, it is highly specific to obstacle types and cannot meet the needs of all obstacle types. Third, it has extremely demanding scene requirements. For example, in situations where an obstacle occupies the center line of the lane, detour planning cannot be completed.

[0004] In the field of low-speed driverless vehicles, on the one hand, the vehicle speed requirement is not high, and no high requirements are placed on the specific traffic efficiency; on the other hand, higher requirements are placed on the vehicle's passing efficiency and scenario restrictions. Summary of the invention

[0005] The purpose of the present invention is: in order to solve the problems existing in the current methods, the present invention provides an obstacle avoidance and detour method for low-speed unmanned vehicles, which comprehensively considers the disadvantages of the current general methods and the requirements for high scene passability, and improves the scene passability of unmanned vehicles.

[0006] The technical solution of the present invention is:

[0007] A method for avoiding obstacles and circumventing traffic for a low-speed unmanned vehicle comprises the following steps:

[0008] S1: Real-time input of the vehicle's surrounding perception environment information, static map information, vehicle real-time position information and vehicle real-time status information, and pre-processing to generate a drivable cost map;

[0009] S2: Generate trigger detour logic. According to the preset detour logic, when the conditions are met, enter the detour planning logic;

[0010] S3: Generate a target detour distance; obtain a detour target point according to the target detour distance;

[0011] S4: According to the detour target point and the traversable cost map, a detour trajectory is planned according to a preset planning algorithm.

[0012] Preferably, in step S1, the real-time acquisition of input vehicle surrounding perception environment information, static map information, vehicle real-time posture information and vehicle real-time status information are respectively specifically:

[0013] The input real-time perception environment data includes the traversable area and obstacle clustering results;

[0014] The input static map data includes a static point cloud map representing static environmental characteristics and a high-precision map of road environmental characteristics;

[0015] The input vehicle posture information includes the vehicle's real-time position, real-time heading, pitch angle and other information;

[0016] The input vehicle real-time status information includes real-time vehicle speed, real-time steering wheel angle, real-time gear position, real-time vehicle mode, and real-time vehicle fault information.

[0017] Preferably, the preprocessing in step S1 to generate a traversable cost map specifically includes the following steps:

[0018] S1-2-1: Obstacle clustering results generate the first traversable area cost map, including:

[0019] (1) Coordinate transformation: The obstacle clustering result is transformed from the vehicle coordinate system to the global coordinate system. The specific formula is as follows, where x, y, and z are the global coordinates after transformation. ′ ,y ′ , z ′ is the local coordinate before conversion, and the other parameters are conversion coefficients:

[0020]

[0021] (2) According to the center coordinates and length and width information of the obstacle, the grid points in the area selected by the obstacle are filled, and finally the first traversable area cost map is generated according to the obstacle clustering result; the conversion formula is as follows, where x i ,y i is the row and column value of each grid point occupied by the obstacle; x1, y1, x2, y2 are the coordinates of the obstacle vertices:

[0022]

[0023] Step 1-2-2: The passable area perception result generates the second passable area cost map; the passable area perception result itself is represented by the occupied grid. In this step, the relevant data can be cropped according to the vehicle position information and the occupied grid size limit;

[0024] Step 1-2-3: The high-precision map of the road environment features generates a third passable area cost map; the road environment map represents lanes, curbs, sidewalks, and intersections through geometric lines; extracts relevant geometric lines and features to generate a static passable area cost map where vehicles can pass;

[0025] Step 1-2-4: Merge the first, second, and third passable area cost maps to generate the final total passable area cost map: Overlay the passable area cost maps generated by the above three steps to generate a complete passable area cost map at the current moment.

[0026] Preferably, the triggering detour logic generated in step S2 includes the following items:

[0027] (1) The unmanned vehicle is driving on the normal mission path;

[0028] (2) During driving, the vehicle senses that the task path ahead is blocked by an obstacle and cannot continue driving;

[0029] (3) Generate a deceleration trajectory and decelerate before generating a detour trajectory;

[0030] (4) When the actual vehicle speed is less than the vehicle speed threshold v thre , the distance between the vehicle and the obstacle is within the distance threshold d thre When within range, detour is triggered.

[0031] Preferably, generating the detour target point in step S3 includes the following steps:

[0032] S3-1: Obtain the obstacle type and generate the bypass target distance according to the obstacle type and size. The specific formula is:

[0033] d goal =d to_obj +l obj +d to_goal +d safe ;

[0034] where d goal is the target distance, d to_obj is the longitudinal distance between the vehicle and the obstacle, l obj is the obstacle length, d to_goal is the preset distance from the obstacle to the target point, d safe The safety threshold space is set;

[0035] S3-2: According to the detour target distance and the actual position of the vehicle, the detour target point is searched on the original mission path. The specific steps are as follows:

[0036] S3-2-1: According to the current position of the vehicle, find the matching point that satisfies the minimum distance on the given trajectory. The specific formula is as follows:

[0037]

[0038] where d i is the distance from all points on the trajectory to the current position of the vehicle, x i ,y i is the coordinate of the i-th track point on the track, x, y are the position coordinates of the vehicle;

[0039] After calculating the distances from all trajectory points to the actual position of the vehicle, compare all distance values ​​and take the trajectory point j with the smallest distance value. This trajectory point j is preset as the vehicle matching point found on the trajectory.

[0040] S3-2-2: According to the found matching points and the preset target distance in step S3-1, find the preset target point on the trajectory; the specific method is to start from the matching point found in step S3-2-1, and continue to index and calculate the distance between each point:

[0041]

[0042] Until the calculated distance and d act When the distance is just greater than the target distance calculated in step S3-1, the trajectory index value k at this moment is preset, and the coordinate point corresponding to the index value k is the target detour trajectory point.

[0043] Preferably, planning the detour trajectory in step S4 includes the following steps:

[0044] S4-1: Heuristic function design; The heuristic function is used to estimate the estimated cost from the node to the target node and the actual cost from the node to the start node. The formula is as follows:

[0045] f = g + h;

[0046]

[0047] Where f is the total cost, g is the cost of moving from the vehicle position to the intermediate node, h is the cost of moving from the intermediate node to the target node, and x i ,y i is the coordinate value of the middle point, x goal ,y goal is the coordinate value of the target point;

[0048] S4-2: Obstacle avoidance, design a path cost function to balance the vehicle's approach to obstacles and path length, so as to plan a reasonable obstacle avoidance trajectory. The formula is as follows:

[0049]

[0050] where d θ (x,y) and d v (x, y) are the distances to the nearest obstacle and the edge of the generalized traversable area graph, and α>0, d θ >0 is a constant used to control the attenuation rate and maximum effective range of the cost function; through the setting of this path cost function, the cost value is proportional to the total traversable area of ​​the plan;

[0051] S4-3: Path interpolation, smoothing and optimization; The obstacle avoidance path planned in step S4-2 has the problem that the path points are too sparse and the path is not smooth. The path needs to be smoothed, interpolated and optimized, as follows:

[0052] (1) Preset trajectory interpolation point resolution d res ;

[0053] (2) Step S4-2 is repeated to plan the distance and angle between two consecutive nodes on the path. The formula is as follows:

[0054]

[0055] α angle = atan((x i -x i-1 ),(y i -y i-1 )) / 180×π;

[0056] where d dis is the distance between two consecutive nodes, α angle is the angle between two consecutive nodes, x i ,y i and x i-1 ,y i-1 is the coordinate value of two consecutive nodes;

[0057] (3) Calculate the interpolation resolution d res_act , the formula is as follows:

[0058] d res_act =d dis / (ceil(d dis / d res ));

[0059] (4) Interpolate relevant trajectory points, the formula is as follows:

[0060]

[0061] y j =y i +j×dres_act ×sin(α angle / 180×π);

[0062] (5) The objective function of smoothing and optimization processing is as follows:

[0063] cost = cost1 + cost2 + cost3;

[0064]

[0065] Among them, cost1 is the smoothness cost, cost2 is the length cost, and cost3 is the offset cost relative to the original point;

[0066] The constraints for smoothing and optimization include position constraints, curvature constraints, and slack variables:

[0067] x i-ref -x l ≤x i ≤x i-ref +x u ;

[0068] y i-ref -y l ≤y i ≤x i-ref +x u ;

[0069] 0≤stack i ≤+∞;

[0070] (x i-1 +x i+1 -2×x i ) 2 +(y i-1 +y i+1 -2×y i ) 2 -stack i ≤(Δs 2 ×cur cstr ) 2 ;

[0071] Among them, Δs is the average length between discrete points, cur cstr is the maximum removal rate constraint, stack i is a slack variable greater than 0;

[0072] S4-4: Real-time adjustment and feedback: During the driving process, the vehicle continuously monitors environmental data and adjusts the trajectory in real time as needed; if a new obstacle is detected or the vehicle status changes, the path needs to be replanned and a new trajectory needs to be generated in the above manner;

[0073] S4-5: Safety and redundancy measures: During the entire detour trajectory generation and tracking process, a series of safety and redundancy measures are implemented to ensure that the vehicle can safely stop or take measures to set up emergency braking systems, monitor vehicle status, and implement fault detection and diagnosis in the event of any unexpected situation.

[0074] The advantages of the present invention are:

[0075] The present invention proposes an obstacle avoidance and detour method for low-speed unmanned vehicles, which involves the input of complete perception information, static map input, and vehicle real-time status information, and generates a passable area cost map based on the input information processing, and finally plans an obstacle avoidance and detour trajectory. Through the above steps, the unmanned vehicle can generate and execute a safe and effective detour trajectory to avoid obstacles and continue to perform tasks, thereby improving the scene passability of the unmanned vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0077] Figure 1 The present invention is a flow chart of the obstacle avoidance and detour method for low-speed unmanned vehicles. DETAILED DESCRIPTION

[0078] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0079] like Figure 1 As shown, the present invention proposes an obstacle avoidance and detour method for a low-speed unmanned vehicle, comprising the following steps:

[0080] S1: Real-time input of the vehicle's surrounding perception environment information, static map information, vehicle real-time position information and vehicle real-time status information, and pre-processing to generate a drivable cost map;

[0081] S1-1: In step S1, the vehicle surrounding perception environment information, static map information, vehicle real-time posture information and vehicle real-time status information are obtained in real time, which are respectively:

[0082] The input real-time perception environment data includes the traversable area and obstacle clustering results;

[0083] The input static map data includes a static point cloud map representing static environmental characteristics and a high-precision map of road environmental characteristics;

[0084] The input vehicle posture information includes the vehicle's real-time position, real-time heading, pitch angle and other information;

[0085] The input vehicle real-time status information includes real-time vehicle speed, real-time steering wheel angle, real-time gear position, real-time vehicle mode, and real-time vehicle fault information.

[0086] S1-2: The preprocessing in step S1 generates a traversable cost map, which specifically includes the following steps:

[0087] S1-2-1: Obstacle clustering results generate the first traversable area cost map, including:

[0088] Coordinate transformation: Transform the obstacle clustering result from the vehicle coordinate system to the global coordinate system. The specific formula is as follows, where x, y, and z are the transformed global coordinates. ′ ,y ′ , z ′ is the local coordinate before conversion, and the other parameters are conversion coefficients:

[0089]

[0090] According to the center coordinates and length and width information of the obstacle, the grid points in the area selected by the obstacle are filled, and finally the first passable area cost map is generated according to the obstacle clustering result; the conversion formula is as follows, where x i ,y i is the row and column value of each grid point occupied by the obstacle; x1, y1, x2, y2 are the coordinates of the obstacle vertices:

[0091]

[0092] Step 1-2-2: The passable area perception result generates the second passable area cost map; the passable area perception result itself is represented by the occupied grid. In this step, the relevant data can be cropped according to the vehicle position information and the occupied grid size limit;

[0093] Step 1-2-3: The high-precision map of the road environment features generates a third passable area cost map; the road environment map represents lanes, curbs, sidewalks, intersections, etc. through geometric lines; extract relevant geometric lines and features to generate a static passable area cost map where vehicles can pass;

[0094] Step 1-2-4: Merge the first, second, and third passable area cost maps to generate the final total passable area cost map: Overlay the passable area cost maps generated by the above three steps to generate a complete passable area cost map at the current moment.

[0095] S2: Generate trigger detour logic. According to the preset detour logic, when the conditions are met, enter the detour planning logic. Generate trigger detour logic including the following items:

[0096] (1) The unmanned vehicle is driving on the normal mission path;

[0097] (2) During driving, the vehicle senses that the task path ahead is blocked by an obstacle and cannot continue driving;

[0098] (3) Generate a deceleration trajectory and decelerate before generating a detour trajectory;

[0099] (4) When the actual vehicle speed is less than the vehicle speed threshold v thre , the distance between the vehicle and the obstacle is within the distance threshold d thre When within range, detour is triggered.

[0100] S3: Generate a target detour distance; obtain a detour target point according to the target detour distance; specifically includes the following steps:

[0101] S3-1: Obtain the obstacle type and generate the bypass target distance according to the obstacle type and size. The specific formula is:

[0102] d goal =d to_obj +l obj +d to_goal +d safe ;

[0103] where d goal is the target distance, d to_obj is the longitudinal distance between the vehicle and the obstacle, l obj is the obstacle length, d to_goal is the preset distance from the obstacle to the target point (this distance has different preset values ​​depending on the obstacle type), d safe The safety threshold space is set;

[0104] S3-2: According to the detour target distance and the actual position of the vehicle, the detour target point is searched on the original mission path. The specific steps are as follows:

[0105] S3-2-1: According to the current position of the vehicle, find the matching point that satisfies the minimum distance on the given trajectory. The specific formula is as follows:

[0106]

[0107] where d i is the distance from all points on the trajectory to the current position of the vehicle, x i ,y i is the coordinate of the i-th track point on the track, x, y are the position coordinates of the vehicle;

[0108] After calculating the distances from all trajectory points to the actual position of the vehicle, compare all distance values ​​and take the trajectory point j with the smallest distance value (assuming the sequence number of the trajectory point is j). The trajectory point j is preset as the vehicle matching point found on the trajectory.

[0109] S3-2-2: According to the found matching points and the preset target distance in step S3-1, find the preset target point on the trajectory; the specific method is to start from the matching point found in step S3-2-1, and continue to index and calculate the distance between each point:

[0110]

[0111] Until the calculated distance and d act When the distance is just greater than the target distance calculated in step S3-1, the trajectory index value k at this moment is preset, and the coordinate point corresponding to the index value k is the target detour trajectory point.

[0112] S4: According to the detour target point and the traversable cost map, a detour trajectory is planned according to a preset planning algorithm. Specifically, the following steps are included:

[0113] S4-1: Heuristic function design; The heuristic function is used to estimate the estimated cost from the node to the target node and the actual cost from the node to the start node. The formula is as follows:

[0114] f = g + h;

[0115]

[0116] Where f is the total cost, g is the cost of moving from the vehicle position to the intermediate node (calculated using the moving steps from the vehicle's current position to the intermediate node), h is the cost of moving from the intermediate node to the target node, and x i ,y i is the coordinate value of the middle point, x goal ,y goal is the coordinate value of the target point;

[0117] S4-2: Obstacle avoidance, design a path cost function to balance the vehicle's approach to obstacles and path length, so as to plan a reasonable obstacle avoidance trajectory. The formula is as follows:

[0118]

[0119] where d θ (x,y) and d v (x, y) are the distances to the nearest obstacle and the edge of the generalized traversable area graph, and α>0, d θ>0 is a constant used to control the attenuation rate and maximum effective range of the cost function. Through the setting of the path cost function, the cost value is proportional to the total planned traversable area. Therefore, even in a narrow traversable area, passage can be guaranteed, thereby improving the obstacle avoidance and detour efficiency of the vehicle.

[0120] S4-3: Path interpolation, smoothing and optimization; The obstacle avoidance path planned in step S4-2 has the problem that the path points are too sparse and the path is not smooth. The path needs to be smoothed, interpolated and optimized, as follows:

[0121] (1) Preset trajectory interpolation point resolution d res ;

[0122] (2) Step S4-2 is repeated to plan the distance and angle between two consecutive nodes on the path. The formula is as follows:

[0123]

[0124] a angle = atan((x i -x i-1 ),(y i -t i-1 )) / 180×π;

[0125] where π dis is the distance between two consecutive nodes, α angle is the angle between two consecutive nodes, x i ,y i and x i-1 ,y i-1 is the coordinate value of two consecutive nodes;

[0126] (3) Calculate the interpolation resolution d res_act , the formula is as follows:

[0127] d res_act =d dis / (ceil(d dis / d res ));

[0128] (4) Interpolate relevant trajectory points, the formula is as follows:

[0129]

[0130] y j =y i +j×d res_act ×sin(α angle / 180×v);

[0131] (5) The objective function of smoothing and optimization processing is as follows:

[0132] cost = cost1 + cost2 + cost3;

[0133]

[0134] Among them, cost1 is the smoothness cost, cost2 is the length cost, and cost3 is the offset cost relative to the original point;

[0135] The constraints for smoothing and optimization include position constraints, curvature constraints, and slack variables:

[0136] x i-ref -x l ≤x i ≤x i-ref +x u ;

[0137] y i-ref -y l ≤y i ≤x i-ref +x u ;

[0138] 0≤stack i ≤+∞;

[0139] (x i-1 +x i+1 -2×x i ) 2 +(y i-1 +y i+1 -2×y i ) 2 -stack i ≤(Δs 2 ×cur cstr ) 2 ;

[0140] Among them, Δs is the average length between discrete points, cur cstr is the maximum removal rate constraint, stack i is a slack variable greater than 0;

[0141] S4-4: Real-time adjustment and feedback: During the driving process, the vehicle continuously monitors environmental data (including obstacle location, vehicle status, etc.) and adjusts the trajectory in real time as needed; if a new obstacle is detected or the vehicle status changes (such as increased speed, changed road conditions, etc.), the path needs to be replanned and a new trajectory needs to be generated in the above manner;

[0142] S4-5: Safety and redundancy measures: A series of safety and redundancy measures are implemented throughout the detour trajectory generation and tracking process to ensure that the vehicle can safely stop or take other appropriate actions in any unexpected situation. This may include setting up emergency braking systems, monitoring vehicle status, and implementing fault detection and diagnosis.

[0143] The above embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable people familiar with the technology to understand the content of the present invention and implement it accordingly, and they cannot be used to limit the protection scope of the present invention. Any modifications made according to the spirit of the main technical solution of the present invention should be included in the protection scope of the present invention.

Claims

1. An obstacle avoidance method for a low-speed unmanned vehicle, characterized in that: The following steps are involved: S1: Real-time input of the vehicle's surrounding perception environment information, static map information, vehicle real-time position information and vehicle real-time status information, and pre-processing to generate a drivable cost map; S2: Generate trigger detour logic. According to the preset detour logic, when the conditions are met, enter the detour planning logic; S3: Generate a target detour distance; obtain a detour target point according to the target detour distance; S4: According to the detour target point and the traversable cost map, a detour trajectory is planned according to a preset planning algorithm.

2. The obstacle avoidance method according to claim 1, characterized in that: In step S1, the vehicle surrounding perception environment information, static map information, vehicle real-time posture information and vehicle real-time status information are obtained in real time, which are respectively: The input real-time perception environment data includes the traversable area and obstacle clustering results; The input static map data includes a static point cloud map representing static environmental characteristics and a high-precision map of road environmental characteristics; The input vehicle posture information includes the vehicle's real-time position, real-time heading, pitch angle and other information; The input vehicle real-time status information includes real-time vehicle speed, real-time steering wheel angle, real-time gear position, real-time vehicle mode, and real-time vehicle fault information.

3. The obstacle avoidance method according to claim 2, characterized in that: The preprocessing in step S1 to generate a traversable cost map specifically includes the following steps: S1-2-1: Obstacle clustering results generate the first traversable area cost map, including: (1) Coordinate transformation: The obstacle clustering result is transformed from the vehicle coordinate system to the global coordinate system. The specific formula is as follows, where x, y, and z are the global coordinates after transformation. ′ ,y ′ , z ′ is the local coordinate before conversion, and the other parameters are conversion coefficients: (2) According to the center coordinates and length and width information of the obstacle, the grid points in the area selected by the obstacle are filled, and finally the first traversable area cost map is generated according to the obstacle clustering result; the conversion formula is as follows, where x i ,y i is the row and column value of each grid point occupied by the obstacle; x1, y1, X2, Y2 are the coordinates of the obstacle vertex: Step 1-2-2: The passable area perception result generates the second passable area cost map; the passable area perception result itself is represented by the occupied grid. In this step, the relevant data can be cropped according to the vehicle position information and the occupied grid size limit; Step 1-2-3: The high-precision map of the road environment features generates a third passable area cost map; the road environment map represents lanes, curbs, sidewalks, and intersections through geometric lines; extracts relevant geometric lines and features to generate a static passable area cost map where vehicles can pass; Step 1-2-4: Merge the first, second, and third passable area cost maps to generate the final total passable area cost map: Overlay the passable area cost maps generated by the above three steps to generate a complete passable area cost map at the current moment.

4. The obstacle avoidance method according to claim 1, characterized in that: The trigger detour logic generated in step S2 includes the following items: (1) The unmanned vehicle is driving on the normal mission path; (2) During driving, the vehicle senses that the task path ahead is blocked by an obstacle and cannot continue driving; (3) Generate a deceleration trajectory and decelerate before generating a detour trajectory; (4) When the actual vehicle speed is less than the vehicle speed threshold v thre , the distance between the vehicle and the obstacle is within the distance threshold d thre When within range, detour is triggered.

5. The obstacle avoidance method according to claim 1, characterized in that: Generating the detour target point in step S3 includes the following steps: S3-1: Obtain the obstacle type and generate the bypass target distance according to the obstacle type and size. The specific formula is: d goal =d to_obj +l obj +d to_goal +d safe ; where d goal is the target distance, d to_obj is the longitudinal distance between the vehicle and the obstacle, l obj is the obstacle length, d to_goal is the preset distance from the obstacle to the target point, d safe The safety threshold space is set; S3-2: According to the detour target distance and the actual position of the vehicle, the detour target point is searched on the original mission path. The specific steps are as follows: S3-2-1: According to the current position of the vehicle, find the matching point that satisfies the minimum distance on the given trajectory. The specific formula is as follows: where d i is the distance from all points on the trajectory to the current position of the vehicle, x i ,y i is the coordinate of the i-th track point on the track, x, y are the position coordinates of the vehicle; After calculating the distances from all trajectory points to the actual position of the vehicle, compare all distance values ​​and take the trajectory point j with the smallest distance value. This trajectory point j is preset as the vehicle matching point found on the trajectory. S3-2-2: According to the found matching points and the preset target distance in step S3-1, find the preset target point on the trajectory; the specific method is to start from the matching point found in step S3-2-1, and continue to index and calculate the distance between each point: Until the calculated distance and d act When the distance is just greater than the target distance calculated in step S3-1, the trajectory index value k at this moment is preset, and the coordinate point corresponding to the index value k is the target detour trajectory point.

6. The obstacle avoidance method according to claim 1, characterized in that: Planning the detour trajectory in step S4 includes the following steps: S4-1: Heuristic function design; The heuristic function is used to estimate the estimated cost from the node to the target node and the actual cost from the node to the start node. The formula is as follows: Where f is the total cost, g is the cost of moving from the vehicle position to the intermediate node, h is the cost of moving from the intermediate node to the target node, and x i ,y i is the coordinate value of the middle point, x goal ,y goal is the coordinate value of the target point; S4-2: Obstacle avoidance, design a path cost function to balance the vehicle's approach to obstacles and path length, so as to plan a reasonable obstacle avoidance trajectory. The formula is as follows: where d θ (x,y) and d v (x, y) are the distances to the nearest obstacle and the edge of the generalized traversable area graph, and α>0, d θ >0 is a constant used to control the attenuation rate and maximum effective range of the cost function; through the setting of this path cost function, the cost value is proportional to the total traversable area of ​​the plan; S4-3: Path interpolation, smoothing and optimization; The obstacle avoidance path planned in step S4-2 has the problem that the path points are too sparse and the path is not smooth. The path needs to be smoothed, interpolated and optimized, as follows: (1) Preset trajectory interpolation point resolution d res ; (2) Step S4-2 is repeated to plan the distance and angle between two consecutive nodes on the path. The formula is as follows: a angle =atan((x i -x i-1 ),(and i -and i-1 )) / 180×π; where d dis is the distance between two consecutive nodes, α angle is the angle between two consecutive nodes, x i ,y i and x i-1 ,y i-1 is the coordinate value of two consecutive nodes; (3) Calculate the interpolation resolution d res_act , the formula is as follows: d res_act =d dis / (ceil(d dis / d res )); (4) Interpolate relevant trajectory points, the formula is as follows: and j =and i +j×d res_act ×sin(α angle / 180×π); (5) The objective function of smoothing and optimization processing is as follows: cost = cost1 + cost2 + cost3; Among them, cost1 is the smoothness cost, cost2 is the length cost, and cost3 is the offset cost relative to the original point; The constraints for smoothing and optimization include position constraints, curvature constraints, and slack variables: x i-ref -x l ≤x i ≤x i-ref +x u ; and i-ref -and l ≤y i ≤x i-ref +x u ; 0≤stack i ≤+∞; (x i-1 +x i+1 -2×x i ) 2 +(y i-1 +y i+1 -2×y i ) 2 -stack i ≤(Δs 2 ×cur cstr ) 2 ; Among them, Δs is the average length between discrete points, cur cstr is the maximum removal rate constraint, stack i is a slack variable greater than 0; S4-4: Real-time adjustment and feedback: During the driving process, the vehicle continuously monitors environmental data and adjusts the trajectory in real time as needed; if a new obstacle is detected or the vehicle status changes, the path needs to be replanned and a new trajectory needs to be generated in the above manner; S4-5: Safety and redundancy measures: During the entire detour trajectory generation and tracking process, a series of safety and redundancy measures are implemented to ensure that the vehicle can safely stop or take measures to set up emergency braking systems, monitor vehicle status, and implement fault detection and diagnosis in the event of any unexpected situation.

Images