Positioning and mapping method and system based on multi-source data

Abstract

The invention belongs to the technical field of robot pose positioning, and discloses a positioning and mapping method and system based on multi-source data. The method analyzes the multi-source SLAM scheme. Secondly, how to establish a laser inertial odometer by using an extended Kalman filtering algorithm is proposed, inter-frame matching of laser point cloud is realized, and a laser radar grid map is established; acquiring three-dimensional image information by using a depth camera, extracting ORB features of the image, solving pose information of the depth camera through a PnP algorithm, and converting the pose information into two-dimensional point cloud data; and finally, by improving a Bayesian algorithm based on multi-source data, constructing a raster map by using the multi-source data. According to the invention, the sensor with low cost is used to realize good positioning and composition effects.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot posture positioning, and in particular relates to a positioning and mapping method and system based on multi-source data. Background Art

[0002] With the rapid development of robotics, more and more technological products such as unmanned vehicles and drones have begun to enter people's lives. Among them, positioning is an important technology for realizing the intelligence of various mobile robots. In practical applications, when mobile robots face complex scenes, such as lighting changes and many dynamic obstacles, it is easy to cause tracking failure, which in turn affects the positioning and mapping process of mobile robots. Therefore, simultaneous localization and mapping (SLAM) in complex environments is a hot topic and an important direction in current mobile robot research.

[0003] SLAM methods based on a single sensor are unstable when the operating environment conditions are poor or the robot moves or turns too fast. For example, the scanning observation distance of the lidar sensor is limited and it is easily affected by the complex geometric structure in the environment. The camera has certain requirements for the lighting conditions of the robot's surrounding environment. The encoder motor will produce cumulative errors after working for a long time. Luo Yuan et al. fused the odometer and radar data, optimized the proposed distribution function, effectively reduced the uncertainty of the robot's posture in the prediction stage, and reduced the number of particles required for SLAM.

[0004] In 2015, Leutenegger et al. proposed an optimization-based VIO fusion solution OKVIS (OpenKey-frame-based Visual-Inertial SLAM). They introduced a tightly coupled fusion framework of IMU and image key points in nonlinear optimization problems, and applied linearization and marginalization to implement key frames. In 2015, Zhang et al. proposed a SLAM system that integrates vision and laser, namely VLOAM (Visual-lidar Odometry and Mapping). The online method they proposed starts with visual odometry to estimate its own motion and records point clouds from scanning laser radars at high frequency and low fidelity. Then, the motion estimation is improved by the laser radar odometry based on scan matching, and the point cloud registration is completed. In 2017, Shen Shaojie et al. from the Hong Kong University of Science and Technology proposed VINS-Mono (Visual-Inertial System Monocular). The system fuses the data of the monocular camera and IMU, and uses a tightly coupled, nonlinear optimization-based method to achieve high-precision visual inertial odometry by fusing pre-integrated IMU measurements and feature observations. For the loop detection module, it is combined with the tightly coupled formula to complete the relocalization with the lowest computational cost. In addition, the framework also completes the pose graph optimization of four degrees of freedom to improve global consistency. In 2021, Wang et al. proposed a direct vision laser fusion SLAM framework consisting of three modules. The framework includes a two-stage direct visual odometry module, a laser mapping module using dynamic objects, and a parallel global and local search loop detection module that combines visual word bags with LIDAR-Iris features. The results show that the algorithm achieves more accurate pose estimation compared to the state-of-the-art methods.

[0005] Through the above analysis, the problems and defects of the prior art are as follows: In the prior art, the filter-based laser SLAM has high complexity and low positioning accuracy, and has poor applicability. Summary of the invention

[0006] In order to overcome the problems existing in the related art, the disclosed embodiments of the present invention provide a positioning and mapping method and system based on multi-source data.

[0007] The present invention is implemented in this way: a positioning and mapping method based on multi-source data, using an extended Kalman filter algorithm to fuse lidar and IMU data, obtaining multi-source environmental data, and constructing a multi-source data grid map by fusing two-dimensional point cloud data, inertial data and three-dimensional image data through an improved Bayesian algorithm based on multi-source data; specifically comprising the following steps: S1, inter-frame matching of laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of laser; S2, using the extended Kalman filter algorithm to fuse the mobile robot's posture; S3, depth camera data processing: Through the image feature extraction and description algorithm ORB, the main direction of the FAST corner point is used to rotate the BRIEF descriptor to make the BRIEF descriptor rotationally invariant, and the matching point pairs are obtained. The PnP method is used to solve the movement of the depth camera 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method; S4, improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data raster map, and solve the raster probability based on multi-source data.

[0008] In step S1, the PL-ICP algorithm is used to improve the error form, including the steps of: S101: according to the odometer data, set the initial transformation matrix, set the number of iterations and the convergence threshold, and convert the current frame laser data to the reference coordinate system; S102: For each point in the current point cloud, find the two closest points to the point in the reference point cloud, and determine the connection line between the two points; S103: Based on the distance from the point to the line, calculate the rotation and translation transformation matrix that minimizes the total distance; S104: Transform the current point cloud according to the calculated transformation matrix, and calculate the error between the transformed point cloud and the reference point cloud. If the error is less than the preset threshold or reaches the maximum number of iterations, stop the iteration and output the final transformation matrix as the registration result; otherwise, return to step S101 to continue the iteration.

[0009] In step S2, the extended Kalman filter algorithm is used to fuse the mobile robot posture, including the steps of: S201, define a state vector equation and an observation vector equation; the state vector includes position, velocity, attitude, and IMU deviation; the observation vector includes distance and angle information measured by the laser radar, and acceleration and angular velocity measurement values ​​of the IMU; S202, estimate the state variables in the last a posteriori state of the system Perform a first-order Taylor expansion at , and linearize the observation vector equation. Linearization at S203, performing a first-order Taylor expansion based on the state vector equation and the observation vector equation to obtain a linearized result, performing prediction and correction of the extended Kalman filter, and updating the real-time position of the mobile robot; S204, constructing a lidar grid map based on the principle of binary Bayesian filtering, using the input of multiple lidars to determine the obstacle occupancy probability of each grid in the lidar grid map, and adjusting the grid occupancy probability according to the new observation data updated according to the real-time position of the mobile robot.

[0010] In step S201, the state vector equation is: ; In the formula, For the system The state variables at time, is the state vector operation function, For the system The state variables at time, For the system Control input at all times; is the process noise, which represents the random disturbance in the state change due to system uncertainty or modeling error; The observation vector equation is: ; In the formula, is the observation vector operation function, is the observation noise, which represents the random error or uncertainty in the observation process; and Satisfy normal distribution; In step S202, the state variable is estimated in the last a posteriori state of the system Perform a first-order Taylor expansion at the position for linearization, and the expression is: ; In the formula, Estimate the state variables for the last posterior state, The error is 0, let , Estimate state variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , , then: ; In the formula, Predict state variables for the current state; The observation vector equation is Linearization at, the expression is: ; because is the error, so let is 0, let , Predict observed variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , ; ; In the formula, is the probability, is the extrinsic parameter matrix of the depth camera, is a normal distribution, is the observation radius of the depth camera.

[0011] In step S203, the prediction of the extended Kalman filter includes: 1) State vector prediction: ; 2) Error covariance prediction: ; In the formula, for The prior estimate error covariance at time , for -1 time probability, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the Kalman filter, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; Extended Kalman filter correction, including: (i) Calculation of Kalman gain: Kalman gain is used to determine the relative weight between the predicted value and the observed value; it is calculated based on the prediction error covariance and the observation noise covariance, and the expression is: ; In the formula, is the Kalman gain, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; (ii) State vector update: Use the Kalman gain and observations to update the state estimate; ; In the formula, is the current updated state variable, is the Kalman gain; (iii) Covariance update: Update the error covariance of the state estimate to reflect the corrected uncertainty; ; In the formula, is the error covariance, is the estimated value of the state; In step S204, for the laser radar grid map A grid on , expressed in terms of probability, as the probability of being occupied , the probability value of 1 represents the occupied state, and the probability value of 0 represents the idle state. The ratio of the two is introduced Indicates The state of the grid; After the laser radar performs a scanning measurement, the measurement value is obtained. , update the grid status to: ; In the formula, To update the status value of the grid, is the probability of a grid being occupied from the occupied state to the measured value, is the probability that a grid in the idle state is occupied by the measured value; According to the Bayesian formula: ; ; In the formula, is the probability that the measured value obtained after scanning measurement is occupied, is the probability that a grid is occupied from the measured value to the occupied state, is the probability that a grid is occupied when the measured value reaches the idle state; Combining the two formulas we get: ; In the formula, For the The state of the grid; Taking the logarithm of both sides of the above equation, we get: ; In the above formula, only Contains the measurement value. This ratio is called the measurement model. It indicates that the laser radar observation results of the grid have only two states: occupied and idle. By default, the probability of occupied and idle in the initial state of the grid is 0.5. and represents the grid state before and after the measurement, the update rule is simplified to: ; The larger the probability value of a grid is, the greater the probability that the grid is in an occupied state and has an obstacle.

[0012] In step S3, PnP is used to solve the motion of the depth camera 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method, including: Assume that the coordinates of the depth camera in space are expressed as , the three-dimensional coordinates are projected onto the normalized plane, and the projection coordinates are , according to the pinhole imaging principle; the relationship between the three-dimensional space coordinates and the projection coordinates is derived as follows: ; In the formula, is the intrinsic parameter matrix of the depth camera, is the extrinsic parameter matrix of the depth camera, It is the relationship value between the three-dimensional space coordinates and the projection coordinates.

[0013] In step S3, the method of minimizing the reprojection error includes: the reprojection error is expressed as the Euclidean distance between the actual observation value and the projection coordinate to construct a least squares error objective function: ; In the formula, is the least squares error target value, is the projection coordinate, is the coordinate of the depth camera in space, is the least squares absolute function; Solve for the derivative of the error term with respect to the optimization variable: ; In the formula, is the error, is the state variable, is the state variable change value, is the error function under the state variable, is the linearized value of the state variable; According to the error objective function, record the spatial point Coordinates in the camera coordinate system ,definition As an intermediate variable; Using the chain rule, we can get the Lie group Left multiplication disturbance Taking the derivative, we get the following formula: ; In the formula, is the left-multiplied perturbation of the Lie algebra, is the disturbance value, is the disturbance error; is the error with respect to the projection point The derivative of , according to the camera imaging principle, the Jacobian matrix is ​​introduced as follows: ; In the formula, For function right The Jacobian matrix of the partial derivatives of , For function right The Jacobian matrix of the partial derivatives of ; Middle Pair Regarding the derivation of Lie algebra, taking the first three dimensions, we get the following formula: ; In the formula, Derivative of the Lie algebra for the projection point; Multiplying the above two equations, we get the Jacobian matrix: ; The above formula represents the changing relationship between the reprojection error and the first-order derivative of the Lie algebra of the camera pose; By obtaining the Jacobian matrix, the linearization of the objective function is completed, and then the posture transformation matrix is ​​obtained through the Gauss-Newton method, so as to obtain the linearized incremental equation of the objective function; ; In the formula, For linearization, is the perturbation value objective function under the Jacobian matrix constraint, For the linearization of the disturbance value objective function, is the change value of the disturbance value, is the disturbance error; Using the known Jacobian matrix and initial pose value for iterative optimization, when When the value is lower than the set value, the iteration stops and the pose transformation matrix is ​​obtained. Otherwise, the Continue iterating until the requirements are met.

[0014] In step S4, a multi-source data grid map is constructed, and the grid probability based on the multi-source data is solved, including: Assume that the map consists of a set of grid cells, each of which is represented by , indicating the state of the grid unit; the data observed by the sensor is ,express Observation data of sensors at all times; According to Bayes' theorem, we get: ; In the formula, Given observation data Under these conditions, the grid cells The posterior probability of In a given map state And all previous observations Under the condition of The probability of , that is, the observation model; is all the observed data before given Under the condition of The prior probability of is all the observed data before given Under the condition of probability; Assume that the map is divided into a series of grids, each grid is free or occupied; use To represent the grid The probability of being occupied is To represent the grid is the probability of being idle.

[0015] Furthermore, for the grid map, the update comes from the observation of the sensor, and the Bayesian update formula is expressed as: ; In the formula, In observing the data Afterwards, the grid Probability of being occupied; Is in the grid Data observed when occupied probability; Is a grid Prior probability of being occupied; Observed data probability; In the process of processing multi-source data, when the same grid is detected by different sensors such as lidar and depth camera, a threshold is set before calculating the probability value of each grid. ; When the probability value result of the grid Greater than , the grid is set to the occupied state, and the transition probability value is 1; otherwise, the probability value of the grid is kept ; ; In the formula, Represents the probability value of a certain grid after calculating various sensor data; finally, the Bayesian rule is used to solve the grid probability based on multi-source data for: ; In the formula, is the raster probability based on multi-source data.

[0016] Another object of the present invention is to provide a positioning and mapping system based on multi-source data, which implements the positioning and mapping method based on multi-source data, and the system includes: The inter-frame matching module of laser point cloud is used for inter-frame matching of laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of laser; Extended Kalman filter algorithm fusion module, used to fuse the mobile robot posture using the extended Kalman filter algorithm; Depth camera data processing module, used for depth camera data processing: through the image feature extraction and description algorithm ORB, use the main direction of the FAST corner point to rotate the BRIEF descriptor to make the BRIEF descriptor rotationally invariant, obtain matching point pairs, use PnP to solve the movement of the depth camera 3D to 2D point pairs, and obtain the optimal camera pose by minimizing the reprojection error method; The multi-source data fusion module is used to improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data raster map, and solve the raster probability based on multi-source data.

[0017] Combining all the above technical solutions, the beneficial effects of the present invention are as follows: The present invention first analyzes the multi-source SLAM solution. Secondly, it proposes how to use the extended Kalman filter algorithm to establish a laser inertial odometer, realize inter-frame matching of laser point clouds, and establish a lidar grid map. Then use a depth camera to obtain three-dimensional image information, extract image ORB features, and use the PnP algorithm to solve the posture information of the depth camera and convert it into two-dimensional point cloud data. Finally, by improving the Bayesian algorithm based on multi-source data, multi-source data is used to construct a grid map to improve the precision and accuracy of the composition. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The accompanying drawings herein are incorporated in and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the description, serve to explain the principles of the present disclosure; Figure 1 is a flow chart of a positioning and mapping method based on multi-source data provided by an embodiment of the present invention; Figure 2 is a flow chart of the PL-ICP algorithm provided by an embodiment of the present invention; Figure 3 is a schematic diagram of reprojection error provided by an embodiment of the present invention; Figure 4 is a schematic diagram of a positioning and mapping system based on multi-source data provided by an embodiment of the present invention; Figure 5 is a schematic diagram of a real environment of an indoor corridor provided by an embodiment of the present invention; Figure 6 It is a single laser SLAM mapping provided by an embodiment of the present invention; Figure 7 It is a multi-source SLAM mapping provided by an embodiment of the present invention; In the figure: 1. Inter-frame matching module of laser point cloud; 2. Extended Kalman filter algorithm fusion module; 3. Depth camera data processing module; 4. Multi-source data fusion module. DETAILED DESCRIPTION

[0019] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific implementation disclosed below.

[0020] The innovation of the present invention is that it proposes a unique SLAM solution for multi-source data, and proposes the fusion of IMU, laser radar, and depth camera data; it uses low-cost sensors to achieve better positioning and composition effects. The present invention comprehensively utilizes laser radar, depth camera and IMU to obtain multi-source environmental data, and by improving the Bayesian algorithm based on multi-source data, it integrates two-dimensional point cloud data, inertial data and three-dimensional image data, and constructs a multi-source data grid map with rich environmental information and high accuracy.

[0021] Embodiment 1, as Figure 1 As shown, the positioning and mapping method based on multi-source data provided by the embodiment of the present invention includes: S1, inter-frame matching of laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of laser; The position and posture information of a mobile robot can be obtained through IMU data, but due to the cumulative effect of integral operations, significant drift errors will occur after long-term operation. This error will gradually increase over time, resulting in a decrease in the accuracy of position and posture estimation. Therefore, the current method of using laser radar based on inter-frame matching to solve the change in robot position and posture is mainly used. Traditional inertial information is more of an auxiliary method to enhance the accuracy of positioning.

[0022] In the laser frame matching solution, the iterative closest point (ICP) algorithm is a commonly used point cloud registration algorithm for aligning and matching two or more point clouds. It continuously optimizes the minimum square error between two point clouds in an iterative manner to find the best point cloud registration result. However, the ICP algorithm assumes that the transformation between point clouds is rigid, that is, it only contains rotation and translation transformations. But in practical applications, there may be non-rigid deformations, resulting in a decrease in algorithm performance.

[0023] To address the above problems, the PL-ICP algorithm improves the error form by adopting a point-to-line registration method to improve the robustness to noise, the iteration speed and the matching accuracy. The specific process is as follows: S101: according to the odometer data, set the initial transformation matrix, set the number of iterations and the convergence threshold, and convert the current frame laser data to the reference coordinate system; S102: For each point in the current point cloud, find the two closest points to the point in the reference point cloud, and determine the connection line between the two points; S103: Based on the distance from the point to the line, calculate the rotation and translation transformation matrix that minimizes the total distance; S104: Transform the current point cloud according to the calculated transformation matrix, and calculate the error between the transformed point cloud and the reference point cloud. If the error is less than the preset threshold or reaches the maximum number of iterations, stop the iteration and output the final transformation matrix as the registration result; otherwise, return to step S101 to continue the iteration.

[0024] S2, using the extended Kalman filter algorithm to fuse the mobile robot's posture; In the process of solving the real-time position and posture of the mobile robot, both IMU and LiDAR have measurement errors. IMU estimates the motion state of the object by measuring acceleration and angular velocity and integrating them. However, due to the accumulation of measurement noise and errors, the integration operation will cause the estimated value to gradually deviate from the true value, resulting in the so-called "integral drift" phenomenon. This drift will increase over time, affecting the accuracy and reliability of IMU data. During the scanning process of LiDAR, when the laser beam falls on an area with a large difference in reflectivity or an object with a large distance difference, mixed pixels may occur. This will cause the measured distance to be actually the average distance of multiple target points, which will then form an error and affect the accurate estimation of the robot's position and posture.

[0025] The Extended Kalman Filter (EKF) is an effective recursive state estimation algorithm for nonlinear dynamic systems. When fusing LiDAR and IMU data, EKF can help integrate the information of these two sensors, thereby improving the accuracy and robustness of state estimation.

[0026] The specific process is as follows.

[0027] S201, define a state vector equation and an observation vector equation; the state vector includes position, velocity, attitude, and IMU deviation; the observation vector includes distance and angle information measured by the laser radar, and acceleration and angular velocity measurement values ​​of the IMU; The state vector equation is: ; In the formula, For the system The state variables at time, is the state vector operation function, For the system The state variables at time, For the system Control input at all times; is the process noise, which represents the random disturbance in the state change due to system uncertainty or modeling error; The observation vector equation is: ; In the formula, is the observation vector operation function, is the observation noise, which represents the random error or uncertainty in the observation process; and Satisfy normal distribution; S202, estimate the state variables in the last a posteriori state of the system Perform a first-order Taylor expansion at , and linearize the observation vector equation. Linearization at The results are as follows: ; In the formula, Estimate the state variables for the last posterior state, The error is 0, let , Estimate state variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , , then: ; In the formula, Predict state variables for the current state; The observation vector equation is Linearization at, the expression is: ; because is the error, so let is 0, let , Predict observed variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , ; ; In the formula, is the probability, is the extrinsic parameter matrix of the depth camera, is a normal distribution, is the observation radius of the depth camera.

[0028] S203, performing a first-order Taylor expansion based on the state vector equation and the observation vector equation to obtain a linearized result, performing prediction and correction of the extended Kalman filter, and updating the real-time position of the mobile robot; 1) State vector prediction: ; 2) Error covariance prediction: ; In the formula, for The prior estimate error covariance at time , for -1 time probability, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the Kalman filter, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; Extended Kalman filter correction, including: (i) Calculation of Kalman gain: Kalman gain is used to determine the relative weight between the predicted value and the observed value; it is calculated based on the prediction error covariance and the observation noise covariance, and the expression is: ; In the formula, is the Kalman gain, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; (ii) State vector update: Use the Kalman gain and observations to update the state estimate; ; In the formula, is the current updated state variable, is the Kalman gain; (iii) Covariance update: Update the error covariance of the state estimate to reflect the corrected uncertainty; ; In the formula, is the error covariance, is the estimated value of the state; In step S204, for the laser radar grid map A grid on , expressed in terms of probability, as the probability of being occupied , the probability value of 1 represents the occupied state, and the probability value of 0 represents the idle state. The ratio of the two is introduced Indicates The state of the grid; In practical applications, the acceleration and angular velocity data are read from the IMU, and the IMU data is used to predict the robot's motion state through integration operations. The laser radar is used to scan the surrounding environment, obtain point cloud data, and calculate the change of the laser radar data relative to the previous moment through point cloud registration to obtain the relative posture change, and convert the relative posture change into an absolute posture. Based on the IMU data, the nonlinear state transfer equation is used to predict the state at the next moment; the posture change obtained by the laser radar is used as the observation value, the Kalman gain is calculated, and the Kalman gain, predicted value, and observed value are used to update the state estimate. Repeat the above steps continuously to update the real-time position of the robot through continuous prediction and correction.

[0029] S204, constructing a laser radar grid map based on the principle of binary Bayesian filtering, using the input of multiple laser radars to determine the obstacle occupancy probability of each grid in the laser radar grid map, and adjusting the grid occupancy probability according to the new observation data after the real-time position of the mobile robot is updated; The occupancy grid map is a map description method commonly used in robot navigation modules. It divides the environment into a series of grids, each grid is given a possible value, indicating the probability of the grid being occupied, and the grid is divided into three states: occupied, idle, and unknown according to the size of the probability value. Each grid is described by the grid occupancy probability and coordinates, and each map grid is considered to be independent. By calculating the occupancy probability for each grid, the entire map can be fully described. The construction of the occupancy grid map is mainly based on the principle of binary Bayesian filtering, using the input of multiple sensors (such as lidar, depth camera, etc.) to determine the obstacle occupancy probability of each grid in the lidar grid map.

[0030] For LiDAR raster maps A grid on ,use The probability of being occupied is expressed in the form of probability, where a probability value of 1 represents an occupied state and a probability value of 0 represents an idle state. The ratio of the two is introduced Indicates When the laser radar performs a scanning measurement, the measurement value is obtained. , update the grid status to: ; In the formula, To update the status value of the grid, is the probability of a grid being occupied from the occupied state to the measured value, is the probability that a grid in the idle state is occupied by the measured value; According to the Bayesian formula: ; ; In the formula, is the probability that the measured value obtained after scanning measurement is occupied, is the probability that a grid is occupied from the measured value to the occupied state, is the probability that a grid is occupied when the measured value reaches the idle state; Combining the two formulas we get: ; In the formula, For the The state of the grid; Taking the logarithm of both sides of the above equation, we get: ; In the above formula, only Contains the measurement value. This ratio is called the measurement model. It indicates that the laser radar observation results of the grid have only two states: occupied and idle. By default, the probability of occupied and idle in the initial state of the grid is 0.5. and represents the grid state before and after the measurement, the update rule is simplified to: ; The larger the probability value of a grid is, the greater the probability that the grid is in an occupied state and has an obstacle.

[0031] From the above formula, we can see that the basic idea of ​​updating the occupied grid map is to adjust the occupancy probability of the grid according to the new observation data. The larger the probability value of the grid, the greater the probability that the grid is occupied and has an obstacle.

[0032] S3, depth camera data processing: Through the image feature extraction and description algorithm ORB, the main direction of the FAST corner point is used to rotate the BRIEF descriptor to make the BRIEF descriptor rotationally invariant, and the matching point pairs are obtained. The PnP method is used to solve the movement of the depth camera 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method; The depth camera can obtain point cloud information of the mobile robot's motion environment. First, the depth camera emits infrared light or laser beams to illuminate the objects in the scene. When the light hits the surface of the object, part of the light will be reflected back to the camera by the object. The depth camera will record the time or angle information of the reflected light. Based on the speed of light propagation and the time difference of light reflection, the depth camera can calculate the distance between the object and the camera, thereby generating a depth map.

[0033] ORB (Oriented FAST and Rotated BRIEF) is an algorithm for image feature extraction and description. It is a combination of the FAST corner detector and the BRIEF descriptor, and adds directionality. The FAST (Features from Accelerated Segment Test) feature detection algorithm is a commonly used corner detection algorithm used to quickly detect corners with obvious grayscale changes in images. The detection process is as follows: (a) Corner point candidate selection: First, a set of candidate points are selected from the image.

[0034] (b) Pixel grayscale comparison: For each candidate point, the grayscale values ​​of the surrounding pixels are compared with the grayscale value of the candidate point to determine whether the candidate point is a corner point. Usually 16 surrounding pixels of the candidate point are selected for comparison.

[0035] (c) Grayscale change judgment: For each candidate point, the FAST algorithm divides the 16 pixels around it into two groups: pixels with grayscale values ​​larger than the candidate point and pixels with grayscale values ​​smaller than the candidate point. If there are more than 12 pixels in a group of continuous pixels with grayscale values ​​larger (or smaller) than the candidate point, the candidate point is considered to be a corner point.

[0036] (d) Corner point determination: After grayscale change determination, candidate points may be marked as corner points. At this point, non-maximum suppression (NMS) can be further performed to remove duplicate corner points in adjacent areas.

[0037] (e) In order to quickly eliminate non-feature points, we can directly observe the pixel brightness of four specific positions: 1, 5, 9 and 13. If the brightness of at least three of these four pixels simultaneously meets or exceeds the preset threshold condition, then the point can be regarded as a feature point; conversely, if none of the conditions are met, it can be concluded that the point is not a feature point and can be directly eliminated.

[0038] In addition, the original FAST corner points themselves have no directionality, which may cause anomalies when matching feature points. ORB calculates the main direction of the FAST corner points and assigns a direction to each corner point, making it rotationally invariant. This allows ORB feature points to maintain stable matching performance when images of different scales are rotated.

[0039] BRIEF is a feature descriptor used to describe detected feature points. It abandons the traditional method of describing feature points using regional grayscale histograms and adopts binary encoding to speed up the creation of feature descriptors and reduce feature matching time. The generation process of BRIEF descriptors usually involves the following steps: (I) Image sampling: First, the input image is sampled, usually using a Gaussian filter to smooth the image, and then down-sampled to reduce the amount of computation and increase the speed.

[0040] (II) Keypoint Detection: Detect keypoints in the image using FAST feature detection etc.

[0041] (III) Compute local image patches: For each detected keypoint, a local image patch is extracted around the keypoint.

[0042] (IV) Calculate the gradient direction: For each local image block, calculate the gradient direction and magnitude of its pixel points.

[0043] (V) Calculate the descriptor: In each local image block, select a set of pixel pairs and calculate the grayscale differences of these pixel pairs. Then, these differences are binarized to generate a binary string as the descriptor of the key point.

[0044] (VI) Descriptor matching: Use the Hamming distance to compare the descriptors of two key points to perform feature point matching.

[0045] ORB rotates the BRIEF descriptor by using the main direction of the FAST corner point to make it rotationally invariant, which further improves the matching stability and accuracy of ORB feature points.

[0046] Matched point pairs can be obtained through the steps of feature point extraction and matching, and the movement of the camera can be estimated using these point pairs. The present invention adopts PnP (Perspective-n-Point) as a method for solving the movement of 3D to 2D point pairs. The PnP algorithm is a method for estimating camera motion, which estimates the camera's pose using the pixel positions of n known feature points in an image and their corresponding 3D space point positions. This algorithm is widely used in fields such as visual SLAM and three-dimensional reconstruction. In two images, a depth camera is used to determine the 3D position of the feature points of one of them, and then the PnP method can be used to estimate the movement of the camera. The core of the PnP algorithm is to use the known 3D-2D point correspondence to solve the camera's pose, which usually involves an optimization problem, and the optimal camera pose estimation is obtained by minimizing the reprojection error and other methods.

[0047] Reprojection error refers to projecting the coordinates of a 3D point into the depth camera coordinate system, converting it into the image coordinate system, and comparing it with the corresponding observation value. In this invention, the PnP problem is constructed as a nonlinear least squares problem about the reprojection error, the purpose of which is to solve the position and posture of the depth camera, that is, the rotation matrix and the translation vector, so as to minimize the reprojection error.

[0048] Assume that the coordinates of the depth camera in space are expressed as , the three-dimensional coordinates are projected onto the normalized plane, and the projection coordinates are , according to the pinhole imaging principle; the relationship between the three-dimensional space coordinates and the projection coordinates is derived as follows: ; In the formula, is the intrinsic parameter matrix of the depth camera, is the extrinsic parameter matrix of the depth camera, It is the relationship value between the three-dimensional space coordinates and the projection coordinates.

[0049] The method of minimizing the reprojection error includes: the reprojection error is expressed as the Euclidean distance between the actual observation value and the projected coordinate to construct the least squares error objective function: ; In the formula, is the least squares error target value, is the projection coordinate, is the coordinate of the depth camera in space, is the least squares absolute function; The error term in the above formula is the difference between the projection coordinates and the observed coordinates of the three-dimensional space point, that is, the reprojection error, such as Figure 3 Schematic diagram of reprojection error.

[0050] Figure 3 middle, and It is the point obtained by feature matching The projection of The projection and actual They do not completely overlap, so the distance between them needs to be reduced by adjusting the camera position. Usually this process needs to consider multiple points, and finally the overall error is reduced. In order to use the optimization algorithm to solve the least squares optimization problem, it is necessary to solve the derivative of the error term with respect to the optimization variable: Solve for the derivative of the error term with respect to the optimization variable: ; In the formula, is the error, is the state variable, is the state variable change value, is the error function under the state variable, is the linearized value of the state variable; According to the error objective function, record the spatial point Coordinates in the camera coordinate system ,definition As an intermediate variable; Using the chain rule, we can get the Lie group Left multiplication disturbance Taking the derivative, we get the following formula: ; In the formula, is the left-multiplied perturbation of the Lie algebra, is the disturbance value, is the disturbance error; is the error with respect to the projection point The derivative of , according to the camera imaging principle, the Jacobian matrix is ​​introduced as follows: ; In the formula, For function right The Jacobian matrix of the partial derivatives of , For function right The Jacobian matrix of the partial derivatives of ; Middle Pair Regarding the derivation of Lie algebra, taking the first three dimensions, we get the following formula: ; In the formula, Derivative of the Lie algebra for the projection point; Multiplying the above two equations, we get the Jacobian matrix: ; The above formula represents the changing relationship between the reprojection error and the first-order derivative of the Lie algebra of the camera pose; By obtaining the Jacobian matrix, the linearization of the objective function is completed, and then the posture transformation matrix is ​​obtained through the Gauss-Newton method, so as to obtain the linearized incremental equation of the objective function; ; In the formula, For linearization, is the perturbation value objective function under the Jacobian matrix constraint, For the linearization of the disturbance value objective function, is the change value of the disturbance value, is the disturbance error; Using the known Jacobian matrix and initial pose value for iterative optimization, when When the value is lower than the set value, the iteration stops and the pose transformation matrix is ​​obtained. Otherwise, the Continue iterating until the requirements are met.

[0051] S4, improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data raster map, and solve the raster probability based on multi-source data.

[0052] Assume that the map consists of a set of grid cells, each of which can be represented as , indicating the state of the grid cell. The data observed by the sensor is ,express The observation data of the sensor at each moment. According to Bayes' theorem, we can get: ; In the formula, Given observation data Under these conditions, the grid cells The posterior probability of In a given map state And all previous observations Under the condition of The probability of , that is, the observation model; is all the observed data before given Under the condition of The prior probability of is all the observed data before given Under the condition of probability; For grid maps, updates come from sensor observations. The Bayesian update formula can be expressed as follows: Assume that the map is divided into a series of grids, each grid is free or occupied; To represent the grid The probability of being occupied is To represent the grid is the probability of being idle.

[0053] For grid maps, updates come from sensor observations, and the Bayesian update formula is expressed as: ; In the formula, In observing the data Afterwards, the grid Probability of being occupied; Is in the grid Data observed when occupied probability; Is a grid Prior probability of being occupied; Observed data probability; In the process of processing multi-source data, the same grid may have different prediction states under the detection of different sensors (lidar and depth camera). At that time, the multi-source data fusion is completed according to the rules in Table 1.

[0054] Table 1 LiDAR and depth camera fusion rules

[0055] In the process of processing multi-source data, when the same grid is detected by different sensors such as lidar and depth camera, a threshold is set before calculating the probability value of each grid. ; When the probability value result of the grid Greater than , the grid is set to the occupied state, and the transition probability value is 1; otherwise, the probability value of the grid is kept ; ; In the formula, Represents the probability value of a certain grid after calculating various sensor data; finally, the Bayesian rule is used to solve the grid probability based on multi-source data for: ; In the formula, is the raster probability based on multi-source data.

[0056] Embodiment 2, as Figure 4 As shown, a positioning and mapping system based on multi-source data includes: The inter-frame matching module 1 of the laser point cloud is used for inter-frame matching of the laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of the laser; Extended Kalman filter algorithm fusion module 2, used for fusing the position and posture of the mobile robot using the extended Kalman filter algorithm; Depth camera data processing module 3, used for depth camera data processing: through the image feature extraction and description algorithm ORB, the main direction of the FAST corner point is used to rotate the BRIEF descriptor to make the BRIEF descriptor rotationally invariant, and the matching point pairs are obtained. The PnP method is used to solve the movement of the depth camera 3D to 2D point pairs, and the optimal camera posture is obtained by minimizing the reprojection error method; The multi-source data fusion module 4 is used to improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data grid map, and solve the grid probability based on multi-source data.

[0057] Application scenario 1: By running the positioning and mapping method based on multi-source data provided by the embodiment of the present invention, the robot can realize autonomous patrol inspection in a campus or industrial park.

[0058] Application scenario 2: By running the positioning and mapping method based on multi-source data provided by an embodiment of the present invention, the robot can complete material delivery indoors.

[0059] Application scenario 3: By running the positioning and mapping method based on multi-source data provided in the embodiment of the present invention, a better point cloud map or raster map can be established, laying a good foundation for autonomous driving.

[0060] The present invention uses laser radar, depth camera and IMU to obtain multi-source environmental data and conducts mapping comparison experiments in real environments. Figure 5-Figure 7 As shown in the figure, the sensor detection range of single laser SLAM is limited, there are many noise points on the edge of the map, and there are large deviations between the obstacle boundaries and the actual environment. After integrating the visual and inertial navigation sensor information, the local map not only contains the main spatial structure information, but also can capture the detailed features of the environment. The noise is properly controlled, and the established map is more accurate than that of single laser SLAM.

[0061] The present invention comprehensively utilizes laser radar, depth camera and IMU to obtain multi-source environmental data, and by improving the Bayesian algorithm based on multi-source data, constructs a grid map that integrates two-dimensional point cloud data, inertial data and three-dimensional image data, and has rich environmental information and high precision. The laser radar and depth camera provide real-time perception capabilities. As the robot moves, new sensor data is constantly acquired. These new data can be compared and matched with existing map data to achieve real-time updating of the map. When environmental changes are detected, such as new obstacles or road changes, the map can be automatically updated, grid cells that have not been updated for a long time are removed, and the map is smoothed to ensure the timeliness and accuracy of the map.

[0062] In addition, in order to ensure the stability and reliability of the map, data quality control and exception handling also need to be considered. For example, noise and interference in sensor data can be processed through filtering and noise reduction techniques. Calibration errors and synchronization issues between sensors also need to be corrected and optimized accordingly.

[0063] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered within the protection scope of the present invention.

Claims

1. A positioning and mapping method based on multi-source data, characterized in that: The method uses the extended Kalman filter algorithm to fuse the lidar and IMU data to obtain multi-source environmental data. By improving the Bayesian algorithm based on multi-source data, the two-dimensional point cloud data, inertial data and three-dimensional image data are fused to construct a multi-source data grid map. The method specifically includes the following steps: S1, inter-frame matching of laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of laser; S2, using the extended Kalman filter algorithm to fuse the mobile robot's posture; S3, depth camera data processing: Through the image feature extraction and description algorithm ORB, the main direction of the FAST corner point is used to rotate the BRIEF descriptor to make the BRIEF descriptor rotationally invariant, and the matching point pairs are obtained. The PnP method is used to solve the movement of the depth camera 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method; S4, improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data raster map, and solve the raster probability based on multi-source data.

2. The positioning and mapping method based on multi-source data according to claim 1, characterized in that: In step S1, the PL-ICP algorithm is used to improve the error form, including the steps of: S101: according to the odometer data, set the initial transformation matrix, set the number of iterations and the convergence threshold, and convert the current frame laser data to the reference coordinate system; S102: For each point in the current point cloud, find the two closest points to the point in the reference point cloud, and determine the connection line between the two points; S103: Based on the distance from the point to the line, calculate the rotation and translation transformation matrix that minimizes the total distance; S104: transforming the current point cloud according to the calculated transformation matrix, calculating the error between the transformed point cloud and the reference point cloud, and if the error is less than a preset threshold or reaches a maximum number of iterations, stopping the iteration and outputting the final transformation matrix as the registration result; Otherwise, return to step S101 to continue iteration.

3. The positioning and mapping method based on multi-source data according to claim 1, characterized in that: In step S2, the extended Kalman filter algorithm is used to fuse the mobile robot posture, including the steps of: S201, define a state vector equation and an observation vector equation; the state vector includes position, velocity, attitude, and IMU deviation; the observation vector includes distance and angle information measured by the laser radar, and acceleration and angular velocity measurement values ​​of the IMU; S202, estimate the state variables in the last a posteriori state of the system Perform a first-order Taylor expansion at , and linearize the observation vector equation. Linearization at S203, performing a first-order Taylor expansion based on the state vector equation and the observation vector equation to obtain a linearized result, performing prediction and correction of the extended Kalman filter, and updating the real-time position of the mobile robot; S204, constructing a lidar grid map based on the principle of binary Bayesian filtering, using the input of multiple lidars to determine the obstacle occupancy probability of each grid in the lidar grid map, and adjusting the grid occupancy probability according to the new observation data updated according to the real-time position of the mobile robot.

4. The positioning and mapping method based on multi-source data according to claim 3, characterized in that: In step S201, the state vector equation is: ； In the formula, For the system The state variables at time, is the state vector operation function, For the system The state variables at time, For the system Control input at all times; is the process noise, which represents the random disturbance in the state change due to system uncertainty or modeling error; The observation vector equation is: ； In the formula, is the observation vector operation function, is the observation noise, which represents the random error or uncertainty in the observation process; and Satisfy normal distribution; In step S202, the state variable is estimated in the last a posteriori state of the system Perform a first-order Taylor expansion at the position for linearization, and the expression is: ； In the formula, Estimate the state variables for the last posterior state, The error is 0, let , Estimate state variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , , then: ； In the formula, Predict state variables for the current state; The observation vector equation is Linearization at, the expression is: ； because is the error, so let is 0, let , Predict observed variables for the current state; For function right The Jacobian matrix of the partial derivatives of , ; For function right The Jacobian matrix of the partial derivatives of , ; ； In the formula, is the probability, is the extrinsic parameter matrix of the depth camera, is a normal distribution, is the observation radius of the depth camera.

5. The positioning and mapping method based on multi-source data according to claim 4, characterized in that: In step S203, the prediction of the extended Kalman filter includes: 1) State vector prediction: ； 2) Error covariance prediction: ； In the formula, for The prior estimate error covariance at time , for -1 time probability, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the Kalman filter, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; Extended Kalman filter correction, including: (i) Calculation of Kalman gain: Kalman gain is used to determine the relative weight between the predicted value and the observed value; it is calculated based on the prediction error covariance and the observation noise covariance, and the expression is: ； In the formula, is the Kalman gain, is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of , is the function under the constraints of the extrinsic matrix of the depth camera right The Jacobian matrix of the partial derivatives of ; (ii) State vector update: Use the Kalman gain and observations to update the state estimate; ； In the formula, is the current updated state variable, is the Kalman gain; (iii) Covariance update: Update the error covariance of the state estimate to reflect the corrected uncertainty; ； In the formula, is the error covariance, is the estimated value of the state; In step S204, for the laser radar grid map A grid on , expressed in terms of probability, as the probability of being occupied , the probability value of 1 represents the occupied state, and the probability value of 0 represents the idle state. The ratio of the two is introduced Indicates The state of the grid; After the laser radar performs a scanning measurement, the measurement value is obtained. , update the grid status to: ； In the formula, To update the status value of the grid, is the probability of a grid being occupied from the occupied state to the measured value, is the probability that a grid in the idle state is occupied by the measured value; According to the Bayesian formula: ； ； In the formula, is the probability that the measured value obtained after scanning measurement is occupied, is the probability that a grid is occupied from the measured value to the occupied state, is the probability that a grid is occupied when the measured value reaches the idle state; Combining the two formulas we get: ； In the formula, For the The state of the grid; Taking the logarithm of both sides of the above equation, we get: ； In the above formula, only Contains the measurement value. This ratio is called the measurement model. It indicates that the laser radar observation results of the grid have only two states: occupied and idle. By default, the probability of occupied and idle in the initial state of the grid is 0.

5. and represents the grid state before and after the measurement, the update rule is simplified to: ； The larger the probability value of a grid is, the greater the probability that the grid is in an occupied state and has an obstacle.

6. The positioning and mapping method based on multi-source data according to claim 5, characterized in that: In step S3, PnP is used to solve the motion of the depth camera 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method, including: Assume that the coordinates of the depth camera in space are expressed as , the three-dimensional coordinates are projected onto the normalized plane, and the projection coordinates are , according to the pinhole imaging principle; the relationship between the three-dimensional space coordinates and the projection coordinates is derived as follows: ； In the formula, is the intrinsic parameter matrix of the depth camera, is the extrinsic parameter matrix of the depth camera, It is the relationship value between the three-dimensional space coordinates and the projection coordinates.

7. The method for positioning and mapping based on multi-source data according to claim 6, characterized in that: In step S3, the method of minimizing the reprojection error includes: the reprojection error is expressed as the Euclidean distance between the actual observation value and the projection coordinate to construct a least squares error objective function: ； In the formula, is the least squares error target value, is the projection coordinate, is the coordinate of the depth camera in space, is the least squares absolute function; Solve for the derivative of the error term with respect to the optimization variable: ； In the formula, is the error, is the state variable, is the state variable change value, is the error function under the state variable, is the linearized value of the state variable; According to the error objective function, record the spatial point Coordinates in the camera coordinate system ,definition As an intermediate variable; Using the chain rule, we can get the Lie group Left multiplication disturbance Taking the derivative, we get the following formula: ； In the formula, is the left-multiplied perturbation of the Lie algebra, is the disturbance value, is the disturbance error; is the error with respect to the projection point The derivative of , according to the camera imaging principle, the Jacobian matrix is ​​introduced as follows: ； In the formula, For function right The Jacobian matrix of the partial derivatives of , For function right The Jacobian matrix of the partial derivatives of ; Middle Pair Regarding the derivation of Lie algebra, taking the first three dimensions, we get the following formula: ； In the formula, Derivative of the Lie algebra for the projection point; Multiplying the above two equations, we get the Jacobian matrix: ； The above formula represents the changing relationship between the reprojection error and the first-order derivative of the Lie algebra of the camera pose; By obtaining the Jacobian matrix, the linearization of the objective function is completed, and then the posture transformation matrix is ​​obtained through the Gauss-Newton method, so as to obtain the linearized incremental equation of the objective function; ； In the formula, For linearization, is the perturbation value objective function under the Jacobian matrix constraint, For the linearization of the disturbance value objective function, is the disturbance value change, is the disturbance error; Using the known Jacobian matrix and initial pose value for iterative optimization, when When the value is lower than the set value, the iteration stops and the pose transformation matrix is ​​obtained. Otherwise, the Continue iterating until the requirements are met.

8. The positioning and mapping method based on multi-source data according to claim 1, characterized in that: In step S4, a multi-source data grid map is constructed, and the grid probability based on the multi-source data is solved, including: Assume that the map consists of a set of grid cells, each of which is represented by , indicating the state of the grid unit; the data observed by the sensor is ,express Observation data of sensors at all times; According to Bayes' theorem, we get: ； In the formula, Given the observation data Under these conditions, the grid cell The posterior probability of In a given map state And all previous observations Under the condition of The probability of , that is, the observation model; is all the observed data before given Under the condition of The prior probability of is all the observed data before given Under the condition of probability; Assume that the map is divided into a series of grids, each grid is free or occupied; use To represent the grid The probability of being occupied is To represent the grid is the probability of being idle.

9. The positioning and mapping method based on multi-source data according to claim 8, characterized in that: For grid maps, updates come from sensor observations, and the Bayesian update formula is expressed as: ； In the formula, In observing the data Afterwards, the grid Probability of being occupied; Is in the grid Data observed when occupied The probability of Is a grid Prior probability of being occupied; Observed data The probability of In the process of processing multi-source data, when the same grid is detected by different sensors such as lidar and depth camera, a threshold is set before calculating the probability value of each grid. ; When the probability value result of the grid Greater than , the grid is set to the occupied state, and the transition probability value is 1; otherwise, the probability value of the grid is kept ; ； In the formula, Represents the probability value of a certain grid after calculating various sensor data; finally, the Bayesian rule is used to solve the grid probability based on multi-source data for: ； In the formula, is the raster probability based on multi-source data.

10. A positioning and mapping system based on multi-source data, characterized in that: The system implements the positioning and mapping method based on multi-source data as described in any one of claims 1 to 9, and the system includes: The inter-frame matching module (1) of the laser point cloud is used for inter-frame matching of the laser point cloud: the PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is used to complete the iteration speed and matching accuracy solution in the inter-frame matching of the laser; An extended Kalman filter algorithm fusion module (2), used for fusing the position and posture of the mobile robot using an extended Kalman filter algorithm; The depth camera data processing module (3) is used for depth camera data processing: the image feature extraction and description algorithm ORB uses the main direction of the FAST corner point to rotate the BRIEF descriptor so that the BRIEF descriptor has rotation invariance, obtains matching point pairs, uses PnP to solve the movement of the depth camera 3D to 2D point pairs, and obtains the optimal camera pose by minimizing the reprojection error method; The multi-source data fusion module (4) is used to improve the Bayesian algorithm based on multi-source data, perform multi-source data fusion, construct a multi-source data grid map, and solve the grid probability based on the multi-source data.

Images