A method and system for detecting deviation between laser mapping odometer and integrated navigation positioning

By processing and fitting laser mapping and combining navigation track points, and calculating and outputting coordinates of points with larger deviations, the problem of low detection efficiency of laser odometer and combined navigation tracks is solved, and the detection efficiency of high-precision maps is improved and quality enhance the positioning accuracy of driverless vehicles.

CN113532472BActive Publication Date: 2024-11-19BEIJING ZHIXINGZHE TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202010293823.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2020-04-15
Publication Date
2024-11-19
Estimated Expiration
2040-04-15

AI Technical Summary

Technical Problem

In the prior art, laser odometer and combined navigation trajectory have deviations in high-precision map production, resulting in point cloud maps that do not match the actual environment, affect the positioning effect, and lack effective deviation detection methods, resulting in low detection efficiency and false detection rate. high.

Method used

By preprocessing the coordinates of the laser mapping odometer and combined navigation track points, a deviation equation is established, the least squares method is used to fit the track points, the deviation value of the track point is calculated, and the sliding window and threshold are used to determine the point of the larger deviation. Coordinates, output the coordinates of its corresponding point.

Benefits of technology

It realizes rapid positioning and correcting abnormal points in high-precision maps, improves the efficiency and quality of automated detection of high-precision maps, and enhances the positioning accuracy and reliability of unmanned vehicles.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113532472B_ABST
    Figure CN113532472B_ABST
Patent Text Reader

Abstract

The present invention discloses a method for detecting the deviation between a laser mapping odometer and a combined navigation positioning, comprising the following steps: (1) preprocessing of trajectory data; (2) establishing a deviation equation; (3) calculating the deviation value of trajectory points; (4) judging and outputting the coordinates of points with larger differences. Corresponding to the above method, the present invention provides a system for detecting the deviation between a laser mapping odometer and a combined navigation positioning. The application of the present invention can realize rapid positioning to a certain point where the deviation between the two is large, and give the deviation value, thereby greatly improving the mapping efficiency in the process of high-precision map construction; at the same time, for quality inspection personnel, it can more efficiently achieve the quality of high-precision maps in large-scale scenarios, and can effectively control the accuracy and reliability of unmanned driving laser positioning, thereby promoting the application of high-precision maps in large-scale scenarios.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of high-precision maps for driverless vehicles, and in particular to a method and system for detecting deviations between a laser mapping odometer and combined navigation positioning. Background Art

[0002] With the continuous improvement of science and technology in recent years, artificial intelligence has developed rapidly and has been widely used in various fields. Among them, since autonomous driving vehicles can efficiently utilize traffic resources, alleviate traffic congestion and reduce carbon emissions, autonomous driving technology has become more and more the focus of people's attention. At present, passenger car autonomous driving is still a long way from true commercialization, and small, low-speed sanitation sweepers in a limited environment provide a specific application scenario for the implementation of autonomous driving technology. On the other hand, due to the aggravation of the aging population, the cost of domestic labor has been rising year by year, and heavy repetitive physical labor has increased people's workload. For example, for environmental sanitation cleaning operations in parks, campuses, large shopping malls, industrial parks and other scenes, sanitation workers need to perform repetitive physical labor for a long time, and the work is heavy. Therefore, it is unstoppable to replace heavy manual cleaning with intelligent unmanned automatic cleaning.

[0003] Small low-speed sanitation sweepers can achieve high-precision positioning, path planning and other technologies that are inseparable from high-precision maps. The key to high-precision maps lies in the technology of creating point cloud maps, that is, simultaneous localization and mapping (SLAM) based on laser sensors. The point cloud map is constructed by matching point cloud frames between 3D laser matching algorithms (the trajectory formed by matching becomes the laser odometer), and combined navigation (GPS, wheel speed meter, inertial measurement unit and other multi-sensor fusion methods) observation information is combined to optimize the point cloud map. However, in actual applications, due to the influence of environmental, terrain and human factors, the laser odometer and the combined navigation trajectory cannot be completely overlapped. Excessive deviation between the two will result in: 1. The point cloud map does not match the actual physical environment 2. The point cloud map has local ghosting and distortion, which affects the laser matching and multi-sensor fusion positioning effect.

[0004] However, there is no mature solution for detecting deviations between different trajectories in high-precision map production. Most of them calculate the trajectory points of 3D laser matching and MSF trajectory points respectively, and then establish two interpolation curves by curve fitting and visualize them. In high-precision map production and quality inspection, the deviation of the above two trajectories is completely judged by the map builders based on their experience. Although it is possible to detect large deviations within a certain area or a certain time period by relying on the experience of map builders to judge the deviation changes of two trajectory points, it is inefficient and prone to false detection. It is impossible to judge the error size of the two and the specific location of abnormal points in the trajectory points in the order of tens of thousands, which seriously affects the construction quality and detection effect of high-precision maps. Summary of the invention

[0005] The present invention provides a method and system for detecting the deviation between the laser mapping odometer and the integrated navigation positioning, which processes the coordinates (discrete) of the trajectory points of the two and realizes their smooth transition, then finds out the points with large deviations between the two by setting a threshold, and outputs the coordinates of their corresponding points, thereby helping high-precision map builders to quickly locate abnormal points in the map and effectively improve the automatic detection of high-precision maps.

[0006] To achieve the purpose of the present invention, the present invention provides a method for detecting the deviation between a laser mapping odometer and an integrated navigation positioning, characterized in that it comprises the following steps:

[0007] (1) Preprocessing of trajectory data;

[0008] (2) Establish the deviation equation;

[0009] (3) Calculate the deviation value of the trajectory point;

[0010] (4) Determine and output the coordinates of the point with the largest difference.

[0011] Preferably, in step (1), a laser point cloud map is first established, the point cloud is reasonably analyzed, and the density of the point cloud is controlled by setting the distance of the key frame. After the point cloud map is initially established, the trajectory data of the map acquisition platform is calculated by the laser matching method, and the stored data includes the position and heading information; the effectiveness of the combined navigation is judged, and its trajectory data is saved, which includes the position and heading information.

[0012] Preferably, in step (2), the laser matching trajectory is locally fitted using the least squares method. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows:

[0013]

[0014] Establish a laser matching trajectory point array queue, fill it with the position coordinates of a trajectory points in sequence, establish the straight line equation through the least squares formula, then remove 1 data at the head of a trajectory point, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in sequence; similarly, in the case of time alignment, the combined navigation trajectory line also uses the same method to establish N straight line equations in sequence.

[0015] Preferably, in step (3), N straight line equations of one of the two trajectories, the laser matching trajectory and the combined navigation trajectory, are selected, and then the trajectory points of the relative trajectory are used to obtain the deviations of the two trajectories through the distance formula from the point to the straight line.

[0016] Preferably, in step (4), the coordinates of the trajectory points are organized into a queue, a sliding window is established in the queue, b values ​​of the data after the difference between the position and the heading are sequentially taken and put into the sliding window, the variance of the data in the sliding window is calculated and recorded, the first data in the window will pop up in real time, and new dead position interpolation values ​​will be added sequentially at the end of the window, and so on, so that the variance of the dead position change corresponding to each point on the entire trajectory line can be obtained, and by comparing the dead position variance with a preset threshold, the position of the larger point where the two trajectories deviate can be determined, and the coordinate value of the corresponding trajectory point is output according to the ID of the trajectory point.

[0017] Corresponding to the above method, the present invention provides a system for detecting the deviation between the laser mapping odometer and the integrated navigation positioning, comprising:

[0018] (1) Trajectory data preprocessing unit;

[0019] (2) Establishing the deviation equation unit;

[0020] (3) Calculating the deviation value unit of the trajectory point;

[0021] (4) Determine and output the coordinate unit of the point with the largest difference.

[0022] Preferably, the trajectory data preprocessing unit is used to first establish a laser point cloud map, reasonably analyze the point cloud, control the density of the point cloud by setting the distance of the key frame, and after the point cloud map is initially established, calculate the trajectory data of the map acquisition platform by the laser matching method, and the stored data includes the position and heading information; judge the effectiveness of the combined navigation, and save its trajectory data, which includes the position and heading information.

[0023] Preferably, the deviation equation establishing unit is used to locally fit the laser matching trajectory using the least squares method. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows:

[0024]

[0025] Establish a laser matching trajectory point array queue, fill it with the position coordinates of a trajectory points in sequence, establish the straight line equation through the least squares formula, then remove 1 data at the head of a trajectory point, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in sequence; similarly, in the case of time alignment, the combined navigation trajectory line also uses the same method to establish N straight line equations in sequence.

[0026] Preferably, the trajectory point deviation value calculation unit is used to select N straight line equations of one of the two trajectories, the laser matching trajectory and the combined navigation trajectory, and then use the trajectory points of the relative trajectory to obtain the deviations of the two trajectories through the point-to-straight line distance formula.

[0027] Preferably, the unit for judging and outputting the coordinates of the points with larger differences is used to form a queue of the coordinates of the trajectory points, establish a sliding window in the queue, sequentially take b values ​​of the data after the position and heading are subtracted and put into the sliding window, calculate the variance of the data in the sliding window and record it, the first data in the window will pop up in real time, and add new navigation interpolation values ​​to the end of the window in sequence, and so on, so as to obtain the variance of the navigation change corresponding to each point on the entire trajectory line, and by comparing the navigation variance with a preset threshold, the position of the point where the two trajectories deviate more significantly can be determined, and the coordinate value of the corresponding trajectory point can be output according to the ID of the trajectory point.

[0028] Compared with the prior art, the beneficial effect of the present invention is that the application of the present invention can achieve rapid positioning of a certain point where the deviation between the two is large, and give the deviation value, thereby greatly improving the efficiency of high-precision map construction in the process of high-precision map construction; at the same time, for quality inspection personnel, the quality of high-precision maps in large-scale scenarios can be more efficiently achieved, and the accuracy and reliability of unmanned driving laser positioning can be effectively controlled, thereby promoting the application of high-precision maps in large-scale scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1Shown is a schematic diagram of the method flow of the present application;

[0030] Figure 2 Shown is a schematic diagram of the laser matching and combined navigation trajectory of this application;

[0031] Figure 3 Shown is a schematic diagram of the trajectory line and trajectory points of this application;

[0032] Figure 4 Shown is a schematic diagram of trajectory deviation calculation for this application;

[0033] Figure 5 Shown is a schematic diagram of the trajectory deviation calculation results of this application. DETAILED DESCRIPTION

[0034] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application may be combined with each other.

[0035] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0036] Example 1

[0037] The present invention provides a method for detecting the deviation between the laser mapping odometer and the integrated navigation positioning, comprising the following four steps: Figure 1 As shown,

[0038] S1. Preprocessing of trajectory data

[0039] First, a laser point cloud map is established, and the point cloud is reasonably analyzed. The density of the point cloud is controlled by setting the distance of the key frame. After the point cloud map is initially established, the trajectory data of the map acquisition platform is calculated by the laser matching method. The stored data contains the position (x, y) and heading information, such as Figure 2 As shown, the effectiveness of the combined navigation is then judged, and its trajectory data is saved, which also includes x, y and heading information, such as Figure 3 shown.

[0040] S2. Establish the deviation equation

[0041] Since the laser matching trajectory points are not linear, but are discretely distributed on both sides of the trajectory line, according to their distribution characteristics, the least squares method proposed by Heinrich Olbers is used to locally fit the trajectory. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows:

[0042]

[0043] For the rationality of calculation, we first establish an array queue, fill it with the x and y coordinates of a trajectory points in order, establish the straight line equation through the least squares formula, then remove 1 data from the head of the a trajectory points, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in order. Similarly, in the case of time alignment, the combined navigation trajectory also uses the same method to establish N straight line equations in order. The value of a above can be 10, 9 or 8, and there is no limit on the number here.

[0044] S3. Calculate the deviation value of the trajectory point

[0045] Since the laser matching trajectory points and the combined navigation trajectory points are matched by timestamps, N sets of straight line pairs can be established between the two trajectory lines. You only need to select the N straight line equations of one of the two trajectories, and then use the trajectory points of the relative trajectory line and the distance formula from the point to the straight line to get the deviation of the two trajectories. The trajectory deviation calculation is as follows: Figure 4 shown.

[0046] S4. Determine and output the coordinates of the point with the largest difference

[0047] The coordinates of the trajectory points are organized into a queue, and a sliding window is established in the queue. The data after the difference between the position and the heading are sequentially taken as b values ​​and put into the sliding window. The variance of the data in the sliding window is calculated and recorded. The first data in the window will pop up in real time, and new position (position, heading) interpolation values ​​will be added sequentially at the end of the window. Similarly, the variance of the position change corresponding to each point on the entire trajectory line can be obtained. By comparing the position variance with the preset threshold, the position of the larger point where the two trajectories deviate can be determined, and the coordinate value of the corresponding trajectory point can be output according to the ID of the trajectory point, such as Figure 5 It should be noted that the value of a above can be 5, 6 or 7, and there is no limit on the number.

[0048] This embodiment reads 10 trajectory points on the trajectory line in timestamp order to establish a straight line equation, and performs cyclic elimination and filling to maximize the elimination of the influence of local jumps on the calculation accuracy of trajectory deviation, and realizes the design method of distance calculation under the smoothest condition; wherein, the trajectory points on the trajectory line use the least squares method to fit the local straight line equation between discrete points to calculate the distance between the discrete points and the fitted trajectory line; this embodiment designs a sliding window of fixed length to effectively filter the influence of abnormal points in the trajectory on the heading; a calculation method for introducing variance when obtaining heading changes; in the sliding window, the variance of the array composed of each navigation point and the two navigation points before and after it is calculated, and the variance represents the fluctuation of the heading data at this point. When judging the change amplitude of the two trajectory lines, this embodiment is designed to judge the size of the heading variance value and the maximum value of the position deviation to automatically detect the range points with large deviations between the two trajectories.

[0049] Corresponding to the above method, the present invention also provides a system for detecting the deviation between the laser mapping odometer and the integrated navigation positioning, comprising:

[0050] (1) Trajectory data preprocessing unit;

[0051] (2) Establishing the deviation equation unit;

[0052] (3) Calculating the deviation value unit of the trajectory point;

[0053] (4) Determine and output the coordinate unit of the point with the largest difference.

[0054] Preferably, the trajectory data preprocessing unit is used to first establish a laser point cloud map, reasonably analyze the point cloud, control the density of the point cloud by setting the distance of the key frame, and after the point cloud map is initially established, calculate the trajectory data of the map acquisition platform by the laser matching method, and the stored data includes the position and heading information; judge the effectiveness of the combined navigation, and save its trajectory data, which includes the position and heading information.

[0055] Preferably, the deviation equation establishing unit is used to locally fit the laser matching trajectory using the least squares method. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows:

[0056]

[0057] Establish a laser matching trajectory point array queue, fill it with the position coordinates of a trajectory points in sequence, establish the straight line equation through the least squares formula, then remove 1 data at the head of a trajectory point, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in sequence; similarly, in the case of time alignment, the combined navigation trajectory line also uses the same method to establish N straight line equations in sequence.

[0058] Preferably, the trajectory point deviation value calculation unit is used to select N straight line equations of one of the two trajectories, the laser matching trajectory and the combined navigation trajectory, and then use the trajectory points of the relative trajectory to obtain the deviations of the two trajectories through the point-to-straight line distance formula.

[0059] Preferably, the unit for judging and outputting the coordinates of the points with larger differences is used to form a queue of the coordinates of the trajectory points, establish a sliding window in the queue, sequentially take b values ​​of the data after the position and heading are subtracted and put into the sliding window, calculate the variance of the data in the sliding window and record it, the first data in the window will pop up in real time, and add new navigation interpolation values ​​to the end of the window in sequence, and so on, so as to obtain the variance of the navigation change corresponding to each point on the entire trajectory line, and by comparing the navigation variance with a preset threshold, the position of the point where the two trajectories deviate more significantly can be determined, and the coordinate value of the corresponding trajectory point can be output according to the ID of the trajectory point.

[0060] The above is only a preferred embodiment of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for detecting the deviation between laser mapping odometer and integrated navigation positioning, characterized in that: The steps include: (1) Preprocessing of trajectory data; (2) Establish the deviation equation; (3) Calculate the deviation value of the trajectory point; (4) Determine and output the coordinates of the point with the largest difference; In step (2), the least squares method is used to perform local fitting on the laser matching trajectory. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows: Establish a laser matching trajectory point array queue, fill it with the position coordinates of a trajectory points in order, establish the straight line equation through the least squares formula, then remove 1 data from the head of a trajectory point, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in order; similarly, in the case of time alignment, the combined navigation trajectory line also uses the same method to establish N straight line equations in order; In step (3), select N straight line equations of one of the two trajectories, the laser matching trajectory and the combined navigation trajectory, and then use the trajectory points of the relative trajectory to obtain the deviation of the two trajectories through the distance formula from the point to the straight line; In step (4), the coordinates of the trajectory points are organized into a queue, a sliding window is established in the queue, and the data after the difference between the position and the heading are sequentially taken as b values ​​and placed in the sliding window. The variance of the data in the sliding window is calculated and recorded. The first data in the window will pop up in real time, and new dead position interpolation values ​​will be added sequentially at the end of the window. By analogy, the variance of the dead position change corresponding to each point on the entire trajectory line can be obtained. By comparing the dead position variance with the preset threshold, the position of the larger point where the two trajectories deviate can be determined, and the coordinate value of the corresponding trajectory point can be output according to the ID of the trajectory point.

2. A method for detecting the deviation between laser mapping odometer and integrated navigation positioning according to claim 1, characterized in that: In step (1), a laser point cloud map is first established, and the point cloud is reasonably analyzed. The density of the point cloud is controlled by setting the distance of the key frame. After the point cloud map is initially established, the trajectory data of the map acquisition platform is calculated by the laser matching method. The stored data includes the position and heading information; the effectiveness of the combined navigation is judged, and its trajectory data, which includes the position and heading information, is saved.

3. A system for detecting deviations between laser mapping odometer and integrated navigation positioning, characterized in that: include: (1) Trajectory data preprocessing unit; (2) Establishing the deviation equation unit; (3) Calculating the deviation value unit of the trajectory point; (4) Determine and output the coordinate unit of the point with the largest difference; The deviation equation establishment unit is used to locally fit the laser matching trajectory using the least squares method. Given a data point p i (x i ,y i ,z i ), where i = 1, 2, ..., m, find the approximate curve And make the deviation between the approximate curve and y = f (x) minimum, the approximate curve at point p i Deviation The curve fitting formula is as follows: Establish a laser matching trajectory point array queue, fill it with the position coordinates of a trajectory points in order, establish the straight line equation through the least squares formula, then remove 1 data from the head of a trajectory point, and finally fill in a data at the tail in a loop, and so on to establish N straight line equations in order; similarly, in the case of time alignment, the combined navigation trajectory line also uses the same method to establish N straight line equations in order; The trajectory point deviation value calculation unit is used to select N straight line equations of one of the two trajectories of the laser matching trajectory and the combined navigation trajectory, and then use the trajectory points of the relative trajectory to obtain the deviations of the two trajectories through the point-to-line distance formula; The unit for judging and outputting the coordinates of the points with larger differences is used to form a queue of the coordinates of the trajectory points, establish a sliding window in the queue, sequentially take b values ​​of the data after the position and heading are subtracted and put into the sliding window, calculate the variance of the data in the sliding window and record it, the first data in the window will pop up in real time, and add new dead position interpolation values ​​to the end of the window in sequence, and so on, so as to obtain the variance of the dead position change corresponding to each point on the entire trajectory line, and by comparing the dead position variance with a preset threshold, the position of the point with the larger deviation between the two trajectories can be judged, and the coordinate value of the corresponding trajectory point is output according to the ID of the trajectory point.

4. A system for detecting deviations between laser mapping odometer and integrated navigation positioning according to claim 3, characterized in that: The trajectory data preprocessing unit is used to first establish a laser point cloud map, reasonably analyze the point cloud, control the density of the point cloud by setting the distance of the key frame, and after the point cloud map is initially established, calculate the trajectory data of the map acquisition platform by the laser matching method, and the stored data includes position and heading information; judge the effectiveness of the combined navigation and save its trajectory data, which includes position and heading information.