刘海鸥，韩雨轩，刘庆霄，李世豪，陈慧岩，陈力
（1. 北京理工大学 机械与车辆学院，北京 100081；
2. 北方信息控制研究院集团有限公司，江苏 南京 211153）
侦察任务是一种在野外非结构化环境下执行的作战任务类型，地面无人机动平台需要在指定区域内搜索隐蔽点并进行隐蔽，在隐蔽后侦察远距离目标. 在“跨越险阻”装备竞赛的侦察任务中，隐蔽点坐落在直径300～500 m 的不确定环境内[1]. 侦察任务因环境具有较强的随机性和缺少高精度电子地图，是一种不确定环境下进行搜索的任务类型. 另一方面，侦察任务要求地面无人机动平台具备距离300～500 m 的探测能力，远高于通常结构化城市道路的10～80 m 的探测距离[2]，也高于非结构化道路行驶的100 m 探测距离[3]. 搜索任务包括感知目标物体并基于地图进行路径规划.
国内外学者针对搜索任务和无人机动平台在不确定环境中的搜索方法展开了广泛的研究工作，目标搜索方法包括了图搜索和基于采样的搜索方法，较为经典的搜索方法有深度优先搜索[4]、广度优先搜索[5]、启发式搜索、随机搜索和有限状态机搜索[6].侦察任务中地面无人机动平台处于大范围的开阔环境，AMIGONI 等[6]的研究表明随机搜索方法整体效果优于深度优先搜索、有限状态机搜索、启发式搜索等方法，更适用于在开阔区域的搜索任务. 基于随机搜索方法有：概率道路图法（probabilistic roadmap method，PRM）和快速随机搜索树法（rapidly-exploring random tree，RRT）及其衍生算法，快速随机搜索树法可以不依靠全局路点地图且搜索距离可调适合在大范围环境下进行搜索任务.
SCHMID 等[7]为解决RRT 算法在随机采样陷入局部最小值导致次优轨迹的问题，提出了一种新的RRT*启发的在线信息路径规划算法并基于TSDF 的3D 重建方法获取环境信息用于路径规划. WANG等[8]在已知全局地图的基础上提出一种弹性带的分层框架快速随机搜索树算法，该算法通过基于时间的RRT 算法和重建地图生成可行的启发式路径，并在RRT 地图搜索中不断优化启发式路径，使得搜索效率达到最高. HU 等[9]使用路径变形策略扩展了快速随机搜索树算法，该策略将航路点从碰撞点移开，从而可以在没有任何碰撞的情况下生成搜索轨迹，搜索轨迹不互相重叠. 韩丰建等[10]提出了一种目标导向的快速随机搜索树算法，命名为Bi-goal. 该算法通过目标导向思想对随机树中采样点的产生进行改进，引导随机树偏向目标点生长，提高了收敛速度，降低了扩展过程的复杂度；
搜索过程中只考虑了目标方向而没有考虑会出现搜索超过目标点的情况.DELMERICO 等[11]通过无人履带车辆和无人机的异构载体方式进行主动空中搜索，由无人机构造环境高程地图并重建3D 信息，通过三维信息估计全局地图. 基于全局地图采用RRT 方法进行路径规划. 该方法相比于直接全区域穷举和贪婪算法搜索策略相应效率提高，在室外情况下响应速度快，效果较好.
研究现状表明，在开阔环境以及全局地图下的搜索任务中，RRT 搜索方法相比于启发式算法具有更高的搜索效率[12]. 但是RRT 搜索方法在侦察任务中主要存在以下两个问题：①导向RRT 方法忽略了不同探测距离传感器对搜索效率的影响；
②由于采样完全随机，在行进方向上存在障碍物狭窄通道时通行效率降低. 因此，本研究主要针对不同探测距离的传感器在搜索方法中的应用与优化问题，以及RRT 搜索方法缺少方向性导致的路径规划效率较低问题开展研究工作.
文中提出了一种基于不同探测距离的传感器在搜索任务中建图并采用导向快速随机搜索规划路径的搜索类型任务策略[12]，命名为：组合传感器与规划组合搜索策略（combined planning path and combined sensor search method, CP&CS）. 以装备了光电平台、相机和激光雷达的地面无人机动平台为例，在未知环境中进行目标隐蔽点搜索. 该平台通过远距离传感器发现目标隐蔽点和环境障碍物，并根据传感器感知结果构造搜索方向上的地图，以目标定位情况构造搜索目标方向的正态分布，根据正态分布使用基于传感器的导向快速随机搜索树方法（sensor based goaled rapidly-exploring random tree ,SG-RRT）方法，并对环境的凹形障碍物进行处理；
在SG-RRT 方法规划的路径运动过程中检测到障碍物构成狭窄通道，基于环境障碍物结果构建适应的局部地图，使用启发式A*方法进行局部狭窄通道的路径规划，由于A*方法的路径规划采样效率高于RRT 及其衍生方法随机采样效率，有效解决了面对狭窄通道SG-RRT 方法通行效率低的情况. 通过仿真试验的定性与定量分析验证了文中搜索策略的优越性.
为实现侦察任务要求，基于长探测距离光电平台、短探测距离的激光雷达和短焦相机、SG-RRT 方法、A*规划方法设计如图1 所示的CP&CS 搜索策略.
图1 CP&CS 搜索策略逻辑流程图Fig. 1 Logical flow chart of search method CP&CS
其中长探测距离搜索目标点并对目标点结果进行测距实现定位，定位结果作为侦察任务的路径终点约束；
短探测距离传感器对无人激动平台周围的障碍物进行检测，并确定障碍物的位置情况，约束路径不发生障碍物碰撞.
地面无人机动平台行驶进入搜索任务区域后，驱动长探测距离的光电平台对目标隐蔽点进行搜索与定位；
若未检测到目标，地面无人机动平台围绕起始点以螺旋轨迹线在搜索目标区域内运动继续搜索目标点，直到光电平台搜索到目标点. 当地面无人机动平台通过光电平台的感知信息搜索到目标隐蔽点时，基于正态分布计算搜索方向的分布，并根据分布结果使用SG-RRT 方法规划路径. 沿方向概率下进行随机搜索方向规划路径前进，前进过程中对SG-RRT方法生成的路线进行平滑处理，最终输出符合车辆动力学控制要求的底层控制信号. 狭窄通道对于随机采样效率较低，造成SG-RRT 方法的路径规划效率降低. 在运动过程中，使用短探测距离感知环境障碍物信息，若检测到有障碍物形成的狭窄通道时，使用A*方法重新进行路径规划. 根据A*规划方法的路径平滑后控制车辆来通过狭窄通道[13]. 使用传感器感知动态变化的侦察任务环境，并根据感知结果确定不同阶段使用的路径规划算法，有助于提高在侦察任务规定时间内无人机动平台准确停在目标隐蔽点的概率，从而提高了侦察任务的成功率.
1.1 传感器目标搜索与定位
传感器进行目标搜索和定位使用深度学习网络，在实验过程中需要保证60 帧的深度学习网络处理速度满足相机和光电平台的采样速度，同时平台算力为430TOPS. 在400 m 的范围内目标物体的大小占据70*70 以内的像素范围. 根据算力要求和目标像素大小的要求，本研究基于单阶段深度学习网络Yolo v5、DeepSort 进行目标检测和追踪[14]，确定并跟踪检测结果，计算各个检测结果与当前光电平台的定位关系与定位误差，分类检测结果为障碍物和目标隐蔽点. 在使用过程中可以考虑到平台算力的要求，可以选用如SSD 等其他深度学习网络输出搜索和定位结果完成相关任务[15].
Yolo v5 是一种基于一体化卷积网络的目标检测算法，具有单层次目标检测结构，检测速度更快适合非结构化环境运动检测. 通过直接回归的方式一次处理并标记对应物体的边界框、类别、特征和计算置信度，根据目标检测结果将类别分为目标隐蔽点和运动障碍物[16].
图2 所示DeepSort 算法包括了预测模块、匹配模块和追踪模块，基于Yolo v5 目标检测结果作为跟踪输入，主要输入检测结果边界框、置信度、特征.其中置信度主要用于检测框的筛选；
边界框与特征用于与跟踪器的匹配计算. 预测模块利用卡尔曼滤波器预测并更新追踪的效果；
匹配模块将追踪预测结果与Yolo v5 的检测结果利用并交比（intersection over union，IOU）区域的大小进行匈牙利算法的匹配；
匹配成功后在追踪模块输出，提供给下一帧进行新的卡尔曼滤波预测；
如有网络检测结果但是没有相应的追踪结果则创建新的追踪id 进行追踪；
如果追踪预测结果存在但是在时间阈值内没有匹配到检测结果，则丢弃追踪结果. 通过卡尔曼滤波、IOU 区域和匈牙利算法匹配和追踪，实现基于Yolo v5 和Deep-Sort 目标检测与追踪算法.
图2 Yolo v5 网络输入结果进行DeepSort 目标追踪Fig. 2 Using DeepSort for object detection based on Yolov5 result
1.2 SG-RRT 方法
传统RRT 方法随机向所有扩展，搜索效率不高；
而导向RRT 方法解决了搜索方向的问题[16],但是很少配合不同探测距离的传感器使用，单探测距离传感器的搜索效率不高. 文中通过在融合不同探测距离的传感器，使用长探测距离搜索目标并根据正态分布确定搜索方向的导向RRT 方法提高搜索效率，并命名为SG-RRT. 已知光电平台角度定位误差且随目标点距离D增大而增大，误差与目标距离成正比；
目标检测准确度P降低也会导致角度定位误差增大，误差与准确率成反比. 通过正态分布确定RRT 随机搜索的搜索角度分布，设置均值为目标在大地坐标系下的角度 θ，确定方差 σ，图3 对比RRT 和SG-RRT正态分布概率，导向搜索的概率特征性高于RRT 随机搜索.
图3 SG-RRT 与RRT 扩展概率对比Fig. 3 Comparison of SG-RRT and RRT extension probabilities
由方差、搜索角度均值得到式（2）的采样分布情况θ(x)
根据传感器回传结果和定位结果，构造基于不同探测距离传感器的SG-RRT 搜索方法. 算法1 输入Yolo v5 检测到并能被DeepSort 算法模块稳定追踪的目标结果，输入包括目标检测的测量距离D、目标检测的准确度P和目标在大地坐标系下的测量角度θ，确定搜索的正态分布情况θ(x)，根据长距离传感器识别的相关障碍物构造搜索方向上的地图C.
从起点开始运动当前点不是目标点则开始迭代，图4(a)中搜索方向概率一致的RRT 算法向所有方向扩展，文中的SG-RRT 方法如图4(b)向有目标的概率方向上以正态分布的形式进行随机搜索，通过Sample()函数在地图上进行采样叶节点集合. 根据Near()函数在选取叶节点集合中靠近上一个叶节点的点作为新的扩展点. 在扩张点的基础上使用Steer()函数在步长和新扩展点选区的双重约束下选取搜索树添加的新点；
经过碰撞检测后将新点添加到新的搜索树中. 通过不断按照搜索概率方向的正态分布方法进行采样，不断延展采样搜索树直到到达目标点，连接到达目标点的相关采样树的各部分节点组成到达目标点运动轨迹数组，如图4(b)所示构成导向RRT 树的路径规划结果，基于目标角度区间的正态分布和地图采样取并集后，文中SG-RRT 方法采样有特定方向和目标大致位置参数，其搜索过程中不会出现传统导向RRT 算法搜索过程中搜索范围超过目标点原本范围的情况. 并经过符合车辆运动学特征的平滑处理后输入到车辆底盘进行控制，实现路径规划.
图4 500 m×500 m 仿真环境中圆形和扇形部分是搜索范围Fig. 4 Roundness and sector search field in 500 m×500 m simulation environment
1.3 启发式A＊规划方法
SG-RRT 算法采用随机采样对于狭窄通道采样稀疏的情况，搜索效率低于启发式搜索方法. 根据这一缺陷，提出基于传感器检测的狭窄通道的方法. 方法通过传感器确定障碍物位置并判断是否出现影响随机采样的狭窄通道，根据图5 结果使用启发式A*算法通过障碍物狭窄通道.
图5 500m×500m 仿真环境中遇到障碍物之间狭窄通道情况进行路径规划Fig. 5 Path planning for narrow passage cases encountered between obstacles algorithm for path planning in 500m×500m simulation environment
向量夹角关系 φ确定当前狭窄通道是否影响了路径规划.
算法2 通过传感器感知环境，通过式（10）确定是否存在影响导向RRT 方法路径规划的障碍物，在确定存在影响障碍物后，通过式（11）～（14）计算局部启发式算法的起始点和终点的坐标S和G，根据坐标结果和障碍物识别定位结果构造局部地图Cpart. 在局部地图上从起始点的邻域开始搜索，同时维护一个开集合保存查询的路点信息，闭集合保存代价函数较近的点，代价函数直接选用已有的A*算法代价函数. 在邻域点没有和障碍物发生碰撞的前提下，计算子节点和终点的欧式距离，将与终点欧氏距离较小的子节点放入闭集合继续扩展，直到目标点扩展到了目标点，将所有闭集合目标点进行回溯，找到最短距离的路线Rpath. 将最短距离路线作为路径规划结果，经过路径平滑后输入底盘控制模块，在遇到障碍物形成行进方向上相关的狭窄通道时，使用启发式
2.1 试验平台介绍
试验平台为如图6 (a)的双侧独立电驱动履带无人机动平台，如在图6 (b)的Vrep 仿真软件中构造环境进行仿真（白色为垂直墙、红色为障碍物水马、黄色为搜索任务目标点），并将仿真结果同步在使用matlibplot 库绘制路径. 考虑到越野性能和检测的准确性，设置平台最高速度不超过5 m/s，具有目标隐蔽区的试验场地直径为500 m.目标隐蔽区域直径为6 m.实验平台搭载了由相机、激光雷达组成的短探测距离传感器组合和长探测距离的光电平台，具体参数由表1 所示. 实验使用的地面无人机动平台及主要传感器布置如图6 所示，平台参数如表2所示.
表1 光电平台参数Tab. 1 Parameter of photoelectric platform
表2 无人机动平台参数Tab. 2 Parameter of unmanned platform
图6 无人机动平台中目标搜索与定位系统Fig. 6 Target search and position of unmanned maneuver platform
光电平台被安装在固定高度架上，光电平台光心轴线、固定高度架轴线与平台质心前向轴线重合.若要保证越野环境的平稳性，则需降低固定高度架的离地高度A；
而若要减少搜索盲区和遮挡，则要增高固定高度架高度. 固定高度架的高度在平稳性和搜索盲区的双重约束下经过式（15）的光电平台架设高度最优化设计、式（16）光电平台安装位置对车辆运动稳定性的约束条件、式（17）光电平台的最小目标探测范围. 其中光电平台与质心在质心轴线上距离差为0.5m.
经过最优化设计得到，光电平台光心位置距离地面高度A为2.2 m.光电平台俯仰视场角 τ 为 30°. 以如图7 所示的俯仰角度为0°例，计算最近测量距离Lmin，根据式（18）结果得到此时光电平台可以搜索到平坦地面7.71 m 半径范围外的目标隐蔽点. 通过光电平台长焦相机焦距以及光电平台高度确定的光电平台传感器长探测距离为200 m；
根据表3 所示相机与激光雷达的参数，确定短探测距离为25 m.
图7 光电平台俯仰角度对测距影响Fig. 7 Effect of photoelectric platform pitch angle on ranging
表3 相机参数Tab. 3 Parameter of camera
2.2 试验流程
无人机动平台进入搜索区域并采用不同传感器融合进行搜索. 试验以仿真试验形式完成，分别对比：文中提出的搜索策略；
HU 等[9]提出的短探测距离传感器的路径优化RRT 搜索策略；
韩丰建等[10]提出的基于长探测距离传感器的目标导向的快速随机搜索树（Bi-goalRRT）搜索策略；
在文中提出搜索策略中对比了遇到障碍物狭窄通道的情况下，使用A*算法和动态A*算法的差异.
考虑到Vrep 仿真软件的传感器结果与实际相差较大，使用数组存储障碍物信息的方式进行传感器检测结果的实际性质约束，模拟使用了Yolo v5 和DeepSort 组合的目标追踪结果的范围. 根据长探测距离和短探测距离分别为200m 和25m 的参数设置障碍物定位信息的数组大小，短探测距离传感器的搜索数组为25×25 栅格，长探测距离传感器的搜索数组为200×200 栅格，根据数组大小输入环境障碍物情况，模拟使用不同探测距离传感器进行障碍物检测并规划轨迹的情况.
目标隐蔽点设置为5 个油桶并排排列情况，障碍物包括人工制造的垂直墙和红色塑料水马. 每次仿真试验后记录在图8 所示的区域内搜索并停入目标隐蔽点的花费时间并对比规划轨迹所用时间确定算法效率，并对比最终规划路径的长短情况，确定行进所需要花费的时间.
图8 进入搜索区域进行目标搜索并停靠目标点Fig. 8 Search area for target search and dock the target point
以上试验流程分别进行100 次的仿真试验，每次实验从地图中心出发，以恒定的5 m/s 速度运动，生成的目标点与起始点的直线距离大于150 m. 如图9中的地图所示，随机生成8 个水马大小的障碍物，平面图中标记为圆形；
8 个垂直墙大小的障碍物，标记为矩形. 其中环境中央的正方形点为搜索任务的起始点、绿色正方形点为搜索任务的目标点.
图9 仿真环境随机生成无障碍通道地图Fig. 9 Map generated randomly without barricade in simulation environment
在实验过程中，由于路径规划过程中将无人机动平台视为一个质点进行规划，规划任务不具备无人机动平台的外形约束特性，为满足外形约束则需要相应膨胀障碍物的大小适应无人机动平台实际的外形轮廓，在无人机动平台参数中已知无人机动平台的外形尺寸，根据外形尺寸将障碍物大小膨胀. 其中，水马类型障碍物膨胀后占据5×5 个栅格、垂直墙类障碍物膨胀后占据30×5 个栅格以适应车辆外形尺寸要求. 同时考虑到履带式底盘在一定运动速度下存在转弯半径和滑移率，进一步将得到的路径结果进行平滑，满足无人机动平台在越野环境下的运动.
图9 在空旷环境下生成没有构成通道的搜索情况，而图10 则生成了搜索区域内出现了通道环境的搜索情况，宽阔区域定义为是没有构成障碍物狭窄通道区域的情况，分别运行三种搜索方法，寻找路径并进行三种方法仿真，记录规划的平均所需时长、规划中路点个数和规划路径的长短. 图11 中比较没有出现障碍物狭窄通道前和出现障碍物狭窄通道后的平均所需时长、规划中路点个数和规划路径的长短，验证文中提出的CP&CS 方法的有效性与准确性.
图10 仿真环境生成有障碍物通道的地图Fig. 10 Map generated randomly with barricade in simulation environment
图11 基于随机生成地图在仿真环境下在有障碍物狭窄通道组合轨迹Fig. 11 Narrow passage combined trajectory based on randomly generated maps in the simulation environment with barricade
表4 中对比不同方法在宽阔区域进行搜索任务的过程中所规划的平均路径长度、使用的平均路径点个数和平均规划时长. 表5 中则对比不同方法在有障碍物狭窄通道的情况下进行搜索任务的过程中所规划的平均路径长度、使用的平均路径点个数和平均规划时长. 通过对比不同算法的表现，可以发现空旷环境下的搜索过程中路径规划所花费的时间文中的CP&CS 算法和导向RRT 算法所花费时间基本相同，搜索的路径效率也相同，导向方法相比于没有导向的RRT 方法搜索效率得到了提高，进行定量分析可以，带有导向的CP&CS 算法比文献[9]的无方向RRT 搜索策略效率平均提升了3.11 倍，搜索的路径长度缩短了9.63%；
遇到障碍物狭窄通道时，文中提出RRT 与启发式A*搜索策略的优势更加明显，相比于文献[10]在未知区域的搜索策略过程中耗时更短，路径长度也显著缩短，定量分析来看，策略效率平均提升了3.53 倍，搜索的路径长度缩短了12.06%.
表4 宽阔区域仿真试验统计Tab. 4 Time of search task in simulation
表5 通道区域仿真试验统计Tab. 5 Time of search task in simulation
在遇到狭窄通道的过程中，通常使用启发式算法规划路径，构造满足如式（12）的障碍物狭窄通道，对比使用A*的启发式算法和DijKstra 的非启发式算法的规划效果. 进行1 000 次仿真后效果如表6所示.
表6 使用A＊算法和DijKstra 算法试验统计Tab. 6 Time of search task in simulation
通过实验对比，发现A*算法和DijKstra 算法的规划效果中，采用了启发式和非启发式算法，对于狭窄通道的路径规划平均路径长度和轨迹点个数差距不大. 由于DijKstra 算法没有目标位置进行约束，路径搜索的过程中向所有方向进行扩展搜索，表现规划的平均时间较长. 故文中选择启发式的A*算法作为文中在狭窄通道的搜索策略.
综合已有的规划时间和道路长度结果，文中提出的CP&CS 搜索策略应用了多种探测距离的传感器进行侦察任务的搜索，在搜索过程中以优化了路径规划方法相较于短探测距离传感器的RRT 算法和长探测距离传感器的Bi-goal RRT 的导向搜索方法的搜索效率以及搜索的简洁性有效得到提高，文中提出的CP&CS 搜索策略在侦察任务中的搜索子任务表现效率优势明显[17].
使用文中提出的在侦察任务中搜索目标隐蔽区规划路径的搜索策略，显著提高了搜索的成功率，应用感知传感器的手段优化了路径规划算法. 文中提出的方法具有以下优势：
①在陌生环境建图过程中采用多种探测距离传感器的探测手段，探测效率与探测精度同时得到提高.
②融合不同路径规划算法，在高于道路范围的区域内进行搜索的过程中采用长探测距离的传感器并建立搜索区域地图，基于地图采用可调步长导向的RRT 方法并进行规划. 靠近障碍物之间形成的狭窄通道切换到A*算法进行路径规划. 总体实验效果良好，特别是对于障碍物狭窄通道后的目标隐蔽点搜索效率提高，融合了随机采样和启发式算法的共同优点. 使侦察任务的搜索效率提高，证明了算法的优越性.
猜你喜欢障碍物距离规划高低翻越动漫界·幼教365(中班)(2020年3期)2020-04-20SelTrac®CBTC系统中非通信障碍物的设计和处理铁道通信信号(2020年9期)2020-02-06算距离小学生导刊(2018年34期)2018-12-18规划引领把握未来领导决策信息(2018年50期)2018-02-22快递业十三五规划发布商周刊(2017年5期)2017-08-22多管齐下落实规划中国卫生(2016年2期)2016-11-12每次失败都会距离成功更近一步山东青年(2016年3期)2016-02-28迎接“十三五”规划中国工程咨询(2016年4期)2016-02-14爱的距离母子健康(2015年1期)2015-02-28距离有多远延河(下半月)(2014年3期)2014-02-28