# US 2025/0147185 A1 · External rotation 3d lidar device and simultaneous localization and mapping (slam) method thereof

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

- Number: US 2025/0147185 A1
- Document ID: US20250147185A1
- Status: Published application
- Kind: A1
- Application number: 18/784,970
- Filed: 2024-07-26
- Published: 2025-05-08
- Earliest claimed: 2023-11-08
- Assignees: Beijing Institute of Technology
- Applicants: Beijing Institute of Technology
- Inventors: Hao FANG; Haoyu YANG; Yiren HAO; Aobo WANG; Kefan ZHENG; Delong WU; Bochen XU; Shaozhun WEI; Qingkai YANG
- Web page: https://patentgrep.com/patent/US20250147185A1

## CPC

- G01S 7/4813: {Housing arrangements}
- G01S 17/89: for mapping or imaging
- G01S 7/4802: {using analysis of echo signal for target characterisation; Target signature; Target cross-section}
- G01S 7/4808: {Evaluating distance, position or velocity data}
- G01S 7/4817: {relating to scanning}
- G01S 17/10: using transmission of interrupted, pulse-modulated waves (determination of distance by phase measurements G01S17/32)
- G01S 17/86: Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
- 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

## Abstract

An external rotation 3D lidar device and a simultaneous localization and mapping (SLAM) method comprises a 3D hybrid solid-state lidar device that is driven to rotate by an external motor. The device significantly improves the horizontal field of view of the lidar and can be mounted on a ground robot to comprehensively improve its 360-degree environment sensing capabilities. Error-state Kalman filtering and pose graph optimization are combined and the overall framework is divided into two parts: front-end odometry and back-end loop-closure optimization. Therefore, high-frequency odometry that meets the requirements of the robot can be output in real time and cumulative errors can be eliminated through the back-end loop-closure optimization.

## Claims

### Claim 1

An external rotation 3D lidar device, comprising:
a processor,
a hybrid solid-state lidar,
a camera,
a slip ring,
a coupling,
a stepper motor,
a pulse generator,
buck modules,
a power distribution board,
a power supply, and
a motor drive, wherein
the processor is mounted above the hybrid solid-state lidar and is configured to receive real-time data of the hybrid solid-state lidar;
the camera is fixedly mounted on one side of the hybrid solid-state lidar and is configured to record videos;
the stepper motor and the slip ring are both connected to the hybrid solid-state lidar via the coupling, that is, the stepper motor, the slip ring, the coupling, and the hybrid solid-state lidar form a rotating mechanism;
the pulse generator is configured to control the motor drive and accordingly control the stepper motor;
power provided by the power supply is split by the power distribution board into four paths, two of the four paths being configured for powering the pulse generator and the motor drive, and other two of the four paths being configured for powering the processor and the hybrid solid-state lidar through the buck modules and the slip ring.

### Claim 2 (depends on 1)

A simultaneous localization and mapping (SLAM) method adapted to the external rotation 3D lidar device according to claim 1, comprising:
(1) acquiring data of an inertial measurement unit (IMU) and predicting an IMU state according to the data of the IMU to obtain an initial value of the IMU state;
(2) performing a nearest neighbor motion compensation on an original point cloud of the hybrid solid-state lidar to restore a real scene structure based on the initial value of the IMU state obtained in the step (1);
(3) filtering a point cloud obtained by the nearest neighbor motion compensation in the step (2) to obtain a filtered point cloud;
(4) performing k-nearest neighbor (KNN) search on the filtered point cloud; finding, in a local map, five nearest points to each point in the filtered point cloud for plane fitting; and if plane conditions are satisfied, computing a normal vector to a plane and computing a distance from the each point to the plane as a lidar residual;
(5) inputting the lidar residual obtained in the step (4) and the initial value of the IMU state obtained in the step (1) into an error-state iterated Kalman filter, to form a maximum a posteriori probability problem and solve the problem iteratively, wherein when an update amount is less than a set threshold, iteration convergence occurs and an optimal pose estimate of a current frame is output; and
(6) performing loop-closure frame detection based on the optimal pose estimate obtained in the step (5) and the filtered point cloud obtained in the step (3); performing loop-closure verification after a loop-closure frame is detected, wherein if a loop-closure is verified, a pose graph optimization is performed and an optimization result is updated into an odometry and a map, and if the loop-closure is invalid, no processing is performed; and outputting a global map when a robot task is completed.

### Claim 3 (depends on 2)

The SLAM method adapted to the external rotation 3D lidar device according to claim 2, wherein:
in the step (1), the IMU state is predicted by the following method:
setting state variables to be predicted to:
x=[GpIT GRIT GvITbωTbaT GgT]T
wherein GpIT, GRIT, and GvIT respectively represent a position, an attitude, and a velocity of the IMU in a global coordinate system G, bωT and baT respectively represent biases in an angular velocity and an acceleration of the IMU, and GgT represents a gravity vector in the global coordinate system G;
if noises in a state prediction process are set to 0, a state prediction formula being:
x
ˆ
m
+
1
=
x
ˆ
m
⊕
(
Δ
⁢
tg
⁡
(
x
ˆ
m
,
u
m
,
0
)
)
wherein {circumflex over (x)}m+1 represents roughly estimated state variables of an (m+1)th frame of the IMU, {circumflex over (x)}m represents roughly estimated state variables of an mth frame of the IMU, ⊕ represents a generalized addition operator, Δt represents a time difference between two adjacent frames of the IMU, and g({circumflex over (x)}m, um, 0) represents an increment function taking the roughly estimated state variables {circumflex over (x)}m of the mth frame of the IMU and data um of the mth frame of the IMU as inputs;
propagating a covariance by using an error-state dynamic model:
x
~
m
+
1
=
x
m
+
1
⊖
x
ˆ
m
+
1
=
(
x
m
⊕
Δ
⁢
tg
⁡
(
x
m
,
u
m
,
w
m
)
)
⊖
(
x
ˆ
m
⊕
Δ
⁢
tg
⁡
(
x
ˆ
m
,
u
m
,
0
)
)
wherein {tilde over (x)}m+1 represents differences between state variable true values {tilde over (x)}m+1 of the (m+1)th frame of the IMU and the roughly estimated state variables {circumflex over (x)}m+1 of the (m+1)th frame of the IMU, ⊕ represents a generalized subtraction operator, xm represents state variable true values of the mth frame of the IMU, and g(xm, um, wm) represents an increment function taking the state variable true values xm of the mth frame of the IMU, the data um of the mth frame of the IMU, and data noises wm of the mth frame of the IMU as inputs;
completing state prediction and obtaining an initial value of pose estimation, that is, the initial value of the IMU state.

### Claim 4 (depends on 2)

The SLAM method adapted to the external rotation 3D lidar device according to claim 2, wherein:
in the step (2), the nearest neighbor motion compensation is performed by the following method:
calculating amounts of compensation required for the position, the velocity, and the attitude:
I
n
p
ˇ
I
j
=
I
n
p
ˇ
j
-
1
+
I
n
v
ˇ
I
j
⁢
Δ
⁢
t
I
n
v
ˇ
I
j
=
I
n
v
ˇ
I
j
-
1
+
I
n
R
ˇ
I
j
⁢
(
a
f
m
-
1
-
b
ˆ
a
n
)
⁢
Δ
⁢
t
+
I
n
g
^
n
⁢
Δ
⁢
t
I
n
R
ˇ
I
j
=
I
n
R
ˇ
I
j
-
1
⁢
Exp
⁢
(
(
b
ˆ
ω
n
-
ω
f
m
-
1
)
⁢
Δ
⁢
t
)
T
wherein n=1, 2, 3, . . . , K, K representing a total number of frames of the point cloud of the lidar, j=1, 2, 3, . . . , N, N representing a total number of sampling points in each of the frames of the point cloud of the lidar, Inp̌Ij-1,Inp̌Ij respectively represent the amounts of the compensation required for the position from a (j−1)th and a jth point to an end of an nth frame of the point cloud in an IMU coordinate system, and Δt represents a time difference from the jth point to a nearest neighbor IMU frame; Inv̌Ij-1,Inv̌Ij respectively represent the amounts of the compensation required for the velocity from the (j−1)th and the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, afm-1 represents an acceleration input of an (m−1)th frame of the IMU, ban represents an acceleration bias at the end of the nth frame of the point cloud, and Inĝn represents a gravity vector at the end of the nth frame of the point cloud in the IMU coordinate system; InŘIj-1,InŘIj respectively represent the amounts of the compensation required for the attitude from the (j−1)th and the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, bon represents an angular velocity bias at the end of the nth frame of the point cloud, and ωfm-1 represents an angular velocity input of the (m−1)th frame of the IMU;
a compensated point being:
L
n
p
h
j
=
I
T
L
-
1
⁢
I
n
T
ˇ
I
j
I
⁢
T
L
L
j
⁢
p
h
j
where Lnphj represents a position of the jth point after being compensated to the end of the nth frame of the point cloud, that is, the compensated point, In{circumflex over (T)}Ij represents an amount of the compensation required from the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, ITL represents a transformation from a lidar coordinate system to the IMU coordinate system, and LjPhj represents a position of the jth point in the lidar coordinate system, that is, an uncompensated point.

### Claim 5 (depends on 4)

The SLAM method adapted to the external rotation 3D lidar device according to claim 4, wherein:
in the step (3), the filtering of the point cloud obtained by the nearest neighbor motion-compensation is conducted by the following method:
employing minimal preprocessing with two filters, namely, a box filter and a downsampling voxel filter, wherein the box filter filters out point clouds within a cubic range around a device platform; and the downsampling voxel filter downsamples collected environment point clouds and sets a voxel grid size for screening to reduce a number of point clouds to be processed while maintaining a main structure of a surrounding environment.

### Claim 6 (depends on 2)

The SLAM method adapted to the external rotation 3D lidar device according to claim 2, wherein:
in the step (4), a lidar residual formula is:
z
j
i
=
U
j
(
G
T
^
I
n
i
⁢
I
T
L
L
n
⁢
p
h
j
-
G
q
j
)
wherein i=1, 2, 3, . . . , M, M representing a total number of iterations, zji represents a residual of a jth point in an ith iteration, Ui represents a normal vector to a fitted plane, G{circumflex over (T)}Ini represents a pose of the IMU at an end of an nth frame of the point cloud in the ith iteration, ITL represents a transformation from a lidar coordinate system to an IMU coordinate system, Lnphj represents a compensated point, and Gqj represents coordinates of one point in a corresponding plane of the jth point in a global coordinate system.

### Claim 7 (depends on 2)

The SLAM method adapted to the external rotation 3D lidar device according to claim 2, wherein:
in the step (5), the maximum a posteriori probability problem is:
min
⁡
(
∑
j
=
1
r

z
j
i
+
H
j
i
⁢
x
~
n
i

R
j
-
1
2
+

x
n
⊖
x
ˆ
n

P
ˆ
n
-
1
2
)
wherein r represents a number of points in a frame of the filtered point cloud, Hji represents a Jacobian of a residual corresponding to a jth point in an ith iteration, {circumflex over (x)}ni represents a state error amount at an end of an nth frame of the point cloud in the ith iteration, Rj represents a measured variance of the original point cloud, xn and {circumflex over (x)}n respectively represent true values and estimated values of the state variables at the end of the nth frame of the point cloud, and {circumflex over (P)}n represents a noise covariance propagated to the end of the nth frame of the point cloud.

### Claim 8 (depends on 2)

The SLAM method adapted to the external rotation 3D lidar device according to claim 2, wherein:
in the step (6), the loop-closure frame detection is performed by the following method:
judging similarity between two frames of a laser point cloud by means of depth map matching, generating different feature expressions for the two frames of the point cloud based on heights of highest point clouds in different grids, and computing similarity between different feature graphs after feature graph description of each of the two frames of the point cloud is obtained;
a vector distance between two feature graphs being:
d
⁡
(
c
k
i
,
c
k
j
)
=
c
k
i
·
c
k
j

c
k
i

⁢

c
k
j

wherein cki and ckj respectively represent kth column vectors in the feature graphs i and j;
averaging distances of all the column vectors to obtain a similarity function of the two feature graphs:
d
⁡
(
F
i
,
F
j
)
=
1
N
s
⁢
∑
k
=
1
N
s
(
1
-
c
k
i
·
c
k
j

c
k
i

⁢

c
k
j

)
wherein Fi and Fj respectively represent the two feature graphs i and j, and Ns represents a number of the column vectors in the feature graphs;
using a maximum value of the similarity function of a first one of the feature graphs and a rotated graph of a second one of the feature graphs as a final similarity function, that is:
D
⁡
(
F
i
,
F
j
)
=
min
n
∈
[
N
s
]
⁢
d
⁡
(
F
i
,
F
n
j
)
wherein Fnj represents a new feature graph formed by moving first n columns of the feature graph Fj to an end of the feature graph Fj, and a point cloud frame with a highest similarity exceeding a similarity threshold is used as the loop-closure frame according to a computation result of the final similarity function.

### Claim 9 (depends on 8)

The SLAM method adapted to the external rotation 3D lidar device according to claim 8, wherein:
whether the loop-closure is valid is verified by the following method: when the loop-closure frame is detected by loop-closure detection, performing a nearest neighbor search on a current frame to search for point cloud frames within a range of 12 m, and arranging the point cloud frames according to distances in ascending order, wherein if an id number of the loop-closure frame is among first 10 numbers, that is, a first condition, and a timestamp of the loop-closure frame is more than 25 s from a current time point, that is, a second condition, the loop-closure frame is considered accurate and pose graph optimization is performed; while if the first condition and the second condition are not satisfied at the same time, the loop-closure frame is considered invalid and no processing is performed.

### Claim 10 (depends on 8)

The SLAM method adapted to the external rotation 3D lidar device according to claim 8, wherein:
the pose graph optimization is performed by the following method:
solving loop-closure constraints formed after verification, projecting feature points of k point cloud frames near the loop-closure frame into a coordinate system at a time point of the loop-closure frame to obtain a local map in the coordinate system at this time point, and establishing point-to-line residual and point-to-plane residual equations to form a least squares optimization problem regarding relative pose transformation of the current frame in the coordinate system of the loop-closure frame:
min
⁡
(
∑
k
∈
ℒ
ε
ρ
⁡
(

r
ℒ
s
(
p
k
,
T
i
j
)

p
I
k
2
)
+
∑
k
∈
ℒ
ℋ
ρ
⁡
(

r
ℒ
ℋ
(
p
k
,
T
i
j
)

p
I
k
2
)
)
wherein ε and respectively represent a number of local line features and a number of local plane features, rε(pK, Tij) represents point-to-line residual constraints on pose transformation between matched line feature points pk and two key frames i and j, (pK, Tij) represents point-to-plane residual constraints on pose transformation between matched plane feature points pk and the two key frames i and j, and
ρ
⁡
(


p
I
k
2
)
represents a distance variance; and
solving the least squares optimization problem by using a Levenberg-Marquardt (LM) method to obtain relative pose transformation between two loop-closure frames, updating a pose optimization result into an odometry output, and updating a map based on the pose optimization result to complete an optimization process.

### Claim 11 (depends on 3)

The SLAM method adapted to the external rotation 3D lidar device according to claim 3, wherein:
in the step (2), the nearest neighbor motion compensation is performed by the following method:
calculating amounts of compensation required for the position, the velocity, and the attitude:
I
n
p
ˇ
I
j
=
I
n
p
ˇ
j
-
1
+
I
n
v
ˇ
I
j
⁢
Δ
⁢
t
I
n
v
ˇ
I
j
=
I
n
v
ˇ
I
j
-
1
+
I
n
R
ˇ
I
j
⁢
(
a
f
m
-
1
-
b
ˆ
a
n
)
⁢
Δ
⁢
t
+
I
n
g
^
n
⁢
Δ
⁢
t
I
n
R
ˇ
I
j
=
I
n
R
ˇ
I
j
-
1
⁢
Exp
⁢
(
(
b
ˆ
ω
n
-
ω
f
m
-
1
)
⁢
Δ
⁢
t
)
T
wherein n=1, 2, 3, . . . , K, K representing a total number of frames of the point cloud of the lidar, j=1, 2, 3, . . . , N, N representing a total number of sampling points in each of the frames of the point cloud of the lidar, Inp̌Ij-1,Inp̌Ij respectively represent the amounts of the compensation required for the position from a (j−1)th and a jth point to an end of an nth frame of the point cloud in an IMU coordinate system, and Δt represents a time difference from the jth point to a nearest neighbor IMU frame; Inv̌Ij-1,Inv̌Ij respectively represent the amounts of the compensation required for the velocity from the (j−1)th and the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, afm-1 represents an acceleration input of an (m−1)th frame of the IMU, ban represents an acceleration bias at the end of the nth frame of the point cloud, and Inĝn represents a gravity vector at the end of the nth frame of the point cloud in the IMU coordinate system; InŘIj-1,InŘIj respectively represent the amounts of the compensation required for the attitude from the (j−1)th and the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, bon represents an angular velocity bias at the end of the nth frame of the point cloud, and ωfm-1 represents an angular velocity input of the (m−1)th frame of the IMU;
a compensated point being:
L
n
p
h
j
=
I
T
L
-
1
⁢
I
n
T
ˇ
I
j
I
⁢
T
L
L
j
⁢
p
h
j
where Lnphj represents a position of the jth point after being compensated to the end of the nth frame of the point cloud, that is, the compensated point, In{circumflex over (T)}Ij represents an amount of the compensation required from the jth point to the end of the nth frame of the point cloud in the IMU coordinate system, ITL represents a transformation from a lidar coordinate system to the IMU coordinate system, and LjPhj represents a position of the jth point in the lidar coordinate system, that is, an uncompensated point.

### Claim 12 (depends on 9)

The SLAM method adapted to the external rotation 3D lidar device according to claim 9, wherein:
the pose graph optimization is performed by the following method:
solving loop-closure constraints formed after verification, projecting feature points of k point cloud frames near the loop-closure frame into a coordinate system at a time point of the loop-closure frame to obtain a local map in the coordinate system at this time point, and establishing point-to-line residual and point-to-plane residual equations to form a least squares optimization problem regarding relative pose transformation of the current frame in the coordinate system of the loop-closure frame:
min
⁡
(
∑
k
∈
ℒ
ε
ρ
⁡
(

r
ℒ
s
(
p
k
,
T
i
j
)

p
I
k
2
)
+
∑
k
∈
ℒ
ℋ
ρ
⁡
(

r
ℒ
ℋ
(
p
k
,
T
i
j
)

p
I
k
2
)
)
wherein ε and respectively represent a number of local line features and a number of local plane features, rε(pk, Tij) represents point-to-line residual constraints on pose transformation between matched line feature points pk and two key frames i and j, (pk, Tij) represents point-to-plane residual constraints on pose transformation between matched plane feature points pk and the two key frames i and j, and
ρ
⁡
(


p
I
k
2
)
represents a distance variance; and
solving the least squares optimization problem by using a Levenberg-Marquardt (LM) method to obtain relative pose transformation between two loop-closure frames, updating a pose optimization result into an odometry output, and updating a map based on the pose optimization result to complete an optimization process.
