Mobile robot tracking and positioning method based on multi-lidar point cloud on the ground

Abstract

The present invention provides a mobile robot tracking and positioning method based on multi-laser point cloud fusion on the ground side, including point cloud tracking and point cloud positioning. In the point cloud tracking stage, the Kalman filter method is used to track the target point cloud of the robot cluster in the scene, a Kalman filter model is established, the target point cloud is searched according to the depth-first strategy, the intersection-over-union method is used for correlation matching, the result with the maximum correlation is used as the target point cloud of the current frame, and the Kalman filter model parameters are updated to achieve real-time tracking and rough positioning of the target point cloud. In the point cloud positioning stage, a grid probability model of the template robot is established in an offline manner, and the real-time point cloud of the robot when it is running online is matched with the grid probability model to improve the positioning accuracy, and complete the real-time precise positioning of the robot. The mobile robot tracking and positioning method designed by the present invention effectively improves the tracking robustness and positioning accuracy of the robot's target point cloud.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning technology, and in particular to a mobile robot tracking and positioning method based on a multi-laser radar point cloud on a ground side. Background Art

[0002] LiDAR is an active modern optical sensor that measures the flight time of a laser beam from emission to reflection and then to reception to achieve the purpose of accurately measuring distance. Thanks to the characteristics of laser such as high brightness, high directivity, high monochromaticity and high coherence, LiDAR can provide a large amount of accurate distance information at a high frequency under conditions of poor ambient light, making LiDAR have an outstanding performance in the field of environmental perception. In recent years, with the expansion of the industry scale and the development of technology, the production cost of LiDAR has been reduced, and the measurement accuracy has been further improved. It has been favored by more and more researchers and has been widely used in robot navigation, autonomous driving and industrial applications.

[0003] Positioning and tracking technology is widely considered to be one of the key technologies for mobile robots to achieve autonomous mobility, and its positioning accuracy will directly affect the working ability of mobile robots. LiDAR has the advantages of high acquisition frequency and high measurement accuracy, so it has high research value in robot positioning. The data set measured by the LiDAR will be communicated in the form of point cloud. The position and shape information of objects around the LiDAR can be accurately obtained through the laser point cloud. Based on the above principles, the processing of point clouds can be used to achieve target tracking and positioning in dynamic scenes. Among them, the tracking of target point clouds is the technical prerequisite for achieving positioning. Point cloud target tracking is the process of determining the continuous position of the identified target in the laser point cloud at different times by the tracker. After the target point cloud tracking work is completed, the target point cloud is matched to obtain the real-time posture of the mobile robot during the entire continuous motion process, thereby completing the positioning of the mobile robot.

[0004] At present, the mobile robot tracking and positioning method based on LiDAR point cloud has high computational complexity, poor tracking robustness and positioning real-time performance. For example, a 2D LiDAR positioning and mapping method proposed in the Chinese invention patent with publication number CN116165627A directly traverses the stored historical frames within a certain period of time for loop detection to optimize the cumulative error after positioning the robot posture. However, the algorithm complexity of continuously traversing the historical frames is high, which requires more computational cost and has a certain impact on the real-time performance of robot positioning. A planar mapping and positioning method based on 2D LiDAR is proposed in the Chinese invention patent with publication number CN112684468A. This method uses LiDAR to collect the coordinates of the feature points of the reflective column and calculates the current coordinates of the robot through the three-point positioning method. The final result is not compensated and optimized in combination with other methods, and the robustness of the positioning result is poor. Summary of the invention

[0005] In order to solve the problems of the prior art, the present invention provides a mobile robot tracking and positioning method based on ground-side multi-lidar point cloud, which effectively improves the tracking robustness and positioning accuracy of the robot's target point cloud.

[0006] The method proposed in the present invention is divided into two stages: point cloud tracking and point cloud positioning. In the point cloud tracking stage, the Kalman filter method is used to track the target point cloud of the robot cluster in the scene. First, a bounding box set is established for the point cloud after hierarchical clustering. Secondly, a Kalman filter model is established for the target point cloud tracking frame, and the optimal estimate of the previous frame tracking frame is compared with the current frame bounding box, and the target point cloud is searched through depth-first search. Thirdly, the intersection-over-union method is used for correlation matching, and the result with the maximum correlation is used as the target point cloud of the current frame. Finally, the Kalman filter model parameters are updated to complete the tracking stage of the target point cloud at the current moment.

[0007] In the point cloud positioning stage, offline template creation and online real-time positioning are performed respectively. First, the point cloud coordinate system and grid coordinate system, as well as the transformation model between the two coordinate systems, are defined. In the point cloud coordinate system, the geometric contour of the robot target point cloud is fitted, and the robot pose is calculated according to the geometric center. Secondly, an offline template is created. In the point cloud coordinate system, the reference absolute pose of the robot is calculated according to the geometric contour of the template target point cloud. Then, the target point cloud of the template robot is transformed to the grid coordinate system, and the prior probability grid model of the template robot is constructed in the grid coordinate system. Thirdly, the real-time precise positioning of the robot in the online stage is performed. In the point cloud coordinate system, the real-time pose of the robot is calculated according to the geometric contour of the target point cloud of the current frame, which is used as the initial value of the template matching optimization. Then, the target point cloud of the current frame is converted to the real-time probability grid model in the grid coordinate system. In the grid coordinate system, the optimization model of the robot's real-time relative pose is constructed through the template matching optimization of the model and the prior probability grid model, and the optimization value of the robot's real-time pose is obtained through LM nonlinear optimization.

[0008] The present invention is mainly achieved through the following technical solutions:

[0009] Step 1: LiDAR deployment and point cloud preprocessing: Deploy multi-source LiDAR sensors in the working scene, collect multi-source laser point cloud data for the template robot in the offline stage or the target robot in the online stage, set indexes according to different radar sources, merge laser point clouds from different radars into the same coordinate system to form a target point cloud; convert the target point cloud into an AABB bounding box set through hierarchical clustering, build a spatial hierarchical tree with the bounding box as the node, and judge whether the laser point cloud in the bounding box comes from the same radar, that is, whether it is the same source, according to different radar indexes;

[0010] Furthermore, the laser radar deployment and point cloud preprocessing includes the following steps:

[0011] Step 1.1: LiDAR deployment: deploy two or more ground-based LiDAR sensors at the corners of the work area. Each LiDAR faces the work area for scanning and measurement.

[0012] Step 1.2: Target point cloud acquisition: When in the offline stage, move the template robot to the reference position of the working area; when in the online stage, run the target robot in the working scene; use multi-source laser radar to obtain the robot's point cloud data, and set the radar index according to the laser radar from which the point cloud data comes; transform the point cloud data obtained by different laser radars into the same point cloud coordinate system through coordinate transformation to form the target point cloud;

[0013] Step 1.3: Establishment of target point cloud bounding box set: Perform hierarchical clustering on the target point cloud and construct point cloud clusters P with inclusion or independence relationships through bounding boxes. i , with point cloud cluster P i Construct a spatial hierarchical tree for the nodes; the bounding box parameters of the target point cloud are defined as O i {P i , l i , w i , Cx i ,cy i , b}, where P i is the point cloud cluster after segmentation and clustering, l i is the length of the target point cloud bounding box, w i Cx is the width of the target point cloud bounding box. i is the x-axis coordinate of the center of the target point cloud bounding box, cy i is the y-axis coordinate of the center of the target point cloud bounding box, b represents the Boolean value of whether the point cloud data in the bounding box is of the same source; and P i ={p i1 (x i1 ,y i1 ), ...., p ik (x ik ,y ik )}, then we have l i =max{x ik}-min{x ik},w i =max{y ik}-min{y ik}, cx i =(max{x ik}+min{x ik}) / 2,cy i =(max{y ik}+min{yik}) / 2, where k represents the point cloud cluster P i There are k points in total;

[0014] Step 2: Kalman filter point cloud tracking: Use an auxiliary positioning device or manual method to roughly locate the target bounding box of the template robot or the target robot, use the target bounding box to initialize the tracking frame and establish the Kalman filter model, and use the initial tracking frame parameters as the optimal value of the tracking frame of the initial frame; when performing laser point cloud tracking, adopt a depth-first search strategy, and match the previous frame tracking frame with the bounding box in the current frame hierarchy tree through the intersection-and-union method, and determine whether the target point cloud is searched based on the matching results; if the target point cloud is not searched, use the optimal value of the parameter of the previous frame tracking frame in the prediction equation to calculate the optimal value of the parameter of the current frame tracking frame; if the target point cloud is searched, use the Kalman filter gain coefficient as the weight, comprehensively consider the predicted value of the tracking frame parameter, the observed value and the absolute true value of the proximity, and perform weighted summation of the predicted value and the observed value of the tracking frame parameter as the final optimal value of the tracking frame parameter; and use the optimal value of the tracking frame parameter of the current frame to update the Kalman filter model parameters;

[0015] Furthermore, the Kalman filter point cloud tracking includes the following steps:

[0016] Step 2.1: Kalman filter model establishment: Set the tracking box parameters of the Kalman filter model according to the bounding box parameters of the target point cloud:

[0017]

[0018] Where x is the horizontal coordinate of the center point of the tracking frame, y is the vertical coordinate of the center point of the tracking frame, w is the width of the tracking frame, h is the height of the tracking frame, dx is the change in the horizontal coordinate of the center point of the tracking frame, and dy is the change in the vertical coordinate of the center point of the tracking frame;

[0019] Step 2.2: Kalman filter model initialization: In the initial stage, use auxiliary positioning devices or manual methods to roughly locate the target point cloud bounding box of the template robot or the target robot, and use the bounding box parameters in the initial stage as the optimal value of the tracking box of the initial frame.

[0020] Step 2.3: Depth-first search of target point cloud: When performing laser point cloud tracking, a depth-first search strategy is used on the spatial hierarchy tree to traverse the bounding box nodes of each layer from top to bottom, and the optimal estimate of the tracking box of the previous frame is Perform correlation matching with the bounding box of the current layer of the spatial hierarchy tree; if the tracking box and the bounding box have an intersection, continue to traverse downward until the leaf node; if the tracking box and the bounding box do not have an intersection, stop traversing; when traversing to the leaf node at the bottom layer, calculate the intersection over union (IoU) value of the bounding box in the leaf node and the tracking box; if the maximum IoU value of all leaf nodes is greater than the set threshold, the point cloud cluster in the bounding box corresponding to the maximum IoU is taken as the target point cloud, and proceed to step 2.4; if the maximum IoU value is less than the set threshold, it is considered that the target point cloud is not found, and proceed to step 2.5;

[0021] Step 2.4: Update the Kalman filter of the target point cloud: set the tracking box measurement value of the current frame according to the target point cloud bounding box parameters Substitute it into the Kalman filter target observation equation to calculate the tracking frame observation value Z k :

[0022]

[0023] Among them, H is the state observation matrix, v k is the observation noise;

[0024] Use the optimal value of the tracking box parameters of the previous frame Calculate the predicted value of the tracking box parameters of the current frame Using the error covariance matrix P of the previous frame k-1 Calculate the error covariance matrix prediction value of the current frame

[0025]

[0026] Where A is the state transfer matrix, Q is the process noise matrix;

[0027] Predict the value based on the error covariance matrix of the current frame Calculate the Kalman filter gain coefficient K of the current frame k :

[0028]

[0029] Where R is the measurement noise matrix;

[0030] The weighted sum of the measured value and the predicted value of the tracking box is used as the optimal value of the parameter of the tracking box

[0031]

[0032] Predict the value based on the error covariance matrix of the current frame Calculate the optimal value P of the error covariance matrix k :

[0033]

[0034] Where I is the identity matrix; go to step 2.6;

[0035] Step 2.5: Kalman filter update for target point cloud not found: set the optimal value of the tracking box parameters of the previous frame Substitute the state prediction equation to calculate the optimal value of the tracking box parameters of the current frame

[0036]

[0037] Step 2.6: Determine whether the laser point cloud tracking is completed. If the tracking is completed, the process terminates, otherwise returns to step 2.3;

[0038] Step 3: Coarse positioning of target point cloud pose estimation: In the point cloud coordinate system, classify the point cloud bounding box of the template robot or the target robot; when the bounding box point cloud data is a homologous point cloud, classify the target point cloud as an "L" type or "I" type point cloud. If it is an "L" type, use the rotating caliper method to fit the minimum area rectangle of the target point cloud; when the bounding box point cloud data is a non-homologous point cloud or an "I" type homologous point cloud, fit the minimum area rectangle of the target point cloud through convex hull point set extraction and rotating caliper method; and use the minimum area rectangle to calculate the central pose vector of the target point cloud to achieve coarse positioning of the robot;

[0039] Furthermore, the target point cloud pose estimation coarse positioning includes the following steps:

[0040] Step 3.1: Target point cloud classification: For the bounding box of the template robot or the target robot, the target point cloud is divided into homologous point cloud and non-homologous point cloud according to whether it is homologous; if the target point cloud is non-homologous point cloud, proceed to step 3.2; if the target point cloud is homologous point cloud, the homologous point cloud is divided into "L" type and "I" type point cloud: using the sequentiality of homologous point cloud data, extract the first and last endpoints, construct the straight line equation of the endpoint connection, and calculate the maximum distance value from the middle point to the endpoint connection; if the maximum distance value is greater than the set threshold, the point cloud is considered to be "L" type, and the convex hull point set is constructed with the middle point and the two end points of the maximum distance value, and proceed to step 3.3; if the maximum distance value is less than the set threshold, the point cloud is considered to be "I" type, and proceed to step 3.2;

[0041] Step 3.2: Convex hull point set extraction: Initialize the convex hull point set to be empty, sort the target point cloud from small to large according to the x coordinate, if the x values ​​are the same, sort them from small to large according to the y values, and construct the target point cloud sequence; traverse each point in the target point cloud sequence, if the number of points in the convex hull point set is not greater than 2, then add the current point to the convex hull point set; if the number of points in the convex hull point set is greater than 2, then take out the last two points in the convex hull point set as the basic points, and judge whether the current point and the basic points can construct a convex polygon: taking the clockwise construction as an example, if the current point is in the clockwise direction of the basic point composition vector, then add the current point to the convex hull point set; otherwise, backtrack, delete the last point in the convex hull point set, and then take out the last two points in the convex hull point set as the basic points to judge the convex polygon construction; repeat the above process until the current point and the last two points can form a convex polygon or continue to delete until only one point remains in the convex hull point set, and then add the current point to the convex hull point set;

[0042] Step 3.3: Rotating caliper method to fit the minimum area rectangle of the target point cloud: Use the convex hull point set of the target point cloud to construct a convex polygon, start from a vertex of the convex polygon and traverse it through the sliding window method, take the above vertex and its adjacent vertex in the clockwise direction to construct a straight line, record it as the main parallel line; traverse the other vertices of the convex hull point set, find the vertex with the farthest vertical distance from the main parallel line, and make a parallel line of the main parallel line through the above vertex, record it as the secondary parallel line; construct the minimum enclosing rectangle of the convex polygon between the main and secondary parallel lines, and record the area of ​​the rectangle and the coordinates of its endpoints; rotate in a clockwise direction through the sliding window method, select the vertex and its adjacent vertices in the clockwise direction to construct the main and secondary parallel lines in turn, repeat the above minimum enclosing rectangle construction, and update the minimum rectangle area and its endpoint coordinates; after traversing all the vertices of the convex polygon, take the last updated minimum area rectangle as the fitting result, and use the minimum area rectangle to calculate the central pose vector of the target point cloud;

[0043] Step 4: Offline stage template robot probability model construction: In the offline stage, similarity transformation is used to transform the target point cloud of the template robot from the point cloud coordinate system to the grid coordinate system. The Bresenham line drawing algorithm is used to update the grid probability during multiple sampling processes, and the prior probability grid model of the template robot is constructed in the form of a grid probability matrix.

[0044] Furthermore, the offline stage template robot probability model construction includes the following steps:

[0045] Step 4.1: Obtaining the point cloud center pose vector: Use the target point cloud coarse positioning method to obtain the point cloud center pose vector of the template robot, denoted as p xt , p yt , are the center coordinates and attitude angle of the minimum area rectangle of the template robot in the point cloud coordinate system;

[0046] Step 4.2: Initialize the probability grid matrix: Describe the robot probability model in the form of a probability grid matrix. Let n be the grid matrix dimension, s be the grid resolution size, and A n×n A probability grid matrix for storing grid probabilities;

[0047] Step 4.3: Point cloud data coordinate system transformation: Let the coordinates of the template robot point cloud in the world coordinate system be P w =(x w ,y w ) T , the coordinate in the grid coordinate system is P s =(x s ,y s ) T , use similarity transformation to transform the template robot point cloud from the point cloud coordinate system to the grid coordinate system:

[0048]

[0049] Among them, the similarity transformation matrix T in two-dimensional space s for:

[0050]

[0051] Where s is the scaling factor, R 2×2 is a two-dimensional rotation matrix, t 2×1 is a two-dimensional translation vector:

[0052]

[0053] Step 4.4: Grid probability update: Set the number of grid probability updates m. In the kth (1≤k≤m) update, for each point in the target point cloud, determine the radar from which each point comes according to the radar index, use the Bresenham line drawing algorithm to connect each point with the origin of the radar from which it comes to form a line segment, and calculate the grid probability matrix A. n×n Each grid a ij The probability value P in the kth update k (a ij ), if the grid is a line segment grid, then the grid probability value P k (a ij )=0; if the grid is a grid where the line segment ends, the grid probability value P k (a ij )=1; otherwise the grid probability value P k (a ij )=0.5; after m updates, the final grid probability value P(a ij )for:

[0054]

[0055] Step 4.5: Prior probability grid model construction: Use the Kalman filter tracking model to track the template robot’s point cloud, and continuously update the grid probability matrix until the change in each grid probability value in the grid probability matrix is ​​less than the set threshold, thereby constructing a prior probability grid model.

[0056] Step 5: Real-time precise positioning of the target robot in the online stage: In the online stage, the target point cloud of the target robot in the current frame is converted from the global point cloud coordinate system to the local grid coordinate system through two steps of rigid body transformation and similarity transformation, and the real-time probabilistic grid model of the target robot is constructed by bilinear interpolation. The error model of the real-time relative posture of the target robot is constructed through model matching optimization between the real-time probabilistic grid model and the prior probabilistic grid model, and the grid probability value in the error model is recursively iterated using nonlinear optimization to find the optimal value of the real-time relative posture, thereby obtaining the real-time absolute posture of the target robot and realizing real-time precise positioning of the robot.

[0057] Furthermore, the real-time precise positioning of the target robot in the online stage includes the following steps:

[0058] Step 5.1: Get the point cloud center pose vector: Use the target point cloud coarse positioning to get the point cloud center pose vector of the target robot, denoted as The relative pose vector of the target robot and the template robot is ξ t is the template point cloud pose estimation vector;

[0059] Step 5.2: Grid coordinate system point cloud projection: Let the coordinates of the target robot in the global point cloud coordinate system be p i =(x i ,y i ), first through the rigid body transformation Transformed to the local point cloud coordinate system of the template robot, the resulting coordinates T i (ξ r )for:

[0060]

[0061] Then through the similarity transformation T s The coordinates T of the target robot in the local point cloud coordinate system i (ξ r ) is transformed into the coordinate P in the local grid coordinates i (ξ r ):

[0062]

[0063] Using the above rigid body transformation ξ r and similarity transformation T s The projection point of the target point cloud in the local grid coordinate system is obtained as P(x, y), and the grid probability value Odds(P(x, y)) of the above projection point is calculated by bilinear interpolation method:

[0064]

[0065] Among them, point Q 11 (x1, y1), Q 12 (x1, y2), Q 21 (x2, y1), Q 22 (x2, y2) are the midpoint coordinates of the four neighboring grids of the projection point P in the grid coordinate system;

[0066] The directional gradient of the grid probability value Odds(P(x, y)) on the coordinate axis is approximately:

[0067]

[0068] The bilinear interpolation method is used to calculate the grid probability values ​​Odds(P(x, y)) of all projection points of the target point cloud, and a real-time probability grid model of the target robot is constructed in the form of a grid probability matrix.

[0069] Step 5.3: LM nonlinear optimization model construction: Let ξ r0 The optimal relative pose vector for the real-time probability grid model and the prior probability grid model to achieve an ideal alignment state:

[0070] ξ r0 =ξ r +Δξ r (19)

[0071] where Δξ r is the relative posture error; the optimal value of the relative posture error Δξ r * The process is transformed into a nonlinear least squares problem, and a probabilistic grid coincidence error model of the target robot's real-time relative posture is constructed:

[0072]

[0073] Assuming λ is the damping coefficient, equation (19) can be simplified to:

[0074]

[0075] The matrix H i , vector g i They are:

[0076]

[0077] Step 5.4: Solving the LM nonlinear optimization model: Set the number of iterations and the minimum relative pose error threshold as the convergence condition, with ξ r0 =ξ r As the initial value of the iteration, calculate the point cloud coordinates T obtained after the target point cloud undergoes rigid body transformation and similarity transformation i (ξ r ), grid coordinate P i (ξ r ) and grid probability values ​​Odds(P i (ξ r )), by calculating the matrix H i , vector g i Get the relative posture error value Δξ of the current step r , to determine whether the iterative optimization has reached the convergence condition: If the iterative optimization has not converged, the relative posture error value Δξ is used r The target point cloud is transformed, calculated and judged iteratively; if the iterative optimization has converged, the optimal value of the relative pose error Δξ is output r * , thereby calculating the precise pose vector ξ of the target point cloud p , which is the real-time accurate position and posture of the target robot:

[0078] ξ p =ξ t -(ξ r +Δξ r * ) (twenty four)

[0079] The beneficial effects of the present invention are:

[0080] 1. In order to solve the problem that searching for target point cloud requires high computational cost during point cloud tracking, a deep search strategy is used to search for target point cloud in the bounding box set containing the point cloud cluster, and the intersection-and-union ratio is used to obtain the result with maximum correlation as the target point cloud of the current frame.

[0081] 2. Compared with the direct traversal search method, the deep search method has low time complexity, effectively improving the search efficiency of the target point cloud, and improving the real-time performance of point cloud tracking after combining with the Kalman filter algorithm. At the same time, it addresses the accuracy and robustness issues of point cloud positioning results.

[0082] 3. The robot pose obtained by fitting the geometric center of the rectangle is used as the initial value for optimization. The bilinear interpolation and LM nonlinear optimization method are used to align the current target point cloud with the prior probability grid model, thereby optimizing the robot pose estimation error and improving the overall positioning accuracy.

[0083] 4. Multi-source LiDAR sensors are deployed on the ground. Compared with the traditional sensors deployed on the robot, the number of LiDARs deployed is reduced, which effectively reduces the deployment and maintenance costs of the robot positioning system while ensuring a certain positioning accuracy. At the same time, ground deployment can provide a more stable platform, reduce disturbances caused by robot movement or vibration, help obtain more accurate data, and further improve the accuracy of positioning results.

[0084] In summary, the mobile robot tracking and positioning method designed in the present invention effectively improves the robustness, real-time performance and positioning accuracy of the robot's target point cloud tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0086] Figure 1 This is a flow chart of a mobile robot tracking and positioning method based on multi-laser radar point cloud on the ground side of the present invention;

[0087] Figure 2 Kalman filter tracking flow chart for target point cloud;

[0088] Figure 3 This is a schematic diagram of the depth-first traversal search strategy;

[0089] Figure 4 This is a schematic diagram of fitting the minimum area rectangle using the rotating caliper method;

[0090] Figure 5 Schematic diagram of the robot point cloud prior probability model. DETAILED DESCRIPTION

[0091] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only 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.

[0092] like Figure 1 As shown, the mobile robot tracking and positioning method based on the ground-side multi-lidar point cloud of the present invention specifically includes the following steps:

[0093] First, deploy multi-source LiDAR sensors on the ground side of the working scene to obtain multi-source laser point cloud data; then use the Kalman filter method to track the point cloud: establish a bounding box set for the hierarchical clustered point cloud, initialize the tracking box information and establish a Kalman filter model for it. Use the intersection-and-union method to match the current frame bounding box set with the previous frame tracking box point cloud, obtain the current frame target point cloud bounding box, and update the Kalman filter model.

[0094] Secondly, the target point cloud pose estimation and rough positioning are performed: the geometric contour of the robot target point cloud in the point cloud coordinate system is fitted, the robot pose is calculated according to the geometric center, and the target point cloud is roughly positioned.

[0095] Then, the template is created in the offline stage: the robot is deployed to a static position, and the reference absolute pose of the robot in the point cloud coordinate system is obtained using the rotating caliper method. A grid coordinate system is established and a priori probability grid model of the template robot is constructed in it, and the obtained target point cloud probability value is stored as a text file.

[0096] Finally, the real-time precise positioning of the robot is performed in the online stage: the real-time posture of the robot is obtained using the rotating caliper method as the initial value for the current frame model matching optimization. The target point cloud of the current frame is converted into a real-time probabilistic grid model in grid coordinates, and the optimization model of the robot's real-time relative posture is constructed through template matching optimization of this model and the prior probabilistic grid model in the grid coordinate system, and the optimization value of the robot's real-time posture is obtained through LM nonlinear optimization.

[0097] like Figure 2 As shown, the Kalman filter tracking process of the target point cloud includes the following steps:

[0098] First, the moving robot target is initially positioned by manual setting or auxiliary positioning device, so as to initialize the robot tracking frame. A Kalman filter model is established for the initialized target tracking frame. The initialized target tracking frame is used as the optimal value of the target tracking frame of the current frame. Prepare for the next frame target point cloud search.

[0099] Next, search for the current frame target point cloud to see if it exists. Use the optimal value of the previous frame target tracking box A deep search strategy is used in the current frame spatial hierarchy tree to query the target point cloud and the next step is determined based on the search results.

[0100] Finally, if the target point cloud exists, the target point cloud bounding box information is extracted as the target tracking box measurement value Z of the current frame k , and use the previous frame tracking frame state information to predict the current frame tracking frame state information, substitute the previous frame tracking frame parameters into the state variable prediction equation and the error covariance prediction equation to obtain the predicted state variables and the forecast error covariance Then calculate the Kalman filter gain coefficient K k , and finally update the current frame state variable and error covariance P k ; If the target point cloud does not exist, Substitute the state variable prediction equation to update Repeat the above steps until the tracking process is completed.

[0101] like Figure 3 As shown, the depth-first traversal search strategy mainly includes the following steps:

[0102] First, a depth-first traversal search is performed on the spatial hierarchical tree of the hierarchical clustered point cloud from top to bottom, and the nodes in the current layer are compared with the optimal estimated value of the tracking box in the previous frame. Perform an intersection test. If the two subsets have no intersection, stop traversing. If the two subsets have an intersection, continue traversing downward until the leaf node.

[0103] Secondly, the IoU value between the point cloud cluster bounding box and the tracking box is calculated for the leaf nodes traversed. It is determined whether the maximum IoU value is greater than the set threshold. If it is greater than the set threshold, the next step is performed.

[0104] Finally, the point cloud in the bounding box whose IoU value meets the requirements is taken as the target point cloud, and the target point cloud bounding box information is extracted as the target tracking box measurement value of the current frame.

[0105] like Figure 4 As shown, the rotating caliper method for fitting the minimum area rectangle mainly includes the following steps:

[0106] First, the initial main and secondary parallel lines are established: the initial main parallel line is set as the line that coincides with the first two points of the convex hull extreme point set. By traversing the convex hull extreme point set, the maximum projection distance in the direction perpendicular to the initial main parallel line is calculated, and the line passing through its corresponding point and parallel to the main parallel line is set as the initial secondary parallel line.

[0107] Secondly, the main parallel line is rotated clockwise around the extreme point of the convex hull by the sliding window method, and the secondary parallel line is rotated following the main parallel line. After the rotation, the coordinates of each end point of the rectangle are calculated.

[0108] Finally, the area of ​​each rectangle is calculated according to the endpoint coordinates, the minimum area matrix is ​​compared, and the orientation vector of the minimum area rectangle is obtained by solving its endpoint coordinates.

[0109] like Figure 5 As shown in FIG. 1 , the method for establishing a priori probability model of robot point cloud mainly includes the following steps:

[0110] First, initialize the probability grid matrix: set the parameter grid matrix dimension n, grid resolution s, and the two-dimensional square array container A that stores the grid probability. n×n . Preset grid occupancy probability Odds (Q m )=0.5, update the occupied frequency value P occ = +0.1 and update the idle probability value P free =-0.1.

[0111] Secondly, the point cloud data of the robot target contour in a stationary state is extracted, and the radar index of each point cloud is recorded.

[0112] The point cloud data is then converted to a grid coordinate system, and the Bresenham line drawing algorithm is used to draw lines from each radar far point to the point cloud coordinates based on the radar index and update the grid probability value. The grids along the line segment are updated with idle probability values, and the grids hit by the line segment are updated with occupied probability values.

[0113] Finally, the target contour point cloud data is extracted and the radar index is recorded repeatedly until the grid probability state is stable.

[0114] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, the above is only a preferred embodiment of the present invention. Since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field within the technical scope disclosed by the present invention, for ordinary technicians in the technical field, can easily think of changes or replacements, without departing from the principle of the present invention, should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A mobile robot tracking and positioning method based on multi-laser point cloud fusion on the ground side, characterized by: The following steps are involved: Step 1: LiDAR deployment and point cloud preprocessing: Deploy multi-source LiDAR sensors in the working scene, collect multi-source laser point cloud data for the template robot in the offline stage or the target robot in the online stage, set indexes according to different radar sources, merge the laser point clouds of different radars into the same coordinate system to form the target point cloud; convert the target point cloud into an AABB bounding box set through hierarchical clustering, build a spatial hierarchical tree with the bounding box as the node, and judge whether the laser point cloud in the bounding box comes from the same radar, that is, whether it is the same source, according to different radar indexes; Step 2: Kalman filter point cloud tracking: Use an auxiliary positioning device or manual method to roughly locate the target bounding box of the template robot or the target robot, use the target bounding box to initialize the tracking frame and establish the Kalman filter model, and use the initial tracking frame parameters as the optimal value of the tracking frame of the initial frame; when performing laser point cloud tracking, use the depth-first search strategy to match the previous frame tracking frame with the bounding box in the current frame hierarchy tree through the intersection-and-union method, and determine whether the target point cloud is searched based on the matching results; if the target point cloud is not searched, use the optimal value of the parameter of the previous frame tracking frame in the prediction equation to calculate the optimal value of the parameter of the current frame tracking frame; If the target point cloud is found, the Kalman filter gain coefficient is used as the weight, and the predicted value of the tracking frame parameter, the observed value and the absolute true value are considered comprehensively. The predicted value and the observed value of the tracking frame parameter are weighted summed as the optimal value of the tracking frame parameter; and the optimal value of the tracking frame parameter of the current frame is used to update the Kalman filter model parameters; Step 3: Coarse positioning of target point cloud pose estimation: In the point cloud coordinate system, classify the point cloud bounding box of the template robot or the target robot; when the bounding box point cloud data is a homologous point cloud, classify the target point cloud as an "L" type or an "I" type point cloud. If it is an "L" type, use the rotating caliper method to fit the minimum area rectangle of the target point cloud; when the bounding box point cloud data is a non-homologous point cloud or an "I" type homologous point cloud, fit the minimum area rectangle of the target point cloud through convex hull point set extraction and rotating caliper method; and use the minimum area rectangle to calculate the central pose vector of the target point cloud to achieve coarse positioning of the robot; Step 4: Offline stage template robot probability model construction: In the offline stage, similarity transformation is used to transform the target point cloud of the template robot from the point cloud coordinate system to the grid coordinate system. The Bresenham line drawing algorithm is used to update the grid probability during multiple sampling processes, and the prior probability grid model of the template robot is constructed in the form of a grid probability matrix. Step 5: Real-time precise positioning of the target robot in the online stage: In the online stage, the target point cloud of the target robot in the current frame is converted from the global point cloud coordinate system to the local grid coordinate system through two steps of rigid body transformation and similarity transformation, and the real-time probabilistic grid model of the target robot is constructed by bilinear interpolation. The error model of the real-time relative posture of the target robot is constructed through model matching optimization between the real-time probabilistic grid model and the prior probabilistic grid model, and the grid probability value in the error model is recursively iterated using nonlinear optimization to find the optimal value of the real-time relative posture, thereby obtaining the real-time absolute posture of the target robot and realizing real-time precise positioning of the robot.

2. The mobile robot tracking and positioning method based on ground-side multi-laser point cloud fusion according to claim 1 is characterized in that: The laser radar deployment and point cloud preprocessing described in step 1) includes the following steps: Step 1.1: LiDAR deployment: deploy two or more ground-based LiDAR sensors at the corners of the work area. Each LiDAR faces the work area for scanning and measurement. Step 1.2: Target point cloud acquisition: When in the offline stage, move the template robot to the reference position of the working area; when in the online stage, run the target robot in the working scene; use multi-source laser radar to obtain the robot's point cloud data, and set the radar index according to the laser radar from which the point cloud data comes; transform the point cloud data obtained by different laser radars into the same point cloud coordinate system through coordinate transformation to form the target point cloud; Step 1.3: Establishment of target point cloud bounding box set: Perform hierarchical clustering on the target point cloud and construct point cloud clusters P with inclusion or independence relationships through bounding boxes. i , with point cloud cluster P i Construct a spatial hierarchical tree for the nodes; the bounding box parameters of the target point cloud are defined as O i {P i ,l i ,w i ,cx i ,cy i ,b}, where P i is the point cloud cluster after segmentation and clustering, l i is the length of the target point cloud bounding box, w i is the width of the target point cloud bounding box, cx i is the x-axis coordinate of the center of the target point cloud bounding box, cy i is the y-axis coordinate of the center of the target point cloud bounding box, b represents the Boolean value of whether the point cloud data in the bounding box is of the same source; and P i ={p i1 (x i1 ,y i1 ),....,p ik (x ik ,y ik )}, then we have l i =max{x ik }-min{x ik },w i =max{y ik }-min{y ik }, cx i =(max{x ik }+min{x ik }) / 2,cy i =(max{y ik }+min{y ik }) / 2, where k represents the point cloud cluster P i There are a total of k points in it.

3. The mobile robot tracking and positioning method based on ground-side multi-laser point cloud fusion according to claim 1 is characterized in that: The Kalman filter point cloud tracking described in step 2) includes the following steps: Step 2.1: Kalman filter model establishment: Set the tracking box parameters of the Kalman filter model according to the bounding box parameters of the target point cloud: Where x is the horizontal coordinate of the center point of the tracking frame, y is the vertical coordinate of the center point of the tracking frame, w is the width of the tracking frame, h is the height of the tracking frame, dx is the change in the horizontal coordinate of the center point of the tracking frame, and dy is the change in the vertical coordinate of the center point of the tracking frame; Step 2.2: Kalman filter model initialization: In the initial stage, the target point cloud bounding box of the template robot or the target robot is roughly positioned using an auxiliary positioning device or manually, and the bounding box parameters in the initial stage are used as the optimal value of the tracking box of the initial frame; Step 2.3: Depth-first search of target point cloud: When performing laser point cloud tracking, a depth-first search strategy is used on the spatial hierarchy tree to traverse the bounding box nodes of each layer from top to bottom, and the optimal estimate of the tracking box of the previous frame is Perform correlation matching with the bounding box of the current layer of the spatial hierarchy tree; if the tracking box and the bounding box have an intersection, continue to traverse downward until the leaf node; if the tracking box and the bounding box do not have an intersection, stop traversing; when traversing to the leaf node at the bottom layer, calculate the intersection over union (IoU) value of the bounding box in the leaf node and the tracking box; if the maximum IoU value of all leaf nodes is greater than the set threshold, the point cloud cluster in the bounding box corresponding to the maximum IoU is taken as the target point cloud, and proceed to step 2.4; if the maximum IoU value is less than the set threshold, it is considered that the target point cloud is not found, and proceed to step 2.5; Step 2.4: Update the Kalman filter of the target point cloud: set the tracking box measurement value of the current frame according to the target point cloud bounding box parameters Substitute it into the Kalman filter target observation equation to calculate the tracking frame observation value Z k : Among them, H is the state observation matrix, v k is the observation noise; Use the optimal value of the tracking box parameters of the previous frame Calculate the predicted value of the tracking box parameters of the current frame Using the error covariance matrix P of the previous frame k-1 Calculate the error covariance matrix prediction value of the current frame Where A is the state transfer matrix, Q is the process noise matrix; Predict the value based on the error covariance matrix of the current frame Calculate the Kalman filter gain coefficient K of the current frame k : Where R is the measurement noise matrix; The weighted sum of the measured value and the predicted value of the tracking box is used as the optimal value of the parameter of the tracking box Predict the value based on the error covariance matrix of the current frame Calculate the optimal value P of the error covariance matrix k : Where I is the identity matrix; go to step 2.6; Step 2.5: Kalman filter update for target point cloud not found: set the optimal value of the tracking box parameters of the previous frame Substitute the state prediction equation to calculate the optimal value of the tracking box parameters of the current frame Step 2.6: Determine whether the laser point cloud tracking is completed. If the tracking is completed, the process terminates, otherwise return to step 2.

3.

4. The mobile robot tracking and positioning method based on ground-side multi-laser point cloud fusion according to claim 1 is characterized in that: The target point cloud pose estimation rough positioning method described in step 3) comprises the following steps: Step 3.1: Target point cloud classification: For the bounding box of the template robot or the target robot, the target point cloud is divided into homologous point cloud and non-homologous point cloud according to whether it is homologous; if the target point cloud is non-homologous point cloud, proceed to step 3.2; if the target point cloud is homologous point cloud, the homologous point cloud is divided into "L" type and "I" type point cloud: using the sequentiality of homologous point cloud data, extract the first and last endpoints, construct the straight line equation of the endpoint connection, and calculate the maximum distance value from the middle point to the endpoint connection; if the maximum distance value is greater than the set threshold, the point cloud is considered to be "L" type, and the convex hull point set is constructed with the middle point and the two end points of the maximum distance value, and proceed to step 3.3; if the maximum distance value is less than the set threshold, the point cloud is considered to be "I" type, and proceed to step 3.2; Step 3.2: Convex hull point set extraction: Initialize the convex hull point set to be empty, sort the target point cloud from small to large according to the x coordinate, if the x values ​​are the same, sort them from small to large according to the y values, and construct the target point cloud sequence; traverse each point in the target point cloud sequence, if the number of points in the convex hull point set is not greater than 2, then add the current point to the convex hull point set; if the number of points in the convex hull point set is greater than 2, then take out the last two points in the convex hull point set as the basic points, and judge whether the current point and the basic points can construct a convex polygon: taking the clockwise construction as an example, if the current point is in the clockwise direction of the basic point composition vector, then add the current point to the convex hull point set; otherwise, backtrack, delete the last point in the convex hull point set, and then take out the last two points in the convex hull point set as the basic points to judge the convex polygon construction; repeat the above process until the current point and the last two points can form a convex polygon or continue to delete until only one point remains in the convex hull point set, and then add the current point to the convex hull point set; Step 3.3: Rotating caliper method to fit the minimum area rectangle of the target point cloud: Use the convex hull point set of the target point cloud to construct a convex polygon, start from a vertex of the convex polygon and traverse it through the sliding window method, take the above vertex and its adjacent vertex in the clockwise direction to construct a straight line, recorded as the main parallel line; traverse the other vertices of the convex hull point set, find the vertex with the farthest vertical distance from the main parallel line, and make a parallel line of the main parallel line through the above vertex, recorded as the secondary parallel line; construct the minimum enclosing rectangle of the convex polygon between the main and secondary parallel lines, and record the area of ​​the rectangle and the coordinates of its endpoints; rotate in a clockwise direction through the sliding window method, select the vertex and its adjacent vertices in the clockwise direction to construct the main and secondary parallel lines in turn, repeat the above minimum enclosing rectangle construction, and update the minimum rectangle area and its endpoint coordinates; after traversing all the vertices of the convex polygon, take the last updated minimum area rectangle as the fitting result, and use the minimum area rectangle to calculate the central pose vector of the target point cloud.

5. The mobile robot tracking and positioning method based on ground-side multi-laser point cloud fusion according to claim 1 is characterized in that: The offline stage robot probability model construction described in step 4) includes the following steps: Step 4.1: Obtaining the point cloud center pose vector: Use the target point cloud coarse positioning method to obtain the point cloud center pose vector of the template robot, denoted as are the center coordinates and attitude angle of the minimum area rectangle of the template robot in the point cloud coordinate system; Step 4.2: Initialize the probability grid matrix: Describe the robot probability model in the form of a probability grid matrix. Let n be the grid matrix dimension, and let A n×n A probability grid matrix for storing grid probabilities; Step 4.3: Point cloud data coordinate system transformation: Let the coordinates of the template robot point cloud in the point cloud coordinate system be P w =(x w ,y w ) T , the coordinate in the grid coordinate system is P s =(x s ,y s ) T , use similarity transformation to transform the template robot point cloud from the point cloud coordinate system to the grid coordinate system: Among them, the similarity transformation matrix T in two-dimensional space s for: Where s is the scaling factor, R 2×2 is a two-dimensional rotation matrix, t 2×1 is a two-dimensional translation vector: Step 4.4: Grid probability update: Set the number of grid probability updates m. In the kth update, 1≤k≤m, for each point in the target point cloud, determine the radar from which each point comes according to the radar index, use the Bresenham line drawing algorithm to connect each point with the origin of the radar from which it comes to form a line segment, and calculate the grid probability matrix A. n×n Each grid a ij The probability value P in the kth update k (a ij ), if the grid is a line segment grid, then the grid probability value P k (a ij )=0; if the grid is a grid where the line segment ends, the grid probability value P k (a ij )=1; otherwise the grid probability value P k (a ij )=0.5; after m updates, the final grid probability value P(a ij )for: Step 4.5: Prior probability grid model construction: Use the Kalman filter tracking model to track the template robot’s point cloud, and continuously update the grid probability matrix until the change in each grid probability value in the grid probability matrix is ​​less than the set threshold, thereby constructing a prior probability grid model.

6. The mobile robot tracking and positioning method based on ground-side multi-laser point cloud fusion according to claim 1 is characterized in that: The online stage robot real-time precise positioning described in step 5) comprises the following steps: Step 5.1: Get the point cloud center pose vector: Use the target point cloud coarse positioning to get the point cloud center pose vector of the target robot, denoted as The relative pose vector of the target robot and the template robot is ξ t is the template point cloud pose estimation vector; Step 5.2: Grid coordinate system point cloud projection: Let the coordinates of the target robot in the global point cloud coordinate system be p i =(x i ,y i ), first through the rigid body transformation Converted to the local point cloud coordinate system of the template robot, the resulting coordinates T i (ξ r )for: Then through the similarity transformation T s The coordinates of the target robot in the local point cloud coordinate system are i (ξ r ) is transformed into the coordinate P in the local grid coordinates i (ξ r ): Using the above rigid body transformation ξ r and similarity transformation T s The projection point of the target point cloud in the local grid coordinate system is obtained as P(x,y), and the grid probability value Odds(P(x,y)) of the above projection point is calculated by bilinear interpolation method: Among them, point Q 11 (x1,y1),Q 12 (x1,y2),Q 21 (x2,y1),Q 22 (x2, y2) is the coordinate of the center point of the four-neighborhood grid of the projection point P(x, y) in the local grid coordinate system; The directional gradient of the grid probability value Odds(P(x,y)) on the coordinate axis is approximately: The bilinear interpolation method is used to calculate the grid probability values ​​Odds(P(x,y)) of all projection points of the target point cloud, and the real-time probability grid model of the target robot is constructed in the form of a grid probability matrix. Step 5.3: LM nonlinear optimization model construction: Let ξ r0 The optimal relative pose vector for the real-time probability grid model and the prior probability grid model to achieve an ideal alignment state: x r0 =ξ r +Dx r (19) where Δξ r is the relative posture error; the optimal value of the relative posture error Δξ r * The estimation process is transformed into a nonlinear least squares problem, and a probabilistic grid coincidence error model of the target robot's real-time relative posture is constructed: Assuming λ is the damping coefficient, equation (20) can be simplified to: The matrix H i , vector g i They are: Step 5.4: Solving the LM nonlinear optimization model: Set the number of iterations and the minimum relative pose error threshold as the convergence condition, with ξ r0 =ξ r is the initial value of the iteration, and the point cloud coordinates Τ obtained after the target point cloud undergoes rigid body transformation and similarity transformation are calculated. i (ξ r ), grid coordinate P i (ξ r ) and grid probability values ​​Odds(P i (ξ r )), by calculating the matrix H i , vector g i Get the relative posture error value Δξ of the current step r , to determine whether the iterative optimization has reached the convergence condition: If the iterative optimization has not converged, the relative posture error value Δξ is used r The target point cloud is transformed, calculated and judged iteratively; if the iterative optimization has converged, the optimal value of the relative pose error Δξ is output r * , calculate the precise pose vector ξ of the target point cloud p , which is the real-time accurate position and posture of the target robot: x p =ξ t -(x) r +Dx r * ) (24).

Images