An optimal trajectory planning method and system for turning at intersections of articulated mining equipment

Abstract

The present invention discloses an optimal trajectory planning method and system for turning at an intersection of an articulated mining equipment, the method comprising: building a kinematic model for a trajectory planning algorithm; designing an environment perception module to detect whether the equipment has entered the area to be turned; designing a trajectory planning module based on quadratic planning to decouple the trajectory planning problem into longitudinal and lateral trajectory planning; designing a key parameter determination module based on three-dimensional search, taking the driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, respectively designing a search outer loop based on the driving time, a search middle loop based on the longitudinal driving distance and a search inner loop based on the lateral driving distance, and obtaining the optimal solution through the trajectory planning algorithm on this basis; designing a constraint checking module to perform constraint checking on the obtained trajectory. The present invention can reduce the time for debugging and deploying the autonomous driving system, is conducive to giving full play to the performance of the automation equipment, and improving production efficiency.

Description

Technical Field

[0001] The invention relates to the technical field of trajectory planning for underground mobile mining equipment, and in particular to an optimal trajectory planning method and system for intersection turning of articulated mining equipment. Background Art

[0002] Due to its small turning radius and strong terrain adaptability, articulated mobile mining equipment has been widely used in underground mines. This type of equipment mainly includes: underground shovel loaders, drilling trolleys, charging trolleys, rock drilling trolleys, etc. The main task of this type of articulated mobile underground mining equipment in underground tunnels is to open up tunnels and transport ore. In underground mines, mining tunnels and transportation tunnels intersect with each other, and this type of mining equipment travels back and forth between mining tunnels and transportation tunnels. For example, the main task of underground shovel loaders is to move ore from the loading point of the mining tunnel to the unloading point of the transportation tunnel. Therefore, turning at intersections is a common driving behavior for underground shovel loaders. With the development of the mining industry, the mining depth continues to increase, and the high temperature and high humidity environment underground is extremely harmful to drivers. Therefore, autonomous driving has become a necessary function of the new generation of intelligent shovel loaders.

[0003] For this type of mining equipment, the current common method of autonomous driving is to conduct manual teaching first, and then autonomous driving. Before autonomous driving, the driver drives along the route and locates the path information, and then conducts multiple test runs from high speed to low speed to finally obtain the best trajectory for autonomous driving of the scraper. Therefore, the existing manual teaching method is greatly affected by the driver's experience, and it is not easy to obtain stable and high-quality teaching information in specific implementation, which not only affects the time for debugging and deployment of the scraper's autonomous driving system, but also is not conducive to the full performance of the automation equipment, ultimately affecting production efficiency.

[0004] In the field of autonomous driving of passenger cars, high-quality autonomous driving is accomplished through local trajectory planning and tracking control after the driving behavior is determined. Therefore, improving the passing capacity of scraper in turning areas through local trajectory planning is an efficient and feasible solution. At present, the hybrid trajectory planning method based on decoupling is a research hotspot and frontier of passenger car trajectory planning methods. After decoupling the trajectory, state space sampling and optimization algorithms are used in different dimensions to obtain the optimal trajectory. For passenger cars, this method can quickly obtain a trajectory with completeness and optimality. However, for articulated vehicles, it is difficult to establish a trajectory planning model based on quadratic program (QP) in the lateral dimension due to the physical constraints of the articulation angle. Therefore, the optimal solution may not be guaranteed. Summary of the invention

[0005] The purpose of the present invention is to provide an optimal trajectory planning method and system for intersection turning of articulated mining equipment, so as to solve the problem of low working efficiency of autonomous shovel loader due to difficulty in obtaining high-quality teaching information during turning, and the problem that the trajectory planning model cannot guarantee the optimal solution due to the physical constraints of the articulation angle. A turning trajectory that guarantees optimality and completeness can be obtained, which can guide the articulated mobile mining equipment to pass the curve safely and efficiently under the minimum acceleration and minimum acceleration change rate. It can not only reduce the time for debugging and deployment of the autonomous driving system, but also is conducive to giving full play to the performance of the automation equipment and improving production efficiency.

[0006] To solve the above technical problems, the embodiments of the present invention provide the following solutions:

[0007] On the one hand, a method for optimal trajectory planning of an intersection turn of an articulated mining equipment is provided, comprising the following steps:

[0008] Build a kinematic model of an articulated mobile mining rig for use in trajectory planning algorithms;

[0009] Design an environmental perception module to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module;

[0010] Design a trajectory planning module based on quadratic programming to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, design a longitudinal trajectory planning model to generate a longitudinal trajectory, and then design a lateral trajectory planning model to generate a lateral trajectory.

[0011] Design a key parameter determination module based on three-dimensional search, take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, design the outer search loop based on driving time, the middle search loop based on longitudinal driving distance and the inner search loop based on lateral driving distance respectively. When the trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted through the inner loop, middle loop and outer loop in sequence until the appropriate driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm.

[0012] A constraint checking module is designed to perform constraint checking on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop according to the three-dimensional search strategy until the constraint check is met, and this trajectory is taken as the optimal trajectory.

[0013] Preferably, the kinematic model of the articulated mobile mining equipment is expressed as:

[0014]

[0015] Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, L r is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ.

[0016] Preferably, the trajectory planning module designed based on quadratic programming specifically includes:

[0017] The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as:

[0018]

[0019] And satisfy the following constraints:

[0020]

[0021]

[0022]

[0023] x min ≤x t ≤x max ,t=1,......,N (6)

[0024]

[0025]

[0026] x0=x in (9)

[0027]

[0028]

[0029] x N =x end (12)

[0030]

[0031] Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmax and Δa xmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle;

[0032] Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as:

[0033]

[0034]

[0035]

[0036]

[0037] y min ≤y t ≤y max ,t=1,......,N (18)

[0038]

[0039]

[0040] y0=y in (twenty one)

[0041]

[0042]

[0043] y N =y end (twenty four)

[0044]

[0045] Among them, y t , and is the decision variable, y t is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, v ymin and v ymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of , ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0. in and end x t The start and end constraints of and They are and The starting point constraint.

[0046] Preferably, the key parameter determination module for designing based on three-dimensional search specifically includes:

[0047] The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula:

[0048] Δt i** =Δt initial +(i-1)·Δt′, (26)

[0049] in,

[0050] Δt initial ≥t lomin orΔt initial ≥max{t lomin ,t lamin},

[0051]

[0052]

[0053] i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment;

[0054] The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance of the middle cycle is Determined by the following formula:

[0055]

[0056] Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes;

[0057] The inner loop is designed to determine the lateral terminal position. The lateral travel distance of the inner loop is Determined by the following formula:

[0058]

[0059] Among them, k is the loop variable of the inner loop, y initial is the horizontal coordinate of the initial point of the iteration, and Δl is the discrete interval of the horizontal end position.

[0060] Preferably, the constraint checking module is used to perform constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including:

[0061] According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated;

[0062] Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints:

[0063] γ min ≤γ tr-max ≤γ max (29)

[0064] γ min ≤γ tr-min ≤γ max (30)

[0065]

[0066]

[0067] Among them, γ tr-max and γ tr-min are the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, respectively. min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively.

[0068] On the other hand, a system for optimal trajectory planning for intersection turning of articulated mining equipment is provided, comprising:

[0069] Kinematic model of articulated mobile mining equipment for trajectory planning algorithms;

[0070] The environmental perception module is used to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module;

[0071] The trajectory planning module based on quadratic programming is used to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, the longitudinal trajectory planning model is designed to generate the longitudinal trajectory, and then the lateral trajectory planning model is designed to generate the lateral trajectory.

[0072] A key parameter determination module based on three-dimensional search is used to take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, and respectively design an outer search loop based on driving time, a middle search loop based on longitudinal driving distance and an inner search loop based on lateral driving distance. When a trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted successively through the inner loop, the middle loop and the outer loop until a suitable driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm.

[0073] The constraint checking module is used to perform constraint checking on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop according to the three-dimensional search strategy until the constraint check is met, and the trajectory is taken as the optimal trajectory.

[0074] Preferably, the kinematic model of the articulated mobile mining equipment is expressed as:

[0075]

[0076] Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, Lr is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ.

[0077] Preferably, the design of the trajectory planning module based on quadratic programming includes:

[0078] The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as:

[0079]

[0080] And satisfy the following constraints:

[0081]

[0082]

[0083]

[0084] x min ≤x t ≤x max ,t=1,......,N (6)

[0085]

[0086]

[0087] x0=x in (9)

[0088]

[0089]

[0090] x N =x end (12)

[0091]

[0092] Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmax and Δa xmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle;

[0093] Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as:

[0094]

[0095]

[0096]

[0097]

[0098] y min ≤y t ≤y max ,t=1,......,N (18)

[0099]

[0100]

[0101] y0=y in (twenty one)

[0102]

[0103]

[0104] y N =y end (twenty four)

[0105]

[0106] Among them, y t , and is the decision variable, yt is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, v ymin and v ymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of , ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0. in and end x t The start and end constraints of and They are and The starting point constraint.

[0107] Preferably, the design of the key parameter determination module based on three-dimensional search includes:

[0108] The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula:

[0109] Δt i** =Δt initial +(i-1)·Δt′, (26)

[0110] in,

[0111] Δt initial ≥t lomin orΔt initial ≥max{t lomin ,t lamin},

[0112]

[0113]

[0114] i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment;

[0115] The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance of the middle cycle is Determined by the following formula:

[0116]

[0117] Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes;

[0118] The inner loop is designed to determine the lateral terminal position. The lateral travel distance of the inner loop is Determined by the following formula:

[0119]

[0120] Among them, k is the loop variable of the inner loop, y initial is the horizontal coordinate of the initial point of the iteration, and Δl is the discrete interval of the horizontal end position.

[0121] Preferably, the constraint checking module is used to perform constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including:

[0122] According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated;

[0123] Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints:

[0124] γ min ≤γ tr-max ≤γ max (29)

[0125] γ min ≤γ tr-min ≤γ max (30)

[0126]

[0127]

[0128] Among them, γ tr-max and γ tr-min are the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, respectively. min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively.

[0129] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0130] The optimal trajectory planning method and system provided by the embodiments of the present invention have the advantages of strong adaptability, simple core optimization model, and easy solution; the present invention does not need to carefully design the initial values ​​of the decision variables, and can generate a solution that satisfies the constraints regardless of whether the end point position is close to the finish line or the end point position is fixed on the finish line, and the obtained trajectory has optimality and completeness guarantees; the present invention can reduce the time for debugging and deployment of the autonomous driving system, which is conducive to giving full play to the performance of the automation equipment and improving production efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0131] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0132] Figure 1 It is a flow chart of an optimal trajectory planning method for intersection turning of articulated mining equipment provided by an embodiment of the present invention;

[0133] Figure 2 is a schematic diagram of a kinematic model of an articulated vehicle provided by an embodiment of the present invention;

[0134] Figure 3 is a schematic diagram of an application scenario provided by an embodiment of the present invention;

[0135] Figure 4 is a schematic diagram of a key parameter discretization process provided by an embodiment of the present invention;

[0136] Figure 5 is a schematic diagram of a specific flow chart of a trajectory planning process provided by an embodiment of the present invention;

[0137] Figure 6a-6b is a velocity and acceleration trajectory diagram of an articulated vehicle provided by an embodiment of the present invention;

[0138] Figure 7a-7f It is a trajectory diagram of the front and rear body heading angular velocity and articulation angular velocity of an articulated vehicle provided by an embodiment of the present invention;

[0139] Figure 8 It is a position curve diagram of an articulated vehicle provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0140] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0141] The embodiment of the present invention first provides an optimal trajectory planning method for intersection turning of an articulated mining equipment, such as Figure 1As shown, the method comprises the following steps:

[0142] Build a kinematic model of an articulated mobile mining rig for use in trajectory planning algorithms;

[0143] Design an environmental perception module to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module;

[0144] Design a trajectory planning module based on quadratic programming to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, design a longitudinal trajectory planning model to generate a longitudinal trajectory, and then design a lateral trajectory planning model to generate a lateral trajectory.

[0145] Design a key parameter determination module based on three-dimensional search, take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, design the outer search loop based on driving time, the middle search loop based on longitudinal driving distance and the inner search loop based on lateral driving distance respectively. When the trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted through the inner loop, middle loop and outer loop in sequence until the appropriate driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm.

[0146] A constraint checking module is designed to perform constraint checking on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop according to the three-dimensional search strategy until the constraint check is met, and this trajectory is taken as the optimal trajectory.

[0147] In the embodiment of the present invention, a scraper is taken as an example. Figure 2 As shown, the expression of its kinematic model is:

[0148]

[0149] Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, L r is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ.

[0150] Furthermore, a trajectory planning module based on quadratic programming is designed, which specifically includes:

[0151] The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as:

[0152]

[0153] And satisfy the following constraints:

[0154]

[0155]

[0156]

[0157] x min ≤x t ≤x max ,t=1,......,N (6)

[0158]

[0159]

[0160] x0=x in (9)

[0161]

[0162]

[0163] x N =x end (12)

[0164]

[0165] Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmax and Δaxmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle, such as Figure 3 As shown;

[0166] Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as:

[0167]

[0168]

[0169]

[0170]

[0171] y min ≤y t ≤y max ,t=1,......,N (18)

[0172]

[0173]

[0174] y0=y in (twenty one)

[0175]

[0176]

[0177] y N =y end (twenty four)

[0178]

[0179] Among them, y t , and is the decision variable, y t is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, vymin and v ymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of , ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0. in and end x t The start and end constraints of and They are and The starting point constraint.

[0180] Furthermore, a key parameter determination module based on three-dimensional search is designed, which specifically includes:

[0181] The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula:

[0182] Δt i** =Δt initial +(i-1)·Δt′, (26)

[0183] in,

[0184]

[0185] i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment;

[0186] Here, due to so

[0187]

[0188] The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance of the middle cycle is Determined by the following formula:

[0189]

[0190] Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes;

[0191] The inner loop is designed to determine the lateral terminal position. The lateral travel distance of the inner loop is Determined by the following formula:

[0192]

[0193] Among them, k is the loop variable of the inner loop, y initial is the horizontal coordinate of the initial point of the iteration, and Δl is the discrete interval of the horizontal end position, such as Figure 4 shown.

[0194] like Figure 5 As shown in the figure, when the trajectory planning algorithm based on quadratic programming has no solution and cannot obtain the trajectory with the given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are continuously adjusted in the inner loop, middle loop and outer loop according to the above three-dimensional search strategy until the appropriate driving time and driving distance are found, and the optimal solution is obtained on this basis to obtain the optimal trajectory.

[0195] Furthermore, a constraint checking module is designed, which performs constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including:

[0196] According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated;

[0197] Constraint checks are performed on the articulation angle and articulation angular velocity, such as Figure 4 As shown in the figure, if the current trajectory satisfies the constraints, the current trajectory is taken as the optimal trajectory. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop according to the three-dimensional search strategy, and constraint checks are performed until the constraint checks are met and the trajectory is taken as the optimal trajectory.

[0198] Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints:

[0199] γ min ≤γ tr-max ≤γ max (29)

[0200] γ min ≤γ tr-min ≤γ max (30)

[0201]

[0202]

[0203] Among them, γ tr-max and γ tr-minare the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, respectively. min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively. The experimental environment and vehicle parameters are given in Table 1.

[0204] Table 1 Experimental parameters

[0205]

[0206] The calculation results are shown in Tables 2 to 4. It can be seen that, by adopting the method proposed in the present invention, the optimal turning trajectory can be quickly obtained under different lateral / longitudinal entry speed conditions.

[0207] Table 2

[0208]

[0209] Table 3

[0210]

[0211] Table 4

[0212]

[0213] In order to display the trajectory more intuitively, the trajectory corresponding to Table 4 is Figure 6a-6b , Figure 7a-7f , Figure 8 Given. Among them, Figure 6a-6b is a velocity and acceleration trajectory diagram of an articulated vehicle provided by an embodiment of the present invention, Figure 7a-7f is a trajectory diagram of the front and rear body heading angular velocity and articulation angular velocity of an articulated vehicle provided by an embodiment of the present invention, Figure 8 It is a position curve diagram of an articulated vehicle provided in an embodiment of the present invention.

[0214] The method of the present invention can solve the problem of low working efficiency of autonomous shovel loaders due to difficulty in obtaining high-quality teaching information during turning, and the problem that the trajectory planning model cannot guarantee the optimal solution due to the physical constraints of the articulation angle. The present invention can obtain a turning trajectory that guarantees optimality and completeness, and can guide articulated mobile mining equipment to pass through the curve safely and efficiently under the conditions of minimum acceleration and minimum acceleration change rate. It can not only reduce the time for debugging and deployment of the autonomous driving system, but also help to give full play to the performance of automation equipment and improve production efficiency.

[0215] Accordingly, an embodiment of the present invention further provides an optimal trajectory planning system for intersection turning of an articulated mining equipment, the system comprising:

[0216] Kinematic model of articulated mobile mining equipment for trajectory planning algorithms;

[0217] The environmental perception module is used to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module;

[0218] The trajectory planning module based on quadratic programming is used to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, the longitudinal trajectory planning model is designed to generate the longitudinal trajectory, and then the lateral trajectory planning model is designed to generate the lateral trajectory.

[0219] A key parameter determination module based on three-dimensional search is used to take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, and respectively design an outer search loop based on driving time, a middle search loop based on longitudinal driving distance and an inner search loop based on lateral driving distance. When a trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted successively through the inner loop, the middle loop and the outer loop until a suitable driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm.

[0220] The constraint checking module is used to perform constraint checking on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop according to the three-dimensional search strategy until the constraint check is met, and the trajectory is taken as the optimal trajectory.

[0221] Furthermore, the kinematic model of the articulated mobile mining equipment is expressed as:

[0222]

[0223] Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, L r is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ.

[0224] Furthermore, the design of the trajectory planning module based on quadratic programming includes:

[0225] The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as:

[0226]

[0227] And satisfy the following constraints:

[0228]

[0229]

[0230]

[0231] x min ≤x t ≤x max ,t=1,......,N (6)

[0232]

[0233]

[0234] x0=x in (9)

[0235]

[0236]

[0237] x N =x end (12)

[0238]

[0239] Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmaxand Δa xmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle;

[0240] Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as:

[0241]

[0242]

[0243]

[0244]

[0245] y min ≤y t ≤y max ,t=1,......,N (18)

[0246]

[0247]

[0248] y0=y in (twenty one)

[0249]

[0250]

[0251] y N =y end (twenty four)

[0252]

[0253] Among them, y t , and is the decision variable, y t is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, v ymin and vymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of , ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0. in and end x t The start and end constraints of and They are and The starting point constraint.

[0254] Furthermore, the design of the key parameter determination module based on three-dimensional search includes:

[0255] The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula:

[0256] Δt i** =Δt initial +(i-1)·Δt′, (26)

[0257] in,

[0258]

[0259] i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment;

[0260] The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance of the middle cycle is Determined by the following formula:

[0261]

[0262] Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes;

[0263] The inner loop is designed to determine the lateral terminal position. The lateral travel distance of the inner loop is Determined by the following formula:

[0264]

[0265] Among them, k is the loop variable of the inner loop, y initialis the horizontal coordinate of the initial point of the iteration, and Δl is the discrete interval of the horizontal end position.

[0266] Furthermore, the constraint checking module is used to perform constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including:

[0267] According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated;

[0268] Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints:

[0269] γ min ≤γ tr-max ≤γ max (29)

[0270] γ min ≤γ tr-min ≤γ max (30)

[0271]

[0272]

[0273] Among them, γ tr-max and γ tr-min are the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, respectively. min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively.

[0274] The system of this embodiment can be used to execute Figure 1 The technical solution of the method embodiment shown has similar implementation principles and technical effects, which will not be repeated here.

[0275] In summary, the optimal trajectory planning method and system provided by the embodiments of the present invention have the advantages of strong adaptability, simple core optimization model, and easy solution; the present invention does not need to carefully design the initial values ​​of the decision variables, and can generate a solution that satisfies the constraints regardless of whether the end point position is close to the finish line or the end point position is fixed on the finish line, and the obtained trajectory has optimality and completeness guarantees; the present invention can reduce the time for debugging and deployment of the autonomous driving system, which is conducive to giving full play to the performance of the automation equipment and improving production efficiency.

[0276] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for optimal trajectory planning for intersection turning of articulated mining equipment, characterized in that: The following steps are involved: Build a kinematic model of an articulated mobile mining rig for use in trajectory planning algorithms; Design an environmental perception module to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module; Design a trajectory planning module based on quadratic programming to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, design a longitudinal trajectory planning model to generate a longitudinal trajectory, and then design a lateral trajectory planning model to generate a lateral trajectory. Design a key parameter determination module based on three-dimensional search, take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, design the outer search loop based on driving time, the middle search loop based on longitudinal driving distance and the inner search loop based on lateral driving distance respectively. When the trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted through the inner loop, middle loop and outer loop in sequence until the appropriate driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm. Design a constraint check module to perform constraint checks on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop, and outer loop according to the three-dimensional search strategy until the constraint check is met, and the trajectory is taken as the optimal trajectory. The kinematic model of the articulated mobile mining equipment is expressed as follows: Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, L r is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ; The trajectory planning module designed based on quadratic programming specifically includes: The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as: And satisfy the following constraints: x min ≤x t ≤x max ,t=1,......,N (6) x0=x in (9) x N =x end (12) Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmax and Δa xmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle; Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as: y min ≤y t ≤y max ,t=1,......,N (18) y0=y in (21) and N =and end (24) Among them, y t , and is the decision variable, y t is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, v ymin and v ymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0, y in and end x t The start and end constraints of and They are and The starting point constraint; The key parameter determination module designed based on three-dimensional search specifically includes: The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula: Δt i** =Δt initial +(i-1)·Δt′, (26) in, Δt initial ≥t lomin orΔt initial ≥max{t lomin ,t lamin }, i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment; The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance x of the middle cycle end*j* Determined by the following formula: x end*j* =x initial +(j-1)·Δd·cosα (27) Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes; The inner loop is designed to determine the lateral terminal position. The lateral travel distance y of the inner loop is end *jk is determined by the following formula: y end*jk =y initial -(j-1)·Δd·sinα-(k-1)·Δl (28) Among them, k is the loop variable of the inner loop, y initial is the horizontal coordinate of the initial point of the iteration, Δl is the discrete interval of the horizontal end position; The constraint checking module is used to perform constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including: According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated; Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints: c min ≤γ tr-max ≤γ max (29) c min ≤γ tr-min ≤γ max (30) Among them, γ tr-max and γ tr-min are the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, γ min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively.

2. An optimal trajectory planning system for intersection turning of articulated mining equipment, characterized in that: include: Kinematic model of articulated mobile mining equipment for trajectory planning algorithms; The environmental perception module is used to detect whether the articulated mobile mining equipment has entered the waiting area, and input the entry speed, position and posture into the trajectory planning module; The trajectory planning module based on quadratic programming is used to decouple the trajectory planning problem into longitudinal and lateral trajectory planning. First, the longitudinal trajectory planning model is designed to generate the longitudinal trajectory, and then the lateral trajectory planning model is designed to generate the lateral trajectory. A key parameter determination module based on three-dimensional search is used to take driving time, longitudinal driving distance and lateral driving distance as key factors of the search strategy, and respectively design an outer search loop based on driving time, a middle search loop based on longitudinal driving distance and an inner search loop based on lateral driving distance. When a trajectory planning algorithm based on quadratic programming fails to find a solution with a given initial driving time and driving distance, the driving time, longitudinal driving distance and lateral driving distance are adjusted successively through the inner loop, the middle loop and the outer loop until a suitable driving time and driving distance are found, and on this basis, the optimal solution is obtained through the trajectory planning algorithm. The constraint check module is used to perform constraint check on the trajectory obtained by the trajectory planning algorithm based on quadratic programming to ensure that the trajectory meets the constraints of the articulation angle and articulation angular velocity. If the current trajectory does not meet the constraints, a new trajectory is iteratively planned in the inner loop, middle loop and outer loop in turn according to the three-dimensional search strategy until the constraint check is met, and the trajectory is taken as the optimal trajectory; The kinematic model of the articulated mobile mining equipment is expressed as follows: Among them, x f and f are the ordinate and abscissa of the front body axis position, θ f is the heading angle of the front body of the scraper, L f is the distance from the articulation angle to the front body axis, L r is the distance from the articulation angle to the rear body axis, γ is the articulation angle, is the angular velocity of the articulation angle γ; The design of the trajectory planning module based on quadratic programming includes: The longitudinal trajectory planning model is designed. The objective function of the longitudinal trajectory planning model is composed of the longitudinal acceleration and its rate of change, which can be expressed as: And satisfy the following constraints: x min ≤x t ≤x max ,t=1,......,N (6) x0=x in (9) x N =x end (12) Among them, x t , and is the decision variable, N is the number of discrete intervals, Δt is the time interval, t represents the time interval index, and x t is the longitudinal position of the front body axis, x min and x max is x t The borders of is the longitudinal velocity of the front vehicle axis, v xmin and v xmax yes The borders of is the longitudinal acceleration of the front vehicle axis, a xmin and a xmax yes The borders of Δa xmax and Δa xmin yes The boundary of x is λ1 and λ2, where λ1+λ2=1, λ1>0, λ2≥0, and x in and x end x t The start and end constraints of and They are and The starting point constraint, α is the turning angle; Design a lateral trajectory planning model. The objective function of the lateral trajectory planning model consists of the lateral acceleration and its rate of change, which can be expressed as: y min ≤y t ≤y max ,t=1,......,N (18) y0=y in (21) and N =and end (24) Among them, y t , and is the decision variable, y t is the lateral position of the front body axis, y min and max Yes t The borders of is the lateral velocity of the front vehicle axis, v ymin and v ymax yes The borders of is the lateral acceleration of the front vehicle axis, a ymin and a ymax yes The borders of Δa ymax and Δa ymin yes The boundary of ρ1 and ρ2 are weight coefficients, ρ1+ρ2=1, ρ1>0, ρ2≥0, y in and end x t The start and end constraints of and They are and The starting point constraint; The design of the key parameter determination module based on three-dimensional search includes: The outer loop is designed to determine the time interval, the time interval Δt of the outer loop i** Determined by the following formula: Δt i** =Δt initial +(i-1)·Δt′, (26) in, Δt initial ≥t lomin orΔt initial ≥max{t lomin ,t lamin }, i is the loop variable of the outer loop, Δt′ is the time interval of each loop increment; The middle cycle is designed to determine the longitudinal terminal position. The longitudinal travel distance x of the middle cycle end*j* Determined by the following formula: x end*j* =x initial +(j-1)·Δd·cosα (27) Among them, j is the loop variable of the middle loop, x initial is the ordinate of the initial point of the iteration, Δd is the discrete interval of the end line, and α is the angle between the two lanes; The inner loop is designed to determine the lateral terminal position. The lateral travel distance y of the inner loop is end*jk Determined by the following formula: y end*jk =y initial -(j-1)·Δd·sinα-(k-1)·Δl (28) Among them, k is the loop variable of the inner loop, y initial is the horizontal coordinate of the initial point of the iteration, Δl is the discrete interval of the horizontal end position; The constraint checking module is used to perform constraint checking on the articulation angle and articulation angular velocity of the trajectory obtained by the trajectory planning algorithm, including: According to the longitudinal acceleration and lateral acceleration sequence obtained by the trajectory planning algorithm and the kinematic model shown in formula (1), the articulation angle and angular velocity sequence corresponding to the trajectory are calculated; Perform constraint checks on the articulation angle and articulation angular velocity. The articulation angle and articulation angular velocity should satisfy the following constraints: c min ≤γ tr-max ≤γ max (29) c min ≤γ tr-min ≤γ max (30) Among them, γ tr-max and γ tr-min are the maximum and minimum articulation angles in the trajectory, and are the maximum and minimum articulation angular velocities in the trajectory, γ min and γ max and and are the boundaries of the articulation angle and articulation angular velocity, respectively.

Images