A method for simultaneous localization and mapping of an underground complex environment

Abstract

This invention belongs to the field of underground autonomous exploration technology and discloses a synchronous localization and mapping method for complex underground environments. This method aims to explore complex underground environments without satellite signals, making breakthroughs in key areas such as hardware system design, data synchronization between sensors, kinematic modeling of quadruped robots, and multi-sensor fusion technology. This results in a precise and robust SLAM system, thereby promoting the development of autonomous exploration in the field of underground mobile robots. The underground SLAM method based on a quadruped robot dog features reliable performance, strong obstacle-crossing ability, good real-time performance, and strong robustness. Besides applications in underground resource exploration, natural disaster relief, and digital management of mine environments, it can also meet the application needs of underground spaces in large cities, large caverns, and subway spaces. This invention will fill a gap in the development of my country's underground autonomous exploration field and has significant economic and social benefits in both civilian and military applications.

Description

Technical Field

[0001] This invention relates to the field of underground autonomous exploration technology, and in particular to a method for synchronous positioning and mapping in complex underground environments. Background Technology

[0002] From tunnels and urban underground environments to complex cave networks, the demand for autonomous, unmanned exploration of various underground environments is constantly growing. However, the underground environment, as a typical example of a satellite-rejected complex environment, greatly limits human perception capabilities. Understanding underground space is of great significance for military operations, disaster relief, and infrastructure monitoring, thus necessitating the development of unmanned detection systems for underground environments.

[0003] Legged robots, due to their unique locomotion structure, are widely used in various fields, such as military reconnaissance, inspection and patrol, and resource exploration. Especially in complex environments (such as steps, ditches, gravel, and steep tunnels), quadruped robots can perform tasks quickly and flexibly, while traditional wheeled robots struggle to adapt to such environments and may even lack mobility. Overall, compared to other types of land-based mobile robots, quadruped robots have significant advantages in mobility, balance, and environmental adaptability, and have broad application prospects.

[0004] When quadruped robots perform tasks in complex environments, the first step is to acquire data through sensors to perceive and understand the changing state of the surrounding environment and their own position within it. Only then can they perform subsequent tasks such as autonomous obstacle avoidance and path planning. Real-time localization and mapping (SLAM) is a crucial technology for autonomous mobile robots to perform tasks in unknown environments, laying a solid foundation for subsequent path planning. Extensive research has been conducted by scholars both domestically and internationally on SLAM technology based on quadruped robot dogs. In the article “Ma, Jeremy, Max Bajracharya, Sara Susca, Larry Matthies, and Matt Malchano. Real-time poseestimation of a dynamic quadruped in GPS-denied environments for 24-hour operation.” The International Journal of Robotics Research 35, no.6 (2016): 631-653,” Jeremy Ma et al. proposed a multi-sensor fusion scheme based on the LS3 quadruped robot, incorporating a stereo camera, inertial measurement unit, leg odometry, and Global Positioning System (GPS). This scheme uses an extended Kalman filter (EKF) for estimation and achieved good results in outdoor environments. In the paper "Nubert, Julian, Shehryar Khattak, and MarcoHutter. Self-supervised learning of lidar odometry for robotic applications." In the 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 9601-9607. IEEE, 2021," Julian Nubert et al. proposed a self-supervised radar odometry and applied it to the ANYmal quadruped robot platform, achieving reliable localization in degraded scenarios. However, they did not consider the robustness of the algorithm under severe robot operation conditions.In “Wisth, David, Marco Camurri, Sandipan Das, and Maurice Fallon. Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometry.” IEEE Robotics and Automation Letters 6, no.2 (2021): 1004-1011,” David Wisth et al. proposed a tightly coupled laser vision-inertial odometry that uses a factor graph to jointly optimize vision, lidar, and inertial information, thus overcoming the limitations of a single sensor and achieving accurate and robust estimation even in scenes without texture or lacking geometric structure. In “Wisth, David, Marco Camurri, and Maurice Fallon. VILENS: Visual, inertial, lidar, and leg odometry for all-terrain-legged robots.” IEEE Transactions on Robotics 39, no.1 (2022): 309-326,” David Wish et al., building on previous research, added leg odometry to the factor graph, forming the visual-inertial radar legged navigation system VILENS, further improving the robustness of the algorithm. Currently, most quadruped robot-based SLAM methods focus more on handling general scenarios or are only applicable to a specific robot, lacking universal applicability. In underground environments with challenges such as no GPS signal, slippery and uneven ground, limited sensor field of view, severe shaking of the mobile platform, and severe feature degradation, pose estimation accuracy decreases, or even system failure occurs. Deploying an accurate and robust SLAM system in underground environments remains very difficult. The fusion of asynchronous data from multiple sensors, the design of features that can be reliably detected and matched in a wide range of scenarios, and the construction of a reasonable optimization framework are key to determining the performance of the SLAM system.

[0005] Simultaneous localization and mapping (SLAM) in large-scale, unknown, and complex underground environments is a challenging problem. Most SLAM systems often suffer from performance limitations when deployed in perceptually degraded underground environments. For example, sensors must operate under non-nominal conditions; uneven and slippery terrain makes foot odometry inaccurate, while underground tunnels or mines without significant features can easily cause laser odometry perception blur and drift; finally, spurious loops often occur in recurring environments, which can lead to severe distortion of the entire map. Summary of the Invention

[0006] To address the aforementioned problems, this invention aims at simultaneous localization and mapping in complex underground environments without satellite signals. It makes breakthroughs in key areas such as hardware system design, data synchronization between sensors, kinematic modeling of quadruped robots, and multi-sensor fusion technology, forming a precise and robust SLAM system that promotes the development of autonomous exploration in underground mobile robots.

[0007] The technical solution of the present invention is as follows: a synchronous positioning and mapping method for complex underground environments, which constructs a laser inertial odometry front-end based on an iterative error state Kalman filter (IESKF) and a back-end based on pose graph optimization. The two are combined for closed-loop detection to construct a globally consistent map.

[0008] The laser inertial odometry front end based on iterative error state Kalman filtering includes a lidar and an IMU;

[0009] After the extrinsic parameters between the lidar and the IMU are calibrated, the lidar data and the IMU data are synchronized. After synchronization, the IMU is initialized at rest to obtain the initial zero bias and initial gravity direction of the IMU, which are used to run the iterative error state Kalman filter.

[0010] The lidar and the quadruped robot dog move together, causing motion distortion in a normally acquired frame of point cloud. The state prediction part of the iterative error state Kalman filter is used to calculate the IMU pose of each laser frame. Linear interpolation is performed on each laser point in the current laser frame to obtain the pose relative to the start time of each laser frame. Finally, each laser point in a frame is compensated to the end time of each laser frame to correct the motion distortion.

[0011] The iterative error state Kalman filter predicts the pose data of the laser in the current frame based on the pose data after laser motion distortion correction in the previous frame. Then, the predicted pose data is used as the initial value for registration from the current laser frame to the local map. The registration method is incremental normal distribution transformation. After registration, the calculated residual is used as the observation data and put into the state update part of IESKF for iterative update until convergence, which is used for pose map optimization.

[0012] When the difference in pose increment between the current laser frame pose and the previous laser keyframe pose exceeds a fixed threshold, the current laser frame is set as a keyframe. At the same time, the laser inertial odometry factor corresponding to the keyframe is added to the pose graph created by the backend based on the pose graph optimization, and the registered lidar point cloud is updated to each voxel of the incremental normal distribution transformation for the next registration.

[0013] The correction and conversion formula for the motion distortion is:

[0014] L ′ =T LIT(t s ) IW T(t) WI T IL L t

[0015] The extrinsic parameter between the IMU and the lidar is T. IL , T(t s ) WI The end time of the IMU scan. T(t) WI L represents the IMU pose at time t. t = (x, y, z) T , where is the position of a single scanning point of the lidar, t∈(0, t... s );L ′ For each laser point after distortion correction.

[0016] In the laser inertial odometry, for the state update part of the IESKF, the IESKF uses the incremental normal distribution transformation registration residual as the observation equation, and uses the observation equation as input to iteratively update the Kalman gain K in the Kalman filter. k Error state δx of x k Covariance P k+1 This updates the high-dimensional state variables of the iterative error state Kalman filter;

[0017] The IMU provides constraints on the motion process, and the lidar point cloud provides constraints on the observation equations; the high-dimensional state variables of the iterative error state Kalman filter are uniformly denoted as x, which is defined in a high-dimensional manifold space M:

[0018] x = [p, v, R, b] g b a , g] T , x∈M

[0019] p is displacement, v is velocity, R is rotation, and b is rotation. g b a The bias is zero, and g is gravity;

[0020] The variable x is obtained by summing the nominal state and the error state, where the error state of x is defined in the tangent space of the high-dimensional manifold M:

[0021] The IMU measurement noise model is:

[0022]

[0023]

[0024] R represents the measurement from the IMU accelerometer. T Let a be the rotation matrix from the world coordinate system to the IMU coordinate system, where a is the actual acceleration value of the IMU, and b is the rotation matrix from the world coordinate system to the IMU coordinate system. a For the zero bias of the IMU accelerometer, η a Measurement noise of the IMU accelerometer; Here, w represents the measured value from the IMU gyroscope, and b represents the actual gyroscope value from the IMU gyroscope. g For the IMU gyroscope to have zero bias, η g Measurement noise of the IMU gyroscope;

[0025] Iterative error state Kalman filtering recursively calculates the IMU measurement value and the current state variable; let the IMU measurement value between t and t+Δt be... The discrete-time recursive process is as follows:

[0026]

[0027]

[0028]

[0029] b g (t+Δt)=b g (t)

[0030] b a (t+Δt)=b a (t)

[0031] g(t+Δt)=g(t)

[0032] During IMU prediction, the nominal state of the high-dimensional variable x is updated according to the above formula. Since the mean of the error state is zero, only its variance is updated:

[0033] P pred =FPF T +Q

[0034] Where Q is the measurement noise matrix, P is the state covariance of the previous time step, and F is given by the linear form of the error kinematics:

[0035]

[0036] The Kalman gain in the iterative error state Kalman filter adopts... Calculate; where K k Let Kalman gain be the value of the k-th iteration. Find the inverse of the covariance matrix at the k-th iteration, H k Let V be the Jacobian matrix of the laser point cloud observation equation with respect to the variables at the k-th iteration, and let V be the noise of the laser point cloud observation equation.

[0037] The incremental normal transformation registration residual is used as the observation equation. For incremental normal transformation registration of a set of laser point clouds, the laser point cloud has a total of N points. When considering the registration of the j-th point, the j-th point falls within a voxel in the target point cloud being registered, and an incremental normal transformation-related residual r is generated. j :

[0038] r j =Rq j +pu j

[0039] Where R, p are the external transformation parameters from the registered point cloud to the registered point cloud, u j Let q be the mean value of the voxels falling at the j-th point. j For the j-th point in the registered point cloud, r j The Jacobian matrix of p relative to R is:

[0040]

[0041] According to the definition order of the state variables in the iterative error state Kalman filter, let J be... j for:

[0042]

[0043] At this point, the iteration error state of H in the Kalman filter... k The j-th row of the array is J j The noise matrix V is derived from the residual information matrix, which represents the normal distribution parameters of the grid. The diagonal pieces formed;

[0044]

[0045] Therefore, in the Kalman gain formula... Write it in summation form:

[0046]

[0047]

[0048] Multiply by zh(x) k )for:

[0049]

[0050] Therefore δx k for:

[0051]

[0052] Update P regarding covariance k+1 for:

[0053]

[0054] In the backend based on pose graph optimization, foot odometry is constructed by analyzing the motion model of the quadruped robot dog; foot odometry factor, laser odometry factor and loop closure detection factor are added to the pose graph, and the pose graph is optimized based on the three factors. The optimized pose graph result is input to the map update module to update the constructed map.

[0055] The foot odometer is constructed as follows: a coordinate system fixed to the body is used as coordinate system {0}, a coordinate system that follows hip rotation is used as coordinate system {1}, a coordinate system that follows thigh rotation is used as coordinate system {2}, and a coordinate system that follows lower leg rotation is used as coordinate system {3}.

[0056]

[0057]

[0058]

[0059] In the above formula, T 01 T is the homogeneous transformation matrix of coordinate system {1} relative to coordinate system {0}; 12 T is the homogeneous transformation matrix of coordinate system {2} relative to coordinate system {1}; 23 Let {3} be the homogeneous transformation matrix of coordinate system {2} relative to coordinate system {3}.

[0060]

[0061] In the above formula, l abad l is the offset on the y-axis from the thigh joint to the lower leg joint. hip Let q be the offset along the z-axis; let point q be the foot of the robot, and the homogeneous coordinates of point q in coordinate system {3} be:

[0062]

[0063] In the above formula, l knee Let P0 be the offset of the foot relative to coordinate system {3} on the z-axis; the coordinate P0 of the foot in coordinate system {0} is:

[0064]

[0065]

[0066] Simplifying the above equation, we get P0 = f(φ) ∈ R3, where φ represents a vector containing all joint angles, f(·) represents the forward kinematic function, and P0 represents the position of the foot tip in the fuselage center-of-mass coordinate system. Differentiating both sides with respect to time, we get:

[0067]

[0068] In the formula, v p Let J(φ) represent the linear velocity of the foot tip in the coordinate system of the quadruped robot dog's center of mass, and J(φ) be the Jacobian matrix obtained by differentiating the forward kinematics. The joint angular velocity; the transformation parameter from the quadruped robot dog's center-of-mass coordinate system to the world coordinate system is R. ′ If P′, then the position P of the end of the quadruped robot dog's paws in the world coordinate system. w for:

[0069] P w =P ′ +R ′ P0

[0070] Differentiating both sides with respect to time yields the linear velocity v of the foot in the world coordinate system. w :

[0071]

[0072] in Let v represent the velocity of the quadruped robot dog in the world coordinate system, then v is...

[0073]

[0074] Integrating the above equation yields the displacement of the quadruped robot dog in the world coordinate system.

[0075] The loop closure detection is based on multi-resolution incremental normal distribution transformation. The overall loop closure detection process consists of three steps: candidate selection, candidate matching score calculation, and loop closure factor construction. Based on the laser point cloud of the current frame after motion distortion correction, laser candidate keyframes within a set radius are searched in the map update module. Incremental normal distribution transformation is used for registration and matching score calculation. When the score exceeds a fixed threshold, the loop closure is established. Loop closure detection factors are constructed for the current frame and candidate keyframes, and the loop closure detection factors are added to the pose factor map.

[0076] The beneficial effects of this invention are as follows: The method of this invention can effectively solve problems such as missing underground environmental features, complex structures, difficulty in loop closure detection, and limited scenarios. It can achieve rapid synchronous positioning and mapping in complex underground conditions without GPS signals, forming a precise and robust SLAM system for underground use. This lays a solid foundation for underground resource exploration, autonomous exploration, natural disaster relief, and digital management of mine environments. It greatly reduces the consumption of human and material resources in both military and civilian fields, and ultimately promotes the further development of underground SLAM technology in my country. Attached Figure Description

[0077] Figure 1 Design diagrams for the hardware platform system;

[0078] Figure 2 A flowchart of a synchronous positioning and mapping method for complex underground environments;

[0079] Figure 3 This is a schematic diagram illustrating the synchronization of IMU data and laser data.

[0080] Figure 4 This is a schematic diagram of the pose graph structure;

[0081] Figure 5 Diagram showing the connection relationships of the coordinate system for each leg of the quadruped robot dog;

[0082] Figure 6 A simplified diagram of a robot with one leg. Detailed Implementation

[0083] This invention addresses the challenges of unstructured underground environments, characterized by uneven surfaces, irregular spatial shapes, dim lighting, and limited performance under conditions of single sensor degradation. It establishes a hardware platform design system, such as… Figure 1 As shown; the LiDAR and IMU data are transmitted together to the Phantom Canyon NUC, and the Phantom Canyon NUC transmits information to the switch via a network bridge; the switch transmits the transmitted information to the motion control board, thereby operating and controlling the quadruped robot dog's actuators.

[0084] This invention designs a robust and accurate method for synchronous positioning and mapping in complex underground environments, such as... Figure 2As shown, to address the severe degradation and sparse features in the underground environment, incremental NDT (Neural Time Determination) is used for registration at the front end to preserve more point cloud data. A tightly coupled LIO (Low-Input IoV) system based on Iterative Error State Kalman Filtering (IESKF) is constructed by combining IMU (In-Mechanical Unit) and laser point cloud data. To avoid the formation of false loop closures in the underground environment and to address the issue of poor initial values ​​for loop closure detection pose estimation, a multi-resolution NDT method is used for coarse-to-fine registration. Due to factors such as sensor errors, foot slippage, and mechanical vibration, the cumulative error of the robot's sensors increases over time. To effectively utilize sensor information and reduce cumulative errors, pose graph optimization is used at the back end. This involves adding laser odometry factors, loop closure factors, and foot odometry factors to construct a pose graph, thereby further correcting the cumulative error.

[0085] This invention is based on a quadrupedal robotic dog and utilizes foot-end odometry, lidar, and an inertial measurement unit (IMU) to construct an accurate and robust SLAM system. Its key technologies include:

[0086] 1. LIO front-end based on IESKF:

[0087] In multi-sensor fusion systems, sensor time synchronization is crucial. Laser sensors typically use a 10Hz frequency, while IMUs are usually at a higher 200Hz. To ensure that data collected by different sensors is based on the same time reference, a timestamp mechanism based on data from the ROS system is used for time synchronization. This invention uses the lidar data timestamp as a reference to align IMU data with the laser data, as illustrated in the diagram below. Figure 3 As shown.

[0088] For LiDAR, since it moves along with the robot during scanning, the actual distance to the scanned object inevitably differs from the measured distance, resulting in motion distortion. To eliminate this distortion, it's sufficient to transform all laser points in a frame to the start or end time of that frame. After aligning the LiDAR and IMU data, the transformation of the laser points relative to the starting pose of that frame is obtained by integrating the IMU poses of two consecutive frames, using the timestamp of each point and the timestamp of each IMU at the sampling time of the LiDAR point, and linearly interpolating. Assume the single scan time of one LiDAR frame is t. s The position of a single scan point (under radar system) is L. t = (x, y, z) T , t∈(0, t s The IMU pose at time t is T(t). WI The end time is T(t) s ) WITo achieve motion compensation, all laser points are compensated up to the moment the scan ends. Let the extrinsic parameter between the IMU and the radar be T. IL ,but L ′ For each laser point after distortion correction, the motion compensation conversion formula is:

[0089] L ′ =T LI T(t s ) IW T(t) WI T IL L t

[0090] In IESKF, we typically refer to the original state variables as nominal state variables, and the state variables within IESKF as error state variables. The sum of the nominal and error state variables is called the truth value. We handle noise within the error state variable equation, so the equations for the nominal state variables can be considered noise-free. IESKF mainly comprises two processes: prediction and update. In a tightly coupled LIO system, the IMU provides constraints for the motion process, and the laser point cloud provides constraints for the observation equations. For convenience, we denote the high-dimensional state variables in IESKF as x, defined in a high-dimensional manifold space M:

[0091] x = [p, v, R, b] g b a , g] T , x∈M

[0092] All its subscripts are from the current system to the world system, where p is displacement, v is velocity, R is rotation, and b is rotation. g b a The bias is zero, and g is gravity. Its error state is defined in the tangent space near M: The IMU measurement noise model is:

[0093]

[0094]

[0095] When IMU data arrives, the filter recursively calculates based on the IMU reading and the current state variable. Let the IMU reading between t and t+Δt be... The discrete-time recursive process can then be written as:

[0096]

[0097]

[0098]

[0099] b g (t+Δt)=b g (t)

[0100] b a (t+Δt)=b a (t)

[0101] g(t+Δt)=g(t)

[0102] During the prediction process, the nominal state is updated according to the above formula, and since the mean of the error state is zero, only its variance needs to be updated:

[0103] P pred =FPF T +Q

[0104] Where Q is the noise matrix, P is the state covariance of the previous time step, and the F matrix is ​​given by the linear form of the error kinematics:

[0105]

[0106] In tightly coupled systems, we incorporate the incremental NDT registration residuals as the observation equation into the IESKF model. Since the observation equation requires iteration, each iteration necessitates recalculating the Kalman gain and updating the process. Regarding the iteration of the P matrix, we project the initial Prep into the current tangent space. In the final iteration, we update the P matrix uniformly and project it onto the new operating point. For the k-th iteration within a single time step, P... k The Jacobian matrix J, representing the tangent space transformation on the manifold, is... k Defined as:

[0107]

[0108] but Therefore, the IESKF update process is as follows:

[0109]

[0110] δx k =K k (zh(x k ))

[0111] If the filter does not converge, P is not updated for the time being; otherwise, if the filter converges, P is updated. k+1 It should be updated according to the Kalman formula:

[0112]

[0113] However, once laser point clouds are incorporated into the observation equations, the equations easily become thousands or even tens of thousands of dimensions. If we calculate the Kalman gain in the way shown in the previous equation, we will inevitably encounter a matrix inversion problem with a very high dimension. Therefore, for K... k Using the SMW identity, we can obtain:

[0114]

[0115] Note that the inversion within this formula becomes 18×18 dimensional, significantly reducing the dimension of the inverted matrix. When dealing with high-dimensional observations, we can use this formula to calculate the Kalman gain as much as possible. For incremental NDT registration of a set of laser point clouds, assuming the point cloud has N points, when considering the registration of the j-th point, it should fall within a voxel of the target point cloud being registered, generating an NDT-related residual r. j :

[0116] r j =Rq j +pu j

[0117] Where R, p are the external transformation parameters from the registered point cloud to the registered point cloud, u j Let r be the mean value of the voxels falling at the j-th point. j The Jacobian matrix of p relative to R is:

[0118]

[0119] Following the IESKF state variable definition order, let J be... j for:

[0120]

[0121] At this time, H in the filter k The j-th row of the array should be J j The noise matrix is ​​derived from the residual information matrix, which represents the normal distribution parameters of the grid. The diagonal pieces formed;

[0122]

[0123] Therefore, in the Kalman gain formula... This can be written in the form of a summation:

[0124]

[0125]

[0126] And because Multiply by zh(x) k )for:

[0127]

[0128] Therefore δx k for:

[0129]

[0130] Update P regarding covariance k+1 for:

[0131]

[0132] This completes the derivation of the IESKF prediction and update process.

[0133] 2. Loop closure detection;

[0134] The quality of point cloud maps largely depends on adequate loop closure detection. Without loop closure detection, the shape of the point cloud depends primarily on the state of the laser odometry. However, laser odometry only processes point clouds at consecutive time points, which means that while our map point cloud is correct under continuous motion, observations at different times and locations are not well registered.

[0135] To address this, we perform loop closure detection on every two keyframes that are spatially close but temporally distant. We call such a pair of keyframes a checkpoint. To prevent the number of checkpoints from becoming too large, we set an ID interval, meaning we perform a check every 5 keyframes. For each keyframe pair, we use a scan-to-map registration method, extracting a certain number of point clouds near the keyframe as a sub-map, and then registering the scan and the sub-map. The registration is performed by incremental NDT. Due to the presence of many false loops underground and the challenge of poor initial pose estimation, we designed a coarse-to-fine registration process by changing the size of the incremental NDT voxel grid. Furthermore, we designed an incremental NDT registration scoring function, using a threshold to determine the effectiveness of loop closure detection; in our experiments, the scoring threshold was set to 0.6.

[0136] 3. Backend pose graph optimization;

[0137] To effectively utilize sensor information, reduce accumulated errors, and improve mapping quality and positioning accuracy, a multi-sensor fusion algorithm based on factor graph optimization is adopted. We aim for the final optimized trajectory to take into account the inputs from various sensors; therefore, the back-end graph optimization needs to include foot odometry factors, laser odometry factors, and loop closure detection factors. Its pose graph structure is as follows: Figure 4As shown. Compared to other multi-sensor fusion algorithms, this algorithm framework is more suitable for quadruped robots, enabling simultaneous localization and mapping of quadruped robots in complex underground environments.

[0138] To construct the foot-based odometer factor, a kinematic analysis of the quadruped robot dog is first required. The quadruped robot dog has a total of 12 degrees of freedom, with each leg having three joints: the body joint, the thigh joint, and the lower leg joint. Each joint is connected to a link, corresponding to the hip, thigh, and lower leg, respectively. The coordinate system connections for each leg are as follows: Figure 5 As shown. To facilitate the calculation of the homogeneous transformation matrix, a simplified version of the robot's single leg is used. Figure 6 In this example, we placed the origins of the three coordinate systems {0}, {1}, and {2} at the same location. Figure 5 and Figure 6 It can be seen that coordinate systems {0} and {1} are connected by fuselage joints, and the fuselage joints rotate around the x-axis, so T 01 for:

[0139]

[0140] Similarly, coordinate systems {1} and {2} rotate around the y-axis through the thigh joint, so T 12 for:

[0141]

[0142] Although the lower leg joint between coordinate systems {2} and {3} also rotates around the y-axis, the translation must be considered because the origins of coordinate systems {2} and {3} are not the same. 23 for:

[0143]

[0144]

[0145] In the above formula, l abad l is the offset on the y-axis from the thigh joint to the lower leg joint. hip Let q be the offset along the z-axis. Let point q be the robot's foot, and its homogeneous coordinates in coordinate system {3} be:

[0146]

[0147] In the above formula, l knee Let P0 be the offset of the foot relative to coordinate system {3} on the z-axis. The coordinates P0 of the foot in coordinate system {0} are:

[0148]

[0149]

[0150] To facilitate subsequent derivations, the above equation is simplified to P0 = f(φ) ∈ R3, where φ represents a vector containing all joint angles, f(·) represents the forward kinematic function, and P0 represents the position of the foot distal end in the fuselage center-of-mass coordinate system. Differentiating both sides with respect to time yields:

[0151]

[0152] In the above formula, v p Let J(φ) represent the linear velocity of the foot end in the robot's center-of-mass coordinate system, and J(φ) be the Jacobian matrix derived from the positive kinematics. Let R be the joint angular velocity. Assume the transformation parameter from the robot's center-of-mass coordinate system to the world coordinate system is R. ′ If P′, then the position P of the end of the quadruped robot dog's paws in the world coordinate system. w for:

[0153] P w =P ′ +R ′ P0

[0154] Differentiating both sides with respect to time yields the velocity v of the foot at the bottom of the world. w :

[0155]

[0156] in Let v represent the velocity of the quadruped robot dog in the world coordinate system, then v is...

[0157]

[0158] Integrating the above equation yields the displacement of the quadruped robot dog in the world coordinate system, thus completing the calculation of the foot-end odometer.

[0159] Adding laser odometry factors and closed-loop detection factors is relatively simple. Laser odometry factors can be directly obtained from the IESKF-based front end, while closed-loop detection factors can be added by determining whether a closed loop has been formed through a closed-loop detection program. If a closed loop has been formed, a closed-loop factor is added; otherwise, it is not added.

[0160] The specific implementation steps are as follows:

[0161] A) Using the external parameter calibration procedure, first perform the external parameter calibration between the lidar and the IMU;

[0162] B) Start the underlying driver and LiDAR and IMU driver of the quadruped robot dog, and synchronize the LiDAR and IMU data. After synchronization, in order to make the Iterative Error State Kalman Filter (IESKF) run, it is also necessary to perform static initialization, that is, to keep the IMU stationary for a period of time and obtain the initial zero bias of the initial IMU, the initial direction of gravity, etc.

[0163] C) Next, since the laser is distorted during the movement of the quadruped robot dog, the laser point in each frame is linearly interpolated by using the prediction value of each IMU in IESKF to obtain its pose, and then compensated to the end time of each frame to correct the motion distortion.

[0164] D) Then, the predicted pose data is obtained from IESKF, and scan-to-map registration is performed using incremental NDT. After registration, the pose is treated as observation data and iteratively updated in IESKF. Simultaneously, if the pose increment difference between the pose obtained by incremental NDT registration and the pose of the previous keyframe is too large, the current frame is treated as a keyframe, and a laser odometry factor is added to the factor map. Then, the registered point cloud is updated to each voxel of the incremental NDT for the next registration.

[0165] E) At the same time, when the loop closure detection program detects a loop, it will build a local map for the earlier frame in the candidate pair, and then use incremental NDT to perform a coarse-to-fine scan-to-map registration. If the loop closure is established, the loop closure detection factor will be added to the pose map.

[0166] F) Finally, the foot odometer factor is added to the factor graph based on the foot odometer data. The pose graph is then optimized and the accumulated error is corrected based on the received laser odometer factor, loop closure detection factor, and foot odometer factor.

[0167] This invention presents a synchronous positioning and mapping method for complex underground environments, characterized by reliable performance, strong obstacle-crossing ability, good real-time performance, and strong robustness. Besides applications in underground resource exploration, natural disaster relief, and digital management of mine environments, it can also meet the needs of underground spaces in large cities, large caverns, and subway systems. This invention will fill a gap in my country's independent underground exploration field, possessing significant economic and social benefits in both civilian and military applications.

Claims

1. A method for synchronous positioning and mapping in complex underground environments, characterized in that, A laser inertial odometry front-end based on the Iterative Error State Kalman Filter (IESKF) and a back-end based on pose graph optimization are constructed. The two are combined to detect closed loops and build a globally consistent map. The laser inertial odometry front end based on iterative error state Kalman filtering includes a lidar and an IMU; After the extrinsic parameters between the lidar and the IMU are calibrated, the lidar data and the IMU data are synchronized. After synchronization, the IMU is initialized at rest to obtain the initial zero bias and initial gravity direction of the IMU, which are used to run the iterative error state Kalman filter. The lidar and the quadruped robot dog move together, causing motion distortion in a normally acquired frame of point cloud. The state prediction part of the iterative error state Kalman filter is used to calculate the IMU pose of each laser frame. Linear interpolation is performed on each laser point in the current laser frame to obtain the pose relative to the start time of each laser frame. Finally, each laser point in a frame is compensated to the end time of each laser frame to correct the motion distortion. The iterative error state Kalman filter predicts the pose data of the laser in the current frame based on the pose data after laser motion distortion correction in the previous frame. Then, the predicted pose data is used as the initial value for registration from the current laser frame to the local map. The registration method is incremental normal distribution transformation. After registration, the calculated residual is used as the observation data and put into the state update part of IESKF for iterative update until convergence, which is used for pose map optimization. When the difference between the pose increment of the current laser frame and the pose of the previous laser key frame exceeds a fixed threshold, the current laser frame is set as a key frame. At the same time, the laser inertial odometry factor corresponding to the key frame is added to the pose graph created by the backend based on the pose graph optimization, and the registered lidar point cloud is updated to each voxel of the incremental normal distribution transformation for the next registration. The correction and conversion formula for the motion distortion is: The extrinsic parameters between the IMU and the lidar are ; The end time of the IMU scan ; Let t be the IMU pose at time t; This represents the position of a single scanning point of the lidar. ; For each laser point after distortion correction; In the laser inertial odometry, for the state update part of the IESKF, the IESKF uses the incremental normal distribution transformation registration residual as the observation equation, and uses the observation equation as input to iteratively update the Kalman gain in the Kalman filter. Error state of x Covariance This updates the high-dimensional state variables of the iterative error state Kalman filter; The IMU provides constraints on the motion process, while the lidar point cloud provides constraints on the observation equations; the high-dimensional state variables of the iterative error state Kalman filter are uniformly denoted as... It is defined in a high-dimensional manifold space. : p is displacement, v is velocity, and R is rotation. The bias is zero, and g is gravity; variable It is formed by the sum of the nominal state and the error state, where the error state of x is defined in the high-dimensional manifold space. Similar tangent spaces: ; The IMU measurement noise model is: The values ​​are measured by the IMU accelerometer. This is the rotation matrix from the world coordinate system to the IMU coordinate system. This represents the actual acceleration value of the IMU. For the zero bias of the IMU accelerometer, Measurement noise of the IMU accelerometer; The values ​​measured by the IMU gyroscope. This represents the actual gyroscope value of the IMU gyroscope. For IMU gyroscope zero bias, Measurement noise of the IMU gyroscope; Iterative error state Kalman filtering recursively calculates the IMU measurement and the current state variable; let t to t+ The IMU measurements between The discrete-time recursive process is as follows: High-dimensional variables in IMU prediction process The nominal state is updated according to the above formula. Since the mean of the error state is zero, only its variance is updated: Where Q is the measurement noise matrix, P is the state covariance of the previous time step, and F is given by the linear form of the error kinematics: 。 2. The simultaneous localization and mapping method of an underground complex environment according to claim 1, wherein, The Kalman gain in the iterative error state Kalman filter adopts... Calculation; where Let Kalman gain be the value of the k-th iteration. Find the inverse of the covariance matrix at the k-th iteration. For the laser point cloud observation equation with respect to the variables at the k-th iteration, This represents the noise in the laser point cloud observation equation.

3. The simultaneous localization and mapping method of an underground complex environment according to claim 1, wherein, The incremental normal transformation registration residual is used as the observation equation. For incremental normal transformation registration of a set of laser point clouds, the laser point cloud has a total of N points. When considering the registration of the j-th point, the j-th point falls within a voxel in the target point cloud being registered, and an incremental normal transformation-related residual is generated. : in , The external transformation parameters are those for the registered point cloud to the registered point cloud. Let be the mean value of the voxels falling at the j-th point. For the j-th point in the registered point cloud, Compared to , The Jacobian matrix is: ， According to the definition order of the state variables of the iterative error state Kalman filter, let be: At this point, the iteration error state of the Kalman filter... The j-th row of the array The noise matrix V is derived from the residual information matrix, which is the normal distribution parameter in the voxels. The diagonal pieces formed; Thus the Kalman gain formula becomes In summation form: times is: So Regarding the update of the covariance is: 。 4. The simultaneous localization and mapping method of an underground complex environment according to claim 1, wherein, In the backend based on pose graph optimization, foot odometry is constructed by analyzing the motion model of the quadruped robot dog; foot odometry factor, laser odometry factor and loop closure detection factor are added to the pose graph, and the pose graph is optimized based on the three factors. The optimized pose graph result is input to the map update module to update the constructed map.

5. The simultaneous localization and mapping method of an underground complex environment according to claim 4, characterized in that, The foot odometer is constructed as follows: a coordinate system fixed to the body is used as coordinate system {0}, a coordinate system that follows hip rotation is used as coordinate system {1}, a coordinate system that follows thigh rotation is used as coordinate system {2}, and a coordinate system that follows lower leg rotation is used as coordinate system {3}. In the above formula Let {1} be the homogeneous transformation matrix of coordinate system {1} relative to coordinate system {0}. Let {2} be the homogeneous transformation matrix of coordinate system {2} relative to coordinate system {1}; Let {3} be the homogeneous transformation matrix of coordinate system {2} relative to coordinate system {3}. In the above formula This represents the offset on the y-axis from the thigh joint to the lower leg joint. Let q be the offset along the z-axis; let point q be the foot of the robot, and the homogeneous coordinates of point q in coordinate system {3} be: In the above formula The offset of the foot relative to coordinate system {3} on the z-axis; the coordinates of the foot in coordinate system {0} for: Simplify the above equation to ,in This represents a vector containing all joint angles. Represents the forward kinematic function. This indicates the position of the foot tip in the coordinate system of the fuselage's center of mass; differentiating both sides with respect to time yields: In the formula, This represents the linear velocity of the foot tip in the coordinate system of the quadruped robot dog's center of mass. The Jacobian matrix for differentiating positive kinematics. The joint angular velocity; the transformation parameters from the quadruped robot dog's center-of-mass coordinate system to the world coordinate system are: Then the position of the quadruped robot dog's paws in the world coordinate system for: Taking the derivative of both sides with respect to time gives the linear velocity of the foot in the world coordinate frame : wherein represents the velocity of the quadruped robot dog in the world coordinate system, and thus is Integrating the above equation yields the displacement of the quadruped robot dog in the world coordinate system.

6. The simultaneous localization and mapping method of an underground complex environment according to claim 5, wherein, The loop closure detection is based on multi-resolution incremental normal distribution transformation. The overall loop closure detection process consists of three steps: candidate selection, candidate matching score calculation, and loop closure factor construction. Based on the laser point cloud of the current frame after motion distortion correction, laser candidate keyframes within a set radius are searched in the map update module. Incremental normal distribution transformation is used for registration and matching score calculation. When the score exceeds a fixed threshold, the loop closure is established. Loop closure detection factors are constructed for the current frame and candidate keyframes, and the loop closure detection factors are added to the pose factor map.

Images