Running track high-performance collision inspection method for unmanned mine car in strip mine area

Abstract

The invention belongs to the field of running tracks of automatic mine cars in open-pit mines, and particularly relates to a running track high-performance collision inspection method for unmanned mine cars in open-pit mines. According to the scheme, the method comprises the following steps that firstly, a map formed by scattered point coordinates of obstacles is marked as map, a driving track to be evaluated is marked as traj, and road points specifically comprise fields x, y, theta and v which represent rear wheel center point coordinates, attitude angles and vehicle speed of a vehicle respectively; calculating the bulking radius R of the vehicle body by using a preset method FunA for the map and the traj, bulking an obstacle according to the R, and removing a road point colliding with the bulking obstacle to obtain a residual track trajres1; and then the vehicle pose information on each road point on the trajres1 is judged in the map by using a preset FinB. According to the scheme, the time required for collision inspection can be shortened for multiple times, the inspection efficiency of collision inspection is improved, and the problem that an existing collision inspection method is not high in calculation efficiency is solved.

Description

Technical Field

[0001] This solution belongs to the field of driving trajectory of autonomous mining vehicles in open-pit mines, and specifically involves a high-performance collision inspection method for driving trajectory of unmanned mining vehicles in open-pit mines. Background Art

[0002] The driving trajectory of a smart car needs to ensure driving safety, that is, to ensure that the vehicle does not collide with obstacles in the environment.

[0003] Obstacle avoidance check on a driving trajectory is an existence check. There are a series of waypoints along the driving trajectory. If any one of them is found to be a collision, the entire driving trajectory is considered unsafe.

[0004] The existing vehicle driving trajectory collision check method often performs collision checks on a series of discrete road points on the driving trajectory from beginning to end. Once the vehicle body is found to overlap with an obstacle, the above-mentioned one-by-one checking process is exited and the collision risk is reported. However, the existing collision check method is not efficient in calculation, because the later points in a series of road points may involve collisions. When the existing collision check method checks the driving trajectory one by one, a lot of checking time is wasted on the front points (points with a lower probability of collision), resulting in an increase in the calculation time of most collision checks, which makes the calculation efficiency of the collision check method low. Summary of the invention

[0005] The purpose of this scheme is to provide a high-performance collision detection method for the driving trajectory of unmanned mining vehicles in open-pit mines to solve the problem of low computational efficiency of existing collision detection methods.

[0006] In order to achieve the above object, the present invention provides a high-performance collision detection method for the driving trajectory of unmanned mining vehicles in open-pit mines, which is characterized by comprising the following steps:

[0007] S10: The map composed of the scattered coordinates of the obstacles is recorded as map, and the driving trajectory to be evaluated is recorded as traj. The road points specifically include the fields x, y, theta, and v, which represent the coordinates of the center point of the rear wheel of the vehicle, the attitude angle, and the speed of the vehicle, respectively. The preset method FunA is used to calculate the expansion radius R of the vehicle body for map and traj, and then the obstacles are expanded according to R, and the points far away from the obstacles are roughly filtered out to obtain the residual trajectory traj_res1.

[0008] S20: using the preset method FinB to judge the vehicle posture information at each waypoint in traj_res1 in the map, further eliminating the points far from the obstacles, and obtaining the set of remaining waypoints traj_res2;

[0009] S30: using the preset method FunC to rearrange all points of traj_res2 in the map according to the driving speed information, giving priority to collision check for the road points with lower absolute speed values ​​in traj_res2, and finally outputting the check result.

[0010] The principle and technical effect of this solution are as follows: First, this solution roughly screens out road points that are far away from obstacles based on the expansion radius R of the vehicle body (road points that are far away from obstacles will not collide). This method can quickly focus computing power on road points with a higher probability of collision. Compared with the existing method of checking collisions one by one, more road points are checked in the same time, which improves the speed of collision checking and thus improves the efficiency of collision checking. Secondly, this solution only uses the vehicle posture information to judge each road point on the residual trajectory. The amount of data of the residual trajectory and vehicle posture information is small. This step can quickly eliminate road points that are far away from obstacles and further quickly narrow the inspection scope. Finally, this solution checks the remaining waypoints one by one. Since the first two steps have narrowed the scope of the check, the scope of the check is greatly reduced, and the time required for the check is also greatly reduced; at the same time, the remaining waypoints are rearranged according to the driving speed information, and the roadpoints with lower absolute speed values ​​on the trajectory are given priority for collision check. This is an innovation and is also a business-related property unique to unmanned mining trucks in mines. That is, when the speed is low, the vehicle is often on the edge of a retaining wall or cliff in the environment, and the risk of collision is higher at this time. Prioritizing high-risk waypoints helps to identify the collision as early as possible, thereby ending the complete collision check process in advance (the complete collision check process is to find the existence of the collision waypoint. Instead of inefficiently checking all waypoints one by one, it is better to quickly check the key waypoints. Once the existence of the collision is identified, the remaining unnecessary judgment process can be omitted, thereby saving CPU time).

[0011] In summary, this solution provides a high-performance collision check method for the driving trajectory of unmanned mining vehicles in open-pit mines, which reduces the time required for collision checking many times, improves the efficiency of collision checking, and solves the problem of low computational efficiency of existing collision checking methods.

[0012] 3. Further, a variable traj_res1 is set in FunA of step S10, and FunA includes the following steps:

[0013] S12: Calculate the expansion radius R of the vehicle body using the vehicle width, front suspension length, wheelbase, and rear suspension length of the vehicle;

[0014] S13: performing a dilation operation on the scattered obstacle points in the map with a dilation radius of R to obtain a dilated map map_dilated;

[0015] S14: Initialize traj_res1 to an empty set;

[0016] S15: Check each waypoint wp on the candidate trajectory traj one by one i Whether it falls into the obstacle area of ​​the expanded map map_dilated. If there is a waypoint falling into the obstacle area, the waypoint falling into the obstacle area will be recorded in the set traj_res1 and traj_res1 will be output.

[0017] Furthermore, the FunA also includes a variable rA, and the step S11 is included before the step S12, and the step S11 is: initializing rA=1; in the step S15, rA and traj_res1 are output simultaneously.

[0018] Furthermore, in step S12, the calculation formula of the puffing radius R is:

[0019]

[0020] Among them, L b , L f , L w and L r They represent vehicle width, front suspension length, wheelbase and rear suspension length respectively.

[0021] This solution uses parameters such as vehicle width, front suspension length, wheelbase and rear suspension length to comprehensively consider the size and dynamic characteristics of the vehicle, which helps to more accurately eliminate road points that are farther away from obstacles, thereby more accurately retaining road points with a high probability of collision.

[0022] Furthermore, in step S20, FunB loops and checks each waypoint wp on traj_res1 one by one. i , when checking, use wp i Calculate the four vertices corresponding to the vehicle, select the four vertices of the vehicle on the map, get the minimum selection range and record it as the conservative rectangular box area, and then determine wp i Whether the corresponding conservative rectangular box area falls into the obstacle area of ​​the original map map, if there is a waypoint falling into it, the waypoint falling into it will be recorded in the set traj_res2, and traj_res2 will be output after the loop ends.

[0023] By checking the waypoints wpi in traj_res1 one by one and calculating the four vertices of the vehicle at that point, the space actually occupied by the vehicle can be simulated more accurately. By calculating the conservative rectangular frame area formed by the vehicle vertices, a minimum rectangular area covering the vehicle can be obtained, so that waypoints with extremely low collision probability can be eliminated more accurately, further reducing the scope of one-by-one inspection, thereby improving calculation efficiency.

[0024] Further, FunB uses wp i The calculation of the four vertices corresponding to the vehicle includes the following steps:

[0025] S22: Extract vehicle in wp i The coordinate position and posture fields of the point are recorded as three-dimensional coordinate values ​​(wp i .x,wp i .y,wp i .theta);

[0026] S23: Calculate the four vertices A, B, C, and D corresponding to the vehicle at the current waypoint based on formula (2), and formula (2) is as follows:

[0027] A=(A x ,A y )=[wp i .x+(L w +L f )·cos(wp i. theta)-0.5L b ·

[0028] sin(wp i. theta),wp i. y+(L w +L f )·sin(wp i. theta)+0.5L b ·cos(wp i. theta)],

[0029] B=(B x ,B y )=[wp i. x+(L w +L f )·cos(wp i. theta)+0.5L b ·sinθ,wp i. y+

[0030] (L w +L f )·sin(wp i. theta)-0.5Lb ·cos(wp i. theta)],

[0031] C=(C x ,C y )=(wp i. xL r ·cosθ+0.5L b ·sin(wp i. theta),wp i. y r ·

[0032] sin(wp i. theta)-0.5L b ·cos(wp i. theta)),

[0033] D=(D x ,D y )=(wp i. xL r ·cosθ-0.5L b ·sin(wp i. theta),wp i. y r ·

[0034] sin(wp i. theta)+0.5L b ·cos(wp i. theta)).(2)

[0035] In this way, by accurately calculating the space occupied by the vehicle at each path point, potential collision risks can be identified more effectively and the collision screening can be more rigorous. At the same time, simplifying the complex vehicle shape into a rectangular box is an approximation, but in many cases it is sufficient for collision detection and simplifies the complexity of the problem.

[0036] Furthermore, when FunB determines the conservative rectangular box area through the four vertices of the vehicle, the following steps are included:

[0037] S24: Calculate xmin, xmax, ymin and ymax based on formula (3), which is as follows:

[0038] xmin=min{A x ,B x ,C x ,D x}

[0039] xmax=max{A x ,Bx ,C x ,D x}

[0040] ymin=min{A y ,B y ,C y ,D y}

[0041] ymax=max{A y ,B y ,C y ,D y} (3)

[0042] S25: Determine a conservative rectangular frame area according to xmin, xmax, ymin and ymax.

[0043] Through these steps, FunB ensures that the space occupied by the vehicle is accurately calculated during path planning and that potential collision risks can be effectively detected. The conservative nature of this approach means that it may produce some "false positive" results (i.e., incorrectly judged as collisions, when in fact no collisions will occur), but doing so ensures that no possible collision risks are missed, so that the calculated expansion results will not have omissions.

[0044] Further, FunB determines wp i When determining whether the corresponding conservative rectangular box area falls into the obstacle area of ​​the original map, the coordinates of an obstacle point in the map are (xi, yi) and the judgment result is calculated by formula (4). Formula (3) is as follows:

[0045] (xi>xmin)∩(xi<xmax)∩(yi> ymin)∩(yi <ymax) (4)。

[0046] Due to the short-circuit characteristic of the "AND" operation, the truth or falsehood judgment of the above expression can be completed quickly, thereby reducing the operation time and improving the calculation efficiency of the collision checking method.

[0047] Further, in the step S30, a variable rC is set in FunC, and FunC includes the following steps:

[0048] S31: rearrange all elements in traj_res2 from small to large according to the absolute values ​​of the speed v field, and call the set of waypoint sequences after the rearrangement S;

[0049] S32: for each element in the set S, check in order whether the vehicle footprint corresponding to the waypoint collides with the map. If a collision occurs, rC=0, and rC is output as the collision result. Otherwise, the element is removed from the set S.

[0050] S33: Repeat the above process until there are no elements in the S set, assign rC a value of 1, and then output rC as the collision result.

[0051] By sorting by absolute speed, waypoints with lower speeds can be prioritized, which may help identify potential collision risks more quickly. At the same time, by checking and eliminating waypoints without collision one by one, unnecessary calculations can be reduced and the efficiency of collision detection can be improved.

[0052] Furthermore, in S32, when checking for collisions one by one in order, judgment is performed at intervals of M elements in the S set. If the current number of elements in the S set is less than M, the last element in the S set is taken for judgment, where M is a parameter specified by the user.

[0053] The rough judgment method of judging every M points can improve the judgment range and identify the potential collision hazards at various locations as early as possible. It can also cooperate with FunA and FunB, using FunA and FunB as pre-processing steps to identify and eliminate the waypoints that are obviously far away from obstacles at a very low time cost, so that FunC can focus on the key waypoints with potential collision hazards. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 This is a schematic diagram of the calculation of the puffing radius R by FunA in the embodiment of this scheme.

[0055] Figure 2 This is a schematic diagram of FunB determining the conservative rectangular frame area in the embodiment of this scheme. DETAILED DESCRIPTION

[0056] The following will clearly and completely describe the concept and technical effects of the present invention in combination with the embodiments, so as to fully understand the purpose, features and effects of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, other embodiments obtained by those skilled in the art without creative work are all within the scope of protection of the present invention:

[0057] Implementation example Figure 1-2 As shown,

[0058] A high-performance collision detection method for the driving trajectory of an unmanned mining vehicle in an open-pit mine includes the following steps:

[0059] S10: The map formed by the scattered coordinates of the obstacles is recorded as map, and the driving trajectory to be evaluated is recorded as traj. The waypoints specifically include the fields x, y, theta, and v, which represent the coordinates of the center point of the rear wheel of the vehicle, the attitude angle, and the vehicle speed, respectively.

[0060] S11: Initialize rA=1;

[0061] S12: As attached Figure 1 As shown, the expansion radius R of the vehicle body is calculated using the vehicle width, front suspension length, wheelbase and rear suspension length, wherein the calculation formula of the expansion radius R is:

[0062]

[0063] The L b , L f , L w and L r They represent vehicle width, front suspension length, wheelbase and rear suspension length respectively.

[0064] S13: performing a dilation operation on the scattered obstacle points in the map with a dilation radius of R to obtain a dilated map map_dilated;

[0065] S14: Initialize traj_res1 to an empty set;

[0066] S15: Check each waypoint wp on the candidate trajectory traj one by one i Whether it falls into the obstacle area of ​​the expanded map map_dilated. If there is a waypoint falling into the obstacle area, the waypoint falling into the obstacle area will be recorded in the set traj_res1 and traj_res1 will be output. rA and traj_res1 will be output at the same time;

[0067] S20: using the preset method FinB to judge the vehicle posture information at each waypoint in traj_res1 in the map, further eliminating the points far from the obstacles, and obtaining the set of remaining waypoints traj_res2;

[0068] It also includes the following steps:

[0069] S22: As attached Figure 2 As shown, extract the vehicle in wp i The coordinate position and posture fields of the point are recorded as three-dimensional coordinate values ​​(wp i .x,wp i .y,wp i .theta);

[0070] S23: Calculate the four vertices A, B, C, and D corresponding to the vehicle at the current waypoint based on formula (2), and formula (2) is as follows:

[0071] A=(A x ,A y )=[wp i. x+(L w +L f )·cos(wp i. theta)-0.5L b ·

[0072] sin(wp i. theta),wp i. y+(L w +L f )·sin(wp i. theta)+0.5L b ·cos(wp i. theta)],

[0073] B=(B x ,B y )=[wp i. x+(L w +L f )·cos(wp i .theta)+0.5L b ·sinθ,wp i .y+(L w +L f )·sin(wp i .theta)-0.5L b ·cos(wp i .theta)],

[0074] C=(C x ,C y )=(wp i .xL r ·cosθ+0.5L b ·sin(wp i .theta),wp i .yL r ·sin(wp i .theta)-0.5L b ·cos(wp i .theta)),

[0075] D=(D x ,D y )=(wp i .xL r·cosθ-0.5L b ·sin(wp i .theta),wp i .yL r ·sin(wp i .theta)+0.5L b ·cos(wp i .theta)). (2)

[0076] S24: Calculate xmin, xmax, ymin and ymax based on formula (3), which is as follows:

[0077] xmin=min{A x ,B x ,C x ,D x}

[0078] xmax=max{A x ,B x ,C x ,D x}

[0079] ymin=min{A y ,B y ,C y ,D y ]

[0080] ymax=max{A y ,B y ,C y ,D y} (3)

[0081] S25: Determine a conservative rectangular frame area according to xmin, xmax, ymin and ymax;

[0082] S26: Set the coordinates of an obstacle point in the map to (xi, yi), and calculate the judgment result using formula (4). Formula (3) is as follows:

[0083] (xi>xmin)∩(xi<xmax)∩(yi> ymin)∩(yi <ymax) (4)

[0084] S30: using the preset method FunC to rearrange all points of traj_res2 in the map according to the driving speed information, and giving priority to collision check for waypoints with lower absolute speed values ​​in traj_res2, and finally outputting the check result;

[0085] The S30 step can be specifically decomposed into the following steps:

[0086] S31: rearrange all elements in traj_res2 from small to large according to the absolute values ​​of the speed v field, and call the set of waypoint sequences after the rearrangement S;

[0087] S32: for each element in the set S, check whether the vehicle footprint corresponding to the waypoint collides with the map one by one every M elements (where M is a parameter specified by the user, and it is recommended to the user that M is 2 to 5. If the current number of elements in the set S is less than M, the last element in the set S is taken for judgment) in order. If a collision occurs, rC=0, and rC is output as the collision result. Otherwise, the element is removed from the set S.

[0088] S33: Repeat the above process until there are no elements in the S set, assign rC a value of 1, and then output rC as the collision result.

[0089] The overall execution process of this solution can be summarized as the following function is_valid = IsTrajectoryValid(traj,map). The input list of this function includes a vehicle trajectory traj to be evaluated, which contains N ordered waypoints, where each waypoint specifically contains the fields x, y, theta, and v, representing the coordinates of the center point of the vehicle's rear wheel, the attitude angle, and the speed. map represents the map composed of the scattered coordinates of obstacles. The return value is the Boolean variable is_valid, which can be 0 or 1.

[0090] The function is_valid = IsTrajectoryValid(traj,map)

[0091] Input: traj and map

[0092] Output: is_valid

[0093] 1. Execute the function FunA(traj,map) to get its return value rA and the residual trajectory traj_res1;

[0094] 2.If (rA is 1), then

[0095] 3. Set is_valid = 1;

[0096] 4.Return;

[0097] 5.End if

[0098] 6. Execute the function FunB(traj_res1,map) to get its return value rB and the residual trajectory traj_res2;

[0099] 7.If (rB is 1), then

[0100] 8. Set is_valid = 1;

[0101] 9.Return;

[0102] 10.End if

[0103] 11. Execute the function FunC(traj_res2,map) and get its return value rC;

[0104] 12. Set is_valid = rC;

[0105] 13.Return;

[0106] Function FunA(traj,map)

[0107] Input: traj and map

[0108] Output: rA and traj_res1

[0109] 1. Initialize rA = 1, initialize

[0110] 2.For each i in {1,2,…,N}, do

[0111] 3. Extract the i-th waypoint wp from traj i ;

[0112] 4. Extract the coordinate position field of the waypoint and record it as a two-dimensional coordinate value (wp i .x,wp i .y);

[0113] 5.If(wp i .x,wp i .y) falls into the obstacle area of ​​the expanded map map_dilated, then

[0114] 6. Set rA = 0;

[0115] 7.traj_res1=[traj_res1,wp i ];

[0116] 8.End if

[0117] 9.End for

[0118] 10.Return rA, traj_res1;

[0119] FunB(traj_res1,map)

[0120] 1. Initialize rB = 1, initialize

[0121] 2.For each i in {1,2,…,N}, do

[0122] 3. Extract the i-th waypoint wp from traj_res1 i ;

[0123] 4. Extract the coordinate position and posture fields of the waypoint and record them as three-dimensional coordinate values ​​(wp i .x,wp i .y,wp i .theta);

[0124] 5. Calculate the four vertices A, B, C, and D of the vehicle corresponding to the current waypoint based on formula (2);

[0125] 6. Calculate xmin, xmax, ymin and ymax based on formula (3);

[0126] 7.If (any obstacle scatter point in the map falls into the conservative rectangle formed by xmin, xmax, ymin and ymax), then

[0127] 8. Set rB = 0;

[0128] 9.traj_res2=[traj_res2,wp i ];

[0129] 10.End if

[0130] 11.End for

[0131] 12.Return rB, traj_res2;

[0132] Function rC = FunC(traj_res2,map)

[0133] Input: traj_res2 and map

[0134] Output: rC

[0135] 1. Sort the elements in the traj_res2 set from small to large according to the absolute value of their field v, and record the rearranged waypoints in the set S;

[0136] 2. While (set S is not empty), do

[0137] 3.For i=1:M:S.size(),do / / Note: S.size() gets the number of elements in set S

[0138] 4. Get the i-th element wp in the set S i ;

[0139] 5. Use formula (2) to calculate the vertices A, B, C, and D of the vehicle body rectangle corresponding to the waypoint;

[0140] 6.If (the obstacle point in the map falls within the rectangular area ABCD), then

[0141] 7. rC = 0;

[0142] 8.Return;

[0143] 9. Else

[0144] 10. Remove the i-th element from the set S;

[0145] 11.End if

[0146] 12. End for

[0147] 13.End while

[0148] 14. rC = 1;

[0149] 15.Return;

[0150] In specific use, mine car A is used in the process of judging trajectory 1. First, the expansion radius R is calculated according to the width, front suspension length, wheelbase and rear suspension length of mine car A, and then the obstacle is expanded with R as the radius in the map composed of the scattered coordinates of the obstacles to obtain the expanded map map_dilated. Through calculation, it is confirmed that there are 5 waypoints falling into the obstacle area of ​​the expanded map map_dilated. These 5 waypoints are: wp1, wp2, wp3, wp4, wp5, and these 5 waypoints are put into the traj_res1 set. Then, the five waypoints are read from traj_res1 in a loop. Each time a waypoint is read, the four vertices of the vehicle A's position at this waypoint are used to select the minimum selection range, and this minimum selection range is determined as the conservative rectangular frame area. After calculation, it is confirmed that there are three waypoints (wp1, wp3, and wp4) where the conservative rectangular area overlaps with the obstacle area in the map. Finally, these three waypoints are checked one by one. After calculation, it is found that one of the three waypoints collides with an obstacle (the vehicle on wp3 collides with an obstacle). Then, trajectory 1 is confirmed to be an unsafe road, that is, a road where a collision will occur.

[0151] Mine cart A is used in the process of judging trajectory 2. After mine cart A obtains the expansion radius R, the calculated traj_res1 is an empty set, and then it is confirmed that trajectory 2 is a safe road, that is, a road that will not collide.

[0152] When mining car A is used to judge trajectory 3, it is calculated that the number of waypoints where the conservative area overlaps with the obstacle area in the map is 0, and trajectory 3 is then confirmed to be a safe path.

[0153] The above are only embodiments of the present invention, and the common knowledge such as the known specific structures and characteristics in the scheme are not described in detail here. It should be pointed out that for those skilled in the art, several deformations and improvements can be made without departing from the structure of the present invention, which should also be regarded as the protection scope of the present invention, and these will not affect the effect of the implementation of the present invention and the practicality of the patent. The scope of protection required by this application shall be based on the content of its claims, and the specific implementation methods and other records in the specification can be used to interpret the content of the claims.

Claims

1. A high-performance collision detection method for the driving trajectory of unmanned mining vehicles in open-pit mines, characterized in that: The following steps are involved: S10: The map composed of the scattered coordinates of the obstacles is recorded as map, and the driving trajectory to be evaluated is recorded as traj. The road points specifically include the fields x, y, theta, and v, which represent the coordinates of the center point of the rear wheel of the vehicle, the attitude angle, and the speed of the vehicle, respectively. The preset method FunA is used to calculate the expansion radius R of the vehicle body for map and traj, and then the obstacles are expanded according to R, and the points far away from the obstacles are roughly filtered out to obtain the residual trajectory traj_res1. S20: using the preset method FinB to judge the vehicle posture information at each waypoint in traj_res1 in the map, further eliminating the points far from the obstacles, and obtaining the set of remaining waypoints traj_res2; S30: using the preset method FunC to rearrange all points of traj_res2 in the map according to the driving speed information, giving priority to collision check for the road points with lower absolute speed values ​​in traj_res2, and finally outputting the check result.

2. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 1 is characterized in that: The variable traj_res1 is set in FunA of step S10, and FunA includes the following steps: S12: Calculate the expansion radius R of the vehicle body using the vehicle width, front suspension length, wheelbase, and rear suspension length of the vehicle; S13: performing a dilation operation on the scattered obstacle points in the map with a dilation radius of R to obtain a dilated map map_dilated; S14: Initialize traj_res1 to an empty set; S15: Check each waypoint wp on the candidate trajectory traj one by one i Whether it falls into the obstacle area of ​​the expanded map map_dilated. If there is a waypoint falling into the obstacle area, the waypoint falling into the obstacle area will be recorded in the set traj_res1 and traj_res1 will be output.

3. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 2 is characterized in that: The FunA also includes a variable rA. The step S11 is included before the step S12. The step S11 is: initializing rA=1; in the step S15, rA and traj_res1 are output simultaneously.

4. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 3 is characterized in that: In the step S12, the calculation formula of the puffing radius R is: Among them, L b , L f , L w and L r They represent vehicle width, front suspension length, wheelbase and rear suspension length respectively.

5. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 4 is characterized in that: In the step S20, FunB loops and checks each waypoint wp on traj_res1 one by one. i , when checking, use wp i Calculate the four vertices corresponding to the vehicle, select the four vertices of the vehicle on the map, get the minimum selection range and record it as the conservative rectangular box area, and then determine wp i Whether the corresponding conservative rectangular box area falls into the obstacle area of ​​the original map map, if there is a waypoint falling into it, the waypoint falling into it will be recorded in the set traj_res2, and traj_res2 will be output after the loop ends.

6. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 5 is characterized in that: FunB uses wp i The calculation of the four vertices corresponding to the vehicle includes the following steps: S22: Extract vehicle in wp i The coordinate position and posture fields of the point are recorded as three-dimensional coordinate values ​​(wp i .x,wp i .y,wp i .theta); S23: Calculate the four vertices A, B, C, and D corresponding to the vehicle at the current waypoint based on formula (2), and formula (2) is as follows: A=(A x ,A y )=[wp i. x+(L w +L f )·cos(wp i. theta)-0.5L b ·sin(wp i. theta),wp i. y+(L w +L f )·sin(wp i. theta)+0.5L b ·cos(wp i. theta)], B=(B x ,B y )=[wp i. x+(L w +L f )·cos(wp i. theta)+0.5L b ·sinθ,wp i. y+(L w +L f )·sin(wp i. theta)-0.5L b ·cos(wp i. theta)], C=(C x ,C y )=(wp i. x-L r ·cosθ+0.5L b ·sin(wp i. theta),wp i. y-L r ·sin(wp i. theta)-0.5L b ·cos(wp i. theta)), <h2 style=";text-align:left;direction:ltr">D=(D<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> ,D<h2 style=";text-align:left;direction:ltr"> y <h2 style=";text-align:left;direction:ltr"> )=(wp<h2 style=";text-align:left;direction:ltr"> i. <h2 style=";text-align:left;direction:ltr"> xL<h2 style=";text-align:left;direction:ltr"> r <h2 style=";text-align:left;direction:ltr"> cosθ-0.5L<h2 style=";text-align:left;direction:ltr"> b <h2 style=";text-align:left;direction:ltr"> sin(wp<h2 style=";text-align:left;direction:ltr"> i. <h2 style=";text-align:left;direction:ltr"> theta),wp<h2 style=";text-align:left;direction:ltr"> i. <h2 style=";text-align:left;direction:ltr"> yL<h2 style=";text-align:left;direction:ltr"> r <h2 style=";text-align:left;direction:ltr"> sin(wp<h2 style=";text-align:left;direction:ltr"> i. <h2 style=";text-align:left;direction:ltr"> theta)+0.5L<h2 style=";text-align:left;direction:ltr"> b <h2 style=";text-align:left;direction:ltr"> cos(wp<h2 style=";text-align:left;direction:ltr"> i. <h2 style=";text-align:left;direction:ltr"> theta)). (2)。 7. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 6 is characterized in that: When FunB determines the conservative rectangular box area through the four vertices of the vehicle, the following steps are included: S24: Calculate xmin, xmax, ymin and ymax based on formula (3), which is as follows: xmin=min{A x ,B x ,C x ,D x } xmax=max{A x ,B x ,C x ,D x } ymin=min{A y ,B y ,C y ,D y } ymax=max{A y ,B y ,C y ,D y } (3) S25: Determine a conservative rectangular frame area according to xmin, xmax, ymin and ymax.

8. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 7 is characterized in that: FunB judge wp i When determining whether the corresponding conservative rectangular box area falls into the obstacle area of ​​the original map, the coordinates of an obstacle point in the map are (xi, yi) and the judgment result is calculated by formula (4). Formula (3) is as follows: (xi>xmin)∩(xi<xmax)∩(yi> ymin)∩(yi <ymax) (4)。 9. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 8 is characterized in that: In the step S30, a variable rC is set in FunC, and FunC includes the following steps: S31: rearrange all elements in traj_res2 from small to large according to the absolute values ​​of the speed v field, and call the set of waypoint sequences after the rearrangement S; S32: for each element in the set S, check in order whether the vehicle footprint corresponding to the waypoint collides with the map. If a collision occurs, rC=0, and rC is output as the collision result. Otherwise, the element is removed from the set S. S33: Repeat the above process until there are no elements in the S set, assign rC a value of 1, and then output rC as the collision result.

10. The high-performance collision inspection method for driving trajectories of unmanned mining vehicles in open-pit mines according to claim 9 is characterized in that: In S32, when checking for collisions one by one in order, judgment is performed at intervals of M elements in the S set. If the current number of elements in the S set is less than M, the last element in the S set is taken for judgment, where M is a parameter specified by the user.

Images