Reflection column point cloud segmentation fitting method and reflection column positioning method based on particle filtering

Abstract

The invention relates to the technical field of laser point cloud data segmentation fitting processing and reflective column positioning, and particularly discloses a reflective column point cloud segmentation fitting method and a reflective column positioning method based on particle filtering, and the reflective column point cloud segmentation fitting method comprises the steps of preparation work and reflective column segmentation fitting. The preparation work step comprises the steps of deploying the reflective column and leveling the laser radar, and the method is a novel segmentation and fitting technical method of the reflective column laser point cloud, so that the application of the reflective column positioning technology is facilitated. The reflective column positioning method based on particle filtering is characterized in that the positioning steps comprise system initialization, state prediction, reflective column mapping, weight calculation and resampling, and the method does not depend on all environment characteristics in a scene and only depends on several fixed positions where reflective columns are deployed. The special circle center fitting method and the dynamic variance particle filtering positioning greatly improve the environmental adaptability, and have high positioning precision and robustness.

Description

Technical Field

[0001] The invention relates to the technical field of segmentation and fitting processing of laser point cloud data and positioning of reflective columns. Background Art

[0002] With the development of artificial intelligence related technologies, intelligent robots are increasingly used in various industries, such as health care, industrial manufacturing, logistics and transportation. Among them, accurate and stable positioning systems are the key to realizing autonomous navigation of robots and completing various tasks. However, the environmental characteristics in some application scenarios are often complex and changeable. This leads to the need to update the map from time to time based on changes in environmental characteristics when using SLAM map-based laser positioning solutions. Otherwise, the positioning accuracy may drop to an unacceptable level, or even become impossible to locate.

[0003] Existing implementation schemes usually adopt the three-sided positioning method, which determines the target position by measuring the distance between the target and three reference points. The advantages of the three-sided positioning method are simplicity and understanding, fast calculation speed, but this method has high requirements for the arrangement of reflective columns, requiring at least three reflective columns to be observed, and in actual applications it will be affected by factors such as obstacle obstruction and signal interference, which can easily cause positioning errors or even errors. It also has high requirements for the geometric relationship of the reflective columns and requires relatively precise calibration parameters. It has low adaptability to complex and changeable environments and has high requirements for the deployment of reflective columns. Summary of the invention

[0004] The purpose of the present invention is to provide a reflective column point cloud segmentation and fitting method, which is a new reflective column laser point cloud segmentation and fitting technical method to facilitate the application of reflective column positioning technology.

[0005] Another object of the present invention is to provide a reflective column positioning method based on particle filtering suitable for the application of positioning technology of mobile robots. This method does not rely on all environmental features in the scene, but only relies on several fixed positions where reflective columns are deployed. Its unique center fitting method and dynamic variance particle filter positioning greatly improve the environmental adaptability and have high positioning accuracy and robustness.

[0006] To achieve the above-mentioned purpose, the technical solution of the present invention is: a reflective column point cloud segmentation and fitting method, including preparation work and reflective column segmentation and fitting steps, the preparation work step includes the step of deploying reflective columns and laser radar leveling; the reflective column segmentation and fitting step includes reflective column detection and segmentation of laser data and reflective column fitting.

[0007] In the step of deploying the reflective columns, the spacing distance between the reflective columns is the spacing distance that can be used for the reflective column detection and segmentation step; and / or, the method of the laser radar leveling step is to place a reflective column at the same height in four directions of front, back, left, and right at a certain distance from the laser radar, and at the same time the laser radar observes the laser data of the four reflective columns. If the laser radar cannot detect the laser data of the four reflective columns at the same time, the posture of the laser radar and the height of the reflective columns are fine-tuned until the laser radar can detect the laser data of the four reflective columns at the same time, and the laser radar is considered to have been leveled.

[0008] The reflective column detection and segmentation is performed by clustering the currently scanned strong reflection points according to the Euclidean distance through the laser radar. The specific method is as follows: Assume that there are two reflective columns P and Q. The laser radar scans six strong reflection points A, B, C, D, E, and F on the two reflective columns in sequence. Among them, points A, B, and C are located on the reflective column P, and D, E, and F are located on the reflective column Q. First, a category S is defined. p , and take point A as the reference point, calculate the distance between point A and point B. If the distance between point A and point B is less than the preset threshold, then point B is an inner point. Slide the reference point to point B, calculate the distance between point B and point C. If the distance between point B and point C is less than the preset threshold, then C is an inner point. Slide the reference point to point C again, calculate the distance between point C and point D. If the distance between point C and point D is greater than the specified threshold, then D is an outer point, that is, point A, B, C ∈ S P , and define a new category S Q , slide the reference point to point D, and use the same judgment method as points A, B, and C above to know that D, E, F ∈ S Q , thus completing the laser point cloud segmentation of all reflective columns.

[0009] The method of the reflective column fitting step is as follows: assuming that the coordinates of the center of the reflective column are (a, b) and the radius is r, the equation of the circle can be defined as (xa) 2 +(yb) 2 =r 2 , we can get in

[0010] Suppose the laser point cloud of a reflective column segmented by the reflective column detection and segmentation step is {(x (0) ,y (0) ),(x (1) ,y (1) ),(x (2) ,y (2) ),...,(x (n) ,y (n))}, these laser point coordinates and the parameters to be estimated together constitute an overdetermined set of equations, which are fitted using the least square method. Let the parameters to be estimated θ = [abc] T , using these n laser points, construct the following X matrix and Y vector:

[0011]

[0012] The optimization objective is defined as: J(θ) = (Xθ-Y) T (Xθ-Y), in order to minimize the cost function J(θ), let We can solve θ=(X T X) -1 X T Y, bring the laser point cloud of the i-th reflective column into θ=(X T X) -1 X T Y, calculate the center coordinates P of the i-th reflective column l (i) (a (i) ,b (i) ) and radius r (i) Similarly, the center coordinates of all other detected reflective columns are calculated to obtain the center coordinates of all reflective columns in the lidar coordinate system.

[0013] The reflective column segmentation and fitting step also includes a coordinate conversion step, which is to convert the center coordinates of all reflective columns in the laser radar coordinate system to the robot base coordinate system according to the posture relationship between the robot base coordinate system and the laser radar coordinate system. Specifically, according to the equation To calculate the conversion, is the observation of the i-th reflective column in the robot base coordinate system, are the rotation matrix and translation vector from the robot base coordinate system to the lidar coordinate system, P l (i) is the center of the reflective column in the laser radar coordinate system.

[0014] A reflective column positioning method based on particle filtering that applies the reflective column point cloud segmentation and fitting method, the positioning steps include system initialization, state prediction, reflective column mapping, weight calculation and resampling steps; in the system initialization, the initial posture of the robot and the variance of the particle distribution are given, and the initial posture is used as the mean, and a Gaussian distribution is constructed with the variance, and then N particles are randomly extracted from the Gaussian distribution, and these particles represent all possible postures of the current robot; in the state prediction, the robot chassis feeds back the speed information of the odometer at a certain sampling frequency, and uses the speed information to perform navigation on the posture of each particle. The trajectory is calculated as the prior estimate of the current moment, and the posture in discrete time is calculated; the reflective column mapping records the reflective column observed at a certain moment and the reflective column center coordinates calculated by the reflective column point cloud segmentation and fitting method as a set, and the posture of each particle in discrete time obtained by state prediction is mapped to the reflective column map; the weights of all particles are calculated in the weight calculation, and the particle with the highest weight represents the best posterior posture estimate of the robot; the best posterior posture estimate is used as the mean in the resampling, and the particles are resampled with the Gaussian distribution constructed by the dynamic variance to provide new particles for the next iteration.

[0015] The state prediction assumes that the robot moves at a constant linear velocity and angular velocity during the sampling interval, denoted as [v x v y ω z ] T , let the system state be [x yθ] T According to the motion model of the omnidirectional mobile robot, its state differential equation can be expressed as follows:

[0016]

[0017]

[0018]

[0019] The posture recursion equation in discrete time is as follows:

[0020] When z =0 o'clock

[0021] x k+1 =x k +v x Δtcosθ k -v y Δtsinθ k

[0022] y k+1 =y k +v x Δtsinθ k+v y Δtcosθ k

[0023] θ k+1 =θ k

[0024] When z ≠0 hours

[0025]

[0026]

[0027]

[0028] Where Δt = t k+1 -t k , represents the sampling time interval.

[0029] In the reflective column mapping, it is assumed that m reflective columns are observed at a certain moment, and the reflective column center coordinates calculated by the reflective column point cloud segmentation and fitting method are set C. The point set C is transformed from the robot base coordinate system to the map coordinate system using the position and posture of each particle obtained by state prediction. The reflective column center coordinate sets corresponding to each particle after transformation are C0, C1, C2, ..., C N , taking C0 as an example, let The coordinate point set of all reflective column centers on the reflective column map Using a one-to-one comparison without replacement, take out a reflective column center coordinate point from C0 Calculate the coordinate points of the center of the reflective column in sequence The Euclidean distance from each coordinate point of the point set M, and take the one with the minimum distance from M At the same time, record the corresponding index pairs <i, j>, and repeat this process until all the center coordinates of the reflective column in C0 have been mapped to the reflective column map.

[0030] The weight calculation is based on the corresponding relationship <i, j> index pair, is the mean, with a certain variance and Construct a bivariate Gaussian distribution and calculate The probability density value in this distribution is used as the weight: in Then the weight of the 0th particle is:

[0031] Similarly, the weights of all other particles can be calculated, and the particle with the highest weight represents the best posterior pose estimate of the robot.

[0032] The specific method of resampling is when the total weight w calculated by all particles is sum Below a certain level lim , the variance is amplified to increase the diversity of particles, so that there is a greater chance of hitting the pose near the true value, thereby increasing the weight of these particles. As the particle weight gradually increases, the dynamic variance decreases, and the diversity of particles also decreases, so that all particles converge to the vicinity of the true value and stabilize within a certain range; since the total weight value of all particles is related to the total number of particles N, according to engineering experience, w is taken lim =1.6N, record The minimum standard deviation at convergence is The maximum standard deviation is

[0033] When sum <w lim When , it means that the positioning does not converge.

[0034] When sum >=w lim When , it means that the positioning has converged, then

[0035] By adopting the above technical scheme, the beneficial effects of the present invention are: the above-mentioned reflective column point cloud segmentation and fitting method, which uses linear least squares to fit the center of the reflective column, solves the problem that reflective columns of the same specifications cannot be arranged in the environment, and the calibration parameters of the reflective columns are required to be lower than the three-sided positioning, and the deployment is relatively easy, which provides a new reflective column laser point cloud segmentation and fitting technical method for the prior art, so as to facilitate the application of reflective column positioning technology.

[0036] The above-mentioned reflective column positioning method based on particle filtering of the present invention is a method different from the existing reflective column positioning technology, especially for the positioning problem of mobile robots in complex and changeable industrial environments, which can be better improved. It uses an odometer to predict the movement of the robot, uses reflective columns as landmarks for observation, and uses a dynamic variance particle filter positioning algorithm to achieve particle filter positioning, thereby correcting the estimated position and posture of the robot. Engineering practice shows that the present invention improves the correction capability while ensuring a high positioning accuracy, and can still maintain the correctness of positioning when there is a short-term occlusion and only 1 to 2 reflective columns can be observed.

[0037] In summary, the method of the present invention does not rely on all the environmental features in the scene, but only relies on several fixed positions where reflective columns are deployed. Its unique center of circle fitting method and dynamic variance particle filter positioning greatly improve the environmental adaptability, have high positioning accuracy and robustness, and can improve the anti-interference ability of positioning. While effectively avoiding the positioning loss problem caused by short-term abnormal factors such as illumination and occlusion during operation, it ensures the positioning accuracy of the system. Therefore, the positioning problem of mobile robots in complex and changeable industrial environments can be better improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 The invention relates to a flow chart of a reflective column positioning method based on particle filtering.

[0039] Figure 2 It is a schematic diagram of reflective column detection and segmentation in a reflective column positioning method based on particle filtering involved in the present invention.

[0040] Figure 3 The present invention relates to a schematic diagram of robot posture state prediction in a reflective column positioning method based on particle filtering. DETAILED DESCRIPTION

[0041] In order to further explain the technical solution of the present invention, the present invention is described in detail below through specific embodiments.

[0042] The present embodiment discloses a reflective column point cloud segmentation and fitting method, that is, a reflective column laser point cloud segmentation and fitting method, the step flow of which includes preparation work and reflective column segmentation and fitting steps, the preparation work step includes the steps of deploying reflective columns and laser radar leveling; the reflective column segmentation and fitting step includes reflective column detection and segmentation of laser data and reflective column fitting, such as Figure 1 The flowchart of the dotted box part.

[0043] The deployment of reflective columns is usually mainly: reflective tape is attached to the surface of a standard cylinder as a reflective column. When deploying, attention should be paid to the spacing between the reflective columns. Too small a spacing will make it impossible to segment the laser point cloud of the reflective column, resulting in misidentification or non-identification. That is, the spacing distance between the reflective columns is a spacing distance that can be used for the reflective column detection and segmentation step. Since the following method uses Euclidean distance as a judgment basis, this requires that the spacing of the reflective columns cannot be too small, which specifically depends on the angular resolution of the laser radar, the scanning distance and the diameter of the reflective column, but is generally preferably greater than 2 times the diameter of the largest reflective column.

[0044] The method of the laser radar leveling step is that, since the single-line laser radar scans a plane, a reflective column is placed at the same height in four directions of front, back, left, and right at a certain distance (about 15m) from the laser radar, and the laser radar observes the laser data of the four reflective columns at the same time. If the laser radar cannot detect the laser data of the four reflective columns at the same time, the posture of the laser radar and the height of the reflective columns are fine-tuned until the laser radar can detect the laser data of the four reflective columns at the same time, and it is considered that the laser radar has been leveled.

[0045] For the reflective column detection and segmentation, the intersection of the horizontal plane where the optical center of the single-line laser radar is located and the reflective column is a perfect circle. The scanning result is an arc composed of a series of discrete laser points. Since the reflection intensity of the laser points hitting the reflective column is much higher than that of other materials, the reflection intensity value of the laser points can be used to filter out only the strong reflection points of the reflective column by setting a threshold. The laser radar data is expressed in polar coordinates with the optical center of the sensor as the origin, and is arranged in order from small to large according to the angle value. Therefore, the laser radar can be used to cluster the currently scanned strong reflection points according to the Euclidean distance. The specific method is as follows, such as Figure 2 As shown in the figure, the dot in the lower right corner is the launch point of the single-line laser radar. Assume that there are two reflective columns P and Q. The laser radar scans six strong reflection points A, B, C, D, E, and F on the two reflective columns in sequence. Points A, B, and C are located on the reflective column P, and D, E, and F are located on the reflective column Q. First, define a category S p , and take point A as the reference point, calculate the distance between point A and point B. If the distance between point A and point B is less than the preset threshold, then point B is an inner point. Slide the reference point to point B, calculate the distance between point B and point C. If the distance between point B and point C is less than the preset threshold, then C is an inner point. Slide the reference point to point C again, calculate the distance between point C and point D. If the distance between point C and point D is greater than the specified threshold, then D is an outer point, that is, point A, B, C ∈ S P , and define a new category S Q , slide the reference point to point D, and use the same judgment method as points A, B, and C above to know that D, E, F ∈ S Q , thus completing the laser point cloud segmentation of all reflective columns.

[0046] The method of the reflective column fitting step is as follows: assuming that the coordinates of the center of the reflective column are (a, b) and the radius is r, the equation of the circle can be defined as (xa) 2 +(yb) 2 =r 2 , we can get in In the previous step, the segmentation of the laser point cloud of each reflective column has been completed. Suppose the laser point cloud of a reflective column is {(x (0) ,y(0) ),(x (1) ,y (1) ),(x (2) ,y (2) ),…,(x (n) ,y (n) )}, these laser point coordinates and the parameters to be estimated together constitute an overdetermined set of equations, which are fitted using the least square method. Let the parameters to be estimated θ = [abc] T , using these n laser points, construct the following X matrix and Y vector:

[0047]

[0048] The optimization objective is defined as: J(θ) = (Xθ-Y) T (Xθ-Y), in order to minimize the cost function J(θ), let We can solve θ=(X T X) -1 X T Y, so we only need to bring the laser point cloud of the i-th reflector into θ=(X T X) -1 X T Y, calculate the center coordinates P of the i-th reflective column l (i) (a (i) ,b (i) ) and radius Similarly, the center coordinates of all other detected reflective columns are calculated to obtain the center coordinates of all reflective columns in the lidar coordinate system.

[0049] Since the above fitting reflective column center coordinates P l (i) It is a representation in the laser radar coordinate system. In the robot positioning application, it is also necessary to convert these center coordinates to the robot base coordinate system according to the pose relationship between the robot base coordinate system and the laser radar coordinate system. Therefore, in the reflective column segmentation and fitting step described in this embodiment, a coordinate conversion step is also performed after the reflective column fitting step is completed. Specifically, according to the pose relationship between the robot base coordinate system and the laser radar coordinate system, the center coordinates of all reflective columns in the laser radar coordinate system are converted to the robot base coordinate system. Specifically, according to the equation To calculate the conversion, is the observation of the i-th reflective column in the robot base coordinate system, are the rotation matrix and translation vector from the robot base coordinate system to the lidar coordinate system, P l (i) is the center of the reflective column in the laser radar coordinate system.

[0050] The above method can complete the segmentation and fitting of the laser point cloud of the reflective column. In this method, the linear least squares method is used to fit the center of the reflective column, which solves the problem that reflective columns of the same specifications cannot be arranged in the environment. The calibration parameters of the reflective column are lower than those of the three-sided positioning, and the deployment is relatively easy.

[0051] Next, this embodiment continues to disclose a reflective column positioning method based on particle filtering that applies the above-mentioned reflective column point cloud segmentation and fitting method. The method steps are as follows: Figure 1 As shown, the positioning step includes system initialization, state prediction, reflector mapping, weight calculation and resampling steps. The specific method is described in detail below.

[0052] The system is initialized by giving the robot an initial posture and the variance of the particle distribution, and taking the initial posture as the mean and the variance to construct a Gaussian distribution, and then randomly extracting N particles from the Gaussian distribution. These particles represent all possible postures of the current robot.

[0053] In the state prediction, the robot chassis feeds back the speed information of the odometer at a certain sampling frequency, and uses the speed information to perform trajectory calculation on the position and posture of each particle as a priori estimate at the current moment. At the same time, it is assumed that the robot moves at a constant linear velocity and angular velocity during the sampling interval, which is recorded as [v x v y ω z ] T , where v x is the speed of the robot in the forward direction, v y is the speed at which the robot moves to the left, ω z is the angular velocity of the robot, and the system state is [xy θ] T (This article uses the right-hand coordinate system, the specific definition is as follows Figure 3 As shown), where x and y represent the coordinates in the reflective column map coordinate system, and θ represents the heading of the robot in the reflective column map coordinate system. According to the motion model of the omnidirectional mobile robot, its state differential equation can be expressed as follows:

[0054]

[0055]

[0056]

[0057] The posture recursion equation in discrete time is as follows:

[0058] When z =0 o'clock

[0059] x k+1 =x k+v x Δtcosθ k -v y Δtsinθ k

[0060] y k+1 =y k +v x Δtsinθ k +v y Δtcosθ k

[0061] θ k+1 =θ k

[0062] When z ≠0 hours

[0063]

[0064]

[0065]

[0066] Where Δt = t k+1 -t k , represents the sampling time interval.

[0067] In the reflective column mapping, it is assumed that m reflective columns are observed at a certain moment, and the reflective column center coordinates calculated by the reflective column point cloud segmentation and fitting method are set C. The point set C is transformed from the robot base coordinate system to the map coordinate system using the position and posture of each particle obtained by state prediction. The reflective column center coordinate sets corresponding to each particle after transformation are C0, C1, C2, ..., C N , taking C0 as an example, let The coordinate point set of all reflective column centers on the reflective column map Here n refers to the number of all reflective columns on the reflective column map. A one-to-one comparison without replacement is used to extract the coordinate point of the center of a reflective column from C0. (Take out the coordinates of the center of the i-th reflective column from C0) Calculate the coordinates of the center of the reflective column one by one The Euclidean distance from each coordinate point of the point set M, and take the one with the minimum distance from M Represents the coordinates of the jth reflector in the reflector map coordinate system, and records the corresponding index pair <i, j>, and repeats this process until the coordinates of the centers of all reflector circles in C0 have been mapped to the reflector map.

[0068] The weight calculation is based on the index pair of the reflective column correspondence <i, j> found previously, is the mean, with a certain variance and Construct a bivariate Gaussian distribution ( and In the experiment of the method of the present invention, the two variances are taken as 0.09), and the The probability density value in this distribution is,

[0069] And use it as weight: in Then the weight of the 0th particle is:

[0070] Similarly, the weights of all other particles can be calculated, and the particle with the highest weight represents the best posterior pose estimate of the robot.

[0071] In the resampling, the best a posteriori pose estimation is used as the mean, and the Gaussian distribution constructed by dynamic variance is used for particle resampling to provide new particles for the next iteration. The dynamic variance strategy can take into account the robustness and accuracy of robot positioning. The principle is that when the total weight w calculated by all particles of the robot sum Below a certain level lim , the variance is amplified to increase the diversity of particles, so that there is a greater chance of hitting the pose near the true value, thereby increasing the weight of these particles. As the particle weight gradually increases, the dynamic variance decreases, and the diversity of particles also decreases, so that all particles converge to the vicinity of the true value and stabilize within a certain range; since the total weight value of all particles is related to the total number of particles N, according to engineering experience, w is taken lim =1.6N, record The minimum standard deviation at convergence is The maximum standard deviation is where Δσ x , σ y It indicates the maximum increment of the maximum standard deviation based on the minimum standard deviation;

[0072] When sum <w lim When , it means that the positioning does not converge.

[0073] When sum >=w lim When , it means that the positioning has converged, then

[0074] The estimated position and posture of the robot is corrected by the above method of the present invention. Engineering practice shows that the present invention improves the error correction capability while ensuring a high positioning accuracy. The positioning accuracy can still be maintained when there is a short-term occlusion and only 1 to 2 reflective columns can be observed.

[0075] In summary, the above-mentioned purpose and effect of the present invention are achieved.

[0076] The above embodiments and drawings do not limit the product form and style of the present invention. Any appropriate changes or modifications made by ordinary technicians in the relevant technical field should be deemed to be within the patent scope of the present invention.

Claims

1. A reflective column point cloud segmentation and fitting method, characterized in that: The method comprises preparation work and reflective column segmentation and fitting steps, wherein the preparation work step comprises the steps of deploying reflective columns and laser radar leveling; the reflective column segmentation and fitting steps comprises reflective column detection and segmentation of laser data and reflective column fitting.

2. A reflective column point cloud segmentation and fitting method as claimed in claim 1, characterized in that: In the step of deploying the reflective columns, the spacing distance between the reflective columns is the spacing distance that can be used for the reflective column detection and segmentation step; and / or, the method of the laser radar leveling step is to place a reflective column at the same height in four directions of front, back, left, and right at a certain distance from the laser radar, and at the same time the laser radar observes the laser data of the four reflective columns. If the laser radar cannot detect the laser data of the four reflective columns at the same time, the posture of the laser radar and the height of the reflective columns are fine-tuned until the laser radar can detect the laser data of the four reflective columns at the same time, and the laser radar is considered to have been leveled.

3. A reflective column point cloud segmentation and fitting method as claimed in claim 1 or 2, characterized in that: The reflective column detection and segmentation is performed by clustering the currently scanned strong reflection points according to the Euclidean distance through the laser radar. The specific method is as follows: Assume that there are two reflective columns P and Q. The laser radar scans six strong reflection points A, B, C, D, E, and F on the two reflective columns in sequence. Among them, points A, B, and C are located on the reflective column P, and D, E, and F are located on the reflective column Q. First, a category S is defined. p , and take point A as the reference point, calculate the distance between point A and point B. If the distance between point A and point B is less than the preset threshold, then point B is an inner point. Slide the reference point to point B, calculate the distance between point B and point C. If the distance between point B and point C is less than the preset threshold, then C is an inner point. Slide the reference point to point C again, calculate the distance between point C and point D. If the distance between point C and point D is greater than the specified threshold, then D is an outer point, that is, point A, B, C ∈ S P , and define a new category S Q , slide the reference point to point D, and use the same judgment method as points A, B, and C above to know that D, E, F ∈ S Q , thus completing the laser point cloud segmentation of all reflective columns.

4. A reflective column point cloud segmentation and fitting method as claimed in claim 3, characterized in that: The method of the reflective column fitting step is as follows: assuming that the coordinates of the center of the reflective column are (a, b) and the radius is r, the equation of the circle can be defined as (xa) 2 +(yb) 2 =r 2 , we can get in Suppose the laser point cloud of a reflective column segmented by the reflective column detection and segmentation step is {(x (0) ,y (0) ),(x (1) ,y (1) ),(x (2) ,y (2) ),...,(x (n) ,y (n) )}, these laser point coordinates and the parameters to be estimated together constitute an overdetermined set of equations, which are fitted using the least square method. Let the parameters to be estimated θ = [abc] T , using these n laser points, construct the following X matrix and Y vector: The optimization objective is defined as: J(θ) = (Xθ-Y) T (Xθ-Y), in order to minimize the cost function J(θ), let We can solve θ=(X T X) -1 X T Y; Substitute the laser point cloud of the i-th reflective column into θ=(X T X) -1 X T Y, calculate the center coordinates P of the i-th reflective column l (i) (a (i) ,b (i) ) and radius r (i) Similarly, the center coordinates of all other detected reflective columns are calculated to obtain the center coordinates of all reflective columns in the lidar coordinate system.

5. A reflective column point cloud segmentation and fitting method as claimed in claim 4, characterized in that: The reflective column segmentation and fitting step also includes a coordinate conversion step, which is to convert the center coordinates of all reflective columns in the laser radar coordinate system to the robot base coordinate system according to the posture relationship between the robot base coordinate system and the laser radar coordinate system. Specifically, according to the equation To calculate the conversion, is the observation of the i-th reflective column in the robot base coordinate system, are the rotation matrix and translation vector from the robot base coordinate system to the lidar coordinate system, P l (i) is the center of the reflective column in the laser radar coordinate system.

6. A reflective column positioning method based on particle filtering using the reflective column point cloud segmentation and fitting method as described in any one of claims 1 to 5, characterized in that: The positioning step includes system initialization, state prediction, reflective column mapping, weight calculation and resampling steps; in the system initialization, the initial posture of the robot and the variance of the particle distribution are given, and the initial posture is used as the mean, and the Gaussian distribution is constructed with the variance, and then N particles are randomly selected from the Gaussian distribution, and these particles represent all possible postures of the current robot; in the state prediction, the robot chassis feeds back the speed information of the odometer at a certain sampling frequency, and uses the speed information to perform track calculation on the posture of each particle as a priori estimate at the current moment, and calculates The position and posture in discrete time are calculated; in the reflective column mapping, the reflective column observed at a certain moment is recorded as a set of the reflective column center coordinates calculated by the reflective column point cloud segmentation and fitting method, and the position and posture of each particle in discrete time obtained by state prediction is mapped to the reflective column map; in the weight calculation, the weights of all particles are calculated, and the particle with the highest weight represents the best posterior pose estimate of the robot; in the resampling, the best posterior pose estimate is used as the mean, and the Gaussian distribution constructed by dynamic variance is used to resample the particles to provide new particles for the next iteration.

7. A reflective column positioning method based on particle filtering as claimed in claim 6, characterized in that The specific method of resampling is when the total weight w calculated by all particles is sum Below a certain level lim , the variance is amplified to increase the diversity of particles, so that there is a greater chance of hitting the pose near the true value, thereby increasing the weight of these particles. As the particle weight gradually increases, the dynamic variance decreases, and the diversity of particles also decreases, so that all particles converge to the vicinity of the true value and stabilize within a certain range; since the total weight value of all particles is related to the total number of particles N, according to engineering experience, w is taken lim =1.6N, record The minimum standard deviation at convergence is The maximum standard deviation is in represents the maximum increment of the minimum standard deviation; When sum <w lim When , it means that the positioning does not converge. When sum >=w lim When , it means that the positioning has converged, then 8. A reflective column positioning method based on particle filtering as claimed in claim 6 or 7, characterized in that: The state prediction assumes that the robot moves at a constant linear velocity and angular velocity during the sampling interval, denoted as [v x v y ω z ] T , let the system state be [xy θ] T According to the motion model of the omnidirectional mobile robot, its state differential equation can be expressed as follows: The posture recursion equation in discrete time is as follows: When z =0 o'clock x k+1 =x k +v x Δtcosθ k -v y Δtsinθ k y k+1 =y k +v x Δtsinθ k +v y Δtcosθ k i k+1 =θ k When z ≠0 hours Where Δt = t k+1 -t k , represents the sampling time interval.

9. A reflective column positioning method based on particle filtering as claimed in claim 8, characterized in that: In the reflective column mapping, it is assumed that m reflective columns are observed at a certain moment, and the reflective column center coordinates calculated by the reflective column point cloud segmentation and fitting method are set C. The point set C is transformed from the robot base coordinate system to the map coordinate system using the position and posture of each particle obtained by state prediction. The reflective column center coordinate sets corresponding to each particle after transformation are C0, C1, C2, ..., C N , taking C0 as an example, let The coordinate point set of all reflective column centers on the reflective column map Using a one-to-one comparison without replacement, take out a reflective column center coordinate point from C0 Calculate the coordinate points of the center of the reflective column in sequence The Euclidean distance from each coordinate point of the point set M, and take the one with the minimum distance from M At the same time, record the corresponding index pairs <i, j>, and repeat this process until all the center coordinates of the reflective column in C0 have been mapped to the reflective column map.

10. A reflective column positioning method based on particle filtering as claimed in claim 9, characterized in that: The weight calculation is based on the corresponding relationship <i, j> index pair, is the mean, with a certain variance and Construct a bivariate Gaussian distribution and calculate The probability density value in this distribution is, And use it as weight: in Then the weight of the 0th particle is: Similarly, the weights of all other particles can be calculated, and the particle with the highest weight represents the best posterior pose estimate of the robot.

Images