A path planning method for unmanned pusher and rake machine in ship cabin

By designing the path planning method of unmanned push rakes in the cabin, and using area traversal and dubins planning algorithms, the problem of inefficient manual driving is solved, efficient gathering and precise arrival of coal piles is achieved, and work efficiency is improved and work efficiency is reduced. Health risks.

CN114662738BActive Publication Date: 2024-12-06ZHEJIANG UNIV OF TECH +1
View PDF 3 Cites 0 Cited by

Patent Information

Application Number
CN202210178628.5
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-02-25
Publication Date
2024-12-06
Estimated Expiration
2042-02-25

AI Technical Summary

Technical Problem

In the prior art, the operation of the cabin push rake machine requires manual driving, resulting in low working efficiency, harsh environment and dust, making it difficult to drive accurately, causing health damage.

Method used

A path planning method suitable for cabin unmanned rake machines was designed, and the pattern of area traversal first and single point traversal was adopted. The target point control area traversal algorithm and the improved dubins planning algorithm were used to realize the gathering and precise arrival of coal piles. .

Benefits of technology

Improve the working efficiency and accuracy of the rake pusher, reduce manual intervention, and reduce health risks to drivers.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114662738B_ABST
    Figure CN114662738B_ABST
Patent Text Reader

Abstract

A path planning method for an unmanned pusher and rake machine in a ship cabin includes: the first step of regional pusher and rake: in response to the working condition that there are a large number of coal piles in the cabin, a target point control area traversal algorithm is used to traverse the entire pusher and rake area and gather the coal piles to the coal pile. The second step is point-to-point in the area: in response to the working condition that there are a few coal piles remaining in the cabin, an improved Dubins planning algorithm is used to gather the coal piles to the coal pile. This planning scheme makes the path planning of the pusher and rake machine have good robustness and effectively improves the flexibility of the pusher and rake machine's operation, and improves the efficiency of the pusher and rake machine in completing the operation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of pusher and rake operation path planning methods, and specifically to an implementation of a path planning method for an automatic pusher and rake in a ship cabin. According to the working tasks of the pusher and rake in a ship cabin environment and the mechanical characteristics of the pusher and rake, a path planning mode of first traversing an area and then traversing a single point is designed, which can effectively ensure the high efficiency of the pusher and rake operation. Background Art

[0002] my country is a large coal producer and consumer. A large amount of coal is needed and consumed in many places. Ports, as transit stations for coal transportation, play an important role in coal transportation. In port operations, cargo ships are docked at the dock, and ship unloaders are generally used to grab the coal in the cabin. Due to the limitations of the top baffle of the cabin and the working range of the ship unloader, the ship unloader can only grab the coal pile in the middle area of ​​the cabin. The remaining coal pile is often piled under the top baffle of the cabin and attached to the two walls of the cabin. At this time, it is necessary to use a crane to put the pusher into the cabin, manually drive the pusher to gather the coal pile to the middle of the cabin, and then use the grab of the ship unloader to grab the remaining coal pile.

[0003] At present, the operation of the pusher is basically driven manually. The operator needs to go deep into the bottom of the cabin and drive the pusher to gather the coal piles around the cabin to the middle area of ​​the cabin, and then the ship unloader will grab it. In order to speed up the unloading of coal, the driver needs to work day and night. The working environment in the cabin is harsh, the air quality is poor, and the working environment is noisy. Working in such a harsh environment for a long time will cause great damage to human health. On the other hand, the cabin is filled with dust and the visibility is poor. The driver cannot accurately drive to the target pusher area, which greatly reduces the work efficiency.

[0004] In order to solve the above problems, the present invention proposes a path planning method suitable for an unmanned pusher and rake in a cabin. According to the working tasks of the pusher and rake in the cabin and the mechanical characteristics of the unmanned pusher and rake, a path planning mode of first traversing the area and then traversing the single point is designed to ensure that the pusher and rake can accurately reach the target point to complete the operation, thereby improving work efficiency. Summary of the invention

[0005] The present invention aims to overcome the above-mentioned shortcomings of the prior art and provides a path planning method suitable for an unmanned pusher and rake machine in a cabin.

[0006] The technical solution adopted by the present invention to solve the path planning problem of unmanned pusher rake machine is:

[0007] A path planning method for an unmanned pusher and rake machine in a cabin, comprising the following steps:

[0008] Step 1: Regional raking: For the working condition where there are a large number of coal piles in the cabin, the target point control regional traversal algorithm is used to traverse the entire raking area and gather the coal piles to the coal pile. Specifically, it includes:

[0009] Step 1-1: Use the rotation center as the position point of the pusher and rake; use a 3*1 column vector to represent the position of the target point. In different coordinate systems, the column vector representing the position is different. Use the following formula to transform the target point to the vehicle coordinate system;

[0010]

[0011] P c , P w They represent the position and pose of the target point in the vehicle coordinate system and the world coordinate system respectively, both of which are 3*1 column vectors. is a 3*3 rotation matrix representing the rotation transformation from the world coordinate system to the vehicle coordinate system, P c w It is a 3*1 column vector representing the position of the origin of the world coordinate system in the vehicle coordinate system.

[0012] Step 1-2: The controller accepts the corner points of the area to construct a rectangular raking area and calculates whether the size of the area is within the set threshold range;

[0013] Step 1-3: Select the lower boundary line of the rectangular pushing and raking area as the coal pile line of the regional pushing and raking, and extract the coal pile area closest to the coal pile line as the pushing and raking starting line; segment the pushing and raking starting line and the coal pile line according to the set vehicle width to obtain the position set of the pushing and raking target point and the coal pile point.

[0014] Step 1-4: Traverse the position points in the above two sets in the order of pushing the target point first and then the coal pile point. For a position point, use the transformation formula in step 1-1 to transform the position point to the vehicle coordinate system. According to the x, y coordinates of the transformed position point in the vehicle coordinate system, control the left and right rotation of the vehicle and adjust the posture of the vehicle so that the position point is directly in front of the vehicle.

[0015] Step 1-5: Calculate the Euclidean distance between the position point and the center of the vehicle. When the distance is less than the set value, it is considered that the vehicle has reached the position point, and then the vehicle posture is corrected. When correcting the vehicle posture, the change in the vehicle position is no longer considered, and only the vehicle posture value is judged. Step 1-5 decouples the correction of the vehicle's position and the correction of the vehicle's posture. The modified unmanned pusher is only equipped with a single motor. When rotating, only one track can be set to move, and the other track remains stationary. This causes the center of rotation of the vehicle to not be in the exact center of the vehicle, that is, it cannot rotate in place. At this time, the rotation will cause a non-negligible change in the position of the vehicle. If the position of the vehicle and the posture of the vehicle are considered at the same time, the vehicle will be trapped in a long correction, and the work efficiency will be greatly reduced;

[0016] Step 1-6: Obtain the Euler angle representation of the vehicle posture and the position point posture and compare them, and use the difference between the two to control the rotation correction of the vehicle; when the difference between the two is less than the set threshold, the posture correction is determined to be successful, and the corresponding push-hake action is performed;

[0017] Step 1-7: Repeat steps 1-4 to 1-6 until the position set of the push-rake target point and the coal pile point is traversed;

[0018] Step 1-8: extract the coal pile area closest to the coal pile line as the new push rake starting line and determine whether the new push rake starting line has exceeded the range of the rectangular push rake area. If so, send the information that the area push rake is completed and enter the point-to-point push rake mode in the area (go to step 2). If not, go to step 1-3 to calculate the position set of the new push rake target point and the coal pile point, and continue to execute steps 1-4 to 1-7;

[0019] Step 2, point-to-point within the area: For the working condition where there is a little coal pile left in the cabin, the improved Dubins planning algorithm is used to gather the coal pile to the coal pile, which includes:

[0020] Step 2-1: Get the Euler angle representation of the vehicle posture and the target point posture, calculate the difference in yaw angles between the two, and use this difference to control the rotation of the pusher and rake so that the difference in yaw angles is within the set threshold. The above is the preprocessing before enabling Dubins planning, which can avoid Dubins planning of suboptimal solutions, thereby improving the efficiency of path planning;

[0021] Step 2-2: Use the three path segment method to divide the planned path into three segments, starting point T i The arc segment to tangent point 1, the arc segment (or straight line segment) from tangent point 1 to tangent point 2, and the arc segment (or straight line segment) from tangent point 2 to the end point T gAccording to the direction of each path segment, the feasible path types are integrated into the Dubins set D = {LSL, RSR, RSL, LSR, RLR, LRL}; where L represents the arc motion in the counterclockwise direction, R represents the arc motion in the clockwise direction, and S represents the straight line motion;

[0022] Step 2-3: Transform the coordinate system and convert the original world coordinate system into the starting point T i (x i ,y i ,α i ) points to the end point T g (x g ,y g ,β g ) is the x-axis, and the y-axis is perpendicular to it. After the coordinate system is transformed, the coordinates of the starting point are (0,0,α i -θ), the end point is (d,0,β g -θ); where θ is the heading angle difference from the starting point to the end point;

[0023] θ=arctan((y g -y i ) / (x g -x i )) (2)

[0024] D = sqrt((x g -x i ) 2 +(y g -y i ) 2 ) (3)

[0025] d=D / R (4)

[0026] In the above formula, R is the turning radius of the vehicle, and D is the Euclidean distance from the vehicle to the target point. Dividing D by R is to regularize R so that the turning radius of the vehicle becomes 1. In this way, the arc length is equal to the radian of the angle, which is convenient for subsequent calculations. Therefore, the radius 1 is omitted before cosα seen later.

[0027] The following takes the LSL case as an example to calculate the path length planned by Dubins.

[0028] According to the geometric relationship after the coordinate system transformation, the following constraint equations can be obtained:

[0029] α=α i -θ (5)

[0030] β=β g -θ (6)

[0031] pcos(α+t)-sinα+sinβ=d (7)

[0032] p sin(α+t)+cosα-cosβ=0 (8)

[0033] α+t+g=β{mod 2π} (9)

[0034] In the above formula, p is the distance between the two tangent points, and t is the number of arcs that the starting segment has turned, that is, the starting point T i The arc segment to the tangent point 1, g is the number of arcs the terminal segment has turned, that is, the arc segment from the tangent point 2 to the terminal point T g The arc segment. Solving the above equations, we can get:

[0035]

[0036]

[0037]

[0038] The total length can be calculated as:

[0039] L=t lsl +p lsl +g lsl =-α+β+p lsl (13)

[0040] The length calculations for other path types are similar.

[0041] Step 2-4: Calculate the length of the feasible path in step 2-2 using the formula described in step 2-3, and select the shortest Dubins path as the planned path;

[0042] Step 2-5: Execute the planned path in 3 sections, starting from the starting point T i The arc segment to tangent point 1, the arc segment from tangent point 1 to tangent point 2 or the straight line segment from tangent point 2 to the end point T g After reaching the target point, the vehicle posture is corrected and the required pushing and raking action is performed;

[0043] Step 2-6: Repeat steps 2-1 to 2-5 with the coal pile point as the target point to gather the coal pile into the coal pile area.

[0044] Step 2-7: Repeat steps 2-1 to 2-6 until there is no residual coal pile in the pushing and raking area. At this point, the path planning of the unmanned pushing and raking machine is completed, and the pushing and raking is terminated and the next instruction is received to start a new planning.

[0045] The advantages and positive effects of the present invention are: the present invention divides the path planning scheme into two steps in combination with the working environment and working requirements of the pusher rake machine. The first step is regional pushing and raking: to deal with the working condition where there are a large number of coal piles in the cabin, the target point control area traversal algorithm is used to traverse the entire pushing and raking area, and the coal piles are gathered to the coal piles. The second step is point-to-point within the area: to deal with the working condition where there are some coal piles remaining in the cabin, the improved Dubins planning algorithm is used to gather the coal piles to the coal piles. This planning scheme makes the path planning of the pusher rake machine have good robustness and effectively improves the flexibility of the pusher rake machine's operation, and improves the efficiency of the pusher rake machine in completing the work. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is a schematic diagram of a path planning module for a push-rake machine of the present invention;

[0047] Figure 2 The vehicle rotation point of the present invention is used as the vehicle posture point;

[0048] Figure 3 It is a suboptimal solution of the planning of Dubins of the present invention;

[0049] Figure 4 It is a schematic diagram of the dubins planning path of the present invention;

[0050] Figure 5 is the coordinate transformation of dubins of the present invention;

[0051] Figure 6 is a schematic diagram of the LSL path of the present invention;

[0052] Figure 7 It is a diagram of the regional push rake simulation result of the present invention. DETAILED DESCRIPTION

[0053] The present invention is further described in detail below with reference to the accompanying drawings.

[0054] The path planning method for an unmanned rake machine based on ROS communication of the present invention divides the path planning scheme into two steps according to the working environment and working requirements of the rake machine, such as Figure 1 shown.

[0055] The specific steps are as follows:

[0056] Step 1: Regional raking: For the working condition where there are a large number of coal piles in the cabin, the target point control regional traversal algorithm is used to traverse the entire raking area and gather the coal piles to the coal pile. Specifically, it includes:

[0057] Step 1-1: By controlling the pusher to rotate left and right and measuring on site, the left and right rotation centers of the pusher are calculated and the rotation centers are used as the position points of the pusher. Figure 2As shown in the figure, a 3*1 column vector is used to represent the position of the target point in space. In different coordinate systems, the column vector representing the position is different. The target point is transformed to the vehicle coordinate system using the following formula;

[0058]

[0059] P c , P w The poses of the target points in the vehicle coordinate system and the world coordinate system are both 3*1 column vectors. The rotation transformation from the world coordinate system to the vehicle coordinate system is a 3*3 rotation matrix, P c w It is the position vector of the origin of the world coordinate system in the vehicle coordinate system.

[0060] Step 1-2: The controller constructs a rectangular harrowing area based on the received area corner points, specifically the upper left corner point and the lower right corner point of the area, and calculates whether the area size of the given area is within the set threshold range. According to the cabin size considered in the early stage, the area threshold of the area is set to 10 square meters in the present invention. When the calculated area size is less than 10 square meters, the harrowing machine sends a message that the area is too small to prompt the control room to resend the area corner points;

[0061] Step 1-3: Select the lower boundary of the rectangular pushing and raking area as the coal pile line of the regional pushing and raking, and extract the coal pile area closest to the coal pile line as the pushing and raking starting line; perform linear segmentation on the pushing and raking starting line and the coal pile line according to the measured pushing and raking machine width to obtain the coordinate set of the pushing and raking target point and the coal pile point.

[0062] Step 1-4: Traverse the position points in the above two sets in the order of raking the target point first and then the coal pile point. That is, go to the target point to rake the coal and gather the coal pile to the corresponding coal pile point. For a position point, use the transformation formula described in step 1-1 to transform the position point to the vehicle coordinate system. The vehicle is controlled to rotate left and right according to the x and y offsets of the transformed position point in the vehicle coordinate system. Specifically, the quadrant of the position point in the vehicle coordinate system is obtained according to the x and y coordinates. Taking the first quadrant as an example, the other quadrants are similar. If the position point is in the first quadrant, it means that it is in the right front of the vehicle coordinate system. The vehicle posture needs to be rotated to the right so that the position point is in the area directly in front of the vehicle's movement;

[0063] Step 1-5: Calculate the Euclidean distance between the position point and the center of the vehicle. Considering the inertia of the pusher and the slippage of the track, set the vehicle to have reached the position point when the distance is less than 20cm, and then correct the vehicle's posture. When correcting the vehicle's posture, the change in the vehicle's position is no longer considered. Only the vehicle's posture value is judged to decouple the vehicle's position and posture. Since the modified unmanned pusher is only equipped with a single motor, only one track can be set to move during rotation, and the other track remains stationary. This will result in a non-negligible position offset when the vehicle rotates; if the vehicle's position and posture are considered at the same time, the vehicle will not be able to reach the expected state for a long time, and work efficiency will be greatly reduced.

[0064] Step 1-6: Obtain the Euler angle representation of the vehicle posture and the position point posture and compare them, and use the difference between the two to control the rotation correction of the vehicle; when the difference between the two is less than the set threshold, the posture correction is determined to be successful, and the corresponding push-hake action is performed;

[0065] Step 1-7: Repeat steps 1-4 to 1-6 until the position set of the push-rake target point and the coal pile point is traversed;

[0066] Step 1-8: extract the coal pile area closest to the coal pile line as the new push rake starting line and determine whether the new push rake starting line has exceeded the range of the rectangular push rake area. If so, send the information that the area push rake is completed and enter the point-to-point push rake mode in the area (go to step 2). If not, go to step 1-3 to calculate the position set of the new push rake target point and the coal pile point, and continue to execute steps 1-4 to 1-7;

[0067] Step 2, point-to-point within the area: For the working condition where there is a little coal pile left in the cabin, the improved Dubins planning algorithm is used to gather the coal pile to the coal pile, which includes:

[0068] Step 2-1: Get the Euler angle representation of the vehicle posture and the target point posture, calculate the difference between the yaw angles, and use this difference to control the rotation of the pusher so that the target point is in the area directly in front of the vehicle. Since Dubins planning sets that the vehicle can only move in one direction, it is easy to plan a suboptimal solution when the vehicle orientation and the target point orientation are inconsistent. Figure 3 As shown, the efficiency of path planning is reduced;

[0069] Step 2-2: Use the three path segment method to plan an arc with the starting point and the end point of the vehicle as the center and the minimum rotation radius of the vehicle as r, and plan the common tangent of the two arcs. Divide the planned path into three segments: starting point T i The arc segment to tangent point 1, the arc segment or straight line segment from tangent point 1 to tangent point 2, and the tangent point 2 to the end point T gAccording to the direction of each path segment, the feasible path types are integrated into the Dubins set D = {LSL, RSR, RSL, LSR, RLR, LRL} as follows Figure 4 As shown; where L represents circular motion in the counterclockwise direction, R represents circular motion in the clockwise direction, and S represents linear motion;

[0070] Step 2-3: To facilitate the subsequent calculations, the coordinate system is transformed into the original world coordinate system with the starting point T i (x i ,y i ,α i ) points to the end point T g (x g ,y g ,β g ) is the x-axis, and the y-axis is perpendicular to it. Figure 5 As shown; after the coordinate system conversion, the coordinates of the starting point are (0,0,α i -θ), the end point is (d,0,β g -θ); where θ is the heading angle difference from the starting point to the end point;

[0071] θ=arctan((y g -y i ) / (x g -x i )) (2)

[0072] D = sqrt((x g -x i ) 2 +(y g -y i ) 2 ) (3)

[0073] d=D / R (4)

[0074] In the above formula, R is the turning radius of the vehicle, and D is the Euclidean distance from the vehicle to the target point. Dividing D by R is to regularize R so that the turning radius of the vehicle becomes 1. In this way, the arc length is equal to the radian of the angle, which is convenient for subsequent calculations. Therefore, the radius 1 is omitted before cosα seen later.

[0075] Let's take the LSL case as an example to calculate the path length of Dubins planning, such as Figure 6 shown.

[0076] According to the geometric relationship after the coordinate system transformation, the following constraint equations can be obtained:

[0077] α=α i -θ (5)

[0078] β=β g -θ (6)

[0079] p cos(α+t)-cosα+cosβ=d (7)

[0080] p sin(α+t)+cosα-cosβ=0 (8)

[0081] α+t+g=β{mod 2π} (9)

[0082] In the above formula, p is the distance between the two tangent points, and t is the number of arcs that the starting segment has turned, that is, the starting point T i The arc segment from tangent point 1 to the tangent point 1, q is the arc angle of the terminal segment, that is, the arc segment from tangent point 2 to the terminal point Tr. Solving the above equations, we can get:

[0083]

[0084]

[0085]

[0086] The total length can be calculated as:

[0087] L=t lsl +p lsl +g lsl =-α+β+p lsl (13)

[0088] The length calculations for other path types are similar.

[0089] Step 2-4: Calculate the length of the feasible path in step 2-2 using the formula described in step 2-3, and select the shortest Dubins path as the planned path;

[0090] Step 2-5: Execute the planned path in 3 sections, starting from the starting point T i The arc segment to tangent point 1, the arc segment from tangent point 1 to tangent point 2 or the straight line segment from tangent point 2 to the end point T g After reaching the target point, the vehicle posture is corrected and the required pushing and raking action is performed;

[0091] Step 2-6: Repeat steps 2-1 to 2-5 with the coal pile point as the target point to gather the coal pile into the coal pile area.

[0092] Step 2-7: Repeat steps 2-1 to 2-6 until there is no residual coal pile in the pushing and raking area. At this point, the path planning of the unmanned pushing and raking machine is completed, and the pushing and raking is terminated and the next instruction is received to start a new planning.

[0093] In order to ensure the accuracy of path planning logic, the present invention built a simulation platform based on movebase and rviz, and conducted simulation experiments. Figure 7 The light-colored line in front of the push-raker is the currently planned path, and the remaining dark-colored lines are the historical movement trajectories of the unmanned push-raker.

[0094] After verifying the accuracy of the path planning logic, we conducted a field test and the experimental data obtained are shown in Tables 1 and 2.

[0095] Table 1 is the experimental data of the first step of path planning. The regional corner points in the table are the corner points of the given push-harrow area, the target points are automatically generated according to the regional push-harrow logic, and the number of push-harrow lines represents the number of times the push-harrow starting line is obtained.

[0096] Table 2 shows the experimental data of the second step of path planning. The experimental results show that the overall push-hake error is controlled at about 0.5m.

[0097] Table 1: Experimental data for the first step of path planning

[0098]

[0099] Table 2: Experimental data of the second step of path planning

[0100]

[0101] The present invention divides the path planning into two steps according to the working condition of the pusher and rake machine. The first step is regional pusher and rake: to deal with the working condition that there are a large number of coal piles in the cabin, the target point control area traversal algorithm is used to traverse the entire pusher and rake area and gather the coal piles to the coal pile. The second step is point-to-point in the area: to deal with the working condition that there are a few coal piles remaining in the cabin, the improved Dubins planning algorithm is used to gather the coal piles to the coal pile.

[0102] It should be emphasized that the implementation cases described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific implementation plans. Any other similar implementation plans derived by those skilled in the art based on the technical solution of the present invention also fall within the scope of protection of the present invention.

Claims

1. A path planning method for an unmanned pusher and rake machine in a cabin, comprising the following steps: Step 1: Regional raking: For the working condition where there are a large number of coal piles in the cabin, the target point control regional traversal algorithm is used to traverse the entire raking area and gather the coal piles to the coal pile. Specifically, it includes: Step 1-1: Use the rotation center as the position point of the pusher; A 3*1 column vector is used to represent the position of the target point. In different coordinate systems, the column vector representing the position is different. The target point is transformed to the vehicle coordinate system using the following formula; P c , P w Respectively represent the position and pose of the target point in the vehicle coordinate system and the world coordinate system, both are 3*1 column vectors; w c R is a 3*3 rotation matrix representing the rotation transformation from the world coordinate system to the vehicle coordinate system. c w is a 3*1 column vector representing the position of the origin of the world coordinate system in the vehicle coordinate system; Step 1-2: The controller accepts the corner points of the area to construct a rectangular raking area and calculates whether the size of the area is within the set threshold range; Step 1-3: Select the lower boundary line of the rectangular pushing and raking area as the coal pile line of the regional pushing and raking, and extract the coal pile area closest to the coal pile line as the pushing and raking starting line; segment the pushing and raking starting line and the coal pile line according to the set vehicle width to obtain the position set of the pushing and raking target point and the coal pile point; Step 1-4: traverse the position points in the above two sets in the order of first pushing the target point and then the coal pile point; for a position point, use the transformation formula in step 1-1 to transform the position point to the vehicle coordinate system; control the vehicle to rotate left and right according to the x, y coordinates of the transformed position point in the vehicle coordinate system, and adjust the vehicle's posture so that the position point is directly in front of the vehicle's movement; Step 1-5: Calculate the Euclidean distance between the position point and the center of the vehicle. When the distance is less than the set value, it is considered that the vehicle has reached the position point, and then the vehicle posture is corrected; when correcting the vehicle posture, the change of the vehicle position is no longer considered, and only the vehicle posture value is judged; Step 1-5 decouples the correction of the vehicle position and the correction of the vehicle posture; the modified unmanned pusher is only equipped with a single motor, and only one track can be set to move when rotating, and the other track remains stationary; this causes the rotation center of the vehicle to not be in the exact center of the vehicle when rotating, that is, it cannot rotate in place. At this time, the rotation will cause a non-negligible change in the position of the vehicle; if the position and posture of the vehicle are considered at the same time, the vehicle will be trapped in a long correction, and the work efficiency will be reduced; Step 1-6: Obtain the Euler angle representation of the vehicle posture and the position point posture and compare them, and use the difference between the two to control the rotation correction of the vehicle; when the difference between the two is less than the set threshold, the posture correction is determined to be successful, and the corresponding push-hake action is performed; Step 1-7: Repeat steps 1-4 to 1-6 until the position set of the push-rake target point and the coal pile point is traversed; Step 1-8: extract the coal pile area closest to the coal pile line as the new push rake starting line and determine whether the new push rake starting line has exceeded the range of the rectangular push rake area. If so, send the information that the area push rake is completed and enter the point-to-point push rake mode in the area, that is, go to step 2. If not, go to step 1-3 to calculate the position set of the new push rake target point and the coal pile point, and continue to execute steps 1-4 to 1-7; Step 2, point-to-point within the area: For the working condition where there is a little coal pile left in the cabin, the improved Dubins planning algorithm is used to gather the coal pile to the coal pile, which includes: Step 2-1: Obtain the Euler angle representation of the vehicle posture and the target point posture, calculate the difference in yaw angles between the two, and use this difference to control the rotation of the pusher and rake so that the difference in yaw angle is within the set threshold; Step 2-2: Use the three path segment method to divide the planned path into three segments, starting point T i The arc segment to tangent point 1, the arc segment or straight line segment from tangent point 1 to tangent point 2, and the tangent point 2 to the end point T g According to the direction of each path segment, the feasible path types are integrated into the Dubins set D = {LSL, RSR, RSL, LSR, RLR, LRL}; where L represents the arc motion in the counterclockwise direction, R represents the arc motion in the clockwise direction, and S represents the straight line motion; Step 2-3: Transform the coordinate system and convert the original world coordinate system into the starting point T i (x i ,y i ,α i ) points to the end point T g (x g ,y g ,β g ) is the x-axis, and the y-axis is perpendicular to it. After the coordinate system is transformed, the coordinates of the starting point are (0,0,α i -θ), the end point is (d,0,β g -θ); where θ is the heading angle difference from the starting point to the end point; θ=arctan((and g -and i ) / (x g -x i )) (2) D=sqrt((x g -x i ) 2 +(y g -y i ) 2 ) (3) d=D / R (4) In the above formula, R is the turning radius of the vehicle, and D is the Euclidean distance from the vehicle to the target point. The purpose of dividing D by R is to regularize R so that the turning radius of the vehicle becomes 1. In this way, the arc length is equal to the radian of the angle, which is convenient for subsequent calculations. Therefore, the radius 1 is omitted before cosα seen later. In the case of LSL, the path length of Dubins planning is calculated; According to the geometric relationship after the coordinate system transformation, the following constraint equations can be obtained: a = a i -θ (5) β=β g -θ (6) pcos(α+t)-sinα+sinβ=d (7) p sin(α+t)+cosα-cosβ=0 (8) α+t+g=β{mod 2π} (9) In the above formula, p is the distance between the two tangent points, and t is the number of arcs that the starting segment has turned, that is, the starting point T i The arc segment to the tangent point 1, g is the number of arcs the terminal segment has turned, that is, the arc segment from the tangent point 2 to the terminal point T g Arc segment; Solving the above equations, we can get: The total length can be calculated as: L=t lsl +p lsl +g lsl =-α+β+p lsl (13) Step 2-4: Calculate the length of the feasible path in step 2-2 using the formula described in step 2-3, and select the shortest Dubins path as the planned path; Step 2-5: Execute the planned path in 3 sections, starting from the starting point T i The arc segment to tangent point 1, the arc segment from tangent point 1 to tangent point 2 or the straight line segment from tangent point 2 to the end point T g After reaching the target point, the vehicle posture is corrected and the required pushing and raking action is performed; Step 2-6: Repeat steps 2-1 to 2-5 with the coal pile point as the target point to gather the coal pile into the coal pile area; Step 2-7: Repeat steps 2-1 to 2-6 until there is no residual coal pile in the pushing and raking area. At this point, the path planning of the unmanned pushing and raking machine is completed, and the pushing and raking is terminated and the next instruction is received to start a new planning.

2. A path planning method for an unmanned pusher and rake machine in a cabin as claimed in claim 1, characterized in that: The coordinates of the rotation center of the pusher and rake are used as the real position of the pusher and rake. The target point is transformed from the world coordinate system to the vehicle coordinate system, and the movement of the pusher and rake is controlled by the quadrant where the target point is located in the vehicle coordinate system.

3. A path planning method for an unmanned pusher and rake machine in a cabin as claimed in claim 1, characterized in that: In order to solve the suboptimal solution of Dubins planning, preprocessing is added before Dubins planning. According to the Euler angle representation of the vehicle posture and the target point posture, the difference between the yaw angles is calculated, and this difference is used to control the rotation of the pusher and rake so that the target point is in the area directly in front of the vehicle, thereby avoiding the suboptimal solution of Dubins planning and improving the efficiency of path planning.

Citation Information

Patent Citations

  • Dubbins algorithm and neural network algorithm-fused flight path planning method

    CN109916405A

  • Unmanned bulldozer path planning method and system, storage medium and equipment

    CN113739802A

  • Path planning for an unmanned vehicle

    WO2019077600A1