Robot localization method in truck compartment based on dynamic grid map

Abstract

The present invention discloses a method for positioning a robot in a truck compartment based on a dynamic grid map, comprising the following steps: establishing a laser SLAM grid map and calibrating the compartment mapping posture; obtaining a current laser frame containing the truck outline at the compartment mapping posture, obtaining the truck outline in the grid map, and simultaneously updating the occupancy probability value and two-dimensional normal distribution parameters of the occupied grid; determining the robot posture at the previous moment, projecting the current point cloud onto the grid map with the candidate posture at the next moment for scoring, and the posture corresponding to the highest score is the coarse positioning posture of the robot; constructing the occupancy probability value and the objective function of the two-dimensional normal distribution under the coarse positioning posture, and obtaining the precise positioning posture of the robot through nonlinear optimization. In the case where the truck is not parked in a fixed position and there are no positioning markers, the present invention significantly improves the problem of low positioning accuracy of the robot in the truck compartment, and the method does not rely on markers and does not require the extraction of truck compartment straight lines.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and more specifically, the present invention relates to a robot positioning method in a truck compartment based on a dynamic grid map. Background Art

[0002] In industrial unmanned warehousing and logistics, robots automatically load and unload goods inside trucks, making positioning and navigation technology for robots within truck compartments increasingly crucial. Key technologies currently used include magnetic navigation, visual QR code navigation, visual SLAM navigation, laser reflector positioning, and laser SLAM positioning. Magnetic navigation uses a fixed trajectory, requiring electromagnetic wires or magnetic strips to be laid along the path. Visual QR code navigation often uses markings such as QR codes or special graphic codes indoors, requiring regular maintenance. Visual SLAM navigation is significantly affected by ambient lighting and has low reliability. LiDAR navigation offers precise laser positioning, flexible and adaptable paths, and adaptability to a variety of field environments. With breakthroughs and the maturity of LiDAR technology, costs have decreased, and it has now become the mainstream positioning and navigation method for mobile robots.

[0003] Laser navigation uses two methods, depending on the presence or absence of landmarks: one is laser navigation with landmarks, such as reflector positioning navigation. Reflector positioning navigation requires the placement of laser reflectors in the environment. The other is laser navigation without landmarks, which is the laser SLAM positioning navigation currently used on mainstream AGVs. With existing laser SLAM navigation, a map is pre-created using a SLAM mapping algorithm before the robot enters the truck compartment for positioning. However, each truck parked during loading and unloading is located in a different position. As a result, after the robot enters the truck, the current laser frame and the previous map (i.e., the outline of the previous truck in the map) do not match. This results in significant deviations in the robot's positioning within the truck.

[0004] CN114265374A, patent title: A system and method for AGV accessing cargo in a van truck. This solution locates the truck's outline by first extracting the navigation wall data of the truck during the mapping process and storing it as map information. Then, after the robot enters the truck's compartment, it uses the real-time extracted navigation wall data to perform matching positioning. However, this method has the following problems:

[0005] This solution extracts the straight lines, start and end coordinates, from the left and right sides of the truck compartment for matching and positioning. However, the interior walls of truck compartments are wrinkled and uneven, and the compartment point cloud is composed of thick line segments, resulting in low accuracy in the extracted straight lines. Furthermore, for longer trucks, the point cloud is sparse in the distance, resulting in a low success rate for extracting fitted straight lines. Consequently, this method results in a low success rate and accuracy for AGV positioning. Summary of the Invention

[0006] The present invention provides a robot positioning method in a truck compartment based on a dynamic grid map, aiming to improve the above-mentioned problem.

[0007] The present invention is implemented as follows: a method for positioning a robot in a truck compartment based on a dynamic grid map, the method specifically comprising the following steps:

[0008] S1. Build a laser SLAM grid map based on the laser radar on the robot and calibrate the car body’s mapping pose Pose_f.

[0009] S2. Obtain the current laser frame Scan containing the truck outline at the vehicle compartment mapping pose Pose_f, obtain the truck outline in the grid map, and update the occupancy probability value and two-dimensional normal distribution parameters of the occupied grid;

[0010] S3: Determine the robot's pose at the previous moment, project the current point cloud onto the grid map based on the candidate pose at the next moment, and score it. The pose corresponding to the highest score is the coarse positioning pose of the robot.

[0011] S4. Construct the occupancy probability value under the coarse positioning posture and the objective function of the two-dimensional normal distribution, and obtain the precise positioning posture of the robot through nonlinear optimization.

[0012] Furthermore, the method for obtaining the outline of the truck in the raster map is as follows:

[0013] S21, determining the grid coordinates of the four vertices of the fixed area, and setting the occupancy probability value of all grid points within the four grid coordinate areas to 0.5, which is an unknown grid;

[0014] S22: Project the object outline in the current laser frame Scan onto the grid map. The object outline within the fixed area is the truck outline.

[0015] Furthermore, the projection method of the object outline in the current laser frame Scan on the grid map is as follows:

[0016] Based on the car body mapping pose Pose_f, the object outline in the laser frame Scan is projected to the world coordinate system, and then the object outline in the world coordinate system is projected to the grid map.

[0017] Furthermore, the calculation formula for the two-dimensional normal distribution parameters of the grid g_ind is as follows:

[0018]

[0019]

[0020] Among them, the coordinate mean E and variance Cov of the laser point occupying the grid g_ind, pt_w krepresents the world coordinates of the kth laser point in the grid g_ind, and M represents the number of laser points in the grid g_ind.

[0021] Furthermore, the method for obtaining the candidate pose at the next moment is as follows:

[0022] The current robot pose Pose_s (x_s, y_s, theta_s) is used to calculate the candidate robot pose Pose_ts (x_ts, y_ts, theta_ts) at the next moment based on the maximum linear velocity Vel_max, maximum angular velocity Ang_max, and radar frequency Scan_Hz.

[0023] x_ts∈[x_s-Vel_max / Scan_Hz,x_s+Vel_max / Scan_Hz];

[0024] y_ts∈[y_s-Vel_max / Scan_Hz,y_s+Vel_max / Scan_Hz];

[0025] theta_ts∈[theta_s-Ang_max / Scan_Hz, theta_s+Ang_max / Scan_Hz].

[0026] Traverse x_ts, y_ts and theta_ts with the set step size to form the robot's candidate pose Pose_ts at the next moment.

[0027] Furthermore, the current point cloud is projected onto the probability map with the selected pose Pose_ts and scored. The scoring calculation formula is as follows:

[0028]

[0029] S i (Pose_ts) represents the two-dimensional grid coordinates (r, c) obtained by projecting the laser point i in the current laser point cloud onto the grid map through the candidate pose Pose_ts, and M(r, c) represents the probability of the two-dimensional grid coordinates (r, c) occupying the grid map.

[0030] Furthermore, the calculation formula of the rate residual function Loss_occ of the occupancy probability value is as follows

[0031]

[0032] N represents the number of laser points in the current laser frame, M(g_ind i ) represents the grid occupancy probability after the i-th laser point is projected onto the grid map.

[0033] Furthermore, the calculation formula of the residual function Loss_nd of the two-dimensional normal distribution is as follows:

[0034]

[0035] E i and Cov i Represents the grid g_ind i The two-dimensional normal distribution parameters, Pose_t*Scan_p i Indicates that the i-th laser point in the laser frame Scan_p i The coarse positioning pose Pose_t is converted to the world coordinate.

[0036] Furthermore, the calculation formula of the nonlinear optimization objective function Loss_Fun is as follows:

[0037] Loss_Fun=(w_occ*Loss_occ+w_nd*Loss_nd)→0

[0038] Among them, w_occ and w_nd represent the set occupancy probability weight and two-dimensional normal distribution weight respectively;

[0039] By using the GaussNewton or LM method to perform nonlinear optimization with the coarse positioning pose Pose_t as the initial value, the optimized pose Pose_result can be obtained, which is the fine positioning pose of the robot at time t.

[0040] When the truck's parking position is not fixed and there are no positioning markers, the present invention significantly improves the problem of low positioning accuracy of the robot in the truck compartment. This method does not rely on markers and does not require the extraction of straight lines in the truck compartment. Through probability maps and two-dimensional normal distribution matching optimization, the robot can be accurately positioned in the truck compartment. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A flow chart of a method for positioning a robot in a truck compartment based on a dynamic grid map according to an embodiment of the present invention;

[0042] Figure 2 A schematic diagram of a fixed area (i.e., an area where trucks are parked) in a world coordinate system according to an embodiment of the present invention;

[0043] Figure 3 A schematic diagram of a truck outline within a fixed area provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0044] 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.

[0045] Figure 1 A flowchart of a method for positioning a robot in a truck compartment based on a dynamic grid map is provided in an embodiment of the present invention. The method specifically includes the following steps:

[0046] S1. Use laser SLAM to build a grid map and determine the car body pose Pose_f(x, y, theta);

[0047] First, the laser SLAM grid map M is established using the laser radar on the robot. The grid occupancy probability value is established by default in the map data, and a two-dimensional normal distribution parameter value is retained. After the grid map is built, the truck outline with random poses in a fixed area is established in the grid map M. Since the truck is rear-mounted, the robot scans the truck compartment with a fixed pose Pose_f(x, y, theta) outside the rear of the truck outline in the grid map. Pose_f(x, y, theta) is the pose of the laser radar in the world coordinate system.

[0048] S2, select and clear the fixed area grid and set it as an unknown grid;

[0049] The fixed area is set according to the maximum size of the truck and the loading and unloading area. The fixed area is the designated parking area for the truck. The length and width of the fixed area are (Length, width). Calculate the world coordinates of the four vertices of the fixed area, such as Figure 2 As shown:

[0050] Pt_a=trans(Pose_f*T_a(0,Width / 2.0,0));

[0051] Pt_b=trans(Pose_f*T_b(0,-Width / 2.0,0));

[0052] Pt_c=trans(Pose_f*T_c(-Length,-Width / 2.0,0));

[0053] Pt_d=trans(Pose_f*T_d(-Length,Width / 2.0,0));

[0054] Among them, Pt_a, Pt_b, Pt_c, and Pt_d represent the coordinates of point a, point b, point c, and point d in the fixed area in the world coordinate system, respectively. T_a(0,Width / 2.0,0), T_b(0,-Width / 2.0,0), T_c(-Length,-Width / 2.0,0), and T_d(-Length,Width / 2.0,0) represent the poses of point a, point b, point c, and point d in the fixed area in the lidar coordinate system, respectively. trans(*) represents the position of the pose. The coordinates of point a, point b, point c, and point d in the fixed area coordinate system are (0,Width / 2.0,0), (0,-Width / 2.0,0), (-Length,-Width / 2.0,0), and (-Length,Width / 2.0,0). The fixed area coordinate system is as follows: Figure 2 shown.

[0055] The world coordinates are projected onto the grid map. The calculation formula is as follows. The grid coordinates of the fixed area point are calculated from the maximum world coordinate point Max of the grid map:

[0056] Grid = (Max-Pt) / Res;

[0057] Res is the resolution of the occupied grid map, Max is the maximum world coordinate of the grid map, Pt represents the world coordinate, and Grid represents the grid coordinate;

[0058] Substituting the world coordinates of points a, b, c, and d into the above formula yields the grid coordinates Grid_a, Grid_b, Grid_c, and Grid_d of the four vertices of a fixed region in the map. Traverse the four grid coordinate regions and set the occupancy probability M(p) for all grid points in the region to 0.5, effectively setting the region to unknown.

[0059] S3, update the truck outline point cloud to the grid map, update the grid occupancy probability value and two-dimensional normal distribution parameters;

[0060] At the robot's mapping pose Pose_f, the laser frame Scan of the current lidar is obtained. The contour set of all objects in the laser frame Scan, including the truck's contour, is extracted. The laser point Scan_p(px,py) in the contour set in the radar coordinate system is projected to the world coordinate system pt_w. The calculation formula is as follows:

[0061] pt_w=Pose_f*p(ptx,pty,0);

[0062] Calculate the grid coordinates g_ind of each laser point. The calculation formula is as follows:

[0063] g_ind = (Max - pt_w) / Res;

[0064] To determine whether the grid coordinate g_ind is within the fixed area, compare its grid coordinate with the horizontal and vertical coordinates of the four points in the fixed area. If the point is within the fixed area, such as Figure 3 As shown, the occupancy probability value is updated, and the occupancy probability value M(g_ind) is set to 1, and a black occupancy grid is displayed in the grid map, that is, the outline of the truck in the grid map is obtained.

[0065] At a fixed mapping pose Pose_f, calculate the two-dimensional normal distribution parameters for the grid hit by each laser frame, where there are M laser points in the grid g_ind. Count the mean E and variance Cov of the points in the grid. The calculation formula is as follows:

[0066]

[0067]

[0068] Among them, pt_w k Represents the world coordinates of the k-th laser point in the grid.

[0069] S4, laser frame and grid map are matched through correlation area to achieve coarse positioning of the robot;

[0070] Then the robot enters the truck compartment. Knowing the robot's pose Pose_s (x_s, y_s, theta_s), maximum linear velocity Vel_max, maximum angular velocity Ang_max, and radar frequency Scan_Hz at time t-1, we can calculate the range set of Pose_ts (x_ts, y_ts, theta_ts) at time t:

[0071] x_ts∈[x_s-Vel_max / Scan_Hz,x_s+Vel_max / Scan_Hz];

[0072] y_ts∈[y_s-Vel_max / Scan_Hz,y_s+Vel_max / Scan_Hz];

[0073] theta_ts∈[theta_s-Ang_max / Scan_Hz, theta_s+Ang_max / Scan_Hz].

[0074] By traversing the three-dimensional pose at the empirical resolution, a series of candidate poses are formed. The point cloud at time t is projected onto the probability map and scored using the candidate pose Pose_ts. The scoring calculation formula is as follows:

[0075]

[0076] S i (Pose_ts) represents the 2D grid coordinates (r, c) obtained by projecting the laser point i in the current laser point cloud onto the grid map using the candidate poses Pose_ts. M(r, c) represents the probability that the 2D grid coordinates (r, c) occupy the grid map. All candidate poses Pose_ts are ranked by their scores, and the pose Pose_t corresponding to the highest score is the coarse positioning pose of the robot at time t.

[0077] In the embodiment of the present invention, positioning is performed by a reflective plate outside the vehicle compartment, and the posture of the robot at the moment of entering the vehicle compartment can be obtained, and this posture is the initial posture of natural positioning in the vehicle compartment.

[0078] S5. Construct the objective function of occupancy probability value and two-dimensional normal distribution, and obtain more accurate positioning posture through nonlinear optimization.

[0079] It is known that the laser frame point set collected by the laser radar at time t has a total of N points. i Convert the coarse positioning pose Pose_t to the world coordinate Pose_t*Scan_p i , and then calculate the map grid coordinates g_ind i , find its corresponding occupation probability M(g_ind i ), we can get the occupancy probability residual function Loss_occ, and its calculation formula is as follows:

[0080]

[0081] grid g_ind i The corresponding two-dimensional normal distribution parameters are E i and Cov i , we can get the two-dimensional distribution probability residual function Loss_nd, and its calculation formula is as follows:

[0082]

[0083] Set the occupancy probability weight w_occ and the two-dimensional normal distribution weight w_nd to obtain the final nonlinear optimization objective function Loss_Fun, whose expression is as follows:

[0084] Loss_Fun=(w_occ*Loss_occ+w_nd*Loss_nd)→0

[0085] By using the Gauss-Newton or LM method to perform nonlinear optimization with the coarse positioning pose Pose_t as the initial value, the optimization result Pose_result can be obtained, which is the precise positioning pose of the robot at time t.

[0086] 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 method for positioning a robot in a truck compartment based on a dynamic grid map, characterized in that: The method specifically comprises the following steps: S1. Build a laser SLAM grid map based on the laser radar on the robot and calibrate the car body’s mapping pose Pose_f. S2. Obtain the current laser frame Scan containing the truck outline at the vehicle compartment mapping pose Pose_f, obtain the truck outline in the grid map, and update the occupancy probability value and two-dimensional normal distribution parameters of the occupied grid; S3: Determine the robot's pose at the previous moment, project the current point cloud onto the grid map based on the candidate pose at the next moment, and score it. The pose corresponding to the highest score is the coarse positioning pose of the robot. S4. Construct the occupancy probability value under the coarse positioning posture and the objective function of the two-dimensional normal distribution, and obtain the precise positioning posture of the robot through nonlinear optimization; The calculation formula of the nonlinear optimization objective function Loss_Fun is as follows: Loss_Fun=(w_occ*Loss_occ+w_nd*Loss_nd)→0 Among them, w_occ and w_nd represent the set occupancy probability weight and two-dimensional normal distribution weight respectively; The Gauss-Newton or LM method is used to perform nonlinear optimization with the coarse positioning pose Pose_t as the initial value, and the optimized pose Pose_result is the fine positioning pose of the robot at time t.

2. The method for positioning a robot in a truck compartment based on a dynamic grid map according to claim 1, wherein: The method for obtaining the truck outline in the raster map is as follows: S21, determining the grid coordinates of the four vertices of the fixed area, and setting the occupancy probability value of all grid points within the four grid coordinate areas to 0.5, which is an unknown grid; S22: Project the object outline in the current laser frame Scan onto the grid map. The object outline within the fixed area is the truck outline.

3. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 2, wherein: The projection method of the object outline in the current laser frame Scan on the grid map is as follows: Based on the car body mapping pose Pose_f, the object outline in the laser frame Scan is projected to the world coordinate system, and then the object outline in the world coordinate system is projected to the grid map.

4. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 1, wherein: The calculation formula for the two-dimensional normal distribution parameters of the occupied grid g_ind is as follows: Among them, the coordinate mean E and variance Cov of the laser point occupying the grid g_ind, pt_w k represents the world coordinates of the kth laser point in the grid g_ind, and M represents the number of laser points in the grid g_ind.

5. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 1, wherein: The method for obtaining the candidate pose at the next moment is as follows: The current robot pose Pose_s (x_s, y_s, theta_s) is used to calculate the candidate robot pose Pose_ts (x_ts, y_ts, theta_ts) at the next moment based on the maximum linear velocity Vel_max, maximum angular velocity Ang_max, and radar frequency Scan_Hz. x_ts∈[x_s-Vel_max / Scan_Hz,x_s+Vel_max / Scan_Hz]; y_ts∈[y_s-Vel_max / Scan_Hz,y_s+Vel_max / Scan_Hz]; theta_ts∈[theta_s-Ang_max / Scan_Hz, theta_s+Ang_max / Scan_Hz]; Traverse x_ts, y_ts and theta_ts with the set step size to form the robot's candidate pose Pose_ts at the next moment.

6. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 1, wherein: The current point cloud is projected onto the probability map using the selected pose Pose_ts and scored. The scoring calculation formula is as follows: S i (Pose_ts) represents the two-dimensional grid coordinates (r, c) obtained by projecting the laser point i in the current laser point cloud onto the grid map through the candidate pose Pose_ts, and M(r, c) represents the probability of the two-dimensional grid coordinates (r, c) occupying the grid map.

7. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 1, wherein: The calculation formula of the rate residual function Loss_occ of the occupancy probability value is as follows N represents the number of laser points in the current laser frame, M(g_ind i ) represents the grid occupancy probability after the i-th laser point is projected onto the grid map.

8. The method for positioning a robot in a truck compartment based on a dynamic grid map as claimed in claim 1, wherein: The calculation formula of the residual function Loss_nd of the two-dimensional normal distribution is as follows: E i and Cov i Represents the grid g_ind i The two-dimensional normal distribution parameters, Pose_t*Scan_p i Indicates that the i-th laser point in the laser frame Scan_p i The coarse positioning pose Pose_t is converted to the world coordinate.

Images