Method and system for path planning of unmanned vehicle in three-dimensional terrain

Patent metadata

Field	Value
Publication number	US-12353219-B2
Application number	US-202318096587-A
Country	US
Kind code	B2
Filing date	Jan 13, 2023
Priority date	Apr 21, 2022
Publication date	Jul 8, 2025
Grant date	Jul 8, 2025

Abstract

The present disclosure provides a method and system for path planning of an unmanned vehicle in a three-dimensional terrain and relates to the technical field of path planning. The method includes building a random tree with an initial point of a to-be-planned path as a node; generating a random node based on a goal bias strategy and a multi-sampling strategy; determining a node in the random tree and with a minimum two-dimensional distance from the random node as a nearest node; determining a direction from the nearest node to the random node as an extension direction; determining a point corresponding to a preset step length as a to-be-determined node in the extension direction with the nearest node as a starting point; and updating the random tree or determining a path cut-off random tree through elevation detection and path search cut-off detection, so as to determine the to-be-planned path.

First claim

What is claimed is:

1. A method for path planning of an unmanned vehicle in a three-dimensional terrain, comprising:
acquiring a map of a path planning region, and an initial point and a target point of a to-be-planned path, wherein the map comprises two-dimensional coordinates and an elevation value of each point in the path planning region;
building a random tree with the initial point as a node;
generating a random node based on a goal bias strategy and a multi-sampling strategy,
wherein the generating a random node based on a goal bias strategy and a multi-sampling strategy specifically comprises:
randomly obtaining a goal bias probability based on uniform probability distribution and formula

p
=

p
1

+

p
2

(

1
-

a
b

)

,

 wherein p is a goal bias probability: p 1 and p 2 are constants, a is a minimum distance between the random tree and the target point after random node expansion ends, and b is a two-dimensional straight-line distance between the initial point and the target point;
determining whether the goal bias probability is less than a goal bias probability threshold to obtain a first determining result; and
determining the target point as a random node if the first determining result is yes; or
determining one of any two points that is in the path planning region and is closest to the target point as the random node if the first determining result is no;
determining a node in the random tree and with a minimum two-dimensional distance from the random node as a nearest node;
determining a direction from the nearest node to the random node as an extension direction;
determining a point corresponding to a preset step length as a to-be-determined node in the extension direction with the nearest node as a starting point;
performing elevation detection on the to-be-determined node based on the elevation value of the to-be-determined node and the elevation value of the nearest node;
adding the to-be-determined node that passes the elevation detection to the random tree as a child node of the nearest node, and determining the child node of the nearest node as a nearest updated node;
performing path search cut-off detection on the nearest updated node;
returning to the step of generating a random node based on a goal bias strategy and a multi-sampling strategy when the nearest updated node does not pass the path search cut-off detection;
determining, when the nearest updated node passes the path search cut-off detection, the target point as a child node of the nearest updated node, and adding the child node of the nearest updated node to the random tree, to obtain a path cut-off random tree;
constructing the to-be-planned path based on the path cut-off random tree; and
controlling a robot to perform a task according to the to-be-planned path.

2. The method for path planning of an unmanned vehicle in a three-dimensional terrain according to claim 1 , wherein the performing elevation detection on the to-be-determined node based on the elevation value of the to-be-determined node and the elevation value of the nearest node specifically comprises:
determining whether the elevation value of the to-be-determined node is less than an elevation threshold to obtain a second determining result,
determining that the to-be-determined node does not pass the elevation detection if the second determining result is no; or
determining whether an absolute value of a difference between the elevation values of the to-be-determined node and the nearest node is less than an elevation difference threshold, to obtain a third determining result if the second determining result is yes;
determining that the to-be-determined node does not pass the elevation detection if the third determining result is no; or
inserting a plurality of virtual nodes at equal intervals between the to-be-determined node and the nearest node if the third determining result is yes;
determining whether an absolute value of a difference between the elevation values of any adjacent virtual nodes is greater than or equal to the elevation difference threshold to obtain a fourth determining result; and
determining that the to-be-determined node does not pass the elevation detection if the fourth determining result is yes; or
determining that the to-be-determined node passes the elevation detection if the fourth determining result is no.

3. The method for path planning of an unmanned vehicle in a three-dimensional terrain according to claim 2 , wherein after the determining that the to-be-determined node does not pass the elevation detection, the method further comprises:
deflecting the extension direction clockwise and anticlockwise by a preset angle with the nearest node as a rotation center, to obtain a first alternative direction and a second alternative direction;
determining a point corresponding to a preset step length as a first alternative node in the first alternative direction with the nearest node as a starting point;
determining a point corresponding to a preset step length as a second alternative node in the second alternative direction with the nearest node as a starting point;
performing elevation detection on the first alternative node and the second alternative node;
determining whether the two alternative nodes pass the elevation detection to obtain a fifth determining result, wherein the alternative node is the first alternative node or the second alternative node;
determining, if the fifth determining result is yes, one of the two alternative nodes with a minimum absolute value of a difference between the elevation values of the alternative node and the nearest node as a to-be-determined node passing the elevation detection, and returning to the step of performing path search cut-off detection on the to-be-determined node passing the elevation detection; or
determining whether neither of the two alternative nodes passes the elevation detection if the fifth determining result is no, to obtain a sixth determining result; and
returning to the step of generating a random node based on a goal bias strategy and a multi-sampling strategy if the sixth determining result is yes; or
determining, if the sixth determining result is no, the alternative node passing the elevation detection as a to-be-determined node passing the elevation detection, and returning to the step of performing path search cut-off detection on the to-be-determined node passing the elevation detection.

4. The method for path planning of an unmanned vehicle in a three-dimensional terrain according to claim 1 , wherein the performing path search cut-off detection on the nearest updated node specifically comprises:
determining whether a two-dimensional distance between the nearest updated node and the target point is less than a distance threshold to obtain a seventh determining result;
determining that the nearest updated node does not pass the path search cut-off detection if the seventh determining result is no; or
performing elevation detection on the target point based on the elevation v

Assignees

• Univ Chongqing
• Univ Shanghai

Inventors

• Luo Jun
• Ding Lele
• Pu Huayan
• Ma Jie
• Zheng Xuyang
• Liu Hongliang
• Huang Jing
• Chen Yongbing
• Shen Anming

Classifications

• B60W60/001

  Planning or execution of driving tasks · CPC title

• G08G5/74

  for monitoring terrain · CPC title

• G08G5/32

  for flight plan preparation · CPC title

• G01C21/20

  Instruments for performing navigational calculations (G01C21/24, G01C21/26 take precedence) · CPC title

• G05D1/0214

  in accordance with safety or protection criteria, e.g. avoiding hazardous areas (monitoring the location of vehicles within a certain area, e.g. forbidden or allowed areas, in traffic control systems for road vehicles G08G1/13) · CPC title