一种利用可见性信息的实时路径规划方法

1.本发明涉及机器人路径规划以及避障导航技术领域，特别是涉及到一种利用可见性信息的实时路径规划方法。


背景技术：

2.自主移动机器人由于其智能和低风险的工作模式而被广泛用于搜索和救援，货物运输，地图绘制和探索等许多工作中。当机器人在未知环境中移动时，有效的导航策略将在探索过程中发挥重要作用。路径规划算法的时间开销及其鲁棒性将决定机器人能否实时选择合适的前进方向，并搜索出一条连接起点与终点且中途不会与障碍物发生碰撞的最优路径。
3.在传统方法中，路径规划一直是一项计算复杂且耗时的工作。相关领域研究人员往往倾向于使用基于采样的方法，例如rrg或rrt来解决此问题。过去的几十年中，在传统方法的基础上还出现了许多改进策略，例如rrt*和prm*。这些方法通过在工作空间占用图中重复随机选择路径节点来创建图或树，然后借助最短路径搜索算法(例如dijkstra算法)找到一条最佳路径。基于采样的方法通常需要随机采样大量的节点，这会增加算法计算时间，无法满足路径规划任务的实时性要求。除此之外，一些其他的图搜索方法(例如a*和d*算法)通常会将全局地图划分为许多单一网格或单元。为了提高最优路径的分辨率，获得较为理想的结果，网格的密度可能非常大，这就会增加运算平台的存储器存储负担。
4.本申请提供一种新的路径规划策略——快速可见树(rvt)算法，该方法充分利用点云可见性信息来辅助机器人进行避障探索工作。与传统的空间大量采样再在生成的随机树中搜索可行路径的方法不同，通过计算机器人当前位置所有可见点云确定机器人可以正常工作的区域范围，如图1所示，将这些点按照顺序连接起来可以得到一个多边形，在该多边形区域内运动机器人可以避免任何碰撞。给定工作起点与终点后，机器人将会在每一时刻的可见区域内探索出一条满足工作要求的路线。考虑到机器人工作时安全性与尽可能多的探索未知空间的需要，我们提出了一种路径树增长模型，通过可见区域中心与边界线确保以上两点要求以辅助在每一时刻向机器人下达正确的工作指令。


技术实现要素：

5.为了解决上述存在问题。本发明提供一种利用可见性信息的实时路径规划方法，该方法充分利用点云可见性信息来辅助机器人进行避障探索工作。与传统的空间大量采样再在生成的随机树中搜索可行路径的方法不同，通过计算机器人当前位置所有可见点云确定机器人可以正常工作的区域范围。
6.本发明提供一种利用可见性信息的实时路径规划方法，其特征在于，具体步骤如下：
7.将起点p
s
，终点p
g
和当前位置p处雷达采集的环境点云x(p)作为原始输入数据；
8.机器人在工作过程中通过激光雷达对点x(p)进行采样来局部感知周围的障碍物，
通过rvt算法在输入点云数据存在一定噪音与缺失的情况下，引导机器人探索未知环境并找到从p
s
到p
g
的最佳路径；
9.所述rvt算法的基本流程：
10.首先创建一个路径树t，该树由起始节点p
s
进行初始化，然后在每一时间帧使用移动向量决策函数来指示机器人接下来的前进方向，从而使路径树t逐渐增长，每当机器人遇到死路时，该算法都会指引其回溯到t的上一个分岔路口，并重新选择移动方向，t在机器人探索工作过程中会保持增长，直到终点p
g
也被包含其中；
11.在整个流程中，为了保证p
s
至p
g
之间的路径树正确创建，rvt算法中主要包含三个工作模块分别为，可见气泡计算模块、移动向量决策模块和路径树增长模块。
12.作为本发明进一步改进，所述可见气泡计算模块：每一时刻机器人在当前位置采集到的原始点云数据都可以通过球面映射公式映射到一个镜面空间：
[0013][0014]
其中x
i
为原始点云x(p)中的任意一点坐标，fp为球面映射函数表达式，r为球面映射半径，通过改变r的值可以调整机器人可见视野范围大小，p为机器人当前位置坐标，为映射空间中x
i
的对应点。
[0015]
具体映射步骤如下：
[0016]
连接镜面空间中所有凸包上的点后，把这些点与连接关系逆映射回原始空间便将机器人在当前位置的可见区域形象化为一个可见气泡v(p)，在该气泡内将不再存在任何障碍物点，因此机器人在气泡内部移动是完全安全的，同时通过分析气泡的几何结构，机器人也可以确定自身的移动策略。
[0017]
作为本发明进一步改进，所述移动向量决策模块：基于可见气泡计算模块得到的每一时刻机器人在当前位置得到的v(p)特征，rvt算法会理科确定当前时刻的移动策略并实时向机器人下达正确的移动指令，从图5与图6可以看出，可见气泡v(p)的气泡中心c以及其最优开口边界线b将会成为影响机器人移动方向的两大因素，且移动方向向量v由下式决定：
[0018][0019]
其中e为自然对数，a为可见气泡v(p)的面积大小，b为最优开口边界线，c 为气泡中心，p为机器人当前位置坐标，p
g
为终点坐标。
[0020]
作为本发明进一步改进，路径树增长模块：为了记录下从起点到终点的整个路径，rvt算法选择树形数据结构保存机器人在移动过程中经过的每一个节点，在每一个节点处都会将机器人计算得到的可见性信息以及移动方向信息保存下来，从而可以帮助其记录下整个移动路线并在遇到死路时及时回溯到正确的分岔路口。
[0021]
本发明一种利用可见性信息的实时路径规划方法，设计点如下：
[0022]
(1)本申请一种新颖的可见树算法，该算法可以帮助机器人在未知环境中完成实时路径规划和探索任务；
[0023]
(2)本申请一种路径树增长方法，该算法可在每一时刻向机器人下达工作指令确保其在移动过程中避免与障碍物发生碰撞；
[0024]
(3)本申请在模拟环境和真实场景中均对该方法进行了实验，结果表明快速可见树方法适用于机器人路径规划工作。
附图说明
[0025]
图1为本发明的整体流程图；
[0026]
图2为本发明的整体示意图；
[0027]
图3为本发明中可见气泡计算流程图；
[0028]
图4为计算可见气泡示意图；
[0029]
图5为本发明中移动向量决策流程图；
[0030]
图6为本发明中移动向量决策示意图；
[0031]
图7为本发明中路径树增长流程图。
具体实施方式
[0032]
下面结合附图与具体实施方式对本发明作进一步详细描述：
[0033]
本发明提供一种利用可见性信息的实时路径规划方法，该方法充分利用点云可见性信息来辅助机器人进行避障探索工作。与传统的空间大量采样再在生成的随机树中搜索可行路径的方法不同，通过计算机器人当前位置所有可见点云确定机器人可以正常工作的区域范围。
[0034]
作为发明的一种实施例，本申请将首先介绍由点云x(p)生成的可见气泡 (visible cell)，这些气泡将用于存储局部环境特征。之后会提出一种移动策略，该策略可以依据可见气泡实时帮助机器人确定一个合理的移动方向向量，从而找到 t上相邻节点之间的最优路径。最后将简单阐述路径树t的增长模式以及遇到死路时的回溯策略。
[0035]
本申请将起点p
s
，终点p
g
和当前位置p处雷达采集的环境点云x(p)作为原始输入数据。机器人在工作过程中全局地图未知，仅通过激光雷达对点x(p)进行采样来局部感知周围的障碍物，因此机器人感知到的环境信息可能会受到硬件条件的约束。提出的rvt算法旨在输入点云数据存在一定噪音与缺失的情况下，引导机器人探索未知环境并找到从p
s
到p
g
的最佳路径。在图1与图2中，本申请展示了rvt算法的基本流程：首先创建一个路径树t，该树由起始节点p
s
进行初始化，然后在每一时间帧使用移动向量(moving vector)决策函数(moving vectordecision)来指示机器人接下来的前进方向，从而使路径树t逐渐增长。而每当机器人遇到死路时，该算法都会指引其回溯到t的上一个分岔路口，并重新选择移动方向。t在机器人探索工作过程中会保持增长，直到终点p
g
也被包含其中。
[0036]
在整个流程中，为了保证p
s
至p
g
之间的路径树正确创建，rvt算法中主要包含三个工作模块：
[0037]
可见气泡计算模块：使用激光雷达或其他传感器获取的原始点云数据通常十分嘈杂，也可能存在硬件抖动造成的数据缺失问题，因此直接连接原始点云作为可见区域会产生嘈杂且不规则的多边形，而在该区域内中移动机器人极有可能与障碍物发生碰撞。受点云可见性的启发，本方法从采集的点云x(p)中计算出视点 p位置的可视气泡。具体来说，本文定义了一个相对于视点p的可见点云集合 v(p)，假设机器人在位置p处采集到的n个点的集合为x＝{x
i
}
i＝1，2，...，n
，本文定义了一个相对于视点p的可见点云集合v(p)＝{v
j
∈
x}
j＝1，2，...，m
来表示原始点中所有可视点创建的多边形，如图2所示。v(p)中的点为x的子集，若机器人在p处向四周观察，则v(p)中任意相邻两点v
j
，v
j+1
之间的连线都不会被x中的任何其他点遮挡。可以看出将v(p)中所有点连接而成的可见气泡内区域是空白且连续的，这意味着v(p)中不存在会与机器人发生碰撞的障碍物。换句话来说，在v(p)内移动时，机器人是十分安全的。katz等人提出了一种算法[3]，无需进行网格构建或计算每个点的法向量即可从原始点中区分出所有的可视点云。本文设计了二维场景下计算集合v(p)的方法，该方法包括两个步骤：首先以视点p的位置作为输入，每个原始点x
i
∈x首先经过球面映射得到其映射点的集合
[0038][0039]
具体映射步骤如图3与图4所示，连接镜面空间中所有凸包上的点后，把这些点与连接关系逆映射回原始空间便可以将机器人在当前位置的可见区域形象化为一个可见气泡v(p)，在该气泡内将不再存在任何障碍物点，因此机器人在气泡内部移动是完全安全的。同时通过分析气泡的几何结构，机器人也可以确定自身的移动策略。
[0040]
该算式中可以通过设置视野半径r来调整可见范围。考虑到机器人的大小，我们将r设置为10r，其中r为机器人包围盒的边长。在下文中，将映射点集所在空间称为映射空间。
[0041]
接着再计算出映射空间中形成的的凸包并选择凸包上的点作为可见点。假设和为凸包上为连接的相邻两点，则原始点云中对应的两点x
i
和x
j
也会被连接起来。通过连接这些点，我们获得了一个称为可见气泡v(p)的多边形。
[0042]
移动向量决策模块：
[0043]
基于可见气泡计算模块得到的每一时刻机器人在当前位置得到的v(p)特征，rvt算法会理科确定当前时刻的移动策略并实时向机器人下达正确的移动指令，从图5与图6可以看出，可见气泡v(p)的气泡中心c以及其最优开口边界线b将会成为影响机器人移动方向的两大因素，且移动方向向量v由下式决定：
[0044]
如图2所示，本申请移动策略存在两个影响因素：首先，希望机器人在移动过程中应尽量避免发生碰撞并具有一个良好的视野。其次，机器人更偏向于向未探索过的区域和目的地移动，而可见气泡v(p)则很好地展现了从视点p出发的可见性信息以及其还未探索的边界区域。v(p)内的空白区域通常是从视点p出发向四周观察时不存在遮挡的区域，不存在遮挡即没有障碍物，因此机器人在该区域内是安全的。通过计算该空白区域的面积a便可确定机器人当前时刻自由移动的空间。此外，当机器人越靠近靠近可见气泡的中心c，其越安全，因为气泡中的四周往往具有最大的空白区域。但是，可见气泡可能为凹多边形，用传统方法得到的多边形重心来代替气泡中心可能会导致c靠近气泡边界。在这种情况下，简单地将v(p)的重心定义为可见中心并不理想。为了保持机器人与可见气泡边界之间的安全距离，本文利用多边形均值来定义可见中心[4]。在以往的工作中，多边形均值往往被用来控制多边形进行形变操作。
[0045][0046]
其中，α
i
是向量和之间的夹角，权重ω
i
表示多边形上第i个顶点对点 p的影响。与传统的重心不同，多边形均值在任意多边形的中心区域会与点数均值相近，而在边界区域则与点数均值存在较大偏差，因此本文使用多边形均值来计算可见中心。具体方法为：可见中心处对应的应多边形均值无限接近点数平均值的坐标。如果多边形上有m个顶点，则归一化后的多边形应全部接近因此，通过解决下式的优化问题，即可获得了多边形内部的可见中心：
[0047][0048]
机器人在可见中心处能够更好地看到四周的环境，与重心相比，将机器人放置在可见中心也更为安全。因此可以将c作为决定移动向量的第一个影响因子。
[0049]
除此之外，机器人也应更偏向于向可见气泡的边界线处移动，因为边界线以外的区域往往是还未探索过的区域。可见气泡的边界线一般为障碍物上的连线或是移动到下一个可见气泡的开口线，本文将开口边界线定义为c。区分c和障碍物的关键指标是边界线的长度。c的长度往往比障碍物上的边界线长的多。将一个可见气泡上所有边界线的平均长度l0做为参考，通过多组测试，本文将5倍的 l0设置为区分的阈值，若边界线长度大于该阈值则会被标记位c。但是，一个可见气泡有时可能包含多个开口边界线。标记好所有的c后，将会在其中进一步选出最优的开口b∈c，该开口可以在最短的时间内将机器人引导到目的地。因此之后将会计算每个开口与p
g
之间的距离，而机器人则会更倾向于向最靠近p
g
的开口移动。本文使用b的中心点坐标来代替开口边界线以方便计算距离，并将其用作决定移动向量的另一个影响因子。
[0050]
利用可见气泡获得c和b来表示两个影响因子之后便可以确定机器人当前时刻的移动向量。该决定策略基于以下规则：如果可见气泡的面积a很小，那么机器人可以安全移动的空间范围将会十分狭窄。因此，考虑到安全性要求，机器人将倾向于移动到当前c附近的位置。而如果机器人已经足够接近c，则b应对其更具吸引力，以帮助机器人继续探索任务。因此，本发明将用以下公式来确定移动向量v:
[0051][0052]
其中b和c都将产生对机器人引力，并且每个引力的权重将取决于其相应可见气泡v(p)的面积a以及b和p
g
之间的距离。确定移动向量后，运动控制系统会首先将机器人的前进朝向转到与移动向量方向匹配的位置，之后再移动到下一位置 p
next
。
[0053]
路径树增长模块：
[0054]
为了记录下从起点到终点的整个路径，rvt算法选择树形数据结构保存机器人在移动过程中经过的每一个节点，具体实施方式如图7所示。在每一个节点处都会将机器人计算得到的可见性信息以及移动方向信息保存下来，从而可以帮助其记录下整个移动路线并
在遇到死路时及时回溯到正确的分岔路口。
[0055]
如图4所示，根据每一时刻计算出的移动向量，本方法会创建一个路径树来记录已经走过的路线并探索未知空间。每当机器人移动时，都会添加一个新节点 p
next
并将p和p
next
连接到创建路径树中使路径树不断增长。而下一时刻，机器人抵达p
next
坐标后将会在该处计算一个新的可见单元格v(p
next
)并更新移动向量 v。该路径树会随系统迭代计算可见气泡和移动向量而增长，直到机器人到达目标地点p
g
。
[0056]
同时，机器人还需要处理道路分叉处可见气泡会具有多个开口的情况。本文移动策略会贪婪地选择一个最接近p
g
的开口边界线，然而贪婪的策略可能会使路径树在死路停止生长。当出现该问题时，本方法还需要确保路径回溯策略能够正常将机器人带回道路分叉处，并尝试向另一个开口边界线移动。在本文的方法中，除了每个树节点的位置坐标信息外，每个节点中还存储了其对应的环境信息：可见气泡v(p)及其开口边界线c的集合以支持树的生长。同时，每一个开口边界线也会被添加上一个标签用来表明机器人是否已经访问过(visited)该开口方向用以区分已经探索过和还未探索的区域。
[0057]
当一个可见气泡v(p)仅存在一个已经被访问过的开口边界线时，节点p将会被视为死路位置。在这种情况下，本方法会利用存储在每个节点中的移动向量来帮助机器人回溯到包含未访问开口边界线的上一个节点。当机器人回到分叉区域时，系统将停止回溯模式并选择一个未访问的开口边界线，利用移动向量决定模块重新开始路径树的生长。
[0058]
本发明不仅可以实时完成路径规划任务，而且可以在模拟或真实场景中探索未知环境。
[0059]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。