一种基于距离网格地图的移动机器人路径规划方法
【技术领域】
[0001] 本发明设及移动机器人路径规划领域，具体是一种基于距离网格地图的移动机器 人路径规划方法。
【背景技术】
[0002] 在移动机器人短短几十年的发展过程中，避障及路径规划算法一直是学者们研究 的热口问题，该问题也是智能移动移动机器人自主能力的关键问题之一。如何在未知不确 定环境中无碰撞的进行路径规划是高度自主化的行为，每年在该方向的论文与算法都非常 多。目前一般采取全局路径规划和局部避障规避相结合的方法实现移动移动机器人的自 主导航。其中比较流行的避障算法有基于边缘检测法巧dge-Detection Method)、势场法 (Potential Field Methods)、虚拟力场法（Virtual Force Field Method)、矢量场直方图 法（Vector Field Histogram Method) W及后续改进的VFH+、V即等方法，该些方法都是局 部避障算法，除了 VFH*算法在对后续路径的预测加入A*捜索来进行路径选择优化外，其余 算法都不对全局的路径进行规划。
[0003] 距离网格地图表示的是单元格与障碍距离的最小值，通常用于移动移动机器人的 避障预警、非完备移动移动机器人的路径规划W及定位等问题。在静止的环境中，距离网格 地图变动不大，所需要更新维护的计算成本几乎为零。然而，移动移动机器人所处的环境往 往是一个动态的环境，障碍物的变动，存在其他移动物体，甚至是传感器噪声，都需要在改 变概率网格地图的基础上来改变距离网格地图。
[0004] 传统的基于距离网格地图的路径规划方法存在的缺陷：
[000引 （1)传统距离网格地图更新耗费计算量大，地图维护占用时间长，难W动态更新；
[0006] (2)路径规划时使用启发式捜索算法，在捜索时由于不考虑移动移动机器人的运 动限制和移动机器人自身的尺寸，使得所规划路径在局部与障碍物距离过小、在部分转向 之处（路径拐点）为锐角，无法直接使用规划出的路径来控制移动移动机器人的运动。
【发明内容】
[0007] 本发明的目的在于提供一种基于距离网格地图的移动机器人路径规划方法，解决 了基于距离网格地图的地图动态更新管理问题和未知地图下的地图构建及路径规划问题。 [000引为实现上述目的，本发明提供如下技术方案：
[0009] 一种基于距离网格地图的移动机器人路径规划方法，包含W下步骤：
[0010] 步骤1;设定初始概率网格地图和距离网格地图为空地图；控制移动机器人在目 标区域内移动，并通过传感器采集目标区域的地形信息，构建W移动机器人为中屯、的活动 窗口，所述的活动窗口对应存储在移动机器人内的初始距离网格地图中的一个子区域，选 取与活动窗口对应的目标区域中的部分地形信息对所述的子区域进行更新，直至活动窗口 遍历整个初始距离网格地图，得到更新后的距离网格地图；
[001U 步骤2 ;在更新后的距离网格地图上，确定移动机器人的起点和终点，将每个网格 的中屯、作为节点，采用A*捜索算法并根据移动机器人在每个节点的姿态角进行节点展开， 确定移动机器人由起点运动至终点所有经过的节点，连线起点、所有经过的节点和终点，规 划出移动机器人的路径。
[0012] 作为本发明进一步的方案；步骤1中，距离网格地图更新过程中的活动窗口和前 沿网格的选择方法；
[0013] 为了分散距离网格地图的更新计算量，对距离网格地图的更新计算被限制在了活 动窗口之内，活动窗口过大，会导致重复的、不必要的更新队列插入操作，而活动窗口选择 过小，更新向量生成器所产生的某个更新向量（X，y，d)的更新坐标x，y超出活动窗口的范 围，由此将会使得对应距离网格地图上没有体现数据的最近更新，导致距离网格地图与概 率网格地图的不一致。
[0014] 一般来讲在使用声纳等不精确数据时，数据值越大，即意味着离机器人越远，其可 靠度越差，所W更新向量生成器在使用声纳数据时，将会接受一定范围内的数据，超过最远 距离Sm"的数据将会被丢弃，W保证数据的可靠性。所W在活动窗口的高和宽应该满足下 式：
[0015]
[0016] 式中；馬为活动窗口的高，W,为活动窗口的宽，8。"为保持声响数据可靠的最远距 离，INTO为取整操作。
[0017] 由于移动机器人的数据获取与控制并非连续的过程，而是一系列离散过程所组成 的。传感器数据更新间隔一个固定的时间At,，在一个或者几个At,内完成移动机器人的 地图构建、导航计算等工作，所W虽然在现实中，宏观上移动机器人的位置是连续变化的过 程，但是对于导航计算的算法程序而言，改变是离散的、跳变的，无论移动机器人沿着哪一 条路径由一点到达另一点，所引起的变化对局部的距离网格地图而言，首先需要根据活动 窗口的移动情况，计算出需要进行距离网格地图更新计算的"前沿"（待更新距离网格），然 后再采用队列方法进行地图更新。
[0018] 作为本发明进一步的方案；步骤1中，将时间为t-1时移动机器人的中屯、坐标记为 (V yp)，当前计算更新时间为t时的移动机器人中屯、坐标记为（Xr，y,)，移动机器人坐标变 化量为（A X，A y)，A i和A j分别为活动窗口中屯、坐标行列序号的变化量，为整数，距离网 格单元格的边长为S。，W单元格数目来表示的活动窗口的宽和高分别为W。、H。。
[0019] A X和A y按照下式计算；
[0020]
[0021] Ai和A j按照下式计算；
[0022]
[0023] 分为W下四种情况计算"前沿"网格的行列序号范围：
[0024] (1)如果Ai、A j全为0,则不进行活动窗口刷新计算；
[0025] (2)如果Ai = 0, A j声0,则不存在纵向"前沿"网格，仅需计算出横向"前沿" 网格的序号范围；
[0026] (3)如果Ai声0, A j = 0,则不存在横向"前沿"网格，仅需计算出纵向"前沿" 网格的序号范围；
[0027] (4)如果A i、A j均非零值，则需要分别计算出横向和纵向两个方向上的"前沿" 网格的序号范围。
[002引作为本发明进一步的方案；步骤1中，距离网格地图障碍物增加及删除时的距离 网格地图动态更新方法；
[0029] 在障碍增加时对距离网格地图的更新需要设定一个障碍增加更新队列如，其中存 放的是距离网格地图更新单元，更新单元包含单元格的行列序号（i，j)。当距离网格地图中 的某个单元格被更新时，根据该单元格的信息生成对应的增加更新单元，并将其加入障碍 增加更新队列如。障碍增加更新算法从障碍增加更新队列如取出一个更新单元，并将其 从障碍增加更新队列如中删除，根据增加更新单元提供的行列序号（i，j)，尝试更新（i，j) 周围的八个单元格，并将新生成的更新单元加入障碍增加更新队列如。只要障碍增加更新 队列如不为空，障碍增加更新算法将一直继续下去；
[0030] 在进行清除时同样需要使用一个障碍清除更新队列Qe，根据被清除的单元格信息 生成清除更新单元，并将其加入障碍清除更新队列Qe，障碍清除更新算法从障碍清除更新 队列Qe中取出一个清除更新单元，按照与障碍增加更新算法类似的迭代方式，对所有与清 除更新单元的单元格处相关联的单元格进行清除操作，直到到达与之不相关的单元格，之 后按照"边缘"的信息生成更新单元，并将其加入障碍增加更新队列如，并调用障碍增加更 新算法对距离网格地图进行更新。
[0031] 作为本发明进一步的方案；步骤1中，距离网格地图更新队列的优化方法。
[0032] 在实际应用中，距离传感器数据的更新往往是一组多个，会导致概率网格地图Me 中数个单元产生超过或低于阔值Tc的变化，该种变化使得对应的距离网格地图进行更新 操作，体现在障碍增加更新队列如上，就是队列中同时加入了数个元素，随着更新操作的 进行，一些重复的元素将被添加进来。因此，在设计更新队列排序函数的时候需要考虑位置 参数，同时需要纳入考虑的是更新的层级Lu，所谓的Lu反映的是网格单元展开的层次，LU越 大的单元距离障碍的距离必然比较大，应该放到队列的尾部，越晚被更新。因此需要设计一 个考虑层级Lu和位置的排序估值函数，对该函数的表述如下式所示：
[003引 P (i，j) = Lu ? Wg ? Hg+Wg ? j+i
[0034] 在上式中Wg和H g为距离网格地图的高度和宽度，i和j是待更新单元的行序号和 列序号，该函数将不同的待更新网格单元进行了区分。
[0035] 作为本发明进一步的方案；步骤2中，对考虑机器人姿态方位的状态进行离散化 处理后捜索节点展开方法；
[0036] 本发明中，移动机器人的状态向量除了其坐标值之外，还加上了移动机器人姿态 方位（即移动机器人出发时朝向的方位），在对节点进行展开时，排除了那些不可迁移的状 态，使得所规划的路径对于该些有运动限制的移动机器人可行，同时使得生成的路径更容 易进行平滑处理。
[0037] 本发明中，移动机器人的状态向量使用Si