Mobile robot positioning and navigation method based on laser SLAM

Abstract

The invention discloses a mobile robot positioning and navigation method based on laser SLAM (Simultaneous Localization and Mapping). The method relates to the fields of machine learning, automatic driving, computer vision and the like. Aiming at the balance of real-time performance and calculation efficiency, the method comprises the following steps: firstly, tracking and estimating a motion track of a sensor by using laser radar data in real time, and fusing LiDAR and IMU; secondly, selecting a key frame and expressing the key frame by using a neural network implicit scene; adjusting parameters represented by a key frame pose and a neural scene through joint optimization to minimize a loss function; and finally, dynamically adjusting the loss function margin of laser radar rays by adopting a JS dynamic margin loss function, and converting an implicit neural representation three-dimensional scene into an explicit three-dimensional network model, thereby facilitating visualization, evaluation and use. Compared with other methods, the method has the advantages that the method has continuous and fine scene modeling capability, has real-time performance of positioning and mapping, and can be expanded to the fields of automatic driving, urban planning and the like.

Description

Technical Field

[0001] The present invention relates to the fields of machine learning, autonomous driving, computer vision, and in particular to a mobile robot positioning and navigation method based on laser SLAM. Background Art

[0002] Mobile robots are an advanced technology platform that integrates multiple sensors and intelligent control systems. They are widely used in the automotive, service, medical, and industrial industries. With the development of intelligent transformation and digital transformation in various industries in my country, the research and development of mobile robot technology has become a key prerequisite for promoting my country's global robotics technology innovation source, high-end manufacturing cluster, and integrated application new highland. In this context, mobile robots have become indispensable intelligent assistants with their autonomous navigation capabilities, efficient operation performance, precise control technology, strong environmental adaptability, remote monitoring and control functions, continuous learning and optimization mechanisms, and multifunctional integration characteristics. In identifying densely distributed or fine-sized obstacles and complex environments such as rain, snow, high temperature, and low temperature, how mobile robots can achieve accurate pose estimation of simultaneous localization and mapping (SLAM) of laser radar and quickly and real-time provide their own positioning and environmental information has become the core challenge of mobile robot positioning and navigation.

[0003] At present, the positioning and navigation methods of mobile robots are still not mature enough. Among the current mainstream solutions, the ultrasonic sensing system is easily affected by temperature changes and air density disturbances, and is prone to multipath interference in complex reflection scenes. Although the visual navigation solution has the advantage of environmental perception, its light sensitivity and high computational complexity make it difficult to guarantee real-time performance. Some researchers have achieved sub-meter reconstruction accuracy in structured scenes through the implicit mapping method of lidar, but the effectiveness of the algorithm depends on prior pose information or requires reducing computational efficiency. This technical compromise may lead to insufficient adaptability to dynamic environments and computing resource bottlenecks.

[0004] In response to the above problems and actual needs, the present invention proposes a mobile robot positioning and navigation method based on laser SLAM, which aims to quickly build a high-precision map of the environment through a real-time laser radar SLAM algorithm, and determine its precise position in the map in real time. The data of the laser radar (Light Detection and Ranging, LiDAR) and the inertial measurement unit (Inertial Measurement Unit, IMU) are fused. In an environment with insufficient features, the IMU can assist in positioning and significantly improve the stability and reliability of the system. At the same time, a dynamic error compensation model is constructed based on the iterative closest point (Iterative Closest Point, ICP) algorithm to achieve real-time tracking and trajectory optimization of the motion trajectory. Firstly, the real-time LiDAR SLAM algorithm is deployed on the mobile robot platform, and a dynamic mapping system based on Multi-Layer Perceptron (MLP) is constructed. The network uses the real-time collected LiDAR point cloud data for online training, and synchronously generates a high-precision environment map and the robot's motion trajectory. Secondly, the LiDAR-IMU tight coupling real-time tracking is realized through the iterative closest point algorithm, and the IMU motion prior is introduced to compensate for the positioning drift in the feature sparse scene, which significantly improves the robustness of the system. Subsequently, the key frame selection mechanism is introduced, and the scene is implicitly represented by the neural network. The volume density of each point in the prediction space is trained by MLP, so that the robot can have a deeper understanding of the environmental structure. Then, the key frame pose and neural scene representation parameters are adjusted through a joint optimization process to minimize the loss function and improve the environmental adaptability. Finally, the JS dynamic margin loss function is used to dynamically adjust the loss function margin according to the learning degree of different regions during the online training process, optimize the network training and improve the scene reconstruction accuracy, and finally convert the implicit neural representation of the three-dimensional scene into an explicit three-dimensional network model for visualization, evaluation and practical application. Summary of the invention

[0005] The present invention aims to provide a mobile robot positioning and navigation method based on laser SLAM in an outdoor environment, so that the robot can estimate its position in the environment and construct an environmental map in real time.

[0006] The specific implementation steps are as follows:

[0007] Step 1: Real-time LiDAR pose estimation and pose update optimization. First, the LiDAR sensor on the mobile robot scans the surrounding environment in real time, processes the scan data to estimate the sensor pose, and then selects a key frame for pose optimization.

[0008] Step 1.1: Pose estimation, in order to reduce computational complexity and increase processing speed, the high-frequency lidar data is downsampled to 5Hz. Then, the ICP algorithm is used to estimate the relative transformation between consecutive scans. That is, the relative transformation from the previous lidar scan to the current scan Finally, the lidar data is motion compensated based on the pose estimates from the consecutive scans, while the lidar moves at a constant speed between consecutive scans.

[0009] The preliminary pose estimation formula is:

[0010] ,

[0011] in, Indicates the current position. Indicates the posture at the previous moment.

[0012] Step 1.2: Pose update optimization. During the positioning and navigation process of the mobile robot, pose update optimization is implemented through two parallel threads: the LiDAR tracking thread and the mapping thread. The tracking thread processes real-time data, preliminarily estimates the robot's pose, and sends these real-time data to the mapping thread; the mapping thread selects key frames based on time intervals, scanning quality, etc., and feeds back the optimized pose to the tracking thread.

[0013] First, a set of keyframes are selected from the real-time data to jointly optimize the poses of the map and keyframes. For each keyframe, the estimated pose is used Generates a rotation vector in world coordinates As optimization variables, for The axis angle representation of the rotational component is, is the translation component. Secondly, in the forward propagation, the twist vector Convert to pose for calculating ray starting point.

[0014] Generate the transformation matrix:

[0015] T i = [ R u ^ i 0 1 ] ,

[0016] in, Is The resulting rotation matrix;

[0017] In back propagation, the MLP and feature grid parameters are calculated Gradient and twist vector The gradient of is used to optimize the pose.

[0018] Calculating the loss function For MLP parameters and characteristic mesh parameters The gradient is:

[0019] ,

[0020] in, is the volume density predicted by the MLP.

[0021] Calculating the loss function To twist the vector The gradient is:

[0022] ,

[0023] in, Is The resulting transformation moment,

[0024] Update the parameters using gradient descent:

[0025] ,

[0026] ,

[0027] in, is the learning rate, is the optimized feature grid parameter, is the optimized twist vector.

[0028] Step 1.3: Posture update and iteration. Convert to transformation matrix When the mapping thread completes the pose optimization of the keyframe, it needs to synchronize the optimized pose information to the tracking thread so that subsequent tracking is performed relative to the optimized pose.

[0029] Step 2: LiDAR-IMU fusion. IMU can provide continuous measurement of the angular velocity and acceleration of the mobile robot, helping the mobile robot to locate more accurately in environments where the LiDAR data is feature-deficient. LiDAR-IMU fusion is to supplement the low-frequency data of LiDAR with the high-frequency data of IMU, so as to achieve more stable pose estimation in dynamic or feature-sparse environments. First, the raw inertial data of the mobile robot is obtained from the IMU, including the measurements of the accelerometer and gyroscope. Since IMU data usually contains noise, zero bias and range errors, the data needs to be denoised, zero bias corrected and range corrected. Then, the state estimation of the implicit representation method is initialized with the IMU data to provide an initial pose and velocity estimate for the mobile robot system. At the same time, ensure that the IMU data and LiDAR data are aligned in timestamps to avoid errors caused by time asynchrony. Finally, the prediction model and observation model are established, and the predicted state of the IMU is fused with the actual observation value of the LiDAR through the extended Kalman filter (EKF).

[0030] Step 2.1: Establish prediction model and observation model. Use the acceleration and angular velocity data of IMU to predict the state of the robot at the next moment. Use the scanning data of LiDAR to provide actual environmental observation information for correcting the predicted state. Finally, by calculating the Kalman gain, combining the predicted state and the actual observation value, correct the state estimate, and effectively fuse the data of IMU and LiDAR.

[0031] Step 2.2: State vector and Kalman filter. The state vector of IMU usually includes information such as position, velocity, attitude, angular velocity, etc. The state vector is expressed as:

[0032] x i = [ p v q oh ] T ,

[0033] in, Represents the three-dimensional position of the IMU in the world coordinate system, Represents the velocity of the IMU in the world coordinate system, Represents the rotation matrix The corresponding direction quaternion is, Indicates the angular velocity of the gyroscope.

[0034] The time prediction update equation of the extended Kalman filter is:

[0035] ,

[0036] Among them, A is the state transfer matrix, B is the control matrix, is the control input, is the process noise.

[0037] The measurement update equation is:

[0038] ,

[0039] in, is the observation matrix at all times k, To measure noise.

[0040] The Kalman gain calculation formula is:

[0041] ,

[0042] in, is the state covariance matrix, is the covariance matrix of the measurement noise.

[0043] Step 3: Implicit map construction. First, the mobile robot learns and represents the implicit model of the detected environment based on MLP, and introduces hierarchical feature grid encoding to divide the space into a multi-level grid structure, and each grid unit stores a feature vector. This encoding method can significantly improve the learning efficiency of MLP, making it more effective in reasoning about the structure of the environment. Then, during the operation of the robot, the parameters of the MLP and the feature grid are updated online to predict the volume density of each point in the space. By continuously updating the parameters of the MLP and the feature grid, the robot can adapt to changes in the environment in real time. The neural network is then trained using lidar data to enable it to predict the volume density, color or other features of the location based on the input spatial position. The trained neural network is then used for volume rendering, and the expected termination depth of the lidar ray is estimated by calculating the volume density and transparency along the line of sight path and outputting weights. Finally, the current scan and the selected historical keyframe are used together to update the neural scene representation.

[0044] Step 3.1: Volume rendering is an important technique in implicit scene representation, which is used to synthesize new views from a continuous 3D space. Multiple sampling points are generated along the line of sight path of the LiDAR ray, and the 3D coordinates of each sampling point are calculated. Then the parameters are updated in the MLP of the mobile robot. and feature network to predict the volume density of each point Then, the transparency and weight of each sampling point are calculated by integration, and the weight reflects the contribution of the point to the final rendering result. Finally, the expected termination depth of the LiDAR ray is estimated by weighted averaging the distances of all sampling points.

[0045] Select distance t i ∈ [ t near ， t far ] ,calculate The three-dimensional coordinates of the sampling points:

[0046] ,

[0047] in, is the starting point of the ray, i.e. the position of the lidar, is the direction vector of the ray, is the distance chosen on this ray.

[0048] Step 3.2: Predict the volume density of each sampling point based on the parameters of the MLP and the spatial position of the sampling point, and combine the feature network and MLP into , and then calculate the weight transparency by the following formula and weight :

[0049] ,

[0050] ,

[0051] in, It is The volume density of the sampling points, is the sampling distance from this point to the next point.

[0052] The desired stopping depth of the ray The weighted average distance of all sampling points is calculated as:

[0053] .

[0054] Step 4: Keyframe optimization map. First, the mobile robot selects keyframes from continuous LiDAR scan data for training and refining the neural network, and uses the current scan and selected historical keyframes together to update the neural scene representation. Then, a keyframe window containing the current frame and past frames is selected to jointly optimize the pose of the map and keyframes. The MLP is then trained using data from the current frame and keyframes to improve the implicit representation of the scene. Finally, a loss function is applied to measure the difference between the prediction and the actual observation, and this information is used to update the weights of the network.

[0055] Step 4.1: Define the loss function. The JS dynamic margin loss function is used to optimize the implicit representation of the scene by the neural network, especially to dynamically adjust the learning degree of different areas; the depth loss function reduces the difference between the rendered depth and the actual measured depth of the LiDAR; the sky loss function ensures that the weight of the LiDAR rays pointing to the sky is zero, because these rays do not provide information about the scene structure.

[0056] The total loss function is used to comprehensively evaluate the performance of the model and is expressed as:

[0057] ,

[0058] in, represents the parameters of the neural network, is the JS dynamic margin loss function, is the depth loss function, is the sky loss function, and It is a hyperparameter that adjusts the weight of each loss term.

[0059] ,

[0060] ,

[0061] ,

[0062] in, The main loss is is the target weight, is the parameter weight, is the opacity loss, To measure depth, is the weight vector for the ray pointing toward the sky.

[0063] Step 4.2: Dynamic margin loss processing. First, JS divergence is used to measure the similarity between the target distribution and the predicted sample distribution to determine the dynamic margin of each lidar ray. Then, a dynamic margin is set for each lidar ray according to the known degree of the map. For areas that have not been fully learned, a larger margin is used; for areas that have been learned, a smaller margin is used. Finally, by adjusting the margin, the adaptability of the network to new data is optimized, while retaining the learned knowledge to avoid forgetting old data due to the addition of new data.

[0064] Dynamic margin refers to the margin parameter that is dynamically adjusted according to the learning status of each LiDAR ray in the loss function, expressed as:

[0065] ,

[0066] ,

[0067] in, is the dynamic margin, is the minimum margin, is the JS divergence, is a constant scaling parameter, express The upper threshold of express The lower threshold of Score less than hour, equal .

[0068] Step 5: Convert the implicit neural representation of the 3D scene into an explicit 3D network model for easy visualization, evaluation, and use. First, a virtual lidar is placed at the estimated keyframe pose to sample data from the implicit map, and weights are calculated along the virtual lidar ray, which reflects the density prediction of the neural network for each sample point. The calculated weights are then aggregated into a 3D mesh and the surface mesh is extracted from the voxel data. The generated network is then optimized to remove redundant vertices and faces and simplify complex areas. Finally, the generated mesh can be used for visualization and evaluation.

[0069] This method designs a mobile robot positioning and navigation method based on laser SLAM, which is used to solve the problem of real-time estimation of dense maps and sensor trajectories of mobile robots in outdoor environments. Compared with the prior art, the technical solution of the present invention has the following beneficial effects:

[0070] 1. The present invention provides a continuous three-dimensional spatial representation through neural implicit scene representation, which can capture the details and complexity of the scene more finely than traditional discrete maps. It can also generalize and fill in blank areas in the observed data, which helps to reconstruct the scene in the case of sparse or incomplete data. The neural implicit scene continuously updates the representation of the scene through online learning, enhancing the adaptability and scalability of dynamic environments.

[0071] 2. Based on the JS dynamic margin loss function, the margin of the loss function can be dynamically adjusted according to the learning situation of each lidar ray, so that the network can learn new data faster while retaining the learned knowledge, improving the pose estimation and thus improving the trajectory accuracy of the entire SLAM system, reducing the forgetting phenomenon, and improving the robustness and generalization of the system in different environments and conditions.

[0072] 3. Mobile robot positioning and mapping based on LiDAR and IMU fusion, mainly through machine learning and a novel loss function to estimate the map and sensor trajectory in real time. This method can build maps in complex and dense environments, so it is not only suitable for mobile robots, but can also be extended to other fields and scenarios, such as self-driving cars, large bulk equipment, urban planning and management, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is an overall flow chart of an example of the present invention. DETAILED DESCRIPTION

[0074] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0075] See attached Figure 1 , this embodiment provides a mobile robot positioning and navigation method based on laser SLAM. Taking the outdoor environment as an example, it specifically includes the following processes: First, the laser radar data is used to track in real time through the point-to-plane iterative closest point algorithm to estimate the motion trajectory of the sensor; at the same time, LiDAR-IMU fusion positioning helps LiDAR data to locate in an environment without sufficient features. Secondly, key frames are selected and a neural network is used to implicitly represent the outdoor scene, and a multi-layer perceptron is trained to predict the volume density of each point in space; then, the posture of the key frame and the parameters of the neural scene representation are adjusted through a joint optimization process to minimize the loss function; finally, the JS dynamic margin loss function is used to dynamically adjust the loss function margin of each laser radar ray according to the degree of learning in different areas during the online training process to optimize the network training and scene reconstruction, thereby completing the real-time SLAM task and converting the implicit neural representation of the three-dimensional scene into an explicit three-dimensional network model, which is convenient for visualization, evaluation and use.

[0076] The specific implementation instructions are as follows:

[0077] Implementation step 1: Real-time LiDAR pose estimation and pose update optimization. First, the LiDAR sensor on the mobile robot scans the surrounding environment in real time, processes the scan data to estimate the sensor pose, and uses this data to estimate the sensor pose, and then selects a key frame for pose optimization.

[0078] Step 1.1: Pose estimation, in order to reduce computational complexity and increase processing speed, the high-frequency lidar data is downsampled to 5Hz. Then, the ICP algorithm is used to estimate the relative transformation between consecutive scans. That is, the relative transformation from the previous lidar scan to the current scan Finally, the lidar data is motion compensated based on the pose estimates from the consecutive scans, while the lidar moves at a constant speed between consecutive scans.

[0079] The initial pose estimation formula is:

[0080] ,

[0081] in, Indicates the current position. Indicates the posture at the previous moment.

[0082] Step 1.2: Pose update optimization. During the positioning and navigation process of the mobile robot, pose update optimization is implemented through two parallel threads: the LiDAR tracking thread and the mapping thread. The tracking thread processes real-time data, preliminarily estimates the robot's pose, and sends these real-time data to the mapping thread; the mapping thread selects key frames based on time intervals, scanning quality, etc., and feeds back the optimized pose to the tracking thread.

[0083] First, a set of keyframes are selected from the real-time data to jointly optimize the poses of the map and keyframes. For each keyframe, the estimated pose is used Generates a rotation vector in world coordinates As optimization variables, for The axis angle representation of the rotational component is, is the translation component. Secondly, in the forward propagation, the twist vector Convert to pose for calculating ray starting point.

[0084] Generate the transformation matrix:

[0085] T i = [ R u ^ i 0 1 ] ,

[0086] in, Is The resulting rotation matrix;

[0087] In back propagation, the MLP and feature grid parameters are calculated Gradient and twist vector The gradient of is used to optimize the pose.

[0088] Calculating the loss function For MLP parameters and characteristic mesh parameters The gradient is:

[0089] ,

[0090] in, is the volume density predicted by the MLP.

[0091] Calculating the loss function To twist the vector The gradient is:

[0092] ,

[0093] in, Is The resulting transformation matrix is

[0094] Update the parameters using gradient descent:

[0095] ,

[0096] ,

[0097] in, is the learning rate, is the optimized feature grid parameter, is the optimized twist vector.

[0098] Step 1.3: Posture update and iteration. Convert to transformation matrix When the mapping thread completes the pose optimization of the keyframe, it needs to synchronize the optimized pose information to the tracking thread so that subsequent tracking is performed relative to the optimized pose.

[0099] Implementation step 2: LiDAR-IMU fusion. IMU can provide continuous measurement of the angular velocity and acceleration of the mobile robot, helping the mobile robot to locate more accurately in environments where the LiDAR data is feature-deficient. LiDAR-IMU fusion is to supplement the low-frequency data of LiDAR with the high-frequency data of IMU, so as to achieve more stable pose estimation in dynamic or feature-sparse environments. First, the raw inertial data of the mobile robot is obtained from the IMU, including the measurements of the accelerometer and gyroscope. Since IMU data usually contains noise, zero bias and range errors, the data needs to be denoised, zero bias corrected and range corrected. Then, the state estimation of the implicit representation method is initialized with the IMU data to provide an initial pose and velocity estimate for the mobile robot system. At the same time, ensure that the IMU data is aligned with the LiDAR data in terms of timestamps to avoid errors caused by time asynchrony. Finally, the prediction model and observation model are established, and the predicted state of the IMU is fused with the actual observation value of the LiDAR through the extended Kalman filter (EKF).

[0100] Step 2.1: Establish prediction model and observation model. Use the acceleration and angular velocity data of IMU to predict the state of the robot at the next moment. Use the scanning data of LiDAR to provide actual environmental observation information for correcting the predicted state. Finally, by calculating the Kalman gain, combining the predicted state and the actual observation value, correct the state estimate, and effectively fuse the data of IMU and LiDAR.

[0101] Step 2.2: State vector and Kalman filter. The state vector of IMU usually includes information such as position, velocity, attitude, angular velocity, etc. The state vector is expressed as:

[0102] x i = [ p v q oh ] T ,

[0103] in, Represents the three-dimensional position of the IMU in the world coordinate system, Represents the velocity of the IMU in the world coordinate system, Represents the rotation matrix The corresponding direction quaternion is, Indicates the angular velocity of the gyroscope.

[0104] The time prediction update equation of the extended Kalman filter is:

[0105] ,

[0106] Among them, A is the state transfer matrix, B is the control matrix, is the control input, is the process noise.

[0107] The measurement update equation is:

[0108] ,

[0109] in, is the observation matrix at all times k, To measure noise.

[0110] The Kalman gain calculation formula is:

[0111] ,

[0112] in, is the state covariance matrix, is the covariance matrix of the measurement noise.

[0113] Implementation step 3: Implicit map construction. First, the mobile robot learns and represents an implicit model of the detected environment based on MLP, and introduces hierarchical feature grid encoding to divide the space into a multi-level grid structure, where each grid unit stores a feature vector. This encoding method can significantly improve the learning efficiency of MLP, making it more effective in reasoning about the structure of the environment. Then, during the operation of the robot, the parameters of the MLP and feature grid are updated online to predict the volume density of each point in the space. By continuously updating the parameters of the MLP and feature grid, the robot can adapt to changes in the environment in real time. The neural network is then trained using lidar data to enable it to predict the volume density, color, or other features of the location based on the input spatial position. The trained neural network is then used for volume rendering, and the expected termination depth of the lidar ray is estimated by calculating the volume density and transparency along the line of sight path and outputting weights. Finally, the current scan and the selected historical keyframe are used together to update the neural scene representation.

[0114] Step 3.1: Volume rendering is an important technique in implicit scene representation, which is used to synthesize new views from a continuous 3D space. Multiple sampling points are generated along the line of sight path of the LiDAR ray, and the 3D coordinates of each sampling point are calculated. Then the parameters are updated in the MLP of the mobile robot. and feature network to predict the volume density of each point Then, the transparency and weight of each sampling point are calculated by integration, and the weight reflects the contribution of the point to the final rendering result. Finally, the expected termination depth of the LiDAR ray is estimated by weighted averaging the distances of all sampling points.

[0115] Select distance t i ∈ [ t near ， t far ] ,calculate The three-dimensional coordinates of the sampling points:

[0116] ,

[0117] in, is the starting point of the ray, i.e. the position of the lidar, is the direction vector of the ray, is the distance chosen on this ray.

[0118] Step 3.2: Predict the volume density of each sampling point based on the parameters of the MLP and the spatial position of the sampling point, and combine the feature network and MLP into , and then calculate the weight transparency by the following formula and weight :

[0119] ,

[0120] ,

[0121] in, It is The volume density of the sampling points, is the sampling distance from this point to the next point.

[0122] The desired stopping depth of the ray The weighted average distance of all sampling points is calculated as:

[0123] .

[0124] Implementation step 4: keyframe optimized mapping. First, the mobile robot selects keyframes from continuous LiDAR scan data for training and refining the neural network, and the current scan and selected historical keyframes are used together to update the neural scene representation. A keyframe window containing the current frame and past frames is selected to jointly optimize the pose of the map and keyframes. The MLP is then trained using data from the current frame and keyframes to improve the implicit representation of the scene. Finally, a loss function is applied to measure the difference between the prediction and the actual observation, and this information is used to update the weights of the network.

[0125] Step 4.1: Define the loss function. The JS dynamic margin loss function is used to optimize the implicit representation of the scene by the neural network, especially to dynamically adjust the learning degree of different areas; the depth loss function reduces the difference between the rendered depth and the actual measured depth of the LiDAR; the sky loss function ensures that the weight of the LiDAR rays pointing to the sky is zero, because these rays do not provide information about the scene structure.

[0126] The total loss function is used to comprehensively evaluate the performance of the model and is expressed as:

[0127] ,

[0128] in, represents the parameters of the neural network, is the JS dynamic margin loss function, is the depth loss function, is the sky loss function, and It is a hyperparameter that adjusts the weight of each loss term.

[0129] ,

[0130] ,

[0131] ,

[0132] in, The main loss is is the target weight, is the parameter weight, is the opacity loss, To measure depth, is the weight vector for the ray pointing toward the sky.

[0133] Step 4.2: Dynamic margin loss processing. First, use JS divergence to measure the similarity between the target distribution and the predicted sample distribution, and determine the dynamic margin of each lidar ray. Then set the dynamic margin for each lidar ray according to the degree of knowledge of the map. For areas that have not been fully learned, use a larger margin; for areas that have been learned, use a smaller margin. Finally, by adjusting the margin, the adaptability of the network to new data is optimized, while retaining the learned knowledge to avoid forgetting old data due to the addition of new data.

[0134] Dynamic margin refers to the margin parameter that is dynamically adjusted in the loss function according to the learning situation of each LiDAR ray, expressed as:

[0135] ,

[0136] ,

[0137] in, is the dynamic margin, is the minimum margin, is the JS divergence, is a constant scaling parameter, express The upper threshold of express The lower threshold of Score less than hour, equal .

[0138] Implementation step 5: Convert the implicit neural representation of the 3D scene into an explicit 3D network model. First, a virtual lidar is placed at the estimated keyframe pose to sample data from the implicit map, and weights are calculated along the virtual lidar ray, which reflects the density prediction of the neural network for each sample point. The calculated weights are then aggregated into a 3D mesh and the surface mesh is extracted from the voxel data. Then, the generated network is optimized to remove redundant vertices and faces and simplify complex areas. Finally, the generated mesh can be used for visualization and evaluation.

Claims

1. A mobile robot positioning and navigation method based on laser SLAM, characterized in that: The following steps are involved: Step 1: The laser radar sensor carried by the mobile robot scans the surrounding environment in real time, processes the scan data to estimate the sensor pose, and then selects a key frame for pose optimization; Step 2: Obtain the original inertial data of the mobile robot from the IMU, perform denoising, zero bias correction and range correction on the data, use the IMU data to initialize the state estimation of the implicit representation method, and fuse the predicted state of the IMU with the actual observation value of the LiDAR through the EKF; Step 3: Introduce a keyframe selection mechanism, use a neural network to implicitly represent the scene, and train the MLP to predict the volume density of each point in the space; Step 4: Adjust the keyframe poses and neural scene representation parameters through a joint optimization process, apply a loss function to measure the difference between the prediction and the actual observation, and update the network weights; Step 5: Convert the implicit neural representation of the three-dimensional scene into an explicit three-dimensional network model.

2. The mobile robot positioning and navigation method based on laser SLAM according to claim 1 is characterized in that: The laser radar sensor carried by the mobile robot in step 1 scans the surrounding environment in real time, processes the scan data to estimate the sensor posture, and then selects a key frame for posture optimization, which is specifically implemented in the following steps: The high-frequency lidar data is downsampled to 5 Hz and the ICP algorithm is used to estimate the relative transformation between consecutive scans, that is, the relative transformation from the previous lidar scan to the current scan. ; The lidar moves at a constant speed between consecutive scans, and motion compensation is performed on the lidar data based on the pose estimation of the consecutive scans; The initial pose estimation formula is: ， in, Indicates the current position. Indicates the posture at the last moment; The pose update optimization is implemented through two parallel threads: LiDAR tracking thread and mapping thread. The tracking thread processes real-time data, preliminarily estimates the robot pose, and sends these real-time data to the mapping thread; the mapping thread selects key frames based on the time interval and scanning quality, and feeds the optimized pose back to the tracking thread. First, a set of keyframes are selected from the lidar data to jointly optimize the pose of the map and the keyframes; for each keyframe, the estimated pose is used Generates a rotation vector in world coordinates As optimization variables, for The axis angle representation of the rotational component is, is the translation component; secondly, in the forward propagation, the twist vector Convert to pose for calculating the starting point of the ray; Generate the transformation matrix: ， in, Is The resulting rotation matrix; In back propagation, the MLP and feature grid parameters are calculated Gradient and twist vector Gradient, used to optimize the pose; Calculating the loss function For MLP parameters and characteristic mesh parameters The gradient is: ， in, is the volume density predicted by MLP; Calculating the loss function To twist the vector The gradient is: ， in, Is The resulting transformation matrix is Update the parameters using gradient descent: ， ， in, is the learning rate, is the optimized feature grid parameter, is the optimized torsion vector; The optimized torsion vector Convert to transformation matrix ; When the mapping thread completes the pose optimization of the key frame, it needs to synchronize the optimized pose information to the tracking thread.

3. The mobile robot positioning and navigation method based on laser SLAM according to claim 1 is characterized in that: In step 2, the original inertial data of the mobile robot is obtained from the IMU, and the data is denoised, biased and range corrected. The state estimation of the implicit representation method is initialized using the IMU data, and the predicted state of the IMU is fused with the actual observation value of the LiDAR through the EKF. The specific implementation is as follows: Obtain the raw inertial data of the mobile robot from the IMU, perform denoising, zero bias correction and range correction on the data, use the IMU data to initialize the state estimation of the implicit representation method, ensure that the IMU data and LiDAR data are aligned in timestamp, establish the prediction model and observation model, and fuse the IMU prediction state with the actual observation value of LiDAR through EKF; The state vector of the IMU usually includes position, velocity, attitude, and angular velocity, which can be expressed as: ， in, Represents the three-dimensional position of the IMU in the world coordinate system, Represents the velocity of the IMU in the world coordinate system, Represents the rotation matrix The corresponding direction quaternion is, represents the angular velocity of the gyroscope; The time prediction update equation of the extended Kalman filter is: ， Among them, A is the state transfer matrix, B is the control matrix, is the control input, is the process noise; The measurement update equation is: ， in, is the observation matrix at all times k, To measure noise; The Kalman gain calculation formula is: ， in, is the state covariance matrix, is the covariance matrix of the measurement noise.

4. The mobile robot positioning and navigation method based on laser SLAM according to claim 1 is characterized in that: The key frame selection mechanism described in step 3 is introduced, a neural network is used to implicitly represent the scene, and the MLP is trained to predict the volume density of each point in the space. The specific implementation is as follows: Firstly, an implicit model of the environment is constructed based on MLP, and hierarchical feature grid coding is introduced to divide the space into a multi-level grid structure. During the operation of the robot, the lidar data is received in real time, and the weight parameters of MLP and the feature vectors in the feature grid are updated online through the back propagation algorithm to dynamically optimize the implicit representation of the scene. The MLP takes the parameters optimized at the previous moment as the initial value, and outputs the volume density, color attributes and transparency of the point according to the input three-dimensional space coordinate point. During the online training process, by updating the parameters of the MLP and feature network to predict the volume density of each point ; Second, choose the distance ,calculate The three-dimensional coordinates of the sampling points: ， in, is the starting point of the ray, i.e. the position of the lidar, is the direction vector of the ray, is the distance chosen on that ray; The volume density of each sampling point is predicted based on the parameters of MLP and the spatial position of the sampling point. The feature network and MLP are combined into , weight transparency is calculated by the following formula and weight : ， ， in, It is The volume density of the sampling points, is the sampling distance from this point to the next point; The desired stopping depth of the ray The weighted average distance of all sampling points is calculated as: 。 5. The mobile robot positioning and navigation method based on laser SLAM according to claim 1 is characterized in that: The joint optimization process described in step 4 adjusts the keyframe poses and neural scene representation parameters, applies a loss function to measure the difference between the prediction and the actual observation, and updates the network weights. The specific implementation is as follows: First, keyframes are selected from continuous LiDAR scan data for training the neural network, and the current scan and selected historical keyframes are used together to update the neural scene representation; a keyframe window containing the current frame and past frames is selected to jointly optimize the pose of the map and keyframes; the MLP is then trained using data from the current frame and keyframes to improve the implicit representation of the scene; finally, a loss function is applied to measure the difference between the prediction and the actual observation, and this information is used to update the weights of the network; The JS dynamic margin loss function is used to optimize the implicit representation of the scene by the neural network, and dynamically adjusts the degree of learning in different areas; the depth loss function reduces the difference between the rendered depth and the actual measured depth of the LiDAR; the sky loss function ensures that the weight of the LiDAR ray pointing to the sky is zero; The total loss function is used to comprehensively evaluate the performance of the model and is expressed as: ， in, represents the parameters of the neural network, is the JS dynamic margin loss function, is the depth loss function, is the sky loss function, and It is a hyperparameter that adjusts the weight of each loss item; ， ， ， in, The main loss is is the target weight, is the parameter weight, is the opacity loss, To measure depth, is the weight vector of the ray pointing to the sky; Use JS divergence to measure the similarity between the target distribution and the predicted sample distribution, and determine the dynamic margin of each lidar ray; use a larger margin for areas that have not been fully learned; use a smaller margin for areas that have been learned; finally, by adjusting the margin, optimize the network's adaptability to new data while retaining the learned knowledge to avoid forgetting old data due to the addition of new data; The dynamic margin is expressed as: ， ， in, is the dynamic margin, is the minimum margin, is the JS divergence, is a constant scaling parameter, express The upper threshold of express The lower threshold of Score less than hour, equal .

Images