Laser forklift positioning method and laser forklift

Abstract

This invention discloses a laser forklift positioning method for automated loading of vans, comprising the following steps: S1, within a warehouse, scanning reflective pillars with a laser radar to obtain the position and pose of the laser forklift on a map; S2, when the forklift travels to a predetermined distance from the rear of the van, stopping the laser forklift, and scanning the van with a laser radar to obtain the van's position and pose on a map. c_m S3. When the laser forklift enters the truck bed, the LiDAR scanner scans the outline of the truck bed to obtain the truck's pose in the radar coordinate system. c_r The system then calculates the pose of the laser-guided forklift on the map to control its movement to the target location on the truck bed. The entire process utilizes LiDAR to solve the positioning problem during automated truck loading. The system is simple, low-cost, requires minimal environmental modifications, and offers high positioning accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of robot technology, and more specifically, the present invention relates to a laser forklift positioning method and a laser forklift. Background Art

[0002] With the development of the market and technology, in order to reduce production costs and improve production efficiency, the application of automated warehousing and logistics systems is becoming more and more widespread. Among them, the automated outbound loading system is an important component module of the automated warehousing and logistics system. Most of the existing automated outbound loading solutions have complex structures, require multiple transfers, and even require the installation of large-scale electromechanical equipment. The loading modules at the end of the system can be roughly divided into conveyor belt type, truss type and mechanical arm type. These loading methods require large site modifications and high costs, and have great limitations in application. The flexible loading system based on mobile AGV can perfectly avoid the above shortcomings and can directly transfer goods from the warehouse to the logistics truck, solving the troubles caused by multiple transfers and the cost and installation problems caused by large electromechanical equipment. The flexible loading system based on mobile AGV has high requirements for positioning accuracy.

[0003] The existing AGV flexible loading system uses laser reflective columns for positioning in the warehouse, and the loading and unloading of goods in the truck compartment is positioned by a SLAM algorithm based on straight line features. This technical solution has the following two disadvantages: using line feature SLAM positioning in the truck, the line features may not be extracted due to real-time dynamic changes in the on-site environment, and the real-time SLAM positioning of line features requires large calculations and has low accuracy. Summary of the invention

[0004] The present invention provides a laser forklift positioning method, which only relies on the on-board laser radar to locate the position and posture of the mobile robot during the entire process of transporting from a cargo warehouse to the interior of a truck, and has high positioning accuracy.

[0005] The present invention is implemented as follows: a laser forklift positioning method for automatic loading of a van, the method specifically comprising the following steps:

[0006] S1. In the warehouse, the laser radar is used to scan the reflective column to obtain the position of the laser forklift in the map;

[0007] S2: When the forklift reaches the set distance from the rear of the truck, it stops and scans the truck with the laser radar to obtain the truck's pose in the map. c_m ;

[0008] S3: When the laser forklift enters the truck compartment, the laser radar scans the compartment contour to obtain the truck's position pose in the radar coordinate system. c_r , and then calculate the position of the laser forklift in the map to control the laser forklift to the target position in the truck compartment.

[0009] Furthermore, the truck's position in the map c_m The measurement method is as follows:

[0010] 21) The laser radar scans the reflective columns in the environment and determines the pose of the laser radar in the map r_m ;

[0011] 22) The radar scans the environment, extracts the outline of the truck, and obtains the pose of the truck in the radar coordinate system c_r ;

[0012] 23) Determine whether the number of scans reaches the number threshold, if the detection result is no, return to step 21), if the detection result is yes, execute step 24);

[0013] 24) Calculate pose r_m , pose c_r The mean of the pose posture

[0014] 25) Based on pose posture Calculate the truck's pose in the map c_m .

[0015] Furthermore, when the laser forklift enters the truck compartment, the laser forklift is in the map pose r_m The specific method for obtaining the pose in is as follows:

[0016]

[0017] Among them, pose c_r is the position of the truck in the radar coordinate system, pose r_m is the position of the lidar in the map.

[0018] Furthermore, the method for extracting the truck outline is specifically as follows:

[0019] 311) The radar scans the surrounding environment and obtains point cloud data point_cloud;

[0020] 312) Select each point sequentially and calculate the current point p i The distance from all fitted straight lines, if point p i If the distance from the known straight line equation is less than the set distance, then point p is identified i belongs to the known line, otherwise point p i Select the next point as the point to be fitted for other lines;

[0021] 313) For the point p to be fittedi , based on point p i And five points before and after it to calculate the curvature C i , if the curvature C i is less than the curvature threshold, then point p i The five points before and after it are fitted into a straight line;

[0022] 314) Using the RANSAC algorithm to extract all points belonging to the straight line in the point cloud data point_cloud, and fitting them again into the final straight line equation;

[0023] 315) After all the straight lines are extracted, all the straight lines are compared in pairs to determine whether two straight lines l 1 , l 2 Are they parallel? If the straight line l 1 , l 2 If they are parallel, then calculate two parallel lines l 1 , l 2 If the distance d between them is equal to the width of the carriage, then the parallel line l 1 , l 2 For the side of the freight car compartment;

[0024] 316) Search for a line l that is perpendicular to these two parallel lines from the remaining lines. 3 , if the straight line l 3 The intercept of is less than 0, then the straight line l 3 The front of the truck compartment.

[0025] Furthermore, the pose of the truck in the radar coordinate system c_r The specific method of obtaining it is as follows:

[0026] Calculate the straight line l 3 With two parallel lines l 1 , l 2 The intersection point P 1 , P 2 Coordinates, and calculate the midpoint P of the two intersection points, and establish the truck coordinate system with the midpoint P as the origin;

[0027] Reuse straight line l 3 Calculate the attitude of the truck in the radar coordinate system, straight line l 3 The equation is as follows:

[0028] a 3 x+b 3 y+c 3 =0

[0029] Let θ = atan(b 3 / a 3), when θ is greater than zero, let θ = -θ, and get the pose (x, y, θ) of the truck in the radar coordinate system, that is, the pose c_r .

[0030] The present invention is implemented as follows: a laser forklift is used for automatic loading of goods in a van, the laser forklift is integrated with a laser radar, and the laser radar is communicatively connected with a positioning unit;

[0031] When the laser forklift receives a loading instruction, it automatically picks up the goods from the warehouse and delivers the goods to the target location in the carriage. During the process of picking up and delivering goods, the laser radar scans the surrounding environment in real time, and the positioning unit performs positioning based on the above-mentioned laser forklift positioning method;

[0032] Among them, reflective panels are arranged in the warehouse, and a parking area is set up at the boundary of the warehouse. When the truck to be loaded parks in the parking area, the doors are opened and locked to both sides of the truck compartment.

[0033] The present invention uses laser radar throughout the entire process to solve the positioning problem in the process of automated loading of trucks. The system is simple and low-cost, requires less environmental modification, and has high positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 A schematic diagram of a truck coordinate system and a radar coordinate system provided by an embodiment of the present invention;

[0035] Figure 2 A schematic diagram of truck parking provided by an embodiment of the present invention;

[0036] Figure 3 A schematic diagram of a laser forklift positioning solution provided by an embodiment of the present invention;

[0037] Figure 4 This is a flow chart of the laser forklift positioning method provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The specific implementation modes of the present invention are 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.

[0039] The truck in the present invention must be a box truck, which is fully enclosed and has a known length and width. The origin of the truck body coordinate system is at the midpoint of the front of the truck, the y coordinate axis points to the positive direction of the truck head, and the x-axis direction of the radar coordinate system is consistent with the front direction of the forklift. The forklift enters the truck in reverse mode, as shown in the attached figure. Figure 1 ) as shown; after the truck stops, the distance between it and the warehouse loading platform shall not exceed 10cm, and the angle shall not exceed 5°. Figure 2), after the truck stops, open the truck door, lock the truck door to the side of the truck compartment, and notify the laser forklift to pick up the goods from the warehouse.

[0040] Figure 1 The flowchart of the laser forklift positioning method provided by the embodiment of the present invention is shown in FIG. Figure 3 As shown, the method is used for automatic loading of cargo into a box truck, and specifically comprises the following steps:

[0041] S1. In the warehouse, the laser radar is used to scan the reflective column to obtain the position of the laser forklift in the map;

[0042] S2. When the laser forklift reaches a set distance from the rear of the truck, it stops and scans the truck with the laser radar to obtain the position of the truck in the map;

[0043] S3. When the laser forklift enters the truck compartment, the laser radar is used to scan the compartment contour to obtain the position of the laser forklift in the map, and the laser forklift is controlled to drive to the target position in the truck compartment.

[0044] In an embodiment of the present invention, the laser forklift positioning method based on the reflective plate specifically includes the following steps:

[0045] 11) Traverse the current scanning frame of the laser radar, extract the reflection points of the reflective column in the point cloud according to the relationship between intensity and distance, cluster the reflection points of the reflective column according to the distance relationship, and fit the center point of the reflective column:

[0046]

[0047]

[0048] Among them, θ and d are the scanning angle and distance of the center point of the reflective column in the scanning frame, θ i is the scanning angle of scanning point i in the scanning frame, d i is the distance of scanning point i in the scanning frame, r is the radius of the reflective column;

[0049] x=d*cos(θ)

[0050] y=d*sin(θ)

[0051] Where (x, y) is the coordinate of the center of the reflective column in the scanning frame. Based on the above method, all points are traversed to obtain the reflective column set ref_points;

[0052] 12) Calculate the distance between each pair of reflective columns in the reflective column set ref_points, match them with the map points in the map, and find the map point set map_points with the smallest matching error with the reflector;

[0053]

[0054] Among them, (x i ,y i ) is the coordinate of the center of the reflector i in the radar frame, (x j ,y j ) is the coordinate of the center of the reflector j in the radar frame, (X i ,Y i ) is the coordinate of the map point i in the map, (X j ,Y j ) are the coordinates of map point j in the map, where i≠j. In addition, the map point in the map is a reflector whose position in the map is known.

[0055] 13) Given the reflective column set ref_pointss in the current scan frame and the matching map point set map_points, the position and posture of the laser radar, that is, the position and posture of the laser forklift, are calculated by the following formula:

[0056] X m =RX r +T

[0057] Among them, X m is the coordinate of the map point on the map, X r is the coordinate of the center of the reflective column in the radar frame, R and T are the rotation matrix and translation vector. Solving the above equations, we can calculate the position and posture of the laser radar. Assume that:

[0058]

[0059]

[0060] Then the position of the laser radar in the map is:

[0061] In the embodiment of the present invention, after the laser forklift picks up the goods, it drives to a position 1 meter away from the middle of the tailgate of the truck and stops to measure the truck posture, that is, to measure the position of the truck in the map. The measurement steps are as follows:

[0062] 21) The laser radar scans the reflective columns in the environment and uses steps 11) to 13) to determine the pose of the laser radar in the map r_m ;

[0063] 22) The radar scans the environment and uses steps 311) to 316) to extract the outline of the truck to obtain the pose of the truck in the radar coordinate system c_r ;

[0064] 23) Determine whether the scan has been completed 50 times. If it has not reached 50 times, return to step 21) to continue scanning. If it has reached 50 times, execute step 24);

[0065] 24) Calculate pose r_m And pose c_r The mean of posture Indicates pose r_m , pose c_r The mean of is (x, y, θ), the calculation formula is as follows:

[0066]

[0067] Among them, (x i ,y i ,θ i ) is the pose of the laser radar in the map during the i-th scan r_m .

[0068] 25) Based on pose posture Calculate the exact pose of the truck in the map c_m , and its calculation formula is as follows:

[0069]

[0070] In the embodiment of the present invention, when entering a truck compartment, the method for obtaining the position of the laser forklift in the map is as follows:

[0071] 31) The laser radar scans the truck, extracts the truck outline, establishes the truck coordinate system, and calculates the truck's pose in the radar coordinate system. c_r ;

[0072] 32) The truck’s pose in the map is known c_m , then the position of the radar in the map is pose r_m :

[0073]

[0074] In the embodiment of the present invention, the method for extracting the truck outline is specifically as follows:

[0075] 311) The radar scans the surrounding environment and obtains point cloud data point_cloud;

[0076] 312) Select each point sequentially and calculate the current point p i The distance from all fitted straight lines. If the straight line equation Ax+By+C=0, then point pi (x i ,y i ) to the straight-line distance is calculated as follows:

[0077]

[0078] If point p i If the distance from the known straight line equation is less than 0.02 meters, then point p is identified. i belongs to the known line, otherwise point p i Select the next point as the point to be fitted for other lines;

[0079] 313) For the point p to be fitted i , based on point p i Calculate the curvature C at the five points before and after i , curvature C i The calculation formula is as follows:

[0080]

[0081] If the curvature C i If it is less than 0.02, p i These eleven points at the center belong to the plane points;

[0082] 314) to point p i And the five points before and after it fit a straight line to form a new straight line. Assume that the straight line equation is in the form of:

[0083] y=f(x)=ax+b

[0084] According to the least squares method, the error equation can be listed as follows:

[0085]

[0086] After linearization, we can get:

[0087]

[0088]

[0089] Solve the above equations to get the straight line equation y=ax+b, then use the RANSAC algorithm to extract all the points belonging to the straight line in the point cloud data point_cloud, and fit them again to get the final straight line equation;

[0090] 315) After all straight lines are extracted based on steps 311 to 314), all straight lines are compared in pairs to determine whether two straight lines are parallel. 1 , l 2 The equation is

[0091]

[0092] When a 1 b 2 -a 2 b 1 = 0, the two straight lines l 1 , l 2 Parallel, straight line equations can be transformed into:

[0093] ax+by+c 1 =0

[0094] ax+by+c 2 =0

[0095] Calculate two parallel lines l 1 , l 2 Distance between:

[0096]

[0097] If d is equal to the width of the carriage, then the two parallel lines l 1 , l 2 For the side of the freight car;

[0098] 316) Search the remaining lines for a line perpendicular to the two parallel lines.

[0099] Known straight line l 3 The equation is:

[0100] a 3 x+b 3 y+c 3 =0

[0101] When a 1 a 3 -b 1 b 3 = 0, indicating that the straight line l 3 Parallel to the line l 1 , l 2 Vertical, if the line l 3 The intercept of is less than 0, then the straight line l 3 The front of the truck compartment.

[0102] In the embodiment of the present invention, the pose of the truck in the radar coordinate system c_r The specific acquisition method is as follows:

[0103] 317) Calculate the straight line l 3 With two parallel lines l 1 , l 2 The intersection point P 1 , P 2The intersection of the straight lines can be obtained by solving the linear equations, and the midpoint of the two intersections can be calculated to obtain the coordinate value P (x, y) of the truck. The truck coordinate system is established with point P as the origin, the width direction as the x-axis, and the front direction as the y-axis.

[0104]

[0105] Reuse straight line l 3 Calculate the attitude of the truck in the radar coordinate system, straight line l 3 The equation is as follows:

[0106] a 3 x+b 3 y+c 3 =0

[0107] Let θ = atan(b 3 / a 3 ), when θ is greater than zero, let θ = -θ, and get the pose (x, y, θ) of the truck in the radar coordinate system, where (x, y) is the coordinate of the midpoint P, i.e., pose c_r , then the position of the radar in the truck coordinate system is

[0108] The embodiment of the present application also provides a laser forklift for automatically loading goods on a van. The laser forklift is integrated with a laser radar, which is communicatively connected with a positioning unit.

[0109] When the laser forklift receives a loading instruction, it automatically picks up the goods from the warehouse and delivers the goods to the target location in the carriage. During the process of picking up and delivering goods, the laser radar scans the surrounding environment in real time, and the positioning unit performs positioning based on the above-mentioned laser forklift positioning method;

[0110] Among them, reflective panels are arranged in the warehouse, and a parking area is set up at the boundary of the warehouse. When the truck to be loaded parks in the parking area, the doors are opened and locked to both sides of the truck compartment.

[0111] The present invention is described above by way of example in conjunction with the accompanying drawings. It is obvious that 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 protection scope of the present invention.

Claims

1. A laser forklift positioning method for automatic loading of vans, characterized in that: The method specifically comprises the following steps: S1. In the warehouse, the laser radar is used to scan the reflective column to obtain the position of the laser forklift in the map; S2: When the forklift reaches the set distance from the rear of the truck, it stops and scans the truck with the laser radar to obtain the truck's pose in the map. c_m ; S3: When the laser forklift enters the truck compartment, the laser radar scans the compartment contour to obtain the truck's position pose in the radar coordinate system. c_r , and then calculate the position of the laser forklift in the map to control the laser forklift to drive to the target position of the truck compartment; The truck's pose in the map c_m The measurement method is as follows: 21) The laser radar scans the reflective columns in the environment to determine the pose of the laser forklift in the map r_m ; 22) The radar scans the environment, extracts the outline of the truck, and obtains the pose of the truck in the radar coordinate system c_r ; 23) Determine whether the number of scans reaches the number threshold, if the detection result is no, return to step 21), if the detection result is yes, execute step 24); 24) Calculate pose r_m , pose c_r The mean of the pose posture 25) Based on pose posture Calculate the truck's pose in the map c_m ; When the laser forklift enters the truck compartment, the laser forklift's pose in the map r_m The specific acquisition method is as follows: Among them, pose c_r is the position of the truck in the radar coordinate system, pose c_m is the position of the truck in the map; The method for extracting the truck outline is specifically as follows: 311) The radar scans the surrounding environment and obtains point cloud data point_cloud; 312) Select each point sequentially and calculate the current point p i The distance from all fitted straight lines, if point p i If the distance from the known straight line equation is less than the set distance, then point p is identified i belongs to the known line, otherwise point p i Select the next point as the point to be fitted for other lines; 313) For the point p to be fitted i , based on point p i And five points before and after it to calculate the curvature C i , if the curvature C i is less than the curvature threshold, then point p i The five points before and after it are fitted into a straight line; 314) Using the RANSAC algorithm to extract all points belonging to the straight line in the point cloud data point_cloud, and fitting them again into the final straight line equation; 315) After all the straight lines are extracted, all the straight lines are compared in pairs to determine whether two straight lines l1 and l2 are parallel. If the straight lines l1 and l2 are parallel, the distance d between the two parallel straight lines l1 and l2 is calculated. If d is equal to the width of the carriage, the parallel lines l1 and l2 are the sides of the truck carriage. 316) Search for a straight line l3 perpendicular to the two parallel lines from the remaining straight lines. If the intercept of the straight line l3 is less than 0, the straight line l3 is the front of the truck compartment; The pose of the truck in the radar coordinate system c_r The specific acquisition method is as follows: Calculate the coordinates of the intersection points P1 and P2 of the straight line l3 and the two parallel straight lines l1 and l2 respectively, and calculate the midpoint P(x, y) of the two intersection points; Then use the straight line l3 to calculate the posture of the truck in the radar coordinate system. The equation of the straight line l3 is as follows: a3x+b3y+c3=0 Let θ = atan(b3 / a3). When θ is greater than zero, let θ = -θ. Then we can get the pose (x, y, θ) of the truck in the radar coordinate system. c_r .

2. A laser forklift, used for automatic loading of goods into a van, characterized in that: The laser forklift is integrated with a laser radar, which is communicatively connected with the positioning unit; When the laser forklift receives a loading instruction, it automatically picks up the goods from the warehouse and delivers the goods to the target location in the carriage. During the process of picking up and delivering goods, the laser radar scans the surrounding environment in real time, and the positioning unit performs positioning based on the laser forklift positioning method described in claim 1; Among them, reflective panels are arranged in the warehouse, and a parking area is set up at the boundary of the warehouse. When the truck to be loaded parks in the parking area, the doors are opened and locked to both sides of the truck compartment.

Images