A parking planning method for a mine site four-wheel steer vehicle

Abstract

This invention belongs to the field of unmanned parking planning technology in mining areas, specifically disclosing a parking planning method for four-wheel steering vehicles in mining areas. The method selects a suitable parking posture based on the initial position and orientation of the entry point. The parameters of the parking posture include vehicle position and vehicle orientation, and two parking postures with the same vehicle position and different vehicle orientations are selected. Based on different entry points and different radii of curvature, multiple paths are generated as a candidate set using Reeds-sheep curves according to three reversing modes: front-wheel steering, rear-wheel steering, and four-wheel steering. An evaluation function is established to calculate the cost of each parking path used by the vehicle, and the optimal path with the lowest cost in the candidate set is selected. This allows the vehicle to make better choices based on its initial and final positions. The parking planning method of this invention avoids "reversing and switching paths" and fully utilizes the dynamic performance of four-wheel steering vehicles.

Description

Technical Field

[0001] This invention relates to the field of unmanned parking planning technology in mining areas, and more specifically, to a parking planning method for four-wheel steering vehicles in mining areas. Background Technology

[0002] In mining areas, mining trucks typically park at low speeds in designated locations within the loading and unloading area. Parking scenarios in mining areas are often narrow and complex, thus placing certain requirements on the driving paths and parking positions of the mining trucks.

[0003] Patent CN114812580A, "Global Path Planning Method and System for Unmanned Vehicles in Mining Areas," proposes a global path planning method based on curve fitting and screening, which can realize parking planning in loading and unloading areas.

[0004] Patent CN112572416A, "A Parking Method and System for Unmanned Vehicles in Mining Areas," proposes a parking path planning method for front-wheel steering mining trucks.

[0005] Current parking algorithms in mining areas generally use the kinematic model of front-wheel steering mining trucks to perform curve fitting to complete path planning, without taking into account the steering characteristics of four-wheel steering vehicles, and thus failing to fully utilize their dynamic performance. Therefore, a parking planning method for four-wheel steering vehicles in mining areas is proposed to solve the above-mentioned problems. Summary of the Invention

[0006] The purpose of this invention is to propose a parking method for unmanned mining vehicles that can better utilize the kinematic characteristics of four-wheel steering vehicles. This parking method can avoid collisions with obstacles in the parking area while ensuring smooth vehicle operation, thus leveraging the kinematic characteristics of four-wheel steering vehicles, supporting the parking function of mining trucks in narrow areas, and improving the transportation efficiency in mining areas.

[0007] In view of this, a first aspect of the present invention is to provide a parking planning method for four-wheel steering vehicles in mining areas.

[0008] The first aspect of this invention provides a parking planning method for four-wheel steering vehicles in mining areas, comprising the following steps: S1, selecting two parking poses based on the initial position and initial orientation of the entry point, wherein the parameters of the parking poses include the vehicle position and the vehicle orientation, and the two parking poses have the same vehicle position and different vehicle orientations; S2, generating multiple parking paths as a candidate set using Reeds-sheep curves according to the entry point and radius of curvature, based on three reversing modes: front-wheel steering, rear-wheel steering, and four-wheel steering; S3, establishing an evaluation function to calculate the cost of the vehicle using each parking path in the candidate set, and selecting the path with the lowest cost in the candidate set as the optimal path; S4, using a spiral modeling method to search for drivable areas on the optimal path obtained in S3, and determining whether a narrow area is encountered. If so, the optimal path selected this time is deleted from the candidate set and returned to S3; otherwise, proceed to the next step; S5, establishing a quadratic optimization model and using the Sequential Quadratic Optimization (SQP) algorithm to smooth the optimal path without narrow areas, and outputting the result to obtain the final parking path.

[0009] This invention provides a parking planning method for four-wheel steering vehicles in mining areas. Based on the kinematic characteristics of four-wheel steering mining trucks, two parking postures are selected for parking planning, providing selectable parking postures so that the vehicle can choose according to the initial and final positions. This can avoid the occurrence of "reversing to switch paths". Furthermore, if only the vehicle orientation is used for path planning, the obtained path may have "reversing to switch paths" and is not the optimal path for four-wheel steering vehicles.

[0010] In the three reversing modes of front-wheel steering, rear-wheel steering, and four-wheel steering, the reeds-sheep curve and variable radius of curvature are used to fit the path to obtain a set of parking paths. This fully utilizes the dynamic performance of four-wheel steering vehicles and obtains a sufficient number of parking paths so that the best path can be selected in subsequent steps.

[0011] By using a path evaluation function to select the best parking path, the lowest cost path for a four-wheel steering mining truck can be obtained. By using a spiral modeling method, the drivable area of ​​the mining truck in the mining area can be effectively searched and obstacle avoidance can be achieved. It also provides a search area for subsequent path smoothing problems and determines whether there are narrow areas in the area to avoid the lowest cost path being difficult to implement or causing collisions in actual parking.

[0012] By establishing a quadratic optimization problem to smooth the path, an optimal path that is convenient for mining truck pre-aiming and tracking control can be obtained. The sequential quadratic optimization (SQP) method is used to solve the quadratic optimization problem, which reduces the difficulty of solving the optimization problem and improves the solution quality.

[0013] In addition, the technical solutions provided by embodiments of the present invention may also have the following additional technical features:

[0014] In any of the above technical solutions, the vehicles in the two parking positions in S1 face opposite directions.

[0015] In this technical solution, based on the actual parking direction of the vehicle, forward entry from the front of the vehicle and reverse entry from the rear of the vehicle are added to adapt to more initial positions and further obtain parking paths that can be selected.

[0016] In any of the above technical solutions, step S2 specifically includes: S201, obtaining the steering geometry corresponding to front-wheel steering, rear-wheel steering, and four-wheel steering for the four-wheel vehicle respectively; S202, obtaining the corresponding reeds-sheep parking path according to the different entry points and different turning radii in the three steering modes and according to the two parking postures respectively, and adding it to the candidate set; wherein, the steering geometry includes the turning radius and the turning center point.

[0017] In this technical solution, since the vehicle can perform four-wheel steering, there are three different steering modes: front-wheel steering, rear-wheel steering, and four-wheel steering, each with different steering radii and steering geometry. For two different parking positions, multiple different parking path schemes can be obtained. All obtained paths are added to the candidate set for subsequent selection, and the best path that can be driven is selected.

[0018] In any of the above technical solutions, the parameters of the cost value mentioned in S3 include: distance cost, direction cost, and reversing switching cost.

[0019] In this technical solution, three cost aggregation methods are used to determine the cost, which can provide accurate determination results and make the selected path adapt to the actual vehicle parking needs.

[0020] In any of the above technical solutions, the evaluation function mentioned in S3 is specifically the following formula: f:Min( distance +ost heading +ost isHeadBack ); where cost distance The distance cost is determined by the total length of the parking path. heading The directional cost is determined by the circular path portion of the parking path. isHeadBack The cost of switching to reverse is determined by the reverse driving path portion of the parking path.

[0021] In this technical solution, cost distance Distance cost headingDirectional cost and price isHeadBack The cost of reversing and switching is aggregated into a complete decision function using an additive approach, allowing for consideration from multiple perspectives. distance The distance cost represents the total length of the parking path; this term indicates that the chosen parking path should be as short as possible. heading The directional cost represents the circular path portion of the parking path. This term indicates the selection of a parking path that is as smooth as possible and minimizes the turning process. isHeadBack The cost for reversing indicates the reversing path during parking, and this option indicates that reversing should be avoided as much as possible during parking.

[0022] In any of the above technical solutions, the step of searching for the drivable area in S4 specifically includes: S401, spiraling outwards from each point in the parking path; S402, after each expansion, judging the distance between the expansion point and the obstacle, if the requirements are met, continuing the expansion; if the expansion point does not meet the requirements, recording the coordinates of the point as the maximum point in the expansion direction; S403, stopping the expansion when the maximum points in all four expansion directions have been found, and the area enclosed by the maximum points in the four directions is the drivable area of ​​the path point.

[0023] In this technical solution, after obtaining the optimal path with the lowest cost, the drivable area of ​​a certain point in the parking path is detected, and the drivable areas of all points are summarized to obtain the total drivable area of ​​the path.

[0024] In any of the above technical solutions, the distance determination in S402 is: determining whether the distance between the extension point and the obstacle is greater than half the width of the vehicle.

[0025] In this technical solution, the distance between the extension point and the obstacle is determined, and the determination requirements are limited to avoid the vehicle body from colliding with external obstacles during actual driving, thus ensuring normal parking of the vehicle.

[0026] In any of the above technical solutions, the step of determining the narrow area in S4 specifically involves: during the search process, if the first four expansion attempts fail or the area of ​​the drivable region after expansion is less than 0.5m... 2 If so, it is determined to be a narrow area.

[0027] In this technical solution, the presence of narrow areas indicates that vehicles traveling on such routes are prone to collisions.

[0028] The beneficial effects of this invention compared to the prior art are as follows:

[0029] This patent addresses the kinematic characteristics of four-wheel steering mining trucks by selecting two parking positions for parking planning, which can avoid the occurrence of "reversing to switch paths";

[0030] This patent innovatively uses the reeds-sheep curve and variable radius of curvature to perform path fitting in three reversing modes: front-wheel steering, rear-wheel steering, and four-wheel steering, to obtain a set of parking paths, thus giving full play to the dynamic performance of four-wheel steering vehicles.

[0031] This patent proposes a path evaluation function for four-wheel steering mining trucks, which selects the best parking path from the obtained parking paths to obtain the lowest cost path for four-wheel steering mining trucks.

[0032] This patent innovatively proposes a spiral modeling method, which can effectively search the drivable area of ​​mining trucks in mining scenarios and achieve obstacle avoidance, and provides a search area for subsequent path smoothing problems;

[0033] This patent establishes a quadratic optimization problem to smooth the path, thereby obtaining an optimal path that facilitates pre-aiming and tracking control of mining trucks. The Sequential Quadratic Optimization (SQP) method is used to solve the quadratic optimization problem, reducing the difficulty of solving the optimization problem and improving the quality of the solution.

[0034] Additional aspects and advantages of embodiments of the invention will become apparent in the following description or may be learned by practice of embodiments of the invention. Attached Figure Description

[0035] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of the invention.

[0036] Figure 1 This is a flowchart of a parking method for a four-wheel steering vehicle in a mining area according to the present invention.

[0037] Figure 2 This invention provides three steering kinematic models: front-wheel steering, rear-wheel steering, and four-wheel steering.

[0038] Figure 3 This is a process diagram of a spiral modeling method for searching the drivable area of ​​a mining truck according to the present invention. Detailed Implementation

[0039] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0040] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.

[0041] Please see Figure 1-3 The first aspect of the present invention provides a parking planning method for four-wheel steering vehicles in mining areas, comprising the following steps:

[0042] Step 1: Select a suitable parking posture based on the initial position and orientation of the entry point. The parameters of the parking posture include the vehicle position p and the vehicle orientation φ, and select two parking postures: (p,φ) and (p,φ+180°).

[0043] If only the parking heading φ is used for path planning, the resulting path may include "reversing path switching" and is not the optimal path for a four-wheel steering vehicle.

[0044] Step 2: Based on different entry points and different radii of curvature, generate multiple paths as a set of candidates using reeds-sheep curves according to the three reversing modes of front-wheel steering, rear-wheel steering, and four-wheel steering.

[0045] The radius of curvature is the minimum turning radius inherent in the vehicle's kinematics, denoted as r. r, 2r, and 3r are used as the radii of curvature, respectively.

[0046] Specifically, such as Figure 2 As shown, the three steering modes of a four-wheel steering vehicle—front-wheel steering, rear-wheel steering, and four-wheel steering—correspond to different steering radii and steering geometries. Under each of the three steering modes, based on two different positions, the corresponding reeds-sheep parking paths are obtained according to different entry points and different radii of curvature, and added to the same candidate set for comparison.

[0047] When the steering geometry is different, the (x, y) coordinates represent different geometric positions of the vehicle. When the front wheels are turning, the (x, y) coordinates are the rear axle center of the vehicle; when the rear wheels are turning, they are the front axle center of the vehicle; when all four wheels are turning, (x, y) is the geometric center point of the vehicle.

[0048] Step 3: Establish an evaluation function to calculate the cost of each parking path used by the vehicle, and select the optimal path with the lowest cost from the candidate set. The evaluation function includes three parameters: distance cost, direction cost, and reversing switching cost, as follows:

[0049] f:Mincost distance +oSt heading +ost isHeadBack

[0050] Among them, cost distance The distance cost represents the total length of the parking path, indicating the desired shortest possible parking path; heading The directional cost represents the circular path portion of the parking path; this term indicates the desired smoothness of the parking path and minimization of steering input. isHeadBack The reversing switching cost represents the reversing path portion of the parking process, indicating the goal of minimizing reversing during parking. The cost for each alternative path is calculated using the method described above, and the path with the minimum cost is selected.

[0051] Step 4: Use the spiral modeling method to search for the drivable area of ​​the optimal path obtained in Step 3. If an area that is too narrow appears during the search, it is determined that the path has a collision risk.

[0052] in, Figure 3 The spiral modeling method shown is applied to discrete points along the parking path. For example... Figure 3 As shown, at a certain point in the parking path, the path expands spirally in all directions. After each expansion, it first checks if the current expansion point is too close to an obstacle. If the distance from the current expansion point to the nearest obstacle is less than half the vehicle width, it is considered too close. If the requirement is met, the next expansion point is checked. If the next expansion point also meets the requirement, the expansion is considered successful and the current expansion point is retained. If the expansion does not meet the above requirements, the coordinates of the point are recorded as the maximum point in that expansion direction. The search stops when the maximum points in all four expansion directions have been found. The maximum points obtained in these four directions at this point represent the drivable area of ​​that path point.

[0053] Step 5: Determine whether the drivable area obtained in Step 4 has a collision risk. If so, delete the selected path from the candidate set and return to Step 3. If not, proceed to Step 6.

[0054] Step 6: Establish a quadratic optimization model and use the Sequential Quadratic Optimization (SQP) algorithm to smooth the collision-free optimal path, thus establishing a quadratic optimization problem. The optimization quantity of the quadratic optimization model is the discrete point (x... i y i ), and specifically the following formula:

[0055] f:Mincost1+ost2+ost3

[0056] Cost1, cost2, and cost3 are respectively solved using the following formulas:

[0057]

[0058]

[0059]

[0060] Among them, (x i-ref y i-ref ) represents the path point (x) i y i The original path point corresponding to ). cost1, cost2, and cost3 represent the smoothness cost, length cost, and deviation cost from the original path, respectively. n is the total number of path points, i represents the i-th path point, x is the x-axis coordinate, and y is the y-axis coordinate.

[0061] The constraints of the quadratic optimization model are:

[0062]

[0063] In this context, formulas ① and ② represent constraints on the optimization variables; formula ③ represents slack variables introduced to facilitate solving the quadratic optimization problem; and formula ④ represents the average distance between discrete points and the maximum curvature constraint.

[0064] At this point, constraint ④ is still a nonlinear constraint and cannot constitute a quadratic optimization problem.

[0065] Therefore, the following function:

[0066] F=(x i-1 + i+1 -2* i ) 2 +(y i-1 + i+1 -2* i ) 2 -tack i

[0067] Performing a Taylor expansion of the function at and retaining only the first-order terms yields:

[0068] F = F(X) ref )+ ′ (X ref )*(X- ref )

[0069] Therefore, the fourth nonlinear constraint can be transformed into a linear constraint:

[0070] F(X ref )+ ′ (X ref )*(X- ref )≤(Δs 2 *ur cstr ) 2

[0071] At this point, the constraints of the above optimization problem have all been transformed into linear constraints. The SQP method is then used to solve the problem in order to improve the solution quality and speed.

[0072] In the description of this invention, it should be understood that the terms "longitudinal", "lateral", "up", "down", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing this invention, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention.

[0073] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made by those skilled in the art to the technical solutions of the present invention without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A parking planning method for four-wheel steering vehicles in mining areas, characterized in that, Includes the following steps: S1. Select two parking positions based on the initial position and initial orientation of the entry point. The parameters of the parking positions include the vehicle position and the vehicle orientation. The two parking positions have the same vehicle position and different vehicle orientations. S2, based on the entry point and radius of curvature, uses the Reeds-Shepp curve to generate multiple parking paths as a set of candidates according to the three reversing modes of front wheel steering, rear wheel steering and four-wheel steering; S3. Establish an evaluation function to calculate the cost of each parking path in the candidate set for the vehicle, and select the path with the lowest cost in the candidate set as the optimal path. S4: Use the spiral modeling method to search for drivable areas of the optimal path obtained in S3, and determine whether a narrow area is found. If so, delete the selected optimal path from the candidate set and return to S3. If not, proceed to the next step. S5. Establish a quadratic optimization model and use the Sequential Quadratic Optimization (SQP) algorithm to smooth the optimal path without narrow areas, and output the result to obtain the final parking path.

2. The parking planning method for four-wheel steering vehicles in mining areas according to claim 1, characterized in that, The two parking positions in S1 have vehicles facing opposite directions.

3. The parking planning method for four-wheel steering vehicles in mining areas according to claim 1, characterized in that, Step S2 specifically includes: S201, respectively obtain the steering geometry of the four-wheeled vehicle when the front wheels are steering, the rear wheels are steering, and all four wheels are steering; S202, under the three steering modes, according to the two parking postures, obtain the corresponding Reeds-Shepp parking path based on the different entry points and different radii of curvature, and add it to the candidate set; The steering geometry includes the steering radius and the steering center point.

4. The parking planning method for four-wheel steering vehicles in mining areas according to claim 1, characterized in that, The parameters of the cost value described in S3 include: distance cost, direction cost, and reversing switching cost.

5. A parking planning method for four-wheel steering vehicles in mining areas according to claim 4, characterized in that, The evaluation function described in S3 is specifically the following formula: ； in, The distance cost is determined by the total length of the parking path. The directional cost is determined by the circular path portion of the parking path. The cost of switching to reverse is determined by the reverse driving path portion of the parking path.

6. A parking planning method for four-wheel steering vehicles in mining areas according to claim 1, characterized in that, The steps for searching the drivable area in S4 specifically include: S401 expands spirally outwards from each point in the parking path; S402: After each expansion, the distance between the expansion point and the obstacle is judged. If the requirements are met, the expansion continues. If the expansion point does not meet the requirements, the coordinates of the point are recorded as the maximum point in the expansion direction. S403, when the maximum point in all four directions has been found, the expansion stops and the area enclosed by the maximum points in the four directions is the drivable area of ​​that path point.

7. A parking planning method for four-wheel steering vehicles in mining areas according to claim 6, characterized in that, The distance determination in S402 is as follows: determining whether the distance between the extension point and the obstacle is greater than half the width of the vehicle.

8. A parking planning method for four-wheel steering vehicles in mining areas according to claim 6, characterized in that, The step for determining the narrow area in S4 specifically involves: during the search process, if the first four expansion attempts fail or the area of ​​the drivable region after expansion is less than 0.5... If so, it is determined to be a narrow area.

Images