A dynamic path planning method

Abstract

The present invention discloses a dynamic path planning method, comprising the following steps: S1. Obtaining the row number m1 and column number n1 of the arrangement position of the pallet currently to be loaded within a van container, and determining the position #imgabs0# of the pallet currently to be loaded in the local coordinate system; S2. Converting the current position #imgabs1# to the global coordinate system, and obtaining the target position #imgabs2# of the pallet currently to be loaded; S3. Real-time planning of a travel path from the standby position to the terminal position; when a laser forklift reaches the terminal position, the pallet is placed in the van container at the target position. By obtaining the position of each pallet in the global coordinate system and planning the travel path from the standby position to the corresponding pallet placement position in real time, automatic loading of the van container can be achieved using a laser forklift.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic loading, and more specifically, the present invention relates to a dynamic path planning method. Background Art

[0002] Since the beginning of the 21st century, with the rapid development of the world economy and the large-scale circulation of commodity logistics, the logistics industry has become one of the most important industries in economic construction. This has been accompanied by the prosperity of the logistics industry and the increase in labor costs. Manual loading has gradually been unable to meet market demand.

[0003] Existing automatic loading devices are mostly for side-opening containers. The truck is parked at a designated location, and fixed tracks are distributed on both sides of the truck. The cargo trolley travels along the track to the cargo placement point and then stops, lifts the cargo, and moves the cargo from the trolley to the compartment to realize automatic loading and unloading of the cargo. Since van containers generally load and unload cargo through the tailgate, the loading method can only be applied to side-opening containers and cannot be applied to van containers. Summary of the Invention

[0004] The present invention provides a dynamic path planning method, which aims to realize path implementation planning during automatic loading of box containers, thereby realizing automatic loading of box containers.

[0005] The present invention is implemented as follows: a dynamic path planning method, the method specifically comprising the following steps:

[0006] S1. Obtain the row number m1 and column number n1 of the current pallet to be loaded in the van container, and determine the position of the pallet to be loaded in the local coordinate system.

[0007] S2, the current posture Convert to the global coordinate system and obtain the target position of the pallet to be loaded in the global coordinate system

[0008] S3. Real-time planning of the driving path from the standby position to the terminal position. When the laser forklift reaches the terminal position, the pallet is placed in the van container in the target position.

[0009] Furthermore, the method specifically comprises the following steps:

[0010] The rear center point L0 of the van container is the coordinate origin (0,0,0) in the local coordinate system, and the position of the rear center point L0 of the van container in the global coordinate system is calibrated (x0, y0, θ0);

[0011] The pose in the local coordinate system Convert to global coordinate system The conversion formula is as follows:

[0012]

[0013] Furthermore, the driving path planning method is as follows:

[0014] Between the platform point and the target position (Px m1n1 ,Py m1n1 ) The distance is far, then the driving path point set is: waiting position point -> platform point -> van container entrance center point -> van container middle driving turning point -> van container delivery point forward adjustment point -> end point;

[0015] The platform point P1 is the point that the laser forklift must pass before loading, and its global position is (Px1, Py1, Pθ1); the local coordinate position of the center point P2 of the container entrance Corresponding to the pose (Px2, Py2, Pθ2) in the global coordinate system; the local coordinates of the turning point P3 in the middle of the van container are The corresponding pose in the global coordinate system is (Px3, Py3, Pθ3); the local coordinates of the forward adjustment point P4 of the box container loading point are The corresponding pose in the global coordinate system is (Px4, Py4, Pθ4); the terminal position P5 in the box container, when the center of the laser forklift is at the terminal position, the pose of the center of the pallet in the global coordinate system is (Px m1n1 ,Py m1n1 ,Pθ m1n1 ).

[0016] Furthermore, the driving path planning method is as follows:

[0017] Between the platform point and the target position (Px m1n1 ,Py m1n1 ) The distance is relatively close, and the driving path point set is: waiting position point -> platform point -> forward sight adjustment point of the box container loading point -> terminal position.

[0018] Furthermore, when the row number m1 of the current arrangement position of the pallet to be loaded in the van container is ≥ m-3, it is determined that the platform point is close to the target position; otherwise, it is determined that the platform point is far from the target position.

[0019] Furthermore, the path segments between adjacent path points in the driving path point set are segmented based on the grid resolution, and the adjacent path points P i 、P i+1 The path between them is divided, and the path points formed after the division are (P′x i ,P′y i ) indicates that its calculation formula is as follows:

[0020]

[0021] The initial value j = 0, each cycle, set j = j + 1, until j * t > dist (P i ,P i+1 ) The cycle ends and the adjacent path point P is completed i 、P i+1 The path between the two is divided, and the points after division (P′x i ,P′y i ) insert the path points P of the driving path point set in sequence i 、P i+1 between.

[0022] Furthermore, the path points of the driving path point set are smoothed.

[0023] Furthermore, before step S1, the method further includes:

[0024] Determine the position and orientation of all pallets in the local coordinate system.

[0025] Furthermore, the pallet coordinate P(Lx ij ,Ly ij ,Lθ ij )for:

[0026]

[0027] Among them, d1 is the set safe column spacing, d2 is the set safe row spacing, W and L are the width and length of the box respectively, and w and l are the width and length of the pallet respectively.

[0028] The present invention obtains the position of each pallet in the global coordinate system and plans the driving path from the standby position to the corresponding pallet placement position in real time, thereby realizing automatic loading of the van container by a laser forklift. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 A flow chart of a dynamic path planning method provided by an embodiment of the present invention;

[0030] Figure 2 A schematic diagram of a local coordinate system provided by an embodiment of the present invention;

[0031] Figure 3 A schematic diagram of path planning when the platform point is far away from the target location provided by an embodiment of the present invention;

[0032] Figure 4 A schematic diagram of path planning when the platform point is close to the target location provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0033] The specific implementation methods of the present invention will be further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0034] Figure 1 A flow chart of a dynamic path planning method provided by an embodiment of the present invention, which specifically includes the following steps:

[0035] S1. Determine the position and orientation of all pallets in the local coordinate system;

[0036] In the embodiment of the present invention, the local coordinate system refers to the van container coordinate system, with the rear midpoint L0 of the van container as the origin, the width direction as the x-axis, and the length direction as the y-axis, such as Figure 2 shown.

[0037] In the embodiment of the present invention, the total number of pallets accommodated in the van container is m*n, i.e., m rows and n columns. The positions of the centers of the m*n single-sided inserted pallets in the local coordinate system are calculated. In the local coordinate system, the coordinates P(Lx ij ,Ly ij ,Lθ ij )for:

[0038]

[0039] Among them, d1 is the set safe column spacing, d2 is the set safe row spacing, W and L are the width and length of the box respectively, and w and l are the width and length of the pallet respectively.

[0040] S2. Obtain the row number m1 and column number n1 of the current arrangement position of the pallet to be loaded in the van container, and calculate the position of the pallet to be loaded in the local coordinate system based on formula (1)

[0041] S3, based on the position of the origin L0 of the local coordinate system in the global coordinate system, the current position Convert to the global coordinate system and obtain the target position of the pallet to be loaded in the global coordinate system

[0042] The arrangement principle of pallets in the box container is: when the first row is full, the second row is arranged, and so on, until all the pallets are arranged. The global coordinate system is the world coordinate system. The pallet is loaded with goods. After the laser forklift forks the pallet, the driving path of the laser forklift in the local coordinate system is planned in real time, and the driving path is converted to the global coordinate system. The laser forklift navigates and positions itself in the global coordinate system, and places the inserted pallet in the box container in the target position to complete the loading of the goods.

[0043] Scan the van container to be loaded, output the width W, length L and rear center point L0 of the van container, the rear center point L0 of the van container is the origin (0,0,0) in the local coordinate system, calibrate the coordinates of the rear center point L0 of the van container in the global coordinate system to (x0,y0,θ0), then based on formula (2) the pose in the local coordinate system is Convert to global coordinate system

[0044]

[0045] S4. The driving path from the standby position to the terminal position is planned in real time. When the laser forklift reaches the terminal position, the pallet is placed in the van container in the target position.

[0046] If m1<m-3, that is, the distance between the platform point and the target position is far, the target posture (Px m1n1 ,Py m1n1 ,Pθ m1n1 ) is determined by the target position (Px m1n1 ,Py m1n1 ) and target posture Pθ m1n1 The driving path point set is: waiting position point -> platform point -> van container entrance center point -> van container middle driving turning point -> van container delivery point forward adjustment point -> terminal position, such as Figure 2 As shown;

[0047] First, based on a fixed driving path, it drives from the standby position to the platform point P1, which is the point that the laser forklift must pass before loading. The position of the platform point is (Px1, Py1, Pθ1);

[0048] P2 is the center point of the container entrance. The calculation method is to first calculate the local coordinate position corresponding to this point. Then substitute into formula (2) to calculate the pose (Px2, Py2, Pθ2) of P2 in the global coordinate system;

[0049] P3 is the turning point in the middle of the van container. If the distance between the longitudinal coordinate of the position of this point and the longitudinal coordinate of the loading point is l1, then the local coordinate of this point is Then substitute into formula (2) to calculate the pose (Px3, Py3, Pθ3) of P3 in the global coordinate system;

[0050] P4 is the forward-looking adjustment point of the container loading point. If the distance between the longitudinal coordinate of the point and the longitudinal coordinate of the loading point is l2, then the local coordinate of the point is l2>l1, then substitute into formula (2) to calculate the pose (Px4, Py4, Pθ4) of P4 in the global coordinate system;

[0051] P5 is the end position in the van container. When the center of the laser forklift is at the end position, the position of the pallet center in the global coordinate system is (Px m1n1 ,Py m1n1 ,Pθ m1n1 ).

[0052] The corresponding driving path point set is: P0->P1->P2->P3->P4->P5, where P0 is the position of the laser forklift at the standby position. Figure 3 As shown;

[0053] If m1≥m-3, the platform is close to the target location, so the driving path needs to be shortened. At this time, the temporary path point set is: waiting position point -> platform point -> forward adjustment point of the container loading point -> terminal position, such as Figure 3 As stated;

[0054] First, based on a fixed driving path, it drives from the standby position to the platform point, which is the point that the laser forklift must pass before loading. P1 is the global position of the platform point (Px1, Py1, Pθ1);

[0055] P4 is the forward-looking adjustment point of the container loading point. If the distance between the longitudinal coordinate of the point and the longitudinal coordinate of the loading point is l2, then the local coordinate of the point is Then substitute into formula (1) to calculate the pose (Px4, Py4, Pθ4) of P4 in the global coordinate system;

[0056] P5 is the end position in the van container. When the center of the laser forklift is at the end position, the position of the pallet center in the global coordinate system is (Px m1n1 ,Py m1n1 ,Pθ m1n1 ).

[0057] The corresponding driving path point set is: P0->P1->P4->P5, P0 is the position of the laser forklift at the standby position, such as Figure 4 As shown;

[0058] In the embodiment of the present invention, the paths between adjacent path points in the driving path point set are straight path paths, and the paths are segmented. Assuming that the grid resolution in the map is t, the straight path is segmented according to the grid resolution. The processing method is as follows:

[0059] Calculate two adjacent path points P i ,P i+1 The Euclidean distance between i ,P i+1 ) and angle θ i,i+1 :

[0060]

[0061]

[0062] dist(P i ,P i+1 ) represents the adjacent path point P i 、P i+1 The Euclidean distance between (Px i ,Py i ) represents the path point P i The global position coordinates, (Px i+1 ,Py i+1 ) represents the path point P i+1 The global position coordinates of .

[0063] For adjacent path points P i 、P i+1 The paths between are divided, and the path points formed by the division are (P′x i ,P′y i ) indicates that its calculation formula is as follows:

[0064]

[0065] The initial value j = 0, each cycle, set j = j + 1, until j * t > dist (P i ,P i+1 ) The cycle ends and the adjacent path point P is completed i 、P i+1 The path between the two is divided, and the points after division (P′x i ,P′y i ) insert the path points P of the driving path point set in sequence i 、P i+1 Based on the above method, the paths between all adjacent paths in the driving path point set are divided in sequence.

[0066] In the embodiment of the present invention, in order to make the path of the laser forklift smoother, the path and the driving path point set formed after segmentation are smoothed, and the path points in each driving path point set are smoothed using the Bezier function. The present invention adopts a second-order Bezier function model, let k = 1, and extract the path point P from the driving path point set. k 、P k+1 、P k+2 , waypoint P k 、P k+1 、P k+2 The function B(t) tracks:

[0067] B(t)=(1-t) 2 P k +2t(1-t)P k+1 +t 2 P k+2 t∈[0,1]

[0068] After the parameter t changes continuously in the range of [0,1], a second-order Bezier curve will be obtained. The obtained curve is set as the tracking line of the laser forklift to avoid the vehicle having a turning radius that is too large. Let k = k + 1 and repeat the above steps until all the path points in the driving path point set are traversed.

[0069] The present invention has been described exemplarily. Obviously, the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the scope of protection of the present invention.

Claims

1. A dynamic path planning method, characterized in that: The method specifically comprises the following steps: S1. Obtain the row number m1 and column number n1 of the current arrangement position of the pallet to be loaded in the van container, and determine the position of the pallet to be loaded in the local coordinate system. S2, the current posture Convert to the global coordinate system and obtain the target position of the pallet to be loaded in the global coordinate system S3: Real-time planning of the driving path from the standby position to the final position. When the laser forklift reaches the final position, the pallet is placed in the van container in the target position. The specific method for planning the driving path is as follows: Between the platform point and the target position (Px m1n1 ,Py m1n1 ) The distance is far, then the driving path point set is: waiting position point -> platform point -> van container entrance center point -> van container middle driving turning point -> van container delivery point forward adjustment point -> end point; The platform point P1 is the point that the laser forklift must pass before loading, and its global position is (Px1, Py1, Pθ1); the local coordinate position of the center point P2 of the container entrance Corresponding to the pose (Px2, Py2, Pθ2) in the global coordinate system; the local coordinates of the turning point P3 in the middle of the van container are The corresponding pose in the global coordinate system is (Px3, Py3, Pθ3); the local coordinates of the forward adjustment point P4 of the box container loading point are The corresponding pose in the global coordinate system is (Px4, Py4, Pθ4); the terminal position P5 in the box container, when the center of the laser forklift is at the terminal position, the pose of the center of the pallet in the global coordinate system is (Px m1n1 ,Py m1n1 ,Pθ m1n1 ).

2. The dynamic path planning method according to claim 1, wherein: The method specifically comprises the following steps: The rear center point L0 of the van container is the coordinate origin (0,0,0) in the local coordinate system, and the position of the rear center point L0 of the van container in the global coordinate system is calibrated (x0, y0, θ0); The pose in the local coordinate system Convert to global coordinate system The conversion formula is as follows:

3. The dynamic path planning method according to claim 1, wherein: The specific method for planning the driving path is as follows: Between the platform point and the target position (Px m1n1 ,Py m1n1 ) The distance is relatively close, and the driving path point set is: waiting position point -> platform point -> forward sight adjustment point of the box container loading point -> terminal position.

4. The dynamic path planning method according to claim 1 or 3, characterized in that: When the row number m1 of the current pallet to be loaded in the van container is greater than or equal to m-3, it is determined that the platform point is closer to the target position; otherwise, it is determined that the platform point is farther from the target position.

5. The dynamic path planning method according to claim 1 or 3, characterized in that: The path segments between adjacent path points in the driving path point set are segmented based on the grid resolution. i 、P i+1 The path between them is divided, and the path points formed after the division are (P′x i ,P′y i ) indicates that its calculation formula is as follows: Where t is the grid resolution in the map, the initial value j = 0, each cycle, set j = j + 1 until j * t > dist (P i ,P i+1 ) The cycle ends and the adjacent path point P is completed i 、P i+1 The path between the two is divided, and the points after division (P′x i ,P'y i ) insert the path points P of the driving path point set in sequence i 、P i+1 between.

6. The dynamic path planning method according to claim 5, wherein: Smoothing is performed on the path points of the travel path point set.

7. The dynamic path planning method according to claim 1, wherein: Before step S1, the method further includes: Determine the position and orientation of all pallets in the local coordinate system.

8. The dynamic path planning method according to claim 7, wherein: The coordinates of the pallet at row i and column j in the local coordinate system are P(Lx ij ,Ly ij ,Lθ ij )for: Among them, d1 is the set safe column spacing, d2 is the set safe row spacing, W and L are the width and length of the box respectively, and w and l are the width and length of the pallet respectively.

Images