一种未知环境下用于特定目标搜索的路径规划方法

1.本发明涉及目标搜索的路径规划技术领域，特别涉及一种未知环境下用于特定目标搜索的路径规划方法。


背景技术：

2.自主无人车辆在军事、农业、消防、运输等领域有着重要的意义。当自主无人车辆在野外执行搜索任务时，由于野外环境是未知的，且具有高度不确定性，为了有效进行路径规划，需要准确构建任务环境的全局栅格地图。另外在执行任务前通常只能得到特定目标的外形、种类等信息，而目标的位置信息往往是未知的，需要自主无人车辆自动的规划路径去搜寻目标。现有的自主无人车辆在执行特定目标搜索任务时存在着搜索效率低、规划路径不合理和重复搜索等现象。
3.路径规划作为自主无人车辆从起始位置安全抵达目标位置的关键技术，是自主无人车辆自主性的重要体现。路径规划算法主要包括astar算法、rrt算法、人工势场算法、遗传算法、蚁群算法等。这些方法都存在着各自相应的弊端。


技术实现要素：

4.本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种未知环境下用于特定目标搜索的路径规划方法，以解决上述背景技术中提出的问题。
5.一种未知环境下用于特定目标搜索的路径规划方法，具体步骤包括：
6.构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物；
7.将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系；
8.根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置；
9.利用人工势场引导的rrt路径规划算法规划出自主无人车辆当前位置到目标位置的路径；
10.自主无人车辆根据规划路径进行路径跟踪，同时进行激光slam建图并更新全局占据栅格地图；
11.判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置。
12.作为本发明的进一步的方案：所述构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物的具体步骤包括：
13.根据待搜索未知区域构建全局占据栅格地图，该区域的目标中心作为地图中心，并建立坐标系；
14.通过自主无人车辆的环境感知系统获取车辆当前位置周围的障碍物，构建局部占
据栅格地图；
15.将局部占据栅格地图进行匹配校正，通过坐标转换映射到全局占据栅格地图。
16.作为本发明的进一步的方案：所述将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系的具体步骤包括：
17.将全局占据栅格图平均划分为若干个矩形区域，每个矩形区域π
i
具有规定数量的占据栅格，其中当边缘部分的区域π
i
中的栅格数量少于规定数量时，表示剩余的栅格被障碍物占据；
18.所述每个矩形区域π
i
设置有区域属性，其中包括区域位置坐标(π
x
,π
y
)、障碍物数量未访问栅格数量和未访问栅格坐标集合
19.作为本发明的进一步的方案：所述根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置的具体步骤包括：
20.首先根据就近原则，确定车辆当前所在区域与待搜索区域之间的欧几里得距离作为距离代价c
d
；
21.根据未访问优先原则，确定待搜索区域的未访问栅格数量作为未访问栅格数量代价c
nvg
；
22.根据远离障碍物原则，确定待搜索区域的障碍物数量作为障碍物代价c
ob
；
23.根据上述距离代价c
d
，未访问栅格数量代价c
nvg
和障碍物代价c
ob
建立代价函数，并选择代价最小的区域π
i
作为下一次访问的目标区域，所述代价函数为：
24.j
cost
＝ω
d
c
d
+ω
nvg
c
nvg
+ω
ob
c
ob
；
25.其中，ω
d
为距离代价c
d
的权重系数，ω
nvg
为未访问栅格数量代价c
nvg
的权重系数，ω
ob
为障碍物代价c
ob
的权重系数；
26.在目标区域π
i
内将所有的障碍物向外膨胀距离ρ，再在该目标区域π
i
内随机选择一个未被障碍物占据且未被访问的栅格作为下一次规划的位置。
27.作为本发明的进一步的方案：所述利用人工势场引导的rrt路径规划算法规划出自主无人车辆当前位置到目标位置的路径的具体步骤包括：
28.初始化随机树t及其参数，所述参数包括初始位置q
init
，目标位置q
goal
，随机树t的生长步长λ；
29.构建目标节点q
goal
对随机树t中的节点q的引力；
30.首先建立目标节点的引力势场，引力势场为：
[0031][0032]
则目标节点q
goal
与当前节点q之间的目标引力f
goal
为：
[0033]
f
goal
＝k
goal
||q
goal
‑
q||；
[0034]
其中k
goal
是引力系数；
[0035]
构造随机树新节点：以特定概率在全局占据栅格地图中随机采样得到随机点q
rand
，选择随机节点q
rand
的最近节点q
near
，在目标引力f
goal
的作用下，新节点q
new
的位置会向目标点方向偏转，q
new
的计算公式为：
[0036][0037]
判断若q
near
与q
new
之间的连线没有障碍物，将q
near
作为q
new
的父节点，q
new
作为q
near
的子节点，再继续随机采样扩展随机树t，否则舍弃当前新节点q
new
，重新随机采样；
[0038]
循环搜索，当随机树t新的叶节点q
new
扩展达到目标节点附近时，从目标节点q
goal
通过对应的父节点反向追溯到根节点q
init
，得到连接q
init
到q
goal
的无碰撞路径；
[0039]
节点剪枝后处理：从随机树t的根节点开始依次连接后序路径点，当路径点之间的连线与膨胀后的障碍物不相交时即可删除两者之间的路径点，依次循环下去，当与新节点之间出现障碍物时，以新节点的父节点为新起点重复上述操作，直至到达目标路径点为止，得到平滑的路径；
[0040]
路径平滑后处理：对于剪枝后得到的路径，使用贝塞尔曲线进行平滑处理，得到平滑无碰撞的路径。
[0041]
作为本发明的进一步的方案：所述自主无人车辆根据规划路径进行路径跟踪，同时进行激光slam建图并更新全局占据栅格地图的具体步骤包括：
[0042]
建立自主无人车辆的车辆动力学模型，并进行路径跟踪；
[0043]
在路径跟踪过程中，构建车辆实时位置的环境地图，并同步到全局占据栅格地图，同时更新各个区域的障碍物数量；
[0044]
车辆坐标系转换到全局占据栅格地图的坐标系；
[0045]
获取已访问全局占据栅格集合p；
[0046]
对集合p中的每个栅格坐标(x
i
,y
i
)，计算该坐标在全局占据栅格地图中对应的区域π
i
；
[0047]
若区域π
i
中未访问栅格坐标集合中存在该元素，则删除该元素，并将区域π
i
中未访问栅格数量减一；
[0048]
若区域π
i
中未访问栅格坐标集合中不存在该元素，对集合p中的下一个元素重复上述操作。
[0049]
作为本发明的进一步的方案：所述判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置的具体步骤包括：
[0050]
当自主无人车辆的视觉检测系统扫描到特定目标并成功识别时，将特定目标的图像与三维点云数据进行匹配，得到特定目标的准确位置；
[0051]
最后规划出到特定目标的路径，并进行路径跟踪到达目标位置，完成搜索任务。
[0052]
与现有技术相比，本发明存在以下技术效果：
[0053]
通过采用上述的技术方案，将未知区域进行栅格化，并构建全局占据栅格地图，同时利用自主无人车辆的雷达装置扫描当前位置周围的障碍物并投影到地图。划分该全局占据栅格地图并设置区域属性形成映射关系，在当前位置搜索下一次的目标位置，同时利用路径规划算法规划出一条平滑无碰撞的路径。采用自主无人车辆进行路径跟踪，并构建地图更新区域属性。根据配备的视觉检测系统识别确认是否为特定目标，从而确定下一步操作。利用划分区域计算每个区域的访问代价，能够最大化发现特定目标的概率，避免重复搜索，提高目标搜索效率。同时使用人工势场引导的rrt路径规划算法以特定概率选择随机节
点，能够引导随机树向目标位置扩展，减弱了其随机性，提高了规划速度。
附图说明
[0054]
下面结合附图，对本发明的具体实施方式进行详细描述：
[0055]
图1为本技术公开的一些实施例的路径规划方法的步骤示意图；
[0056]
图2为本技术公开的一些实施例的局部占据栅格地图投影到全局占据栅格地图的示意图；
[0057]
图3为本技术公开的一些实施例的全局占据栅格地图的若干个区域的示意图；
[0058]
图4为本技术公开的一些实施例的人工势场引导的rrt构建新节点的示意图；
[0059]
图5为本技术公开的一些实施例的人工势场引导的rrt算法流程图；
[0060]
图6为本技术公开的一些实施例的人工势场引导的rrt算法生成的路径示意图；
[0061]
图7为本技术公开的一些实施例的节点剪枝处理后的路径示意图；
[0062]
图8为本技术公开的一些实施例的路径平滑处理后的路径示意图。
具体实施方式
[0063]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0064]
请参考图1，本发明实施例中，一种未知环境下用于特定目标搜索的路径规划方法，具体步骤包括：
[0065]
s1、构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物，具体步骤包括：
[0066]
s11、根据待搜索未知区域构建全局占据栅格地图，该区域的目标中心作为地图中心，并建立坐标系；
[0067]
具体实施方式中，将待搜索未知区域的中心作为全局占据栅格地图的中心，如图2所示，图示出以全局占据栅格地图的左上角作为坐标原点，向东为x轴正方向，向南为y轴正方向，构建x
g
×
y
g
大小的全局占据栅格地图，其中x
g
为全局占据栅格地图在东西方向上的长度，y
g
为全局占据栅格地图在南北方向上的长度，每个栅格的大小设为d
g
×
d
g
，其中d
g
为栅格的边长。在目标搜索任务开始前预先设定地图中栅格的位置和尺寸，任务中保持不变，实时更新栅格的占据状态。
[0068]
s12、通过自主无人车辆的环境感知系统获取车辆当前位置周围的障碍物，构建局部占据栅格地图；
[0069]
具体的，利用自主无人车辆的环境感知系统，通过设置有的车辆定位系统、激光雷达、毫米波雷达、相机等传感器获取车辆当前位置周围的障碍物信息。如图2所示，图示出的灰色区域为构建的局部占据栅格地图，具体为x
i
×
y
i
大小的局部占据栅格图地图，其中x
i
为局部占据栅格地图在车辆前进方向上的长度，y
i
为局部占据栅格地图在垂直于x
i
方向上的长度。
[0070]
s13、将局部占据栅格地图进行匹配校正，通过坐标转换映射到全局占据栅格地
图。
[0071]
s2、将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系，具体步骤包括：
[0072]
如图3所示，将全局占据栅格图平均划分为若干个矩形区域，每个矩形区域π
i
具有规定数量的占据栅格，其中当边缘部分的区域π
i
中的栅格数量少于规定数量时，表示剩余的栅格被障碍物占据；
[0073]
所述每个矩形区域π
i
设置有区域属性，其中包括区域位置坐标(π
x
,π
y
)、障碍物数量未访问栅格数量和未访问栅格坐标集合
[0074]
所述区域位置坐标(π
x
,π
y
)以全局占据栅格地图的左上角区域为坐标原点，向东为x轴方向，向南为y轴方向，障碍物数量代表区域π
i
内被障碍物所占据的栅格个数，随着全局占据栅格图的更新而随之动态更新；
[0075]
未访问栅格数量和集合分别代表区域π
i
内未被自主无人车辆视觉检测系统扫描到的栅格数量和相应的集合，和会随着车辆的移动而实时更新。
[0076]
s3、根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置，具体步骤包括：
[0077]
s31、首先根据就近原则，确定车辆当前所在区域与待搜索区域之间的欧几里得距离作为距离代价c
d
；
[0078]
s32、根据未访问优先原则，确定待搜索区域的未访问栅格数量作为未访问栅格数量代价c
nvg
；
[0079]
s33、根据远离障碍物原则，确定待搜索区域的障碍物数量作为障碍物代价c
ob
；
[0080]
s34、根据上述距离代价c
d
，未访问栅格数量代价c
nvg
和障碍物代价c
ob
建立代价函数，并选择代价最小的区域π
i
作为下一次访问的目标区域，所述代价函数为：
[0081]
j
cost
＝ω
d
c
d
+ω
nvg
c
nvg
+ω
ob
c
ob
；
[0082]
其中，ω
d
为距离代价c
d
的权重系数，ω
nvg
为未访问栅格数量代价c
nvg
的权重系数，ω
ob
为障碍物代价c
ob
的权重系数；
[0083]
s35、在目标区域π
i
内将所有的障碍物向外膨胀距离ρ，再在该目标区域π
i
内随机选择一个未被障碍物占据且未被访问的栅格作为下一次规划的位置。
[0084]
s4、利用人工势场引导的rrt路径规划算法规划出自主无人车辆当前位置到目标位置的路径，具体步骤包括：
[0085]
s41、初始化随机树t及其参数，所述参数包括初始位置q
init
，目标位置q
goal
，随机树t的生长步长λ；
[0086]
s42、构建目标节点q
goal
对随机树t中的节点q的引力；
[0087]
首先建立目标节点的引力势场，引力势场为：
[0088][0089]
则目标节点q
goal
与当前节点q之间的目标引力f
goal
为：
[0090]
f
goal
＝k
goal
||q
goal
‑
q||；
[0091]
其中k
goal
是引力系数；
[0092]
s43、构造随机树新节点：如图4所示，图示为随机树新节点的构造方法，具体以特定概率在全局占据栅格地图中随机采样得到随机点q
rand
，选择随机节点q
rand
的最近节点q
near
，在目标引力f
goal
的作用下，新节点q
new
的位置会向目标点方向偏转，q
new
的计算公式为：
[0093][0094]
判断若q
near
与q
new
之间的连线没有障碍物，将q
near
作为q
new
的父节点，q
new
作为q
near
的子节点，再继续随机采样扩展随机树t，否则舍弃当前新节点q
new
，重新随机采样；
[0095]
如图5所示，图示为人工势场引导的rrt路径规划方法的算法流程图，
[0096]
s44、循环搜索，当随机树t新的叶节点q
new
扩展达到目标节点附近时，从目标节点q
goal
通过对应的父节点反向追溯到根节点q
init
，得到连接q
init
到q
goal
的无碰撞路径；其中节点的位置记录在路径点集合o中。
[0097]
如图6所示，图示为人工势场引导的rrt路径规划算法规划出的路径示意图，具体是根据对规划出的路径进行节点剪枝后处理和路径平滑后处理，从而得到一条平滑的无碰撞的路径。
[0098]
s45、节点剪枝后处理：从随机树t的根节点开始依次连接后序路径点，当路径点之间的连线与膨胀后的障碍物不相交时即可删除两者之间的路径点，依次循环下去，当与新节点之间出现障碍物时，以新节点的父节点为新起点重复上述操作，直至到达目标路径点为止，得到平滑的路径，如图7所示，图示为rrt节点剪枝后的路径。
[0099]
s46、路径平滑后处理：对于剪枝后得到的路径，使用贝塞尔曲线进行平滑处理，得到平滑无碰撞的路径，如图7所示，图示为路径平滑处理后的路径。
[0100]
s5、自主无人车辆根据规划路径进行路径跟踪，同时进行激光slam建图并更新全局占据栅格地图；具体步骤包括：
[0101]
获取上述步骤s4中生成的路径，再建立自主无人车辆的车辆动力学模型，并使用模型预测控制方法进行路径跟踪；
[0102]
在路径跟踪过程中，使用激光slam方法构建车辆实时位置的环境地图，并同步到全局占据栅格地图，同时更新全局占据栅格地图中各个区域的障碍物数量；
[0103]
同时在路径跟踪的过程中，将视觉检测系统扫描过的范围从车辆坐标系转换到全局占据栅格地图的坐标系，从而获取已访问全局占据栅格集合p；
[0104]
对集合p中的每个栅格坐标(x
i
,y
i
)，计算该坐标在全局占据栅格地图中对应的区域π
i
；
[0105]
若区域π
i
中未访问栅格坐标集合中存在该元素，则删除该元素，并将区域π
i
中未访问栅格数量减一；
[0106]
若区域π
i
中未访问栅格坐标集合中不存在该元素，对集合p中的下一个元素重复上述操作。
[0107]
s6、判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置，具体步骤包括：
[0108]
当自主无人车辆的视觉检测系统扫描到特定目标并成功识别时，将特定目标的图
像与三维点云数据进行匹配，得到特定目标的准确位置；
[0109]
再使用步骤s4中规划出的一条平滑无碰撞的路径进行跟踪，并控制车辆到达目标位置，完成搜索任务。若未发现特定目标，返回步骤s3重复操作，确定下一次的目标位置。
[0110]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。