一种三维地形下无人车路径规划方法及系统

1.本发明涉及路径规划技术领域，特别是涉及一种三维地形下无人车路径规划方法及系统。


背景技术：

2.近年来，机器人技术的飞速发展，给各行各业都带来很大的便利，以野外敌情侦查、抢险救灾为例，机器人因具有机动性强、通过电池补给可以连续工作、辅助或替代救援侦查人员，降低安全风险等优点而被广泛应用。而如何在复杂地形下规划一条安全、无碰撞的路径是对机器人自身安全甚至整项工作的前提。
3.目前，关于移动机器人路径规划算法层出不穷，例如图搜索法、仿生智能算法、可视图法等等，而在1998年lavalle提出的基于采样的快速随机搜索树算法(rapidly-exploring randomtree，rrt)因具有概率完备性、算法结构简单、在复杂环境中搜索能力强而被广泛使用。但目前大多数有关rrt算法的研究都是基于二维障碍物空间，这与大多数机器人的应用场合都不相符合，比如军用移动机器人在进行侦查时需要根据地形和敌情挑选出一条渐进最优的路线。因此，rrt算法在三维地形下的路径规划问题是亟需解决的问题。


技术实现要素：

4.本发明的目的是提供一种三维地形下无人车路径规划方法及系统，能够完成在三维地形下的路径规划，提高路径规划合理性。
5.为实现上述目的，本发明提供了如下方案：
6.一种三维地形下无人车路径规划方法，包括：
7.获取路径规划区域的地图，以及待规划路径的起始点和目标点；所述地图包括所述路径规划区域上每个点的二维坐标和高程值；
8.将所述起始点作为节点构建随机树；
9.基于目标偏置策略和多点采样策略生成随机节点；
10.确定所述随机树中与所述随机节点的二维距离最小的节点为最近节点；
11.确定所述最近节点到所述随机节点的方向为延展方向；
12.以所述最近节点为起点，沿所述延展方向，确定预设步长处对应的点为待定节点；
13.根据所述待定节点的高程值和所述最近节点的高程值，对所述待定节点进行高程检测；
14.将通过所述高程检测的待定节点作为所述最近节点的子节点添加至所述随机树中，并确定所述最近节点的子节点为最近更新节点；
15.对所述最近更新节点进行路径搜索截止检测；
16.在所述最近更新节点未通过所述路径搜索截止检测时返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
17.在所述最近更新节点通过所述路径搜索截止检测时，确定所述目标点为所述最近
更新节点的子节点，并将所述最近更新节点的子节点添加至所述随机树中，得到路径截止随机树；
18.根据所述路径截止随机树构建所述待规划路径。
19.可选的，所述基于目标偏置策略和多点采样策略生成随机节点，具体包括：
20.按照均匀概率分布，根据公式随机获取一个目标偏置概率；其中，p为目标偏置概率；p1和p2均为常数，a为随机节点扩展结束后随机树与目标点的最近距离，b为起始点和目标点质检的二维直线距离；
21.判断所述目标偏置概率是否小于目标偏置概率阈值，得到第一判断结果；
22.若第一判断结果为是，则确定目标点为随机节点；
23.若第一判断结果为否，则确定路径规划区域中任意两个点中距离所述目标点最近的一个点为所述随机节点。
24.可选的，所述根据所述待定节点的高程值和所述最近节点的高程值，对所述待定节点进行高程检测具体包括：
25.判断所述待定节点的高程值是否小于高程阈值，得到第二判断结果；
26.若所述第二判断结果为否，则所述待定节点未通过高程检测；
27.若所述第二判断结果为是，则判断所述待定节点与所述最近节点的高程值之差的绝对值是否小于高程差阈值，得到第三判断结果；
28.若所述第三判断结果为否，则所述待定节点未通过高程检测；
29.若所述第三判断结果为是，则在所述待定节点和最近节点之间等间距插入多个虚拟节点；
30.判断是否存在任意相邻虚拟节点之间的高程值之差的绝对值大于或者等于所述高程差阈值，得到第四判断结果；
31.若所述第四判断结果为是，则所述待定节点未通过高程检测；
32.若所述第四判断结果为否，则所述待定节点通过高程检测。
33.可选的，在所述待定节点未通过高程检测之后，还包括：
34.以所述最近节点为旋转中心，将延展方向分别向顺时针和逆时针偏转预设角度，得到第一替代方向和第二替代方向；
35.以所述最近节点为起点，沿所述第一替代方向，确定预设步长处对应的点为第一替代节点；
36.以所述最近节点为起点，沿所述第二替代方向，确定预设步长处对应的点为第二替代节点；
37.分别对所述第一替代节点和所述第二替代节点均进行高程检测；
38.判断两个所述替代节点是否均通过高程检测，得到第五判断结果；所述替代节点为所述第一替代节点或所述第二替代节点；
39.若所述第五判断结果为是，则确定两个替代节点中与所述最近节点的高程值之差的绝对值最小的一个点为通过所述高程检测的待定节点，并返回步骤“对通过所述高程检测的待定节点进行路径搜索截止检测”；
40.若所述第五判断结果为否，则判断两个替代节点是否均未通过高程检测，得到第
六判断结果；
41.若所述第六判断结果为是，则返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
42.若所述第六判断结果为否，则将通过所述高程检测替代节点作为通过所述高程检测的待定节点并返回步骤“对通过所述高程检测的待定节点进行路径搜索截止检测”。
43.可选的，所述对所述最近更新节点进行路径搜索截止检测，具体包括：
44.判断通过所述最近更新节点与目标点的二维距离是否小于距离阈值，得到第七判断结果；
45.若所述第七判断结果为否，则所述最近更新节点未通过路径搜索截止检测；
46.若所述第七判断结果为是，则根据所述目标点的高程值和所述最近更新节点的高程值，对所述目标点进行高程检测；
47.在所述目标点通过高程检测时，判定所述最近更新节点通过路径搜索截止检测。
48.一种三维地形下无人车路径规划系统，包括：
49.数据获取模块，用于获取路径规划区域的地图，以及待规划路径的起始点和目标点；所述地图包括所述路径规划区域上每个点的二维坐标和高程值；
50.随机树构建模块，用于将所述起始点作为节点构建随机树；
51.随机节点生成模块，用于基于目标偏置策略和多点采样策略生成随机节点；
52.最近节点确定模块，用于确定所述随机树中与所述随机节点的二维距离最小的节点为最近节点；
53.延展方向确定模块，用于确定所述最近节点到所述随机节点的方向为延展方向；
54.第一待定节点确定模块，用于以所述最近节点为起点，沿所述延展方向，确定预设步长处对应的点为待定节点；
55.第一高程检测模块，用于根据所述待定节点的高程值和所述最近节点的高程值，对所述待定节点进行高程检测；
56.随机树更新模块，用于将通过所述高程检测的待定节点作为所述最近节点的子节点添加至所述随机树中，并确定所述最近节点的子节点为最近更新节点；
57.路径搜索截止检测模块，用于对所述最近更新节点进行路径搜索截止检测；
58.循环模块，用于在所述最近更新节点未通过所述路径搜索截止检测时返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
59.路径截止随机树确定模块，用于在所述最近更新节点通过所述路径搜索截止检测时，确定所述目标点为所述最近更新节点的子节点，并将所述最近更新节点的子节点添加至所述随机树中，得到路径截止随机树；
60.待规划路径构建模块，用于根据所述路径截止随机树构建所述待规划路径。
61.可选的，所述随机节点生成模块，具体包括：
62.目标偏置概率获取单元，用于按照均匀概率分布，根据公式随机获取一个目标偏置概率；其中，p为目标偏置概率；p1和p2均为常数，a为随机节点扩展结束后随机树与目标点的最近距离，b为起始点和目标点质检的二维直线距离；
63.第一判断单元，用于判断所述目标偏置概率是否小于目标偏置概率阈值，得到第
一判断结果；若第一判断结果为是，则调用第一随机节点确定单元；若第一判断结果为否，则调用第二随机节点确定单元；
64.第一随机节点确定单元，用于确定目标点为随机节点；
65.第二随机节点确定单元，用于确定路径规划区域中任意两个点中距离所述目标点最近的一个点为所述随机节点。
66.可选的，所述第一高程检测模块具体包括：
67.第二判断单元，用于判断所述待定节点的高程值是否小于高程阈值，得到第二判断结果；若所述第二判断结果为否，则调用高程检测未通过判定单元；若所述第二判断结果为是，则调用第三判断单元；
68.高程检测未通过判定单元，用于判定所述待定节点未通过高程检测；
69.第三判断单元，用于判断所述待定节点与所述最近节点的高程值之差的绝对值是否小于高程差阈值，得到第三判断结果；若所述第三判断结果为否，则调用高程检测未通过判定单元；若所述第三判断结果为是，则调用虚拟节点插入单元；
70.虚拟节点插入单元，用于在所述待定节点和最近节点之间等间距插入多个虚拟节点；
71.第四判断单元，用于判断是否存在任意相邻虚拟节点之间的高程值之差的绝对值大于或者等于所述高程差阈值，得到第四判断结果；若所述第四判断结果为是，则调用高程检测未通过判定单元；若所述第四判断结果为否，则调用高程检测通过判定单元；
72.高程检测通过判定单元，用于判定所述待定节点通过高程检测。
73.可选的，所述系统还包括：
74.替代方向确定模块，用于以所述最近节点为旋转中心，将延展方向分别向顺时针和逆时针偏转预设角度，得到第一替代方向和第二替代方向；
75.第一替代节点确定模块，用于以所述最近节点为起点，沿所述第一替代方向，确定预设步长处对应的点为第一替代节点；
76.第二替代节点确定模块，用于以所述最近节点为起点，沿所述第二替代方向，确定预设步长处对应的点为第二替代节点；
77.第二高程检测模块，用于分别对所述第一替代节点和所述第二替代节点均进行高程检测；
78.第五判断模块，用于判断两个所述替代节点是否均通过高程检测，得到第五判断结果；所述替代节点为所述第一替代节点或所述第二替代节点；若所述第五判断结果为是，则调用第二待定节点确定模块；若所述第五判断结果为否，则调用第六判断模块；
79.第二待定节点确定模块，用于确定两个替代节点中与所述最近节点的高程值之差的绝对值最小的一个点为通过所述高程检测的待定节点，并调用所述路径搜索截止检测模块；
80.第六判断模块，用于判断两个替代节点是否均未通过高程检测，得到第六判断结果；若所述第六判断结果为是，则调用所述随机节点生成模块；若所述第六判断结果为否，则调用第三待定节点确定模块；
81.第三待定节点确定模块，用于将通过所述高程检测替代节点作为通过所述高程检测的待定节点调用所述路径搜索截止检测模块。
82.可选的，所述路径搜索截止检测模块具体包括：
83.第七判断单元，用于判断通过所述最近更新节点与目标点的二维距离是否小于距离阈值，得到第七判断结果；若所述第七判断结果为否，则调用第一路径搜索截止检测结果判定单元；若所述第七判断结果为否，则调用目标点高程检测单元；
84.第一路径搜索截止检测结果判定单元，用于判定所述最近更新节点未通过路径搜索截止检测；
85.目标点高程检测单元，用于根据所述目标点的高程值和所述最近更新节点的高程值，对所述目标点进行高程检测；
86.第二路径搜索截止检测结果判定单元，用于在所述目标点通过高程检测时，判定所述最近更新节点通过路径搜索截止检测。
87.根据本发明提供的具体实施例，本发明公开了以下技术效果：
88.本发明提供了一种三维地形下无人车路径规划方法及系统，将待规划路径的起始点作为节点构建随机树；基于目标偏置策略和多点采样策略生成随机节点；确定随机树中与随机节点的二维距离最小的节点为最近节点；确定最近节点到随机节点的方向为延展方向；以最近节点为起点，沿延展方向，确定预设步长处对应的点为待定节点；通过高程检测和路径搜索截止检测更新随机树或确定路径截止随机树，进而确定待规划路径。通过目标偏置策略、多点采样策、高程检测和路径搜索截止检测优化rrt算法(使用随机树规划路径的方法为rrt算法)，能够完成在三维地形下的路径规划，提高路径规划合理性。
附图说明
89.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
90.图1为本发明实施例一中三维地形下无人车路径规划方法流程图；
91.图2为本发明实施例一中路径规划区域的三维地图；
92.图3为本发明实施例一中待规划路径示意图。
具体实施方式
93.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
94.本发明的目的是提供一种三维地形下无人车路径规划方法及系统，能够完成在三维地形下的路径规划，提高路径规划合理性。
95.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
96.实施例一
97.如图1，本发明提供了一种三维地形下无人车路径规划方法，包括：
98.获取路径规划区域的地图，以及待规划路径的起始点和目标点；地图包括路径规划区域上每个点的二维坐标和高程值；
99.将起始点作为节点构建随机树；
100.基于目标偏置策略和多点采样策略生成随机节点；
101.确定随机树中与随机节点的二维距离最小的节点为最近节点；
102.确定最近节点到随机节点的方向为延展方向；
103.以最近节点为起点，沿延展方向，确定预设步长处对应的点为待定节点；
104.根据待定节点的高程值和最近节点的高程值，对待定节点进行高程检测；
105.将通过高程检测的待定节点作为最近节点的子节点添加至随机树中，并确定最近节点的子节点为最近更新节点；
106.对最近更新节点进行路径搜索截止检测；
107.在最近更新节点未通过路径搜索截止检测时返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
108.在最近更新节点通过路径搜索截止检测时，确定目标点为最近更新节点的子节点，并将最近更新节点的子节点添加至随机树中，得到路径截止随机树；
109.根据路径截止随机树构建待规划路径。
110.基于目标偏置策略和多点采样策略生成随机节点，具体包括：
111.按照均匀概率分布，根据公式随机获取一个目标偏置概率；其中，p为目标偏置概率；p1和p2均为常数，a为随机节点扩展结束后随机树与目标点的最近距离，b为起始点和目标点质检的二维直线距离；
112.判断目标偏置概率是否小于目标偏置概率阈值，得到第一判断结果；
113.若第一判断结果为是，则确定目标点为随机节点；
114.若第一判断结果为否，则确定路径规划区域中任意两个点中距离目标点最近的一个点为随机节点。
115.根据待定节点的高程值和最近节点的高程值，对待定节点进行高程检测具体包括：
116.判断待定节点的高程值是否小于高程阈值，得到第二判断结果；
117.若第二判断结果为否，则待定节点未通过高程检测；
118.若第二判断结果为是，则判断待定节点与最近节点的高程值之差的绝对值是否小于高程差阈值，得到第三判断结果；
119.若第三判断结果为否，则待定节点未通过高程检测；
120.若第三判断结果为是，则在待定节点和最近节点之间等间距插入多个虚拟节点；
121.判断是否存在任意相邻虚拟节点之间的高程值之差的绝对值大于或者等于高程差阈值，得到第四判断结果；
122.若第四判断结果为是，则待定节点未通过高程检测；
123.若第四判断结果为否，则待定节点通过高程检测。
124.在待定节点未通过高程检测之后，还包括：
125.以最近节点为旋转中心，将延展方向分别向顺时针和逆时针偏转预设角度，得到
第一替代方向和第二替代方向；
126.以最近节点为起点，沿第一替代方向，确定预设步长处对应的点为第一替代节点；
127.以最近节点为起点，沿第二替代方向，确定预设步长处对应的点为第二替代节点；
128.分别对第一替代节点和第二替代节点均进行高程检测；
129.判断两个替代节点是否均通过高程检测，得到第五判断结果；替代节点为第一替代节点或第二替代节点；
130.若第五判断结果为是，则确定两个替代节点中与最近节点的高程值之差的绝对值最小的一个点为通过高程检测的待定节点，并返回步骤“对通过高程检测的待定节点进行路径搜索截止检测”；
131.若第五判断结果为否，则判断两个替代节点是否均未通过高程检测，得到第六判断结果；
132.若第六判断结果为是，则返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
133.若第六判断结果为否，则将通过高程检测替代节点作为通过高程检测的待定节点并返回步骤“对通过高程检测的待定节点进行路径搜索截止检测”。
134.对最近更新节点进行路径搜索截止检测，具体包括：
135.判断通过最近更新节点与目标点的二维距离是否小于距离阈值，得到第七判断结果；
136.若第七判断结果为否，则最近更新节点未通过路径搜索截止检测；
137.若第七判断结果为是，则根据目标点的高程值和最近更新节点的高程值，对目标点进行高程检测；
138.在目标点通过高程检测时，判定最近更新节点通过路径搜索截止检测。
139.具体的，本发明主要研究移动机器人在野外山地环境下作业时的路径规划，主要解决原rrt算法在三维地形下进行随机点采样时存在采样范围大、盲目性强而导致的路径规划时间久和得到的路径最优性较差等问题。本发明通过对rrt算法进行改进，在采样区域内对采样点的生成采取一定的策略，加速新生成的节点向目标点靠近，进一步减少规划时间；通过建立新节点评估扩展机制，提高规划路径的质量；使得到的路径更利于无人车工作的进行。
140.具体步骤如下：
141.步骤一：建立三维地图如图2所示(图2-3中的横纵坐标表示路径规划区域的二维坐标，竖坐标表示高程数据)，获取地图高度数据，获取机器人的起始点x
start
，给定目标点x
goal
，初始化随机树t，并将起始点x
start
加入到随机树t中；
142.步骤二：改变原rrt算法随机点生成的方法，融合rrt goal bias(目标偏置策略)和multi sampling strategy(多采样点策略)两种采样规则生成随机点；具体做法如下：
143.1.对目标偏置策略进行改进，将目标偏置概率p动态化处理，即根据算法执行的程度来确定p的值，具体做法如下：
144.为保证算法的概率完备性和收敛速度通常p1＝0.1，p2＝0.2，a为每次新节点扩展结束后随机树与目标点的最近距离，b为起始点和目标点的直线距离。即当扩展达到一定程
度时加大将目标点作为x
rand
，加速算法的收敛。
145.2.在每次采样开始之前通过随机数函数random()获取一个(0,1)之间的随机值rand，按其与p的大小关系做如下处理：
[0146][0147]
其中x
better
为按多采样点策略在三维空间内获取的两个随机点(x
rand1
和x
rand2
)中距离目标点较近的一个，具体如下：
[0148][0149]
步骤三：根据x
rand
的位置在随机树t中找到距离最近的节点x
nearest
作为x
rand
的父节点x
parent
，此时x
rand
为x
parent
的子节点，根据父子节点的位置关系确定新节点的扩展方向通过给定步长l确定新节点x
new
；
[0150]
步骤四：通过建立的新的评估准则对扩展得到的新节点x
new
进行检测，判断新节点是否满足要求，具体判断如下：
[0151]
准则一：通过设定高度阈值h
max
，在得到新节点x
new
之后，判断x
new
的高度值是否满足δh≤h
max
；
[0152]
准则二：通过设定高度差阈值h
max
判断新节点x
new
与其父节点的高度差δh是否满足δh≤h
max
；
[0153]
准则三：为防止由于扩展步长过大而导致的父子节点之间出现不可通行的情况，根据无人车的越障和越壕能力对当前父子节点进行等间距插值得到两点之间的若干控制点x1，x2…
xn，对这些相邻控制点按准则二设定的高度差阈值h
max
进行检测，确保父子节点之间不会出现高度突变的情况；
[0154]
步骤五：若新节点x
new
通过步骤四的检测，则将该节点及其对应的父节点信息加入到随机树t中；对没有通过步骤四检测的新节点x
new
做半接纳处理，即在此节点附近找到两个替代点x1，x2，重新对其进行检测，筛选出较优的一个作为当前随机点确定的新节点加入到随机树中；具体做法如下：
[0155]
考虑到三维地形的特点，当按原rrt算法扩展方式得到的新节点没有通过步骤四的检测时，不会立即选择舍弃，而是将原扩展方向由变为变为为预设角度，以此得到两个替代点，重新对这两个替代点按步骤四步骤进行检测，挑选最优的一个作为x
new
的替代点，并将其加入到随机树中；两个替代点分别与父节点的高度差中较小的一个为最优替代点，若两个找到的两个替代点x1，x2都没有通过步骤四的检测，则全部舍弃，重新回到步骤二；
[0156]
步骤六：设定终点连接阈值λ，通过计算确定每次通过上述检测的新节点x
new
到目标点x
goal
的距离z是否满足上述三个准则和z≤λ。若满足，则将该新节点x
new
作为目标点x
goal
的父节点直接连接，并将目标点x
goal
及其对应的父节点信息加入到随机树t中。若不满足两者之一则返回到步骤二，直到将目标点x
goal
加入到随机树中；
[0157]
步骤七：在随机树t中从目标点x
goal
向起始点x
start
回溯路径；即从x
goal
开始，依次
逐级找到每个节点对应的父节点，直到回溯到x
start
为止。将这些父节点加入到数组path()中，依次连接path()中的节点，以此得到的路径作为此改进算法的初步路径，如图3所示。
[0158]
本发明融合目标偏置策略和多采样点策略两种采样规则生成随机点，加速算法的收敛速度；对目标偏置策略提出改进，将目标偏置概率p动态化处理，即根据算法执行的程度来确定p的值；对扩展得到的新节点建立的新的评估准则进行检测，间接地优化路径的质量；对未通过检测的节点进行半接纳处理，通过找到最优替代点间接地达到提高采样的成功率的目的，解决了原rrt算法在进行随机点采样时存在范围大、盲目性强而导致的路径规划时间久和得到的路径最优性较差等问题。通过考虑地图上的不可通行区域、父子节点的高度差以及他们之间的可通行性，对扩展得到的新节点是否接纳提出三条评估准则，使得到的路径更利于无人车作业的进行。
[0159]
此外，本发明还提供了一种三维地形下无人车路径规划系统，包括：
[0160]
数据获取模块，用于获取路径规划区域的地图，以及待规划路径的起始点和目标点；地图包括路径规划区域上每个点的二维坐标和高程值；
[0161]
随机树构建模块，用于将起始点作为节点构建随机树；
[0162]
随机节点生成模块，用于基于目标偏置策略和多点采样策略生成随机节点；
[0163]
最近节点确定模块，用于确定随机树中与随机节点的二维距离最小的节点为最近节点；
[0164]
延展方向确定模块，用于确定最近节点到随机节点的方向为延展方向；
[0165]
第一待定节点确定模块，用于以最近节点为起点，沿延展方向，确定预设步长处对应的点为待定节点；
[0166]
第一高程检测模块，用于根据待定节点的高程值和最近节点的高程值，对待定节点进行高程检测；
[0167]
随机树更新模块，用于将通过高程检测的待定节点作为最近节点的子节点添加至随机树中，并确定最近节点的子节点为最近更新节点；
[0168]
路径搜索截止检测模块，用于对最近更新节点进行路径搜索截止检测；
[0169]
循环模块，用于在最近更新节点未通过路径搜索截止检测时返回步骤“基于目标偏置策略和多点采样策略生成随机节点”；
[0170]
路径截止随机树确定模块，用于在最近更新节点通过路径搜索截止检测时，确定目标点为最近更新节点的子节点，并将最近更新节点的子节点添加至随机树中，得到路径截止随机树；
[0171]
待规划路径构建模块，用于根据路径截止随机树构建待规划路径。
[0172]
随机节点生成模块，具体包括：
[0173]
目标偏置概率获取单元，用于按照均匀概率分布，根据公式随机获取一个目标偏置概率；其中，p为目标偏置概率；p1和p2均为常数，a为随机节点扩展结束后随机树与目标点的最近距离，b为起始点和目标点质检的二维直线距离；
[0174]
第一判断单元，用于判断目标偏置概率是否小于目标偏置概率阈值，得到第一判断结果；若第一判断结果为是，则调用第一随机节点确定单元；若第一判断结果为否，则调用第二随机节点确定单元；
[0175]
第一随机节点确定单元，用于确定目标点为随机节点；
[0176]
第二随机节点确定单元，用于确定路径规划区域中任意两个点中距离目标点最近的一个点为随机节点。
[0177]
第一高程检测模块具体包括：
[0178]
第二判断单元，用于判断待定节点的高程值是否小于高程阈值，得到第二判断结果；若第二判断结果为否，则调用高程检测未通过判定单元；若第二判断结果为是，则调用第三判断单元；
[0179]
高程检测未通过判定单元，用于判定待定节点未通过高程检测；
[0180]
第三判断单元，用于判断待定节点与最近节点的高程值之差的绝对值是否小于高程差阈值，得到第三判断结果；若第三判断结果为否，则调用高程检测未通过判定单元；若第三判断结果为是，则调用虚拟节点插入单元；
[0181]
虚拟节点插入单元，用于在待定节点和最近节点之间等间距插入多个虚拟节点；
[0182]
第四判断单元，用于判断是否存在任意相邻虚拟节点之间的高程值之差的绝对值大于或者等于高程差阈值，得到第四判断结果；若第四判断结果为是，则调用高程检测未通过判定单元；若第四判断结果为否，则调用高程检测通过判定单元；
[0183]
高程检测通过判定单元，用于判定待定节点通过高程检测。
[0184]
本发明提供的一种三维地形下无人车路径规划系统，还包括：
[0185]
替代方向确定模块，用于以最近节点为旋转中心，将延展方向分别向顺时针和逆时针偏转预设角度，得到第一替代方向和第二替代方向；
[0186]
第一替代节点确定模块，用于以最近节点为起点，沿第一替代方向，确定预设步长处对应的点为第一替代节点；
[0187]
第二替代节点确定模块，用于以最近节点为起点，沿第二替代方向，确定预设步长处对应的点为第二替代节点；
[0188]
第二高程检测模块，用于分别对第一替代节点和第二替代节点均进行高程检测；
[0189]
第五判断模块，用于判断两个替代节点是否均通过高程检测，得到第五判断结果；替代节点为第一替代节点或第二替代节点；若第五判断结果为是，则调用第二待定节点确定模块；若第五判断结果为否，则调用第六判断模块；
[0190]
第二待定节点确定模块，用于确定两个替代节点中与最近节点的高程值之差的绝对值最小的一个点为通过高程检测的待定节点，并调用路径搜索截止检测模块；
[0191]
第六判断模块，用于判断两个替代节点是否均未通过高程检测，得到第六判断结果；若第六判断结果为是，则调用随机节点生成模块；若第六判断结果为否，则调用第三待定节点确定模块；
[0192]
第三待定节点确定模块，用于将通过高程检测替代节点作为通过高程检测的待定节点调用路径搜索截止检测模块。
[0193]
其中，路径搜索截止检测模块具体包括：
[0194]
第七判断单元，用于判断通过最近更新节点与目标点的二维距离是否小于距离阈值，得到第七判断结果；若第七判断结果为否，则调用第一路径搜索截止检测结果判定单元；若第七判断结果为否，则调用目标点高程检测单元；
[0195]
第一路径搜索截止检测结果判定单元，用于判定最近更新节点未通过路径搜索截
止检测；
[0196]
目标点高程检测单元，用于根据目标点的高程值和最近更新节点的高程值，对目标点进行高程检测；
[0197]
第二路径搜索截止检测结果判定单元，用于在目标点通过高程检测时，判定最近更新节点通过路径搜索截止检测。
[0198]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0199]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。