A double-layer safety redundant obstacle avoidance path planning method for narrow underground tunnels

Abstract

The present invention discloses a double-layer safety redundant obstacle avoidance path planning method for narrow underground lanes, and the steps are as follows: step 1, the obstacle sensing module uses a laser radar system to monitor the obstacle ahead; step 2, the monitoring result is no obstacle, the vehicle travels along the global path, and the obstacle avoidance local planning module is not enabled; step 3, the obstacle ahead is detected, the obstacle avoidance local planning module generates an obstacle bypassing path, and the trajectory tracking control module executes it; step 4, real-time monitoring of the effectiveness of the local obstacle bypassing path, the path fails, the side parking path generation module generates a side parking path, and the trajectory tracking control module executes the side parking; step 5, real-time monitoring of the local obstacle bypassing path is continuously effective, and the trajectory tracking control module executes continuous detour, the present invention relates to the field of unmanned driving technology. The present invention solves the problem that the traditional path planning method often ignores the flexibility of real-time obstacle avoidance and the safety redundancy of the path, and is difficult to meet the needs of special underground operations.

Description

Technical Field

[0001] The invention relates to the field of unmanned driving technology, and in particular to a double-layer safety redundant obstacle avoidance path planning method for a narrow underground tunnel. Background Art

[0002] In today's industrial automation process, the integration of artificial intelligence and unmanned driving technology is gradually becoming a key driving force for improving production efficiency. Especially in the mining industry, facing the dual challenges of extreme working environment and labor shortage, the application of unmanned transport vehicles is particularly urgent. As the main channel for the circulation of materials inside the mine, the narrow space and dense vehicle activities of the auxiliary transportation lanes of coal mines make driving safety an urgent problem to be solved. According to statistics, scratch accidents account for as much as 40% of the total vehicle accidents, which seriously affects the efficiency and safety of mine operations.

[0003] Traditional manual driving methods are not only prone to accidents in narrow lanes, but also difficult to deal with emergencies, especially when the lane width is not wide enough for two vehicles to pass safely. Therefore, developing an unmanned driving path planning technology that can adapt to the special environment underground and has highly intelligent obstacle avoidance capabilities is of great significance to improving the intelligence level of mining areas, ensuring operational safety, and improving transportation efficiency.

[0004] At present, in the research of unmanned vehicles in underground mining environments, obstacle avoidance path planning still faces many challenges, such as complex tunnels, frequent dynamic obstacles, and high requirements for real-time and accuracy of path planning. Traditional path planning methods often ignore the flexibility of real-time obstacle avoidance and the safety redundancy of the path, and it is difficult to meet the special needs of underground operations. Therefore, it has become an urgent need in the industry to design a double-layer safety redundant path planning technology that can perceive the environment in real time, dynamically plan a safe obstacle avoidance path, and have a second-layer side parking strategy to ensure that unmanned vehicles can complete transportation tasks safely and efficiently in complex environments. Summary of the invention

[0005] In order to solve the problem that traditional path planning methods often ignore the flexibility of real-time obstacle avoidance and the safety redundancy of the path, and are difficult to meet the special operation requirements underground, the purpose of the present invention is to provide a double-layer safety redundant obstacle avoidance path planning method for narrow underground tunnels.

[0006] In order to achieve the above-mentioned purpose, the present invention adopts the following technical scheme: a double-layer safety redundant obstacle avoidance path planning method for narrow underground tunnels, including an obstacle perception module, a laser radar system, an obstacle avoidance local planning module, a trajectory tracking control module and a side parking path generation module, and the specific steps are as follows:

[0007] Step 1: The obstacle perception module uses the LiDAR system to monitor obstacles ahead in real time;

[0008] Step 2: When the monitoring result shows that there is no obstacle, the vehicle drives along the global path without enabling the obstacle avoidance local planning module;

[0009] Step 3: When an obstacle is detected ahead, the obstacle avoidance local planning module generates an obstacle avoidance path, which is then executed by the trajectory tracking control module.

[0010] Step 4: monitor the effectiveness of the local obstacle avoidance path in real time. If the path fails, the pull-over path generation module generates a pull-over path, and the trajectory tracking control module executes the pull-over.

[0011] Step 5: Real-time monitoring of the local obstacle avoidance path to ensure that it remains effective, and the trajectory tracking control module performs continuous avoidance.

[0012] Preferably, the obstacle avoidance local planning module executes the following steps:

[0013] a. Use the scattered sampling algorithm to plan the path based on the reference trajectory. When the minimum distance between the obstacle and the vehicle is d min When it is less than the preset threshold, detour planning is implemented;

[0014] b. According to the minimum distance value d on the reference trajectory min and subsequent equally spaced points to generate multiple candidate longitudinal coordinates S for subsequent trajectory planning;

[0015] c. Calculate the road boundary deviation at each S, determine the point distribution range of the lateral coordinate d, and ensure the vehicle width and safety gap;

[0016] d. Use the fifth-order polynomial function in the Frenet coordinate system to describe the desired trajectory to ensure the continuity and drivability of the trajectory;

[0017] e. Optimal trajectory selection, eliminating trajectories that cannot bypass obstacles, ensuring vehicle driving safety and efficiency.

[0018] Preferably, in a, the reference trajectory is a portion intercepted from the global trajectory, and the intercepted length is a fixed value T. When the trajectory approaches the end point of the global trajectory and is less than T, it is intercepted to the end point of the global trajectory. The obstacle positions include: A. The obstacle is on the reference trajectory; B. The obstacle is on one side of the reference trajectory; The method for determining the nearest obstacle on the reference trajectory is:

[0019] For A, the obstacle affects the vehicle's driving; for B, the shortest distance I from the four corner points of the obstacle to the trajectory min , if I min <W vehicle / 2+I th , W vehicle is the vehicle width, I thIf it is the safety distance threshold, it is judged to affect the driving of the vehicle.

[0020] Preferably, in the longitudinal direction, the first point of the reference trajectory is taken as the 0 point S0 of the S coordinate, and the obstacle distance threshold d is set. obth ;

[0021] When min ≦d obth When , the detour trajectory planning is performed;

[0022] When min >d obth When , non-detour trajectory planning is performed;

[0023] When planning the detour trajectory, the longitudinal trajectory planning end point S1 is scattered as follows:

[0024] d min , d min +5,d min +10,d min +15, ..., min track end distance;

[0025] When performing non-detour trajectory planning, the longitudinal trajectory planning end point S1 is scattered as follows:

[0026] d obth , d obth +5,d obth +10,d obth +15, ..., min track end distance;

[0027] According to the longitudinal point distribution, the horizontal point distribution is performed at each longitudinal position, and the deviation from the road boundary to the expected trajectory at each S1 [d boundmin ,d boundmax ], the deviation on the left side of the boundary is positive, the deviation on the right side is negative, and the range of the scattered points is [d boundmin +0.5+w vehicle / 2,d boundmax -0.5-w vehicle / 2];

[0028] Among them, w vehicle is the vehicle width; within the value range, select a point every 0.5 meters to obtain several sets of d_1 values ​​corresponding to S_1.

[0029] Preferably, a set of expected trajectories is generated by scattering points in the longitudinal and transverse directions. The prototype of the expected trajectory is represented by a Frenet coordinate system, which is a function of the transverse deviation d relative to the longitudinal deviation s, in the form of a fifth-order polynomial, as follows:

[0030] d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5;

[0031] Constraints at the starting point:

[0032] d0 = the vehicle's current lateral deviation relative to the reference trajectory;

[0033] d0'=tan(yaw vehice -yaw road );

[0034] d0''=0;

[0035] Constraints at the end point:

[0036] d1 = deviation of lateral scattering points;

[0037] d1'=0;

[0038] d1''=0;

[0039] After s1, the value of d is constant d1;

[0040] Substitute the s value of each point on the reference trajectory into the trajectory equation and calculate the lateral deviation of each point;

[0041] When s≦s1, d=f(s);

[0042] When s>s1, d=d1;

[0043] A series of (s, d) point sequences are obtained. According to the coordinate relationship, the point sequence is transformed into the Cartesian coordinate system to obtain a detour trajectory.

[0044] The square of the lateral deviation of each point on the trajectory is summed to obtain the lateral deviation cost parameter cost d :

[0045] ;

[0046] The curvature cost parameter of the trajectory is obtained by summing the square of the curvature of each point on the trajectory. c :

[0047] ;

[0048] Trajectory change cost parameter cost l Used to indicate the deviation between the current planned trajectory and the trajectory of the previous cycle:

[0049] ;

[0050] but ;

[0051] Optimal trajectory selection:

[0052] Eliminate the trajectories that cannot bypass obstacles, in d min ≦d obth When , the elimination operation is performed; the cost of the trajectories that are not eliminated is compared, and the trajectory with the smallest cost is taken as the optimal trajectory and output to the subsequent nodes.

[0053] Preferably, the pull-over path generation module executes the steps of:

[0054] S1. When the vehicle cannot bypass the obstacle directly, plan a pull-over route with the stop point located a certain distance in front of the obstacle to ensure that there is enough safety clearance between the vehicle and the wall;

[0055] S2, using a quintic polynomial model to plan the relationship between the longitudinal displacement s and the time t to ensure that the vehicle stops smoothly while considering the constraints of the vehicle's current state;

[0056] S3. Determine the lateral parking position d to ensure that the vehicle maintains a safe distance from the lane wall when parking on the right side and does not obstruct traffic.

[0057] Preferably, when the pull-over path generation module is executed in the parking state, the prototype of the expected trajectory is expressed in a Frenet coordinate system as a function of the longitudinal displacement s relative to t in the form of a fifth-order polynomial, as follows:

[0058] s=f(t)=a0+a1t+a2t^2+a3t^3+a4t^4+a5t^5;

[0059] Constraints at the starting point:

[0060] s0 = vehicle's current longitudinal displacement;

[0061] s0' = current longitudinal speed;

[0062] s0''=0;

[0063] Constraints at the end point:

[0064] s1 = longitudinal displacement of the vehicle stop point;

[0065] s1'=0;

[0066] s1''=0;

[0067] The expected trajectory set is generated according to the longitudinal and lateral scattering points. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a quintic polynomial, as follows:

[0068] d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5;

[0069] Constraints at the starting point:

[0070] d0 = the vehicle's current lateral deviation relative to the reference trajectory;

[0071] d0'=tan(yaw vehice -yaw road );

[0072] d0''=0;

[0073] Constraints at the end point:

[0074] d1=road width / 2-safe distance between vehicle and wall+vehicle width / 2;

[0075] d1'=0;

[0076] d1''=0;

[0077] Replace this path with the current global reference path to implement local path planning.

[0078] Compared with the prior art, the present invention has the following beneficial effects:

[0079] 1. Enhanced safety and reliability: The integrated obstacle perception module monitors the surrounding environment in real time and promptly detects potential collision risks. Combined with a two-layer path planning strategy, it ensures that the vehicle can flexibly bypass or safely stop when encountering obstacles, significantly reducing the risk of collision.

[0080] 2. Improve traffic efficiency: When there are no obstacles, the vehicle can travel efficiently along the predetermined global path. When there are obstacles, it can quickly switch to local obstacle avoidance path planning, avoiding unnecessary stops and ensuring smooth traffic flow.

[0081] 3. Path planning flexibility: The scattered point sampling algorithm and quintic polynomial trajectory fitting technology are used to generate accurate detour trajectories, while taking into account vehicle width and safety distance, achieving high precision and dynamic adaptability in path planning.

[0082] 4. Optimal utilization of resources: When the obstacle avoidance path is not feasible, the pull-over strategy is automatically triggered to avoid unnecessary energy consumption and time waste. The vehicle will continue driving when conditions permit, thus optimizing resource utilization.

[0083] 5. Cost-effectiveness: By comprehensively considering the cost values ​​of lateral deviation, curvature and trajectory change, the optimal path is selected, which not only reduces the requirements for vehicle hardware, but also reduces operating costs, while improving the overall economic benefits.

[0084] 6. Improved intelligence level: This technology promotes the intelligent process of underground unmanned transportation, reduces dependence on manual driving, provides scientific and technological support for solving the problem of difficulty in recruiting miners, and is in line with the future trend of intelligent development. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0086] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0087] Figure 2 A schematic diagram of the local planning process of the obstacle avoidance detour of the present invention;

[0088] Figure 3 It is a schematic diagram of the parking point position of the present invention. DETAILED DESCRIPTION

[0089] The following is a description of the implementation of the present invention by means of specific embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification.

[0090] See also Figures 1 to 3 . It should be noted that the structures, proportions, sizes, etc. illustrated in the drawings in this specification are only used to match the contents disclosed in the specification for people familiar with this technology to understand and read, and are not used to limit the limiting conditions for the implementation of the present invention, so they have no substantial technical significance. Any modification of the structure, change in the proportional relationship, or adjustment of the size should still fall within the scope of the technical content disclosed by the present invention without affecting the effects and purposes that can be achieved by the present invention. At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" quoted in this specification are only for the convenience of description, and are not used to limit the scope of the implementation of the present invention. Changes or adjustments in their relative relationships should also be regarded as the scope of the implementation of the present invention without substantially changing the technical content.

[0091] Embodiment 1, a method for planning a double-layer safety redundant obstacle avoidance path in a narrow underground tunnel, comprising the following steps:

[0092] Step 1: The obstacle perception module based on the lidar system perceives the obstacle information in front in real time, including the relative position information between the obstacle and the vehicle, and the obstacle size information, and gives it to the motion planning model to make obstacle avoidance decisions.

[0093] Step 2: If the obstacle perception module does not sense the existence of an obstacle ahead, the obstacle avoidance planning function is not enabled and the vehicle follows the global path for tracking.

[0094] Step 3: If the obstacle perception module senses the presence of an obstacle ahead, the unmanned driving system activates the obstacle avoidance local path planning module to generate a local path to avoid the obstacle, and the trajectory tracking control model tracks the current obstacle avoidance path in real time.

[0095] Step 4: During the obstacle avoidance process of the unmanned vehicle, the local obstacle avoidance path is updated in real time. When the current wheel obstacle avoidance path planning fails during the vehicle tracking process, the second-layer safety redundant pull-over path generation module is turned on. This pull-over path is generated once and is not updated in real time.

[0096] Step 5: Based on the path generated in step 4, the vehicle determines in real time whether a local path to bypass obstacles can be generated during tracking driving. If a local path to bypass obstacles can be generated, return to step 3. Otherwise, continue tracking driving.

[0097] Specific technical details:

[0098] (1) The obstacle avoidance local path planning module is the first-level local path planning:

[0099] The local path planning scheme for oncoming vehicles and avoiding obstacles is implemented by using the scattered point sampling algorithm:

[0100] Intercept the reference trajectory:

[0101] The reference trajectory is a part intercepted from the global trajectory, and the intercepted length is fixed at 40m (tentatively set at 40m, adjusted according to actual needs). When the trajectory is close to the end of the global trajectory and is less than 40m, it is intercepted to the end of the global trajectory.

[0102] When intercepting the trajectory, start traversing from the index of the key point calculated in the previous cycle (the point closest to the vehicle on the global trajectory), traverse 40 trajectory points, and traverse the key point index of the current cycle. Start intercepting the reference trajectory from the key point.

[0103] Determination of the nearest obstacle on the reference trajectory:

[0104] In this step, we mainly determine whether there are obstacles that affect the vehicle's driving on the reference trajectory. We traverse all obstacles in the obstacle list and calculate whether the obstacles affect the vehicle's driving. If they do, we calculate the distance between the obstacle and the vehicle, and finally get the distance d of the obstacle that affects the vehicle's driving and is closest to the vehicle. min。

[0105] Obstacle judgment algorithm:

[0106] The function of the obstacle judgment algorithm is to determine whether the obstacle affects the vehicle driving.

[0107] First, the obstacles are divided into two cases: A, the obstacle crosses the reference trajectory; B, the obstacle is on one side of the reference trajectory.

[0108] For A, the obstacle will definitely affect the vehicle's driving. For B, calculate the shortest distance I from the four corner points of the obstacle to the trajectory. min , if I min <W vehicle / 2+I th , W vehicle is the vehicle width, I th If it is the safety distance threshold, it is judged to affect the driving of the vehicle.

[0109] A and B judgment method:

[0110] Calculate the distance and direction from the four corner points to the expected trajectory. Use the four corner points to traverse each point on the trajectory, find the closest point, calculate the distance and the direction angle from the trajectory point to the corner point, and compare it with the trajectory direction to get whether the corner point is on the left or right side of the trajectory. The distance to the left point is positive, and the distance to the right point is negative.

[0111] If the distances from the four corner points to the expected trajectory have positive and negative values, then A is satisfied; if the distances are all positive or all negative, then B is satisfied.

[0112] This part of the algorithm ultimately returns whether there is an obstacle that affects the passage of the vehicle, and the closest distance from the four corner points of the nearest obstacle that affects the passage of the vehicle to the vehicle.

[0113] Vertical scattering calculation:

[0114] The lattice algorithm is implemented using the Frenet coordinate system, with the longitudinal coordinate represented by s and the transverse coordinate represented by d. In the longitudinal direction, the first point of the reference trajectory is used as the 0 point s0 of the s coordinate. Set the obstacle distance threshold d obth (This threshold is tentatively set at 20m);

[0115] When min <=d obth When , the detour trajectory planning is performed;

[0116] When min >d obth When , non-detour trajectory planning is performed;

[0117] When planning the detour trajectory, the longitudinal trajectory planning end point s1 is scattered as follows:

[0118] d min , d min +5,d min +10,d min +15, ..., min (40, distance to the end of the trajectory);

[0119] When performing non-detour trajectory planning, the longitudinal trajectory planning end point s1 is scattered as follows:

[0120] 20, 25, 30, ..., min (40, distance to the end point of the trajectory);

[0121] Horizontal spread calculation

[0122] According to the above longitudinal point distribution, the horizontal point distribution is performed at each longitudinal position. First, the deviation [d boundmin , d boundmax ], the deviation on the left side of the boundary is positive, and the deviation on the right side is negative. Considering the width of the car and the safety gap, the range of the scattered points is [d boundmin +0.5+w vehicle / 2, d boundmax -0.5-w vehicle / 2];

[0123] Among them, w vehicle is the vehicle width. Within the range of values, a point is selected every 0.5 meters to obtain several sets of d_1 values ​​corresponding to s_1.

[0124] Road boundary deviation calculation method:

[0125] First, find the point on the desired trajectory whose distance from the starting point of the reference trajectory is closest to s1. Use this point to calculate the deviation.

[0126] Traverse the boundary points with the point for which the deviation needs to be calculated, find the two points closest and second closest to the point on the left boundary, and the two points closest and second closest to the point on the right boundary. Connect these two points into a straight line, calculate the distance from the point to the straight line, and use it as the calculated value of the deviation.

[0127] Generate a orbit trajectory (a trajectory in space):

[0128] According to the above longitudinal and lateral points, a set of expected trajectories can be generated. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a fifth-order polynomial, as follows:

[0129] d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5;

[0130] Constraints at the starting point:

[0131] d0 = the vehicle's current lateral deviation relative to the reference trajectory;

[0132] d0'=tan(yaw vehice -yaw road );

[0133] d0''=0;

[0134] Constraints at the end point:

[0135] d1 = deviation of lateral scattering points;

[0136] d1'=0;

[0137] d1''=0;

[0138] Based on the above constraints, the expression of the quintic polynomial can be calculated.

[0139] It should be noted that the quintic polynomial calculated by the above method is only planned to s1 and is uncontrollable after s1. This solution requires that the generated detour trajectory needs to be the same length as the reference trajectory to ensure the feasibility of the trajectory when there are multiple obstacles. After s1, the value of d is a constant d1.

[0140] Substitute the s value of each point on the reference trajectory into the trajectory equation to calculate the lateral deviation of each point

[0141] When s<= s1, d=f(s);

[0142] When s>s1, d=d 1；

[0143] After the above calculation, we can get a series of (s, d) point sequences. According to the coordinate relationship, the point sequence is transformed into the Cartesian coordinate system to obtain a detour trajectory and calculate other information on the trajectory.

[0144] Trajectory cost calculation:

[0145] In view of the mining area scenario, the vehicle speed is low and it is mainly used for transportation, so the cost calculation is simplified. The cost includes:

[0146] (1) Lateral deviation cost:

[0147] The parameter is obtained by summing the square of the lateral deviation of each point on the trajectory

[0148] ;

[0149] (2) Trajectory curvature cost:

[0150] For the curvature calculation of the smart car, the curvature calculation formula can be used, that is, the curvature K=y'' / [1+(y')^2]^(3 / 2), where y'' is the second-order derivative of the curve, and y' is the first-order derivative of the curve. First, it is necessary to obtain the function expression of the smart car's driving path. The polynomial fitting technology can be used to fit the smart car's driving path into a function expression to obtain the first-order derivative and second-order derivative of the curve. Secondly, to calculate the first-order derivative and second-order derivative of the curve, the calculus method can be used to derive the fitted function expression to obtain the first-order derivative and second-order derivative of the curve. Finally, according to the curvature calculation formula, the first-order derivative and second-order derivative of the curve are substituted into the formula to calculate the curvature of the smart car's driving path.

[0151] The parameter is obtained by summing the square of the curvature of each point on the trajectory

[0152] ;

[0153] (3) Trajectory change cost:

[0154] This parameter is used to indicate the deviation between the currently planned trajectory and the trajectory of the previous cycle. If there is no trajectory in the previous cycle, this parameter is set to 0. This parameter is obtained by summing the squares of the differences in the lateral deviations of the same index on the two trajectories.

[0155] ;

[0156] The final cost is a weighted sum of the above three parameters.

[0157] .

[0158] Optimal trajectory selection:

[0159] When selecting trajectories, first eliminate trajectories that cannot bypass obstacles. min <=d obth When , the culling operation is performed, otherwise it is not performed. The costs of the trajectories that are not culled are compared, and the trajectory with the smallest cost is taken as the optimal trajectory and output to the subsequent nodes.

[0160] Embodiment 2: A double-layer safety redundant obstacle avoidance path planning method for narrow underground tunnels, comprising an obstacle sensing module, a laser radar system, an obstacle avoidance local planning module, a trajectory tracking control module and a side parking path generation module, and the specific steps are as follows:

[0161] Step 1: The obstacle perception module uses the LiDAR system to monitor obstacles ahead in real time;

[0162] Step 2: When the monitoring result shows that there is no obstacle, the vehicle drives along the global path without enabling the obstacle avoidance local planning module;

[0163] Step 3: When an obstacle is detected ahead, the obstacle avoidance local planning module generates an obstacle avoidance path, which is then executed by the trajectory tracking control module.

[0164] Step 4: monitor the effectiveness of the local obstacle avoidance path in real time. If the path fails, the pull-over path generation module generates a pull-over path, and the trajectory tracking control module executes the pull-over.

[0165] Step 5: Real-time monitoring of the local obstacle avoidance path to ensure that it remains effective, and the trajectory tracking control module performs continuous avoidance.

[0166] The obstacle avoidance local planning module performs the following steps:

[0167] a. Use the scattered sampling algorithm to plan the path based on the reference trajectory. When the minimum distance between the obstacle and the vehicle is d min When it is less than the preset threshold, detour planning is implemented;

[0168] b. According to the minimum distance value d on the reference trajectory min and subsequent equally spaced points to generate multiple candidate longitudinal coordinates S for subsequent trajectory planning;

[0169] c. Calculate the road boundary deviation at each S, determine the point distribution range of the lateral coordinate d, and ensure the vehicle width and safety gap;

[0170] d. Use the fifth-order polynomial function in the Frenet coordinate system to describe the desired trajectory to ensure the continuity and drivability of the trajectory;

[0171] e. Optimal trajectory selection, eliminating trajectories that cannot bypass obstacles, ensuring vehicle driving safety and efficiency.

[0172] In a, the reference trajectory is a portion intercepted from the global trajectory, and the intercepted length is a fixed value T. When the trajectory approaches the end point of the global trajectory and is less than T, it is intercepted to the end point of the global trajectory. The obstacle positions include: A. The obstacle is on the reference trajectory; B. The obstacle is on one side of the reference trajectory; The method for determining the nearest obstacle on the reference trajectory is:

[0173] For A, the obstacle affects the vehicle's driving; for B, the shortest distance I from the four corner points of the obstacle to the trajectory min , if I min <W vehicle / 2+I th , W vehicle is the vehicle width, I th If it is the safety distance threshold, it is judged to affect the driving of the vehicle.

[0174] In the vertical direction, the first point of the reference trajectory is taken as the 0 point S0 of the S coordinate, and the obstacle distance threshold d is set. obth ;

[0175] When min ≦d obth When , the detour trajectory planning is performed;

[0176] When min >d obth When , non-detour trajectory planning is performed;

[0177] When planning the detour trajectory, the longitudinal trajectory planning end point S1 is scattered as follows:

[0178] d min , d min +5,d min +10,d min +15, ..., min track end distance;

[0179] When performing non-detour trajectory planning, the longitudinal trajectory planning end point S1 is scattered as follows:

[0180] d obth , d obth +5,d obth +10,d obth +15, ..., min track end distance;

[0181] According to the longitudinal point distribution, the horizontal point distribution is performed at each longitudinal position, and the deviation from the road boundary to the expected trajectory at each S1 [d boundmin ,d boundmax ], the deviation on the left side of the boundary is positive, the deviation on the right side is negative, and the range of the scattered points is [d boundmin +0.5+w vehicle / 2,d boundmax -0.5-w vehicle / 2];

[0182] Among them, w vehicle is the vehicle width; within the value range, select a point every 0.5 meters to obtain several sets of d_1 values ​​corresponding to S_1.

[0183] The expected trajectory set is generated by scattering points in the longitudinal and lateral directions. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a quintic polynomial, as follows:

[0184] d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5;

[0185] Constraints at the starting point:

[0186] d0 = the vehicle's current lateral deviation relative to the reference trajectory;

[0187] d0'=tan(yaw vehice -yaw road );

[0188] d0''=0;

[0189] Constraints at the end point:

[0190] d1 = deviation of lateral scattering points;

[0191] d1'=0;

[0192] d1''=0;

[0193] After s1, the value of d is constant d1;

[0194] Substitute the s value of each point on the reference trajectory into the trajectory equation and calculate the lateral deviation of each point;

[0195] When s≦s1, d=f(s);

[0196] When s>s1, d=d1;

[0197] A series of (s, d) point sequences are obtained. According to the coordinate relationship, the point sequence is transformed into the Cartesian coordinate system to obtain a detour trajectory.

[0198] The square of the lateral deviation of each point on the trajectory is summed to obtain the lateral deviation cost parameter cost d :

[0199] ;

[0200] The curvature cost parameter of the trajectory is obtained by summing the square of the curvature of each point on the trajectory. c :

[0201] ;

[0202] Trajectory change cost parameter cost l Used to indicate the deviation between the current planned trajectory and the trajectory of the previous cycle:

[0203] ;

[0204] but ;

[0205] Optimal trajectory selection:

[0206] Eliminate the trajectories that cannot bypass obstacles, in d min ≦d obthWhen , the elimination operation is performed; the cost of the trajectories that are not eliminated is compared, and the trajectory with the smallest cost is taken as the optimal trajectory and output to the subsequent nodes.

[0207] The steps of the pull-over path generation module are as follows:

[0208] S1. When the vehicle cannot bypass the obstacle directly, plan a pull-over route with the stop point located a certain distance in front of the obstacle to ensure that there is enough safety clearance between the vehicle and the wall;

[0209] S2, using a quintic polynomial model to plan the relationship between the longitudinal displacement s and the time t to ensure that the vehicle stops smoothly while considering the constraints of the vehicle's current state;

[0210] S3. Determine the lateral parking position d to ensure that the vehicle maintains a safe distance from the lane wall when parking on the right side and does not obstruct traffic.

[0211] When the pull-over path generation module is executed in the parking state, the prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the longitudinal displacement s relative to t, in the form of a fifth-order polynomial, as follows:

[0212] s=f(t)=a0+a1t+a2t^2+a3t^3+a4t^4+a5t^5;

[0213] Constraints at the starting point:

[0214] s0 = vehicle's current longitudinal displacement;

[0215] s0'=current longitudinal speed;

[0216] s0''=0;

[0217] Constraints at the end point:

[0218] s1 = longitudinal displacement of the vehicle stop point;

[0219] s1'=0;

[0220] s1''=0;

[0221] The expected trajectory set is generated according to the longitudinal and lateral scattering points. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a quintic polynomial, as follows:

[0222] d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5;

[0223] Constraints at the starting point:

[0224] d0 = the vehicle's current lateral deviation relative to the reference trajectory;

[0225] d0'=tan(yaw vehice -yaw road );

[0226] d0''=0;

[0227] Constraints at the end point:

[0228] d1=road width / 2-safe distance between vehicle and wall+vehicle width / 2;

[0229] d1'=0;

[0230] d1''=0;

[0231] Replace this path with the current global reference path to implement local path planning.

[0232] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the art may modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by a person of ordinary skill in the art without departing from the spirit and technical concept disclosed by the present invention shall still be covered by the claims of the present invention.

Claims

1. A double-layer safety redundant obstacle avoidance path planning method for narrow underground tunnels, comprising an obstacle sensing module, a laser radar system, an obstacle avoidance local planning module, a trajectory tracking control module and a side parking path generation module, characterized in that: The specific steps are: Step 1: The obstacle perception module uses the LiDAR system to monitor obstacles ahead in real time; Step 2: When the monitoring result shows that there is no obstacle, the vehicle drives along the global path without enabling the obstacle avoidance local planning module; Step 3: When an obstacle is detected ahead, the obstacle avoidance local planning module generates an obstacle avoidance path, which is then executed by the trajectory tracking control module. Step 4: monitor the effectiveness of the local obstacle avoidance path in real time. If the path fails, the pull-over path generation module generates a pull-over path, and the trajectory tracking control module executes the pull-over. Step 5: Real-time monitoring of the local obstacle avoidance path to ensure that it is continuously effective, and the trajectory tracking control module performs continuous avoidance; The obstacle avoidance local planning module performs the following steps: a. Use the scattered sampling algorithm to plan the path based on the reference trajectory. When the minimum distance between the obstacle and the vehicle is d min When it is less than the preset threshold, detour planning is implemented; b. According to the minimum distance value d on the reference trajectory min and subsequent equally spaced points to generate multiple candidate longitudinal coordinates S for subsequent trajectory planning; c. Calculate the road boundary deviation at each S, determine the point distribution range of the lateral coordinate d, and ensure the vehicle width and safety gap; d. Use the fifth-order polynomial function in the Frenet coordinate system to describe the desired trajectory to ensure the continuity and drivability of the trajectory; e. Optimal trajectory selection, eliminating trajectories that cannot bypass obstacles, ensuring vehicle driving safety and efficiency; In a, the reference trajectory is a portion intercepted from the global trajectory, and the intercepted length is a fixed value T. When the trajectory approaches the end point of the global trajectory and is less than T, it is intercepted to the end point of the global trajectory. The obstacle positions include: A. The obstacle is on the reference trajectory; B. The obstacle is on one side of the reference trajectory; The method for determining the nearest obstacle on the reference trajectory is: For A, the obstacle affects the vehicle's driving; for B, the shortest distance I from the four corner points of the obstacle to the trajectory min , if I min <W vehicle / 2+I th ,W vehicle is the vehicle width, I th If it is the safety distance threshold, it is judged to affect the driving of the vehicle; In the vertical direction, the first point of the reference trajectory is taken as the 0 point S0 of the S coordinate, and the obstacle distance threshold d is set obth ; When min ≦d obth When , the detour trajectory planning is performed; When min >d obth When , non-detour trajectory planning is performed; When planning the detour trajectory, the longitudinal trajectory planning end point S1 is scattered as follows: d min , d min +5,d min +10,d min +15, ..., min track end distance; When performing non-detour trajectory planning, the longitudinal trajectory planning end point S1 is scattered as follows: d obth , d obth +5,d obth +10,d obth +15, ..., min track end distance; According to the longitudinal point distribution, the horizontal point distribution is performed at each longitudinal position, and the deviation from the road boundary to the expected trajectory at each S1 [d boundmin ,d boundmax ], the deviation on the left side of the boundary is positive, the deviation on the right side is negative, and the range of the scattered points is [d boundmin +0.5+w vehicle / 2,d boundmax -0.5-w vehicle / 2]; Among them, w vehicle is the width of the vehicle; within the range of values, a point is taken every 0.5 meters to obtain several sets of values ​​of d_1 corresponding to S_1; The expected trajectory set is generated by scattering points in the longitudinal and lateral directions. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a quintic polynomial, as follows: d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5; Constraints at the starting point: d0 = the vehicle's current lateral deviation relative to the reference trajectory; d0'=tan(yaw vehice -yaw road ); d0”＝0； Constraints at the end point: d1 = deviation of lateral scattering points; d1'＝0； d1”＝0； After s1, the value of d is constant d1; Substitute the s value of each point on the reference trajectory into the trajectory equation and calculate the lateral deviation of each point; When s≦s1, d=f(s); When s>s1, d=d1; A series of (s, d) point sequences are obtained. According to the coordinate relationship, the point sequence is transformed into the Cartesian coordinate system to obtain a detour trajectory. The square of the lateral deviation of each point on the trajectory is summed to obtain the lateral deviation cost parameter cost d : The curvature cost parameter of the trajectory is obtained by summing the square of the curvature of each point on the trajectory. c : Trajectory change cost parameter cost l Used to indicate the deviation between the current planned trajectory and the trajectory of the previous cycle: cost l =∑(d i -dlast i ) 2 ; Then cost = k d ·cost d +k c ·cost c +k l ·cost l ; Optimal trajectory selection: Eliminate the trajectories that cannot bypass obstacles, in d min ≦d obth When , the elimination operation is performed; the cost of the trajectories that are not eliminated is compared, and the trajectory with the smallest cost is taken as the optimal trajectory and output to the subsequent nodes; The steps of the pull-over path generation module are as follows: S1. When the vehicle cannot bypass the obstacle directly, a path to pull over is planned, and the stop point is located a certain distance in front of the obstacle to ensure that there is enough safety clearance between the vehicle and the wall; S2, using a quintic polynomial model to plan the relationship between the longitudinal displacement s and the time t to ensure that the vehicle stops smoothly while considering the constraints of the vehicle's current state; S3. Determine the lateral parking position d to ensure that the vehicle maintains a safe distance from the lane wall when parking on the right side and does not obstruct traffic; When the pull-over path generation module is executed in the parking state, the prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the longitudinal displacement s relative to t, in the form of a fifth-order polynomial, as follows: s=f(t)=a0+a1t+a2t^2+a3t^3+a4t^4+a5t^5; Constraints at the starting point: s0 = current longitudinal displacement of the vehicle; s0' = current longitudinal speed; s0”＝0； Constraints at the end point: s1 = longitudinal displacement of the vehicle stop point; s1'＝0； s1”＝0； The expected trajectory set is generated according to the longitudinal and lateral scattering points. The prototype of the expected trajectory is expressed in the Frenet coordinate system as a function of the lateral deviation d relative to the longitudinal deviation s, in the form of a quintic polynomial, as follows: d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5; Constraints at the starting point: d0 = the vehicle's current lateral deviation relative to the reference trajectory; d0'=tan(yaw vehice -yaw road ); d0”＝0； Constraints at the end point: d1 = road width / 2-safe distance between vehicle and wall+vehicle width / 2; d1'＝0； d1”＝0； Replace this path with the current global reference path to implement local path planning.

Images