A low-cost multi-sensor fusion cable trench robot adaptive mapping method

Abstract

This invention discloses a low-cost, multi-sensor fusion adaptive mapping method for cable trench robots, belonging to the field of intelligent robot technology. This mapping method includes: collecting observation information from multiple sensors of the inspection robot to obtain its original pose information; fusing sensor data from LiDAR, inertial units, and RGBD cameras to perform motion estimation in stages; adaptively selecting sensors after evaluating the accuracy and performance of each sensor through motion constraints and pose residual weights; eliminating false inter-frame matches using a designed adaptive inter-frame registration method; and performing segmented global optimization to construct a high-precision map. This invention integrates low-cost sensors such as 2D LiDAR, RGBD cameras, inertial devices, and wheeled odometers, enabling the inspection robot to adaptively achieve high-precision mapping even in the absence of prior GPS information and loop closure constraints.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, and in particular to a low-cost, multi-sensor fusion adaptive mapping method for cable trench robots. Background Technology

[0002] Severely structured environments with uneven ground are common in robot applications; underground cable trenches and underground utility tunnels are typical examples. For inspection or operational robots used in such scenarios, the ability to autonomously locate themselves in real-time and incrementally build maps in unknown environments is one of the key technologies affecting the effectiveness of inspections or operations.

[0003] LiDAR-based SLAM technology is currently a relatively mature mapping and localization method. Due to practical application considerations such as cost, most inspection robots used in long-channel environments employ the 2D SLAM method for mapping and localization. This method mainly has two implementations: filtering and graph optimization. Typical filtering-based algorithms, such as Gmapping SLAM, rely heavily on particle confidence and loop closure detection, which can easily lead to map misalignment when building large maps. Typical graph optimization-based algorithms, such as Cartographer SLAM, introduce the idea of ​​loop closure detection, optimizing the robot's overall pose by optimizing the pose of all keyframes of the state variable, thus solving the problem of mapping and localization in large scenes. However, in heavily structured environments, if only laser registration is used to obtain state estimation, graph optimization-based 2D SLAM suffers from problems such as scene degradation and laser odometry failure. Therefore, using multiple sensors for fusion has become a necessary option for accurate mapping and localization.

[0004] When fusing multi-sensor data, either loosely coupled or tightly coupled methods can be used. Currently, 2D SLAM multi-sensor fusion mainly adopts a loosely coupled approach. For example, Cartographer SLAM uses lossless Kalman filtering to fuse multi-source data to obtain the robot's pose. Loosely coupled data fusion is simple and has high real-time performance, but it is prone to problems such as accumulated errors and poor robustness. Tightly coupled methods fuse the states of multiple sensors together to jointly construct motion and observation equations. Even without loop closure, the constructed map and localization accuracy are very high. For example, the VINS_mono algorithm uses vision / IMU pre-integration for tight coupling, and the LIO_SAM algorithm uses 3D LiDAR / IMU pre-integration for tight coupling. Visual cameras have a short detection range and cannot quickly build maps in large scenes such as cable trenches, while 3D LiDAR is expensive, and using 3D LiDAR would greatly increase hardware costs. In addition, the surface of cable trenches is mostly unpaved or contains gravel and sand. When mapping, the lidar will frequently jitter, which reduces the registration quality between lidar odometry frames and causes ghosting in the map. This results in a large error between the constructed map and the actual cable trench, which cannot meet the inspection requirements.

[0005] In summary, the main problems faced by inspection robots in mapping underground cable trenches are as follows: Underground cable trenches are highly structured mapping scenarios, and stable prior GPS pose data and loop closure constraints are often unavailable, leading to significant mapping errors and map degradation. Furthermore, the surface of cable trenches is often uneven due to factors such as gravel, causing odometer drift and laser point misalignment during mapping by the inspection robot, resulting in map degradation and ghosting, severely impacting map accuracy.

[0006] Therefore, those skilled in the art are dedicated to developing a low-cost, multi-sensor fusion adaptive mapping method for cable trench robots. This method avoids the robot's reliance on GPS prior pose and loop closure constraints, and overcomes the problems of map degradation and ghosting during cable trench mapping. It enables the robot to perform high-precision mapping in the complex environment of cable trenches using only the low-cost sensors carried by the robot itself, providing a suitable prerequisite for subsequent engineering applications such as inspection and evaluation. Summary of the Invention

[0007] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is that when the inspection mobile robot is mapping the cable trench, the scene degradation and map ghosting are caused by factors such as the severe structure of the mapping scene making it impossible to perform loop closure to eliminate odometer errors, and uneven road surface causing laser point misalignment.

[0008] To achieve the above objectives, this invention provides a low-cost, multi-sensor fusion adaptive mapping method for cable trench robots, characterized by the following steps:

[0009] Step 1: Collect observation information from the internal and external sensors of the inspection robot to obtain the robot's positioning information;

[0010] Step 2: The motion estimation of the inspection robot is performed in stages by fusing data from LiDAR, inertial unit, and RGBD camera;

[0011] Step 3: After evaluating the accuracy and performance of each sensor through motion constraints and pose residual weights, perform multi-strategy adaptive initialization;

[0012] Step 4: Eliminate false inter-frame matches using the designed adaptive inter-frame registration method;

[0013] Step 5: Perform global optimization in segments and adaptively build a high-precision map.

[0014] Further, step 1 includes:

[0015] Predictive data on relative poses are obtained based on the internal sensors of the inspection robot;

[0016] The inspection robot obtains observation data on relative poses based on external sensors.

[0017] The internal sensor observation information refers to the relative pose information calculated by the inertial unit and wheel encoder of the inspection robot itself, while the external sensor observation information refers to the observation information obtained by the inspection robot through inter-frame matching using a two-dimensional single-line radar and an RGBD camera.

[0018] Furthermore, the motion estimation of the inspection robot in step 2, performed in stages, includes the following steps:

[0019] By coupling an RGBD camera and an IMU, a rough estimate of the robot's motion information is performed at high frequency with relatively low computational cost. The calculation formula is as follows:

[0020] ;

[0021] In the formula, and To normalize the x and y coordinates of the current frame, and This is the rotation matrix and translation vector from the previous historical frame to the current frame. The subscript number indicates which row of the matrix or which element of the vector is being used. The coarsely estimated motion information is then subjected to low-frequency but fine-grained re-motion estimation processing by a two-dimensional lidar.

[0022] Furthermore, the motion estimation in step 2 includes the following:

[0023] When the camera can extract enough feature points in the scene for motion estimation, the motion estimation of visual-inertial fusion is used as the key pose through coordinate system association. Then, the laser points are locally registered between the key poses through IMU interpolation. When the camera or camera-inertial fusion link cannot work properly, the LiDAR is used for motion estimation and constraints are applied through IMU pre-integration.

[0024] Furthermore, in step 3, the multi-strategy adaptive initialization first checks whether the RGBD camera has successfully obtained enough feature matching points and reliable depth information. If successful, it then checks whether the LiDAR has enough point pairs to match successfully. If so, it optimizes and updates the motion and observation information using the PL_ICP method.

[0025] Furthermore, if only the RGBD camera meets the requirements, the laser pose optimization process is skipped. The visual inertial odometry is used to update the laser odometry information and directly estimate the robot's pose state. If neither the camera nor the LiDAR can perform sufficient feature matching to obtain motion estimation, the system will enter degenerate mode and output the filtered predicted pose as the robot pose through filtering processing of the IMU and wheel odometry.

[0026] Furthermore, the filtering method for the IMU and the wheel odometer is iterative Kalman filtering. The discrete motion equations for the IMU part are as follows:

[0027] ;

[0028] In the formula, xk, These represent the robot pose at frame k, the robot's prior pose, and the robot's posterior pose, respectively. This represents the robot's state matrix at frame k-1. This represents the motion input noise matrix at frame k-1.

[0029] The pose observation equation is calculated as follows:

[0030] ;

[0031] In the formula, The robot's observed pose at frame k. This represents the observation information matrix of the robot at frame k. This represents the observation noise matrix of the robot at frame k.

[0032] Furthermore, when the wheel-type odometer is integrated with the IMU, a trust weight allocation method is set up due to the varying flatness of the actual ground. The calculation formula is as follows:

[0033] ;

[0034] n is the observed noise of the angle error during the IEKF filtering process. and The rotation in the world coordinate system is determined by the odometer observation and the IMU integral prediction, respectively. PI is the sampling frequency of the IMU sensor data. When the ground is uneven and the wheels slip or spin, the error between the predicted attitude and the observed attitude increases, which also increases the observation noise. The system will adaptively assign a higher weight to the IMU prediction to reduce the amount of error.

[0035] Furthermore, in step 4, the method for eliminating false inter-frame matches through the designed adaptive inter-frame registration method refers to the adaptive comparison and screening of three state variables: state variable one is the predicted pose TI obtained by IMU integration based on the pose of the most recent historical lidar data, state variable two is the observed pose TL obtained by lidar inter-frame registration, and state variable three is the robot's initial pose T0.

[0036] Furthermore, based on expert analysis, the cumulative error of robot pose and the error threshold between adjacent keyframes of LiDAR are set, and corresponding back-end optimization processing methods are selected to eliminate the registration error between odometry and LiDAR keyframes, thereby eliminating map degradation and ghosting.

[0037] Furthermore, through adaptive filtering, key frame registration between lidar sensors is achieved, and a high-precision map of the cable trench is constructed.

[0038] Compared with existing technologies, this invention has at least the following beneficial technical effects: This invention integrates multiple low-cost sensors such as IMU, 2D single-line LiDAR, RGBD camera, and wheeled odometer for mapping, solving the problem of map degradation caused by severe scene structuring in cable trench mapping. Furthermore, addressing the technical problem of laser point dispersion due to uneven actual road surfaces in cable trenches, resulting in ghosting in the mapping effect, this invention innovatively employs an adaptive inter-frame registration method to meet the requirements of high-precision mapping of cable trenches.

[0039] The following will further explain the concept, specific structure and technical effects of the present invention in conjunction with the accompanying drawings, so as to fully understand the purpose, features and effects of the present invention. Attached Figure Description

[0040] Figure 1 This is a schematic diagram of a method flow according to a preferred embodiment of the present invention;

[0041] Figure 2 This is a schematic diagram of an adaptive registration and filtering process, which is a preferred embodiment of the present invention. Detailed Implementation

[0042] The following description, with reference to the accompanying drawings, illustrates several preferred embodiments of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0043] In the accompanying drawings, components with the same structure are indicated by the same numerical designation, and components with similar structures or functions are indicated by similar numerical designations. The dimensions and thicknesses of each component shown in the drawings are arbitrary, and the present invention does not limit the dimensions and thicknesses of each component. To make the illustrations clearer, the thickness of some components has been appropriately exaggerated in the drawings.

[0044] like Figure 1 The image shows a low-cost, multi-sensor fusion adaptive mapping method for cable trench robots in this embodiment.

[0045] The flowchart includes the following steps:

[0046] Step 1: Collect observation information from the internal and external sensors of the inspection robot to obtain the robot's positioning information;

[0047] Step 2: The motion estimation of the inspection robot is performed in stages by fusing data from LiDAR, inertial unit, and RGBD camera;

[0048] Step 3: After evaluating the accuracy and performance of each sensor through motion constraints and pose residual weights, perform multi-strategy adaptive initialization;

[0049] Step 4: Eliminate false inter-frame matches using the designed adaptive inter-frame registration method;

[0050] Step 5: Perform global optimization in segments and adaptively build a high-precision map.

[0051] Specifically, raw data is collected independently by the equipment mounted on the inspection robot. When estimating the robot's motion state, the RGBD camera and IMU are coupled first to perform a rough estimate of the robot's motion information at high frequency with a relatively small amount of computation.

[0052] ;

[0053] In the formula, and To normalize the x and y coordinates of the current frame, and The system uses a rotation matrix and translation vector from the previous historical frame to the current frame, with the index indicating the row number of the matrix or the element number of the vector. Then, a low-frequency but refined re-evaluation of the roughly estimated motion information is performed using a 2D LiDAR. When neither the camera nor the LiDAR is functioning properly, the system enters a degenerate mode, filtering the data through an IMU and wheeled odometry system, and outputs the filtered predicted pose as the robot's pose.

[0054] Specifically, the filtering method for both the IMU and the wheeled odometer is iterative Kalman filtering. The discrete motion equations for the IMU part are as follows:

[0055] ;

[0056] In the formula, xk, These represent the robot pose at frame k, the robot's prior pose, and the robot's posterior pose, respectively. This represents the robot's state matrix at frame k-1. This represents the motion input noise matrix at frame k-1.

[0057] The pose observation equation is calculated as follows:

[0058] ;

[0059] In the formula, The robot's observed pose at frame k. This represents the observation information matrix of the robot at frame k. This represents the observation noise matrix of the robot at frame k.

[0060] Furthermore, when the wheel-type odometer is integrated with the IMU, a trust weight allocation method is set up due to the varying flatness of the actual ground. The calculation formula is as follows:

[0061] ;

[0062] n is the observed noise of the angle error during the IEKF filtering process. and The rotation in the world coordinate system is determined by the odometer observation and the IMU integral prediction, respectively. PI is the sampling frequency of the IMU sensor data. When the ground is uneven and the wheels slip or spin, the error between the predicted attitude and the observed attitude increases, which also increases the observation noise. The system will adaptively assign a higher weight to the IMU prediction to reduce the amount of error.

[0063] like Figure 2 The diagram shown is a schematic of the adaptive registration and filtering process in this embodiment. Different optimization processes are performed by comparing and filtering the three state variables TI, TL, and T0.

[0064] Where TI is the predicted pose obtained by IMU integration based on the previous frame of lidar data; TL is the observed pose obtained by inter-frame registration of lidar; and T0 is the initial pose of the intelligent robot.

[0065] Based on expert analysis, thresholds were set for the cumulative robot pose error and the error between adjacent keyframes of the LiDAR, and appropriate backend optimization methods were selected.

[0066] Finally, after backend optimization and registration, the robot performs adaptive high-precision mapping in the cable trench environment.

[0067] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A low-cost, multi-sensor fusion adaptive mapping method for cable trench robots, characterized in that, The method includes the following steps: Step 1: Collect observation information from the internal and external sensors of the inspection robot to obtain the robot's positioning information; Step 2: The motion estimation of the inspection robot is performed in stages by fusing data from LiDAR, inertial unit, and RGBD camera; Step 3: After evaluating the positioning accuracy and performance of each sensor through motion constraints and pose residual weights, perform multi-strategy adaptive initialization; Step 4: Eliminate false inter-frame matches using the designed adaptive inter-frame registration method; Step 5: Perform global optimization in segments and adaptively build a high-precision map; In step 2, the motion estimation of the inspection robot in stages refers to first coupling the RGBD camera and IMU to perform a rough estimation of the robot's motion information at high frequency with a relatively small amount of computation. The calculation formula is as follows: ； In the formula, and To normalize the x and y coordinates of the current frame, and The rotation matrix and translation vector from the previous historical frame to the current frame are used. The subscript number indicates which row of the matrix or which element of the vector is taken. The coarsely estimated motion information is re-estimated by a two-dimensional lidar and then processed for low-frequency but fine motion estimation. The re-motion estimation refers to using the motion estimation of visual-inertial fusion as the key pose through coordinate system association when the camera can extract enough feature points in the scene for motion estimation. Then, the laser points are locally registered between the key poses through IMU interpolation. When the camera or camera-inertial fusion link cannot work properly, the LiDAR is used for motion estimation and IMU pre-integration is used for constraint. In step 3, the multi-strategy adaptive initialization part first checks whether the RGBD camera has successfully obtained enough feature matching points and reliable depth information. If successful, it then checks whether the LiDAR has enough point pairs to match successfully. If so, it optimizes and updates the motion estimation and observation information through the PL-ICP method. In step 4, the adaptive inter-frame registration method designed to eliminate false inter-frame matches refers to adaptive inter-frame registration through adaptive comparison and filtering of three state variables. State variable one is the predicted pose obtained by IMU integration based on the pose of the most recent historical LiDAR data. State quantity two is the observation pose obtained by inter-frame registration of the lidar. State variable three represents the robot's initial posture. ; in, To predict the pose for transformation from the robot's carrier coordinate system to the world coordinate system, To predict the position from the robot's coordinate system to the world coordinate system, To transform the observation posture from the robot's carrier coordinate system to the world coordinate system, To transform the observation position from the robot's coordinate system to the world coordinate system, The initial pose for the transformation from the robot's carrier coordinate system to the world coordinate system.

2. The low-cost multi-sensor fusion adaptive mapping method for cable trench robots as described in claim 1, characterized in that, Step 1 includes: Predictive data on relative poses are obtained based on the internal sensors of the inspection robot; The inspection robot obtains observation data on relative poses based on external sensors. The internal sensor observation information refers to the relative pose information calculated by the inertial unit and wheel encoder of the inspection robot itself, while the external sensor observation information refers to the observation information obtained by the inspection robot through inter-frame matching using a two-dimensional single-line radar and an RGBD camera.

3. The low-cost multi-sensor fusion adaptive mapping method for cable trench robots as described in claim 1, characterized in that, The acquisition of the inspection robot's positioning information refers to selecting some key frames through a sliding window to constrain pose residuals, and extracting and retaining historical information by edge prior factors obtained through edge-mapping of past historical observation frames.

4. The low-cost multi-sensor fusion adaptive mapping method for cable trench robots as described in claim 1, characterized in that, If only the RGBD camera meets the requirements, the laser pose optimization process is skipped. The visual inertial odometry is used to update the laser odometry information and directly estimate the robot's pose state. If neither the camera nor the LiDAR can perform sufficient feature matching to obtain motion estimation, the system will enter degenerate mode and output the filtered predicted pose as the robot pose through filtering processing by the IMU and wheel odometry.

5. The low-cost multi-sensor fusion adaptive mapping method for cable trench robots as described in claim 4, characterized in that, The filtering method used for the IMU and wheeled odometer is the iterative Kalman filtering method. The discrete motion equations for the IMU are as follows: ； In the formula, , These represent the robot pose, the robot's prior pose, and the robot's posterior pose at frame k, respectively. This represents the robot's state matrix at frame k-1. The motion input noise matrix at frame k-1 is represented by the following equation: ； In the formula, The robot's observed pose at frame k. This represents the observation information matrix of the robot at frame k. This represents the observation noise matrix of the robot at frame k.

6. The low-cost multi-sensor fusion adaptive mapping method for cable trench robots as described in claim 4, characterized in that, When the wheel-type odometer is integrated with the IMU, a trust weight allocation method is set up due to the varying flatness of the actual ground. The calculation formula is as follows: ； In the formula, n is the observed noise of the angle error during the IEKF filtering process. and Rotation in the world coordinate system based on odometry observations and IMU integral predictions, respectively. It is the sampling frequency of the IMU's sensor data. When the ground is uneven, causing the wheels to slip or spin, the error between the predicted attitude and the observed attitude increases, which also increases the observation noise. The system will adaptively assign higher weights to the IMU prediction to reduce the amount of error.

Images