A nonlinear path optimization method, medium and device for mining vehicles

Abstract

The present invention discloses a nonlinear path optimization method, medium and equipment for mining vehicles, the method comprising the steps of: 1) constructing a path optimization state space model for mining vehicles; 2) solving the path optimization state space model based on a trust region strategy, and then obtaining a planned path based on an accelerated search of a backtracking straight line; 3) performing eight-neighborhood spiral expansion of the planned path with drivable tunnel constraints to obtain an optimal path. The present invention has the advantages of good robustness and high optimization efficiency.

Description

Technical Field

[0001] The present invention mainly relates to the technical field of mining vehicle, and in particular to a nonlinear path optimization method, medium and equipment for mining vehicles. Background Art

[0002] The loading and unloading environment in open-pit mines is complex, and there are obstacles such as stones and operating machinery in the loading and unloading dumping space, and the space is limited. In a complex environment, in order to successfully drive the mining truck into the dumping position and loading position, it is not only necessary to perceive and identify obstacles with high precision to warn of collision hazards in advance, thereby ensuring the safety of the mining truck's travel operation, but also to follow the planned path under various operating conditions with high precision control. This increases the challenge of the path planning algorithm for unmanned mining trucks. Due to the dynamic changes in the position of mechanical equipment in the loading area, the frequent operation of mechanical equipment in the unloading area, and the possibility of vehicle block loss on bumpy paths. The above factors have put forward multiple requirements for the path planning algorithm. Therefore, developing a high-availability path planning strategy that ensures vehicle safety and can quickly adapt to various complex environments has become an important issue in solving unmanned operations in mining areas.

[0003] The paper "Optimization-Based Trajectory Planning for Autonomous Parking With Irregularly Placed Obstacles: A Lightweight Iterative Framework" further proposes a lightweight iterative method based on the establishment of a safe tunnel. This method converts the original non-convex obstacle avoidance constraints into convex obstacle avoidance constraints by inflating obstacles and safe tunnels, and then gradually reduces the kinematic constraints to 0 through multiple rounds of iterations. The above method sometimes fails. The reasons are as follows: First, if the hybrid A-star is inefficient, the corridor will be unavailable. Secondly, if there are dense obstacles, there is a certain risk of collision between the hybrid A-star search path safety envelope and the obstacle, and even the path point is too close to the obstacle, causing the vehicle to collide with the obstacle. Therefore, although this method can quickly build a safe tunnel, it lacks an effective method to bypass obstacles or reselect collision-free path points. Summary of the invention

[0004] In view of the technical problems existing in the prior art, the present invention provides a nonlinear path optimization method, medium and equipment for mining vehicles with high optimization efficiency.

[0005] In order to solve the above technical problems, the technical solution proposed by the present invention is:

[0006] A nonlinear path optimization method for a mining vehicle comprises the following steps:

[0007] 1) Construct a path optimization state space model for mining vehicles;

[0008] 2) Solve the path optimization state space model based on the trust region strategy, and then obtain the planned path based on the accelerated search of the backtracking line;

[0009] 3) Perform eight-neighborhood spiral expansion on the planned path to obtain the optimal path.

[0010] Preferably, the specific process of step 1) is: constructing a vehicle kinematics formula, solving the Jacobian matrix of the vehicle kinematics formula to obtain the Jacobian matrix, and then expanding it near the initial solution of the path according to the Taylor formula to obtain a discretized path optimization state space model.

[0011] Preferably, the process of obtaining the initial solution of the path is:

[0012] First, a driving path that can effectively connect the start and end positions is generated based on the hybrid A star. Then, the path is sampled at equal time, and the path points are roughly matched with the speed, and the path points are roughly matched with the front wheel deflection angle. The final results are recorded in the collection ,gather It is called the initial solution of the path, and the path points formed by the initial solution of the path are recorded as decision variables in the optimization process; Respectively represent vehicles Moment coordinate, Coordinates, heading angle coordinates, speed, front wheel steering angle and travel time; , is the number of discrete steps.

[0013] Preferably, after obtaining the initial solution of the path, the forward Euler numerical discretization method is used according to the initial solution of the path and the vehicle kinematics formula to obtain the kinematic discretization formula, and then the kinematic discretization formula and the vehicle kinematics formula are combined to further obtain the value of the linearized constraint function under the current initial solution of the path, which is recorded as , and .

[0014] Preferably, the path optimization state space model is specifically:

[0015]

[0016] in ; = ;in Represents the matrix transpose symbol; is the Jacobian matrix.

[0017] Preferably, in step 2), the specific process of solving the path optimization state space model based on the trust region strategy is:

[0018] In the inner iteration, the trust region constraint is set, and then the obstacle avoidance constraint and the trust region constraint are unified to obtain a unified constraint formula. Then the trust region constraint, the unified constraint formula and the quadratic objective function are input into the quadratic programming solver for optimization solution.

[0019] The quadratic objective function is a function obtained by fusing the path optimization state space model.

[0020] Preferably, after the optimization solution is found, the quality of the solution and constraint violations are further evaluated. :

[0021]

[0022]

[0023] 0

[0024] in Indicates the violation of the total inequality constraint coefficient matrix; Indicates the violation of the total equality constraint coefficient; is a symbol indicating a constraint violation; Express Find the second norm, Express Find the second norm; represents the total inequality constraint coefficient matrix, represents the constant term in the total inequality constraint, represents the total equality constraint coefficient matrix, represents the constant term in the total equality constraint; is the current initial solution; specifically, when 0 and 0These two conditions are met at the same time, and At the same time, find the two norms and then sum them to get the constraint violation ;

[0025] If two consecutive If the difference is less than a certain threshold, it is considered to be close to the optimal solution; if it is not close to the optimal solution, the trust region radius is adjusted and an accelerated search of the backtracking line is performed.

[0026] Preferably, the specific process of performing the accelerated search of the backtracking line is:

[0027] if The closer it is to 0, the more accurate the trust region search is, and the radius of the trust region needs to be expanded; if The closer it is to 1, the greater the deviation between the trust region radius and the actual situation, and the trust region radius needs to be reduced; therefore, the trust region radius is adjusted according to the size of the linearization error; at the same time, the linearization error is positively correlated with the constraint violation, so the trust region radius is dynamically adjusted using the constraint violation. .

[0028] Preferably, in step 3), a collision check is first performed on the planned path to check whether there is a collision with an obstacle; if there is a collision, the search range is gradually expanded in an eight-neighborhood spiral around the collision path point to try to find a collision-free path point; then the bounding box is gradually expanded until the largest possible collision-free area is found to obtain the optimal path.

[0029] Preferably, the specific process of collision detection is: first construct a rectangular envelope, then use the bounding box method to screen obstacles; then use the ray method to perform refined collision detection, and use the odd and even property of the number of intersections between the ray and the polygon boundary to determine whether the point is inside the vehicle rectangle.

[0030] Preferably, the specific process of eight-neighborhood spiral expansion is:

[0031] If a collision occurs, a loop is entered to gradually expand the range by continuously increasing the search radius and angle; a counter is incremented at each iteration to calculate the angle type of the current iteration to ensure that the search can be cyclically performed in eight directions;

[0032] Calculate the search radius and angle of the current iteration to determine the direction of the search point, and obtain a new search point based on the calculated search radius and angle;

[0033] Find a set of valid bounding boxes based on the new search point.

[0034] The present invention also discloses a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the steps of the method described above are executed.

[0035] The present invention further discloses a computer device, comprising a memory and a processor connected to each other, wherein a computer program is stored in the memory, and when the computer program is executed by the processor, the steps of the method described above are executed.

[0036] Compared with the prior art, the advantages of the present invention are:

[0037] The present invention considers the trust region search method, combines the obstacle avoidance constraint of the drivable tunnel with the eight-neighborhood spiral expansion, and designs a nonlinear path optimization algorithm for mining trucks with a double-layer trust region strategy. The eight-spiral expansion strategy is adopted in the outer layer to make fine adjustments to the path points with collision risks, and the obstacle avoidance constraint scale and the number of obstacles in the environment and the number of vertices of the obstacle convex polygon are decoupled, so that the subsequent optimization strategy is not directly affected by the complexity of the environment; the inner layer loop adopts the backtracking straight line search to finely adjust the inner layer search direction and step size; by dynamically adjusting the size of the trust region radius, the change amplitude of the solution in the optimization process is effectively controlled to prevent the violation of the outer layer obstacle avoidance constraint or instability caused by an excessively large update step size; the number of obstacles and the optimization constraint are decoupled to ensure that the optimization strategy is not interfered by the environment, and at the same time, under the action of the backtracking straight line method and the trust region method, the robustness and efficiency of the algorithm are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is the overall architecture diagram of the nonlinear path optimization algorithm for mining vehicles of the present invention.

[0039] Figure 2 Schematic diagram of the path optimization state space model in the present invention.

[0040] Figure 3 It is a schematic diagram of collision detection in the present invention.

[0041] Figure 4 It is a schematic diagram of fixed curvature path collision detection in the present invention.

[0042] FIG5 is a schematic diagram of the eight-domain spiral method of the present invention; (a) from Start spiral expansion; (b) from Start spiral search; (c) from Start spiral search; (d) from Start spiral search; (e) from Start spiral search; (f) from Start spiral search; (g) from Start spiral search; (h) from Start a spiral search.

[0043] Figure 6 The present invention is a flow chart of a path optimization method in an embodiment. DETAILED DESCRIPTION

[0044] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments.

[0045] like Figure 1 and Figure 6As shown, the nonlinear path optimization method for mining vehicles according to an embodiment of the present invention comprises the following steps:

[0046] 1) Construct a path optimization state space model for mining vehicles;

[0047] 2) Solve the path optimization state space model based on the trust region strategy, and then obtain the planned path based on the accelerated search of the backtracking line;

[0048] 3) Perform eight-neighborhood spiral expansion on the planned path to obtain the optimal path.

[0049] In a specific embodiment, the specific process of step 1) is as follows: the vehicle kinematic constraints are used to describe the vehicle's ability to move on a two-dimensional plane. When the vehicle speed is not high, the two-degree-of-freedom model can meet the actual needs. The model follows the following assumptions:

[0050] (1) The vehicle is traveling on a flat road, and the movement of the vehicle in the direction perpendicular to the road is ignored;

[0051] (2) Ignore the air resistance and lateral friction of the ground that the vehicle encounters during driving;

[0052] (3) The wheels always maintain good rolling contact with the ground;

[0053] (4) The vehicle is a rigid body and the influence of the vehicle body suspension structure is ignored.

[0054] like Figure 2 As shown, the vehicle kinematics formula is:

[0055] = (1)

[0056] in The center of the rear axle of the vehicle is The coordinate position in the inertial coordinate system XOY at all times, Indicates that the vehicle is The posture angle of the moment, express The deflection angle of the front wheel of the vehicle at time express The vehicle's speed at the moment, express The vehicle's acceleration at the moment, express The angular velocity of the vehicle at the moment, It represents the front and rear wheelbase. Indicates the total driving time of the vehicle.

[0057] From formula (1), we can conclude that the control variable is , the state variable is , because the mechanical characteristics of the mining truck and the state variables and control variables correspond one to one. Therefore, the variables are bounded and within a certain allowable range, specifically:

[0058] (2)

[0059] in Indicates the minimum value of the vehicle's front wheel deflection angle, Indicates the maximum value of the vehicle's front wheel deflection angle, and Represent the minimum and maximum values ​​of the vehicle speed, respectively. and Respectively represent the minimum and maximum values ​​of vehicle travel time, and Respectively represent the minimum and maximum values ​​of the vehicle's acceleration. and They represent the minimum and maximum values ​​of the vehicle's angular acceleration respectively.

[0060] In order to ensure that the starting position and the ending position of the vehicle are consistent, the state and Make certain restrictions (including and Respectively indicate that the vehicle is and The state at the moment is:

[0061] (3)

[0062] in and They respectively represent the starting position and ending position of the vehicle during the movement; , Represent the start and end time of the vehicle movement respectively coordinate; , Represent the start and end time of the vehicle movement respectively coordinate; , Represent the start and end time of the vehicle movement respectively coordinate.

[0063] The above formula introduces the constraints that must be met in the vehicle path optimization task. However, there is more than one path that meets these constraints. In order to select a path with the shortest possible transportation time, the smallest possible speed change, and the smallest possible steering angle change, the following cost function is designed to select high-quality and optimized paths through numerical optimization. The cost function is as follows:

[0064] )(4)

[0065] in and Represent the weights of the squared acceleration term and the squared steering angular velocity term respectively.

[0066] However, before numerically solving the path optimization problem, the problem needs to be linearized. The path optimization method based on numerical solution is highly dependent on a feasible initial solution. The initial solution of the present invention is obtained by a sampling method or a search method, such as RRT or hybrid A star. The following takes hybrid A star as an example to introduce the method for generating the initial solution of the path:

[0067] First, based on the hybrid A star, a driving path that can effectively connect the start and end positions is generated (including only , , ), then perform equal-time sampling on the path and roughly match the path points with the speed, and the path points with the front wheel deflection angle;

[0068] Record the final result in the collection , will be collected The resulting path points are recorded as decision variables in the optimization process; middle Respectively represent vehicles Moment coordinate, Coordinates, heading angle coordinates, speed, front wheel steering angle and travel time; , is the number of discrete steps;

[0069] The above collection This is called the initial solution of the path (decision variable).

[0070] According to the collection And the vehicle kinematics formula (1), using the forward Euler numerical discretization method, the kinematic discretization formula is as follows:

[0071] (5)

[0072] Then, combining formula (5) and vehicle kinematics formula (1), we can further obtain Under this condition, the value of the linearized constraint function is recorded as , and :

[0073] (6)

[0074] in For walking distance.

[0075] Solve the Jacobian matrix of kinematic formula (1) and get the Jacobian matrix J, and then according to Taylor's formula, it is expanded near the initial solution of the path, and the discretized state space (i.e., the path optimization state space model) is expressed as follows:

[0076] (7)

[0077] in , = ,in Represents the matrix transpose symbol.

[0078] Ideally, if the initial solution fully satisfies the kinematic constraints, then It should be 0. Therefore, after obtaining the discretized expression (7), it is necessary to reintegrate the objective function to obtain the quadratic objective function as follows:

[0079] (8)

[0080] in represents the initial solution; It corresponds to The coefficients of the other terms in correspond to 0, but A matrix with coefficients of 1; and represents the quadratic term of the objective function; where is the forward and backward speed term The coefficient matrix of the difference represents the speed change; yes Middle front and rear steering angle The coefficient matrix of the difference; represents a soft constraint that penalizes violations of kinematic constraints, represents the penalty coefficient, express The second norm of .

[0081] In a specific embodiment, in step 2), backtracking straight line search and trust region strategy are used to further perform iterative optimization. The trust region method is different from the Newton method and the gradient descent method. As an inexact optimization method, the trust region method is widely used in optimization problems. It limits the solution to a certain range by limiting the update step size of the solution, thereby avoiding the divergence of the solution. In order to find a better solution while avoiding divergence, the backtracking straight line search method is introduced in the trust region strategy to update the step size, thereby finding a better solution.

[0082] Specifically, the trust region constraint is set in the inner iteration as:

[0083] (15)

[0084] in represents the coefficient matrix of the trust region radius constraint, is the trust region updated for each iteration, the current initial solution , then the obstacle avoidance constraint (Equation (14)), trust region constraint (Equation (15)), and other constraints (Equation (2), Equation (6) and Equation (7)) are unified as Equation (16):

[0085] (16)

[0086] in represents the total inequality constraint coefficient matrix, represents the constant term in the total inequality constraint, represents the total equality constraint coefficient matrix, represents the constant term in the total equality constraint.

[0087] The trust region constraints (Equations (15)-(16)) and the quadratic objective function (Equation (9)) can be sent to the quadratic programming solver for optimization and further evaluation of the solution quality and constraint violations. :

[0088] (17)

[0089] (18)

[0090] 0 (19)

[0091] in Indicates the violation of the total inequality constraint coefficient matrix; Indicates the violation of the total equality constraint coefficient; is a symbol that indicates a constraint violation; specifically, when 0 and 0These two conditions are met at the same time, and At the same time, find the bi-norm and then sum it to get the constraint violation ( );in Express Find the second norm, Express Find the second norm;

[0092] If two consecutive If the difference is less than a certain threshold, it is considered to be close to the optimal solution; if it is not close to the optimal solution, the trust region radius and backtracking straight line search are adjusted.

[0093] Combining the constraints constructed by the above formulas, we can completely obtain the path optimization proposition of quadratic programming optimization based on trust region constraints:

[0094] )

[0095] st

[0096]

[0097]

[0098] ,

[0099]

[0100] If the solution changes too much during the optimization process, you need to use the backtracking straight line method to find a better solution. In this process, by gradually reducing the step size And calculate the temporary solution , and find a new solution that can significantly reduce the objective function value.

[0101] Backtracking linear search relies on a merit function, which combines the objective function value and the penalty for constraint violation. It determines whether to accept a new step size by comparing the merit function value of the temporary solution with a threshold based on the current step size. If a satisfactory solution is found during the backtracking process, the current solution is updated. and the corresponding constraint violation ; Otherwise, reduce the step size and try again.

[0102] In the inner layer trust region optimization strategy based on backtracking line, before backtracking line acceleration search, the trust region radius needs to be adjusted, such as Figure 6As shown. The main function of adjusting the trust region radius is to determine the search area, and the function of backtracking straight line search is to confirm the search step and direction in the search area. As an important indicator for adjusting the trust region radius, it is used to measure the accuracy of the trust region search.

[0103]

[0104] in represents the linearization error, which is the sum of the squares of the terms on the right side of formula (7), denoted as = + + ; Indicates that in the current initial solution The linearized constraint violation under They represent the trust region indicator adjustment parameters respectively.

[0105] if The closer this ratio is to 0, the more accurate the trust region search is, and the trust region radius needs to be expanded; if the model ratio is closer to 1, it means that there is a large deviation between the trust region radius and the actual situation, and the trust region radius needs to be reduced. Therefore, the trust region radius can be adjusted according to the size of the linearization error. At the same time, the linearization error is positively correlated with the constraint violation. Therefore, in order to simplify the solution, the trust region radius can be dynamically adjusted according to the constraint violation. .

[0106] ;

[0107] ;

[0108] ;

[0109] in represents the trust region radius adjustment parameter, all of which are less than 1; and They represent the minimum trust region radius and the maximum trust region radius respectively.

[0110] In a specific embodiment, the purpose of step 3) is to find a valid bounding box around a given path point in the planned path. That is, the search range is first gradually expanded in eight neighborhoods around the given collision path point to try to find a collision-free path point; then the bounding box is gradually expanded until the largest possible collision-free area is found. If there is no given collision point, there is no need to perform spiral expansion.

[0111] In the process of spiral expansion of the eight-neighborhood, it is necessary to perform collision detection on the path to check whether there is a collision with an obstacle. The commonly used collision detection idea is: first envelop the vehicle with a rectangle, and then calculate whether the obstacle point is inside the rectangle. The present invention improves the collision detection efficiency by using a ray collision detection method, and performs collision detection in places where obstacles are dense. Figure 3 As shown:

[0112] First, a rectangular envelope is constructed, and then the axis-aligned bounding box method (hereinafter referred to as the AABB method) is used to filter out obstacles.

[0113] Then the ray method is used to perform refined collision detection; Figure 3-Figure 4 As shown in the figure, the ray and boundary detection method is used to determine whether a point is inside the vehicle rectangle. Specifically, the ray collision detection uses the odd-even property of the number of intersections between the ray and the polygon boundary to determine whether a point is inside the polygon. The ray is emitted in the positive direction of the axis, and then the number of intersections with the rectangle boundary is calculated; if the number of intersections is an odd number, it means that the point is inside the rectangle; if it is an even number, it means that the point is outside the rectangle.

[0114] Specifically, once the path collides with an obstacle, the trajectory near the collision path point needs to be adjusted. To this end, an eight-helix method is proposed, as shown in Figure 5. First, the ray collision detection method is used to check the given path point. Whether there is a collision with an obstacle; if there is a collision, enter a loop and continuously increase the search radius and angle to gradually expand the range; each iteration increases the counter , calculate the angle type of the current iteration , ensuring that the search can be circulated in eight directions. Calculate the search radius and angle of the current iteration to determine the direction of the search point. According to the calculated radius and angle, ensure that the new search point , check the validity of the new search point, then keep it (% represents the remainder symbol).

[0115] (9)

[0116] in is an integer The remainder after dividing by 8 indicates the type of helix rotation angle; =0,1,...,7, respectively, indicates that the given path point rotates to the west, northwest, north, northeast, east, southeast, south, and southwest; represents the radius of the spiral around a given path point in each direction; Represents the rotation angle of the spiral around a given path point; and Respectively represent given path points; and They respectively represent the new path points after searching by the eight-helix neighborhood search algorithm.

[0117] The next step is to find a set of valid bounding boxes for each new search point. First, the maximum effective expansion of each path point in each direction is determined. , then verify the extension Is it valid? If valid, the expansion is used as the extreme coordinates of the tunnel, and finally the linear inequality is constructed through the extreme coordinates.

[0118] Specifically, in the above process, the initial expansion step size is first set and maximum expansion limit , then expand the bounding box in four directions (up, down, left, right) ,in Represent the extension direction identifiers respectively, until all directions reach the limit or become invalid, and the update formula for each iteration is:

[0119] 3(10)

[0120] Finally, the legitimacy of the current extension is determined; after the extension is completed, the maximum effective extension length array of the path point in four directions is returned .

[0121] Specifically, the specific process of extending the legality determination is as follows:

[0122] First, based on the new search point and extended length array Determine the coordinates of the four corner points of the bounding box:

[0123] (11)

[0124] Then according to the expansion direction (up, down, left, right, 2,3) and the current extension length determine the new boundary.

[0125] (12)

[0126] Make sure to extend the boundaries ( ) does not exceed the map boundary limit and does not collide with obstacles.

[0127] If the extended boundary is valid, the extreme coordinates of the right boundary of the tunnel are calculated , left extreme value coordinates , upper extreme value coordinates and the lower boundary extreme value :

[0128] (13)

[0129] Based on the extreme value of the tunnel boundary, a linear obstacle avoidance matrix is ​​constructed. and Inequality representing the bounding box:

[0130] , , (14).

[0131] The above obstacle avoidance iteration strategy makes full use of the advantages of the prior trajectory of the hybrid A-star search and the fast collision detection of the ray collision detection. It only needs to slightly adjust the collision point to decouple the obstacle avoidance constraint scale from the number of obstacles in the environment and the number of vertices of the obstacle convex polygon, so that the subsequent optimization strategy is not directly affected by the complexity of the environment.

[0132] In the above outer obstacle avoidance iteration strategy, the purpose of the outer loop is to adjust the collision point and then construct the collision avoidance constraint. First, the maximum number of iterations will be controlled, and the trust region radius needs to be initialized each time the outer loop is restarted; then, before each iteration, the matching trajectory generated by the hybrid A star is tested for ray collision; if no collision occurs, there is no need to adopt the drivable tunnel method for obstacle avoidance constraints; if a collision is detected, the collision point is fine-tuned using the eight-domain spiral expansion drivable obstacle avoidance constraint method, and then the obstacle avoidance constraint is constructed; finally, the sorted objective function, equality constraints, and inequality constraints are sent to the inner trust region search strategy for iteration.

[0133] The present invention considers the trust region search method, combines the obstacle avoidance constraint of the drivable tunnel with the eight-neighborhood spiral expansion, and designs a nonlinear path optimization algorithm for mining trucks with a double-layer trust region strategy. The eight-spiral expansion strategy is adopted in the outer layer to make fine adjustments to the path points with collision risks, and the obstacle avoidance constraint scale and the number of obstacles in the environment and the number of vertices of the obstacle convex polygon are decoupled, so that the subsequent optimization strategy is not directly affected by the complexity of the environment; the inner layer loop adopts the backtracking straight line search to finely adjust the inner layer search direction and step size; by dynamically adjusting the size of the trust region radius, the change amplitude of the solution in the optimization process is effectively controlled to prevent the violation of the outer layer obstacle avoidance constraint or instability caused by an excessively large update step size; at the same time, the backtracking straight line method is introduced to improve the algorithm solution speed; the number of obstacles and the optimization constraint are decoupled to ensure that the optimization strategy is not interfered by the environment, and at the same time, under the action of the backtracking straight line method and the trust region method, the robustness and efficiency of the algorithm are improved.

[0134] Explanation of related terms:

[0135] Eight neighborhoods: represent areas in eight directions: east, south, west, north, northeast, southeast, northwest, and southwest.

[0136] Trust region: Given a so-called trust region radius, the optimization problem is solved within the circle formed by this radius.

[0137] State space discrete model: An iterative model composed of vehicle states, often used to predict the vehicle's motion state at the next moment.

[0138] Backtracking straight line method: If it is found that the optimization step size is too large, the step size is reduced along the search direction until the search point approaches the target point.

[0139] The present invention also discloses a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above method are executed. The present invention further discloses a computer device, including a memory and a processor connected to each other, on which a computer program is stored, and when the computer program is executed by the processor, the steps of the above method are executed. The medium and device of the present invention correspond to the above method and also have the advantages described in the above method.

[0140] The present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by hardware related to computer program instructions. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned method embodiment can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. Computer-readable storage media include: any entity or device that can carry computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium. The memory is used to store computer programs and / or modules. The processor implements various functions by running or executing computer programs and / or modules stored in the memory, and calling data stored in the memory. The memory may include a high-speed random access memory and may also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0141] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A nonlinear path optimization method for mining vehicles, characterized in that: Includes steps: 1) Construct a path optimization state space model for mining vehicles; 2) Solve the path optimization state space model based on the trust region strategy, and then obtain the planned path based on the accelerated search of the backtracking line; 3) Perform eight-neighborhood spiral expansion on the planned path to obtain the optimal path; In step 2), the specific process of solving the path optimization state space model based on the trust region strategy is: In the inner iteration, the trust region constraint is set, and then the obstacle avoidance constraint and the trust region constraint are unified to obtain a unified constraint formula. Then the trust region constraint, the unified constraint formula and the quadratic objective function are input into the quadratic programming solver for optimization solution. The quadratic objective function is a function obtained by fusing the path optimization state space model; After optimizing the solution, further evaluate the solution quality and constraint violations : 0 in Indicates the violation of the total inequality constraint coefficient matrix; Indicates the violation of the total equality constraint coefficient; is a symbol indicating a constraint violation; Express Find the second norm, Express Find the second norm; represents the total inequality constraint coefficient matrix, represents the constant term in the total inequality constraint, represents the total equality constraint coefficient matrix, represents the constant term in the total equality constraint; is the current initial solution; specifically, when 0 and 0These two conditions are met at the same time, and At the same time, find the two norms and then sum them to get the constraint violation ; If two consecutive If the difference is less than a certain threshold, it is considered to be close to the optimal solution; If the optimal solution is not close, the trust region radius is adjusted and an accelerated search of the backtracking line is performed.

2. The nonlinear path optimization method for mining vehicles according to claim 1, characterized in that: The specific process of step 1) is: construct the vehicle kinematics formula, solve the Jacobian matrix of the vehicle kinematics formula to obtain the Jacobian matrix, and then expand it near the initial solution of the path according to the Taylor formula to obtain the discretized path optimization state space model.

3. The nonlinear path optimization method for mining vehicles according to claim 2, characterized in that: The process of obtaining the initial solution of the path is: First, a driving path that can effectively connect the start and end positions is generated based on the hybrid A star. Then, the path is sampled at equal time, and the path points are roughly matched with the speed, and the path points are roughly matched with the front wheel deflection angle. The final results are recorded in the collection ,gather This is called the path initial solution. The resulting path points are recorded as decision variables in the optimization process; Respectively represent vehicles Moment coordinate, Coordinates, heading angle coordinates, speed, front wheel steering angle and travel time; , is the number of discrete steps.

4. The nonlinear path optimization method for mining vehicles according to claim 3, characterized in that: After the initial solution of the path is obtained, the forward Euler numerical discretization method is used according to the initial solution of the path and the vehicle kinematics formula to obtain the kinematic discretization formula. Then, the kinematic discretization formula and the vehicle kinematics formula are combined to further obtain the value of the linearized constraint function under the current initial solution of the path, which is recorded as , and .

5. The nonlinear path optimization method for mining vehicles according to claim 4, characterized in that: The path optimization state space model is as follows: in ; = ;in Represents the matrix transpose symbol; is the Jacobian matrix.

6. The nonlinear path optimization method for mining vehicles according to claim 1, characterized in that: The specific process of accelerating the search for the backtracking line is as follows: If the trust region index coefficient The closer it is to 0, the more accurate the trust region search is, and the radius of the trust region needs to be expanded; if The closer it is to 1, the greater the deviation between the trust region radius and the actual situation, and the trust region radius needs to be reduced; therefore, the trust region radius is adjusted according to the size of the linearization error; at the same time, the linearization error is positively correlated with the constraint violation, so the trust region radius is dynamically adjusted using the constraint violation. .

7. The nonlinear path optimization method for mining vehicles according to any one of claims 1 to 5, characterized in that: In step 3), the planned path is firstly subjected to collision detection to check whether it collides with obstacles; if there is a collision, the search range is gradually expanded in an eight-neighborhood spiral around the collision path point to try to find a collision-free path point; then the bounding box is gradually expanded until the largest possible collision-free area is found to obtain the optimal path.

8. The nonlinear path optimization method for mining vehicles according to claim 7, characterized in that: The specific process of collision detection is as follows: first, a rectangular envelope is constructed, and then the bounding box method is used to screen obstacles; then the ray method is used for refined collision detection, and the parity of the number of intersections between the ray and the polygon boundary is used to determine whether the point is inside the vehicle rectangle.

9. The nonlinear path optimization method for mining vehicles according to claim 8, characterized in that: The specific process of eight-neighborhood spiral expansion is: If a collision occurs, a loop is entered to gradually expand the range by continuously increasing the search radius and angle; a counter is incremented at each iteration to calculate the angle type of the current iteration to ensure that the search can be cyclically performed in eight directions; Calculate the search radius and angle of the current iteration to determine the direction of the search point, and obtain a new search point based on the calculated search radius and angle; Find a set of valid bounding boxes based on the new search point.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the computer program performs the steps of the method according to any one of claims 1 to 9.

11. A computer device comprising a memory and a processor connected to each other, wherein a computer program is stored in the memory, characterized in that: When the computer program is executed by a processor, the computer program performs the steps of the method according to any one of claims 1 to 9.

Images