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

Abstract

本发明公开了一种未知环境下用于特定目标搜索的路径规划方法，具体步骤包括构建待搜索未知区域的全局占据栅格地图，并投影当前位置周围的障碍物；将地图划分为若干个区域，并构建之间的映射关系；根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置；利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径；自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图；若搜索到特定目标则搜索结束，否则确定下一个目标位置。本发明划分若干个区域并计算访问代价，优先搜索代价最小的区域，能够最大化发现特定目标的概率，并有效避免重复搜索，提高目标搜索效率。

Description

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

技术领域

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

背景技术

自主无人车辆在军事、农业、消防、运输等领域有着重要的意义。当自主无人车辆在野外执行搜索任务时，由于野外环境是未知的，且具有高度不确定性，为了有效进行路径规划，需要准确构建任务环境的全局栅格地图。另外在执行任务前通常只能得到特定目标的外形、种类等信息，而目标的位置信息往往是未知的，需要自主无人车辆自动的规划路径去搜寻目标。现有的自主无人车辆在执行特定目标搜索任务时存在着搜索效率低、规划路径不合理和重复搜索等现象。

路径规划作为自主无人车辆从起始位置安全抵达目标位置的关键技术，是自主无人车辆自主性的重要体现。路径规划算法主要包括Astar算法、RRT算法、人工势场算法、遗传算法、蚁群算法等。这些方法都存在着各自相应的弊端。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种未知环境下用于特定目标搜索的路径规划方法，以解决上述背景技术中提出的问题。

一种未知环境下用于特定目标搜索的路径规划方法，具体步骤包括：

构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物；

将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系；

根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置；

利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径；

自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图；

判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置。

作为本发明的进一步的方案：所述构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物的具体步骤包括：

根据待搜索未知区域构建全局占据栅格地图，该区域的目标中心作为地图中心，并建立坐标系；

通过自主无人车辆的环境感知系统获取车辆当前位置周围的障碍物，构建局部占据栅格地图；

将局部占据栅格地图进行匹配校正，通过坐标转换映射到全局占据栅格地图。

作为本发明的进一步的方案：所述将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系的具体步骤包括：

将全局占据栅格图平均划分为若干个矩形区域，每个矩形区域π_i具有规定数量的占据栅格，其中当边缘部分的区域π_i中的栅格数量少于规定数量时，表示剩余的栅格被障碍物占据；

所述每个矩形区域π_i设置有区域属性，其中包括区域位置坐标(π_x,π_y)、障碍物数量未访问栅格数量/>和未访问栅格坐标集合/>

作为本发明的进一步的方案：所述根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置的具体步骤包括：

首先根据就近原则，确定车辆当前所在区域与待搜索区域之间的欧几里得距离作为距离代价C_d；

根据未访问优先原则，确定待搜索区域的未访问栅格数量作为未访问栅格数量代价C_nvg；

根据远离障碍物原则，确定待搜索区域的障碍物数量作为障碍物代价C_ob；

根据上述距离代价C_d，未访问栅格数量代价C_nvg和障碍物代价C_ob建立代价函数，并选择代价最小的区域π_i作为下一次访问的目标区域，所述代价函数为：

J_cost＝ω_dC_d+ω_nvgC_nvg+ω_obC_ob；

其中，ω_d为距离代价C_d的权重系数，ω_nvg为未访问栅格数量代价C_nvg的权重系数，ω_ob为障碍物代价C_ob的权重系数；

在目标区域π_i内将所有的障碍物向外膨胀距离ρ，再在该目标区域π_i内随机选择一个未被障碍物占据且未被访问的栅格作为下一次规划的位置。

作为本发明的进一步的方案：所述利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径的具体步骤包括：

初始化随机树T及其参数，所述参数包括初始位置q_init，目标位置q_goal，随机树T的生长步长λ；

构建目标节点q_goal对随机树T中的节点q的引力；

首先建立目标节点的引力势场，引力势场为：

则目标节点q_goal与当前节点q之间的目标引力F_goal为：

F_goal＝k_goal||q_goal-q||；

其中k_goal是引力系数；

构造随机树新节点：以特定概率在全局占据栅格地图中随机采样得到随机点q_rand，选择随机节点q_rand的最近节点q_near，在目标引力F_goal的作用下，新节点q_new的位置会向目标点方向偏转，q_new的计算公式为：

判断若q_near与q_new之间的连线没有障碍物，将q_near作为q_new的父节点，q_new作为q_near的子节点，再继续随机采样扩展随机树T，否则舍弃当前新节点q_new，重新随机采样；

循环搜索，当随机树T新的叶节点q_new扩展达到目标节点附近时，从目标节点q_goal通过对应的父节点反向追溯到根节点q_init，得到连接q_init到q_goal的无碰撞路径；

节点剪枝后处理：从随机树T的根节点开始依次连接后序路径点，当路径点之间的连线与膨胀后的障碍物不相交时即可删除两者之间的路径点，依次循环下去，当与新节点之间出现障碍物时，以新节点的父节点为新起点重复上述操作，直至到达目标路径点为止，得到平滑的路径；

路径平滑后处理：对于剪枝后得到的路径，使用贝塞尔曲线进行平滑处理，得到平滑无碰撞的路径。

作为本发明的进一步的方案：所述自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图的具体步骤包括：

建立自主无人车辆的车辆动力学模型，并进行路径跟踪；

在路径跟踪过程中，构建车辆实时位置的环境地图，并同步到全局占据栅格地图，同时更新各个区域的障碍物数量；

车辆坐标系转换到全局占据栅格地图的坐标系；

获取已访问全局占据栅格集合P；

对集合P中的每个栅格坐标(x_i,y_i)，计算该坐标在全局占据栅格地图中对应的区域π_i；

若区域π_i中未访问栅格坐标集合中存在该元素，则删除该元素，并将区域π_i中未访问栅格数量/>减一；

若区域π_i中未访问栅格坐标集合中不存在该元素，对集合P中的下一个元素重复上述操作。

作为本发明的进一步的方案：所述判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置的具体步骤包括：

当自主无人车辆的视觉检测系统扫描到特定目标并成功识别时，将特定目标的图像与三维点云数据进行匹配，得到特定目标的准确位置；

最后规划出到特定目标的路径，并进行路径跟踪到达目标位置，完成搜索任务。

与现有技术相比，本发明存在以下技术效果：

通过采用上述的技术方案，将未知区域进行栅格化，并构建全局占据栅格地图，同时利用自主无人车辆的雷达装置扫描当前位置周围的障碍物并投影到地图。划分该全局占据栅格地图并设置区域属性形成映射关系，在当前位置搜索下一次的目标位置，同时利用路径规划算法规划出一条平滑无碰撞的路径。采用自主无人车辆进行路径跟踪，并构建地图更新区域属性。根据配备的视觉检测系统识别确认是否为特定目标，从而确定下一步操作。利用划分区域计算每个区域的访问代价，能够最大化发现特定目标的概率，避免重复搜索，提高目标搜索效率。同时使用人工势场引导的RRT路径规划算法以特定概率选择随机节点，能够引导随机树向目标位置扩展，减弱了其随机性，提高了规划速度。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的一些实施例的路径规划方法的步骤示意图；

图2为本申请公开的一些实施例的局部占据栅格地图投影到全局占据栅格地图的示意图；

图3为本申请公开的一些实施例的全局占据栅格地图的若干个区域的示意图；

图4为本申请公开的一些实施例的人工势场引导的RRT构建新节点的示意图；

图5为本申请公开的一些实施例的人工势场引导的RRT算法流程图；

图6为本申请公开的一些实施例的人工势场引导的RRT算法生成的路径示意图；

图7为本申请公开的一些实施例的节点剪枝处理后的路径示意图；

图8为本申请公开的一些实施例的路径平滑处理后的路径示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，本发明实施例中，一种未知环境下用于特定目标搜索的路径规划方法，具体步骤包括：

S1、构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物，具体步骤包括：

S11、根据待搜索未知区域构建全局占据栅格地图，该区域的目标中心作为地图中心，并建立坐标系；

具体实施方式中，将待搜索未知区域的中心作为全局占据栅格地图的中心，如图2所示，图示出以全局占据栅格地图的左上角作为坐标原点，向东为x轴正方向，向南为y轴正方向，构建X_g×Y_g大小的全局占据栅格地图，其中X_g为全局占据栅格地图在东西方向上的长度，Y_g为全局占据栅格地图在南北方向上的长度，每个栅格的大小设为d_g×d_g，其中d_g为栅格的边长。在目标搜索任务开始前预先设定地图中栅格的位置和尺寸，任务中保持不变，实时更新栅格的占据状态。

S12、通过自主无人车辆的环境感知系统获取车辆当前位置周围的障碍物，构建局部占据栅格地图；

具体的，利用自主无人车辆的环境感知系统，通过设置有的车辆定位系统、激光雷达、毫米波雷达、相机等传感器获取车辆当前位置周围的障碍物信息。如图2所示，图示出的灰色区域为构建的局部占据栅格地图，具体为X_i×Y_i大小的局部占据栅格图地图，其中X_i为局部占据栅格地图在车辆前进方向上的长度，Y_i为局部占据栅格地图在垂直于X_i方向上的长度。

S13、将局部占据栅格地图进行匹配校正，通过坐标转换映射到全局占据栅格地图。

S2、将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系，具体步骤包括：

如图3所示，将全局占据栅格图平均划分为若干个矩形区域，每个矩形区域π_i具有规定数量的占据栅格，其中当边缘部分的区域π_i中的栅格数量少于规定数量时，表示剩余的栅格被障碍物占据；

所述每个矩形区域π_i设置有区域属性，其中包括区域位置坐标(π_x,π_y)、障碍物数量未访问栅格数量/>和未访问栅格坐标集合/>

所述区域位置坐标(π_x,π_y)以全局占据栅格地图的左上角区域为坐标原点，向东为x轴方向，向南为y轴方向，障碍物数量代表区域π_i内被障碍物所占据的栅格个数，/>随着全局占据栅格图的更新而随之动态更新；

未访问栅格数量和集合/>分别代表区域π_i内未被自主无人车辆视觉检测系统扫描到的栅格数量和相应的集合，/>和/>会随着车辆的移动而实时更新。

S3、根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置，具体步骤包括：

S31、首先根据就近原则，确定车辆当前所在区域与待搜索区域之间的欧几里得距离作为距离代价C_d；

S32、根据未访问优先原则，确定待搜索区域的未访问栅格数量作为未访问栅格数量代价C_nvg；

S33、根据远离障碍物原则，确定待搜索区域的障碍物数量作为障碍物代价C_ob；

S34、根据上述距离代价C_d，未访问栅格数量代价C_nvg和障碍物代价C_ob建立代价函数，并选择代价最小的区域π_i作为下一次访问的目标区域，所述代价函数为：

J_cost＝ω_dC_d+ω_nvgC_nvg+ω_obC_ob；

其中，ω_d为距离代价C_d的权重系数，ω_nvg为未访问栅格数量代价C_nvg的权重系数，ω_ob为障碍物代价C_ob的权重系数；

S35、在目标区域π_i内将所有的障碍物向外膨胀距离ρ，再在该目标区域π_i内随机选择一个未被障碍物占据且未被访问的栅格作为下一次规划的位置。

S4、利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径，具体步骤包括：

S41、初始化随机树T及其参数，所述参数包括初始位置q_init，目标位置q_goal，随机树T的生长步长λ；

S42、构建目标节点q_goal对随机树T中的节点q的引力；

首先建立目标节点的引力势场，引力势场为：

则目标节点q_goal与当前节点q之间的目标引力F_goal为：

F_goal＝k_goal||q_goal-q||；

其中k_goal是引力系数；

S43、构造随机树新节点：如图4所示，图示为随机树新节点的构造方法，具体以特定概率在全局占据栅格地图中随机采样得到随机点q_rand，选择随机节点q_rand的最近节点q_near，在目标引力F_goal的作用下，新节点q_new的位置会向目标点方向偏转，q_new的计算公式为：

判断若q_near与q_new之间的连线没有障碍物，将q_near作为q_new的父节点，q_new作为q_near的子节点，再继续随机采样扩展随机树T，否则舍弃当前新节点q_new，重新随机采样；

如图5所示，图示为人工势场引导的RRT路径规划方法的算法流程图，

S44、循环搜索，当随机树T新的叶节点q_new扩展达到目标节点附近时，从目标节点q_goal通过对应的父节点反向追溯到根节点q_init，得到连接q_init到q_goal的无碰撞路径；其中节点的位置记录在路径点集合O中。

如图6所示，图示为人工势场引导的RRT路径规划算法规划出的路径示意图，具体是根据对规划出的路径进行节点剪枝后处理和路径平滑后处理，从而得到一条平滑的无碰撞的路径。

S45、节点剪枝后处理：从随机树T的根节点开始依次连接后序路径点，当路径点之间的连线与膨胀后的障碍物不相交时即可删除两者之间的路径点，依次循环下去，当与新节点之间出现障碍物时，以新节点的父节点为新起点重复上述操作，直至到达目标路径点为止，得到平滑的路径，如图7所示，图示为RRT节点剪枝后的路径。

S46、路径平滑后处理：对于剪枝后得到的路径，使用贝塞尔曲线进行平滑处理，得到平滑无碰撞的路径，如图7所示，图示为路径平滑处理后的路径。

S5、自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图；具体步骤包括：

获取上述步骤S4中生成的路径，再建立自主无人车辆的车辆动力学模型，并使用模型预测控制方法进行路径跟踪；

在路径跟踪过程中，使用激光SLAM方法构建车辆实时位置的环境地图，并同步到全局占据栅格地图，同时更新全局占据栅格地图中各个区域的障碍物数量；

同时在路径跟踪的过程中，将视觉检测系统扫描过的范围从车辆坐标系转换到全局占据栅格地图的坐标系，从而获取已访问全局占据栅格集合P；

对集合P中的每个栅格坐标(x_i,y_i)，计算该坐标在全局占据栅格地图中对应的区域π_i；

若区域π_i中未访问栅格坐标集合中存在该元素，则删除该元素，并将区域π_i中未访问栅格数量/>减一；

若区域π_i中未访问栅格坐标集合中不存在该元素，对集合P中的下一个元素重复上述操作。

S6、判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置，具体步骤包括：

当自主无人车辆的视觉检测系统扫描到特定目标并成功识别时，将特定目标的图像与三维点云数据进行匹配，得到特定目标的准确位置；

再使用步骤S4中规划出的一条平滑无碰撞的路径进行跟踪，并控制车辆到达目标位置，完成搜索任务。若未发现特定目标，返回步骤S3重复操作，确定下一次的目标位置。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。

Claims (5)

1.一种未知环境下用于特定目标搜索的路径规划方法，其特征在于，包括：

构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物；

将全局占据栅格地图划分为若干个区域，并构建区域到全局占据栅格地图之间的映射关系，其具体步骤包括：

将全局占据栅格图平均划分为若干个矩形区域，每个矩形区域π_i具有规定数量的占据栅格，其中当边缘部分的区域π_i中的栅格数量少于规定数量时，表示剩余的栅格被障碍物占据；

所述每个矩形区域π_i设置有区域属性，其中包括区域位置坐标(π_x,π_y)、障碍物数量未访问栅格数量/>和未访问栅格坐标集合/>

根据就近原则、未访问优先原则和远离障碍物原则确定当前车辆位置下一次的目标位置，其具体步骤包括：

首先根据就近原则，确定车辆当前所在区域与待搜索区域之间的欧几里得距离作为距离代价C_d；

根据未访问优先原则，确定待搜索区域的未访问栅格数量作为未访问栅格数量代价C_nvg；

根据远离障碍物原则，确定待搜索区域的障碍物数量作为障碍物代价C_ob；

根据上述距离代价C_d，未访问栅格数量代价C_nvg和障碍物代价C_ob建立代价函数，并选择代价最小的区域π_i作为下一次访问的目标区域，所述代价函数为：

J_cost＝ω_dC_d+ω_nvgC_nvg+ω_obC_ob；

其中，ω_d为距离代价C_d的权重系数，ω_nvg为未访问栅格数量代价C_nvg的权重系数，ω_ob为障碍物代价C_ob的权重系数；

在目标区域π_i内将所有的障碍物向外膨胀距离ρ，再在该目标区域π_i内随机选择一个未被障碍物占据且未被访问的栅格作为下一次规划的位置；

利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径；

自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图；

判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置。

2.根据权利要求1所述一种未知环境下用于特定目标搜索的路径规划方法，其特征在于，所述构建待搜索未知区域的全局占据栅格地图，并投影自主无人车辆的当前位置周围的障碍物的具体步骤包括：

根据待搜索未知区域构建全局占据栅格地图，该区域的目标中心作为地图中心，并建立坐标系；

通过自主无人车辆的环境感知系统获取车辆当前位置周围的障碍物，构建局部占据栅格地图；

将局部占据栅格地图进行匹配校正，通过坐标转换映射到全局占据栅格地图。

3.根据权利要求1所述一种未知环境下用于特定目标搜索的路径规划方法，其特征在于，所述利用人工势场引导的RRT路径规划算法规划出自主无人车辆当前位置到目标位置的路径的具体步骤包括：

初始化随机树T及其参数，所述参数包括初始位置q_init，目标位置q_goal，随机树T的生长步长λ；

构建目标节点q_goal对随机树T中的节点q的引力；

首先建立目标节点的引力势场，引力势场为：

则目标节点q_goal与当前节点q之间的目标引力F_goal为：

F_goal＝k_goal||q_goal-q||；

其中k_goal是引力系数；

构造随机树新节点：以特定概率在全局占据栅格地图中随机采样得到随机点q_rand，选择随机节点q_rand的最近节点q_near，在目标引力F_goal的作用下，新节点q_new的位置会向目标点方向偏转，q_new的计算公式为：

判断若q_near与q_new之间的连线没有障碍物，将q_near作为q_new的父节点，q_new作为q_near的子节点，再继续随机采样扩展随机树T，否则舍弃当前新节点q_new，重新随机采样；

循环搜索，当随机树T新的叶节点q_new扩展达到目标节点附近时，从目标节点q_goal通过对应的父节点反向追溯到根节点q_init，得到连接q_init到q_goal的无碰撞路径；

节点剪枝后处理：从随机树T的根节点开始依次连接后序路径点，当路径点之间的连线与膨胀后的障碍物不相交时即可删除两者之间的路径点，依次循环下去，当与新节点之间出现障碍物时，以新节点的父节点为新起点重复上述操作，直至到达目标路径点为止，得到平滑的路径；

路径平滑后处理：对于剪枝后得到的路径，使用贝塞尔曲线进行平滑处理，得到平滑无碰撞的路径。

4.根据权利要求1所述一种未知环境下用于特定目标搜索的路径规划方法，其特征在于，所述自主无人车辆根据规划路径进行路径跟踪，同时进行激光SLAM建图并更新全局占据栅格地图的具体步骤包括：

建立自主无人车辆的车辆动力学模型，并进行路径跟踪；

在路径跟踪过程中，构建车辆实时位置的环境地图，并同步到全局占据栅格地图，同时更新各个区域的障碍物数量；

车辆坐标系转换到全局占据栅格地图的坐标系；

获取已访问全局占据栅格集合P；

对集合P中的每个栅格坐标(x_i,y_i)，计算该坐标在全局占据栅格地图中对应的区域π_i；

若区域π_i中未访问栅格坐标集合中存在该元素，则删除该元素，并将区域π_i中未访问栅格数量/>减一；

若区域π_i中未访问栅格坐标集合中不存在该元素，对集合P中的下一个元素重复上述操作。

5.根据权利要求1所述一种未知环境下用于特定目标搜索的路径规划方法，其特征在于，所述判断是否搜寻到特定目标，若搜索到特定目标则搜索结束，否则确定下一个目标位置的具体步骤包括：

当自主无人车辆的视觉检测系统扫描到特定目标并成功识别时，将特定目标的图像与三维点云数据进行匹配，得到特定目标的准确位置；

最后规划出到特定目标的路径，并进行路径跟踪到达目标位置，完成搜索任务。