一种适用于崎岖地形的移动机器人自主探索方法

本发明涉及移动机器人，尤其涉及一种适用于崎岖地形的移动机器人自主探索方法。

背景技术：

1、随着移动机器人平台的飞速发展，移动机器人技术在实体工业、农业、医疗、卫生、服务、军事和航空航天领域均得到了广泛的应用，其中在复杂地形以及不平坦的特殊地形作业的移动机器人领域，仍有很多难点亟待突破。

2、针对特殊环境的移动机器人领域，尤其是大规模和复杂的具有不平坦山地或深坑的野外地形勘探，主要包含两个核心技术：自主探索和不平坦地形评估。自主探索是实现移动机器人(unmanned ground vehicle，即ugv)自主安全、快速高效地构建规定范围的环境模型。移动机器人需要在完全未知的环境中，仅通过自身的板载传感器，不需要与外界通讯，自主完成场景的重建、探索决策、路径生成、轨迹优化等任务；不平坦地形评估是指，移动机器人针对未知环境中不可穿越的陡坡和深坑等不平坦危险区域的可通行代价进行评估，使移动机器人找到一条安全、可通行的轨迹进行探索，对于上述复杂的室外工作环境，移动机器人需要高度自主化、鲁棒、可靠地应对各类崎岖地形以及各种突发情况，从而保证机器人依据全局路径安全稳定地运行。

3、目前，自主探索已经在许多领域得到广泛应用，包括工业检测、object search和场景重建等。尽管如此，在大规模和复杂的具有不平坦山地或深坑的野外勘测、农林管理等场景，如何安全有效地探索是一项极具挑战性的任务。未知环境的探索方法大致分为以下三类：基于边界的探索方法、基于学习的探索方法、基于采样的探索方法；前两者中往往通过获取边界的信息增益来引导探索决策，从而忽略地形因素，该探索方法常用于结构化或平缓地形下的环境，通过简化不平坦区域的地形分析获取可通行区域便可执行探索任务。然而，野外环境下ugv更加关注平面信息，该环境存在ugv不可穿越的陡坡和深坑等不平坦危险区域，容易造成区域可穿越性的模糊性而导致ugv碰撞侧翻或者舍弃实际可通行区域，从而无法实现野外崎岖环境的完全探索，现有的探索规划方法没有行之有效的解决方案。本方法使用采样的方法，分析采样点下地形的地貌特征和可通行代价，引导机器人生成一条可穿越非平坦地形的路径。

4、综上，移动机器人针对野外非平坦山地地形评价方法，承担着野外环境探索的重要功能。该方法主要应用于军事野外地形勘测，而野外地形多存在不平坦特性，野外非平坦地形探索评价方法主要集中在：不平坦区域的地形分析获取安全的可通行区域；和非结构化环境影响ugv的探索效率；机器人通过板载传感器实时分析地形可通行代价，排除掉常规算法可能会出现的危险路径，例如穿越山脊陡坡、和无法感知的深坑地形，综合评价可通行路径代价，保证无人车安全性。因此，设计适用于野外崎岖地形的探索方法，是移动机器人实现野外安全探索的基础，如何采用更小的能量消耗和更低的算法复杂度，获取未知野外的地形以及环境信息，同时考虑路径安全程度，实现移动机器人野外崎岖地形自主探索，是该项发明的主要目标。

5、现有的室外移动机器人自主探索方法，多是针对平坦地形或者平面复杂度较低的场景设计的，在面对崎岖山地、复杂草地、深坑地形时，可能存在地形探索不完全，造成计算资源浪费，或是无法保证移动机器人安全的问题，移动机器人无法在行动中实时针对崎岖地形提取有效的信息增益，无法判断深坑地形和陡峭地形的可通行，忽略环境地形增益的同时，降低了探索效率。对于三维环境表示方法主要有点云、数字高程模型、八叉树等等，上述表示形式虽然可以完全表征三维环境信息的同时，存在内存开销过大的问题。常用的路径规划算法均采用栅格地图进行地图表示，点云与数字高程模型不能被直接运用到移动机器人路径规划中，八叉树地图作为三维栅格地图的一种改进，在一定程度上解决了存储开销大的问题，同时也带来了计算复杂度高的问题。目前，移动机器人平台的存储与计算能力较弱，较高的计算复杂度和较大的存储开销导致算法不稳定性的提升。

6、《中国专利cn113805590a一种基于边界驱动的室内机器人自主探索方法及系统》提出了一种基于边界驱动的室内机器人自主探索方法及系统，该方法通过激光雷达数据建立栅格地图，通过前沿边界点判决视野内最佳的前沿边界点，通过路径规划和运动控制驱动移动机器人探索所述前沿边界点。该方法对边界信息进行了增益计算，首先从栅格地图维度上触发，无法完全表征复杂的三维环境信息，无法感知环境信息，其次，该方法默认的运行环境为平坦地形，无法评估不平坦地形的可通行程度，未考虑陡峭地形以及深坑地形的路径采样情况，同时该方法在移动轨迹上具有多出不可导点，非理想状态下大机动很可能导致里程计漂移，导致机器人无法安全运行。

7、《中国专利cn115963832a基于深度强化学习的机器人自主探索方法》提出了一种基于深度强化学习的机器人自主探索方法，机器人获取工作空间的环境图，增量构建地图并检测地图的边界，得到拓扑地图，通过预训练的学习模型自主探索并执行策略，采用无模型强化学习技术在与物理或仿真系统试错交互的过程中学习探索策略。该方法使用强化学习方法，机器人在训练时通过行为和状态获得奖励，该方法并不具备对崎岖地形的评估，即机器人无法从地形的可通行程度中获得奖励，该强化学习模型并不具备崎岖地形的泛用性，未考虑陡峭地形以及深坑地形的路径的奖励以及惩罚情况。同时，仅考虑三维环境的边界增益，并未考虑动态及静态三维地形结构信息，复杂崎岖、深坑地形下容易造成移动机器人的危险系数提升，从而存在相应的安全隐患。

技术实现思路

1、本发明要解决的技术问题是针对上述现有技术的不足，提供一种适用于崎岖地形的移动机器人自主探索方法，在结合地形的相关信息的同时，能够避开深坑、崎岖地形，在保证移动机器人自身安全的同时完成野外探索。

2、为解决上述技术问题，本发明所采取的技术方案是：一种适用于崎岖地形的移动机器人自主探索方法，根据快速搜索树采样点附近的激光点云拟合平面；通过拟合平面的坡度、平整度、稀疏度和平面间的斜率变化量及其高度变化量分析地形的可通行性，从而构建快速搜索树以保证ugv的安全性；使用局部与全局相结合的分层探索方法，评价以该采样点所拟合平面的地形代价和环境信息增益代价，舍弃高代价的风险采样点，在采样点基础上生成安全的探索随机树；综合评价探索随机树的地形代价和环境增益，在保证地形代价满足通行条件的前提下取得环境探索增益选择局部探索路径，并同时构建稠密搜索图作为全局搜索树保证ugv全局路径的质量。具体包括以下步骤：

3、步骤1：确定移动机器人探索环境的表征方式，并定义探索问题；

4、探索环境的表征方式：设定为构型空间，为需要探索的环境空间，为局部探索的空间，v为指定的探索空间体积，ve为已经探索的环境体积，vre为剩余的探索体积，设定全局探索中无剩余待探索子分支后或者当ve/v达到95％，触发ugv返回起点；

5、探索问题定义:给定需要探索的环境空间m和局部探索的空间msub，根据可分析地形的局部探索框架生成一条安全的可通行路径，指引ugv到达未建图的区域；当局部区域探索完成，全局探索根据剩下的未探索分支树继续探索，迭代上述过程直至完成对需要探索的环境空间m的探索；

6、步骤2：针对局部的3d滚动栅格空间中空闲性质的体素进行随机采样；在空间上将采样点向地形表面投影获得一个地面上的点，以地面点为中心依据周围点云拟合平面，对单位平面和平面之间的地形评价指标进行计算，通过平面间转换的代价函数进行计算，优化最优的局部探索路径；

7、步骤2.1：为了保证通行区域的安全性引入对地形的评价指标，通过快速搜索树rrt*构建采样点，依据环境中激光信息拟合平面的坡度、平坦度、稀疏度和平面间的斜率变化量及其高度变化量评估地形的可穿越性；综合评估局部路径代价，求解最优的局部探索路径；

8、步骤2.1.1：扩展快速搜索树，并向下采集对应的地面点；

9、首先定义为快速搜索树(rrt*)的采样点、为快速搜索树采样点投影到地形表面的空间点，地面投影点的平面坐标不变，计算纵向变化计算公式为：

10、

11、其中，为投影到地面点的纵向坐标，z为采样点的空间纵坐标，k为计算次数，为正整数集合；xobs为栅格地图的占用栅格，res为栅格地图的分辨率；

12、此外还要满足zl为滚动栅格的最低高度，在局部的3d滚动栅格空间中空闲性质的体素进行随机采样；

13、步骤2.1.2：根据地面采样点进行svd平面拟合；

14、以投影到地形表面的空间点为中心依据周围点云拟合平面迭代执行上述向下采集对应地面点、根据地面采样点进行svd平面拟合两个过程得到一个搜索树以及相应节点的平面构成的ugv能够通过的带状区域；

15、步骤2.1.3：结合生成的拟合平面，计算评价指标进行地形评估；所述评价指标为包括拟合平面的坡度、平坦度、稀疏度和平面间的斜率变化量及高度变化量；

16、对应符号与计算方式如下：使用s，δsij，δshij，f，λ五个评价指标进行地形评估，s为该拟合平面的坡度，其计算公式为：

17、

18、其中，ks为常量，表示拟合平面法向量在世界坐标系z轴上的投影；

19、δsij为两个拟合平面的斜率变化量，可以对起伏频繁的路面会有较高惩罚，其计算公式为:

20、δsij＝si-sj

21、其中，si和sj为两个采样点xi和xj对应的拟合平面的斜率；

22、δshij为两个拟合平面的高度变化量，该评价指标限制了ugv不会选择平面间相对高度差较大的两个平面，其计算公式为:

23、

24、其中，zi和zj为对应的两采样点xi和xj的空间纵坐标；

25、f表示平坦度，它保证了ugv更倾向于平坦的地形运动减少能量消耗，其计算公式为：

26、

27、其中，n为当前采样点的数量，κf为常量，xj为第j个地面采样点的平面坐标，ez为拟合平面三维向量表示中的z轴一维向量；

28、λ表示稀疏度，它促使ugv远离未感知完全的地面，避免陷入深坑等危险地形，这里将深坑等价为空闲比率，当空闲比率超过阈值时，表明地面可能存在凹陷，其计算公式为：

29、

30、其中，r表示拟合平面中自由空间部分所占比率，即空闲比率，rmin为设定的空闲比率阈值下限，rmax为设定的空闲比率阈值上限，ttrace是判断矩阵迹大小的常数；

31、步骤2.2：根据搜索树节点及其平面的评价指标，计算搜索树节点间路径的局部代价，并针对两搜索树节点之间的计算结果评估可通行程度；

32、所述搜索树节点间路径的局部代价是f(ni,nj)，其计算公式如下：

33、

34、其中，ni,nj表示搜索树的节点，ω，ζ，η为均惩罚比例因子，τi表示采样点所在平面的地形代价；δshmax为两个拟合平面的最大高度变化量，用于归一化；δsmax为两个拟合平面的最大斜率变化量，用于归一化；

35、每个搜索树的节点包含了利用以l为边长的立方体内点云的拟合平面信息和所在平面的地形代价；令τi∈[0,1]，上限表示了该区域完全不可通行，而下限表示完全可以通行，τi的计算公式为：

36、

37、其中，smax为当前采样的所有平面中坡度最大的值，fmax为当前采样的所有平面中平坦度最大的值，λmax为当前采样的所有平面中稀疏度最大的值，α1,α2,α3是相加为1的权重系数；

38、步骤3：通过在快速搜索树的节点间中引入地形评价函数，使局部探索路径能够满足移动机器人在崎岖地形的探索需求，将探索过程分为局部探索和全局探索的分层框架，彼此之间通过条件判断完成状态切换，从而完成高效探索；

39、步骤3.1：在局部探索环节，移动机器人依赖快速搜索树，对树中落在叶子节点的点所引征的路径进行评估，使移动机器人在路径评估达到通行标准的前提下，向可获取信息增益最多的边界移动；

40、步骤3.1.1：设定快速搜索树拓展以及剪枝规则对快速搜索树进行拓展和剪枝操作，并确定路径集合；

41、首先令当前ugv的当前状态ψc为的根节点，设置采样最大上限为k次；每一次在中随机采样得到xi，经地形评估后得到然后，在快速搜索树中选择距离该随机采样点最近的节点判断两点的距离是否小于l/2，如果大于l/2则从快速搜索树上找到向延长l/2的距离作为新的采样点xnew，并向地面投影得到再对两采样点对应的拟合平面计算δsnn和δshnn得到两采样点间的地形代价f(ni,nj)，舍弃超过f阈值的节点其次，对与间进行碰撞检测，舍弃超过δshmax或有碰撞的路径；如果没有碰撞则计算与之间的代价f+μlnn，其中lnn为两节点的欧式距离，μ为权重系数；最后，在规定范围内遍历一组最近节点，判断这些节点与形成的地形距离代价是否小于原代价；如果小于原代价则将最小代价节点作为其父节点执行剪枝操作，迭代采样直到达到最大采样次数或有叶子节点到根节点超过距离阈值结束快速搜索树拓展的操作，保留局部搜索树在局部搜索树中搜索所有从根节点到各个叶子节点的路径，保留为路径集合∑l；

42、步骤3.1.2：在局部搜索树中搜索所有从根节点到各个叶子节点的路径，保留为路径集合∑l，并针对集合路径进行代价评估，在路径集合∑l中选择一条探索增益最大的路径；

43、以采样点为中心模拟激光射线，统计传感器有效范围内能够感知的未知体素的数量，再计算搜索树所有节点的总体积增益vxi；

44、设定路径集合∑l内的每条路径li的环境探索增益el(li)为搜索树所有节点的总体积增益vxi与距离、方向和地形有关的权重函数，如下公式所示：

45、

46、其中，s(li,lexp)为路径朝向，用来评价当前路径li和前一条路径lexp平均朝向的差异；mi为第i条路径中总共的采样点数量；表示从快速搜索树根节点到各顶点的累积欧式距离，为两搜索树节点对应的拟合平面的地形代价，γs、γd、γt分别为对应的权重系数；

47、上述评价路径权重环节迭代重复运行，直到达到设定的最大采样点数或快速搜索树的边数；

48、步骤3.2：按照步骤2中采样，投影，拟合和评价的流程，将局部搜索树加入构成全局搜索图；将局部搜索树中仍未被探索到的高探索增益的边界节点加入全局探索列表中，这些高增益边界节点表示了未被探索的边界点，同时该列表随着探索增益的变化周期性更新；对局部区域的完成探索后，通过djistra算法在全局搜索树中搜索到所有探索边界节点的最短路径∑lg，然后计算每条路径的探索收益值eg(lgi)，该计算公式为：

49、

50、其中，为当前节点到探索边界点的距离代价，为当前节点到探索边界点的地形代价，vf,i为边界节点体积增益，ξt、ξd分别为对应的权重系数；xc表示当前节点三维欧氏空间信息，nc表示当前采样点，表示目标叶子节点的三维欧式空间信息，表示目标叶子采样点；

51、步骤3.3：进行局部探索环节和全局探索环节分层探索状态的切换；当完成整个环境的边界探索或者探索范围达到设定比值ve/v，则搜索一条当前位置到出发点的距离代价和地形代价最小的路径。

52、采用上述技术方案所产生的有益效果在于：本发明提供的一种适用于崎岖地形的移动机器人自主探索方法，由于现实室外场景并不理想，不平坦地形、陡坡山地、深坑地形都会对移动机器人的自主探索决策带来巨大挑战。针对于未知不平坦环境固有存在的危险性，本发明根据快速搜索树采样点附近的激光点云拟合平面，通过平面的坡度、平整度、稀疏度和平面间的斜率变化量及其高度变化量分析地形的可通行性，从而构建快速探索树以保证移动机器人的安全性。同时在自主决策端，为了能够获取最大的环境探索增益，使用分层探索框架，评价以该采样点所拟合平面的地形代价和环境信息增益代价，在采样点基础上生成安全的探索随机树，综合评价探索随机树的地形代价和环境增益，在保证较小地形代价的前提下取得环境探索增益选择局部探索路径，并同时构建稠密搜索图作为全局搜索树保证移动机器人全局路径的质量。通过将两重要部分进行算法融合，验证了整体算法框架的有效性。通过多种类仿真环境和多场景的真实环境，对移动机器人室外复杂崎岖地形的自主探索方法进行了算法验证，证明了本发明提出的崎岖地形的自主探索方法的有效性、安全性和鲁棒性；

53、与现有技术比较，本发明的优点是：

54、1、可以针对移动机器人自主探索中室外崎环境，进行可靠的地形评估，通过平面的坡度、平整度、稀疏度和平面间的斜率变化量及其高度变化量分析地形的可通行性。减少了地形信息增益的损失。

55、2、使用分层探索框架，在局部采样点上生成安全的探索随机树，在保证较小地形代价的前提下取得环境探索增益选择局部探索路径，构建稠密搜索图作为全局搜索树保证移动机器人全局路径的质量，优化路径规划中求解过程耗时引发资源存储问题。

56、3、探索决策保证移动机器人获得最大增益，在候选路径中选择一条探索增益最大的路径。以采样点为中心模拟激光射线，统计传感器有效范围内穿过tsdf中voxel的为未知状态数量，再计算探索树所有节点的总体积增益。保证探索的目的性和可靠性。