A positioning and mapping method and system based on multi-source data

Abstract

The present invention belongs to the technical field of robot pose positioning, and discloses a positioning and mapping method and system based on multi-source data. This method analyzes multi-source SLAM solutions. Secondly, it proposes how to use the extended Kalman filter algorithm to establish a laser inertial odometer to achieve inter-frame matching of laser point clouds and establish a lidar grid map. Then, a depth camera is used to obtain three-dimensional image information, ORB features of the image are extracted, and the pose information of the depth camera is solved through the PnP algorithm and converted into two-dimensional point cloud data. Finally, by improving the Bayesian algorithm based on multi-source data, a grid map is constructed using multi-source data. The present invention uses low-cost sensors to achieve good positioning and mapping effects.

Description

Technical Field

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

[0002] With the rapid development of robot technology, more and more technological products such as autonomous vehicles and drones have begun to enter people's lives. An important technology for realizing the intelligence of various mobile robots is positioning. In practical applications, when mobile robots face complex scenarios, such as changing illumination 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 when the robot moves too fast or turns too quickly. For example, the scanning observation distance of lidar sensors is limited and they are easily affected by complex geometric structures in the environment. Cameras have certain requirements for the lighting conditions of the robot's surrounding environment. Encoder motors will generate cumulative errors after long-term operation. Luo Yuan et al. fused odometer and radar data, optimized the proposal distribution function, effectively reduced the uncertainty of the robot's pose in the prediction stage, and reduced the number of particles required for SLAM.

[0004] In 2015, Leutenegger et al. proposed an optimized VIO fusion scheme, OKVIS (OpenKey-frame-based Visual-Inertial SLAM). They introduced a tightly coupled fusion framework of IMU and image key points into the non-linear optimization problem, and applied linearization and marginalization to implement key frames. In 2015, Zhang et al. proposed a SLAM system that fuses vision and lidar, namely VLOAM (Visual-lidar Odometry and Mapping). The online method they proposed starts from visual odometry to estimate its own motion, and records the point cloud from the scanning lidar with high frequency and low fidelity. Then, the motion estimation is improved through lidar 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). This system fuses the data of a monocular camera and an IMU, and uses a method based on tightly coupled and non-linear optimization 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 minimize the calculation cost while completing repositioning. Moreover, this 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-lidar fusion SLAM framework consisting of three modules. This framework includes a two-stage direct visual odometry module, a lidar mapping module that utilizes dynamic objects, and a parallel global and local search loop detection module that combines the visual bag of words and LIDAR-Iris features. The results show that this algorithm achieves more accurate pose estimation compared with the state-of-the-art methods.

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

[0006] 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 as follows. The positioning and mapping method based on multi-source data uses the extended Kalman filter algorithm to fuse lidar and IMU data to obtain multi-source environmental data, and constructs a multi-source data grid map by improving the Bayesian algorithm based on multi-source data, specifically including the following steps:

[0008] 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 adopted to solve the iteration speed and matching accuracy in the inter-frame matching of the laser.

[0009] S2, Fusing the pose of the mobile robot using the Extended Kalman Filter algorithm.

[0010] S3, Depth camera data processing: Through the image feature extraction and description algorithm ORB, using the main direction of the FAST corner points to rotate the BRIEF descriptor, making the BRIEF descriptor rotation-invariant, obtaining the matching point pairs, using PnP to solve the motion of the depth camera from 3D to 2D point pairs, and obtaining the optimal camera pose by minimizing the reprojection error method.

[0011] S4, Improving the Bayesian algorithm based on multi-source data, performing multi-source data fusion, constructing a multi-source data grid map, and solving the grid probability based on multi-source data.

[0012] In step S1, the PL-ICP algorithm is used to improve the error form, including the steps:

[0013] S101: According to the data of the odometer, set the initial transformation matrix, set the number of iterations and the convergence threshold, and transform the current frame of laser data into the reference coordinate system.

[0014] S102: For each point in the current point cloud, find the two nearest points in the reference point cloud and determine the line segment between these two points.

[0015] S103: Based on the distance from a point to a line, calculate the rotation and translation transformation matrices that minimize the total distance.

[0016] S104: Transform the current point cloud according to the calculated transformation matrix, calculate the error between the transformed point cloud and the reference point cloud. If the error is less than the preset threshold or the maximum number of iterations is reached, stop the iteration and output the final transformation matrix as the registration result; otherwise, return to step S101 to continue the iteration.

[0017] In step S2, fusing the pose of the mobile robot using the Extended Kalman Filter algorithm, including the steps:

[0018] S201, Defining the state vector equation and the observation vector equation; the state vector includes position, velocity, attitude, and the bias of the IMU; the observation vector includes the distance and angle information measured by the lidar, as well as the acceleration and angular velocity measurement values of the IMU.

[0019] S202, Performing a first-order Taylor expansion for linearization at the previous posterior state estimate of the system's state variables and linearizing the observation vector equation at Linearization at

[0020] S203. Based on the state vector equation and the observation vector equation, perform first-order Taylor expansion for the linearization result, and conduct prediction and correction of the extended Kalman filter to update the real-time position of the mobile robot.

[0021] S204. Construct a lidar grid map based on the binary Bayesian filtering principle, use the input of multiple lidars to judge the occupancy probability of obstacles in each grid in the lidar grid map, and adjust the occupancy probability of the grid according to the new observation data updated by the real-time position of the mobile robot.

[0022] In step S201, the state vector equation is:

[0023] ;

[0024] In the formula, is the state variable of the system at time, is the state vector operation function, is the state variable of the system at time, is the control input of the system at time; is the process noise, representing the random perturbation in the state change due to system uncertainty or modeling error;

[0025] The observation vector equation is:

[0026] ;

[0027] In the formula, is the observation vector operation function, is the observation noise, representing the random error or uncertainty in the observation process; and satisfy the normal distribution;

[0028] In step S202, perform first-order Taylor expansion for linearization at the estimated state variable of the previous posterior state of the system, and the expression is:

[0029] ;

[0030] In the formula, is the estimated state variable of the previous posterior state, The error of is 0, let , is the estimated state variable of the current state; is the function with respect to The Jacobian matrix of the partial derivatives ; is the Jacobian matrix of the partial derivatives of the function with respect to ; , then we have:

[0031] ;

[0032] In the formula, is the predicted state variable of the current state;

[0033] Linearize the observation vector equation at , and the expression is:

[0034] ;

[0035] Since is the error, so let be 0, let , is the predicted observation variable of the current state; is the Jacobian matrix of the partial derivatives of the function with respect to ; ; is the Jacobian matrix of the partial derivatives of the function with respect to ; ;

[0036] ;

[0037] In the formula, is the probability, is the external parameter matrix of the depth camera, is the normal distribution, is the observation radius of the depth camera.

[0038] In step S203, the prediction of the extended Kalman filter includes:

[0039] 1) State vector prediction:

[0040] ;

[0041] 2) Error covariance prediction:

[0042] ;

[0043] In the formula, is the prior estimated error covariance at time, is the probability at -1 time, For the function under the constraint of the extrinsic parameter matrix of the depth camera to the Jacobian matrix of the partial derivative, is the Kalman filter, For the function under the constraint of the extrinsic parameter matrix of the depth camera to the Jacobian matrix of the partial derivative;

[0044] The correction of the extended Kalman filter includes:

[0045] (i) Calculate the Kalman gain: The 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:

[0046] ;

[0047] In the formula, is the Kalman gain, For the function under the constraint of the extrinsic parameter matrix of the depth camera to the Jacobian matrix of the partial derivative, For the function under the constraint of the extrinsic parameter matrix of the depth camera to the Jacobian matrix of the partial derivative;

[0048] (ii) State vector update: Use the Kalman gain and the observed value to update the state estimate;

[0049] ;

[0050] In the formula, is the current updated state variable, is the Kalman gain;

[0051] (iii) Covariance update: Update the error covariance of the state estimate, which reflects the uncertainty after correction;

[0052] ;

[0053] In the formula, is the error covariance, is the state estimate value;

[0054] In step S204, for a certain grid on the lidar grid map , the occupancy probability is represented in the form of probability , a probability value of 1 represents the occupied state, and a probability value of 0 represents the free state. The ratio of the two is introduced to represent the The state of a grid;

[0055] After a lidar performs a scanning measurement, the measured value is obtained , and the state of the grid is updated to:

[0056] ;

[0057] In the formula, is the state value of the updated grid, is the probability that a grid in the occupied state is occupied by the measured value, is the probability that a grid in the free state is occupied by the measured value;

[0058] According to Bayes' formula, we have:

[0059] ;

[0060] ;

[0061] In the formula, is the probability that the measured value obtained after the scanning measurement is occupied, is the probability that a grid in the occupied state is occupied by the measured value, is the probability that a grid in the free state is occupied by the measured value;

[0062] Combining the two formulas, we get:

[0063] ;

[0064] In the formula, is the state of the th grid;

[0065] Taking the logarithm of both sides of the above formula, we get:

[0066] ;

[0067] In the above formula, only contains the measured value. This ratio is called the measurement model, indicating that the lidar's observation result of the grid has only two states: occupied and free. By default, the probabilities of occupancy and freedom of the grid's initial state are both 0.5. Using and to represent the grid states before and after the measured value, the update rule is simplified to:

[0068] ;

[0069] If the probability value of the grid is larger, it means that the probability of the grid being in the occupied state with an obstacle is greater.

[0070] In step S3, the PnP method is used to solve the motion of the depth camera from 3D to 2D point pairs, and the optimal camera pose is obtained by minimizing the reprojection error method, including:

[0071] Let the coordinate representation of the depth camera in space be , and after projecting this three-dimensional coordinate onto the normalized plane, the projected coordinate is . According to the pinhole imaging principle, the relationship between the three-dimensional space coordinate and the projected coordinate is deduced as:

[0072] ;

[0073] In the formula, is the internal parameter matrix of the depth camera, is the external parameter matrix of the depth camera, is the relationship value between the three-dimensional space coordinate and the projected coordinate.

[0074] In step S3, the method of minimizing the reprojection error includes: The reprojection error is expressed as the Euclidean distance between the actual observed value and the projected coordinate to construct a least-squares error objective function:

[0075] ;

[0076] In the formula, is the least-squares error target value, is the projected coordinate, is the coordinate of the depth camera in space, is the least-squares absolute function;

[0077] Solve the derivative of the error term with respect to the optimization variable:

[0078] ;

[0079] In the formula, is the error, is the state variable, is the change value of the state variable, is the error function under the state variable, is the linearized value of the state variable;

[0080] According to the error objective function, denote the coordinate of the spatial point in the camera coordinate system, and define as an intermediate variable;

[0081] Using the chain rule, take the derivative of the left multiplication of the Lie group by the perturbation to obtain the following formula:

[0082] ;

[0083] In the formula, is the left multiplication perturbation of the Lie algebra, is the perturbation value, is the perturbation error;

[0084] is the derivative of the error with respect to the projection point . According to the camera imaging principle, the Jacobian matrix is derived as follows:

[0085] ;

[0086] In the formula, is the Jacobian matrix of the partial derivative of the function with respect to , is the Jacobian matrix of the partial derivative of the function with respect to ;

[0087] In , taking the derivative with respect to the Lie algebra and taking its first three dimensions gives the following formula:

[0088] ;

[0089] In the formula, is the derivative value of the projection point Lie algebra;

[0090] Multiplying the above two formulas gives the Jacobian matrix:

[0091] ;

[0092] The above formula represents the relationship between the reprojection error and the change of the first-order derivative of the camera pose Lie algebra;

[0093] By obtaining the Jacobian matrix, the linearization of the objective function is completed, and then the pose transformation matrix is obtained by the Gauss-Newton method, so as to obtain the linearized objective function increment equation;

[0094] ;

[0095] In the formula, is the linearization process, is the objective function of the perturbation value under the Jacobian matrix constraint, is the linearization process of the objective function of the perturbation value, is the change value of the perturbation value, is the perturbation error;

[0096] Using the known Jacobian matrix and the initial pose value for iterative optimization, when Stop iterating after it is lower than the set value and obtain the pose transformation matrix; otherwise, continue iterating until the requirement is met according to Continue iterating until the requirement is met.

[0097] In step S4, construct a multi-source data grid map and solve the grid probability based on multi-source data, including:

[0098] Assume that the map consists of a set of grid cells, and each grid cell is represented as , representing the state of the grid cell; the data observed by the sensor is , representing the observation data of the sensor at time

[0099] According to Bayes' theorem, we get:

[0100] ;

[0101] In the formula, is the posterior probability of grid cell under the condition of given observation data ; is the probability of the sensor observing data under the condition of given map state and all previous observation data , that is, the observation model; is the prior probability of grid cell under the condition of given all previous observation data ; is the probability of the sensor observing data under the condition of given all previous observation data ;

[0102] Assume that the map is divided into a series of grids, and each grid is either free or occupied; use to represent the probability that grid is occupied, and use to represent the probability that grid is free.

[0103] Furthermore, for the grid map, update the observation bureau from the sensor, and the Bayesian update formula is expressed as:

[0104] ;

[0105] In the formula, is the probability that grid is occupied after observing data ; is the probability that grid The probability of observing data when it is occupied ; is the prior probability that the grid is occupied; The probability of observing data ;

[0106] In the process of processing multi-source data, when the same grid is detected by different sensors such as lidar and depth camera, before calculating the probability value of each grid, a threshold is set; when the probability value result of this grid is greater than , then this grid is set to the occupied state and the conversion probability value is 1; otherwise, the probability value of this grid is kept as ;

[0107] ;

[0108] In the formula, represents the probability value of a certain grid obtained by solving after calculating various sensor data; finally, using Bayes' rule, the grid probability based on multi-source data is solved as:

[0109] ;

[0110] In the formula, is the grid probability based on multi-source data.

[0111] 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. This system includes:

[0112] An inter-frame matching module for lidar point clouds, used for inter-frame matching of lidar point clouds: improving the error form using the PL-ICP algorithm, adopting a point-to-line registration method, and solving the iteration speed and matching accuracy in the inter-frame matching of lidar;

[0113] An extended Kalman filter algorithm fusion module, used for fusing the pose of the mobile robot using the extended Kalman filter algorithm;

[0114] A depth camera data processing module, used for depth camera data processing: using the ORB algorithm for image feature extraction and description, using the main direction of FAST corner points to rotate the BRIEF descriptor, making the BRIEF descriptor rotation-invariant, obtaining matching point pairs, using PnP to solve the motion of the depth camera from 3D to 2D point pairs, and obtaining the optimal camera pose by minimizing the reprojection error method;

[0115] A 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 grid map, and solve the grid probability based on multi-source data.

[0116] Combining all the above technical solutions, the beneficial effects of the present invention are as follows: First, the present invention analyzes the multi-source SLAM solution. Secondly, it proposes how to establish a laser inertial odometer using the extended Kalman filter algorithm to achieve inter-frame matching of laser point clouds and establish a lidar grid map. Then, a depth camera is used to obtain three-dimensional image information, extract the ORB features of the image, and solve the pose information of the depth camera through the PnP algorithm and convert it into two-dimensional point cloud data. Finally, by improving the Bayesian algorithm based on multi-source data, a grid map is constructed using multi-source data to improve the accuracy and precision of mapping. Brief Description of the Drawings

[0117] The accompanying drawings herein are incorporated into the specification and form a part of the specification, showing embodiments consistent with the present disclosure and used together with the specification to explain the principles of the present disclosure;

[0118] Figure 1 is a flowchart of a positioning and mapping method based on multi-source data provided by an embodiment of the present invention;

[0119] Figure 2 is a flowchart of the PL-ICP algorithm provided by an embodiment of the present invention;

[0120] Figure 3 is a schematic diagram of the reprojection error provided by an embodiment of the present invention;

[0121] 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;

[0122] Figure 5 is a schematic diagram of a real indoor corridor environment provided by an embodiment of the present invention;

[0123] Figure 6 is the single-laser SLAM mapping provided by an embodiment of the present invention;

[0124] Figure 7 is the multi-source SLAM mapping provided by an embodiment of the present invention;

[0125] 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 Embodiments

[0126] To make the above objects, features, and advantages of the present invention more apparent and understandable, the following will describe in detail the specific embodiments of the present invention with reference to the accompanying drawings. Many specific details are set forth in the following description to facilitate a thorough 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 departing from the spirit of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed below.

[0127] The innovation of the present invention lies in: the present invention proposes a unique SLAM solution for multi-source data and proposes the data fusion of IMU, lidar, and depth camera; using low-cost sensors to achieve good positioning and mapping effects. The present invention comprehensively utilizes lidar, depth camera, and IMU to obtain multi-source environmental data, and by improving the Bayesian algorithm based on multi-source data, fuses two-dimensional point cloud data, inertial data, and three-dimensional image data to construct a multi-source data grid map with rich environmental information and high accuracy.

[0128] Example 1, as Figure 1 shown, the positioning and mapping method based on multi-source data provided by the embodiment of the present invention includes:

[0129] S1, inter-frame matching of laser point clouds: The PL-ICP algorithm is used to improve the error form, and the point-to-line registration method is adopted to complete the solution of the iteration speed and matching accuracy in the inter-frame matching of the laser.

[0130] The pose information of the mobile robot can be obtained through IMU data. However, due to the cumulative effect of the integration operation, significant drift errors will occur after long-term operation. This error will gradually increase over time, resulting in a decrease in the accuracy of pose estimation. Therefore, currently, the method of solving the pose change of the robot mainly uses the inter-frame matching of lidar. And traditional inertial information is more used as an auxiliary to enhance the accuracy of positioning.

[0131] In the solution of the inter-frame matching of the laser, the Iterative Closest Point (ICP) algorithm is a commonly used point cloud registration algorithm for aligning and matching two or more point clouds. It iteratively optimizes the least square error between two point clouds 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 includes rotation and translation transformations. But in practical applications, there may be non-rigid deformations, resulting in a decline in the performance of the algorithm.

[0132] To address the above problems, the PL-ICP algorithm improves the error form. By adopting the point-to-line registration method, it improves the robustness to noise and enhances the iteration speed and matching accuracy. The specific process is as follows:

[0133] S101: Set the initial transformation matrix according to the odometer data, set the number of iterations and the convergence threshold, and transform the current frame of lidar data into the reference coordinate system;

[0134] S102: For each point in the current point cloud, find the two closest points in the reference point cloud and determine the line segment between these two points;

[0135] S103: Calculate the rotation and translation transformation matrices that minimize the total distance based on the distance from a point to a line;

[0136] S104: Transform the current point cloud according to the calculated transformation matrix, calculate the error between the transformed point cloud and the reference point cloud. If the error is less than the preset threshold or the maximum number of iterations is reached, stop the iteration and output the final transformation matrix as the registration result; otherwise, return to step S101 to continue the iteration.

[0137] S2. Use the Extended Kalman Filter algorithm to fuse the pose of the mobile robot;

[0138] In the process of solving the real-time pose of the mobile robot, both the IMU and the lidar have measurement errors. The IMU estimates the motion state of an object by measuring acceleration and angular velocity and performing integration operations on 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 "integration drift" phenomenon. This drift will increase over time, thus affecting the accuracy and reliability of the IMU data. During the scanning process of the lidar, when the laser beam falls on an area with a large difference in reflectivity or objects with a large distance difference, a mixed pixel phenomenon may occur. This will cause the measured distance to be actually the average distance of multiple target points, thereby forming an error and affecting the accurate estimation of the robot's pose.

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

[0140] The specific process is as follows.

[0141] S201. Define the state vector equation and the observation vector equation; the state vector includes position, velocity, attitude, and the bias of the IMU; the observation vector includes the distance and angle information measured by the lidar, as well as the acceleration and angular velocity measurement values of the IMU;

[0142] The state vector equation is:

[0143] ;

[0144] In the formula, is the state variable of the system at moment, is the state vector operation function, is the state variable of the system at moment, is the control input of the system at moment; is the process noise, representing the random perturbation in the state change caused by the system uncertainty or modeling error;

[0145] The observation vector equation is:

[0146] ;

[0147] In the formula, is the observation vector operation function, is the observation noise, representing the random error or uncertainty in the observation process; and satisfy the normal distribution;

[0148] S202, at the previous posterior state estimation state variable of the system, perform first-order Taylor expansion for linearization, and linearize the observation vector equation at ;

[0149] The result is as follows:

[0150] ;

[0151] In the formula, is the previous posterior state estimation state variable, the error of is 0, let be the current state estimation state variable; is the Jacobian matrix of the partial derivative of the function with respect to , ; is the Jacobian matrix of the partial derivative of the function with respect to , , then there is:

[0152] ;

[0153] In the formula, is the current state prediction state variable;

[0154] Linearize the observation vector equation at , and the expression is:

[0155] ;

[0156] 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 , ;

[0157] ;

[0158] 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.

[0159] 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;

[0160] 1) State vector prediction:

[0161] ;

[0162] 2) Error covariance prediction:

[0163] ;

[0164] 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 ;

[0165] The extended Kalman filter correction includes:

[0166] (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:

[0167] ;

[0168] 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 ;

[0169] (ii) State vector update: Use the Kalman gain and observations to update the state estimate;

[0170] ;

[0171] In the formula, is the current updated state variable, is the Kalman gain;

[0172] (iii) Covariance update: Update the error covariance of the state estimate to reflect the corrected uncertainty;

[0173] ;

[0174] In the formula, is the error covariance, is the estimated value of the state;

[0175] 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;

[0176] 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.

[0177] 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;

[0178] 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.

[0179] 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:

[0180] ;

[0181] 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;

[0182] According to Bayes' formula, we have:

[0183] ;

[0184] ;

[0185] In the formula, is the probability that the measured value obtained after scanning measurement is occupied, is the probability that a certain grid from the measured value to the occupied state is occupied, is the probability that a certain grid from the measured value to the free state is occupied;

[0186] Combining the two formulas, we get:

[0187] ;

[0188] In the formula, is the state of the th grid;

[0189] Taking the logarithm of both sides of the above formula, we get:

[0190] ;

[0191] In the above formula, only contains the measured value. This ratio is called the measurement model, indicating that the lidar's observation result of the grid has only two states: occupied and free. By default, the probabilities of the occupied and free states of the grid at the initial state are both 0.5. Using and to represent the grid states before and after the measured value, the update rule is simplified to:

[0192] ;

[0193] If the probability value of the grid is larger, it means that the probability of the grid being in the occupied state with an obstacle is greater.

[0194] As can be seen from the above formula, the basic idea of updating the occupancy grid map is to adjust the occupancy probability of the grid according to the new observation data. If the probability value of the grid is larger, it means that the probability of the grid being in the occupied state with an obstacle is greater.

[0195] S3, Depth camera data processing: Through the image feature extraction and description algorithm ORB, using the main direction of the FAST corner points, rotate the BRIEF descriptor to make the BRIEF descriptor rotation-invariant, obtain the matching point pairs, use PnP to solve the motion of the depth camera from 3D to 2D point pairs, and obtain the optimal camera pose by minimizing the reprojection error method;

[0196] A depth camera can obtain the point cloud information of the motion environment of a mobile robot. First, the depth camera emits infrared light or laser beams and irradiates the light onto the objects in the scene. When the light hits the surface of the object, a part of the light is reflected back to the camera. The depth camera records 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.

[0197] 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, with added directionality. The FAST (Features from Accelerated Segment Test) feature detection algorithm is a commonly used corner detection algorithm for quickly detecting corners with obvious gray-level changes in an image. Its detection process is as follows:

[0198] (a) Corner candidate point selection: First, a set of candidate points is selected from the image.

[0199] (b) Pixel gray-level comparison: For each candidate point, by comparing the gray-level values of the surrounding pixel points with the gray-level value of the candidate point, it is determined whether the candidate point is a corner. Usually, the surrounding pixel points of 16 candidate points are selected for comparison.

[0200] (c) Gray-level change judgment: For each candidate point, the FAST algorithm divides the 16 pixel points around it into two groups: pixel points with gray-level values larger than that of the candidate point and pixel points with gray-level values smaller than that of the candidate point. If there are more than 12 pixel points in one group with gray-level values larger (or smaller) than that of the candidate point, the candidate point is considered likely to be a corner.

[0201] (d) Corner determination: After the gray-level change judgment, the candidate points may be marked as corners. At this time, non-maximum suppression (NMS) can be further performed to remove duplicate corners in the adjacent area.

[0202] (e) To quickly eliminate non-feature points, the pixel brightness at four specific positions, namely 1, 5, 9, and 13, can be directly observed. If at least three of the brightness values of these four pixel points simultaneously meet or exceed the preset threshold condition, then this point can be regarded as a feature point; conversely, if none meet the condition, it can be determined that this point is not a feature point, so it can be directly eliminated.

[0203] In addition, the original FAST corner points do not have directionality themselves, which may cause anomalies during feature point matching. ORB calculates the main direction of the FAST corner points, assigns a direction to each corner point, and thus makes it rotation invariant. This enables the ORB feature points to maintain stable matching performance even when the images at different scales are rotated.

[0204] BRIEF is a feature descriptor used to describe the detected feature points. It abandons the traditional method of using the regional gray histogram to describe feature points and adopts a binary coding method, thereby accelerating the establishment speed of the feature descriptor and reducing the feature matching time. The generation process of the BRIEF descriptor generally involves the following steps:

[0205] (I) Image sampling: First, sample the input image. Usually, use Gaussian filtering to smooth the image, and then perform downsampling to reduce the computational amount and improve the speed.

[0206] (II) Key point detection: Use FAST feature detection, etc. to detect key points in the image.

[0207] (III) Calculate the local image patch: For each detected key point, extract a local image patch around this key point.

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

[0209] (V) Calculate the descriptor: In each local image patch, select a group of pixel pairs and calculate the gray difference of these pixel pairs. Then, binarize these differences to generate a binary string as the descriptor of this key point.

[0210] (VI) Descriptor matching: Use the Hamming distance to compare the descriptors of two key points, thereby performing feature point matching.

[0211] ORB rotates the BRIEF descriptor by utilizing the main direction of the FAST corner points to make it rotation invariant. This further improves the matching stability and accuracy of the ORB feature points.

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

[0213] The reprojection error refers to projecting the 3D point coordinates into the depth camera coordinate system, converting them into the image coordinate system, and comparing them with the corresponding observed values. In the present invention, the PnP problem is constructed as a non-linear least squares problem regarding the reprojection error, aiming to solve the pose of the depth camera, that is, the rotation matrix and the translation vector, to minimize the reprojection error.

[0214] Let the coordinate representation of the depth camera in space be , and after projecting this 3D coordinate onto the normalized plane, the projected coordinate is . According to the pinhole imaging principle, the relationship between the 3D space coordinate and the projected coordinate is derived as:

[0215] ;

[0216] In the formula, is the internal parameter matrix of the depth camera, is the external parameter matrix of the depth camera, is the relationship value between the 3D space coordinate and the projected coordinate.

[0217] The method for minimizing the reprojection error includes: The reprojection error is expressed as the Euclidean distance between the actual observed value and the projected coordinate, and a least squares error objective function is constructed:

[0218] ;

[0219] In the formula, is the least squares error objective value, is the projected coordinate, is the coordinate of the depth camera in space, is the least squares absolute function;

[0220] The error term in the above formula is the difference between the projected coordinates and the observed coordinates of the three-dimensional space points, that is, the reprojection error, as Figure 3 shown in the schematic diagram of the reprojection error.

[0221] Figure 3 In and are the projections of the points obtained by feature matching In the initial value, the projection of does not completely coincide with the actual Therefore, it is necessary to adjust the camera pose to reduce the distance between the two. Usually, this process needs to consider multiple points to finally reduce the overall error. 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:

[0222] Solve the derivative of the error term with respect to the optimization variable:

[0223] ;

[0224] In the formula, is the error, is the state variable, is the change value of the state variable, is the error function under the state variable, is the linearized value of the state variable;

[0225] According to the error objective function, denote the spatial point in the camera coordinate system as , and define as an intermediate variable;

[0226] Using the chain rule, take the derivative of the left multiplication of the Lie group by the perturbation to obtain the following formula:

[0227] ;

[0228] In the formula, is the left multiplication perturbation of the Lie algebra, is the perturbation value, is the perturbation error;

[0229] is the derivative of the error with respect to the projected point . According to the camera imaging principle, the Jacobian matrix is derived as follows:

[0230] ;

[0231] In the formula, is the function with respect to The Jacobian matrix of the partial derivative is the function with respect to The Jacobian matrix of the partial derivative;

[0232] In Derive with respect to the Lie algebra, and take the first three dimensions to get the following formula:

[0233] ;

[0234] In the formula, is the derivative value of the Lie algebra of the projection point;

[0235] Multiply the above two formulas to get the Jacobian matrix:

[0236] ;

[0237] The above formula represents the relationship between the reprojection error and the first-order derivative of the camera pose Lie algebra;

[0238] By obtaining the Jacobian matrix, the linearization of the objective function is completed, and then the pose transformation matrix is obtained through the Gauss-Newton method, so as to obtain the linearized objective function increment equation;

[0239] ;

[0240] In the formula, is the linearization process, is the objective function of the perturbation value under the constraint of the Jacobian matrix, is the linearization process of the objective function of the perturbation value, is the change value of the perturbation value, is the perturbation error;

[0241] Use the known Jacobian matrix and the initial pose value for iterative optimization. When is lower than the set value, stop the iteration to obtain the pose transformation matrix, otherwise continue the iteration according to until the requirements are met.

[0242] S4. 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.

[0243] Assume that the map consists of a set of grid cells, and each grid cell can be represented as , representing the state of the grid cell. The data observed by the sensor is , representing The observation data of the sensor at time. According to Bayes' theorem, we can get:

[0244] ;

[0245] In the formula, is the posterior probability of grid cell under the given observed data ; is the probability that the sensor observes data under the given map state and all previous observed data , that is, the observation model; is the prior probability of grid cell under the given all previous observed data ; is the probability that the sensor observes data under the given all previous observed data ;

[0246] For a grid map, the update comes from the observation of the sensor. The Bayesian update formula can be expressed as: Suppose the map is divided into a series of grids, and each grid is either free or occupied; use to represent the probability that grid is occupied, and use to represent the probability that grid is free.

[0247] For a grid map, the update comes from the observation of the sensor. The Bayesian update formula is expressed as:

[0248] ;

[0249] In the formula, is the probability that grid is occupied after observing data ; is the probability of observing data when grid is occupied; is the prior probability that grid is occupied; the probability of observing data ;

[0250] During the process of processing multi-source data, under the detection of different sensors (lidar and depth camera), the same grid may have different predicted states. At that time, complete the multi-source data fusion according to the rules in Table 1.

[0251] Table 1 Lidar and depth camera fusion rules

[0252]

[0253] In the process of processing multi-source data, before calculating the probability value of each grid under the detection of different sensors such as lidar and depth camera, a threshold is set ; when the probability value result of this grid is greater than , then this grid is set to the occupied state and the conversion probability value is 1; otherwise, keep the probability value of this grid as ;

[0254] ;

[0255] In the formula, represents the probability value of a certain grid obtained after calculation of various sensor data; finally, use the Bayesian rule to solve the grid probability based on multi-source data as:

[0256] ;

[0257] In the formula, is the grid probability based on multi-source data.

[0258] Example 2, as Figure 4 shown, a positioning and mapping system based on multi-source data includes:

[0259] The inter-frame matching module 1 of the laser point cloud is used for inter-frame matching of the laser point cloud: improve the error form by using the PL-ICP algorithm, adopt the point-to-line registration method, and complete the solution of the iteration speed and matching accuracy in the inter-frame matching of the laser;

[0260] The extended Kalman filter algorithm fusion module 2 is used to fuse the pose of the mobile robot by using the extended Kalman filter algorithm;

[0261] The depth camera data processing module 3 is used for depth camera data processing: through the image feature extraction and description algorithm ORB, use the main direction of the FAST corner points to rotate the BRIEF descriptor, make the BRIEF descriptor have rotational invariance, obtain the matching point pairs, adopt PnP to solve the movement of the depth camera from 3D to 2D point pairs, and obtain the optimal camera pose by minimizing the reprojection error method;

[0262] 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.

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

[0264] Application Scenario 2: By running the positioning and mapping method based on multi-source data provided by the embodiments of the present invention, the robot can complete material distribution indoors.

[0265] Application Scenario 3: By running the positioning and mapping method based on multi-source data provided by the embodiments of the present invention, a better point cloud map or grid map can be established, laying a good foundation for autonomous driving.

[0266] The present invention uses lidar, depth camera and IMU to obtain multi-source environmental data and conducts mapping comparison experiments in a real environment. As Figures 5 - 7 shown, the detection range of the sensor of single-laser SLAM is limited, there are more noise points at the map edge, and there are large deviations between the obstacle boundaries, etc. and the actual environment. When fusing the information of vision and inertial sensors, the local map not only contains the main spatial structure information, but also can capture the detailed features of the environment, and the noise is well controlled. Compared with single-laser SLAM, the established map has higher accuracy.

[0267] The present invention comprehensively uses lidar, depth camera and IMU to obtain multi-source environmental data, and constructs a grid map that integrates two-dimensional point cloud data, inertial data and three-dimensional image data, with rich environmental information and high accuracy by improving the Bayesian algorithm based on multi-source data. Lidar and depth camera provide real-time perception capabilities. As the robot moves, new sensor data is continuously acquired. These new data can be compared and matched with the existing map data to achieve real-time update of the map. When environmental changes are detected, such as newly emerging obstacles or road changes, the map can be automatically updated, removing grid cells that have not been updated for a long time, and performing smoothing processing on the map to ensure the timeliness and accuracy of the map.

[0268] In addition, in order to ensure the stability and reliability of the map, it is also necessary to consider data quality control and anomaly handling. For example, noise and interference in sensor data can be processed through filtering and noise reduction techniques. For calibration errors and synchronization problems between sensors, corresponding corrections and optimizations are also required.

[0269] The above are only the relatively preferred specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any modification, equivalent replacement, and improvement made by those skilled in the art within the technical scope disclosed by the present invention, as long as they are made within the spirit and principle of the present invention, shall be covered by 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 grid map, and solve the grid probability based on multi-source data; 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 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; In step S4, a multi-source data grid map is constructed, and a grid probability based on the multi-source data is solved, including: Assume that the map consists of a set of grid cells, each grid cell is represented by m i , indicating the state of the grid unit; the data observed by the sensor is z t , represents the observation data of the sensor at time t; According to Bayes' theorem, we get: In the formula, p(m i |z 1：t ) is the given observation data z 1：t Under this condition, the grid unit m i The posterior probability of t |m i , z 1：t-1 ) is in a given map state m i and all previous observations z 1：t-1 Under the condition of t The probability of observation model; p(m i |z 1：t-1 ) is the total number of observations z before the given 1：t-1 Under the condition of i The prior probability of t |z 1：t-1 ) is the total number of observations z before the given 1：t-1 Under the condition of t probability; Assume that the map is divided into a series of grids, each grid is free or occupied; use p(s) to represent the probability that grid s is occupied, and use 1-p(s) to represent the probability that grid s is free; For grid maps, updates come from sensor observations, and the Bayesian update formula is expressed as: Where p(s|z) is the probability that grid s is occupied after data z is observed; p(z|s) is the probability of observing data z when grid s is occupied; p(s) is the prior probability that grid s is occupied; p(z) is the probability of observing data z; 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 T is set before calculating the probability value of each grid. 0 ; When the probability value result P of the grid 0 Greater than T 0 , then the grid is set to the occupied state, and the transition probability value is 1; otherwise, the probability value of the grid is kept at P 0 ; 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.

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 S201, the state vector equation is: x k =f(x k-1 ,u k-1 ,w k-1 ) In the formula, x k is the state variable of the system at time k, f is the state vector operation function, x k-1 is the state variable of the system at time k-1, u k-1 is the control input of the system at time k-1; w k-1 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: z k =h(x k ,v k ) Where h is the observation vector operation function, v k is the observation noise, representing the random error or uncertainty in the observation process; w k-1 and v k 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 variable for the last posterior state, w k-1 The error is 0, let is the estimated state variable for the current state; A is the Jacobian matrix of the partial derivative of function f with respect to x, W is the Jacobian matrix of the partial derivatives of function f with respect to w, Then we have: In the formula, Predict state variables for the current state; The observation vector equation is Linearization at, the expression is: Because v k is the error, so let v k is 0, let is the observed variable predicted for the current state; H is the Jacobian matrix of the partial derivative of function h with respect to x, V is the Jacobian matrix of the partial derivatives of function h with respect to v, Where P is the probability, T is the external parameter matrix of the depth camera, N is the normal distribution, and R is the observation radius of the depth camera.

4. The positioning and mapping method based on multi-source data according to claim 3, 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, is the prior estimation error covariance at time k, P k-1 is the probability at time k-1, A T is the Jacobian matrix of the partial derivative of function f with respect to x under the constraints of the extrinsic matrix of the depth camera, Q is the Kalman filter, and W T is the Jacobian matrix of the partial derivative of function f with respect to w under the constraints of the extrinsic matrix of the depth camera; The extended Kalman filter correction includes: (i) Calculation of Kalman gain: The 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, K k is the Kalman gain, H T V is the Jacobian matrix of the partial derivative of function h with respect to x under the constraints of the extrinsic matrix of the depth camera, T is the Jacobian matrix of the partial derivative of the function h with respect to v under the constraints of the extrinsic matrix of the depth camera; (ii) State vector update: Use the Kalman gain and observations to update the state estimate; In the formula, is the current updated state variable, K k is the Kalman gain; (iii) Covariance update: Update the error covariance of the state estimate to reflect the corrected uncertainty; Where P k is the error covariance, I is the state estimate; In step S204, for a certain grid m on the laser radar grid map m i , expressed in terms of probability, the probability of being occupied p(m i ), 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 i-th grid; After the laser radar performs a scanning measurement, the measurement value (z~{0,1}) is obtained, and the state of the updated grid is: Where s(m|z) is the state value of the updated grid, p(m i =1|z) is the probability of a grid being occupied from the occupied state to the measured value, p(m i =0|z) is the probability of a grid in the idle state being occupied by the measured value; According to the Bayesian formula: Where p(z) is the probability of the measured value being occupied after scanning measurement, p(z|m i =1) is the probability that a grid is occupied from the measured value to the occupied state, p(z|m i =0) is the probability that a certain grid is occupied when the measured value reaches the idle state; Combining the two formulas we get: Where s(m) is the state of the i-th 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. The default probability of the grid being occupied and idle in the initial state is 0.

5. S + and S - 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.

5. The positioning and mapping method based on multi-source data according to claim 4, 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 represented as P i =[X i , Y i , Z i ] T , the three-dimensional coordinates are projected onto the normalized plane, and the projection coordinates are p i =[u,v i ] T , according to the pinhole imaging principle; the relationship between the three-dimensional space coordinates and the projection coordinates is derived as follows: Where K is the intrinsic parameter matrix of the depth camera, T is the extrinsic parameter matrix of the depth camera, and s i It is the relationship value between the three-dimensional space coordinates and the projection coordinates.

6. The positioning and mapping method based on multi-source data according to claim 5, 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: Where, T * is the least squares error target value, p i is the projection coordinate, P i 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: e(x+Δx)≈e(x)+J T Δx In the formula, e is the error, x is the state variable, Δx is the change value of the state variable, e(x) is the error function under the state variable, J T is the linearized value of the state variable; According to the error objective function, the coordinates of the spatial point P in the camera coordinate system are recorded as P′=[X′, Y′, Z′] T , define P′ as the intermediate variable; Using the chain rule, we can derive the Lie group T multiplied by the disturbance δξ on the left and obtain the following formula: In the formula, is the left multiplication disturbance of Lie algebra, ξ is the disturbance value, and e(ξ) is the disturbance error; is the derivative of the error with respect to the projection point P′. According to the camera imaging principle, the Jacobian matrix is ​​derived as follows: In the formula, f x is the Jacobian matrix of the partial derivative of function f with respect to x, f y is the Jacobian matrix of the partial derivatives of function f with respect to y; In the derivative of P′ with respect to the Lie algebra, taking the first three dimensions, we get the following formula: Where P′^ is the Lie algebraic derivative of the projection point; The above and Multiplying the 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; J(ξ) T ×J(ξ)Δξ=-J(ξ) T e(ξ) Where J is the linearization process, (ξ) T is the disturbance value objective function under the Jacobian matrix constraint, J(ξ) is the linearization of the disturbance value objective function, Δξ is the change of the disturbance value, and e(ξ) is the disturbance error; Using the known Jacobian matrix and initial value of the pose for iterative optimization, when Δξ k When it is lower than the set value, the iteration stops and the posture transformation matrix is ​​obtained. Otherwise, it will be calculated according to ξ k+1 =ξ k +Δξ k Continue iterating until the requirements are met.

7. 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 6, 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: by using the main direction of the FAST corner point through the image feature extraction and description algorithm ORB, the BRIEF descriptor is rotated to make the BRIEF descriptor have rotation invariance, 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 the multi-source data.

Images