A truck cabin contour mapping and positioning method based on lidar

Abstract

The present invention discloses a truck compartment contour mapping and positioning method based on laser radar, comprising the following steps: (1) selecting a truck compartment contour mapping location; (2) repeatedly scanning the truck compartment contour by laser radar to collect multiple frames of valid point clouds; (3) constructing a truck compartment point cloud contour map; (4) constructing a local reference coordinate system for the truck compartment contour; (5) matching the laser frame contour coordinate system with the local reference coordinate system for the truck compartment contour to achieve coarse positioning of a robot; and (6) constructing a nonlinear optimization objective function to achieve precise positioning of the robot. This method can accurately and reliably achieve precise positioning of the robot within the truck compartment area.

Description

Technical Field

[0001] The present invention relates to the field of laser radar positioning, and in particular to a method for truck compartment contour mapping and positioning based on laser radar. Background Art

[0002] In the future unmanned warehousing and logistics process, one of the important links is the automatic loading and unloading of goods by robots in trucks. Therefore, the positioning and navigation technology of robots in truck compartments is crucial.

[0003] Currently, the technologies on the market are mainly divided into magnetic navigation, visual QR code navigation, visual SLAM navigation, laser reflector positioning, and laser SLAM positioning. Magnetic navigation has a fixed trajectory, and the path requires laying electromagnetic wires or magnetic strips. Visual QR code navigation often uses markers such as QR codes or special graphic codes indoors and requires regular maintenance. Visual SLAM navigation is significantly affected by the lighting environment and has low reliability. LiDAR navigation provides precise laser positioning, flexible and changeable paths, and adapts to various field environments. After breakthroughs and maturity of LiDAR technology, the cost has decreased, and it has now become the mainstream positioning and navigation method for mobile robots.

[0004] There are two ways of laser navigation depending on whether there are road signs or not: one is laser navigation with road signs, such as reflector positioning navigation. Reflector positioning navigation requires laser reflectors to be arranged in the environment. The other is laser navigation without road signs, which is the laser SLAM positioning navigation used on current mainstream AGVs. However, the existing methods have some shortcomings: 1) Laser navigation with road signs requires the arrangement of laser reflectors in the truck. In unmanned logistics factories, it is difficult to arrange laser reflectors on every truck. In addition, the arrangement of reflectors will also occupy the internal space of the truck, affecting the efficiency of truck loading and unloading. 2) In the existing laser SLAM navigation, before the robot enters the truck compartment for positioning, the map is pre-established through the SLAM mapping algorithm. However, the parking position of each truck in the truck loading and unloading is different, resulting in the current laser frame and the previous map after the robot enters the truck, that is, the outline of the previous truck in the map does not match, which will cause a large deviation in the robot's positioning in the truck.

[0005] For example, publication number CN114265374A describes a system and method for AGVs to access cargo in van trucks. This solution locates the truck's outline by first extracting the vehicle's navigation wall data during the mapping process and storing it as map information. Then, after the robot enters the truck's cabin, it uses the real-time extracted navigation wall data to perform matching and positioning. However, this method has the following issues:

[0006] 1) This solution extracts the start and end coordinates of the lines on the left and right sides of the truck bed for matching and positioning. However, the inner walls of truck beds are wrinkled, resulting in unevenness. The thicker sidewalls in point cloud measurement result in poor line position accuracy. Especially for trucks with long beds, the LiDAR scans from the outside of the bed, making it difficult to capture two complete lines and even more difficult to accurately extract the two endpoints of the line segment.

[0007] 2) It is difficult to establish more accurate constraints directly using two line segments because the side walls of the two truck compartments are parallel. Directly using the data from the left and right navigation walls can only establish constraints on the AGV's left and right position, without establishing constraints on the AGV's front and back position and posture. It is impossible to establish posture and front and back position constraints by only identifying the inaccurate endpoints of the front and back line segments. Therefore, the AGV positioning posture of this method is inevitably inaccurate. Summary of the Invention

[0008] The purpose of the present invention is to overcome the shortcomings of the existing technology and provide a method for truck compartment contour mapping and positioning based on lidar. In view of the defects of accurate positioning in the existing technology, a method is provided to achieve accurate mapping and real-time accurate positioning of mobile robots.

[0009] To achieve the above objectives, the present invention adopts a technical solution: a truck compartment contour mapping and positioning method based on laser radar, comprising the following steps:

[0010] (1) Select the location for mapping the truck compartment outline;

[0011] (2) Scan the truck compartment contour repeatedly through the LiDAR to collect multiple frames of valid point clouds;

[0012] (3) Construct a point cloud contour map of the truck compartment;

[0013] (4) Constructing a local reference coordinate system for the truck compartment contour;

[0014] (5) The laser frame contour coordinate system is matched with the local reference coordinate system of the truck compartment contour to achieve coarse positioning of the robot;

[0015] (6) Construct a nonlinear optimization objective function to achieve precise positioning of the robot.

[0016] Step (1) includes: parking the robot within the truck compartment contour mapping position area according to the robot navigation and positioning system outside the truck, and ensuring that the scanning field of view of the laser radar at the truck compartment contour mapping position where the robot is parked covers the interior area of ​​the truck compartment.

[0017] Step (2) includes: scanning the truck compartment contour multiple times by at least one laser radar, preprocessing each frame of point cloud data of the multiple frames of laser radar point cloud data obtained by the scan, and extracting effective interior point cloud data of the compartment from each frame of point cloud data.

[0018] The preprocessing includes performing point cloud filtering, region growing, contour interpolation and other point cloud preprocessing on each frame of the multi-frame lidar point cloud data obtained by scanning.

[0019] Perform point cloud filtering on each frame of the car outline point cloud, calculate the distance l from each point p (x, y) in each frame of the point cloud to the origin of the frame point cloud coordinate system, and calculate the maximum effective distance l that the actual truck car can stop at. max and the maximum effective value of radar ranging l maxrange , filter out all unrealistic and unreasonable noises;

[0020] Based on a fixed constraint angle θ of the interior contour of the car scanned by the radar, a small part of the interior point cloud c is directly extracted from the filtered point cloud. Then, by calculating the Euclidean distance between adjacent points, the region is grown from the small part of the point cloud c to both sides to complete the entire contour point cloud of the interior of the car.

[0021] Interpolate the complete contour point cloud c inside the car, and interpolate the point cloud c according to the points (p1, p2, p3, ..., p n ) in the order of calculating the Euclidean distance d between two points, and selecting the maximum constraint distance d between the two points th , get the minimum number of complementary points k between two points, and fill k points (p1, p2, ..., p k ) to obtain all valid point cloud data for each frame.

[0022] Step (3) includes:

[0023] All valid point cloud data extracted by multi-frame lidar and the robot pose corresponding to each frame of valid data are fused to create a point cloud map of the truck compartment;

[0024] When building a map, each laser frame scan of the lidar corresponds to the robot pose of the frame scan. N mapping scans will collect n pairs of laser frame scans and robot poses. According to the robot pose, the n laser frame point clouds are unified into the same reference coordinate system O. ref Next, all points p in the point cloud of frame i i Unify to coordinate system O ref The transformed point cloud p is obtained i_trans

[0025] p i_trans =T(pose i -poseo )*p i (i=1,…,n)

[0026] For n groups of point clouds p unified to the reference coordinate system i_trans All points in the laser frame scanning sequence index are fused into a single frame fused point cloud p. map , the fused single frame point cloud is used as the truck compartment point cloud contour map p map :

[0027]

[0028] Step (4) includes: obtaining the truck compartment point cloud outline map p map Finally, the minimum 2D bounding box OBB of the car body contour map point cloud is calculated. According to the major and minor axis poses of the OBB bounding box, the approximate pose of the car body is determined. The major and minor axis poses are used as pose constraints for straight line fitting. The random sampling consistency RANSAC method is used to continuously perform parallel straight line fitting and Euclidean clustering on the point cloud contour map of the truck body to obtain the poses of the parallel straight lines on both sides of the truck body and the poses of the perpendicular straight line of the head of the car body; the local reference coordinate system O of the truck body contour is constructed at the midpoint of the two intersection points of the parallel straight lines on both sides and the perpendicular straight line of the head of the car body. Local_ref .

[0029] Step (5) includes:

[0030] The robot enters the truck and processes the contour point cloud of each laser frame to obtain the local coordinate system O corresponding to the truck compartment contour under the current laser frame. Local , and the local coordinate system O of the current laser frame car contour is calculated Local To the local reference coordinate system O of the carriage contour Local_ref The transformation matrix The current frame car contour point cloud is converted to the car contour local reference coordinate system to achieve the coarse positioning of the robot after the coarse matching of the car contour local reference coordinate system.

[0031] Based on the coarse positioning results, the effective contour point cloud of the frame is homogeneously transformed, and then the laser frame point cloud and the truck compartment point cloud map are precisely aligned through nonlinear optimization to obtain real-time accurate posture and realize the precise positioning of the robot in the truck compartment area.

[0032] Accurate registration of the laser frame point cloud and the truck compartment point cloud map is achieved through one of the nonlinear optimization methods: NDT, GaussNewton or LM.

[0033] The advantages of the present invention are that, through region growing and contour interpolation methods, the consistency of the point cloud regional density of the interior contour of the truck compartment is improved. Through point cloud interpolation, the pose accuracy of subsequent extraction of left and right compartment lines is improved, achieving stability in line extraction on both sides of the truck compartment. This also significantly improves the repeatability of the local reference coordinate system for the truck compartment contour. By constructing a local reference coordinate system for the truck compartment contour, a rapid, rough match between the current robot pose and the robot pose during mapping can be achieved, enabling a comprehensive estimate of the robot pose to be completed in a very short time. This not only provides initial conditions for subsequent nonlinear optimization fine matching but also reduces the number and time of subsequent fine matching iterations, enabling rapid and accurate positioning of the robot within the truck compartment. This method addresses the issue of inaccurate positioning of the robot after entering the truck compartment due to the truck's unstable parking position and the absence of positioning markers. This method does not rely on markers or require the extraction of truck contour corner points, enabling rapid and accurate positioning of the robot after entering the truck compartment. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The following is a brief description of the contents and symbols in the drawings of the present invention:

[0035] Figure 1 A flow chart of the mapping and positioning method of the present invention;

[0036] Figure 2 A schematic diagram of the principle of adding regions to a point cloud according to the present invention;

[0037] Figure 3 Schematic diagram of the principle of constructing the local reference coordinate system of the truck compartment contour in the present invention. DETAILED DESCRIPTION

[0038] The specific implementation of the present invention will be further explained in detail below by describing the best embodiment with reference to the accompanying drawings.

[0039] This technical solution proposes a method for truck compartment contour mapping and positioning based on LiDAR: First, the robot is parked within the truck compartment contour mapping location area according to the robot navigation and positioning system outside the truck;

[0040] Secondly, the truck compartment contour is scanned multiple times by at least one LiDAR. Each frame of the scanned LiDAR point cloud data is pre-processed using point cloud filtering, region growing, and contour interpolation to extract valid interior point cloud data from each frame.

[0041] Next, all valid point cloud data extracted by the multi-frame lidar and the robot pose corresponding to each frame of valid data are fused to create a point cloud map of the truck compartment.

[0042] Then, the truck compartment point cloud contour map is continuously processed by parallel straight line fitting and Euclidean clustering to construct a local reference coordinate system of the truck compartment contour, thus completing the establishment of the local reference coordinate system and point cloud map of the truck compartment contour.

[0043] Afterwards, the robot enters the truck and uses the same method to obtain the effective contour point cloud and the corresponding truck compartment local coordinate system for each frame of the LiDAR scan. The local coordinate system of the truck compartment obtained from each frame of the point cloud is transformed with the local coordinate system in the contour map to achieve the coarse positioning of the robot.

[0044] Finally, based on the coarse positioning results, the effective contour point cloud of the frame is homogeneously transformed, and then the laser frame point cloud and the truck compartment point cloud map are precisely aligned through nonlinear optimization to obtain real-time accurate posture and realize the precise positioning of the robot in the truck compartment area.

[0045] The present invention aims to solve the problem of how to achieve accurate mapping and real-time precise positioning of the mobile robot inside the truck compartment.

[0046] The specific process of the technical solution is as follows, and its flow chart is as follows Figure 1 shown.

[0047] (1) Select the location for mapping the truck compartment outline

[0048] According to the robot navigation and positioning system outside the truck, the robot is parked within the truck compartment contour mapping area, ensuring that the lidar's scanning field of view at the truck compartment contour mapping position where the robot is parked covers the interior area of ​​the truck compartment;

[0049] (2) Scan the truck compartment contour repeatedly through the lidar to collect multiple frames of valid point clouds

[0050] 1) Scan the truck compartment contour multiple times with at least one laser radar to obtain n frames of compartment contour point cloud; 2) Perform point cloud filtering on the n frames of compartment contour point cloud respectively, and calculate the distance l from each point p(x, y) in each frame of point cloud to the origin of the frame point cloud coordinate system. According to the actual maximum effective distance l of the truck compartment, the maximum effective distance l is calculated. max and the maximum effective value of radar ranging l maxrange , filter out all unrealistic and unreasonable noises; filtering means filtering out those that exceed the maximum effective distance l max and the maximum effective value of radar ranging l maxrange The point where the filtering is mainly over l max The data point of the value; maxrange The maximum effective value is a property of the radar itself;

[0051]

[0052] l<l max

[0053] l<l maxronge

[0054] 3) According to a fixed constraint angle θ of the interior contour of the car scanned by the radar, a small part of the interior point cloud c is directly extracted from the filtered point cloud. Then, by calculating the Euclidean distance of adjacent points, the region growing algorithm is performed from the small part of the point cloud c to both sides to complete the contour point cloud of the entire interior of the car, as shown in the following example: Figure 2 gesture.

[0055] 4) Interpolate the complete contour point cloud c inside the car, and interpolate the point cloud c according to the points (p1, p2, p3, ..., p n ) in the order of calculating the Euclidean distance d between two points, and selecting the maximum constraint distance d between the two points th , get the minimum number of complementary points k between two points, and fill k points (p1, p2, ..., p k ) to obtain all valid point cloud data of each frame, thereby ensuring high consistency of density of contour point cloud area and improving the accuracy and precision of subsequent point cloud straight line fitting.

[0056] d i，i+1 =||p i -p i+1 ||(i=1,…,n-1)

[0057]

[0058]

[0059] Among them, p i Indicates the coordinates of the i-th point in the laser frame scan, d i,i+1 represents the Euclidean distance between the i-th point and the i+1-th point, k i,i+1 Indicates the minimum number of complementary points between the i-th point and the i+1-th point, d th Indicates the maximum constrained distance between points.

[0060] (3) Constructing a point cloud contour map of the truck compartment

[0061] The point cloud data of all valid point cloud data extracted by multi-frame lidar and the robot pose corresponding to each frame of valid data are fused to build a point cloud map of the truck compartment. When building the map, each laser frame scan of the lidar corresponds to the robot pose of the frame scan. N mapping scans will collect n pairs of laser frame scans and robot poses. According to the robot pose, the n laser frame point clouds are unified into the same reference coordinate system O. refNext (the first scan robot pose can be used as the reference coordinate system O ref ), all points p in the point cloud of the i-th frame i Unify to coordinate system O ref The transformed point cloud p is obtained i_trans

[0062] p i_tran ,=T(pose i -pose o )*p i (i=l,…,n)

[0063] Among them, T(pose i -pose o ) is the robot’s position relative to the reference coordinate system O each time the map is constructed ref The homogeneous transformation matrix

[0064] For n groups of point clouds p unified to the reference coordinate system i_trans All points in the laser frame scanning sequence index are fused into a single frame fused point cloud p. map , the fused single frame point cloud is used as the truck compartment point cloud contour map p map .

[0065]

[0066] Among them, p map，index is the coordinate of the point with index index in the fused single-frame point cloud, p i_trans，index It is the coordinate of the point with index index in the single frame point cloud before fusion

[0067] (4) Constructing the local reference coordinate system of the truck compartment contour

[0068] Known truck compartment point cloud outline map p map , calculate the minimum 2D bounding box OBB of the car body outline map point cloud. According to the major and minor axis poses of the OBB bounding box, the approximate pose of the car body can be determined. The major and minor axis poses are used as pose constraints for straight line fitting respectively. The random sampling consistency RANSAC (Random Sample Consensus) method is used to continuously perform parallel straight line fitting and Euclidean clustering on the truck body point cloud outline map to obtain the poses of the parallel straight lines on both sides of the truck body and the perpendicular straight line pose of the head of the car body. The local reference coordinate system O of the truck body outline is constructed at the midpoint of the two intersection points of the parallel straight lines on both sides and the perpendicular straight line of the head of the car body. Local_ref .like Figure 3 gesture.

[0069] (5) The laser frame contour coordinate system is matched with the local reference coordinate system of the truck compartment contour to achieve coarse positioning of the robot

[0070] The robot enters the truck for real-time positioning, and the current lidar scans in real time. Each scan can obtain the current single-frame laser frame contour data.

[0071] 1) Perform point cloud filtering on each frame of laser frame data, calculate the distance l from each point p (x, y) in each frame of point cloud to the origin of the frame point cloud coordinate system, and calculate the maximum effective distance l that the actual truck compartment can stop at. max and the maximum effective value of radar ranging l maxrange , filter out all unrealistic and unreasonable noises;

[0072]

[0073] l<l max

[0074] I<I maxrange

[0075] 2) According to a fixed constraint angle θ of the interior contour of the car scanned by the radar, a small part of the interior point cloud c is directly extracted from the filtered point cloud. Then, by calculating the Euclidean distance of adjacent points, the region is grown from the small part of the point cloud c to both sides to complete the contour point cloud of the interior of the car, as shown in the following example: Figure 2 signal;

[0076] 3) Interpolate the complete contour point cloud c inside the car, and interpolate the point cloud c according to the points (p1, p2, p3, ..., p n )) Calculate the Euclidean distance d between two points in the order of )) and select the maximum constraint distance d between the two points th , get the minimum number of complementary points k between two points, and fill k points (p1, p2, ..., p k ) to obtain all valid point cloud data of each frame, thereby ensuring high consistency of density of contour point cloud area and improving the accuracy and precision of subsequent point cloud straight line fitting.

[0077] d i，i+1 =||p i -p i+1 ||(i=1,…,n-1)

[0078]

[0079]

[0080] Among them, p i Indicates the coordinates of the i-th point in the laser frame scan, d i,i+1represents the Euclidean distance between the i-th point and the i+1-th point, k i,i+1 Indicates the minimum number of complementary points between the i-th point and the i+1-th point, d th Indicates the maximum constrained distance between points.

[0081] 4) Knowing the current truck compartment point cloud laser frame contour data p Local , calculate the minimum 2D bounding box OBB of the car body contour point cloud. According to the major and minor axis poses of the OBB bounding box, the approximate pose of the car body can be determined. The major and minor axis poses are used as pose constraints for straight line fitting respectively. The random sampling consistency RANSAC (Random Sample Consensus) method is used to continuously perform parallel straight line fitting and Euclidean clustering on the truck body point cloud contour map to obtain the poses of the parallel straight lines on both sides of the truck body and the perpendicular straight line pose of the head of the car body. The local coordinate system O corresponding to the truck body contour in the current laser frame is constructed at the midpoint of the two intersection points of the parallel straight lines on both sides and the perpendicular straight line of the head of the car body. Local ,like Figure 3 gesture.

[0082] 5) Calculate the local coordinate system O of the current laser frame car contour Local To the local reference coordinate system O of the carriage contour Local_ref The transformation matrix The current frame car contour point cloud is converted to the car contour local reference coordinate system to achieve the coarse positioning of the robot after the coarse matching of the car contour local reference coordinate system.

[0083] (6) Construct nonlinear optimization objective function to achieve precise positioning of the robot

[0084] According to the coarse positioning results, the effective contour point cloud of the frame is homogeneously transformed, and a nonlinear optimization function for the registration of two sets of point clouds is constructed:

[0085]

[0086] Among them, E represents the nonlinear optimization objective function, Q i Represents the truck compartment point cloud outline map p map The i-th coordinate point in P i Indicates the current laser frame car outline coordinate point after the transformation matrix The i-th coordinate point after transformation, R and T represent the rotation and translation transformation matrices of the coordinates, respectively, ||x|| 2 represents the two-norm of x,

[0087] For the sum operation.

[0088] The precise registration of the laser frame point cloud and the truck compartment point cloud map can be achieved through nonlinear optimization methods such as NDT (normal distribution transformation), Gauss-Newton or LM (Levenberg-Marquard), obtaining real-time accurate pose and realizing precise positioning of the robot in the truck compartment area.

[0089] This solution can achieve:

[0090] 1) Due to the wrinkles on the inner wall of a truck compartment, resulting in unevenness, the side walls are thicker when measured by point cloud, resulting in poor accuracy in the measured straight line position. This is especially true for trucks with longer compartments, where the internal laser scanning contour point cloud has characteristics such as uneven density dispersion and unevenness. This solution improves the consistency of the regional density of the compartment's internal contour point cloud through region growing and contour interpolation methods. Through point cloud interpolation, the pose accuracy of the subsequent extraction of the left and right compartment lines is improved, achieving stability in the extraction of lines on both sides of the compartment. It also greatly improves the repeatability of the local reference coordinate system of the truck compartment contour.

[0091] 2) By constructing a local reference coordinate system for the truck compartment contour, a rapid and rough match between the current robot pose and the pose during mapping can be achieved, allowing for a comprehensive estimation of the robot pose in a very short time. This not only provides initial conditions for subsequent nonlinear optimization fine matching, but also shortens the number and time of subsequent fine matching iterations, enabling rapid and accurate positioning of the robot within the truck compartment area.

[0092] 3) This method solves the problem of inaccurate positioning of the robot after entering the truck due to the unstable parking position and the lack of positioning markers. This method does not rely on markers or extract the truck's outline corner points, and can achieve rapid and accurate positioning of the robot after entering the truck compartment.

[0093] Obviously, the specific implementation of the present invention is not limited to the above-mentioned methods. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, they are all within the scope of protection of the present invention.

Claims

1. A truck compartment contour mapping and positioning method based on LiDAR, characterized by: The steps include: (1) Select the location for mapping the truck compartment outline; (2) Scan the truck compartment contour repeatedly through the LiDAR to collect multiple frames of valid point clouds; (3) Construct a point cloud contour map of the truck compartment; (4) Constructing a local reference coordinate system for the truck compartment contour; (5) The laser frame contour coordinate system is matched with the local reference coordinate system of the truck compartment contour to achieve coarse positioning of the robot; (6) Construct a nonlinear optimization objective function to achieve precise positioning of the robot; Step (2) includes: Perform point cloud filtering on each frame of the car outline point cloud, calculate the distance l from each point p (x, y) in each frame of the point cloud to the origin of the frame point cloud coordinate system, and calculate the maximum effective distance l that the actual truck car can stop at. max and the maximum effective value of radar ranging l maxrange , filter out all unrealistic and unreasonable noises; Based on a fixed constraint angle θ of the interior contour of the car scanned by the radar, a small part of the interior point cloud c is directly extracted from the filtered point cloud. Then, by calculating the Euclidean distance between adjacent points, the region is grown from the small part of the point cloud c to both sides to complete the entire contour point cloud of the interior of the car. Interpolate the complete contour point cloud c inside the car, and interpolate the point cloud c according to the points (p1, p2, p3, ..., p n ) in the order of calculating the Euclidean distance d between two points, and selecting the maximum constraint distance d between the two points th , get the minimum number of complementary points k between two points, and fill k points (p1, p2, ..., p k ) to obtain all valid point cloud data for each frame.

2. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Step (1) includes: parking the robot within the truck compartment contour mapping position area according to the robot navigation and positioning system outside the truck, and ensuring that the scanning field of view of the laser radar at the truck compartment contour mapping position where the robot is parked covers the interior area of ​​the truck compartment.

3. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Step (2) includes: scanning the truck compartment contour multiple times by at least one laser radar, preprocessing each frame of point cloud data of the multiple frames of laser radar point cloud data obtained by the scan, and extracting effective interior point cloud data of the compartment from each frame of point cloud data.

4. The method for truck compartment contour mapping and positioning based on laser radar according to claim 3, characterized in that: The preprocessing includes point cloud filtering, region growing, and contour interpolation processing on each frame of point cloud data obtained by scanning multiple frames of lidar point cloud data.

5. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Step (3) includes: All valid point cloud data extracted by multi-frame lidar and the robot pose corresponding to each frame of valid data are fused to create a point cloud map of the truck compartment; When building a map, each laser frame scan of the lidar corresponds to the robot pose of the frame scan. N mapping scans will collect n pairs of laser frame scans and robot poses. According to the robot pose, the n laser frame point clouds are unified into the same reference coordinate system O. ref Next, all points p in the point cloud of the i-th frame i Unify to coordinate system O ref The transformed point cloud p is obtained i_trans p i_trans =T(pose i -pose o )*p i (i=1,…,n) Among them, T(pose i -pose o ) is the robot’s position relative to the reference coordinate system O each time the map is constructed ref The homogeneous transformation matrix of For n groups of point clouds p unified to the reference coordinate system i_trans All points in the laser frame scanning sequence index are fused into a single frame fused point cloud p. map , the fused single frame point cloud is used as the truck compartment point cloud contour map p map : Among them, p map，index is the coordinate of the point with index index in the fused single-frame point cloud, p i_trans，index It is the coordinate of the point with index index in the single frame point cloud before fusion.

6. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Step (4) includes: obtaining the truck compartment point cloud outline map p map Finally, the minimum 2D bounding box OBB of the car body contour map point cloud is calculated. According to the major and minor axis poses of the OBB bounding box, the approximate pose of the car body is determined. The major and minor axis poses are used as pose constraints for straight line fitting. The random sampling consistency RANSAC method is used to continuously perform parallel straight line fitting and Euclidean clustering on the point cloud contour map of the truck body to obtain the poses of the parallel straight lines on both sides of the truck body and the poses of the perpendicular straight line of the head of the car body; the local reference coordinate system O of the truck body contour is constructed at the midpoint of the two intersection points of the parallel straight lines on both sides and the perpendicular straight line of the head of the car body. Local_ref .

7. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Step (5) includes: The robot enters the truck and processes the contour point cloud of each laser frame to obtain the local coordinate system O corresponding to the truck compartment contour under the current laser frame. Local , and the local coordinate system O of the current laser frame car contour is calculated Local To the local reference coordinate system O of the carriage contour Local_ref The transformation matrix The current frame car contour point cloud is converted to the car contour local reference coordinate system to achieve the coarse positioning of the robot after the coarse matching of the car contour local reference coordinate system.

8. The method for truck compartment contour mapping and positioning based on laser radar according to claim 1, characterized in that: Based on the coarse positioning results, the effective contour point cloud of the frame is homogeneously transformed, and then the laser frame point cloud and the truck compartment point cloud map are precisely aligned through nonlinear optimization to obtain real-time accurate posture and realize the precise positioning of the robot in the truck compartment area.

9. The method for truck compartment contour mapping and positioning based on laser radar according to claim 8, characterized in that: Accurate registration of the laser frame point cloud and the truck compartment point cloud map is achieved through one of the nonlinear optimization methods: NDT, GaussNewton or LM.

Images