Fusion positioning method and device based on real-time dynamic measurement, medium and equipment

Abstract

The application discloses a fusion positioning method based on real-time dynamic measurement RTK, and obtains a fusion positioning key frame of a mobile robot, wherein the fusion positioning key frame comprises laser point cloud data collected by a 3D laser radar at the same time, SLAM pose information based on the 3D laser radar, odometer pose information and RTK positioning information; the validity of the RTK positioning information is judged; when the RTK positioning information is valid, the distribution of the laser point cloud data is analyzed to obtain weight information of the RTK positioning information; thus, the distribution of surrounding buildings is obtained, the strength and weakness of multipath interference are quantitatively given, the weight information of the positioning information collected by the RTK device is obtained, fusion positioning is carried out according to the weight information, the RTK positioning information and the SLAM pose information, and a fusion positioning result is output; the influence of the multipath effect of the RTK device on the fusion positioning result is effectively weakened, and the positioning precision of the fusion RTK laser SLAM technology is effectively improved.

Description

Fusion positioning methods, devices, media, and equipment based on real-time dynamic measurement Technical Field

[0001] This invention relates to the field of robotics, and in particular to a fusion positioning method, apparatus, medium, and equipment based on real-time dynamic measurement (RTK). Background Technology

[0002] Outdoor patrol robots need to first construct point cloud maps of the robot's working environment. Existing technologies mainly use Simultaneous Localization and Mapping (SLAM) technology based on 3D LiDAR. When necessary, Real-time Kinematic (RTK) technology is added to integrate SLAM technology to address the impact of adverse factors such as numerous dynamic interference objects and open spaces in some working environments on positioning accuracy.

[0003] As is well known, the positioning accuracy of RTK technology is positively correlated with the number of satellites acquired and the strength of the differential signal, but it also suffers from errors that RTK equipment cannot actively predict. Among these, due to the influence of the surrounding building environment, the satellite signals received by the receiver also contain various reflected and refracted signals, the so-called multipath effect. The multipath effect has the most severe impact on laser SLAM technology that integrates RTK, leading to anything from decreased positioning accuracy to, in severe cases, incorrect positioning data. However, current methods for laser SLAM integrating RKT do not consider the error caused by the multipath effect of RTK equipment, resulting in unsatisfactory positioning accuracy. Summary of the Invention

[0004] This invention provides a fusion positioning method, apparatus, medium, and device based on real-time dynamic measurement (RTK) to solve the positioning error problem caused by the "multipath effect" of RTK equipment when performing laser SLAM using fusion RKT in the prior art.

[0005] A fusion localization method based on real-time dynamic measurement (RTK), the method comprising:

[0006] Acquire the fusion localization key frame of the mobile robot. The fusion localization key frame includes laser point cloud data collected by 3D LiDAR at the same time, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0007] When the number of frames of the fused positioning keyframes reaches a preset threshold, the validity of the RTK positioning information is determined based on the odometry pose information.

[0008] When the RTK positioning information is valid, the weight information of the RTK positioning information is obtained by analyzing the distribution of the laser point cloud data.

[0009] Based on the weight information, RTK positioning information and SLAM pose information, a fusion positioning is performed, and the fusion positioning result is output.

[0010] Optionally, before acquiring the fusion localization keyframes of the mobile robot, the method further includes:

[0011] External parameter calibration is performed on the sensors of the 3D LiDAR and the antenna of the RTK device.

[0012] Optionally, obtaining the fusion localization keyframes of the mobile robot includes:

[0013] Acquire laser point cloud data collected by 3D LiDAR, SLAM pose information based on 3D LiDAR, odometry pose information, and RTK positioning information;

[0014] Calculate the difference between two adjacent odometer pose information based on the odometer pose information and its acquisition time.

[0015] When the difference is greater than or equal to the preset pose difference threshold, the laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information and the RTK positioning information at the next acquisition time are saved as a fusion positioning key frame.

[0016] If the difference is greater than or equal to the preset pose difference, continue to calculate the difference between the pose information of two adjacent odometers in the next set.

[0017] Optionally, determining the validity of the RTK positioning information based on the odometer pose information includes:

[0018] Obtain the i-th fused localization keyframe and the preceding n-1 consecutive fused localization keyframes to obtain n fused localization keyframes;

[0019] Odometry pose information is obtained from the n fused positioning keyframes to obtain n odometry pose information;

[0020] A straight line is fitted based on the horizontal coordinates of the n odometer pose information;

[0021] Calculate the distance information between the horizontal coordinate of the RTK positioning information in the i-th fused positioning keyframe and the straight line;

[0022] When the distance information is less than a preset distance threshold, the RTK positioning information in the i-th fused positioning keyframe is a valid value;

[0023] When the distance information is greater than or equal to a preset distance threshold, the RTK positioning information in the i-th fusion positioning key frame is invalid, and the (i+1)-th fusion positioning key frame and the preceding n-1 consecutive fusion positioning key frames are obtained for validity determination.

[0024] Optionally, when the RTK positioning information is valid, analyzing the distribution of the laser point cloud data to obtain the weight information of the RTK positioning information includes:

[0025] For valid RTK positioning information, acquire lidar data collected at the same time as the RTK positioning information;

[0026] Obtain the initial screening laser points from the lidar data;

[0027] Cluster analysis was performed on the initial screening laser points to obtain several clusters;

[0028] Calculate the total number of initial screening laser points contained in each cluster to obtain the target number of points;

[0029] The number of point clouds collected by the 3D LiDAR in each frame is obtained, and the weight information of the RTK positioning information is calculated based on the number of target points and the number of point clouds collected in each frame.

[0030] Optionally, each cluster obtained from the cluster analysis satisfies the following condition:

[0031] The number of initial screening laser points in each cluster is greater than the preset threshold number of points;

[0032] The difference between the maximum and minimum values ​​of the vertical coordinates of the initial screening laser points in each cluster is greater than the preset vertical coordinate threshold.

[0033] The difference between the maximum and minimum values ​​of the abscissa of the initial screening laser points in each cluster is greater than the preset abscissa threshold.

[0034] The difference between the maximum and minimum values ​​of the ordinate of the initial screening laser points in each cluster is greater than the preset ordinate threshold.

[0035] Optionally, the weight information of the RTK positioning information is:

[0036] Among them, w i N represents the weight information of the i-th RTK positioning information. i This represents the number of target points corresponding to the laser point cloud data in the i-th fusion positioning keyframe, and M represents the number of point clouds collected by the 3D LiDAR in each frame.

[0037] A fusion positioning device based on real-time dynamic measurement (RTK), the device comprising:

[0038] The acquisition module is used to acquire the fusion positioning key frame of the mobile robot. The fusion positioning key frame includes laser point cloud data collected by 3D LiDAR at the same time, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0039] The judgment module is used to determine the validity of the RTK positioning information based on the odometry pose information when the number of frames of the fused positioning keyframes reaches a preset number threshold.

[0040] The analysis module is used to analyze the distribution of the laser point cloud data to obtain the weight information of the RTK positioning information when the RTK positioning information is valid.

[0041] The fusion localization module is used to perform fusion localization based on the weight information, RTK localization information and SLAM pose information, and output the fusion localization result.

[0042] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the fusion positioning method based on real-time dynamic measurement (RTK) as described above.

[0043] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the fusion positioning method based on real-time dynamic measurement (RTK) as described above.

[0044] This invention acquires keyframes for fusion localization of a mobile robot. These keyframes include laser point cloud data collected by a 3D LiDAR, SLAM pose information based on the 3D LiDAR, odometry pose information, and positioning information collected by an RTK device at the same time. When the number of keyframes reaches a preset threshold, the validity of the positioning information collected by the RTK device is determined. If the positioning information is valid, the distribution of the laser point cloud data is analyzed to obtain the distribution of surrounding buildings, quantitatively assess the strength of multipath interference, and obtain the weight information of the positioning information collected by the RTK device. Finally, fusion localization is performed based on the weight information, positioning information, and SLAM pose information, and the fusion localization result is output. This effectively reduces the impact of the multipath effect of the RTK device on the fusion localization result and effectively improves the positioning accuracy of the fusion RTK laser SLAM technology. Attached Figure Description

[0045] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0046] Figure 1 is a schematic diagram of a fusion positioning method based on real-time dynamic measurement (RTK) provided in an embodiment of the present invention;

[0047] Figure 2 is a schematic diagram of step S101 of the fusion positioning method based on real-time dynamic measurement (RTK) provided in an embodiment of the present invention;

[0048] Figure 3 is a flowchart of step S102 of the fusion positioning method based on real-time dynamic measurement (RTK) provided in an embodiment of the present invention.

[0049] Figure 4 is a flowchart of step S103 of the fusion positioning method based on real-time dynamic measurement (RTK) provided in an embodiment of the present invention.

[0050] Figure 5 is a schematic diagram of a fusion positioning device based on real-time dynamic measurement (RTK) provided in an embodiment of the present invention;

[0051] Figure 6 is a schematic diagram of a computer device according to an embodiment of the present invention. Detailed Implementation

[0052] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0053] In existing technologies, the positioning information acquired by RTK devices suffers from positioning errors due to the "multipath effect." Since current laser SLAM technologies that integrate RTK do not consider the varying degrees of multipath effect affecting the mobile robot due to its different positions, this invention proposes a fusion positioning method based on real-time dynamic measurement. Before using the positioning information acquired by the RTK device, the strength of the multipath effect on the mobile robot at its current position is analyzed. Then, based on the strength of the multipath effect at different stages, the weight information of the positioning information acquired by the RTK device is quantitatively given. The fusion positioning is then performed by combining the weight information, RTK positioning information, and SLAM pose information, and the fusion positioning result is output. This effectively reduces the impact of the multipath effect of the RTK device on the fusion positioning result, and effectively improves the positioning accuracy of laser SLAM technologies that integrate RTK.

[0054] The following provides a detailed description of the fusion positioning method based on real-time dynamic measurement provided in this embodiment of the invention. In this embodiment, the mobile robot is equipped with an RTK device, a 3D LiDAR, and an odometer. The 3D LiDAR is mounted parallel to the mobile robot. The mobile robot establishes a three-dimensional coordinate system with the origin of the LiDAR as the origin of the coordinate system, where the X-axis points towards the front of the mobile robot, the Y-axis points towards the left of the mobile robot, and the Z-axis points towards the top of the mobile robot.

[0055] The RTK device, 3D LiDAR, and odometry continuously acquire data in real time. The 3D LiDAR collects laser point cloud data through a laser emission path. Each laser point is represented by a three-dimensional coordinate system, and the SLAM pose information (slam_x, slam_y, slam_z, slam_yaw, slam_pitch, slam_roll) of the mobile robot is calculated using 3D laser SLAM. The odometry acquires and outputs odom pose information (odom_x, odom_y, odom_z, odom_yaw, odom_pitch, odom_roll), where x, y, and z represent the coordinate components on the x-axis, y-axis, and z-axis, respectively, and yaw, pitch, and roll correspond to the rotational components on the z-axis, y-axis, and x-axis, respectively. The RTK device, based on real-time dynamic positioning technology using carrier phase observations, acquires the three-dimensional positioning information of the mobile robot in real time to obtain RTK positioning information, which consists of three components RTK_x, RTK_y, and RTK_z on the x-axis, y-axis, and z-axis, respectively.

[0056] Figure 1 illustrates a fusion positioning method based on real-time dynamic measurement provided by an embodiment of the present invention. As shown in Figure 1, the fusion positioning method based on real-time dynamic measurement includes:

[0057] In step S101, the fusion positioning key frame of the mobile robot is obtained. The fusion positioning key frame includes laser point cloud data collected by 3D LiDAR at the same time, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0058] The fusion positioning keyframe refers to the effective data frame used to achieve fusion RTK in laser SLAM. Each fusion positioning keyframe includes the number of laser point clouds, SLAM pose information, odometry pose information, and RTK device positioning information acquired at the same time. Optionally, before acquiring the fusion positioning keyframe of the mobile robot, this embodiment of the invention can also perform extrinsic parameter calibration on the 3D LiDAR sensor and the RTK device antenna. By performing extrinsic parameter calibration between the RTK device antenna and the 3D LiDAR sensor in advance, arithmetic operations can be performed on the positioning data acquired by the RTK device and the positioning data output by the 3D LiDAR, directly adding or subtracting them, which helps to improve the computational efficiency of positioning.

[0059] As mentioned above, the fusion localization keyframe refers to the effective data frame used to achieve fusion RTK laser SLAM. Optionally, as a preferred example of the present invention, the effective data frame is determined and obtained by changes in odometry pose information, as shown in Figure 2. Step S101, obtaining the fusion localization keyframe of the mobile robot, includes:

[0060] In step S201, the laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information, and the RTK positioning information are acquired.

[0061] In this embodiment of the invention, when the mobile robot initiates laser SLAM fused with RTK, the first acquired laser point cloud data, SLAM pose information, odometry pose information, and RTK positioning information are saved as the 0th fused positioning keyframe. Then, the robot continues to continuously acquire laser point cloud data, SLAM pose information, odometry pose information, and RTK positioning information.

[0062] In step S202, the difference between two adjacent odometer pose information is calculated based on the odometer pose information and its acquisition time.

[0063] For non-first-time data acquisition, this embodiment of the invention calculates the difference |odom(x, y, z) between two adjacent odometry pose information sets. j -odom(x, y, z) j-1 |, where j represents the acquisition sequence corresponding to the acquisition time.

[0064] In step S203, when the difference is greater than or equal to a preset pose difference threshold, the laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information, and the RTK positioning information at the next acquisition time are saved as a fusion positioning keyframe.

[0065] The pose difference threshold is the criterion for determining whether the odometry pose information has changed, i.e., the criterion for determining whether the mobile robot has moved. If the difference between two adjacent pose information values ​​is odom(x, y, z)... j -odom(x, y, z) j-1 If the position difference is greater than or equal to the preset pose difference threshold, the movement of the mobile robot is considered valid. The laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information, and the positioning information collected by the RTK device at the next acquisition time are saved as a fusion positioning key frame, denoted as the i-th (i = 1, 2, 3...) fusion positioning key frame.

[0066] Optionally, the preset pose difference threshold is preferably 0.4.

[0067] After saving the fusion positioning keyframes, return to step S202 to calculate the difference between the next set of adjacent odometry pose information.

[0068] In step S204, if the difference is greater than or equal to a preset pose difference, the difference between the pose information of two adjacent odometers in the next set is calculated.

[0069] Here, if the difference between two adjacent pose information is |odom(x, y, z) j -odom(x, y, z) j-1 If the difference is less than the preset pose difference threshold, the movement of the mobile robot is considered invalid. The laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information, and the positioning information collected by the RTK device are invalid and discarded. The process then returns to step S202 to calculate the difference between the next set of adjacent odometry pose information.

[0070] In some embodiments, if the mobile robot remains stationary and the data collected by the RTK device, 3D LiDAR, and odometry remain unchanged, there is no need to repeat the fusion RTK laser SLAM. Through the above steps S201 to S204, the embodiments of the present invention can effectively detect valid keyframes, use the valid keyframes for fusion positioning, and remove invalid keyframes, which is beneficial to improving the efficiency of fusion positioning.

[0071] In step S102, when the number of frames of the fused positioning keyframes reaches a preset threshold, the validity of the RTK positioning information is determined based on the odometry pose information.

[0072] The validity refers to the fact that the positioning information collected by the RTK device does not change, and the fusion positioning based on the RTK positioning information that does not change has physical meaning, thereby improving the accuracy of the fusion positioning.

[0073] When the number of fused positioning keyframes obtained through step S101 reaches a preset threshold, this embodiment of the invention performs a validity judgment on each acquired fused positioning keyframe. Here, the preset threshold is the same as the number of fused positioning keyframes required for the validity judgment; for example, the preset threshold can be 3.

[0074] Optionally, as a preferred example of the present invention, as shown in FIG3, the determination of the validity of the RTK positioning information in step S102 includes:

[0075] In step S301, the i-th fusion positioning key frame and the preceding n-1 consecutive fusion positioning key frames are obtained to obtain n fusion positioning key frames.

[0076] Where i ≥ n-1, and n is a positive integer, representing the number of fused positioning keyframes required for validity determination. When determining the validity of the positioning data of the i-th fused positioning keyframe, this embodiment of the invention obtains the i-th fused positioning keyframe and the preceding n-1 consecutive fused positioning keyframes, for a total of n fused positioning keyframes, as the object of validity determination.

[0077] For example, assuming that the number of fused positioning key frames n required for the validity judgment is 3, then according to i≥n-1=3-1=2, we can obtain 3 fused positioning key frames from the second fused positioning key frame onwards, for the i-th fused positioning key frame, the i-th fused positioning key frame and the two consecutive fused positioning key frames before it, i.e. the i-1th and i-2th fused positioning key frames, for a total of 3 fused positioning key frames, which are used as the objects of validity judgment.

[0078] In step S302, odometry pose information is obtained from the n fused positioning keyframes to obtain n odometry pose information.

[0079] For the obtained n fusion positioning keyframes, extract n odometry pose information from them respectively.

[0080] Following the previous example, for the i-th, i-1-th, and i-2-th fused localization keyframes, the i-th, i-1-th, and i-2-th odometry pose information are obtained respectively.

[0081] In step S303, a straight line is fitted based on the horizontal coordinates of the n odometer pose information.

[0082] Following the previous example, this embodiment of the invention uses the horizontal coordinates (odom_x) in the i-th, (i-1)-th, and (i-2)-th odometry pose information. i ,odom_y i (odom_x) i-1 ,odom_y i-1 (odom_x) i-2 ,odom_y i-2 The line Kx + B = y is fitted to the x-axis, where K represents the slope of the line, B represents the intercept of the line, x represents the x-axis variable, and y represents the y-axis variable.

[0083] In step S304, the distance information between the horizontal coordinate of the RTK positioning information in the i-th fused positioning keyframe and the straight line is calculated.

[0084] The distance information between a point and a line represents the point's alignment with or away from the line. After fitting the line, the horizontal coordinate (RTK_x) of the positioning information in the i-th fused positioning keyframe is calculated using the following formula. i RTK_y i The distance information d between the line and the line is calculated using the following formula:

[0085]

[0086] In step S305, when the distance information is less than a preset distance threshold, the RTK positioning information in the i-th fused positioning keyframe is a valid value.

[0087] The preset distance threshold is the criterion for determining whether the positioning information collected by the RTK device is valid, as it does not exhibit any abrupt changes. In this embodiment, the distance information is compared with the preset distance threshold. If the distance information is within the preset distance threshold range, the i-th RTK positioning information is considered to change smoothly relative to a straight line and is therefore a valid value, and step S103 is executed.

[0088] In step S306, when the distance information is greater than or equal to a preset distance threshold, the RTK positioning information in the i-th fusion positioning key frame is invalid, and the (i+1)-th fusion positioning key frame and the preceding n-1 consecutive fusion positioning key frames are obtained for validity determination.

[0089] If the distance information is greater than or equal to the preset distance threshold, it indicates that the i-th RTK positioning information has a large jitter relative to the straight line and a jump occurs. The i-th RTK positioning information is invalid. Then, the laser SLAM processing for this fused RTK is abandoned, and the (i+1)-th fused positioning key frame and the previous n-1 consecutive fused positioning key frames are acquired. Step S301 is executed.

[0090] In step S103, when the RTK positioning information is valid, the distribution of the laser point cloud data is analyzed to obtain the weight information of the RTK positioning information.

[0091] The weight information represents the reliability of the RTK positioning information; a larger weight indicates more reliable RTK positioning information, while a smaller weight indicates less reliable RTK positioning information. Since the "multipath effect" of RTK devices is mainly affected by buildings, trees, etc., which are higher than the antenna on the RTK device, this embodiment of the invention calculates the strength of the "multipath effect" of the RTK device based on laser points higher than the antenna in the laser point cloud data, thereby obtaining the weight information of the RTK positioning information.

[0092] Optionally, as a preferred example of the present invention, as shown in FIG4, the weight information for obtaining the RTK positioning information by analyzing the distribution of the laser point cloud data in step S103 includes:

[0093] In step S401, for valid RTK positioning information, LiDAR data collected at the same time as the RTK positioning information is acquired.

[0094] In step S402, the initial screening laser points are obtained from the lidar data.

[0095] In this embodiment, the initial screening laser points are those whose vertical axis height is greater than or equal to the antenna installation height of the RTK device. For the weight information of the i-th RTK positioning information, when the RTK positioning information is valid through step S102, this embodiment first obtains the lidar data in the i-th fused positioning keyframe. The lidar data includes several laser points, each represented by three-dimensional coordinates (x, y, z). Based on the vertical axis coordinate z of each laser point, laser points greater than or equal to the antenna installation height of the RTK device are selected. Assuming the antenna installation height of the RTK device is h, then the initial screening laser points are those where z ≥ h.

[0096] In step S403, cluster analysis is performed on the initial screening laser points to obtain several clusters.

[0097] After obtaining the initial screening laser points, point cloud clustering is performed on these points to obtain several clusters. Each cluster obtained from the clustering analysis satisfies the following condition:

[0098] The number of initial screening laser points in each cluster is greater than the preset threshold number of points;

[0099] The difference between the maximum and minimum values ​​of the vertical coordinates of the initial screening laser points in each cluster is greater than the preset vertical coordinate threshold.

[0100] The difference between the maximum and minimum values ​​of the abscissa of the initial screening laser points in each cluster is greater than the preset abscissa threshold.

[0101] The difference between the maximum and minimum values ​​of the ordinate of the initial screening laser points in each cluster is greater than the preset ordinate threshold.

[0102] Optionally, as a preferred example of the present invention, the preset point threshold is preferably 100, and the preset vertical coordinate threshold, preset horizontal coordinate threshold, and preset vertical coordinate threshold are all 2.

[0103] In step S404, the total number of initial screening laser points contained in each cluster is calculated to obtain the target number of points.

[0104] In this process, the initial screening laser points are clustered into multiple clusters through step S403. Each cluster is traversed to obtain the number of initial screening laser points contained in each cluster. Then, the number of initial screening laser points in each cluster is summed, and the sum is used as the target number of points. A larger target number indicates a stronger multipath effect of the RTK device, while a smaller target number indicates a weaker multipath effect.

[0105] In step S405, the number of point clouds collected by the 3D LiDAR in each frame is obtained, and the weight information of the RTK positioning information is calculated based on the number of target points and the number of point clouds collected in each frame.

[0106] The number of point clouds collected per frame by the 3D LiDAR is theoretically the number of laser points obtained by the 3D LiDAR scanning one frame, and is an inherent parameter of the 3D LiDAR. For example, for a 16-line 3D LiDAR, the number of point clouds collected per frame is 19,200 laser points. In this embodiment of the invention, the weight information of the RTK positioning information is calculated based on the number of target points and the number of point clouds collected per frame. The calculation formula is as follows:

[0107] Among them, w i N represents the weight information of the i-th RTK positioning information. i This represents the number of target points corresponding to the laser point cloud data in the i-th fusion positioning keyframe, and M represents the number of point clouds collected by the 3D LiDAR in each frame.

[0108] The above weight information calculation formula shows that, since the number of point clouds collected by the 3D LiDAR in each frame is fixed, the larger the number of target points, the stronger the multipath effect of the RTK device, and the smaller the weight information of the RTK positioning information, resulting in lower reliability; conversely, the smaller the number of target points, the weaker the multipath effect of the RTK device, and the larger the weight information of the RTK positioning information, resulting in higher reliability.

[0109] In step S104, fusion localization is performed based on the weight information, RTK localization information and SLAM pose information, and the fusion localization result is output.

[0110] Here, the fusion localization refers to the fusion of RTK and laser SLAM. After obtaining the weight information of the RTK localization information through step S103 above, weights are assigned to the RTK localization information and SLAM pose information according to the weight information to obtain the fusion localization result. The calculation formula for fusion localization is as follows:

[0111] pose i =(1-w i )*pose lidar-i +w i pose RTK-i

[0112] In the above formula, pose i The i-th fusion localization result is represented by pose. lidar-i The pose represents the SLAM pose information in the i-th fused localization keyframe. RTK-i This represents the RTK positioning information in the i-th fused positioning keyframe.

[0113] The fusion localization method based on real-time dynamic measurement (RTK) provided in this invention fully considers the problem of varying intensities of RTK device multipath effects at different locations of the mobile robot during SLAM. It calculates the strength of the RTK device's multipath effect in real time and assigns different weights to the RTK localization information during the fusion phase based on these strengths. i For example, large parks typically have relatively open, slightly open, and not open areas. In not open areas, due to the strong multipath effect of RTK devices, the accuracy of RTK positioning information is low. Therefore, assigning smaller weight information can effectively reduce the impact of the multipath effect of RTK devices on the accuracy of fusion positioning results and improve the accuracy of fusion positioning.

[0114] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0115] In one embodiment, the present invention also provides a fusion positioning device based on real-time dynamic measurement (RTK), which corresponds one-to-one with the fusion positioning method based on real-time dynamic measurement (RTK) in the above embodiments. As shown in FIG5, the fusion positioning device based on real-time dynamic measurement (RTK) includes an acquisition module 51, a judgment module 52, an analysis module 53, and a fusion positioning module 54. Detailed descriptions of each functional module are as follows:

[0116] The acquisition module 51 is used to acquire the fusion positioning key frame of the mobile robot. The fusion positioning key frame includes laser point cloud data collected by 3D LiDAR at the same time, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0117] The judgment module 52 is used to determine the validity of the RTK positioning information based on the odometry pose information when the number of frames of the fused positioning keyframes reaches a preset number threshold.

[0118] Analysis module 53 is used to analyze the distribution of the laser point cloud data to obtain the weight information of the RTK positioning information when the RTK positioning information is valid;

[0119] The fusion localization module 54 is used to perform fusion localization based on the weight information, RTK localization information and SLAM pose information, and output the fusion localization result.

[0120] Optionally, before acquiring the fusion localization keyframes of the mobile robot, the following steps are also included:

[0121] The calibration module is used to calibrate the external parameters of the sensors of the 3D LiDAR and the antennas of the RTK device.

[0122] Optionally, the acquisition module 51 includes:

[0123] The acquisition unit is used to acquire laser point cloud data collected by 3D LiDAR, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0124] The calculation unit is used to calculate the difference between two adjacent odometer pose information based on the odometer pose information and its acquisition time.

[0125] The storage unit is used to save the laser point cloud data collected by the 3D LiDAR, the SLAM pose information based on the 3D LiDAR, the odometry pose information and the RTK positioning information at the next acquisition time as a fusion positioning key frame when the difference is greater than or equal to a preset pose difference threshold.

[0126] The calculation unit is also used to, if the difference is greater than or equal to a preset pose difference, continue to calculate the difference between two adjacent odometer pose information in the next set.

[0127] Optionally, the determination module 52 includes:

[0128] The keyframe acquisition unit is used to acquire the i-th fused positioning keyframe and the preceding n-1 consecutive fused positioning keyframes to obtain n fused positioning keyframes.

[0129] The odometer pose information acquisition unit is used to acquire odometer pose information from the n fused positioning keyframes respectively, and obtain n odometer pose information.

[0130] A fitting unit is used to fit a straight line based on the horizontal coordinates of the n odometer pose information;

[0131] The distance calculation unit is used to calculate the distance information between the horizontal coordinate of the RTK positioning information in the i-th fused positioning keyframe and the straight line.

[0132] The determination unit is used to determine that the RTK positioning information in the i-th fused positioning keyframe is a valid value when the distance information is less than a preset distance threshold.

[0133] The keyframe acquisition unit is further configured to, when the distance information is greater than or equal to a preset distance threshold, invalidate the RTK positioning information in the i-th fused positioning keyframe, and continue to acquire the (i+1)-th fused positioning keyframe and the preceding n-1 consecutive fused positioning keyframes for validity determination.

[0134] Optionally, the analysis module 53 includes:

[0135] The lidar data acquisition unit is used to acquire lidar data collected at the same time as the valid RTK positioning information for valid RTK positioning information.

[0136] The initial screening unit is used to obtain initial screening laser points from the lidar data;

[0137] Clustering unit, used to perform cluster analysis on the initial screening laser points to obtain several clusters;

[0138] The summation calculation unit is used to calculate the sum of the number of initial screening laser points contained in each cluster to obtain the target number of points;

[0139] The weight calculation unit is used to obtain the number of point clouds collected by the 3D LiDAR in each frame, and calculate the weight information of the RTK positioning information based on the number of target points and the number of point clouds collected in each frame.

[0140] Optionally, each cluster obtained from the cluster analysis satisfies the following condition:

[0141] The number of initial screening laser points in each cluster is greater than the preset threshold number of points;

[0142] The difference between the maximum and minimum values ​​of the vertical coordinates of the initial screening laser points in each cluster is greater than the preset vertical coordinate threshold.

[0143] The difference between the maximum and minimum values ​​of the abscissa of the initial screening laser points in each cluster is greater than the preset abscissa threshold.

[0144] The difference between the maximum and minimum values ​​of the ordinate of the initial screening laser points in each cluster is greater than the preset ordinate threshold.

[0145] Optionally, the weight information of the RTK positioning information is:

[0146] Among them, w i N represents the weight information of the i-th RTK positioning information. i This represents the number of target points corresponding to the laser point cloud data in the i-th fusion positioning keyframe, and M represents the number of point clouds collected by the 3D LiDAR in each frame.

[0147] Specific limitations regarding the fusion positioning device based on real-time dynamic measurement (RTK) can be found in the limitations of the fusion positioning method based on RTK mentioned above, and will not be repeated here. Each module in the aforementioned fusion positioning device based on RTK can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in hardware or independently of the processor in a computer device, or stored in memory as software, so that the processor can call and execute the corresponding operations of each module.

[0148] In one embodiment, a computer device, which may be a server, is provided, and its internal structure is shown in Figure 6. The computer device includes a processor, memory, a network interface, and a database connected via a system bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it implements a fusion positioning method based on real-time dynamic measurement (RTK).

[0149] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to perform the following steps:

[0150] Acquire the fusion localization key frame of the mobile robot. The fusion localization key frame includes laser point cloud data collected by 3D LiDAR at the same time, SLAM pose information based on 3D LiDAR, odometry pose information and RTK positioning information.

[0151] When the number of frames of the fused positioning keyframes reaches a preset threshold, the validity of the RTK positioning information is determined based on the odometry pose information.

[0152] When the RTK positioning information is valid, the weight information of the RTK positioning information is obtained by analyzing the distribution of the laser point cloud data.

[0153] Based on the weight information, RTK positioning information and SLAM pose information, a fusion positioning is performed, and the fusion positioning result is output.

[0154] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0155] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is used as an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0156] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A fusion positioning method based on real-time dynamic measurement (RTK), characterized in that, The method includes: acquiring fusion localization keyframes of a mobile robot, wherein the fusion localization keyframes include laser point cloud data collected by a 3D LiDAR at the same time, SLAM pose information based on the 3D LiDAR, odometry pose information, and RTK positioning information; when the number of fusion localization keyframes reaches a preset threshold, determining the validity of the RTK positioning information based on the odometry pose information; when the RTK positioning information is valid, analyzing the distribution of the laser point cloud data to obtain weight information of the RTK positioning information; performing fusion localization based on the weight information, RTK positioning information, and SLAM pose information, and outputting the fusion localization result; wherein, when the RTK positioning information is valid, analyzing the distribution of the laser point cloud data to obtain weight information of the RTK positioning information; and ...3D LiDAR, SLAM pose information, and RTK positioning information to obtain weight information of the 3D LiDAR, SLAM pose information, and RTK positioning information, the fusion localization result is obtained; and when the RTK positioning information is valid, analyzing the distribution of the laser point cloud data to obtain weight information of the 3D LiDAR, SLAM pose information, and RTK positioning information, the fusion localization result is obtained; and when the RTK positioning information is valid, analyzing the distribution of the laser point cloud data to obtain weight information of the 3D LiDAR, SLAM pose information, and RTK positioning information, the fusion localization result is obtained; and when the RTK positioning information is valid, The weight information of the RTK positioning information obtained from the distribution of the data includes: for valid RTK positioning information, acquiring LiDAR data collected at the same time as the RTK positioning information; obtaining initial screening laser points from the LiDAR data; performing cluster analysis on the initial screening laser points to obtain several clusters; calculating the sum of the number of initial screening laser points contained in each cluster to obtain the target number of points, wherein the target number of points is obtained by traversing each cluster, acquiring the number of initial screening laser points contained in each cluster, and then summing the number of initial screening laser points contained in each cluster, and using the sum as the target number of points; acquiring the number of point clouds collected by the 3D LiDAR in each frame, and calculating the weight information of the RTK positioning information based on the target number of points and the number of point clouds collected in each frame.

2. The fusion positioning method based on real-time dynamic measurement (RTK) as described in claim 1, characterized in that, Before acquiring the fusion localization keyframes of the mobile robot, the method further includes: calibrating the external parameters of the sensor of the 3D LiDAR and the antenna of the RTK device.

3. The fusion positioning method based on real-time dynamic measurement (RTK) as described in claim 2, characterized in that, The process of acquiring the fusion localization keyframes for the mobile robot includes: acquiring laser point cloud data collected by 3D LiDAR, SLAM pose information based on 3D LiDAR, odometry pose information, and RTK positioning information; calculating the difference between two adjacent odometry pose information based on the odometry pose information and its acquisition time; when the difference is greater than or equal to a preset pose difference threshold, saving the laser point cloud data collected by 3D LiDAR, SLAM pose information based on 3D LiDAR, odometry pose information, and RTK positioning information at the next acquisition time as a fusion localization keyframe; if the difference is greater than or equal to a preset pose difference value, continuing to calculate the difference between the next set of two adjacent odometry pose information.

4. The fusion positioning method based on real-time dynamic measurement (RTK) as described in claim 3, characterized in that, The step of determining the validity of the RTK positioning information based on the odometer pose information includes: acquiring the i-th fused positioning keyframe and the preceding n-1 consecutive fused positioning keyframes to obtain n fused positioning keyframes; acquiring odometer pose information from each of the n fused positioning keyframes to obtain n odometer pose information; fitting a straight line based on the horizontal coordinates of the n odometer pose information; calculating the distance information between the horizontal coordinates of the RTK positioning information in the i-th fused positioning keyframe and the straight line; when the distance information is less than a preset distance threshold, the RTK positioning information in the i-th fused positioning keyframe is a valid value; when the distance information is greater than or equal to the preset distance threshold, the RTK positioning information in the i-th fused positioning keyframe is invalid, and the validity determination is performed by acquiring the (i+1)-th fused positioning keyframe and the preceding n-1 consecutive fused positioning keyframes.

5. The fusion positioning method based on real-time dynamic measurement (RTK) as described in claim 1, characterized in that, Each cluster obtained by cluster analysis satisfies the following conditions: the number of initial screening laser points in each cluster is greater than the preset threshold; the difference between the maximum and minimum values ​​of the vertical coordinates of the initial screening laser points in each cluster is greater than the preset threshold. The difference between the maximum and minimum values ​​of the abscissa of the initial screening laser points in each cluster is greater than the preset abscissa threshold. The difference between the maximum and minimum values ​​of the ordinate of the initial screening laser points in each cluster is greater than the preset ordinate threshold.

6. The fusion positioning method based on real-time dynamic measurement (RTK) as described in claim 1, characterized in that, The weight information of the RTK positioning information is as follows: ;in, This represents the weight information of the i-th RTK positioning information. This represents the number of target points corresponding to the laser point cloud data in the i-th fused localization keyframe. This indicates the number of point clouds collected by the 3D LiDAR in each frame.

7. A fusion positioning device based on real-time dynamic measurement (RTK), characterized in that, The device includes: an acquisition module for acquiring fusion positioning keyframes of a mobile robot, the fusion positioning keyframes including laser point cloud data collected by a 3D LiDAR at the same time, SLAM pose information based on the 3D LiDAR, odometry pose information, and RTK positioning information; a judgment module for judging the validity of the RTK positioning information based on the odometry pose information when the number of frames of the fusion positioning keyframes reaches a preset threshold; and an analysis module for analyzing the distribution of the laser point cloud data to obtain the weight information of the RTK positioning information when the RTK positioning information is valid, specifically including: for valid RTK positioning information, acquiring laser point cloud data collected at the same time as the RTK positioning information. The system uses lidar data; it obtains initial screening lidar points from the lidar data; it performs cluster analysis on the initial screening lidar points to obtain several clusters; it calculates the sum of the number of initial screening lidar points contained in each cluster to obtain the target number of points, wherein the target number of points is obtained by traversing each cluster, obtaining the number of initial screening lidar points contained in each cluster, and then summing the number of initial screening lidar points contained in each cluster, and using the sum as the target number of points; it obtains the number of point clouds collected by the 3D lidar in each frame, and calculates the weight information of the RTK positioning information based on the target number of points and the number of point clouds collected in each frame; and it uses a fusion positioning module to perform fusion positioning based on the weight information, RTK positioning information and SLAM pose information, and outputs the fusion positioning result.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the fusion positioning method based on real-time dynamic measurement (RTK) as described in any one of claims 1 to 6.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the fusion positioning method based on real-time dynamic measurement (RTK) as described in any one of claims 1 to 6.

Images