基于人工势场与滚动窗口的机器人的动态避障方法与流程

本发明属于机器人路径规划技术领域，涉及一种基于人工势场与滚动窗口的机器人的动态避障方法。

背景技术

路径规划是自主移动机器人的关键要素之一，希望移动机器人能够尽量快速准确地到达目的地，同时也需要机器人能够安全有效的躲避环境中的障碍物。在已知的静态环境中安全有效的躲避障碍物并准确地到达目的地已经有较多较好的解决方案。但是在环境中存在的障碍物是运动的，并且障碍物的位置与速度时刻都在发生变化时，这就对机器人的导航过程的避障算法的实时性和准确性都比静态环境中的要求更高，如果继续使用静态环境中的算法来进行动态环境的导航与避障，那么极大可能会使得避障失败导致最终导航失败。

针对移动机器人的动态避障的研究主要是要对障碍物进行有效的检测和碰撞躲避控制算法设计，使机器人能够准确快速的完成导航任务。对于对障碍物的检测，需要利用机器人自身所带的测量传感器，对障碍物进行距离与位置的测量以及运动状态的判断。目前对于这类传感器的使用一般有超声波传感器、红外传感器、激光传感器、视觉传感器等。

在动态避障算法的研究上，比较常用的方法有人工势场法、vfh类算法、神经网络、遗传算法、模糊逻辑法以及滚动窗口法等。

人工势场法是一种虚拟力场法，及障碍物产生一个对机器人的斥力使机器人与障碍物保持安全距离，目标点产生一个对机器人的引力，使得机器人驶向目标点，引力和斥力的合力决定着机器人的运动方向及速度，该方法计算量小实时性好，但容易出现局部极小值点。

滚动窗口法是一种利用传感器时刻检测机器人视窗范围内的环境信息来进行对障碍物的躲避和导航的，滚动窗口动态路径规划是一种实时在线的路径规划方法，其不进行离线的路径规划，只通过每一步长的窗口子目标点来确定其前进方向；而子目标点只是通过避让窗口内静态、动态障碍物，并到目标点最短距离来确定的，该方法未考虑到全局的优化目标。若将该方法应用在复杂的广阔环境中，得到的全局优化结果会很不理想。

技术实现要素：

有鉴于此，本发明在动态障碍物的状态检测上利用激光传感器来实时扫描移动机器人导航中环境信息，首先存入一个静态全局信息，再在动态避障控制算法上对人工势场法与滚动窗口法相结合控制方法，提供一种基于人工势场与滚动窗口的机器人的动态避障方法。

为达到上述目的，本发明提供如下技术方案：

基于人工势场与滚动窗口的机器人的动态避障方法，该方法包含如下步骤

s1：设置机器人运动时与障碍物的绝对安全距离d以及目的地的位置坐标信息x(x，y)；

s2：通过机器人的历史里程计数据与当前环境信息e0i来判定当前机器人在全局坐标系中的位姿

s3：根据当前机器人位姿、当前的环境特征信息、目的地坐标以及全局坐标信息利用a星寻路算法得到一条静态初始最优路径路线sfirst，并且开始执行；

s4：利用滚动窗口法，时刻扫描并更新当前窗口下的实时环境信息e1i，实时计算出δt时间窗口内的环境中是否有障碍物的存在，并判定是否有障碍物：

如果没有障碍物存在，则继续执行sfirst，更新并保存当前环境信息数据e0i＝e1i(i1，2，3...，n)，其中n为正整数，并执行s6；

如果有障碍物存在，则执行s5；

s5：判断障碍物的距离是否在设置的绝对安全距离以内，如果不在则执行s6；

如果在绝对安全距离以内，则处理后进行路径重新规划更新sfirst，然后执行s2；

s6：判断是否已经到达目的地坐标位置x(x，y)，如果到达则结束本次导航，如果没有到达，则返回s2继续执行。

进一步，步骤s2具体为，通过机器人的历史里程计数据结合当前环境信息e0i与地图环境模型进行匹配，定位当前机器人在全局坐标系中的位姿

根据eti与判断t时刻环境中的障碍物的全局坐标为ooti(xti，xti)，满足：

其中，(xt，yt)为t时刻机器人的全局坐标，d0i为第i个障碍物到机器的距离，θt为机器人t时刻位姿角度，k是障碍物和机器人位姿指向之间的夹角。

进一步，步骤s5具体为：

s51：根据障碍物坐标oo0i(x0i，y0i)与oo1i(x1i，y1i)对当前环境中的障碍物进行分类，满足：

其中，zso0i表示静态障碍物，zdo0i表示动态障碍物，l+m≤n，δs表示障碍物在t时间内所变化的距离，δs＝s1i-s0i，表示障碍物t时刻的位置，s0i表示障碍物初始时刻位置；

s52：根据障碍物类型进行判定，若为静态障碍物，则将静态障碍物与机器人之间的距离dso与绝对安全距离d进行比较，δd＝dso-d，若δd＞0，则继续按照规划的路线sfirst前进，若δd＜0，则重新规划snew，避开静态障碍物，更新sfirst＝snew；

若为动态障碍物，则根据当前动态障碍物的运动状态v1与sfirst判断该动态障碍物是否对机器人当前的路线存在危险，若不存在危险，则继续按照规划的路线sfirst前进，若存在危险，则根据影响情况重新规划snew，避开动态障碍物，更新sfirst＝snew。

进一步，步骤s52中根据当前动态障碍物的运动状态v1与sfirst判断该动态障碍物是否对机器人当前的路线存在危险具体为：

设动态障碍物的运动轨迹参数方程为：

当前导航路线sfirst下机器人的运动轨迹参数方程为：

联立方程fo(t)与方程fr(t)进行求解，并判定：

若方程组有解则表明两个方程有交点(xi,yi,ti)(i＝0,1,…)，即在t＝ti(i＝0,1,…)两个方程在点(xi,yi)(i＝0,1,…)相交，机器人将会与动态障碍物发生碰撞，则当前路线存在危险，需要重新规划；

若方程组无解则表明两个方程没有交点，即当前路线不存在危险，不会与动态障碍物发生碰撞。

进一步，若当前路线存在危险，需要重新规划，避开障碍物重新规划的具体方法满足：当把可能的危险交点(xi,yi)(i＝0,1,…)代入到原来的静态地图中以后，得到一个新的静态地图，然后利用人工势场法，先得到局部最优方向，然后沿着这方向使用a星寻路算法得到新的路径规划snew，更新sfirst＝snew。

本发明的有益效果在于：

本发明提供了一种基于激光传感器的人工势场与动态窗口相结合的移动机器人动态避障方法，加入了全局静态信息，从而弥补了局部动态避障导航中缺少全局信息而导致的局部最优而全局性较差的缺点，局部与全局相结合，有效地提升了动态中全局路径规划最优性。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为本发明流程图。

具体实施方式

下面将结合附图，对本发明的优选实施例进行详细的描述。

如图1所示，在进行步骤s1前，我们需要将环境的静态地图特征信息进行构建并存储到移动机器人内部。

s1：进行参数输入，即移动机器人与障碍物之间的绝对安全距离d和导航的目标点在构建的地图中的坐标信息x(x，y)。

s2：根据机器人的里程计的历史数据即∞→0时刻的里程计数据，然后再结合激光传感器此时扫描的环境特征信息e0i(i＝1,2,…，n)与地图环境模型进行匹配，定位出机器人当前的位姿

由eti(i＝1,2,…，n)与可以判断t时刻环境中的障碍物的全局坐标为ooti(xti,yti)：

式中，(xt,yt)是t时刻机器人的全局坐标，doi是第i个障碍物到机器的距离，θt是机器人t时刻位姿角度，k是障碍物和机器人位姿指向之间的夹角。

于是可以由e0i(i＝1,2,…，n)与计算出t＝0时刻环境中的障碍物的全局坐标为oo0i(x0i,y0i)(i＝1,2,…，n)。

s3：根据当前机器人位姿当前的环境特征信息e0i(i＝1,2,…，n)、目的地坐标x以及全局地图信息初步规划出当前视窗下的一条最优路线sfirst(假设不存在动态障碍物)，并且开始执行sfirst。

s4，激光扫描得到的当前的窗口中的环境数据信息e1i(i＝1,2,…，n)，判断δt时间窗口内是否存在障碍物：如果没有障碍物则更新当前视窗下的最优路径sfirst，同时更新并保存当前的环境信息数据，即令e0i＝e1i(i＝1,2,…，n)，然后跳到步骤s6继续执行；如果有障碍物则执行步骤s5。

s5，根据障碍物z1i(i＝1,2,…，n)的坐标与障碍物z0i(i＝1,2,…，n)的坐标判断出当前环境中的静态障碍物zso0i(i＝1,2,…，l；l+m≤n)与动态障碍物zdo0j(j＝1,2,…，m；l+m≤n)分别为：

假设障碍物i在一个较短时间内做匀变速直线运动，则由运动学公式可得t时刻的速度与0～t时间内所运动的路程分别为vti＝v0i+ait与δs＝s1i-s0i。

再根据障碍物类型进行分别做出判断：

1)静态障碍物情况：将障碍物与机器人的距离dso与绝对安全距离d将比较δd＝dso-d，如果δd≥0则继续按照之前的路径规划sfirst进行前进，当δd＜0则需要进行重新规划snew，绕开当前障碍物，并更新sfirst＝snew；

2)动态障碍物情况：根据得到的当前障碍物的运动状态v1，将v1与sfirst判断出该动态障碍物是否对机器人当前的路线存在危险，

设障碍物的运动轨迹参数方程为当前导航路线sfirst下机器人的运动轨迹参数方程为那么，对方程fo(t)与方程fr(t)进行联立方程组进行求解：

如果方程组有解则表明两个方程有交点(xi,yi,ti)(i＝0,1,…)，即在t＝ti(i＝0,1,…)两个方程在点(xi,yi)(i＝0,1,…)相交，机器人将会与动态障碍物发生碰撞，当前路线存在危险，需要重新规划；

如果方程组无解则表明两个方程没有交点，即当前路线不存在危险，不会与动态障碍物发生碰撞。

如果路线sfirst存在危险，则需要对路线进行重新规划：根据方程组求得的交点(xi,yi)(i＝0,1,…)，将其以障碍物点代入原来的静态地图信息中，避开障碍物然后再进行一次路径规划。避开障碍物重新规划的具体方法：当把可能的危险交点(xi,yi)(i＝0,1,…)代入到原来的静态地图中以后，得到一个新的静态地图，然后利用人工势场法，先得到局部最优方向，然后沿着这方向使用a星寻路算法得到新的路径规划snew，更新sfirst＝snew。

s6：根据机器人的当前位姿判断当前坐标xt(xt,yt)是否等于目标点坐标x(x,y)：相等则表明已经到达，结束此次导航；不相等则未到达，返回步骤s2继续执行，直到达到目标的。

最后说明的是，以上优选实施例仅用以说明发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。