Right-angle machine docking method and device based on 2D laser radar

Abstract

The invention provides a right-angle machine docking method and device based on a 2D laser radar, and relates to the technical field of positioning navigation, and the method comprises the steps: obtaining the 2D scanning data of the laser radar of a mobile robot in real time, clustering laser points, and screening a point set with the point number in a preset range. And determining a butt joint direction based on the screening point set, selecting a unique point set as a source point cloud of the right-angle side of the right-angle machine table, and generating a target point cloud. And the source point cloud, the target point cloud and laser radar external parameters are utilized to obtain a recognition pose, a predicted pose is obtained by combining speedometer speed information, and a current pose is determined and published. Finally, the mobile robot is controlled to be in butt joint with the right-angle machine table according to the released pose navigation. According to the invention, the high-precision lateral butt joint of the mobile robot and the working machine in a narrow channel application scene is realized only by depending on the shape characteristics of the machine and the laser radar sensor, and the space utilization rate is improved.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a right-angle machine docking method and device based on 2D laser radar. Background Art

[0002] In the field of intelligent manufacturing, mobile robots have a wide range of applications. Among them, the precise docking between mobile robots and work machines is a key step in achieving operational tasks such as material transfer and grasping.

[0003] In order to achieve high-precision docking with the work machine, the mobile robot mainly achieves forward docking with the work machine based on the following solutions: one is through the method of laser radar docking, that is, posting a reflector on the workbench to achieve forward docking between the mobile robot and the work machine; the second is through camera vision docking, such as using a side camera or a downward camera to identify a QR code or a ribbon; the third is through surface texture navigation docking. However, the first two methods require additional sensors and deployment, and the ribbons posted on the ground are easy to be contaminated and difficult to maintain. In addition, the method of docking through surface texture navigation also requires an additional downward camera and is easily affected by surface contamination. At the same time, the above-mentioned laser radar docking and camera vision docking solutions are often difficult to implement when applied to the docking of work machines in narrow channels.

[0004] Therefore, when docking with a work machine in a narrow channel, the mobile robot usually needs to use a side docking method. However, due to the accuracy limitation of the robot laser SLAM (Simultaneous Localization and Mapping) technology (usually about ±10mm), the traditional SLAM natural navigation method cannot meet the high-precision docking requirements of the mobile robot and the work machine. Summary of the invention

[0005] To this end, an embodiment of the present invention provides a right-angle machine docking method and device based on 2D laser radar, which is used to solve the problem that the traditional SLAM natural navigation method in the prior art cannot meet the high-precision docking requirements between the mobile robot and the working machine.

[0006] In order to solve the above problems, an embodiment of the present invention provides a right-angle platform docking method based on 2D laser radar, the method comprising:

[0007] Acquire 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points for each frame of laser data, and filter out the point sets with point numbers within a preset range;

[0008] Determine the docking direction of the mobile robot based on the screened point set, select a unique point set as the source point cloud of the right-angle edge of the right-angle machine platform acquired by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle edge of the right-angle machine platform;

[0009] The recognized posture of the mobile robot is obtained according to the source point cloud, the target point cloud and the external parameters of the laser radar, and the current predicted posture of the mobile robot is obtained by using the speed information of the odometer, and the current posture of the mobile robot is determined and published based on the difference between the recognized posture and the predicted posture;

[0010] According to the published posture, navigation control is performed to enable the mobile robot to dock with the right-angle machine.

[0011] Preferably, the real-time acquisition of 2D laser data scanned by a laser radar on a mobile robot, clustering of laser scanning points for each frame of laser data, and screening out point sets with point numbers within a preset range include:

[0012] Acquire 2D laser data scanned by the LiDAR on the mobile robot in real time;

[0013] Traverse each laser radar scanning point in the current frame in turn, obtain the coordinate value of each point in the laser coordinate system, and cluster the scanning points of the current frame according to the preset clustering distance threshold to obtain at least one point set;

[0014] The clustered point sets are filtered to select the point sets with the number of points within the preset range. If the number of point sets filtered out in the current frame is 0, the laser data for the next frame is returned for processing.

[0015] Preferably, the process of clustering the scan points of the current frame according to a preset clustering distance threshold to obtain at least one point set includes:

[0016] Start with an initial point to construct a point set;

[0017] According to the order of the laser radar scanning points, the other laser points of the current frame are traversed in turn, and the distance between the current point and the previous point is calculated. If the distance between the current point and the previous point is less than the preset clustering distance threshold, the current point is grouped into the point set where the previous point is located; if the distance between the current point and the previous point is greater than or equal to the preset clustering distance threshold, a new point set is created and the current point is stored in the new point set until the last point is traversed, and finally a point set of all continuous points is obtained.

[0018] Preferably, the process of determining the docking direction of the mobile robot based on the screened point set, selecting a unique point set as a source point cloud of the right-angle side of the right-angle machine platform acquired by laser radar scanning based on a preset rule, and generating a corresponding target point cloud of the right-angle side of the right-angle machine platform includes:

[0019] For each point set selected, obtain the point in the point set that is closest to the laser radar, and use this point as the boundary to divide the point set into two sub-point sets containing this point; if any sub-point set of the point set contains only one point, discard the point set and its corresponding two sub-point sets;

[0020] A sub-point set is randomly selected from the two obtained sub-point sets, and the slope of the sub-point set is obtained by fitting using a random sampling consensus algorithm;

[0021] The corresponding inclination angle is obtained according to the fitting slope. If the inclination angle is within the preset angle range, it is determined that the mobile robot docks with the right-angle machine in the first docking direction; otherwise, another sub-point set of the two sub-point sets is selected for fitting to obtain the inclination angle corresponding to the slope of the point set, and it is determined whether the inclination angle is within the preset angle range. If so, it is determined that the mobile robot docks with the right-angle machine in the second docking direction.

[0022] Preferably, the process of determining the docking direction of the mobile robot based on the screened point set, selecting a unique point set as a source point cloud of the right-angle side of the right-angle machine platform acquired by laser radar scanning based on a preset rule, and generating a corresponding target point cloud of the right-angle side of the right-angle machine platform also includes:

[0023] Select the point set with the most right-angled edges as the source point cloud of the right-angled edges of the right-angled machine platform obtained by the laser radar scanning;

[0024] Calculating the lengths of two right-angled sides according to the currently selected point set, and generating a right-angled side template according to the lengths, wherein the right-angled side template has a number of target points equal to the number of points in the selected point set;

[0025] A coordinate system of the right-angle edge template is established and used as the coordinate system of the right-angle machine, and the coordinates of each target point of the right-angle edge template are obtained and used as the target point cloud of the right-angle edge.

[0026] Preferably, the process of obtaining the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, obtaining the current predicted posture of the mobile robot by using the speed information of the odometer, and determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture being reasonable includes:

[0027] Assume that the source point cloud and the target point cloud are point sets P = {P 1 ,P 2 ,······,P n} and Q = {Q 1 ,Q 2 ,······,Q n}, where n represents the number of points in the point set, for each point P i and Qi corresponds to a point in two-dimensional space, that is and Where P i is a point in the source point cloud, Q i is the nearest neighbor point in the corresponding target point cloud;

[0028] Calculate the center of mass μ of the source point cloud P and the centroid μ of the target point cloud Q , respectively:

[0029]

[0030] Subtract the centroid of each point in the point sets P and Q, and obtain the point sets P′ and Q′ after removing the centroid respectively:

[0031] P′={P′ 1 =P 1 -μ P ,P′ 2 =P 2 -μ P ,······,P′ n =P n -μ P};

[0032] Q′={Q′ 1 =Q 1 -μ Q ,Q′ 2 =Q 2 -μ Q ,······,Q′ n =Q n -μ Q};

[0033] The point sets P′ and Q′ after removing the centroid are constructed into matrices [P′] and [Q′] respectively. The size of these two matrices is 2×n:

[0034] [P′]=[P′ 1 ,P′ 2 ,······,P′ n ];

[0035] [Q′]=[Q′ 1 ,Q′ 2 ,······,Q′ n ];

[0036] The covariance matrix [H] is calculated based on the obtained matrices [P′] and [Q′]:

[0037] [H]=[P′]·[Q′] T ;

[0038] Among them, the size of the covariance matrix [H] is 2×2, and each element in the covariance matrix [H] represents the covariance of the corresponding dimension between two point sets P′ and Q′; [Q′] T represents the transposed matrix of the matrix [Q′];

[0039] Perform SVD decomposition on the covariance matrix [H]:

[0040] [H]=[U][Σ][V] T ;

[0041] Wherein, [U] and [V] are orthogonal matrices obtained by decomposition, and [Σ] is a diagonal matrix obtained by decomposition;

[0042] Calculate the rotation matrix [R] and translation matrix [t] respectively:

[0043] [R]=[V][U] T ;

[0044] [t]=[μ Q ]-[R][μ P ];

[0045] After the optimal R and t are obtained through continuous iteration, the position and posture of the laser radar in the rectangular coordinate system of the docking station are obtained.

[0046] Obtain the position of the laser radar relative to the mobile robot based on the external parameters of the laser radar And thus obtain the recognition posture of the mobile robot

[0047]

[0048] Preferably, the process of obtaining the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, obtaining the current predicted posture of the mobile robot by using the speed information of the odometer, and determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture being reasonable also includes:

[0049] Get the recognized position T of the mobile robot at time t t , if it is at the initial moment of docking, that is, t=0, initialize a UKF pose estimator to obtain the pose of the mobile robot at the initial moment; the UKF pose estimator is used to record the current time, position and pose information of the mobile robot;

[0050] Get the recognized position T of the mobile robot at time t+1 t+1 , and based on the recognized posture T at time t t, combined with the speed information of the odometer, the predicted position and posture of the mobile robot is obtained

[0051] Get the current recognition pose T t+1 and predicted pose Is the difference less than the preset difference threshold? If the recognized posture T t+1 and predicted pose If the difference is less than the preset difference threshold, the recognition pose T is released. t+1 ; Otherwise, publish the predicted pose

[0052] Preferably, the process of performing navigation control according to the published posture to enable the mobile robot to dock with the right-angle machine includes:

[0053] At the docking starting point, the current position and posture information of the mobile robot in the rectangular coordinate system is obtained according to the published position and posture;

[0054] According to the obtained posture information, the mobile robot is controlled to move to a preset position in front of the docking machine at a right angle, that is, the docking middle point;

[0055] When the mobile robot reaches the docking intermediate point, it remains stationary and collects posture information for a period of time, the posture information including X, Y coordinates and Yaw angle, and performs average processing on the posture information;

[0056] The averaged posture information is compared with the final docking target point to calculate the lateral deviation, longitudinal deviation and angular deviation, wherein the lateral deviation represents the distance between the current position of the mobile robot and the docking target point in the horizontal direction, the longitudinal deviation represents the distance between the current position of the mobile robot and the docking target point in the forward direction, and the angular deviation represents the angle between the current orientation of the mobile robot and the target orientation;

[0057] The mobile robot walks in a straight line, eliminating lateral deviation and angular deviation during the walking process, and finally reaches the set docking target point.

[0058] The embodiment of the present invention further provides a right-angle machine docking device based on 2D laser radar, which is used to implement the right-angle machine docking method based on 2D laser radar described above, specifically comprising:

[0059] The laser data preprocessing module is used to obtain the 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points of each frame of laser data, and filter out the point sets with the number of points within the preset range;

[0060] The source point cloud and target point cloud generation module is used to determine the docking direction of the mobile robot based on the screened point set, and select a unique point set as the source point cloud of the right-angle side of the right-angle machine platform obtained by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle side of the right-angle machine platform;

[0061] A posture determination and publishing module is used to obtain the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, and obtain the current predicted posture of the mobile robot using the speed information of the odometer, and determine and publish the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture;

[0062] The navigation control module is used to perform navigation control according to the published posture so that the mobile robot can dock with the right-angle machine.

[0063] An embodiment of the present invention further provides a computer storage medium storing a computer software product, wherein the computer software product includes a number of instructions for enabling a computer device to execute the above-mentioned 2D laser radar-based right-angle machine docking method.

[0064] It can be seen from the above technical solutions that the present invention has the following beneficial effects:

[0065] (1) High-precision docking without reflectors: The present invention only relies on the shape characteristics of the docking machine, without the need for additional reflectors, to achieve high-precision docking (±3mm) between the mobile robot and the right-angle machine.

[0066] (2) Only relying on LiDAR to achieve precise docking: The present invention does not require the installation of additional visual sensors, and precise docking can be achieved only through LiDAR sensors, reducing hardware costs and technical complexity.

[0067] (3) Supporting lateral docking to improve space utilization: The present invention realizes lateral docking of the machine, which is suitable for application scenarios in narrow channels, improves space utilization, and ensures high docking precision. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] In order to more clearly illustrate the implementation cases of the present invention or the technical solutions in the prior art, the following is a brief description of the drawings required for use in the embodiments. By referring to the drawings, the features and advantages of the present invention will be more clearly understood. The drawings are schematic and should not be understood as limiting the present invention in any way. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. Among them:

[0069] Figure 1 A flowchart of a right-angle machine docking method based on 2D laser radar provided in an embodiment;

[0070] Figure 2 A flowchart of selecting a point set with a number of points within a preset range in an embodiment;

[0071] Figure 3 A schematic diagram of obtaining the point closest to the laser radar in the point set in the embodiment;

[0072] Figure 4 This is a flow chart of calculating the position and posture of the laser radar in the rectangular machine coordinate system in the embodiment;

[0073] Figure 5 A flowchart for obtaining a release posture in an embodiment;

[0074] Figure 6 It is a flow chart of performing navigation control according to the published posture in the embodiment to make the mobile robot dock with the right-angle machine platform;

[0075] Figure 7 A schematic diagram of the points of the mobile robot navigation process in the embodiment;

[0076] Figure 8 A schematic diagram of a mobile robot with a QR code attached thereto in an embodiment;

[0077] Fig. 9 A block diagram of a right-angle machine docking device based on 2D laser radar provided in an embodiment. DETAILED DESCRIPTION

[0078] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0079] Embodiment 1

[0080] In order to solve the problem that the traditional SLAM natural navigation method in the prior art cannot meet the high-precision docking requirements of the mobile robot and the working machine. Figure 1 As shown, an embodiment of the present invention provides a right-angle platform docking method based on 2D laser radar, the method comprising:

[0081] Acquire 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points for each frame of laser data, and filter out the point sets with point numbers within a preset range;

[0082] Determine the docking direction of the mobile robot based on the screened point set, select a unique point set as the source point cloud of the right-angle edge of the right-angle machine platform acquired by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle edge of the right-angle machine platform;

[0083] The recognized posture of the mobile robot is obtained based on the source point cloud, the target point cloud and the external parameters of the laser radar, and the current predicted posture of the mobile robot is obtained using the speed information of the odometer. The current posture of the mobile robot is determined and published based on the reasonableness of the recognized posture and the difference between it and the predicted posture.

[0084] According to the published posture, navigation control is performed to enable the mobile robot to dock with the right-angle machine.

[0085] In the embodiment of the present invention, laser data preprocessing is a key step. This step mainly includes real-time acquisition of 2D laser data scanned by the laser radar on the mobile robot, clustering the laser scanning points of each frame of laser data, and screening out the point set with the number of points within a preset range, such as Figure 2 As shown, specifically including:

[0086] First, obtain the 2D laser data scanned by the laser radar on the mobile robot in real time. The 2D laser radar on the mobile robot will scan the surrounding environment in real time and generate a series of laser data. These data are usually represented in the form of points, and each point contains two pieces of information: distance L and angle θ. The distance L indicates the distance between the laser point and the laser radar, while the angle θ indicates the angle between the laser point and the front of the laser radar.

[0087] Then, each laser radar scanning point in the current frame is traversed in turn, the coordinate value of each point in the laser coordinate system is obtained, and the scanning points of the current frame are clustered according to the preset clustering distance threshold to obtain at least one point set.

[0088] Furthermore, for the convenience of subsequent processing, each laser point needs to be converted from polar coordinates (distance L and angle θ) to coordinate values ​​(x, y) in a rectangular coordinate system. In this embodiment, the method for obtaining the coordinate value of each point in the laser coordinate system is:

[0089] According to the distance L and angle θ of each point in the laser coordinate system, the coordinate value (x, y) of each point in the laser coordinate system can be calculated, where the calculation formula is: x = L*cosθ; y = L*sinθ; θ represents the scanning angle of the laser radar (the angle with the x direction).

[0090] Furthermore, after obtaining the coordinate value of each laser point in the rectangular coordinate system, it is necessary to cluster these points. The purpose of clustering is to classify points with similar distances into a point set for subsequent screening and identification. In this embodiment, the scanning points of the current frame are clustered according to a preset clustering distance threshold, and the process of obtaining at least one point set includes:

[0091] Construct an initial point set: Use the first scan point as the initial point to start constructing a point set.

[0092] Traverse laser points: Traverse the other laser points in the current frame in sequence according to the order of the laser radar scanning points.

[0093] Calculate distance: For each current point, calculate the distance between it and the previous point. This distance is obtained by comparing the coordinate values ​​of the two points in the laser coordinate system.

[0094] Clustering judgment: If the distance between the current point and the previous point is less than the preset clustering distance threshold, the two points are considered to be part of the same object or structure, so the current point is grouped into the point set where the previous point is located. If the distance between the current point and the previous point is greater than or equal to the preset clustering distance threshold, the two points are considered to belong to different objects or structures, so a new point set needs to be created and the current point is stored in the new point set.

[0095] Repeat steps: Continue to traverse the remaining laser points and repeat the above clustering judgment process until the last point is traversed, and finally the point set of all continuous points can be obtained.

[0096] After obtaining all the point sets, it is necessary to filter the clustered point sets to filter out the point sets with the number of points within the preset range. This preset range is determined according to the actual application scenario and docking requirements. If the number of point sets filtered out in the current frame is 0, the laser data for the next frame will be returned for processing.

[0097] In the right-angle machine docking method based on 2D laser radar, another key step is to determine the docking direction of the mobile robot based on the screened point set, and select a unique point set as the source point cloud of the right-angle side of the right-angle machine acquired by the laser radar scanning based on the preset rules, and the process of generating the corresponding target point cloud of the right-angle side of the right-angle machine includes:

[0098] First, for each point set selected, obtain the point in the point set that is closest to the laser radar. This point is regarded as a split point, and the point set is divided into two parts based on this point to obtain two sub-point sets containing this point, such as Figure 3As shown. It should be noted that if any sub-point set of the point set contains only one point (i.e. the split point itself), the point set and its corresponding two sub-point sets will be discarded. This is because a single point cannot provide enough information to determine the slope or direction of the line.

[0099] Then, a sub-point set is randomly selected from the two obtained sub-point sets, and the slope of the sub-point set is obtained by fitting using the random sampling consensus (RANSAC) algorithm.

[0100] The RANSAC algorithm is an iterative algorithm that correctly estimates the parameters of a mathematical model from a set of data containing "outliers". When using the RANSAC algorithm to fit a straight line, first randomly select two non-overlapping points in the point set, and solve the parameters of the line (such as slope and intercept) through these two points. Then, calculate the distance from each remaining point in the point set to the line. If the distance of a point is less than the preset distance threshold, the point is considered an "inlier" and the number of inliers is counted. This process is repeated repeatedly, and each iteration randomly selects two new points to refit the line and recalculate the number of inliers. The number of iterations usually has an upper limit, namely the maximum number of iterations. Among all the iterations, the iteration with the largest number of inliers is found. At this time, the corresponding inlier point set is the "largest consensus set", and the slope of this line (determined by the two points randomly selected in that iteration) is considered to be the fitting slope of the point set.

[0101] Finally, the corresponding inclination angle is obtained according to the fitted slope. If the inclination angle is within the preset angle range, it is determined that the mobile robot docks with the right-angle machine in the first docking direction. Otherwise, another sub-point set of the two sub-point sets is selected for fitting to obtain the inclination angle corresponding to the slope of the point set, and it is determined whether the inclination angle is within the preset angle range. If so, it is determined that the mobile robot docks with the right-angle machine in the second docking direction.

[0102] It should be noted that the first docking direction and the second docking direction can be set according to requirements. For example, the first docking direction corresponds to the right side, and the second docking direction corresponds to the left side.

[0103] In addition, in the present embodiment, the above-mentioned preset angle range is set to ±30°. The setting of this preset angle range is usually closely related to the accuracy of the mobile robot when navigating using laser SLAM technology. Generally speaking, the higher the accuracy of laser SLAM, the smaller the required preset angle range. This means that when the mobile robot moves to the docking range based on laser SLAM technology, the angle deviation between its initial docking position and the side of the work machine is usually not too large. Normally, the angle deviation between the initial docking position and the side of the work machine should be kept within 45°. If the angle deviation exceeds this range, it may indicate that the laser SLAM error of the mobile robot is too large to achieve effective docking. Therefore, in practical applications, the aforementioned preset angle range can be flexibly set according to the performance of the mobile robot SLAM technology to ensure the smooth progress of the docking process.

[0104] Furthermore, in this embodiment, the docking direction of the mobile robot is determined based on the screened point set, and a unique point set is selected based on a preset rule as a source point cloud of the right-angle side of the right-angle machine platform obtained by laser radar scanning, and the process of generating a corresponding target point cloud of the right-angle side of the right-angle machine platform also includes:

[0105] First, the point set with the most right-angled edges is selected as the source point cloud of the right-angled edges of the right-angled machine obtained by the LiDAR scan. This step is to filter out distant or irrelevant scanning data to ensure that the selected point set can accurately reflect the characteristics of the right-angled machine.

[0106] Next, the lengths of the two right-angled sides are calculated based on the currently selected point set, and a uniformly distributed right-angled side template is generated based on the lengths. The right-angled side template has a number of target points equal to the number of points in the selected point set, and these target points are evenly distributed.

[0107] Then, establish the coordinate system of the right-angle template. The origin of this coordinate system is located at the right angle of the right-angle side. The two right-angle sides correspond to the X-axis and the Y-axis respectively. This coordinate system is not only used as the coordinate system of the right-angle template, but also set as the coordinate system of the right-angle machine. Such a setting helps to unify the coordinate system and facilitate subsequent calculations and docking operations.

[0108] Finally, the coordinates of each target point of the right-angle edge template are obtained and used as the target point cloud of the right-angle edge. The right-angle edge template designed in this way can ensure that the two right-angle edges of the generated target point cloud match the right-angle edge lengths of the original point cloud, which is conducive to finding the correct corresponding point pairs in the subsequent ICP (Iterative Closest Point) algorithm and improving the convergence speed and accuracy of the algorithm.

[0109] In addition, in the process of selecting the source point cloud, the present invention also sets a series of judgment conditions to ensure that the selected point set is reasonable. These judgment conditions include: (1) the number of laser points contained in the right-angle edge is within the preset threshold interval; (2) the length of the right-angle edge is within the preset threshold interval; (3) the fitting slope of the right-angle edge is within the preset threshold interval (or its corresponding inclination angle, considering the deviation range of ±30°). Only the point set that meets these judgment conditions will be regarded as a reasonable right-angle edge point set and participate in the subsequent selection process.

[0110] Through the above method, the present invention can not only accurately determine the docking direction of the mobile robot, but also effectively select the source point cloud of the right-angle edge of the right-angle machine and generate the corresponding target point cloud. This provides reliable data support for the subsequent ICP algorithm and helps to achieve accurate docking between the mobile robot and the work machine.

[0111] In actual operation, the present invention obtains the identified posture of the mobile robot based on the source point cloud, the target point cloud and the external parameters of the lidar, and obtains the current predicted posture of the mobile robot using the speed information of the odometer. The current posture of the mobile robot is determined and published based on the reasonableness of the identified posture and the difference between it and the predicted posture.

[0112] Specifically, after successfully acquiring the source point cloud and target point cloud of the rectangular machine, the present invention uses the ICP algorithm to determine the precise position and posture of the laser radar in the rectangular coordinate system of the docking machine. In order to accelerate the matching process of the ICP algorithm, the present invention uses the centroid of the source point cloud as the initial matching value of the ICP algorithm. The advantages of this approach are reflected in the following two aspects, which help to significantly improve the matching efficiency:

[0113] (1) Accelerate convergence: The ICP algorithm is essentially an iterative process that aims to adjust the pose of two point clouds through continuous iteration until they reach the best alignment state. If the initial matching value is close to the true pose, the initial alignment state will be closer to the ideal result, thereby reducing the number of necessary iterations and significantly accelerating the convergence of the algorithm. This not only shortens the calculation time, but also effectively reduces the calculation cost of the algorithm in practical application scenarios.

[0114] (2) Avoiding local optimal solutions: The ICP algorithm may fall into a local optimal solution during the iteration process, especially when the initial alignment is not good. Providing an initial matching value close to the true pose enables the algorithm to start iterating from a position closer to the global optimal solution, thereby reducing the risk of falling into a local optimal solution and improving the accuracy of the final alignment result.

[0115] The basic principle of the ICP algorithm is to find a rotation matrix R and a translation matrix t, and transform the source point cloud through these two matrices so that the transformed source point cloud and the target point cloud overlap as much as possible. When calculating these two transformation matrices, the SVD (Singular Value Decomposition) method is a commonly used method. The SVD method can efficiently solve the optimal rotation matrix and translation matrix by decomposing the relevant matrix, thereby achieving accurate alignment between point clouds.

[0116] Furthermore, if Figure 4 As shown, in this embodiment, the recognized posture of the mobile robot is obtained according to the source point cloud, the target point cloud and the external parameters of the laser radar, and the current predicted posture of the mobile robot is obtained by using the speed information of the odometer. The process of determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture is reasonable includes:

[0117] Assume that the source point cloud and the target point cloud are point sets P = {P 1 ,P 2 ,······,P n} and Q = {Q 1 ,Q 2 ,······,Q n}, where n represents the number of points in the point set. For each point P i and Q i corresponds to a point in two-dimensional space, that is and Where P i is a point in the source point cloud, Q i is the nearest neighbor point in the corresponding target point cloud.

[0118] Calculate the center of mass μ of the source point cloud P and the centroid μ of the target point cloud Q , respectively:

[0119]

[0120] Subtract the centroid of each point in the point sets P and Q, and obtain the point sets P′ and Q′ after removing the centroid respectively:

[0121] P ′ = {P′ 1 =P 1 -μ P ,P′ 2 =P 2 -μ P ,······,P′ n =P n -μ P};

[0122] Q ′ = {Q′ 1 =Q 1 -μ Q ,Q′ 2 =Q 2 -μ Q ,······,Q′ n =Q n -μ Q}.

[0123] The point sets P′ and Q′ after removing the centroid are constructed into matrices [P′] and [Q′] respectively. The size of these two matrices is 2×n:

[0124] [P′]=[P′ 1 ,P′ 2 ,······,P′ n ];

[0125] [Q′]=[Q′ 1 ,Q′ 2 ,······,Q′ n ];

[0126] The covariance matrix [H] is calculated based on the obtained matrices [P′] and [Q′]:

[0127] [H]=[P′]·[Q′] T ;

[0128] Among them, the size of the covariance matrix [H] is 2×2, and each element in the covariance matrix [H] represents the covariance of the corresponding dimension between two point sets P′ and Q′; [Q′] T represents the transposed matrix of the matrix [Q′]. Specifically, the (1,1)th element in the covariance matrix [H] represents the covariance between the x-coordinate of the point set P′ and the x-coordinate of the point set Q′; the (1,2)th element represents the covariance between the x-coordinate of the point set P′ and the y-coordinate of the point set Q′; the (2,1)th element represents the covariance between the y-coordinate of the point set P′ and the x-coordinate of the point set Q′; the (2,2)th element represents the covariance between the y-coordinate of the point set P′ and the y-coordinate of the point set Q′. [Q′] T Represents the transposed matrix of the matrix [Q′].

[0129] Perform SVD decomposition on the covariance matrix [H]:

[0130] [H]=[U][Σ][V] T ;

[0131] Among them, [U] and [V] are orthogonal matrices obtained by decomposition, and [Σ] is a diagonal matrix obtained by decomposition.

[0132] Calculate the rotation matrix [R] and translation matrix [t] respectively:

[0133] [R]=[V][U] T ;

[0134] [t]=[μ Q ]-[R][μ P ].

[0135] After the optimal R and t are obtained through continuous iteration, the position and posture of the laser radar in the rectangular coordinate system of the docking station are obtained.

[0136] In addition, the position of the laser radar relative to the mobile robot is obtained based on the external parameters of the laser radar. And thus obtain the recognition posture of the mobile robot

[0137]

[0138] In view of the fact that the accuracy of right-angle edge posture recognition cannot be guaranteed to reach 100%, the present invention adopts the unscented Kalman filter (UKF) to fuse the mobile robot odometer information, which can enhance the robustness of right-angle edge posture recognition. UKF is a variant of Kalman filtering, which is based on the linear system state equation and combines the system input and output observation data to achieve the optimal estimation of the system state. Since the observation data contains noise and interference, the optimal estimation process can also be regarded as a kind of filtering. UKF is developed on the basis of Kalman filtering. It uses the lossless transformation (UT) technology to apply the Kalman filter originally applicable to linear assumptions to nonlinear systems. The specific process is as follows: Based on the posture solved at the previous frame moment, combined with the speed information provided by the odometer, the posture at the current moment is predicted by UKF. Subsequently, the predicted posture at the current moment is compared with the recognized posture at the current moment. If the difference between the two is less than a preset threshold (usually set to about 5 cm to prevent posture mutations caused by misidentification), the recognized posture is adopted. On the contrary, if the difference is greater than the threshold, the predicted posture is adopted.

[0139] Furthermore, if Figure 5 As shown, in this embodiment, the recognized posture of the mobile robot is obtained according to the source point cloud, the target point cloud and the external parameters of the laser radar, and the current predicted posture of the mobile robot is obtained by using the speed information of the odometer. The process of determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture is reasonable also includes:

[0140] Get the recognized position T of the mobile robot at time t t, if it is at the initial moment of docking, that is, t = 0, initialize a UKF pose estimator to obtain the pose of the mobile robot at the initial moment; wherein the UKF pose estimator is used to record the current time, position and pose information of the mobile robot;

[0141] Get the recognized position T of the mobile robot at time t+1 t+1 , and based on the recognized posture T at time t t , combined with the speed information of the odometer, the predicted position and posture of the mobile robot is obtained

[0142] Get the current recognition pose T t+1 and predicted pose Is the difference less than the preset difference threshold? If the recognized posture T t+1 and predicted pose If the difference is less than the preset difference threshold, the recognition pose T is released. t+1 ; Otherwise, publish the predicted pose

[0143] After obtaining the published posture, this embodiment performs navigation control according to the published posture to enable the mobile robot to dock with the right-angle machine.

[0144] Specifically, after obtaining the precise posture of the mobile robot in the rectangular coordinate system at the current moment (i.e., the docking starting point A), the running speed of the mobile robot is obtained by calculation, and this speed command is sent to the chassis actuator of the mobile robot to guide the mobile robot to move to the specified position in front of the docking machine at a right angle (i.e., the docking intermediate point B). Subsequently, the mobile robot remains stationary, continuously collects posture information within one second, and calculates the average value of this information to obtain more stable posture data. Next, the obtained average posture is compared with the final docking target point C to calculate the deviation of the mobile robot in the lateral, longitudinal and angular directions. Based on these deviation data, the mobile robot will execute a straight-line walking command. During the walking process, the mobile robot will continuously adjust its motion trajectory to eliminate lateral deviations and angular deviations. Finally, the mobile robot will accurately arrive at the preset docking target point.

[0145] Furthermore, if Figure 6 As shown, in this embodiment, according to the published posture, navigation control is performed to enable the mobile robot to dock with the right-angle machine platform, including:

[0146] At the docking starting point (A), the position information of the mobile robot in the rectangular coordinate system at the current moment is obtained according to the published position. The point diagram of the mobile robot navigation process is shown in Figure 7 shown.

[0147] According to the obtained posture information, the mobile robot is controlled to move to the preset position in front of the docking machine at right angles, that is, the docking middle point (B). The preset position is set to be about 30 cm horizontally from the front of the mobile robot to the machine's rectangular coordinate system, and about 5 cm vertically from the side edge of the mobile robot to the machine's rectangular coordinate system to meet the docking requirements.

[0148] When the mobile robot reaches the docking middle point (B), it remains stationary and collects posture information for a period of time. The posture information includes X, Y coordinates and Yaw angle, and the posture information is averaged to obtain more stable posture data.

[0149] The averaged pose information is compared with the final docking target point (C) to calculate the lateral deviation, longitudinal deviation and angular deviation. The lateral deviation represents the distance between the current position of the mobile robot and the docking target point in the horizontal direction (perpendicular to the forward direction of the mobile robot), the longitudinal deviation represents the distance between the current position of the mobile robot and the docking target point in the forward direction (parallel to the forward direction of the robot), and the angular deviation represents the angle between the current orientation of the mobile robot and the target orientation. It should be noted that the calculation of lateral, longitudinal and angular deviations is based on the rectangular machine coordinate system.

[0150] The mobile robot walks in a straight line (at this time, only a forward linear speed command is issued to make the mobile robot move in a straight line along its current direction.), and eliminates lateral deviation and angular deviation during walking (as the mobile robot continues to move forward, the longitudinal deviation will gradually decrease and eventually eliminate. During walking, we are more concerned about eliminating lateral deviation. When reaching the docking target point (C), lateral deviation, longitudinal deviation and angular error all need to be eliminated.). Finally, it reaches the set docking target point (C), at which time the mobile robot stops moving and prepares for subsequent loading and unloading operations.

[0151] Through the above steps, it can be ensured that the mobile robot reaches the target position accurately and stably during the docking process, providing strong support for subsequent automated operations.

[0152] In order to further illustrate the advantages of the present invention, it is described below in conjunction with specific experiments.

[0153] In order to evaluate the accuracy of a mobile robot in repeatedly reaching a specified point, such as Figure 8As shown in the figure, a QR code is pasted on the right front of the mobile robot (the QR code has a side length of 35mm, occupies about 140piexl in the image, and the pixel equivalent is 0.25mm / pixel), and a camera is installed about 50 cm above it. When the mobile robot reaches the docking target point (C), the camera is triggered to take a photo. Through the image processing algorithm, the pixel coordinates P1 (x1, y1) and P2 (x2, y2) of the left and right edges of the upper end of the QR code can be obtained. First, calculate the pixel coordinates of the midpoint P0 of P1 and P2. Then, using the pixel coordinates of P1 and P2, the rotation angle θ of the QR code in the image pixel coordinate system is calculated by the following formula: θ = atan (y1-y2) / (x2-x1) * 180 / PI, where atan represents the inverse tangent function and PI represents pi.

[0154] The accuracy of reaching the specified point using the original SLAM algorithm is shown in Table 1 below. After removing the 14th group of outliers, the position range of the mobile robot in the X direction is 88.5 pixels, and the position range in the Y direction is 69.5 pixels. Using the pixel equivalent of 0.25 mm / pixel for conversion, the actual position range in the X direction is 22.125 mm, and the actual position range in the Y direction is 17.375 mm. Therefore, the error of the mobile robot repeatedly reaching the target point is about ±11 mm (half of the range is taken as the error estimate).

[0155] Table 1

[0156]

[0157]

[0158] The accuracy of the method of the present invention in reaching the designated point is shown in Table 2 below. The position range of the mobile robot in the X direction is 16.5 pixels, and the position range in the Y direction is 22.5 pixels. Similarly, using the pixel equivalent of 0.25 mm / pixel for conversion, the actual position range in the X direction is 4.125 mm, and the actual position range in the Y direction is 5.625 mm. Therefore, the error of the mobile robot repeatedly reaching the target point is about ±3 mm (half of the range is also taken as the error estimate).

[0159] Table 2

[0160]

[0161]

[0162] In summary, the present invention proposes a right-angle machine docking method based on 2D laser radar, aiming to solve the problem that the traditional SLAM natural navigation method cannot meet the high-precision docking of the mobile robot and the working machine. This method does not require the additional deployment of reflectors, and only relies on the shape characteristics of the machine and the laser radar sensor to achieve high-precision lateral docking between the mobile robot and the working machine in narrow channel application scenarios, thereby improving space utilization. Compared with the traditional forward docking method, the lateral docking method supported by the present invention makes it possible to achieve precise docking in narrow channels with high docking accuracy (±3mm). In addition, the method can also automatically identify the left and right side machine docking of the mobile robot without the need to install additional visual sensors. The present invention realizes high-precision, low-cost, and intelligent docking of mobile robots and working machines through an innovative right-angle machine docking method, which has broad application prospects and practical value.

[0163] Embodiment 2

[0164] like Fig. 9 As shown, the present invention provides a right-angle machine docking device based on 2D laser radar, which is used to implement the right-angle machine docking method based on 2D laser radar in the above embodiment 1, specifically comprising:

[0165] The laser data preprocessing module is used to obtain the 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points of each frame of laser data, and filter out the point sets with the number of points within the preset range;

[0166] The source point cloud and target point cloud generation module is used to determine the docking direction of the mobile robot based on the screened point set, and select a unique point set as the source point cloud of the right-angle side of the right-angle machine platform obtained by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle side of the right-angle machine platform;

[0167] The pose determination and publishing module is used to obtain the recognized pose of the mobile robot based on the source point cloud, the target point cloud and the external parameters of the laser radar, and obtain the current predicted pose of the mobile robot using the speed information of the odometer. The current pose of the mobile robot is determined and published based on the difference between the recognized pose and the predicted pose.

[0168] The navigation control module is used to perform navigation control according to the published posture so that the mobile robot can dock with the right-angle machine.

[0169] A right-angle machine docking device based on 2D laser radar in this embodiment is used to implement the aforementioned right-angle machine docking method based on 2D laser radar. Therefore, the specific implementation method of the right-angle machine docking device based on 2D laser radar can be seen in the implementation example part of the right-angle machine docking method based on 2D laser radar in the previous text. In order to avoid redundancy, it will not be repeated here.

[0170] Embodiment 3

[0171] An embodiment of the present invention provides a computer storage medium, which stores a computer software product. The computer software product includes a number of instructions for enabling a computer device to execute the above-mentioned 2D laser radar-based right-angle machine docking method. In order to avoid redundancy, it will not be repeated here.

[0172] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0173] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0174] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide for implementing the process in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0175] Obviously, the above embodiments are merely examples for clear explanation and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived from them are still within the protection scope of the invention.

Claims

1. A right-angle machine docking method based on 2D laser radar, characterized in that: include: Acquire 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points for each frame of laser data, and filter out the point sets with point numbers within a preset range; Determine the docking direction of the mobile robot based on the screened point set, select a unique point set as the source point cloud of the right-angle edge of the right-angle machine platform acquired by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle edge of the right-angle machine platform; The recognized posture of the mobile robot is obtained according to the source point cloud, the target point cloud and the external parameters of the laser radar, and the current predicted posture of the mobile robot is obtained by using the speed information of the odometer, and the current posture of the mobile robot is determined and published based on the difference between the recognized posture and the predicted posture; According to the published posture, navigation control is performed to enable the mobile robot to dock with the right-angle machine.

2. The right-angle machine docking method based on 2D laser radar according to claim 1 is characterized in that: The real-time acquisition of 2D laser data scanned by the laser radar on the mobile robot, clustering of laser scanning points for each frame of laser data, and screening out point sets with point numbers within a preset range include: Acquire 2D laser data scanned by the LiDAR on the mobile robot in real time; Traverse each laser radar scanning point in the current frame in turn, obtain the coordinate value of each point in the laser coordinate system, and cluster the scanning points of the current frame according to the preset clustering distance threshold to obtain at least one point set; The clustered point sets are filtered to select the point sets with the number of points within the preset range. If the number of point sets filtered out in the current frame is 0, the laser data for the next frame is returned for processing.

3. The right-angle machine docking method based on 2D laser radar according to claim 2 is characterized in that: The process of clustering the scan points of the current frame according to the preset clustering distance threshold to obtain at least one point set includes: Start with an initial point to construct a point set; According to the order of the laser radar scanning points, the other laser points of the current frame are traversed in turn, and the distance between the current point and the previous point is calculated. If the distance between the current point and the previous point is less than the preset clustering distance threshold, the current point is grouped into the point set where the previous point is located; if the distance between the current point and the previous point is greater than or equal to the preset clustering distance threshold, a new point set is created and the current point is stored in the new point set until the last point is traversed, and finally a point set of all continuous points is obtained.

4. The right-angle machine docking method based on 2D laser radar according to claim 1 is characterized in that: The process of determining the docking direction of the mobile robot based on the screened point set, selecting a unique point set as a source point cloud of the right-angle side of the right-angle machine platform acquired by laser radar scanning based on a preset rule, and generating a corresponding target point cloud of the right-angle side of the right-angle machine platform includes: For each point set selected, obtain the point in the point set that is closest to the laser radar, and use this point as the boundary to divide the point set into two sub-point sets containing this point; if any sub-point set of the point set contains only one point, discard the point set and its corresponding two sub-point sets; A sub-point set is randomly selected from the two sub-point sets obtained, and the slope of the sub-point set is obtained by fitting using a random sampling consensus algorithm; The corresponding inclination angle is obtained according to the fitting slope. If the inclination angle is within the preset angle range, it is determined that the mobile robot docks with the right-angle machine in the first docking direction; otherwise, another sub-point set of the two sub-point sets is selected for fitting to obtain the inclination angle corresponding to the slope of the point set, and it is determined whether the inclination angle is within the preset angle range. If so, it is determined that the mobile robot docks with the right-angle machine in the second docking direction.

5. The right-angle machine docking method based on 2D laser radar according to claim 1 is characterized in that: The process of determining the docking direction of the mobile robot based on the screened point set, selecting a unique point set as a source point cloud of the right-angle side of the right-angle machine platform acquired by the laser radar scanning based on a preset rule, and generating a corresponding target point cloud of the right-angle side of the right-angle machine platform also includes: Select the point set with the most right-angled edges as the source point cloud of the right-angled edges of the right-angled machine platform obtained by the laser radar scanning; Calculating the lengths of two right-angled sides according to the currently selected point set, and generating a right-angled side template according to the lengths, wherein the right-angled side template has a number of target points equal to the number of points in the selected point set; A coordinate system of the right-angle edge template is established and used as the coordinate system of the right-angle machine, and the coordinates of each target point of the right-angle edge template are obtained and used as the target point cloud of the right-angle edge.

6. The right-angle machine docking method based on 2D laser radar according to claim 1, characterized in that: The process of obtaining the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, obtaining the current predicted posture of the mobile robot by using the speed information of the odometer, and determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture includes: Assume that the source point cloud and the target point cloud are point sets P = {P1, P2, ······, P n } and Q={Q1,Q2,······,Q n }, where n represents the number of points in the point set, for each point P i and Q i corresponds to a point in two-dimensional space, that is and Where P i is a point in the source point cloud, Q i is the nearest neighbor point in the corresponding target point cloud; Calculate the center of mass μ of the source point cloud P and the centroid μ of the target point cloud Q , respectively: Subtract the centroids of each point in the point sets P and Q to obtain the point set P after removing the centroids ′ and Q ′ They are: P′={P′1=P1-μ P ,P′2=P2-μ P ,……,P′ n =P n -μ P }; Q′={Q′1=Q1-μ Q ,Q′2=Q2-μ Q ,……,Q′ n =Q n -μ Q }; The point sets P′ and Q′ after removing the centroid are constructed into matrices [P′] and [Q′] respectively. The size of these two matrices is 2×n: [P′]=[P′1,P′2,……,P′ n ]; [Q′]=[Q′1Q′2,……,Q′ n ]; The covariance matrix [H] is calculated based on the obtained matrices [P′] and [Q′]: [H]=[P′]·[Q′] T ; Among them, the size of the covariance matrix [H] is 2×2, and each element in the covariance matrix [H] represents the covariance of the corresponding dimension between two point sets P′ and Q′; [Q′] T represents the transposed matrix of the matrix [Q′]; Perform SVD decomposition on the covariance matrix [H]: [H]=[U][∑][V] T ; Wherein, [U] and [V] are orthogonal matrices obtained by decomposition, and [Σ] is a diagonal matrix obtained by decomposition; Calculate the rotation matrix [R] and translation matrix [t] respectively: [R]=[V][U] T ; [t]=[μ Q ]-[R][μ P ]; After the optimal R and t are obtained through continuous iteration, the position and posture of the laser radar in the rectangular coordinate system of the docking station are obtained. Obtain the position of the laser radar relative to the mobile robot based on the external parameters of the laser radar And thus obtain the recognition posture of the mobile robot 7. The right-angle machine docking method based on 2D laser radar according to claim 1, characterized in that: The process of obtaining the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, obtaining the current predicted posture of the mobile robot by using the speed information of the odometer, and determining and publishing the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture being reasonable also includes: Get the recognized position T of the mobile robot at time t t , if it is at the initial moment of docking, that is, t=0, initialize a UKF pose estimator to obtain the pose of the mobile robot at the initial moment; the UKF pose estimator is used to record the current time, position and pose information of the mobile robot; Get the recognized position T of the mobile robot at time t+1 t+1 , and based on the recognized posture T at time t t , combined with the speed information of the odometer, the predicted position and posture of the mobile robot is obtained Get the current recognition pose T t+1 and predicted pose Is the difference less than the preset difference threshold? If the recognized posture T t+1 and predicted pose If the difference is less than the preset difference threshold, the recognition pose T is released. t+1 ; Otherwise, publish the predicted pose 8. The right-angle machine docking method based on 2D laser radar according to claim 1, characterized in that: The process of performing navigation control according to the published posture to enable the mobile robot to dock with the right-angle machine includes: At the docking starting point, the current position and posture information of the mobile robot in the rectangular coordinate system is obtained according to the published position and posture; According to the obtained posture information, the mobile robot is controlled to move to a preset position in front of the docking machine at a right angle, that is, the docking middle point; When the mobile robot reaches the docking intermediate point, it remains stationary and collects posture information for a period of time, the posture information including X, Y coordinates and Yaw angle, and performs average processing on the posture information; The averaged posture information is compared with the final docking target point to calculate the lateral deviation, longitudinal deviation and angular deviation, wherein the lateral deviation represents the distance between the current position of the mobile robot and the docking target point in the horizontal direction, the longitudinal deviation represents the distance between the current position of the mobile robot and the docking target point in the forward direction, and the angular deviation represents the angle between the current orientation of the mobile robot and the target orientation; The mobile robot walks in a straight line, eliminating lateral deviation and angular deviation during the walking process, and finally reaches the set docking target point.

9. A right-angle machine docking device based on 2D laser radar, characterized in that: The device is used to implement the right-angle machine docking method based on 2D laser radar according to any one of claims 1 to 8, specifically comprising: The laser data preprocessing module is used to obtain the 2D laser data scanned by the laser radar on the mobile robot in real time, cluster the laser scanning points of each frame of laser data, and filter out the point sets with the number of points within the preset range; The source point cloud and target point cloud generation module is used to determine the docking direction of the mobile robot based on the screened point set, and select a unique point set as the source point cloud of the right-angle side of the right-angle machine platform obtained by the laser radar scanning based on the preset rules, and generate the corresponding target point cloud of the right-angle side of the right-angle machine platform; A posture determination and publishing module is used to obtain the recognized posture of the mobile robot according to the source point cloud, the target point cloud and the external parameters of the laser radar, and obtain the current predicted posture of the mobile robot using the speed information of the odometer, and determine and publish the current posture of the mobile robot based on the difference between the recognized posture and the predicted posture; The navigation control module is used to perform navigation control according to the published posture so that the mobile robot can dock with the right-angle machine.

10. A computer storage medium, characterized in that: The computer storage medium stores a computer software product, which includes a number of instructions for enabling a computer device to execute the 2D laser radar-based right-angle machine docking method described in any one of claims 1 to 8.

Images