Outdoor mobile robot positioning method based on unscented Kalman filtering

Abstract

The invention relates to the field of autonomous navigation and robots, in particular to an outdoor mobile robot positioning method based on unscented Kalman filtering, which comprises the following steps: 1) data acquisition and initialization; 2) based on an unscented Kalman filter, calculating a new mean value and covariance to update a system state; 3) substituting the new sigma point set into the observation equation to obtain a predicted observation quantity; obtaining a new mean value and a new covariance by using a weighting mode, and calculating a Kalman gain; updating the system state and the covariance; 4) when the AGV is shielded, adding a 3D laser radar point cloud inter-frame registration result as an observation value into a trace Kalman filter to replace RTK data; multiplexing an unscented Kalman filter, searching an initial position on the priori point cloud map, and completing the initialization of the filter; and 5) carrying out inter-frame matching on the laser radar point cloud, and updating a system state by combining a position value predicted by IMU data. According to the branch and bound search method based on RTK, the initialization problem in an outdoor large scene is effectively solved.

Description

Technical Field

[0001] The invention relates to the field of autonomous navigation and robots, and in particular to an outdoor mobile robot positioning method based on unscented Kalman filtering. Background Art

[0002] With the rapid development of intelligent robot technology, outdoor mobile robots have shown broad application prospects in industrial automation, logistics distribution, environmental monitoring and other fields. However, the complexity and dynamic changes of the outdoor environment put forward higher requirements on the positioning accuracy and robustness of the robot. In order to solve this problem, this paper proposes an outdoor mobile robot positioning method based on unscented Kalman filter, which realizes high-precision positioning of the robot in outdoor environment by integrating sensor data such as global navigation satellite system (GNSS), inertial measurement unit (IMU), and laser radar (LiDAR).

[0003] Mobile robots need to be able to perceive their surroundings in real time to ensure that they can identify and avoid obstacles and adapt to different terrain and environmental changes. This includes detecting roads, identifying people and other moving objects, and identifying traffic signals. The Global Navigation Satellite System (GNSS) can provide relatively accurate positioning information in open environments, such as farmland and oceans, to help mobile robots determine their positions. However, the GNSS system has the following disadvantages: (1) The measurement accuracy is greatly affected by the environment. In open outdoor environments, it can only guarantee positioning at the meter level. After using RTK technology to improve the accuracy, the positioning accuracy can reach the centimeter level. However, GNSS signals are easily interfered by the external environment during transmission, resulting in a decrease in positioning accuracy; (2) In urban environments, forests or indoors, obstructions will interfere with or block GNSS signals, causing a significant decrease in positioning accuracy or even complete failure; (3) The data output frequency is low and cannot meet the needs of real-time positioning and navigation. IMU can provide high-frequency acceleration and angular velocity data. By fusing with GNSS data, it can make up for the positioning defects caused by GNSS signal loss or obstruction in a short time. Although IMU data can provide high-precision positioning in a short period of time, its inherent drift and accumulated errors will cause the positioning accuracy to drop rapidly over time, and it cannot be used alone for a long time. Due to the existence of accumulated errors, the fusion of IMU and GNSS cannot be used as a reliable positioning source alone and needs to be used in combination with other positioning technologies.

[0004] The core goal of Simultaneous Localization and Mapping (SLAM) technology is that in an unknown environment, the robot can simultaneously build an environmental map and accurately estimate its own posture. This technology enables the robot to autonomously navigate and plan paths without prior preparation, ensuring that it can complete tasks efficiently and safely in complex environments. Commonly used sensors in SLAM technology include 2D / 3D lidar, visual cameras, and IMU, but the perception capabilities of commonly used sensors are degraded in different environments. IMU can provide high-frequency posture information and measure motion parameters through accelerometers and gyroscopes. However, IMU has a cumulative error problem. Over time, the error continues to accumulate, resulting in a rapid decrease in positioning accuracy, which may eventually lead to positioning failure; visual cameras can obtain rich image feature information and are very effective in identifying and tracking feature points in the environment. However, its disadvantage is that it cannot directly obtain distance information and is easily affected by changes in lighting. Especially when outdoor lighting conditions are unstable, the performance of visual cameras will drop significantly; lidar can obtain accurate three-dimensional point cloud data through optical ranging, and is not affected by lighting, so it is suitable for measurements under various lighting conditions. However, the point cloud data obtained by LiDAR has no texture information and is easily distorted during high-speed motion, which affects the measurement accuracy. Mobile robots often work in complex and changing environments. The SLAM system with a single sensor is difficult to ensure sufficient robustness and accuracy when dealing with complex environments. The inherent defects of different sensors make it difficult for a single sensor to cope with various environmental challenges, resulting in unstable performance of the SLAM system. Summary of the invention

[0005] The purpose of the present invention is to provide an outdoor mobile robot positioning method based on unscented Kalman filtering. The method proposes a fusion positioning system that integrates RTK, IMU and laser radar, and realizes the redundant complementarity of multi-sensor data. The unscented Kalman filter is used to reduce the dimension of the laser radar point cloud, which greatly reduces the occupation of computing resources. The accuracy and efficiency of the fusion positioning method are effectively improved.

[0006] The technical solution of the present invention is: a method for positioning an outdoor mobile robot based on unscented Kalman filtering, comprising the following steps:

[0007] 1) Data collection and initialization:

[0008] Collect RTK data, IMU data and 3D LiDAR data in real time; and process the RTK data, IMU data and 3D LiDAR data respectively;

[0009] 2) Unscented Kalman Filter UKF state prediction:

[0010] Generate a sigma point set using IMU data and UT transformation, and calculate the state prediction values ​​of these points; then calculate the new mean and covariance to update the system state;

[0011] 3) Observation update:

[0012] Substitute the new sigma point set into the observation equation to obtain the predicted observation; use the weighted method to obtain the new mean and covariance, and calculate the Kalman gain; update the system state and covariance;

[0013] 4) AGV processing under occlusion conditions:

[0014] When the AGV is blocked, the result of the 3D lidar point cloud inter-frame registration is added as the observation value to the unscented Kalman filter UKF instead of the RTK data; the unscented Kalman filter in step 2) is reused to complete the filter initialization by searching the initial position on the prior point cloud map;

[0015] 5) Use a multi-threaded registration method to match the lidar point cloud between frames, and update the system status based on the position value predicted by the IMU data.

[0016] The step 1) is specifically:

[0017] 1-1) The RTK measuring instrument, inertial measurement unit IMU and laser radar installed on the AGV respectively collect RTK data, IMU data and 3D laser radar data in real time;

[0018] 1-2) For IMU data, the state of the constructed system is predicted through the IMU's kinematic equations, including: estimating the current position, velocity and attitude;

[0019] 1-3) For 3D lidar data, preprocessing is performed, including smoothing, outlier removal, downsampling, and noise removal, and edge and plane features are extracted;

[0020] 1-4) For RTK data, initial values ​​are provided for global initialization on the prior point cloud map and used as observation values ​​to update the system status.

[0021] The steps 1-2) are specifically:

[0022] In continuous time, the motion equation of the IMU is:

[0023]

[0024] Among them, p is the current position, v is the current velocity, R is the current posture, a is the acceleration collected by the inertial measurement unit IMU, and ω is the angular velocity collected by the inertial measurement unit IMU.

[0025] The 3D laser radar data is preprocessed as follows:

[0026] 1-3-1) For point cloud data with irregular density, the moving least squares method is used to set the point cloud data in the window to obey a Gaussian distribution. For a given window size, a center point is selected in the window, and the distance between all points in the window and the center point is calculated. The point cloud in the window is weighted averaged according to the distance value to obtain the smoothed point cloud data;

[0027] 1-3-2) For outliers caused by occlusion, remove them through radius filtering;

[0028] For each point in the point cloud, calculate the distance from it to its nearest neighbor; if the distance from a point to its nearest neighbor is greater than a certain set radius threshold, then the point is judged as an outlier;

[0029] 1-3-3) For a large amount of point cloud data, the number of point cloud data is reduced by random downsampling;

[0030] 1-3-4) For the noise data, perform filtering and removal operations;

[0031] 1-3-5) Start multiple threads to extract point cloud features, including edge feature extraction and plane feature extraction;

[0032] For edge feature extraction, find those points in the point cloud data whose curvature changes are greater than the set threshold. By calculating the difference between the normal vectors of each point in the point cloud and its neighboring points, the points corresponding to the areas where the normal vectors change dramatically are the edge feature points.

[0033] For plane feature extraction, the normal vector consistency of the point cloud is utilized, and the PCA method is used to find a point set with similar normal vectors within the set geometric range as the plane feature.

[0034] The mathematical model of the unscented Kalman filter UKF is: For nonlinear systems, the fusion positioning system state and observation equations are:

[0035]

[0036] Among them, X(k+1) is the state vector of the system at time k+1, x(k) is the state vector of the system at time k, W(k) is the process noise, which represents the uncertainty in the model prediction process, Z(k) is the observation vector, V(k) is the observation noise, which represents the uncertainty in the observation, f(x(k), W(k)) is the nonlinear state transfer function, and h(x(k), V(k)) is the nonlinear observation function.

[0037] The step 2) comprises the following steps:

[0038] 2-1) The fusion positioning system state is transformed into UT to estimate the mean and covariance under the action of nonlinear function, and the sigma sampling point is obtained as:

[0039] in, is the system state, P(k / k) is the error covariance matrix of the system state, n is the state vector dimension, and λ is the scaling parameter used to adjust the range of the sigma point distribution;

[0040] 2-2) Calculate 2n+1 sigma point sets, namely:

[0041] X (i) (k+1 / k)=f[k,X i (k / k)]

[0042] Among them, X(k+1 / k) is the updated system state;

[0043] 2-3) Prediction of the state quantity of the fusion positioning system, namely:

[0044] Among them, ω is the weight corresponding to the sigma point, and Q is the process noise covariance matrix;

[0045] 2-4) Transform the fusion positioning system state UT again to generate a new sigma point set, namely:

[0046]

[0047] Among them, P(k+1 / k) is the updated system state error covariance matrix.

[0048] The step 3) comprises the following steps:

[0049] 3-1) Substitute the new sigma point set into the observation equation to obtain the observed value of the predicted value:

[0050] Z i (k+1 / k)=h[X i (k+1 / k)]

[0051] Among them, Z(k+1 / k) is the observation vector, h[X i (k+1 / k)] is the observation equation;

[0052] 3-2) The new mean and covariance of the observations are obtained by weighted method:

[0053]

[0054] Where R is the observation noise covariance matrix, is the observation covariance matrix, which represents the correlation between observations. is the cross covariance matrix between the state and the observation, which represents the relationship between the state and the observation;

[0055] 3-3) Calculate the Kalman gain K(k+1) as:

[0056]

[0057] 3-4) Update the state and covariance of the fusion positioning system, that is:

[0058]

[0059] The updated state and covariance are obtained to complete the observation update of the GNSS / INS fusion positioning system. In step 4), the unscented Kalman filter in step 2) is reused to complete the filter initialization by searching the initial position on the prior point cloud map, specifically:

[0060] 4-1) Reduce the dimension of 3D LiDAR point cloud and add it to Kalman filter as observation value instead of RTK data;

[0061] 4-2) Establish a point cloud map. When constructing the point cloud map, save the value pairs between the RTK fixed solution and the key frame index to achieve accurate positioning and navigation;

[0062] 4-3) Compress the 3D point cloud map into a 2D point cloud, and then use the branch and bound search method to reduce the time complexity from O(n 2 ) is reduced to O(logn), and the initial position is searched on the prior map to complete the filter initialization.

[0063] In step 4-3), the initial position is searched on the prior map by using the branch and bound search method, specifically:

[0064] a. The optimization problem of the branch and bound search method is a nonlinear least squares problem, and the problem description is expressed as:

[0065]

[0066] Among them, m Indicates the position of the submap; s represents the pose of the scan data scan; ρ represents the loss function; E represents the residual of the constraint; Indicates the pose of each submap. Represents the pose of each frame scan, ξ ijRepresents the relative pose between the j-th scan that successfully matches the i-th submap; Σ ij Represents the covariance matrix corresponding to the relative pose;

[0067] b. Yes m and s The residual between is optimized, and the constraint used for optimization is ξ ij and the corresponding covariance matrix Σ ij , then the residual E is:

[0068]

[0069] in, Represents the rotation matrix in each submap pose; and Represents the translation vector in each submap and each frame scan pose respectively;

[0070] c. In the branch and bound search method, the scan matching problem is formulated as:

[0071]

[0072] Where W represents the search space range; M nearest Indicates the M value of the grid point corresponding to the point; T ξ Represents the transformation matrix from the laser radar coordinate system to the submap coordinate system; h k Represents all points in a scan;

[0073] d. When calculating the scan matching problem, set the angle step to δ θ , through the cosine theorem, we can get:

[0074]

[0075] Among them, d max It represents the distance from the farthest point from the origin in a scan to the origin; r represents the resolution of the map, that is, the granularity of the search;

[0076] e. Set the search width to W x , W y , the search angle is W θ . Then the number of search steps is:

[0077]

[0078] f. According to the number of search steps, the step search space is:

[0079]

[0080] g. Combined with the binary tree, each node in the tree is defined as Among them, c h Represents the height of the leaf node, and the search space represented by a binary tree, that is:

[0081]

[0082] Among them, j x 、j y Indicates the target point; represents the entire search space of node c; Represents the search space of node c in the binary tree;

[0083] Get the maximum confidence value of the leaf node in the search space, recorded as score(c):

[0084]

[0085] h. By comparing the value of score(c), we can obtain the point in the search space that is most similar to point c, i.e., the initial position.

[0086] In step 5), the laser radar point cloud is matched between frames by a multi-threaded registration method, and the system state is updated in combination with the position value predicted by the IMU data, specifically:

[0087] 5-1) Group and match the extracted feature points using the ICP algorithm;

[0088] In one thread, based on the geometric relationship of feature points, the nearest neighbor points corresponding to the target point cloud are found for the feature points of the source point cloud as the initial matching pairs; then, by minimizing the distance error function between the matching point pairs, the rotation and translation parameters are iteratively optimized so that the positions of the source point cloud and the target point cloud coincide in space;

[0089] 5-2) After the extracted feature points are aligned, the system state is updated based on the initial position value predicted by the IMU.

[0090] The present invention has the following advantages and beneficial effects:

[0091] 1. The present invention integrates multi-sensor data to cope with the changing and complex outdoor environment, giving full play to the advantages of each sensor and improving the accuracy and robustness of the positioning system.

[0092] 2. The present invention uses an unscented Kalman filter to replace the traditional extended Kalman filter, eliminates linearization errors, and does not require the derivation of the Jacobian matrix, which greatly reduces the consumption of computing resources by the algorithm. The resulting lightweight system can be used on embedded platforms.

[0093] 3. The present invention is different from the fusion method using the traditional extended Kalman filter (EKF). The linearization error is eliminated through the unscented Kalman filter, and the unscented transformation is used to deal with the nonlinear transmission problem of the mean and covariance. There is no need to derive the Jacobian matrix, which greatly reduces the computational resource consumption of the algorithm.

[0094] 4. The present invention uses RTK data to perform global initialization on the prior map. After determining the approximate posture, the initial value is quickly determined by the branch and bound search method to complete the filter initialization. The branch and bound search method based on RTK effectively solves the initialization problem in large outdoor scenes.

[0095] 5. The present invention combines the advantages of various sensors to achieve redundant complementarity of multi-sensor data. The mobile robot outdoor positioning method provided by the present invention is oriented to outdoor scenes and provides positioning solutions for outdoor mobile robots such as industrial fields, intelligent logistics, and cleaning service robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] Figure 1 A method flow chart of an outdoor mobile robot positioning method based on unscented Kalman filtering of the present invention;

[0097] Figure 2 This is a framework diagram of the outdoor mobile robot positioning method based on unscented Kalman filtering of the present invention. DETAILED DESCRIPTION

[0098] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and specific implementations.

[0099] The SLAM technology that integrates 3D laser radar and IMU can realize the complementary advantages of the two sensors. By integrating the data of 3D laser radar and IMU, the SLAM system can maintain high accuracy and high robustness in complex and dynamic environments and adapt to various environmental changes and challenges. On this basis, the integration of GNSS / IMU combined navigation can provide global coordinates for the positioning system with higher accuracy. The present invention realizes a robot positioning method that integrates RTK, IMU and laser radar.

[0100] like Figure 2As shown in the figure, it is a framework diagram of the outdoor mobile robot positioning method based on unscented Kalman filtering of the present invention. The present invention proposes an outdoor mobile robot positioning method based on unscented Kalman filtering, which is used to solve the positioning problem of outdoor mobile robots in complex environments. First, an unscented Kalman filter is used to tightly couple and fuse IMU and RTK data; next, a GNSS / INS fusion positioning system is constructed, which can provide centimeter-level positioning for mobile robots in open outdoor scenes; then, RTK data is introduced as the initial value, and the initial position is searched on the prior point cloud map, which simplifies the initialization process; finally, in order to solve the problem of urban canyons, the 3D lidar point cloud is reduced in dimension and added to the filter as an observation value, and a more robust and accurate outdoor mobile robot positioning method is constructed.

[0101] like Figure 1 As shown in FIG. 1 , it is a flow chart of the method of the present invention. The present invention is an outdoor mobile robot positioning method based on unscented Kalman filtering, characterized in that it comprises the following steps:

[0102] Step 1: Data collection and initialization;

[0103] Collect RTK data, IMU data and 3D LiDAR data in real time; and process the RTK data, IMU data and 3D LiDAR data respectively;

[0104] 1-1) The RTK measuring instrument, inertial measurement unit IMU and laser radar installed on the AGV respectively collect RTK data, IMU data and 3D laser radar data in real time;

[0105] 1-2) For IMU data, the state of the constructed system is predicted through the IMU's kinematic equations, including: estimating the current position, velocity and attitude;

[0106] In continuous time, the motion equation of the IMU is:

[0107]

[0108] Among them, p is the current position, v is the current velocity, R is the current posture, a is the acceleration collected by the inertial measurement unit IMU, and ω is the angular velocity collected by the inertial measurement unit IMU.

[0109] 1-3) For 3D lidar data, preprocessing is performed, including smoothing, outlier removal, downsampling, and noise removal, and edge and plane features are extracted;

[0110] 1-3-1) For point cloud data with irregular density, the moving least squares method is used to set the point cloud data in the window to obey a Gaussian distribution. For a given window size, a center point is selected in the window, and the distance between all points in the window and the center point is calculated. The point cloud in the window is weighted averaged according to the distance value to obtain the smoothed point cloud data;

[0111] 1-3-2) For outliers caused by occlusion, remove them through radius filtering;

[0112] For each point in the point cloud, calculate the distance from it to its nearest neighbor; if the distance from a point to its nearest neighbor is greater than a certain radius threshold, then the point is judged as an outlier;

[0113] 1-3-3) For a large amount of point cloud data, the number of point cloud data is reduced by random downsampling;

[0114] 1-3-4) For the noise data, perform filtering and removal operations;

[0115] 1-3-5) Start multiple threads to extract point cloud features, including edge feature extraction and plane feature extraction;

[0116] For edge feature extraction, find those points in the point cloud data whose curvature changes are greater than the set threshold. By calculating the difference between the normal vectors of each point in the point cloud and its neighboring points, the points corresponding to the areas where the normal vectors change dramatically are the edge feature points.

[0117] For plane feature extraction, the normal vector consistency of the point cloud is utilized, and the PCA method is used to find a point set with similar normal vectors within the set geometric range as the plane feature.

[0118] 1-4) For RTK data, initial values ​​are provided for global initialization on the prior point cloud map and used as observation values ​​to update the system status.

[0119] Step 2: Unscented Kalman filter UKF state prediction;

[0120] Generate a sigma point set using IMU data and UT transformation, and calculate the state prediction values ​​of these points; then calculate the new mean and covariance to update the system state;

[0121] 2-1) The fusion positioning system state is transformed into UT to estimate the mean and covariance under the action of nonlinear function, and the sigma sampling point is obtained as:

[0122] in, is the system state, P(k / k) is the error covariance matrix of the system state, n is the state vector dimension, and λ is the scaling parameter used to adjust the range of the sigma point distribution;

[0123] 2-2) Calculate 2n+1 sigma point sets, namely:

[0124] X (i) (k+1 / k)=f[k,X i (k / k)]

[0125] Among them, X(k+1 / k) is the updated system state;

[0126] 2-3) Prediction of the state quantity of the fusion positioning system, namely:

[0127] Among them, ω is the weight corresponding to the sigma point, and Q is the process noise covariance matrix;

[0128] 2-4) Transform the fusion positioning system state UT again to generate a new sigma point set, namely:

[0129]

[0130] Among them, P(k+1 / k) is the updated system state error covariance matrix.

[0131] 3) Observation update:

[0132] Substitute the new sigma point set into the observation equation to obtain the predicted observation; use the weighted method to obtain the new mean and covariance, and calculate the Kalman gain; update the system state and covariance;

[0133] 3-1) Substitute the new sigma point set into the observation equation to obtain the observed value of the predicted value:

[0134] Z i (k+1 / k)=h[X i (k+1 / k)]

[0135] Among them, Z(k+1 / k) is the observation vector, h[X i (k+1 / k)] is the observation equation;

[0136] 3-2) The new mean and covariance of the observations are obtained by weighted method:

[0137]

[0138] Where R is the observation noise covariance matrix, is the observation covariance matrix, which represents the correlation between observations. is the cross covariance matrix between the state and the observation, which represents the relationship between the state and the observation;

[0139] 3-3) Calculate the Kalman gain K(k+1) as:

[0140]

[0141] 3-4) Update the state and covariance of the fusion positioning system, that is:

[0142]

[0143] The updated state and covariance are obtained to complete the observation update of the GNSS / INS fusion positioning system.

[0144] 4) AGV processing under occlusion conditions:

[0145] When the AGV is blocked, the result of the 3D lidar point cloud inter-frame registration is added as the observation value to the unscented Kalman filter UKF instead of the RTK data; the unscented Kalman filter in step 2) is reused to complete the filter initialization by searching the initial position on the prior point cloud map;

[0146] 4-1) Reduce the dimension of 3D LiDAR point cloud and add it to Kalman filter as observation value instead of RTK data;

[0147] 4-2) When building a point cloud map, save the value pair between the RTK fixed solution and the keyframe index. When the system receives the first RTK data, it can quickly derive a rough range of the system's position at this time. However, within this range, there may be hundreds of laser point clouds, corresponding to hundreds of poses. If a brute force search is used, the current laser scan data needs to be used with these hundreds of keyframes for ICP or NDT registration, which will take up a lot of computing resources and waste a lot of time.

[0148] 4-3) To improve efficiency, the 3D point cloud map is first compressed into a 2D point cloud, and then the time and space complexity are reduced by the branch and bound search method, reducing the time complexity from O(n 2 ) is reduced to O(logn).

[0149] The branch and bound method is a coarse-to-fine registration method. Its optimization problem is a nonlinear least squares problem. The problem description is expressed as:

[0150] a. The optimization problem of the branch and bound search method is a nonlinear least squares problem, and the problem description is expressed as:

[0151]

[0152] Among them, mIndicates the position of the submap; s represents the pose of the scan data scan; ρ represents the loss function; E represents the residual of the constraint; Indicates the pose of each submap. Represents the pose of each frame scan, ξ ij Represents the relative pose between the j-th scan that successfully matches the i-th submap; Σ ij Represents the covariance matrix corresponding to the relative pose;

[0153] b. Yes m and s The residual between is optimized, and the constraint used for optimization is ξ ij and the corresponding covariance matrix Σ ij , then the residual E is:

[0154]

[0155] in, Represents the rotation matrix in each submap pose; and Represents the translation vector in each submap and each frame scan pose respectively;

[0156] c. In the branch and bound search method, the scan matching problem is formulated as:

[0157]

[0158] Where W represents the search space range; M nearest Indicates the M value of the grid point corresponding to the point; T ξ Represents the transformation matrix from the laser radar coordinate system to the submap coordinate system; h k Represents all points in a scan;

[0159] d. When calculating the scan matching problem, set the angle step to δ θ , through the cosine theorem, we can get:

[0160]

[0161] Among them, d max It represents the distance from the farthest point from the origin in a scan to the origin; r represents the resolution of the map, that is, the granularity of the search;

[0162] e. Set the search width to W x , W y , the search angle is W θ . Then the number of search steps is:

[0163]

[0164] f. According to the number of search steps, the step search space is:

[0165]

[0166] g. Combined with the binary tree, each node in the tree is defined as Among them, c h Represents the height of the leaf node, and the search space represented by a binary tree, that is:

[0167]

[0168] Among them, j x 、j y Indicates the target point; represents the entire search space of node c; Represents the search space of node c in the binary tree;

[0169] Get the maximum confidence value of the leaf node in the search space, recorded as score(c):

[0170]

[0171] h. By comparing the value of score(c), we can obtain the point in the search space that is most similar to point c, i.e., the initial position.

[0172] 5) Use a multi-threaded registration method to match the lidar point cloud between frames, and update the system status based on the position value predicted by the IMU data.

[0173] 5-1) Group and match the extracted feature points using the ICP algorithm;

[0174] In one thread, based on the geometric relationship of feature points, the nearest neighbor points corresponding to the target point cloud are found for the feature points of the source point cloud as the initial matching pairs; then, by minimizing the distance error function between the matching point pairs, the rotation and translation parameters are iteratively optimized so that the positions of the source point cloud and the target point cloud coincide in space;

[0175] 5-2) After the extracted feature points are aligned, the system state is updated based on the initial position value predicted by the IMU.

[0176] In summary, the present invention integrates multi-sensor data to cope with changeable and complex outdoor environments. Give full play to the advantages of each sensor and improve the accuracy and robustness of the positioning system. The use of an unscented Kalman filter instead of a traditional extended Kalman filter eliminates linearization errors and does not require the derivation of the Jacobian matrix, greatly reducing the algorithm's consumption of computing resources. The resulting lightweight system can be used on embedded platforms. The RTK-based branch and bound search method effectively solves the initialization problem in large outdoor scenes.

[0177] The specific implementation modes of the present invention are described above in conjunction with the accompanying drawings, but these descriptions cannot be understood as limiting the scope of the present invention. The protection scope of the present invention is defined by the attached claims, and any changes based on the claims of the present invention are within the protection scope of the present invention.

Claims

1. A method for positioning an outdoor mobile robot based on unscented Kalman filtering, characterized in that: The following steps are involved: 1) Data collection and initialization: Collect RTK data, IMU data and 3D LiDAR data in real time; and process the RTK data, IMU data and 3D LiDAR data respectively; 2) Unscented Kalman Filter UKF state prediction: Generate a sigma point set using IMU data and UT transformation, and calculate the state prediction values ​​of these points; then calculate the new mean and covariance to update the system state; 3) Observation update: Substitute the new sigma point set into the observation equation to obtain the predicted observation; use the weighted method to obtain the new mean and covariance, and calculate the Kalman gain; update the system state and covariance; 4) AGV processing under occlusion conditions: When the AGV is blocked, the result of the 3D lidar point cloud inter-frame registration is added as the observation value to the unscented Kalman filter UKF instead of the RTK data; the unscented Kalman filter in step 2) is reused to complete the filter initialization by searching the initial position on the prior point cloud map; 5) The LiDAR point cloud is matched between frames through a multi-threaded registration method, and the system status is updated based on the position value predicted by the IMU data.

2. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1 is characterized in that: The step 1) is specifically: 1-1) The RTK measuring instrument, inertial measurement unit IMU and laser radar installed on the AGV respectively collect RTK data, IMU data and 3D laser radar data in real time; 1-2) For IMU data, the state of the constructed system is predicted through the IMU's kinematic equations, including: estimating the current position, velocity and attitude; 1-3) For 3D lidar data, preprocessing is performed, including smoothing, outlier removal, downsampling, and noise removal, and edge and plane features are extracted; 1-4) For RTK data, initial values ​​are provided for global initialization on the prior point cloud map and used as observation values ​​to update the system status.

3. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 2 is characterized in that: The steps 1-2) are specifically: In continuous time, the motion equation of the IMU is: Among them, p is the current position, v is the current velocity, R is the current posture, a is the acceleration collected by the inertial measurement unit IMU, and ω is the angular velocity collected by the inertial measurement unit IMU.

4. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 2 is characterized in that: The 3D laser radar data is preprocessed as follows: 1-3-1) For point cloud data with irregular density, the moving least squares method is used to set the point cloud data in the window to obey a Gaussian distribution. For a given window size, a center point is selected in the window, and the distance between all points in the window and the center point is calculated. The point cloud in the window is weighted averaged according to the distance value to obtain the smoothed point cloud data; 1-3-2) For outliers caused by occlusion, remove them through radius filtering; For each point in the point cloud, calculate the distance from it to its nearest neighbor; if the distance from a point to its nearest neighbor is greater than a certain radius threshold, then the point is judged as an outlier; 1-3-3) For a large amount of point cloud data, the number of point cloud data is reduced by random downsampling; 1-3-4) For the noise data, perform filtering and removal operations; 1-3-5) Start multiple threads to extract point cloud features, including edge feature extraction and plane feature extraction; For edge feature extraction, find those points in the point cloud data whose curvature changes are greater than the set threshold. By calculating the difference between the normal vectors of each point in the point cloud and its neighboring points, the points corresponding to the areas where the normal vectors change dramatically are the edge feature points. For plane feature extraction, the normal vector consistency of the point cloud is utilized, and the PCA method is used to find a point set with similar normal vectors within the set geometric range as the plane feature.

5. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1 is characterized in that: The mathematical model of the unscented Kalman filter UKF is: For nonlinear systems, the fusion positioning system state and observation equations are: Among them, X(k+1) is the state vector of the system at time k+1, x(k) is the state vector of the system at time k, W(k) is the process noise, which represents the uncertainty in the model prediction process, Z(k) is the observation vector, V(k) is the observation noise, which represents the uncertainty in the observation, f(x(k), W(k)) is the nonlinear state transfer function, and h(x(k), V(k)) is the nonlinear observation function.

6. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1 is characterized in that: The step 2) comprises the following steps: 2-1) The fusion positioning system state is transformed into UT to estimate the mean and covariance under the action of nonlinear function, and the sigma sampling point is obtained as: in, is the system state, P(k / k) is the error covariance matrix of the system state, n is the state vector dimension, and λ is the scaling parameter used to adjust the range of the sigma point distribution; 2-2) Calculate 2n+1 sigma point sets, namely: X (i) (k+1 / k)=f[k,X i (k / k)] Among them, X(k+1 / k) is the updated system state; 2-3) Prediction of the state quantity of the fusion positioning system, namely: Among them, ω is the weight corresponding to the sigma point, and Q is the process noise covariance matrix; 2-4) Transform the fusion positioning system state UT again to generate a new sigma point set, namely: Among them, P(k+1 / k) is the updated system state error covariance matrix.

7. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1 is characterized in that: The step 3) comprises the following steps: 3-1) Substitute the new sigma point set into the observation equation to obtain the observed value of the predicted value: Z i (k+1 / k)=h[X i (k+1 / k)] Among them, Z(k+1 / k) is the observation vector, h[X i (k+1 / k)] is the observation equation; 3-2) The new mean and covariance of the observations are obtained by weighted method: Where R is the observation noise covariance matrix, is the observation covariance matrix, which represents the correlation between observations. is the cross covariance matrix between the state and the observation, which represents the relationship between the state and the observation; 3-3) Calculate the Kalman gain K(k+1) as: 3-4) Update the state and covariance of the fusion positioning system, that is: The updated state and covariance are obtained to complete the observation update of the GNSS / INS fusion positioning system.

8. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1 is characterized in that: In step 4), the unscented Kalman filter in step 2) is reused to complete the filter initialization by searching the initial position on the prior point cloud map, specifically: 4-1) Reduce the dimension of 3D LiDAR point cloud and add it to Kalman filter as observation value instead of RTK data; 4-2) Establish a point cloud map. When constructing the point cloud map, save the value pairs between the RTK fixed solution and the key frame index to achieve accurate positioning and navigation; 4-3) Compress the 3D point cloud map into a 2D point cloud, and then use the branch and bound search method to reduce the time complexity from O(n 2 ) is reduced to O(logn), and the initial position is searched on the prior map to complete the filter initialization.

9. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 8 is characterized in that: In step 4-3), the initial position is searched on the prior map by using the branch and bound search method, specifically: a. The optimization problem of the branch and bound search method is a nonlinear least squares problem, and the problem description is expressed as: Among them, m Indicates the position of the submap; s represents the pose of the scan data scan; ρ represents the loss function; E represents the residual of the constraint; Indicates the pose of each submap. Represents the pose of each frame scan, ξ ij Represents the relative pose between the j-th scan that successfully matches the i-th submap; Σ ij Represents the covariance matrix corresponding to the relative pose; b. Yes m and s The residual between is optimized, and the constraint used for optimization is ξ ij and the corresponding covariance matrix Σ ij , then the residual E is: in, Represents the rotation matrix in each submap pose; and Represents the translation vector in each submap and each frame scan pose respectively; c. In the branch and bound search method, the scan matching problem is formulated as: Where W represents the search space range; M nearest Indicates the M value of the grid point corresponding to the point; T ξ Represents the transformation matrix from the laser radar coordinate system to the submap coordinate system; h k Represents all points in a scan; d. When calculating the scan matching problem, set the angle step to δ θ , through the cosine theorem, we can get: Among them, d max It represents the distance from the farthest point from the origin in a scan to the origin; r represents the resolution of the map, that is, the granularity of the search; e. Set the search width to W x , W y , the search angle is W θ . Then the number of search steps is: f. According to the number of search steps, the step search space is: g. Combined with the binary tree, each node in the tree is defined as Among them, c h Represents the height of the leaf node, and the search space represented by a binary tree, that is: Among them, j x 、j y Indicates the target point; represents the entire search space of node c; Represents the search space of node c in the binary tree; Get the maximum confidence value of the leaf node in the search space, recorded as score(c): h. By comparing the value of score(c), we can obtain the point in the search space that is most similar to point c, i.e., the initial position.

10. The outdoor mobile robot positioning method based on unscented Kalman filtering according to claim 1, characterized in that: In step 5), the laser radar point cloud is matched between frames by a multi-threaded registration method, and the system state is updated in combination with the position value predicted by the IMU data, specifically: 5-1) Group and match the extracted feature points using the ICP algorithm; In one thread, based on the geometric relationship of the feature points, the nearest neighbor points corresponding to the target point cloud are found for the feature points of the source point cloud as the initial matching pairs; Then, by minimizing the distance error function between the matching point pairs, the rotation and translation parameters are iteratively optimized so that the positions of the source point cloud and the target point cloud coincide in space; 5-2) After the extracted feature points are aligned, the system state is updated based on the initial position value predicted by the IMU.

Images