CN113741455B - 一种移动机器人可视范围覆盖路径规划方法 - Google Patents

Abstract

本发明提出了一种移动机器人可视范围覆盖路径规划方法包括以下步骤：S1，将摄像头可视范围抽象为扇形平面，从而利用扇形平面对地图进行几何填充；S2，根据S1得到的可视范围节点在地图中的覆盖结果进行路径规划：将覆盖结果转化为旅行商问题进行求解，按照遍访顺序进行路径规划。本发明将覆盖问题由点覆盖向面覆盖拓展，更符合搜救任务的实际要求，能有效缩短路径长度，提高搜索效率。

Description

一种移动机器人可视范围覆盖路径规划方法

技术领域

本发明涉及移动机器人路径规划技术领域，特别是涉及一种移动机器人可视范围覆盖路径规划方法。

背景技术

当前世界上的机器人系统正处于蓬勃发展的状态，相关新技术和新产品层出不穷。推动机器人技术发展的主要动力是机器人能减少或替代人们重复的或在危险环境工作中的劳动。随着人工智能和机器人技术的发展，移动机器人在服务业、工业、农业、国防等众多领域取得了广泛应用。移动机器人搜索任务是指利用机器人上安装的有限能力传感器进行搜索，最终实现对目标的锁定，主要涉及到移动机器人路径规划技术。路径规划是移动机器人执行任务最基本的环节，指的是移动机器人如何在有障碍物的工作环境中，规划出一条由出发点前往目标点的运动路径，使移动机器人能够安全高效地完成给定任务。根据对环境信息的掌握程度，路径规划可划分为全局规划和局部规划。全局路径规划是任务开展前的宏观规划，局部规划保证移动机器人在任务执行过程中尽可能贴合全局路径完成任务。对于某一静态问题的路径规划求解，通常指的是求解全局路径，如搜索任务中对给定的静态地图进行覆盖路径规划。

目前，常见的搜索方法主要是基于区域覆盖方法产生的，以避免重复地快速覆盖空间范围为目标进行路径规划。这类问题也被称作覆盖路径规划。典型的覆盖路径规划算法可分为三大类——单元分解法、基于生成树的区域覆盖方法和基于群体智能的区域覆盖方法。单元分解法是早期普遍采用的区域覆盖方法。其主要思想为对整个空间区域进行分割，形成多个形状较为简单且无障碍物、无重叠部分的子区域。每个子区域的覆盖就变成了简单的往复运动，合理分配各子区域的作业顺序，优化子区域间的连通衔接路线以完成整个路径的规划。基于生成树的区域覆盖方法的核心是将工作区域分成不相交的单元，随后由单元生成图的最优生成树，再沿着树生成覆盖路径。这也是目前研究成果最多的一类方法。基于群体智能的区域覆盖方法主要受蚁群、蜂群、狼群等为代表的社会性生物群体行为的启发进行自组织路径规划。此外，还有许多其他方法被应用于解决覆盖路径规划，如贪婪算法、贝叶斯学习方法等。

上述覆盖路径规划算法大都为全覆盖路径规划算法，主要应用于地面清洁、除草、耕种、扫雷等需要遍访整个地图的场景之中。而在搜救场景中，移动机器人一般基于摄像头等具有一定覆盖范围的传感器展开搜索任务。传感器的覆盖范围要远大于机器人在地图中所占据的体积。如果仍采用传统的全覆盖路径规划算法进行全局路径规划，则会扩大任务完成所需的时间开销、增加不必要的路径长度，从而降低搜救效率。如何将区域覆盖问题由栅格地图中的点覆盖转向摄像头传感器可视范围的面覆盖进行建模，有效地利用可视范围对地图进行填充，并转化为易于求解的区域覆盖问题，是基于可视范围设计全局覆盖路径规划方法要解决的关键问题。

发明内容

本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种移动机器人可视范围覆盖路径规划方法。

为了实现本发明的上述目的，本发明提供了一种移动机器人可视范围覆盖路径规划方法，包括以下步骤：

S1，将摄像头可视范围抽象为扇形平面，从而利用扇形平面对地图进行几何填充；

S2，根据S1得到的可视范围节点在地图中的覆盖结果进行路径规划：将覆盖结果转化为旅行商问题进行求解，按照遍访顺序进行路径规划。

所述遍访顺序是一个列表，列表长度等于可视范围节点个数，存储了从起点到终点需要依次经过的可视范围节点ID，移动机器人按照这个顺序经过每个可视范围节点，可使最终的全局路径长度最短。

进一步地，所述摄像头包括：

摄像头安装于移动机器人中心位置，且摄像头朝向与移动机器人朝向一致；

通过摄像头参数将某一位姿下的移动机器人可视范围节点a_i抽象为一个四元组(s_i，r，α_i，θ)来表示，其中s_i为当前位姿下移动机器人在静态地图中的坐标，r为当前摄像头的可视距离，θ为当前摄像头在水平方向的可视角度，α_i为当前摄像头朝向。

进一步地，所述S1包括：

S1-1，确定作业全局信息，对所述作业场景进行格栅化后获得全局栅格地图；

S1-2，在所述全局栅格地图内随机产生若n个可视范围节点，n≥1；

S1-3，使用虚拟势场法计算可视范围内节点所受合力；得到可视范围节点在地图中的覆盖结果。

进一步地，所述S1-3包括：

S-A，对于每个可视范围节点，将其周围栅格地图单元格划分为引力节点与斥力节点两类；

S-B，移动阶段：以可视范围节点的圆心为受力点，计算可视范围节点所受合力，可视范围节点沿合力方向产生对应时间步长内的位移量；

S-C，旋转阶段：以可视范围节点的质心为受力点，再次计算可视范围节点所受合力，可视范围节点绕圆心旋转对应时间步长内的角位移量；

S-D，判断是否满足迭代停止条件，若满足，则停止迭代；若不满足，则跳转执行步骤S-A。

所述迭代停止条件为是否满足设置的迭代次数或设置的覆盖率。

对于每一个可视范围节点，其质心位于对称轴上距圆心距离为

位置。

进一步地，所述移动阶段包括：

对于某一可视范围节点a_i，将距离其圆心距离小于第一参数d₁，且不被其占据的单元格点称作邻居节点p_ij，为分析可视范围节点a_i的移动变化，考虑其圆心s_i所受来自p_ij的作用力，可表示为：

其中，F_ij表示某一邻居节点p_ij对可视范围节点a_i的圆心s_i产生的作用力，F_ij的i、j与p_ij的i、j相对应，i表示可视范围节点自身的ID，j表示邻居节点的ID，所述邻居节点为引力节点或者斥力节点；d_ij1表示s_i与p_ij的欧氏距离，T_ij1表示s_i与p_ij的相对方位角，w_r1为斥力系数，w_a1为引力系数；

对于任一可视范围节点a_i，令其共有n个邻居节点，则其圆心s_i受到的合力为：

进一步地，还包括：

每轮迭代开始时，可视范围节点的初始速度都为0。

根据运动学公式可得，受力后可视范围节点a_i的速度v_i如下：

其中，min(·，·)表示取其中的最小值，m为可视范围节点质量参数，Δt为时间步长参数，

为移动速度上限；

将v_i分别沿世界坐标系x方向和y方向分解为

和/>

则每轮迭代可视范围节点在x方向和y方向的位移可表示为：

由此可完成每轮迭代可视范围节点的位置更新。

进一步地，所述旋转阶段包括：

对于某一可视范围节点a_i，将距离其质心距离小于第二参数d₂，且不被其占据的单元格点称作邻居节点p_ij，为分析可视范围节点a_i的旋转变化，考虑其质心c_i所受来自p_ij的作用力，可表示为：

其中，H_ij表示某一邻居节点p_ij对可视范围节点a_i的质心c_i产生的作用力，d_ij2表示c_i与p_ij的欧氏距离，T_ij2表示c_i与p_ij的相对方位角，w_r2为斥力系数，w_a2为引力系数；

对于任一可视范围节点a_i，令其共有n个邻居节点，则其质心c_i受到的合力为：

进一步地，还包括：

每轮迭代开始时，可视范围节点质心c_i绕圆心转动的角速度都为0。根据刚体转动定理，每轮迭代可视范围节点的角速度可表示为：

其中，

表示合力H_i沿与可视范围节点对称轴垂直方向的分量，k为特征参数，其现实意义与可视范围节点质量和质心到圆心的距离有关，Δt为时间步长参数，/>

为角速度上限；

每轮迭代可视范围节点朝向的角度增量可表示为：

Δθ_i＝ω_i×Δt，

由此可完成每轮迭代可视范围节点的朝向更新。

进一步地，所述S3包括：

S3-1，从移动机器人初始位置出发，寻找一条使得移动机器人遍访每个可视范围节点最短的路径；

S3-2，判断是否为无障碍地图，若是无障碍地图，则使用两个可视范围节点间的欧氏距离构建邻接矩阵进行求解；若不是无障碍地图，则执行下一步骤；

S3-3，判断是否为有障碍物地图，若是有障碍地图，则用路径规划算法求得两个可视范围节点间的路径长度作为距离，从而构建邻接矩阵进行求解；

S3-4，采用遗传算法对旅行商问题进行求解，从而获取到从移动机器人当前位置出发遍访所有可视范围节点的顺序；

S3-5，移动机器人按照遍访顺序，将各可视化范围节点的路径两两连接即可得到可视范围全局覆盖路径，输出机器人在栅格地图中从遍访顺序起点至终点的一系列连续的路径点坐标值。

所述路径规划算法包括：A*算法或Dijkstra算法。

还包括：同时为了减少问题规模，可以将一定范围外的可视范围节点视作不可达目标，即路径长度为无穷大。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

1.将摄像头可视范围与路径规划算法相融合，将可视范围视为已覆盖的区域范围，充分发挥移动机器人的视觉感知能力，提高移动机器人的单体能力，为真实应用场景下的移动机器人搜索提供新的解决方案。

2.基于摄像头可视范围进行覆盖路径规划，将覆盖问题由点覆盖向面覆盖拓展，更符合搜救任务的实际要求，能有效缩短路径长度，提高搜索效率。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明的流程示意图；

图2是本发明的摄像头可视范围示意图；

图3是本发明的一张栅格地图的示意图；

图4是本发明在栅格地图中随机产生40个可视范围节点的示意图；

图5是本发明可视范围节点a₀在其圆心s₀的受力示意图；

图6是本发明可视范围节点a₀在其质心c₀位置的受力示意图；

图7是本发明具体实施例40个可视范围节点利用虚拟势场法覆盖栅格地图后的结果示意图；

图8是本发明具体实施例全局覆盖路径规划结果示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

本发明具体实施的主要流程如图1所示，主要包含可视范围填充阶段和路径规划阶段两个阶段。

1.可视范围填充阶段

图2为摄像头可视范围示意图，坐标系为静态地图的世界坐标系，s_i＝(x_i，y_i)为当前位姿下移动机器人在静态地图中的坐标，r为当前摄像头的可视距离，θ为当前摄像头在水平方向的可视角度，α_i为当前摄像头朝向，随移动机器人朝向的变化而变化。

如图2所示，摄像头安装于移动机器人中心位置，且摄像头朝向与移动机器人朝向一致。通过摄像头参数将某一位姿下的移动机器人可视范围节点a_i抽象为一个四元组(s_i，r，α_i，θ)来表示。

图3为一张栅格地图，黑色为障碍物区域，白色为空白区域。

图4为在栅格地图中随机产生40个可视范围节点的示意图。如图4所示，在给定的静态地图中随机产生n个可视范围节点，静态地图以图3地图为例。n的取值一般由经验确定，应当适中，过大会造成覆盖过度，增添冗余路径；过小则可能导致覆盖率偏低。此处选取n＝40为例进行说明。

虚拟势场的原理是将目标看作虚拟的带电粒子，相邻粒子间存在引力与斥力两种相互作用力。相互作用力的大小受相邻粒子间的距离影响。在合力的作用下，粒子产生自组织移动，最终使粒子均匀地分布在势场中。根据虚拟势场的原理，考虑某一可视范围节点与其周围的栅格地图单元格存在相互作用关系。对于某一可视范围节点a_i，规定在一定范围d内的栅格地图单元格为其邻居节点p_ij。被障碍物和其它可视范围节点占据的邻居节点p_ij称为斥力节点，对a_i产生斥力，未被障碍物和其它可视范围节点占据的邻居节点p_ij称为引力节点，对a_i产生引力。忽略一定范围d₁以外的单元格产生的作用力。

为分析可视范围节点a_i的移动变化，考虑其圆心s_i所受来自p_ij的作用力，可表示为

对于某一可视范围节点a_i，p_ij为其距离d₁内且不在其扇形区域内部的单元格节点，i为其所属的a_i的下标，j为邻居节点的下标，包含引力节点和斥力节点两种类型；

其中，d_ij1表示s_i与p_ij的欧氏距离，τ_ij1表示s_i与p_ij的相对方位角，w_r1为斥力系数，w_a1为引力系数。s_i为当前位姿下移动机器人在静态地图中的坐标，也是即圆心的坐标。

图5表示了可视范围节点a₀圆心的受力情况。图5中实心圆点表示斥力节点p₀₁，对圆心产生斥力F₀₁；空心圆点表示引力节点p₀₂，对圆心产生引力F₀₂。F₀为圆心所受的合力。

对于任一可视范围节点a_i，假设其有n个邻居节点，其圆心受到的合力为：

每轮迭代开始时，可视范围节点的初始速度都为0，则根据运动学公式可得受力后，可视范围节点s_i的速度v_i如下：

其中，m为可视范围节点质量参数，Δt为时间步长参数，

为移动速度上限。

将v_i分别沿世界坐标系x方向和y方向分解为

和/>

则每轮迭代可视范围节点在x方向和y方向的位移可表示为：

可视范围节点a_i首先按照上述位移结果更新位置，再按照如下方法更新朝向。对于每一个可视范围节点，其质心位于对称轴上距圆心距离为

位置。朝向的更新实际可看作可视范围节点的质心绕圆心运动。

现将质心视为带电粒子，将上述方法中对圆心求合力的方法运用到对质心求合力，可得可视范围节点a_i的质心所受合力H_i，进而合力沿与可视范围节点对称轴垂直方向的分量

图6以可视范围节点a₀为例进行了说明。图6中，实心圆点表示斥力节点p₀₁，对质心产生斥力H₀₁；空心圆点表示引力节点p₀₂，对质心产生引力H₀₂。H₀为质心所受的合力。l为过质心作可视范围节点对称轴的垂线。

为合力沿方向的分力。

每轮迭代开始时，可视范围节点质心绕圆心转动的角速度都为0。根据刚体转动定理，每轮迭代可视范围节点的角速度可表示为

其中，k为特征参数，其现实意义与可视范围节点质量和质心到圆心的距离有关，Δt为时间步长参数，

为角速度上限。

每轮迭代可视范围节点朝向的角度增量可表示为

Δθ_i＝ω_i×Δt

，根据上述公式可完成每轮迭代可视范围节点的朝向更新。

迭代的结束条件可通过设置迭代次数或满足一定覆盖率进行判定。未满足结束条件时，重复上述过程；满足结束条件后，进入下一阶段。

2.路径规划阶段

利用虚拟势场法使可视范围结果均匀分布在栅格地图中，可得到图7所示的结果。接下来，将问题转化为旅行商问题求解，从移动机器人初始位置出发，寻找一条使得移动机器人遍访每个可视范围节点最短的路径。

对于无障碍地图，可直接使用两个可视范围节点间的欧氏距离构建邻接矩阵进行求解；对于有障碍物的复杂地图，则可用一些常见路径规划算法如A*算法、Dijkstra算法等，求得两个可视范围节点间的路径长度作为距离进行求解，同时为了减少问题规模，可以将一定范围外的可视范围节点视作不可达目标，即路径长度为无穷大。

目前，已有多种方法可用于求解旅行商问题。由于旅行商问题是NP难题，通常选取近似算法进行求解。求解完成后，即可获取到从移动机器人当前位置出发遍访所有可视范围节点的顺序。按照遍访顺序，将各可视化范围节点的路径两两连接即可得到可视范围全局覆盖路径。图8显示了以图7的覆盖结果为基础，采用A*算法进行路径规划，遗传算法求解遍访顺序后所得出的全局覆盖路径。

邻接矩阵中的第i行第j列的元素表示ID为i和j的两个可视范围节点之间的路径长度。构建邻居矩阵，相当于构建了一个图。图的顶点集为所有可视范围节点的集合，图中的边表示两个可视范围节点之间的路径长度。

旅行商问题的解即找到一个遍访顺序，使得从图中某一点出发，经过图中各点一次再回到出发点的路径总长度最短。当旅行商问题的规模很大时，要寻求最优解是低效的。采用遗传算法这样的近似算法的好处在于，可以通过设置停止迭代条件，提前终止算法。这样就可以在一定时间内寻求到满足要求的次优解，更符合实际应用的需求。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (7)

1.一种移动机器人可视范围覆盖路径规划方法，其特征在于，包括以下步骤：

S1，将摄像头可视范围抽象为扇形平面，从而利用扇形平面对地图进行几何填充；

S1-1，确定作业全局信息，对所述作业场景进行栅格化后获得全局栅格地图；

S1-2，在所述全局栅格地图内随机产生若n个可视范围节点，n≥1；

S1-3，使用虚拟势场法计算可视范围内节点所受合力；得到可视范围节点在地图中的覆盖结果；

所述S1-3包括：

S-A，对于每个可视范围节点，将其周围栅格地图单元格划分为引力节点与斥力节点两类；

S-B，移动阶段：以可视范围节点的圆心为受力点，计算可视范围节点所受合力，可视范围节点沿合力方向产生对应时间步长内的位移量；

S-C，旋转阶段：以可视范围节点的质心为受力点，再次计算可视范围节点所受合力，可视范围节点绕圆心旋转对应时间步长内的角位移量；

S-D，判断是否满足迭代停止条件，若满足，则停止迭代；若不满足，则跳转执行步骤S-A；

所述迭代停止条件为是否满足设置的迭代次数或设置的覆盖率；

S2，根据S1得到的可视范围节点在地图中的覆盖结果进行路径规划：将覆盖结果转化为旅行商问题进行求解，按照遍访顺序进行路径规划。

2.根据权利要求1所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，所述摄像头包括：

摄像头安装于移动机器人中心位置，且摄像头朝向与移动机器人朝向一致；

通过摄像头参数将某一位姿下的移动机器人可视范围节点a_i抽象为一个四元组(s_i，r，α_i，θ)来表示，其中s_i为当前位姿下移动机器人在静态地图中的坐标，r为当前摄像头的可视距离，θ为当前摄像头在水平方向的可视角度，α_i为当前摄像头朝向。

3.根据权利要求1所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，所述移动阶段包括：

对于某一可视范围节点a_i，将距离其圆心距离小于第一参数d₁，且不被其占据的单元格点称作邻居节点p_ij，为分析可视范围节点a_i的移动变化，考虑其圆心s_i所受来自p_ij的作用力，可表示为：

其中，F_ij表示某一邻居节点p_ij对可视范围节点a_i的圆心s_i产生的作用力，d_ij1表示s_i与p_ij的欧氏距离，τ_ij1表示s_i与p_ij的相对方位角，w_r1为斥力系数，w_a1为引力系数；

对于任一可视范围节点a_i，令其共有n个邻居节点，则其圆心s_i受到的合力为：

/>

4.根据权利要求3所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，还包括：

根据运动学公式可得，受力后可视范围节点a_i的速度υ_i如下：

其中，min(·，·)表示取其中的最小值，m为可视范围节点质量参数，Δt为时间步长参数，

为移动速度上限；

将υ_i分别沿世界坐标系x方向和y方向分解为

和/>

则每轮迭代可视范围节点在x方向和y方向的位移可表示为：

由此可完成每轮迭代可视范围节点的位置更新。

5.根据权利要求1所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，所述旋转阶段包括：

对于某一可视范围节点a_i，将距离其质心距离小于第二参数d₂，且不被其占据的单元格点称作邻居节点p_ij，为分析可视范围节点a_i的旋转变化，考虑其质心c_i所受来自p_ij的作用力，可表示为：

其中，H_ij表示某一邻居节点p_ij对可视范围节点a_i的质心c_i产生的作用力，d_ij2表示c_i与p_ij的欧氏距离，τ_ij2表示c_i与p_ij的相对方位角，w_r2为斥力系数，w_a2为引力系数；

对于任一可视范围节点a_i，令其共有n个邻居节点，则其质心c_i受到的合力为：

6.根据权利要求5所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，还包括：

根据刚体转动定理，每轮迭代可视范围节点的角速度可表示为：

其中，

表示合力H_i沿与可视范围节点对称轴垂直方向的分量，k为特征参数，Δt为时间步长参数，/>

为角速度上限；

每轮迭代可视范围节点朝向的角度增量可表示为：

Δθ_i＝ω_i×Δt，

由此可完成每轮迭代可视范围节点的朝向更新。

7.根据权利要求1所述的一种移动机器人可视范围覆盖路径规划方法，其特征在于，所述S2包括：

S2-1，从移动机器人初始位置出发，寻找一条使得移动机器人遍访每个可视范围节点最短的路径；

S2-2，判断是否为无障碍地图，若是无障碍地图，则使用两个可视范围节点间的欧氏距离构建邻接矩阵进行求解；若不是无障碍地图，则执行下一步骤；

S2-3，判断是否为有障碍物地图，若是有障碍地图，则用路径规划算法求得两个可视范围节点间的路径长度作为距离，从而构建邻接矩阵进行求解；

S2-4，采用遗传算法对旅行商问题进行求解，从而获取到从移动机器人当前位置出发遍访所有可视范围节点的顺序；

S2-5，移动机器人按照遍访顺序，将各可视化范围节点的路径两两连接即可得到可视范围全局覆盖路径，输出机器人在栅格地图中从遍访顺序起点至终点的一系列连续的路径点坐标值。

Images