# US 2023/0066441 A1 · Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2023/0066441 A1
- Document ID: US20230066441A1
- Status: Published application
- Kind: A1
- Application number: 17/793,770
- Filed: 2020-12-09
- Published: 2023-03-02
- Earliest claimed: 2020-01-20
- Assignees: Shenzhen Pudu Technology Co., Ltd.
- Applicants: Shenzhen Pudu Technology Co., Ltd.
- Inventors: Kejun HE; Meiwen CHEN; Cong GUO
- Web page: https://patentgrep.com/patent/US20230066441A1

## CPC

- B25J 9/1697: {Vision controlled systems}
- G01C 21/1652: {with ranging devices, e.g. LIDAR or RADAR}
- B25J 9/161: {Hardware, e.g. neural networks, fuzzy logic, interfaces, processor}
- B25J 9/162: {Mobile manipulator, movable base with manipulator arm mounted on it}
- B25J 9/163: {learning, adaptive, model based, rule based expert control}
- G01C 21/1656: {with passive imaging devices, e.g. cameras}
- G01S 7/4808: {Evaluating distance, position or velocity data}
- G01S 17/42: Simultaneous measurement of distance and other co-ordinates (indirect measurement G01S17/46)
- G01S 17/86: Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
- G01S 17/89: for mapping or imaging
- G06V 20/52: Surveillance or monitoring of activities, e.g. for recognising suspicious objects (recognising microscopic objects G06V20/69)
- G06V 10/82: using neural networks
- B25J: MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES ({manipulators specially adapted for use in surgery A61B34/70; manipulators used in cleaning hollow articles B08B9/04}; manipulators associated with rolling mills B21B39/20; manipulators associated with forging machines B21J13/10; {manipulators associated with picking-up and placing mechanisms B23P19/007}; means for holding wheels or parts thereof B60B30/00; {vehicles with ground-engaging propulsion means, e.g. walking members B62D57/02, B62D57/032; devices for picking-up and depositing articles or materials between conveyors B65G47/90, B65G47/91; manipulators with gripping or holding means for transferring packages B65H67/065}; cranes B66C; {manipulators used in the protection or supervision of pipe-line installations F17D5/00; walking equipment adapted for nuclear steam-generators F22B37/006}; manipulators specially adapted for, or associated with, nuclear reactors G21C; {apparatus used for handling wafers during manufacture or treatment of semiconductor H10P72/50})
- G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S)
- G01S: RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING

## Abstract

A multi-sensor fusion SLAM system and a robot. The system operates on a mobile robot and comprises: a visual inertia module, a laser scanning matching module, a loop closure detection module, and a visual laser image optimization module. According to the multi-sensor fusion SLAM system and the robot, the calculation amount of laser matching constraint optimization can be reduced by using a voxel subgraph so that the pose calculation is more accurate, accumulated errors of long-time operation of the system can be corrected in time by means of sufficient fusion of modules, and the robustness of the system and the accuracy of positioning and mapping are integrally improved.

## Claims

### Claim 1

A multi-sensor fusion SLAM system operating on a mobile robot, the multi-sensor fusion SLAM system comprising:
a visual inertia module configured to output pose information;
a laser scanning matching module configured to use the pose information as an initial value, match a laser scanned point cloud with a voxel map to solve an advanced pose, integrate the point cloud into the voxel map according to the advanced pose, and derive a new voxel subgraph, the laser scanning matching module being configured to a laser matching constraint;
a loop closure detection module configured to extract an image descriptor of a visual keyframe by using a deep neural network, compare the image descriptor with an image descriptor of a previous keyframe to determine whether a closed loop exists, and if the closed loop exists, determine pose transformation of the two keyframes by using a perspective-n-point (PnP) method, solve a loop closure pose constraint according to the pose transformation and the voxel subgraph, and send the loop closure pose constraint to the laser scanning matching module; and
a visual laser image optimization module configured to correct an accumulated error of the system according to the pose information, the laser matching constraint, and the loop closure pose constraint after the closed loop occurs.

### Claim 2 (depends on 1)

The multi-sensor fusion SLAM system of claim 1, wherein the laser scanned point cloud is matched with the voxel map to solve the advanced pose by using an iterative closest point (ICP) algorithm.

### Claim 3 (depends on 1)

The multi-sensor fusion SLAM system of claim 1, wherein the visual inertia module includes:
a visual front-end unit configured to select the keyframe;
an inertial measurement unit (IMU) pre-integration unit configured to generate an IMU observation value; and
a sliding window optimization unit configured to jointly optimize a visual reprojection error, an inertial measurement error, and a mileage measurement error.

### Claim 4 (depends on 3)

The multi-sensor fusion SLAM system of claim 3, wherein the visual front-end unit takes a monocular camera or binocular camera as input, the monocular camera or binocular camera being configured to capture initial images,
the visual front-end unit is configured to track feature points of each frame by using a Kanade-Lucas-Tomasi (KLT) sparse optical flow algorithm,
the visual front-end unit includes a detector, the detector detecting corner features and keeping a minimum number of the feature points in each of the initial images, the detector being configured to set a minimum pixel interval between two adjacent feature points, and
the visual front-end unit is configured to remove distortion of the feature points, remove mismatched feature points by using a random sample consensus (RANSAC) algorithm and a fundamental matrix model, and project correctly matched feature points onto a unit sphere.

### Claim 5 (depends on 4)

The multi-sensor fusion SLAM system of claim 4, wherein the selecting the keyframe specifically includes: determining whether an average parallax of the tracked feature points between a current frame and the latest keyframe exceeds a threshold, taking the current frame as a new keyframe if the average parallax exceeds a first threshold, and taking the frame as the new keyframe if the number of the tracked feature points of the frame is below a second threshold.

### Claim 6 (depends on 3)

The multi-sensor fusion SLAM system of claim 3, wherein the laser scanning matching module includes a lidar configured to acquire a scanning point, transform the scanning point according to the pose information and the IMU observation value, and convert the scanning point into a three-dimensional point cloud in a coordinate system where the robot is located at a current moment.

### Claim 7 (depends on 1)

The multi-sensor fusion SLAM system of claim 1, wherein the loop closure detection module includes:
a similarity detection unit configured to extract an image descriptor of a current keyframe, compare the image descriptor with an image descriptor of a keyframe in a keyframe data set, select a similar keyframe with highest similarity, and insert the similar keyframe into the keyframe data set;
a visual pose solving unit configured to match feature points of the current keyframe and the similar keyframe through a fast feature point extraction and description algorithm, remove mismatched feature points by using a RANSAC algorithm and a fundamental matrix model, and when the number of correctly matched feature points reaches a third threshold, solve relative pose transformation from the current keyframe to the similar keyframe by using the RANSAC algorithm and the PnP method; and
a laser pose solving unit configured to select two voxel subgraphs associated with the current keyframe and the similar keyframe respectively, take the relative pose transformation as the initial value, and match the two voxel subgraphs by using an ICP algorithm, to obtain final relative pose transformation.

### Claim 8 (depends on 1)

A robot, wherein the robot includes the multi-sensor fusion SLAM system of claim 1.

### Claim 9 (depends on 1)

A multi-sensor fusion method, wherein the method is applied to the multi-sensor fusion SLAM system of claim 1, and the method includes:
acquiring pose information;
using the pose information as an initial value, matching laser scanned point cloud with a voxel map to solve an advanced pose, integrating the point cloud into the voxel map according to the advanced pose, and deriving a new voxel subgraph;
acquiring a laser matching constraint;
extracting an image descriptor of a visual keyframe by using a deep neural network, comparing the image descriptor with an image descriptor of a previous keyframe to determine whether a closed loop exists, and if the closed loop exists, determining pose transformation of the two keyframes by using a PnP method, and solving a loop closure pose constraint according to the pose transformation and the voxel subgraph; and
correcting an accumulated error of the system according to the pose information, the laser matching constraint, and the loop closure pose constraint after the closed loop occurs.

### Claim 10 (depends on 9)

A robot, wherein the robot includes a processor and a memory, the memory storing a computer program, the processor being configured to execute the computer program to perform steps of the multi-sensor fusion method of claim 9.

### Claim 11 (depends on 9)

A computer storage medium storing a computer program, wherein when the computer program is executed, steps of the multi-sensor fusion method of claim 9 are performed.
