US 12,353,219 B2
Method and system for path planning of unmanned vehicle in three-dimensional terrain
Jun Luo, Chongqing (CN); Lele Ding, Chongqing (CN); Huayan Pu, Chongqing (CN); Jie Ma, Chongqing (CN); Xuyang Zheng, Chongqing (CN); Hongliang Liu, Chongqing (CN); Jing Huang, Chongqing (CN); Yongbing Chen, Chongqing (CN); and Anming Shen, Chongqing (CN)
Assigned to Chongqing University, Chongqing (CN); and Shanghai University, Shanghai (CN)
Filed by CHONGQING UNIVERSITY, Chongqing (CN); and SHANGHAI UNIVERSITY, Shanghai (CN)
Filed on Jan. 13, 2023, as Appl. No. 18/096,587.
Claims priority of application No. 202210422548.X (CN), filed on Apr. 21, 2022.
Prior Publication US 2023/0341869 A1, Oct. 26, 2023
Int. Cl. G05D 1/00 (2024.01)
CPC G05D 1/0274 (2013.01) 4 Claims
OG exemplary drawing
 
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

OG Complex Work Unit Math
 wherein p is a goal bias probability: p1 and p2 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.