CN116380086A - A trajectory planning method for unmanned mining trucks based on drivable area - Google Patents

Abstract

The invention discloses a trajectory planning method for an unmanned mining truck based on a drivable area, which includes acquiring sensor data of an unmanned mining truck; generating a drivable area in real time based on different operating scenarios according to the acquired sensor data; Randomly sample along the starting point in the driving area to generate an initial path, and perform simplification and path key point screening to generate a local path; perform global optimization according to the obstacle information in the sensor data and the local path, and obtain the final path of the global optimization of the path planning in the drivable area . The present invention generates a drivable area by acquiring sensor data, and plans an optimized path, so that trajectory planning can be performed by using existing sensors of the vehicle without relying on high-precision maps. The cost is low, the applicability to the scene is stronger, and the problem of falling into the local optimal dilemma is avoided at the same time.

Description

A Trajectory Planning Method for Unmanned Mining Trucks Based on Drivable Area

technical field

The invention relates to the technical field of track planning for unmanned mining trucks, in particular to a track planning method for unmanned mining trucks based on a drivable area.

Background technique

Unmanned trajectory planning technology is one of the core technologies of autonomous vehicles. At present, the path planning of unmanned mining trucks in open-pit mines is also extremely important, which can make more reasonable planning in mine operations and improve operational efficiency.

The disadvantage of the existing technology is that it relies on high-precision maps for path planning, and high-precision maps are expensive to produce, and mine roads change quickly, making it difficult to maintain later. At present, self-driving vehicles are equipped with sensors such as lidar, millimeter-wave radar, and cameras. Using existing sensors for algorithm processing can obtain the vehicle's feasible area. The information of the drivable area is more accurate than the high-precision map, and it can handle some trajectory planning problems in extreme scenarios.

Contents of the invention

The purpose of the present invention is to overcome the deficiencies in the prior art. In order to achieve the above purpose, a method of trajectory planning for unmanned mining trucks based on the drivable area is adopted to solve the problems raised in the above-mentioned background technology.

A method for trajectory planning of unmanned mining trucks based on a drivable area, comprising:

Step S1, obtaining the sensor data of the unmanned mining truck;

Step S2, generating a drivable area in real time based on the acquired sensor data and based on different operating scenarios;

Step S3, randomly sampling along the starting point in the drivable area to generate an initial path, and performing simplification and screening of key points of the path to generate a partial path;

Step S4, perform global optimization according to the obstacle information in the sensor data and the local path, and obtain the final path of the global optimization of the drivable area path planning.

As a further solution of the present invention: the sensor data in the step S1 includes lidar data, millimeter wave radar data, camera data, and obstacle data.

As a further solution of the present invention: the specific steps of the step S2 include:

Scenes are divided according to the mine operation environment. The scenes include the main road tracking scene, the intersection scene, and the loading and parking scene. At the same time, according to the road and cloud scheduling information, the main road tracking scene, the intersection scene, and the loading and parking scene convert between

If the current main road tracking scene or intersection scene, the sensor data is obtained, and the obtained sensor data is processed to generate obstacle information, and finally the drivable area under the Frenet coordinate system is generated;

If the current scene is loading and parking, acquire sensor data, process the acquired sensor data at the same time to generate obstacle information, set the local coordinate system and the rectangular frame of the obstacle, and finally generate the drivable area.

As a further solution of the present invention: when the current main road tracking scene or intersection scene, the specific steps of generating the drivable area include:

Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information;

Convert the obstacle coordinates to Frenet coordinates, convert the obstacle coordinates to Frenet coordinates according to the road centerline, and construct each obstacle SLBoundary in the Frent coordinate system;

At the same time, filter obstacles greater than 3.5m away from the centerline of the road;

Generate drivable areas in Frenet coordinates from virtual road boundaries and unfiltered obstacles.

As a further solution of the present invention: when the current scene is loading and parking, the specific steps of generating a drivable area include:

Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information;

Obtain the parking space information, and set the origin of the local coordinate system at the same time, where the left vertex of the warehouse is set as the origin, and the left vertex to the right vertex is the x-axis;

Convert the obstacle coordinates to the coordinates of the current local coordinates, and set the rectangular border as the obstacle driving range;

Get the drivable area according to the rectangle frame and obstacle information.

As a further solution of the present invention: the specific steps of the step S3 include:

In the drivable area obtained under different operating scenarios, an initial path is generated along the starting point of the vehicle in a random sampling manner;

performing a simplification operation on the initial path to obtain a simplified path, wherein the simplified path includes path key points after the path points in the initial path are simplified;

Simultaneously perform intelligent sampling on all path key points in the simplified path, and obtain the intelligently sampled path;

Based on the distance between the waypoint and the boundary of the drivable area and the total length of the path, the paths before and after intelligent sampling are filtered, and a local path is generated according to the filtering results.

As a further solution of the present invention: the specific steps of the step S4 include:

S41. Calculate the speed limit for each point of the local path path according to the road speed limit, curvature, and obstacle information, and generate an obstacle ST map according to the obstacle and the local path, where t is the ordinate, s is the abscissa, The slope is the derivative of the s value to t, that is, the speed. The larger the slope, the higher the speed;

S42. Construct a two-dimensional cost_table table based on the generated obstacle ST map, and use a dynamic programming algorithm to traverse to find an optimal value to obtain a final path.

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

Using the above-mentioned technical solution, by acquiring sensor data, generating a drivable area, and planning an optimized path, trajectory planning can be performed by using existing sensors of the vehicle without relying on high-precision maps. The cost is low, the applicability to the scene is stronger, and the problem of falling into the local optimal dilemma is avoided at the same time, and the safety and smoothness of vehicle driving are improved.

Description of drawings

Below in conjunction with accompanying drawing, the specific embodiment of the present invention is described in detail:

FIG. 1 is a schematic diagram of steps of a trajectory planning method disclosed in an embodiment of the present application;

FIG. 2 is a schematic diagram of a scene of an embodiment disclosed in the present application;

Fig. 3 is a schematic diagram of the steps in the main road tracking scene and the intersection scene in the disclosed embodiment of the present application;

FIG. 4 is a schematic diagram of steps in a loading and parking scenario according to an embodiment disclosed in the present application.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

Please refer to Fig. 1, in the embodiment of the present invention, a method for trajectory planning of unmanned mining trucks based on drivable areas, including:

Step S1, acquiring the sensor data of the unmanned mining truck, the sensor data includes lidar data, millimeter wave radar data, camera data, and obstacle data;

In the specific implementation steps of this embodiment, the data information of various sensors in the mining truck is collected in real time through the unmanned mining truck system;

Step S2. Generate drivable areas in real time based on the acquired sensor data and based on different operating scenarios. The specific steps include:

Step S21, divide the scene according to the mine operation environment, as shown in Figure 2, the diagram is a schematic diagram of scene management, the scene includes the main road tracking scene, the intersection scene, and the loading and parking scene, and the drivable area is generated under different scenes The method is different. The default scene is the main road tracking scene. At the same time, according to the road and cloud scheduling information, the main road tracking scene, the intersection scene, and the loading and parking scene are converted;

Step S22, if the current main road tracking scene or intersection scene, as shown in Figure 3, the schematic diagram of the steps in the main road tracking scene and intersection scene is shown, acquire sensor data, and at the same time process the acquired sensor data Perform processing to generate obstacle information, and finally generate the drivable area in the Frenet coordinate system;

When the current main road tracking scene or intersection scene, the specific steps to generate the drivable area include:

Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information;

Convert the obstacle coordinates to Frenet coordinates, convert the obstacle coordinates to Frenet coordinates according to the road centerline, and construct each obstacle SLBoundary in the Frent coordinate system;

At the same time, filter obstacles greater than 3.5m away from the centerline of the road;

Generate drivable areas in Frenet coordinates from virtual road boundaries and unfiltered obstacles.

Step S23, if the current scene is loading and parking, as shown in Figure 4, the diagram is a schematic diagram of the steps in the loading and parking scene, acquire sensor data, and process the acquired sensor data to generate obstacle information, and set local coordinates The rectangular borders of the system and obstacles, and finally generate the drivable area.

When the current parking scene is loaded, the specific steps for generating the drivable area include:

Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information;

Obtain the parking space information, and set the origin of the local coordinate system at the same time, where the left vertex of the warehouse is set as the origin, and the left vertex to the right vertex is the x-axis;

Convert the obstacle coordinates to the coordinates of the current local coordinates, and set the rectangular border as the obstacle driving range;

Get the drivable area according to the rectangle frame and obstacle information.

Step S3, path planning:

Randomly sample along the starting point in the drivable area to generate an initial path, and perform simplification and path key point screening to generate a local path. The specific steps include:

Step S31, by determining the drivable area of the vehicle, and in the drivable area under different operating scenarios, generate an initial path along the starting point of the vehicle in a random sampling manner;

performing a simplification operation on the initial path to obtain a simplified path, wherein the simplified path includes path key points after the path points in the initial path are simplified;

Simultaneously perform intelligent sampling on all path key points in the simplified path, and obtain the intelligently sampled path;

Based on the distance between the waypoint and the boundary of the drivable area and the total length of the path, the paths before and after intelligent sampling are filtered, and a local path is generated according to the filtering results. Based on this method, the scientificity and applicability of the path planning results can be improved, and the global optimization of the path planning in the drivable area can be realized.

The specific steps are:

a. Waypoint sampling:

Sampling of waypoints along the starting point of the vehicle in the practicable area in a random sampling manner;

b. Connect the sampling points with a quintic polynomial;

c. Design the evaluation function to evaluate the path and select the path with the least cost:

Evaluate the sampled paths according to collision cost, path length cost and path smoothness cost, and select the one with the smallest cost as the result of local path planning.

Step S4, speed planning:

According to the obstacle information in the sensor data and the local path, the global optimization is carried out to obtain the final path of the global optimization of the path planning of the drivable area. The specific steps include:

S41. Calculate the speed limit for each point of the local path path according to the road speed limit, curvature, and obstacle information, and generate an obstacle ST map according to the obstacle and the local path, where t is the ordinate, s is the abscissa, The slope is the derivative of the s value to t, that is, the speed. The larger the slope, the higher the speed;

The specific implementation steps of generating the obstacle ST map are as follows:

Calculate the speed limit for each point of the path based on the road speed limit, curvature, and obstacle information. Obstacle ST map is generated according to obstacles and local paths, and obstacle ST map and speed limit are plotted on the coordinate system with t as ordinate and s as abscissa. On the ST diagram, the slope represents the derivative of s value for t, that is, the speed. The larger the slope, the higher the speed; the second order derivative of s for t represents the acceleration.

S42. Construct a two-dimensional cost_table table based on the generated obstacle ST map, and use a dynamic programming algorithm to traverse to find the optimal value to obtain the final path;

The specific implementation steps of decision-making on obstacles are as follows:

First construct a two-dimensional cost_table and initialize the cost_table. Then according to the speed of the current point, the maximum acceleration and minimum deceleration are used to calculate the s range at the next moment, and then the total_cost value is calculated for the cells in the range. The total_cost value includes four parts: the cost related to obstacles, determined by the distance of obstacles; the cost of the distance from the path end point, the closer to the end point, the smaller the cost value; the total_cost at the previous moment; the cost related to speed, acceleration, and jerk . Using the dynamic programming algorithm, the calculation of the total_cost of each grid will depend on the total_cost value at the previous moment, and a problem is divided into a series of sub-problems for processing. The last two types of cost values need to rely on the cells at the previous moment, so the cells at the previous moment are traversed to find the one that minimizes the total_cost value of the current cell as the optimal pre_point. In fact, the data structure of a linked list is used here, so that the cells in each column will be connected to the cell in the previous column with the smallest total_cost value, so that in the end, only by finding the optimal cell at the end point, you can backtrack to get a the whole track.

Finally, the velocity programming solver is constructed as a quadratic optimization problem and solved using the osqp solver.

Although the embodiments of the present invention have been shown and described, those skilled in the art can understand that various changes, modifications and substitutions can be made to these embodiments without departing from the principle and spirit of the present invention. and modifications, the scope of the present invention is defined by the appended claims and their equivalents, all should be included in the protection scope of the present invention.

Claims (7)

1. A kind of unmanned mining truck track planning method based on drivable area, it is characterized in that, comprising: Step S1, obtaining the sensor data of the unmanned mining truck; Step S2, generating a drivable area in real time based on the acquired sensor data and based on different operating scenarios; Step S3, randomly sampling along the starting point in the drivable area to generate an initial path, and performing simplification and screening of key points of the path to generate a partial path; Step S4, perform global optimization according to the obstacle information in the sensor data and the local path, and obtain the final path of the global optimization of the drivable area path planning. 2. A kind of unmanned mining truck track planning method based on drivable area according to claim 1, characterized in that, the sensor data in the step S1 includes lidar data, millimeter wave radar data, camera data, and obstacle data. 3. A kind of unmanned mining truck track planning method based on drivable area according to claim 1, is characterized in that, the concrete steps of described step S2 comprise: Scenes are divided according to the mine operation environment. The scenes include the main road tracking scene, the intersection scene, and the loading and parking scene. At the same time, according to the road and cloud scheduling information, the main road tracking scene, the intersection scene, and the loading and parking scene convert between If the current main road tracking scene or intersection scene, the sensor data is obtained, and the obtained sensor data is processed to generate obstacle information, and finally the drivable area under the Frenet coordinate system is generated; If the current scene is loading and parking, acquire sensor data, process the acquired sensor data at the same time to generate obstacle information, set the local coordinate system and the rectangular frame of the obstacle, and finally generate the drivable area. 4. according to claim 3, a kind of unmanned mining truck trajectory planning method based on drivable area, characterized in that, when the current main road tracking scene or intersection scene, the specific steps of generating drivable area include: Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information; Convert the obstacle coordinates to Frenet coordinates, convert the obstacle coordinates to Frenet coordinates according to the road centerline, and construct each obstacle SLBoundary in the Frent coordinate system; At the same time, filter obstacles greater than 3.5m away from the centerline of the road; Generate drivable areas in Frenet coordinates from virtual road boundaries and unfiltered obstacles. 5. A kind of unmanned mining truck trajectory planning method based on drivable area according to claim 3, characterized in that, when the current scene is loading and parking, the specific steps of generating drivable area include: Obtain lidar, millimeter-wave radar, and camera data, and process the acquired sensor data to generate obstacle information; Obtain the parking space information, and set the origin of the local coordinate system at the same time, where the left vertex of the warehouse is set as the origin, and the left vertex to the right vertex is the x-axis; Convert the obstacle coordinates to the coordinates of the current local coordinates, and set the rectangular border as the obstacle driving range; Get the drivable area according to the rectangle frame and obstacle information. 6. A kind of unmanned mining truck track planning method based on drivable area according to claim 1, is characterized in that, the concrete steps of described step S3 comprise: In the drivable area obtained under different operating scenarios, an initial path is generated along the starting point of the vehicle in a random sampling manner; performing a simplification operation on the initial path to obtain a simplified path, wherein the simplified path includes path key points after the path points in the initial path are simplified; Simultaneously perform intelligent sampling on all path key points in the simplified path, and obtain the intelligently sampled path; Based on the distance between the waypoint and the boundary of the drivable area and the total length of the path, the paths before and after intelligent sampling are filtered, and a local path is generated according to the filtering results. 7. A kind of unmanned mining truck track planning method based on drivable area according to claim 1, is characterized in that, the specific steps of described step S4 comprise: S41. Calculate the speed limit for each point of the local path path according to the road speed limit, curvature, and obstacle information, and generate an obstacle ST map according to the obstacle and the local path, where t is the ordinate, s is the abscissa, The slope is the derivative of the s value to t, that is, the speed. The larger the slope, the higher the speed; S42. Construct a two-dimensional cost_table table based on the generated obstacle ST map, and use a dynamic programming algorithm to traverse to find an optimal value to obtain a final path.

Images