CN-116698029-B - Laser radar indoor positioning method based on vector weight

Abstract

The invention provides a laser radar indoor positioning method based on vector weights, which comprises the steps of scanning a site by a total station to obtain total station original point cloud data, constructing laser radar point cloud data, projecting a laser radar point cloud model and a total station point cloud model, processing features, respectively carrying out overlapping convolution operation on a total station projection grid diagram and the laser radar projection grid diagram, selecting reference areas to be screened, carrying out data processing on point clouds of the reference areas to be screened, carrying out surface fitting processing on points subjected to interpolation and smoothing processing to obtain total station primary screening reference areas, carrying out iterative screening on total station reference features according to primary screening results of the total station primary screening reference areas, mapping the screened total station reference features to corresponding areas of the laser radar point cloud data, and calculating angles and position deviations by the weights of vectors. The invention adopts the point set weight to compensate the angle and position error, and measures the offset angle and position to be corrected.

Inventors

• ZHANG CHUNLIANG
• CHEN YANGZHOU
• WENG RUNTING
• YUE XIA
• LI ZIHAN
• ZHU HOUYAO
• ZHENG ZHONGZHI
• WANG YADONG
• HUANG CANRONG
• ZHONG GUOCHANG

Assignees

• 广州大学

Dates

Publication Date

20260508

Application Date

20230505

Claims (5)

1. 1. The laser radar indoor positioning method based on the vector weight is characterized by comprising the following steps of: s1, scanning a site by a total station to obtain original high-precision static total station point cloud data, removing redundant information in the total station original data, and filtering noise; s2, constructing laser radar point cloud data; s3, projecting the laser radar point cloud model and the total station point cloud model, and screening and eliminating weak features in the projection graph; s4, performing overlapping degree convolution operation on the total station projection grid diagram and the laser radar projection grid diagram respectively, taking the maximum convolution result obtained by the operation as the matching degree of the laser radar point cloud and the total station point cloud around the image rotation center under the global coordinate system, and rotating the total station point cloud projection, so that the primary matching of the total station point cloud and the laser radar point cloud is completed; S5, after the degree of preliminary matching is obtained through overlap convolution operation, selecting the area with the most stable characteristics and the most abundant characteristic information as the reference area to be screened according to the actual distribution of the site, performing interpolation filtering processing on the point clouds of the reference areas to be screened, setting the grid size according to the position boundary value of each reference area to be screened, adopting a natural neighborhood interpolation method to fill the grid points in the point cloud boundary, and performing convolution smoothing processing after the interpolation processing; s6, performing surface fitting treatment on the points subjected to interpolation and smoothing treatment to obtain a total station point cloud point set meeting the reference requirement, and taking the total station point cloud point set meeting the requirement as a point cloud set of a total station primary screening reference area; S7, performing iterative screening on the reference features of the total station again according to the primary screening result of the primary screening reference region of each total station, and setting a screening feature interval of the t th iteration before performing iterative screening on the reference features of the total station Area threshold Simultaneously calculating the contour area S of the screening characteristic interval; The specific step of calculating the contour area S of the screening feature interval in S7 includes: first, initializing screening characteristic intervals Screening the characteristic interval The size of the interval is consistent with that of a primary screening reference area of the total station; Second step, screening characteristic interval Traversing a primary screening reference area of the total station, and judging whether non-reference features exist or not; In the third step, the third step is that, let i=i+1; If i/3 is more than 1, then Is reduced and returns to the first step; If i/3 is more than 2, then Is reduced and returns to the first step; If i/3 remains 0, then t=t+1, will Both the width and length regions of (a) are reduced and updated to Then jumping to the first step; After the characteristic fine screening is carried out, screening out the reference characteristics of k total stations, and then comparing and adjusting the reference characteristics of the total stations and the scanning information of the laser radar by using a vector weight method; The vector weighting method specifically comprises the following steps: (a) Extracting k laser radar reference features positioned under the same global coordinate according to the global coordinate position of each total station reference feature, and then respectively performing surface fitting on the reference features of the selected k laser radars to obtain surface fitting parameters of the corresponding laser radar reference features And (2) and And the normal vector of the fitting surface is , Simultaneously calculating centroid of reference feature of t-th laser radar , Wherein, the For the ith point in the tth lidar reference feature, Calculating the volume of each laser radar reference feature according to the outline of the laser radar reference feature ; (B) Fitting surface normal vector of laser radar reference feature Normal vector to reference feature of corresponding total station Sequentially projecting to yoz, xoz and xoy planes, taking the angle difference of projection included angles projected to yoz, xoz and xoy planes as rotation angles of x, y and z axes in sequence, and enabling the rotation angle of the x axis to be the rotation angle of the x axis Rotation angle of y-axis Rotation angle of z-axis The calculation formula of the rotation angle of each shaft is as follows: Wherein, the , , Normal vector for reference feature of t-th total station Parameters of (2); (c) Calculating the rotation weight of each shaft: First define a function Then the x-axis rotation weight of the t-th lidar reference feature Y-axis rotation weight of t-th laser radar reference feature The z-axis rotation weight of the t-th lidar reference feature, wherein, (1,0,0), (0,1,0), (0, 1) A unit vector representing global coordinates; (d) Rotational weights of the shafts obtained in accordance with a selected manner , , Calculating the global rotation angle of the laser radar, Global rotation angles around an x axis, a y axis and a z axis of the laser radar are respectively set; (e) The point cloud scanned by the laser radar rotates around the z axis in sequence Degree, then rotate about the y-axis Finally, rotate around the x-axis Degree, and center of mass after rotation Substituting a surface fitting equation of the corresponding total station reference feature to calculate the mass center of the kth laser radar reference feature Distance deviation to corresponding total station reference feature , The translation amounts of the x-axis, the y-axis and the z-axis are calculated as follows: (f) According to the amount of displacement of each axis To translate and update centroids of k lidar reference features Then the updated centroid Continuing substituting the surface fitting equation of the corresponding total station reference feature to calculate the mass center of the kth laser radar reference feature Distance deviation to corresponding total station reference feature ; (G) If it is Translation of x-axis Translation of the y-axis Translation of the z-axis And (3) entering a step (h) for the current optimal translation amount of the laser radar point cloud, otherwise, continuously calculating the translation amount of the x-axis, the translation amount of the y-axis and the translation amount of the z-axis as follows Then jump to the sixth step until the optimal global translation of the laser radar point cloud is found ; (H) According to Translating the laser radar data, and then updating the laser radar point cloud data; (i) Take the last several iterations at a time And analyzing the result, if the standard characteristic condition is met, shrinking the standard characteristic, otherwise iterating until the standard characteristic is converged or the maximum iteration number is reached, and then exiting the iteration.
2. 2. The method for indoor positioning of lidar based on vector weights according to claim 1, wherein the step S2 comprises: s21, site environment is scanned in a fixed point mode, and laser radar point cloud data of multiple frames of different visual angles are generated; S22, splicing laser radar point cloud data of multiple frames of different visual angles; And S23, eliminating redundant data in the original point cloud data of the laser radar, and constructing the point cloud data of the laser radar after point cloud preprocessing.
3. 3. The method for indoor positioning of lidar of claim 2, wherein the point cloud preprocessing comprises a filtering operation.
4. 4. The method for indoor positioning of lidar based on vector weights according to claim 1, wherein the step S3 specifically comprises rotating the total station point cloud projection map information clockwise by 359 times with a step length of 1 degree about a rotation axis of an image center, and storing the map information of 0 degree and 359 times of rotation in 360 grid images, respectively.
5. 5. The method for indoor positioning of lidar based on vector weights according to claim 1, wherein the fitting process in S6 is specifically: S61N points exist for a point set in three-dimensional point cloud space N is more than 2, N is the total point number in the point set; Obtaining a point cloud set position average value by averaging N points of a three-dimensional point cloud set Is that , , The coordinate data of the x axis, the y axis and the z axis of the ith point respectively; Vector quantity Is that The first to sixth square components thus have the following formulas, xx, yy, zz, xy, xz, yz, respectively: ; S62, making the square component matrix be And performs singular value decomposition on the CM by SVD, Wherein the singular value third solution is Let vector parameter n=v, then , ; The equation for the fitting plane is derived as Wherein a is a first plane parameter, b is a second plane parameter, c is a third plane parameter, d is a fourth plane parameter, and the singular value first solution is U; s63, substituting each point of the point cloud set into an equation to obtain the distance from the ith point to the fitting plane , If it is The point is a satisfactory fiducial point in the point cloud set, otherwise, the point is an unsatisfactory point in the point cloud set, Is a distance threshold; S64, carrying out surface fitting again on the datum points meeting the requirements in the point cloud set to obtain updated fitting plane parameters a, b, c and d, so that the updated fitting plane equation is 。

Description

Laser radar indoor positioning method based on vector weight Technical Field The invention belongs to the field of laser radar positioning, and particularly relates to a laser radar indoor positioning method based on vector weights. Background With the rapid development of robot technology, more and more scientific products such as unmanned vehicles, unmanned aerial vehicles and the like start to enter the life of people, and one important technology for realizing the intellectualization of various mobile robots is positioning. In practical application, when the mobile robot faces to complex scenes, such as illumination transformation, many dynamic obstacles and large fusion error of multiple sensors, tracking failure is easy to cause, and the positioning of the mobile robot is further affected. At present, the laser radar indoor positioning technology cannot reach the positioning precision specified by the operation standard, so that indoor decoration operation cannot depend on a mobile robot, and the mobile equipment positioning technology for operations such as wall polishing cannot improve the vertical precision and the horizontal precision to the construction requirements, so that the high-precision operations completely depend on manual processing. Most of the existing laser radar positioning technologies combine with vehicle-mounted auxiliary equipment such as cameras, millimeter wave radars, GPS positioning systems, bluetooth, geomagnetism, ultrasonic radars, UWB modules, inertial navigation systems and the like to jointly position, but data information among different equipment is inconsistent, a great deal of time is required for processing data, and the different vehicle-mounted auxiliary equipment has respective local coordinates, and the mobile equipment own coordinates are based on conversion among global coordinates of the environment, so that positioning errors can be increased without fail, and the positioning errors cannot be controlled within construction requirement errors. Therefore, when the method is performed by combining different vehicle-mounted auxiliary equipment, the problems of complex algorithm, low efficiency, large positioning error and the like exist. Disclosure of Invention The invention aims to provide a laser radar indoor positioning method based on vector weight and global matching, which compensates angle and position errors by adopting the vector weight, corrects the measured offset angle and position, and accurately controls the degree to be less than 1.1 degrees and the position to be less than 9 mm. In order to achieve the above purpose, the present invention provides a laser radar indoor positioning method based on vector weight, comprising: S1, scanning a site by a total station to obtain original high-precision static total station original point cloud data, removing redundant information in the total station original data, and filtering noise; s2, constructing laser radar point cloud data; s3, projecting the laser radar point cloud model and the total station point cloud model, and screening and eliminating weak features in the projection graph; S4, performing overlapping degree convolution operation on the total station projection grid diagram and the laser radar projection grid diagram respectively, taking the maximum convolution result obtained by the operation as the matching degree of the laser radar point cloud and the total station point cloud around the image rotation center under the global coordinate system, and rotating the total station point cloud projection, so that the primary matching of the total station point cloud and the laser radar point cloud is completed; S5, after the degree of preliminary matching is obtained through the overlapping degree convolution operation, selecting the area with the most stable characteristics and the most abundant characteristic information as the reference area to be screened according to the actual distribution of the site, performing interpolation filtering processing on the point clouds of the reference areas to be screened, setting the grid size according to the position boundary value of each reference area to be screened, adopting a natural field interpolation method to fill the grid points in the point cloud boundary, and performing convolution smoothing processing after the interpolation processing; s6, performing surface fitting treatment on the points subjected to interpolation and smoothing treatment to obtain a total station point cloud point set meeting the reference requirement, and taking the total station point cloud point set meeting the requirement as a point cloud set of a total station primary screening reference area; S7, iteratively screening the reference features of the total station again according to the primary screening result of the primary screening reference region of each total station, setting a screening feature interval epsilon t of the t-th iteration and an area threshold S r