Path planning method and device based on three-dimensional occupancy grid map

Abstract

The present invention discloses a path planning method and device based on a three-dimensional occupancy grid map. The method comprises the following steps: obtaining and preprocessing the original point cloud data, filtering the original point cloud data using a random sampling consistency algorithm to remove the ground point cloud, and obtaining a non-ground point cloud; performing coordinate transformation on the non-ground point cloud to construct a three-dimensional occupancy grid map, looping through the angle range set in the three-dimensional occupancy grid map to generate path points at different angles, associating the path points with the voxel grid of the sensor sensing range, and obtaining several alternative paths that reach the boundary of the sensor sensing range; selecting the path with the highest score from the several alternative paths based on a preset optimal evaluation function, using a collision check mechanism as a constraint condition, obtaining an optimal path, and using the optimal path as the execution path of the mining vehicle. The present invention can achieve accurate positioning and reliable navigation of the seabed mining vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot path planning, and in particular to a path planning method and device based on a three-dimensional occupancy grid map. Background Art

[0002] Seabed mining vehicles are mainly designed to collect polymetallic nodules on the seafloor surface at 1,000 to 6,000 meters underwater. They are equipped with hydraulic or mechanical working heads, power stations, electronic equipment, and flexible hoses or cables, and have the ability to be remotely operated in extremely harsh seabed environments. However, the working environment on the seabed is extremely harsh, which poses a huge challenge to the control and navigation of mining vehicles. Due to the rapid attenuation of electromagnetic signals in water, traditional land-based GPS positioning systems cannot provide accurate positioning for seabed mining vehicles. Therefore, there is an urgent need to propose a path planning and navigation method suitable for deep seabed environments. Summary of the invention

[0003] In order to solve the above technical problems, the present invention proposes a path planning method and device based on a three-dimensional occupancy grid map, which can achieve accurate positioning and reliable navigation of seabed mining vehicles. It fully considers the complexity of the seabed environment and the safety of the mining vehicle, and provides reliable navigation support for deep seabed mining tasks.

[0004] In order to achieve the above object, the technical solution of the present invention is as follows:

[0005] The path planning method based on the three-dimensional occupancy grid map includes the following steps:

[0006] The original point cloud data is obtained and preprocessed, and the random sampling consistency algorithm is used to filter the original point cloud data to remove the ground point cloud and obtain the non-ground point cloud;

[0007] The coordinate transformation of the non-ground point cloud is performed to construct a three-dimensional occupancy grid map, and the angle range set in the three-dimensional occupancy grid map is looped to generate path points at different angles, and the path points are associated with the voxel grid of the sensor sensing range to obtain several alternative paths to the boundary of the sensor sensing range;

[0008] Based on a preset optimal evaluation function, a path with the highest score is selected from the plurality of candidate paths, a collision check mechanism is used as a constraint condition, an optimal path is obtained, and the optimal path is used as an execution path for the mining vehicle.

[0009] Preferably, the optimal trajectory tracking control is performed by using an Ackerman model predictive control algorithm, so that the mining vehicle can complete autonomous navigation.

[0010] Preferably, the preprocessing includes retaining points whose distance and height are within a preset range while excluding invalid points, and performing downsampling processing.

[0011] Preferably, a random sampling consistency algorithm is used to filter the original point cloud data to remove the ground point cloud, which specifically includes the following steps:

[0012] A certain number of points are randomly selected from the preprocessed point cloud and a plane model is constructed using the least squares method based on the selected sample points;

[0013] Calculate the distance between the remaining points and the fitted plane model;

[0014] Points whose distance is less than the preset threshold are regarded as inliers and added to the plane model. Multiple iterations are performed until the number of inliers reaches the preset condition, completing the fitting of the ground point cloud and obtaining the non-ground point cloud.

[0015] Preferably, a spline interpolation method is used to perform path smoothing processing on a plurality of candidate paths.

[0016] Preferably, in the process of determining the alternative paths, there are three situations in which the path points are not selected, namely: 1) the direction of each path point is compared with the direction of the target point. If the angle between the direction of the path point and the direction of the target point exceeds a set threshold, the path point will be ignored; 2) the angle between the direction of the path point and the direction of the target point meets the set threshold, but the angle between the direction of the path point and the current direction of the mining vehicle exceeds the set threshold, the path point will be ignored; 3) if the mining vehicle needs to turn to the path point, and its turning path direction exceeds plus or minus 90 degrees, the path point will be ignored.

[0017] Preferably, the optimal evaluation function is as follows:

[0018]

[0019] Where W is the weight, θ d is the angle difference between the path and the target point, θ r S is the angle difference between the direction of the path and the current direction of the mining vehicle. p is the penalty score given according to the obstacles on the path.

[0020] Preferably, the collision check mechanism adopts a collision detection method based on circular contour description, and performs collision detection by using two circles with the same radius to describe the contour of the mining vehicle, which specifically includes the following steps:

[0021] Assume the length of the mining vehicle is L s , width W s, the coordinates of the center point P of the mining vehicle are (x, y), and the direction of the vehicle body is θ, then the center point P of the front circumscribed circle is a The coordinates of the rear circumscribed circle and the center P b The coordinates are expressed as:

[0022]

[0023]

[0024] The radii of both circles are:

[0025]

[0026] Assume the coordinates of the obstacle point cloud are (x p ,y p ), the coordinates of the center of the circle are (x, y). When it is outside the circle, it should satisfy:

[0027] (xx p ) 2 +(yy p ) 2 >R 2 (10)

[0028] When the obstacle point cloud coordinates satisfy formula (10), it indicates that the obstacle is far away from the mining vehicle and there is no collision.

[0029] Preferably, the optimal trajectory tracking control using the Ackerman model predictive control algorithm specifically includes the following steps:

[0030] Get the current mining vehicle state x at time k k And the control input u k , calculate the state of the mining vehicle at time k+1, and transform the linear time-varying model into a nonlinear model in the following form:

[0031]

[0032] in is the state quantity of the system, is the input of the system, are the expected state and input quantities respectively, p x ,p y are the x and y coordinates of the rear wheels of the mining vehicle, v is the inertial system velocity, is the attitude angle of the mining vehicle relative to the inertial system, δ is the angle of the front wheel control corner, a is the rear wheel control acceleration, g c is the additional coefficient compared to the linear system,

[0033] The mining vehicle model is nonlinearized, and the formula is as follows:

[0034]

[0035] in L is the wheelbase of the mining vehicle, are the desired inertial system speed, the desired attitude angle, and the desired front wheel control turning angle, respectively. Formula (13) is discretized to facilitate solution and control, and the obtained form is as follows:

[0036]

[0037] Where T s is the discrete time step, I is the unit matrix, and the objective function J of the model predictive control is designed, where x ref ,y ref is the expected coordinate, The expected posture is as follows:

[0038]

[0039] Constraints limit speed and acceleration. The formula is as follows:

[0040]

[0041] Based on the above content, the present invention also discloses a path planning device based on a three-dimensional occupancy grid map, including: a point cloud processing module, a path generation module, a path confirmation module and a control module, wherein:

[0042] The point cloud processing module is used to obtain and pre-process the original point cloud data, and use a random sampling consistency algorithm to filter the original point cloud data to remove the ground point cloud to obtain the non-ground point cloud;

[0043] The path generation module is used to perform coordinate transformation on the non-ground point cloud to construct a three-dimensional occupancy grid map, loop through the angle range set in the three-dimensional occupancy grid map to generate path points at different angles, associate the path points with the voxel grid of the sensor sensing range, and obtain a number of alternative paths to reach the boundary of the sensor sensing range;

[0044] The path confirmation module is used to select the path with the highest score from the plurality of candidate paths based on a preset optimal evaluation function, take the collision check mechanism as a constraint condition, obtain the optimal path and use the optimal path as the mining vehicle execution path;

[0045] The control module is used to track and control the optimal trajectory using an Ackerman model predictive control algorithm, so that the mining vehicle can complete autonomous navigation.

[0046] Based on the above technical solution, the beneficial effects of the present invention are:

[0047] 1) Considering that traditional GPS navigation faces many difficulties in the submarine environment, such as signal attenuation, multipath effects, etc., it is easy to cause positioning errors. The present invention uses sensor technology to obtain submarine point cloud and obstacle point cloud data, and combines the Ackerman model predictive control algorithm to realize path navigation. It does not rely on GPS signals, can realize autonomous positioning and navigation, and ensures positioning accuracy and stability in complex submarine environments;

[0048] 2) Compared with traditional ROV subsea mining vehicles, ROVs rely on the operator's experience and skills to avoid obstacles, which is prone to errors and accidents. The present invention can more efficiently ensure the safety and reliability of work during path planning and navigation;

[0049] 3) Current seabed mining vehicles usually rely on pre-set paths or simple obstacle avoidance algorithms for navigation, but for complex seabed environments, this method is often difficult to achieve accurate path planning and tracking. In contrast, the present invention combines actual conditions and can dynamically adjust the path in real-time perception of the seabed environment, avoid obstacles in a timely manner, and ensure the accuracy and safety of navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 is a schematic diagram of a path planning method based on a three-dimensional occupancy grid map in an embodiment;

[0051] Figure 2 is a sampling diagram of alternative paths in an embodiment;

[0052] Figure 3 is a schematic diagram of an occupancy grid map of a mining vehicle in the Gazebo and Rviz simulation platforms without RANSAC filtering;

[0053] Figure 4 In one embodiment, a mining vehicle is Figure 3 Schematic diagram of the occupancy grid map of the unmanned vehicle in the scenario using RANSAC filtering;

[0054] Figure 5 is a schematic diagram of an obstacle avoidance scenario of a mining vehicle in the Gazebo and Rviz simulation platforms in one embodiment;

[0055] Figure 6 is a schematic diagram of a desired trajectory and an actual trajectory obtained by a mining vehicle performing path tracking according to a model predictive control algorithm in one embodiment;

[0056] Figure 7 is a schematic diagram of the linear speed obtained by a mining vehicle performing path tracking according to a model predictive control algorithm in one embodiment;

[0057] Figure 8is a schematic diagram of the angular velocity obtained by a mining vehicle performing path tracking according to a model predictive control algorithm in one embodiment;

[0058] Fig. 9 The present invention is a schematic block diagram of a path planning device based on a three-dimensional occupancy grid map in one embodiment. DETAILED DESCRIPTION

[0059] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0060] like Figure 1 As shown, this embodiment provides a path planning method based on a three-dimensional occupancy grid map, comprising the following steps:

[0061] A. Ground point filtering process

[0062] Use LiDAR to obtain obstacle information around the mining vehicle and publish point cloud data. First, filter the point cloud to retain points within a certain range of distance and height, and exclude invalid points. Pass the preprocessed point cloud data to the filter for downsampling to obtain the downsampled point cloud data; use the random sampling consensus (RANSAC) algorithm to randomly select a certain number of points from the downsampled point cloud to form a plane model. This plane model can be expressed as the following equation:

[0063] Mx + Ny + Kz + H = 0 (1)

[0064] Among them, (M, N, K) is the normal vector of the plane, (x, y, z) is the coordinates of the point, and H is the offset parameter of the plane.

[0065] Using the selected sample points, the parameters of the plane model (normal vector and offset parameters) are calculated by the least squares method or other methods. The distances from all points to the fitting plane are calculated, and the points whose distances are less than the set threshold are regarded as inliers and added to the plane model. After multiple iterations, if the number of inliers reaches the set threshold, the fitting of the ground point cloud is considered to be completed, and the point cloud data after removing the ground points is obtained, completing the ground filtering process.

[0066] B. TF coordinate transformation of point cloud data

[0067] The point cloud data after removing the ground points is obtained, and then expanded to obtain point cloud data offset by a certain amount in a certain direction. Through the monitoring coordinate transformation in the TF library, the point cloud data in the radar coordinate system is converted to the mining vehicle odometer coordinate system and published as a topic to serve as a local point cloud map for subsequent autonomous navigation of the mining vehicle.

[0068] C. Occupancy grid map construction

[0069] To process a frame of LiDAR data and its corresponding mining vehicle pose information, first calculate the grid number of the mining vehicle's location in the grid map. Then, analyze all the LiDAR data scan points in the frame one by one, and calculate the grid number corresponding to each laser reflection event in the grid map pixel coordinate system. Draw a line from the mining vehicle's current grid number to the grid number of each laser scan point, identify all unoccupied grid numbers on the line path, traverse these unoccupied grids, and update their status to reflect that they are "free". For each grid hit by the laser scan, update its status to ensure that the grid map reflects the latest environmental status.

[0070] D. Path sampling strategy

[0071] Design a path sampling algorithm to generate multiple candidate paths that reach the sensor's sensing range boundary, use the spline interpolation method to smooth the candidate paths, and discretize the candidate paths to obtain the following: Figure 2 The sampling path shown, Figure 3 It is a schematic diagram of the occupancy grid map of the mining vehicle of the present invention under the Gazebo and Rviz simulation platforms without using RANSAC for filtering. Figure 4 For the mining vehicle of the present invention Figure 3 Schematic diagram of the occupancy grid map of the unmanned vehicle in the scenario using RANSAC filtering.

[0072] (1) Generate a set of multiple candidate paths that reach the edge of the sensor's sensing range, use the spline interpolation method to smooth the path, loop through the set angle range to generate paths with different angles, calculate the starting point of the path in the internal loop, use spline interpolation to calculate the angle β of the starting path, and then obtain the x and y coordinates of the starting path. The main calculation formula is as follows:

[0073] x=V×cos(β×pi / 180) (2)

[0074] y=V×sin(β×pi / 180) (3)

[0075] Where V is a 1×101 row vector and β is the angle of the starting path. At the same time, create an array of all zeros of the same size as the x and y coordinates, which is used as the z coordinate of the starting path. Interpolate it in 100 equal parts in the range [0-1], and there are 9×101 path points. The next two nested loops are used to generate the complete path. In the range [1-2], each of the 9 basic paths is linearly interpolated to generate 9 paths, so there are 9×9=81 paths. In the range [2-3], each of the previous 81 paths is linearly interpolated to generate 9 paths, for a total of 81×9=729 paths. The generated path points are saved in three PLY files, which record the path points of the first sampling, which is a 4×909 matrix, all the path points generated by three samplings, which is a 5×219429 matrix, and the last path point of each path, which is a 5×729 matrix.

[0076] (2) Find the correspondence between the path points and voxels, and use a voxel grid covering the sensor range to facilitate subsequent collision checks. The grid voxel range is a trapezoid consisting of (0, r), (0, -r), (X, Y), (X, -Y). Where r is the search radius, and X, Y are the offsets of the voxels.

[0077] Two nested loops are used to generate voxel coordinates: the outer loop traverses the number of voxels in the X direction, and the inner loop traverses the number of voxels in the Y direction. The voxel coordinates are calculated and stored according to the voxel index. The α parameter is introduced in the outer loop to ensure that the sampling path remains unchanged when calculating from the outside to the inside, and the width of the Y-axis position close to the car body decreases as the parameter α increases. The calculation formula for α is as follows:

[0078] From the grid range, we can get

[0079]

[0080] Further

[0081]

[0082] After determining the range of the voxel grid, find the voxel points around the path point, calculate the distance between the path point and the voxel point, and save the distance and correspondence. Then, for each voxel point, loop through the path points around it and write the index of the path point into the TXT file.

[0083] E. Path screening

[0084] When processing the point cloud data, it will be cropped according to the position of the mining vehicle, and only the point cloud data near the mining vehicle will be retained. Point clouds that exceed the set threshold range will be ignored. In addition, there are three situations that will cause the path to not be selected:

[0085] (1) During the path selection process, the directions of path points whose angle difference with the target exceeds a set threshold are ignored, regardless of the current direction of the mining vehicle; during the path planning process, the direction of each path point is compared with the direction of the target point. If the angle between the direction of the path point and the direction of the target point exceeds the set threshold, the path point will be ignored. The purpose of this method is to ensure that the direction of path planning is always towards the target and to reduce the deviation of the path points. The handling of this situation does not take into account the current direction of the mining vehicle, but only depends on the relationship between the direction of the path point and the direction of the target point.

[0086] (2) When considering the current direction of the mining vehicle, if the angle of the mining vehicle toward the target point is between -90 and 90 degrees, but the direction of the waypoint exceeds the threshold range, then the direction is ignored; in this case, not only the difference between the direction of the waypoint and the direction of the target point needs to be considered, but also the current direction of the mining vehicle. If the angle between the current direction of the mining vehicle and the target point is between -90 and 90 degrees (that is, the mining vehicle is basically close to the target direction), but the difference between the direction of the waypoint and the current direction of the mining vehicle exceeds the set threshold angle, then this waypoint will also be ignored.

[0087] (3) If the direction of the mining vehicle's turning path exceeds plus or minus 90 degrees, the direction is also ignored. In this case, if the mining vehicle needs to turn to the direction of a certain path point, but the turning angle exceeds plus or minus 90 degrees, then this path direction will be ignored. This is because a turning angle exceeding 90 degrees usually means that a large turn is required, which not only increases the complexity of the path, but may also increase the difficulty and time cost of turning.

[0088] F. Scoring function design

[0089] Use the path planner to find obstacles through point cloud data and delete paths blocked by obstacles. Only passable paths are retained, and among all passable paths, each path is scored and a more suitable local path with the highest score is selected. The scoring calculation formula is as follows:

[0090]

[0091] Where W is the weight, θ d is the angle difference between the path and the target point, θ r S is the angle difference between the direction of the path and the current direction of the mining vehicle. p is the penalty score given according to the obstacles on the path.

[0092] G. Collision Detection

[0093] A collision detection method based on circular contour description is adopted, in which two circles with the same radius are used to describe the contour of the mining vehicle for collision detection. Figure 5 It is a schematic diagram of the obstacle avoidance scene of the mining vehicle of the present invention under the Gazebo and Rviz simulation platforms.

[0094] Assume the length of the mining vehicle is L s , width W s , the coordinates of the center point P of the mining vehicle are (x, y), and the direction of the vehicle body is θ, then the center point P of the front circumscribed circle is a The coordinates of the center of the rear circumscribed circle P b The coordinates can be expressed as:

[0095]

[0096]

[0097] The radii of both circles are:

[0098]

[0099] Assume the coordinates of the obstacle point cloud are (x p ,y p ), the coordinates of the center of the circumscribed circle are (x*, y*). When it is outside the circle, it should satisfy:

[0100] (x*-x p ) 2 +(y*-y p ) 2 >R 2 (10)

[0101] When the obstacle point cloud coordinates satisfy the above formula, it indicates that the obstacle is far away from the mining vehicle and there is no collision.

[0102] H. Trajectory tracking control

[0103] According to the Ackerman model, a model predictive control algorithm is designed to achieve tracking control of the optimal trajectory. Figure 6 This is a schematic diagram of the expected trajectory and actual trajectory obtained by the mining vehicle through path tracking based on the model predictive control algorithm. Figure 7 Schematic diagram of the linear velocity obtained by the mining vehicle through path tracking based on the model predictive control algorithm.

[0104] Figure 8 This is a schematic diagram of the angular velocity obtained by the mining vehicle through path tracking based on the model predictive control algorithm. The specific steps are:

[0105] Since the steering angles of the two front wheels of the mining vehicle are different, and the rotation radius of the two rear wheels is also different, the four-wheel Ackerman motion model is equivalently simplified and converted into a single vehicle model. According to the Ackerman model, a model predictive control algorithm is designed to achieve tracking control of the optimal trajectory. First, the Ackerman model belongs to a linear time-invariant system, where A and B are coefficients, and the form is as follows:

[0106] x k+1 =Ax k +Bu k (11)

[0107] Get the current mining vehicle state x at time k k And the control input u k , the state of the mining vehicle at time k+1 is obtained through calculation, and the linear time-varying model is converted into a nonlinear model in the following form:

[0108]

[0109] in is the state quantity of the system, is the input of the system, are the expected state and input quantities respectively, p x ,p y are the x and y coordinates of the rear wheels of the mining vehicle, v is the inertial system velocity, is the attitude angle of the mining vehicle relative to the inertial system, δ is the angle of the front wheel control corner, a is the rear wheel control acceleration, g c is the extra coefficient compared to the linear system. At the same time, the above linearization formula is used to nonlinearize the mining vehicle model.

[0110]

[0111] in L is the wheelbase of the mining vehicle, are the desired inertial system speed, the desired attitude angle, and the desired front wheel control turning angle, respectively. The above formula (13) is discretized to facilitate solution and control, and the obtained form is as follows:

[0112]

[0113] Where T s is the discrete time step, and I is the identity matrix. Design the objective function of model predictive control, where x ref ,y ref is the expected coordinate, The expected posture is as follows:

[0114]

[0115] Constraints limit speed and acceleration. The formula is as follows:

[0116]

[0117] The specific steps for actual application in the invention are as follows: obtain the current state of the mining vehicle, including position and posture, by subscribing to the odom topic. Receive path information by subscribing to the / path_t topic, and the path is transmitted in the form of a point cloud. After receiving the path information, process the path and extract the coordinate points and heading angle information on the path. According to the extracted path information, calculate the speed plan and determine the target speed of each path point. Use the model predictive control (MPC) method to optimize and solve the mining vehicle state and path information at each moment to obtain the optimal control amount. Send the obtained control amount to the cmd_vel topic to control the movement of the mining vehicle. Regularly publish predicted path information to the predict_path topic for visualization.

[0118] It should be understood that, although the various steps in the above-mentioned flow chart are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the above-mentioned flow chart may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these sub-steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the sub-steps or stages of other steps.

[0119] like Fig. 9 As shown, in one embodiment, a path planning device 100 based on a three-dimensional occupancy grid map is provided, comprising: a point cloud processing module 110, a path generation module 120, a path confirmation module 130 and a control module 140, wherein:

[0120] Point cloud processing module 110, used to obtain and pre-process the original point cloud data, and filter the original point cloud data using a random sampling consistency algorithm to remove the ground point cloud to obtain a non-ground point cloud;

[0121] A path generation module 120 is used to perform coordinate transformation on the non-ground point cloud to construct a three-dimensional occupancy grid map, loop through the angle range set in the three-dimensional occupancy grid map to generate path points at different angles, associate the path points with the voxel grid of the sensor sensing range, and obtain a number of alternative paths to reach the boundary of the sensor sensing range;

[0122] A path confirmation module 130 is used to select the path with the highest score from the plurality of candidate paths based on a preset optimal evaluation function, take the collision check mechanism as a constraint condition, obtain the optimal path and use the optimal path as the mining vehicle execution path;

[0123] The control module 140 is used to track and control the optimal trajectory using an Ackerman model predictive control algorithm, so that the mining vehicle can complete autonomous navigation.

[0124] The devices and modules described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0125] Each embodiment in the embodiments of this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0126] The above are only preferred embodiments of the embodiments of the present application and are not intended to limit the embodiments of the present application. For those skilled in the art, the embodiments of the present application may have various modifications and variations. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the embodiments of the present application shall be included in the protection scope of the embodiments of the present application.

Claims

1. A path planning method based on a three-dimensional occupancy grid map, characterized in that: The steps include: The original point cloud data is obtained and preprocessed, and the random sampling consistency algorithm is used to filter the original point cloud data to remove the ground point cloud and obtain the non-ground point cloud; The coordinate transformation of the non-ground point cloud is performed to construct a three-dimensional occupancy grid map, and the angle range set in the three-dimensional occupancy grid map is looped to generate path points at different angles, and the path points are associated with the voxel grid of the sensor sensing range to obtain several alternative paths to the boundary of the sensor sensing range; Based on the preset optimal evaluation function, the path with the highest score is selected from the plurality of candidate paths, and the collision check mechanism is used as a constraint condition to obtain the optimal trajectory and use the optimal trajectory as the mining vehicle execution path, wherein, The optimal trajectory tracking control is performed by using the Ackerman model predictive control algorithm, so that the mining vehicle can complete autonomous navigation, which specifically includes the following steps: Get the current mining vehicle state x at time k k And the control input u k , calculate the state of the mining vehicle at time k+1, and transform the linear time-varying model into a nonlinear model in the following form: in is the state quantity of the system, is the input of the system, are the expected state and input respectively, p x ,p y are the x and y coordinates of the rear wheels of the mining vehicle, v is the inertial system velocity, is the attitude angle of the mining vehicle relative to the inertial system, δ is the angle of the front wheel control corner, a is the rear wheel control acceleration, g c is the additional coefficient compared to the linear system, The mining vehicle model is nonlinearized, and the formula is as follows: in L is the wheelbase of the mining vehicle, are the desired inertial system speed, the desired attitude angle, and the desired front wheel control turning angle, respectively. Formula (13) is discretized to facilitate solution and control, and the obtained form is as follows: Where T s is the discrete time step, I is the unit matrix, and the objective function J of the model predictive control is designed, where x ref ,y ref is the expected coordinate, The expected posture is as follows: Constraints limit speed and acceleration. The formula is as follows:

2. The path planning method based on a three-dimensional occupancy grid map according to claim 1, characterized in that: The preprocessing includes retaining points whose distance and height are within a preset range while excluding invalid points, and performing downsampling processing.

3. The path planning method based on a three-dimensional occupancy grid map according to claim 2, characterized in that: The random sampling consistency algorithm is used to filter the original point cloud data to remove the ground point cloud, which specifically includes the following steps: A certain number of points are randomly selected from the preprocessed point cloud and a plane model is constructed using the least squares method based on the selected sample points; Calculate the distance between the remaining points and the fitted plane model; Points whose distance is less than the preset threshold are regarded as inliers and added to the plane model. Multiple iterations are performed until the number of inliers reaches the preset condition, completing the fitting of the ground point cloud and obtaining the non-ground point cloud.

4. The path planning method based on a three-dimensional occupancy grid map according to claim 1, characterized in that: The spline interpolation method is used to smooth the paths of several candidate paths.

5. The path planning method based on a three-dimensional occupancy grid map according to claim 1, characterized in that: In the process of determining the candidate paths, there are three situations where the path points are not selected, namely: 1) the direction of each path point is compared with the direction of the target point, and if the angle between the direction of the path point and the direction of the target point exceeds a set threshold, the path point will be ignored; 2) If the angle between the direction of the waypoint and the direction of the target point meets the set threshold, but the angle between the direction of the waypoint and the current direction of the mining vehicle exceeds the set threshold, the waypoint will be ignored; 3) If the mining vehicle needs to turn to the waypoint and its turning path direction exceeds plus or minus 90 degrees, the waypoint will be ignored.

6. The path planning method based on a three-dimensional occupancy grid map according to claim 1, characterized in that: The optimal evaluation function is as follows: Where W is the weight, θ d is the angle difference between the path and the target point, θ r S is the angle difference between the direction of the path and the current direction of the mining vehicle. p is the penalty score given according to the obstacles on the path.

7. The path planning method based on a three-dimensional occupancy grid map according to claim 1, characterized in that: The collision detection mechanism adopts a collision detection method based on circular contour description, and performs collision detection by using two circles with the same radius to describe the contour of the mining vehicle, which specifically includes the following steps: Assume the length of the mining vehicle is L s , width W s , the coordinates of the center point P of the mining vehicle are (x, y), and the direction of the vehicle body is θ, then the center point P of the front circumscribed circle is a The coordinates of the rear circumscribed circle and the center P b The coordinates are expressed as: The radii of both circles are: Assume the obstacle point cloud coordinates are (x p ,y p ), the coordinates of the center of the circle are (x, y). When it is outside the circle, it should satisfy: (x-x p ) 2 +(y-y p ) 2 >R 2 (10) When the obstacle point cloud coordinates satisfy formula (10), it indicates that the obstacle is far away from the mining vehicle and there is no collision.

8. A path planning device based on a three-dimensional occupancy grid map, characterized in that: include: Point cloud processing module, path generation module, path confirmation module and control module, among which, The point cloud processing module is used to obtain and pre-process the original point cloud data, and use a random sampling consistency algorithm to filter the original point cloud data to remove the ground point cloud to obtain the non-ground point cloud; The path generation module is used to perform coordinate transformation on the non-ground point cloud to construct a three-dimensional occupancy grid map, loop through the angle range set in the three-dimensional occupancy grid map to generate path points at different angles, associate the path points with the voxel grid of the sensor sensing range, and obtain a number of alternative paths to reach the boundary of the sensor sensing range; The path confirmation module is used to select the path with the highest score from the plurality of candidate paths based on a preset optimal evaluation function, take the collision check mechanism as a constraint condition, obtain the optimal trajectory and use the optimal trajectory as the mining vehicle execution path; The control module is used to track and control the optimal trajectory using the Ackerman model predictive control algorithm so that the mining vehicle can complete autonomous navigation. Specifically, it includes the following steps: obtaining the current state x of the mining vehicle at time k k And the control input u k , calculate the state of the mining vehicle at time k+1, and transform the linear time-varying model into a nonlinear model in the following form: in is the state quantity of the system, is the input of the system, are the expected state and input quantities respectively, p x ,p y are the x and y coordinates of the rear wheels of the mining vehicle, v is the inertial system velocity, is the attitude angle of the mining vehicle relative to the inertial system, δ is the angle of the front wheel control corner, a is the rear wheel control acceleration, g c is the additional coefficient compared to the linear system, The mining vehicle model is nonlinearized, and the formula is as follows: in L is the wheelbase of the mining vehicle, are the desired inertial system speed, the desired attitude angle, and the desired front wheel control turning angle, respectively. Formula (13) is discretized to facilitate solution and control, and the obtained form is as follows: Where T s is the discrete time step, I is the unit matrix, and the objective function J of the model predictive control is designed, where x ref ,y ref is the expected coordinate, The expected posture is as follows: Constraints limit speed and acceleration. The formula is as follows:

Images