一种移动机器人的局部路径优化方法与流程

本发明涉及移动机器人技术领域，尤其涉及一种移动机器人的双向搜索局部路径优化算法。

背景技术：

目前，机器人的应用范围越来越广，而移动机器人成为当前发展的主流之一，移动机器人的实时路径规划研究一般分为两大类：一类是基于静态全局环境的规划研究，另一类是基于传感器数据的局部规划研究，基于静态全局信息的机器人规划方使用数据巨大，规划时间长，基于传感器局部信息的局部路径规划算法在机器人实时路径规划中发挥着重要作用。

现有局部路径规划的研究主要有：1.人工势场法，但该方法有其固有缺陷，为解决该问题，一些基于新的势场函数的方法被提出，例如超二次方程势场法、协调函数法、人工力矩算法等，但是上述方法都没有从根本上消除人工势场法固有缺陷；2.沿墙走算法，该算法控制机器人始终沿着障碍物边缘的一个方向运动，从而脱离局部陷阱，但是这种方法对可通行路径的优化考虑较少，局部路径优化是指机器人行进过程中，如果发现有障碍物阻挡机器人走向目标，机器人能够自动以优化的方式绕过障碍物走向目标点，深入研究机器人的局部路径优化方法可为机器人自主航行提供决策依据，提升机器人行进安全性与经济性。

技术实现要素：

为了解决背景技术中问题，本发明公开一种移动机器人的局部路径优化方法，充分利用已知障碍区信息对即将行进的路径进行优化，使机器人在行进过程中时刻保持最优状态。

为实现上述发明目的，本发明采用下述技术方案：

一种移动机器人的局部路径优化方法，其特征是，包括以下步骤：

步骤1：路径初始化

先在机器人r和目的点t之间生成线段l(r,t)，然后计算线段l(r,t)和障碍物gl[n]是否相交，若l(r,t)与障碍物gl[n]不相交，则结束，若l(r,t)与障碍gl[n]相交，则记录交点为pc，从交点pc出发沿着障碍物gl[n]的边缘顺时针方向生成“正向搜索线”clockminrl[n]，并从交点pc出发沿着障碍物gl[n]的边缘逆时针方向生成“反向搜索路线”invertminrl[n]；

步骤2：正向搜索

沿着“正向搜索线”clockminrl[n]移动交点pc，每次移动得到新的交点pc´，判断新交点pc´与目标t之间线段l(pc´,t)和障碍gl[n]是否相交，若相交则继续搜索，否则停止移动并存储该pc´点并标记该点为tmin，同时生成线段l(tmin,t)，当l(tmin,t)和障碍物gl[n]不相交且能够直接和目的点t联通时，将路径记为clockminrl[n]同时进入下一步，否则重复正向搜索步骤继续搜索；

步骤3：反向搜索

沿着“反向搜索线”invertminrl[n]移动交点pc，每次移动得到新的交点pc´´，判断新的交点pc´´与目标t之间线段l(pc´´,t)和障碍gl[n]是否相交，若相交则继续搜索，否则停止移动并存储该pc´´点并标记该点为tmin，同时生成线段l(tmin,t)，当l(tmin,t)和gl[n]不相交且能够直接和目的点t联通时，将路径记为invertminrl[n]同时进入下一步，否则继续重复反向搜索步骤搜索；

步骤4：正反搜索路径优化

首先从clockminrl[n]中的tmin点开始，沿着与步骤3中“反向搜索路径”相反的方向沿障碍物gl[n]的边缘搜寻机器人r，连接点tmin与机器人r的连线l(tmin,r)，若l(tmin,r)与gl[n]相交则设置交点为pcj，同时沿着与反向搜索相反的方向移动pcj点，若l(tmin,r)和障碍物gl[n]不相交，将该pcj点记为rmin，此时rmin与tmin重合，并设置该点与机器人r的连接线l(rmin,r)，继续沿着与“反向搜索路径”相反的方向搜寻机器人r，直到l(rmin,r)和gl[n]不相交且能够直接和机器人r联通时结束；随后从invertminrl[n]中的tmin点开始，沿与步骤2中正向搜索相反的方向沿障碍物gl[n]的边缘搜寻机器人r，在搜索过程中，把越过障碍物并且第一次看到机器人r位置记为rmin，并生成线段l(rmin,r)，最后比较clockminrl[n]和invertminrl[n]路径距离的长短，其中距离短的一条就是要寻找的机器人运行到目的点的最短路径。

与现有技术相比，本发明的有益效果是：

本发明公开一种移动机器人的局部路径优化方法，充分利用已知障碍区信息对即将行进的路径进行优化，在机器人行进过程中双向搜索局部路径，如果发现有障碍物阻挡机器人走向目标，通过设定临时目标点，引导机器人走出局部陷阱，选择优化路径最终达到预期目标点，机器人能够自动以优化的方式绕过障碍物，走向目标点，该算法直观，运算负担小，求解速度快，在实际应用中易于实现。

附图说明

图1是本发明移动机器人的局部路径优化方法的流程图；

图2是本发明的移动机器人的正向搜索示意图；

图3是本发明的移动机器人的反向搜索示意图；

图4是本发明的移动机器人搜索到路径示意图；

图5是本发明实施例中移动机器人与目的地之间障碍物的示意图；

图6是本发明实施例中移动机器人正向搜索到路径示意图；

图7是本发明实施例中移动机器人反向搜索到路径示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请提供的一种移动机器人的局部路径优化方法流程如图1所示，如图5所示，移动机器人r向目标t行进途中遇到障碍物，以本发明的路径优化方法行进，具体步骤如下：

路径初始化：如图2所示，机器人r在向目的点t前进的前方遇到障碍物，此时机器人r需要自主运行越过障碍物到达目的点t，机器人r自身位置和目标点t的位置已知，然后机器人r根据自身位置和目标点t的位置设置一条从r到t的路径l(r,t)，随后将障碍设置为gl[n]并计算路径l(r,t)与障碍gl[n]是否交点，若路径l(r,t)与障碍物gl[n]不相交，则说明机器人r和目的点t之间无障碍，否则记录交点为pc并开始进行路径搜索任务；

正向搜索：如图2所示，机器人r开始沿着障碍物gl[n]边缘顺时针方向进行搜索，当前机器人r与目的点t之间存在障碍物并且其交点为pc，将pc点沿着障碍物gl[n]边缘顺时针移动得到新的交点pc´，之后判断得到新的交点pc´和目的点t的连线l(pc´,t)是否和障碍物gl[n]相交；若连线l(pc´,t)和障碍物gl[n]相交，则继续将pc点顺时针移动并重新计算当前pc´点与目的点t的连线l(pc´,t)是否与障碍物gl[n]相交，若相交则继续，否则停止移动pc点并存储该pc´点并标记该点为tmin，同时生成tmin和目的点t之间的连线l(tmin,t)，随后计算线段l(tmin,t)与障碍物gl[n]交点，并重复上述方法直到l(tmin,t)和gl[n]不相交为止，至此机器人r已经能够沿着该路线绕过障碍物，并将路径记为clockminrl[n]；

反向搜索：如图3所示，机器人r从最初的路径l(r,t)与障碍物gl[n]的交点pc开始，同正向搜索方法相似只是沿着逆时针方向搜索，最终能够得到一条路径并记为invertminrl[n]；

正反搜索路径优化：如图4所示，首先从正向搜索路径clockminrl[n]开始优化，从clockminrl[n]中的tmin点开始，与正向搜索不同的是，优化时是从与反向搜索路径的相反方向进行的，优化时沿着障碍物gl[n]边缘搜寻机器人r的；此时设置t1点与机器人r的连线l(t1,r)，若连线l(t1,r)与障碍物gl[n]相交则将交点设为pcj并设置该点与机器人的连线l(pcj,r)，若连线l(t1,r)与障碍物gl[n]不相交说明t1点和机器人r无障碍，随后pcj沿着“反向搜索路径”移动，每移动一步便判断l(pcj,r)和障碍物gl[n]是否相交，若相交便继续沿着“反向搜索线”移动，直到pcj点能够直接与机器人r联通，此时将pcj点所处的位置为r1，并将其设置为“起始最近点”，同理反向搜索路径invertminrl[n]的优化是从invertminrl[n]中的tmin点开始的，与正向搜索路径优化步骤不同的是其沿着与正向搜索时相反的方向进行的，最终得到pcj点的位置为r1，最后比较clockminrl[n]和invertminrl[n]，其中距离短的一条就是要寻找的机器人运行到目的点的最短路径。

移动机器人绕过该障碍后将继续向目标前进，当机器人遇到新的障碍时，仍旧利用上述方法绕行障碍，直到到达目标，如图6所示，图中所示为将要到达目标的最后一个障碍的正向搜索到的路径，图7为将要到达目标的最后一个障碍的反向逆搜索到的路径，从图6和图7可以发现，图6中正向搜索路径的距离要比图7反向逆搜索到的路径短，因此机器人将自动选择到达目标最近的路径，最终的路径即为图6中所示的路径。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。