# US 2022/0035036 A1 · Method and apparatus for positioning movable device, and movable device

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

- Number: US 2022/0035036 A1
- Document ID: US20220035036A1
- Status: Published application
- Kind: A1
- Application number: 17/386,335
- Filed: 2021-07-27
- Published: 2022-02-03
- Earliest claimed: 2020-07-29
- Assignees: Beijing Tusen Weilai Technology Co., Ltd.
- Applicants: Beijing Tusen Weilai Technology Co., Ltd.
- Inventors: Yong XU; Nan WU
- Web page: https://patentgrep.com/patent/US20220035036A1

## CPC

- G01S 13/86: Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
- G01S 17/89: for mapping or imaging
- G01C 21/20: Instruments for performing navigational calculations (G01C21/24, G01C21/26 take precedence)
- G01S 7/4808: {Evaluating distance, position or velocity data}
- G01S 13/931: of land vehicles
- 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/931: of land vehicles
- G06F 18/21345: {enforcing sparsity or involving a domain transformation}
- G06F 18/22: Matching criteria, e.g. proximity measures
- G06V 20/588: {Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road}
- G06V 20/653: {by matching three-dimensional models, e.g. conformal mapping of Riemann surfaces}
- G01S 13/874: {Combination of several systems for attitude determination (in general G01C, control of attitude G05D1/49)}
- G01S 17/06: Systems determining position data of a target
- G01S 2013/9318: {Controlling the steering}
- 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
- 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)
- G06F: ELECTRIC DIGITAL DATA PROCESSING (computer systems based on specific computational models G06N)
- G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING

## Abstract

The present disclosure relates to autonomous driving technology, and provides a method and an apparatus for positioning a movable device, as well as a movable device. The method includes: obtaining point cloud data for a predetermined area above the movable device; extracting, from the point cloud data, a first type of point cloud and a second type of point cloud on a left side and a right side of the movable device, respectively; matching the first type of point cloud and the second type of point cloud to obtain a transform matrix; and determining pose information of the movable device based on the transform matrix. With the above process, the present disclosure can solve the problem in the related art associated with accurate positioning of a movable device when GNSS signals are affected and it is difficult to accurately obtain point cloud data in front of the movable device.

## Claims

### Claim 1

A method for positioning a movable device moving in a tunnel with a bilaterally symmetrical cross section, the method comprising:
obtaining point cloud data for a predetermined area above the movable device;
extracting, from the point cloud data, a first type of point cloud and a second type of point cloud on a left side and a right side of the movable device, respectively, in accordance with a predetermined rule;
matching the first type of point cloud and the second type of point cloud to obtain a transform matrix; and
determining pose information of the movable device based on the transform matrix.

### Claim 2 (depends on 1)

The method of claim 1, wherein the movable device is provided with a lidar having a scanning plane perpendicular to, or having an angle with, a movement plane of the movable device, and
said obtaining the point cloud data for the predetermined area above the movable device comprises:
obtaining point cloud data of a predetermined area at a top of the tunnel above the movable device using the lidar.

### Claim 3 (depends on 1)

The method of claim 1, wherein the predetermined area is an area having a predetermined distance from a reference point of the movable device.

### Claim 4 (depends on 1)

The method of claim 1, wherein said extracting, from the point cloud data, the first type of point cloud and the second type of point cloud on the left side and the right side of the movable device, respectively, in accordance with the predetermined rule comprises:
obtaining reference point coordinates of the reference point of the movable device and point cloud coordinates of the point cloud data in a predetermined coordinate system; and
dividing the point clouds into the first type of point cloud and the second type of point cloud based on the reference point coordinates and the point cloud coordinates.

### Claim 5 (depends on 4)

The method of claim 4, wherein said matching the first type of point cloud and the second type of point cloud to obtain the transform matrix and determining the pose information of the movable device based on the transform matrix comprises:
matching the first type of point cloud and the second type of point cloud to obtain the transform matrix; and
determining a lateral position and a yaw of the movable device based on the transform matrix.

### Claim 6 (depends on 5)

The method of claim 5, wherein said matching the first type of point cloud and the second type of point cloud to obtain the transform matrix comprises:
obtaining a first number of point clouds of the first type and a second number of point clouds of the second type, and determining which type of point cloud has a larger number of point clouds and which type of point cloud has a smaller number of point clouds;
preprocessing the point clouds with a smaller number of point clouds; and
matching the preprocessed point clouds with the point clouds with the larger number of point clouds using a predetermined registration algorithm to obtain the transform matrix including a translation vector, and
said determining the lateral position of the movable device based on the transform matrix comprises:
determining the lateral position of the movable device based on the translation vector.

### Claim 7 (depends on 6)

The method of claim 6, wherein said preprocessing the point clouds with a smaller number of point clouds comprises:
forming the preprocessed point clouds by rotating the point clouds with the smaller number of point clouds around a Z-axis of the predetermined coordinate system by 180 degrees, wherein the predetermined coordinate system includes an X-axis, a Y-axis, and the Z-axis, where the X-axis is a lateral direction of the movable device, the Y-axis is a longitudinal direction of the movable device, and the Z-axis is perpendicular to a plane where the X-axis and the Y-axis are located,
said matching the preprocessed point clouds with the point clouds with the larger number of point clouds using the predetermined registration algorithm to obtain the transform matrix comprises:
matching the preprocessed point clouds with the point clouds with the larger number of point clouds using an Iterative Closest Point (ICP) algorithm or a Normal Distributions Transform (NDT) algorithm to obtain the transform matrix, the transform matrix including the translation vector t(C, m, n), where C is a displacement in the X-axis direction in the predetermined coordinate system when the point clouds are matched, m is a displacement in the Y-axis direction in the predetermined coordinate system when the point clouds are matched, and n is a displacement in the Z-axis direction in the predetermined coordinate system when the point clouds are matched, and
said determining the lateral position of the movable device based on the translation vector comprises:
determining a lateral distance of the movable device from a center line of the tunnel as

C

2
based on a translation distance |C| along the X-axis in the predetermined coordinate system when the point clouds are matched.

### Claim 8 (depends on 7)

The method of claim 7, wherein said determining the yaw of the movable device based on the transform matrix comprises:
determining a first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation vector in the transform matrix;
selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, a target point cloud having Z-axis coordinates within a predetermined range from a Z-axis coordinate value of the first vertex based on coordinates of the first vertex of the tunnel in the predetermined coordinate system;
determining a straight line equation of a straight line based on the target point cloud, the straight line equation containing one or more parameters to be solved;
determining a cost function for all points in the target point cloud relative to the straight line;
solving the cost function, and determining one or more results of the one or more parameters to be solved when a cost value of the cost function is minimized; and
determining the yaw of the movable device based on the one or more results of the one or more parameters to be solved.

### Claim 9 (depends on 8)

The method of claim 8, wherein said determining the first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation vector in the transform matrix comprises:
determining the first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation distance |C| along the X-axis in the predetermined coordinate system when the point clouds are matched, wherein the first vertex has an X-axis coordinate of
-

C

2
and a Y-axis coordinate of 0 in the predetermined coordinate system, or the first vertex is a point p0 (x0, y0, z0) in the point cloud data that has a minimum distance from a point with an X-axis coordinate of
-

C

2
and a Y-axis coordinate of 0,
said selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, the target point cloud having the Z-axis coordinate within the predetermined range from the Z-axis coordinate value of the first vertex based on the coordinates of the first vertex of the tunnel in the predetermined coordinate system comprises:
selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, the target point cloud having Z-axis coordinates within a predetermined range of (z0−d, z0+d) based on the coordinates of the first vertex of the tunnel in the predetermined coordinate system, where d is a predetermined distance threshold and z0 is the Z-axis coordinate of the first vertex,
said determining the straight line equation of the straight line based on the target point cloud, the straight line equation containing one or more parameters to be solved, comprises:
determining the straight line equation as a(x−x0)+b(y−y0)=0 based on to the target point cloud, where a and b are the parameters to be solved,
said determining the cost function for all points in the target point cloud relative to the straight line comprises:
determining the cost function for all points (xi, yi, zi) in the target point cloud relative to the straight line as:
cost
=
∑
(
a
⁡
(
x
⁢
i
-
x
⁢
0
)
+
b
⁡
(
y
⁢
i
-
y
⁢
0
)
)
2
a
2
+
b
2
,
said solving the cost function and determining the one or more results of the one or more parameters to be solved when the cost value of the cost function is minimized comprises:
obtaining a minimum value optimized solution of the cost function to determine the results a0 and b0 of the parameters to be solved when the cost value of the cost function is minimized, and
said determining the yaw of the movable device based on the one or more results of the one or more parameters to be solved comprises:
determining the yaw of the movable device relative to the tunnel as
arctan
⁢
b
⁢
0
a
⁢
0
.

### Claim 10

A non-transitory computer readable storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the method for positioning a movable device moving in a tunnel with a bilaterally symmetrical cross section, the mothed comprising:
obtaining point cloud data for a predetermined area above the movable device;
extracting, from the point cloud data, a first type of point cloud and a second type of point cloud on a left side and a right side of the movable device, respectively, in accordance with a predetermined rule;
matching the first type of point cloud and the second type of point cloud to obtain a transform matrix; and
determining pose information of the movable device based on the transform matrix.

### Claim 11 (depends on 10)

The non-transitory computer readable storage medium of claim 10, wherein the movable device is provided with a lidar having a scanning plane perpendicular to, or having an angle with, a movement plane of the movable device, and
said obtaining the point cloud data for the predetermined area above the movable device comprises:
obtaining point cloud data of a predetermined area at a top of the tunnel above the movable device using the lidar.

### Claim 12

A computer device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor is configured to, when executing the program, perform the method for positioning a movable device moving in a tunnel with a bilaterally symmetrical cross section, the mothed comprising:
obtaining point cloud data for a predetermined area above the movable device;
extracting, from the point cloud data, a first type of point cloud and a second type of point cloud on a left side and a right side of the movable device, respectively, in accordance with a predetermined rule;
matching the first type of point cloud and the second type of point cloud to obtain a transform matrix; and
determining pose information of the movable device based on the transform matrix.

### Claim 13 (depends on 12)

The computer device of claim 12, wherein the movable device is provided with a lidar having a scanning plane perpendicular to, or having an angle with, a movement plane of the movable device, and
said obtaining the point cloud data for the predetermined area above the movable device comprises:
obtaining point cloud data of a predetermined area at a top of the tunnel above the movable device using the lidar.

### Claim 14 (depends on 12)

The computer device of claim 12, wherein the predetermined area is an area having a predetermined distance from a reference point of the movable device.

### Claim 15 (depends on 14)

The computer device of claim 14, wherein said extracting, from the point cloud data, the first type of point cloud and the second type of point cloud on the left side and the right side of the movable device, respectively, in accordance with the predetermined rule comprises:
obtaining reference point coordinates of the reference point of the movable device and point cloud coordinates of the point cloud data in a predetermined coordinate system; and
dividing the point clouds into the first type of point cloud and the second type of point cloud based on the reference point coordinates and the point cloud coordinates.

### Claim 16 (depends on 15)

The computer device of claim 15, wherein said matching the first type of point cloud and the second type of point cloud to obtain the transform matrix and determining the pose information of the movable device based on the transform matrix comprises:
matching the first type of point cloud and the second type of point cloud to obtain the transform matrix; and
determining a lateral position and a yaw of the movable device based on the transform matrix.

### Claim 17 (depends on 16)

The computer device of claim 16, wherein said matching the first type of point cloud and the second type of point cloud to obtain the transform matrix comprises:
obtaining a first number of point clouds of the first type and a second number of point clouds of the second type, and determining which type of point cloud has a larger number of point clouds;
preprocessing the point clouds with a smaller number of point clouds; and
matching the preprocessed point clouds with the point clouds with the larger number of point clouds using a predetermined registration algorithm to obtain the transform matrix including a translation vector, and
said determining the lateral position of the movable device based on the transform matrix comprises:
determining the lateral position of the movable device based on the translation vector.

### Claim 18 (depends on 17)

The computer device of claim 17, wherein said preprocessing the point clouds with a smaller number of point clouds comprises:
forming the preprocessed point clouds by rotating the point clouds with the smaller number of point clouds around a Z-axis of the predetermined coordinate system by 180 degrees, wherein the predetermined coordinate system includes an X-axis, a Y-axis, and the Z-axis, where the X-axis is a lateral direction of the movable device, the Y-axis is a longitudinal direction of the movable device, and the Z-axis is perpendicular to a plane where the X-axis and the Y-axis are located,
said matching the preprocessed point clouds with the point clouds with the larger number of point clouds using the predetermined registration algorithm to obtain the transform matrix comprises:
matching the preprocessed point clouds with the point clouds with the larger number of point clouds using an Iterative Closest Point (ICP) algorithm or a Normal Distributions Transform (NDT) algorithm to obtain the transform matrix, the transform matrix including the translation vector t(C, m, n), where C is a displacement in the X-axis direction in the predetermined coordinate system when the point clouds are matched, m is a displacement in the Y-axis direction in the predetermined coordinate system when the point clouds are matched, and n is a displacement in the Z-axis direction in the predetermined coordinate system when the point clouds are matched, and
said determining the lateral position of the movable device based on the translation vector comprises:
determining a lateral distance of the movable device from a center line of the tunnel as

C

2
based on a translation distance |C| along the X-axis in the predetermined coordinate system when the point clouds are matched.

### Claim 19 (depends on 18)

The computer device of claim 18, wherein said determining the yaw of the movable device based on the transform matrix comprises:
determining a first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation vector in the transform matrix;
selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, a target point cloud having Z-axis coordinates within a predetermined range from a Z-axis coordinate value of the first vertex based on coordinates of the first vertex of the tunnel in the predetermined coordinate system;
determining a straight line equation of a straight line based on the target point cloud, the straight line equation containing one or more parameters to be solved;
determining a cost function for all points in the target point cloud relative to the straight line;
solving the cost function, and determining one or more results of the one or more parameters to be solved when a cost value of the cost function is minimized; and
determining the yaw of the movable device based on the one or more results of the one or more parameters to be solved.

### Claim 20 (depends on 19)

The computer device of claim 19, wherein said determining the first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation vector in the transform matrix comprises:
determining the first vertex of the tunnel from the point cloud data of the predetermined area at the top of the tunnel above the movable device based on the translation distance |C| along the X-axis in the predetermined coordinate system when the point clouds are matched, wherein the first vertex has an X-axis coordinate of
-

C

2
and a Y-axis coordinate of 0 in the predetermined coordinate system, or the first vertex is a point p0 (x0, y0, z0) in the point cloud data that has a minimum distance from a point with an X-axis coordinate of
-

C

2
and a Y-axis coordinate of 0,
said selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, the target point cloud having the Z-axis coordinate within the predetermined range from the Z-axis coordinate value of the first vertex based on the coordinates of the first vertex of the tunnel in the predetermined coordinate system comprises:
selecting, from the point cloud data of the predetermined area at the top of the tunnel above the movable device, the target point cloud having Z-axis coordinates within a predetermined range of (z0−d, z0+d) based on the coordinates of the first vertex of the tunnel in the predetermined coordinate system, where d is a predetermined distance threshold and z0 is the Z-axis coordinate of the first vertex,
said determining the straight line equation of the straight line based on the target point cloud, the straight line equation containing one or more parameters to be solved, comprises:
determining the straight line equation as a(x−x0)+b(y−y0)=0 based on to the target point cloud, where a and b are the parameters to be solved,
said determining the cost function for all points in the target point cloud relative to the straight line comprises:
determining the cost function for all points (xi, yi, zi) in the target point cloud relative to the straight line as:
cost
=
∑
(
a
⁡
(
x
⁢
i
-
x
⁢
0
)
+
b
⁡
(
y
⁢
i
-
y
⁢
0
)
)
2
a
2
+
b
2
,
said solving the cost function and determining the one or more results of the one or more parameters to be solved when the cost value of the cost function is minimized comprises:
obtaining a minimum value optimized solution of the cost function to determine the results a0 and b0 of the parameters to be solved when the cost value of the cost function is minimized, and
said determining the yaw of the movable device based on the one or more results of the one or more parameters to be solved comprises:
determining the yaw of the movable device relative to the tunnel as
arctan
⁢
b
⁢
0
a
⁢
0
.
