一种基于回溯搜索的生物激励机器人完全遍历路径规划方法与流程

本发明设计一种移动机器人完全遍历路径规划方法，特别是涉及一种在动态未知复杂环境下的移动机器人完全遍历路径规划方法。

背景技术：

未知环境下的完全遍历路径规划是移动机器人自主导航中的一个重要问题，该问题常存在于自动扫地机器人、排雷机器人、海底测绘机器人等应用中。完全遍历路径规划要求机器人的足迹或者传感器探测覆盖整个工作空间。当机器人工作在未知环境下，为了完全遍历工作空间，需要实时构建地图和动态规划路径。

常见的完全遍历路径规划方法有人工势场法、模板模型法、A*算法等。人工势场法在实时避障和平滑轨迹控制方面得到广泛应用，但该方法存在局部极小值问题，在处理局部最优解时，容易产生死锁(即机器人的四周为障碍或已遍历区域)；在靠近障碍物时容易震荡；在狭窄通道中摆动；存在陷阱区域等。模板模型法需要事先定义好典型模板，因此该方法不适于处理动态变化的环境。A*算法需要知道环境的先验知识，当环境信息改变时，需要重新规划，计算量大。Simon X.Yang(Chaomin Luo,Simon X.Yang.A Bioinspired Neural Network for Real-Time Concurrent Map Building and Complete Coverage Rotot Navigation in Unknown Environment.IEEE TRANSACTIONS ON NEURAL NETWORKS,VOL.19,NO.7,JULY2008.)提出了一种基于生物激励的神经网络算法，机器人在利用有限的传感器信息进行路径规划时，通过神经动力学建立了由正方形或矩形单元组成的环境地图，能够有效解决点到点路径规划问题，但该方法也存在一些不足，如分离区域之间的路径重叠率高，路径非最优，逃离死区时需等待较长时间等问题。

技术实现要素：

针对上述问题，本发明提出了一种基于回溯搜索的生物激励完全遍历路径规划方法，该方法综合了生物激励神经网络算法、回溯算法(Ginsberg M L.Dynamic backtracking[J].Journal of Artificial Intelligence Research,1993:25-46.)、D*(D Star)(Stentz A.The focussed D*algorithm for real-time replanning[C]//International Joint Conference on Artificial Intelligence.1995,95:1652-1659.)算法的优点，不存在局部极小值问题，计算量小，实现简单，能实现机器人在复杂环境下的完全遍历路径规划。

一种基于回溯搜索的生物激励完全遍历路径规划方法，包括以下几个步骤：

1)以机器人执行器的工作直径构建栅格地图；

2)将邻域内有未遍历区域的栅格作为节点构建节点队列；

3)将栅格地图中的每一个坐标点作为一个神经元，并初始化每个神经元的活性值为0；

4)判断节点队列的状态，若为空，则结束遍历；否则继续遍历；

5)按一定的时间间隔读取激光传感器采集的障碍物的测距数据，将该测距数据转化成栅格坐标，并更新每个神经元的活性值；

6)依据机器人当前状态，选择采用基于生物激励神经网络模型的路径规划方法或者利用回溯搜索算法与D*算法相结合的规划路径方法决策出下一个目标位置；

7)移动机器人到目标位置，返回步骤4)。

下面具体说明本发明的各个步骤。

步骤1：构建栅格地图

以移动机器人的几何中心为原点，机器人的运动方向为Y轴的正方向，顺时针方向旋转90度为X轴的正方向，建立移动坐标系L；以移动机器人开始移动前的几何中心所在位置为原点，以机器人开始移动前的指向为Y轴的正方向，顺时针方向旋转90度为X轴的正方向，建立全局坐标系G。

以精度p将全局坐标系转化成栅格地图，每个栅格的长和宽分别为Δx＝p和Δy＝p，所述p为机器人执行器的工作直径；对扫地机器人来说，p为机器人底盘的直径，取值范围为0.2m到0.4m。

将环境中已知的静态障碍物以及移动机器人的所在位置对应的栅格坐标设置为：和其中，(x_g，y_g)表示物体与机器人在全局坐标系G中的坐标，(x_s,y_s)表示对应的栅格；移动机器人在全局坐标系下的坐标为(x_r,y_r)，激光传感器安装于机器人的几何中心，探测到前方d处有障碍物，与机器人X轴方向的夹角为α，移动坐标系与全局坐标系的夹角为θ，则该障碍物在全局坐标系下的栅格坐标为：对每个栅格赋值，-1表示无障碍栅格，0表示未知栅格，1表示该栅格被障碍物占据；规定，即使某个栅格只有部分区域被障碍物占据，仍赋值为“1”。

步骤2：构建节点队列

节点定义为邻域内有未遍历的栅格，当前栅格邻域内的未遍历栅格数量定义为：n为与当前栅格相邻的栅格个数，g表示栅格状态，k,l为栅格索引；若N_g＞0，则将当前栅格定义为节点，并加入到节点队列。当节点队列的节点数为0时，说明已经没有未遍历节点，结束遍历工作。

步骤3：将栅格地图中的每一个坐标点作为一个神经元，并初始化每个神经元的活性值为0。

步骤4：判断节点队列是否为空。若节点队列为空，说明已经没有未遍历节点，结束遍历工作；否则需要继续遍历。

步骤5：按时间间隔T读取激光传感器采集的障碍物的测距数据，并将数据转化成栅格坐标，并将全局坐标系的每个神经元(每个神经元对应一个栅格)的活性值按如下公式更新：

其中，x_i表示第i个神经元的状态，即神经元活性；A,B和D是非负常数，A代表衰减率，B和D分别代表神经元状态x_i的上下限，即x_i∈[-D,B]；k是与第i个神经元相邻近的神经元个数；I_i为外部输入，当外部为未清扫区域时I_i＝E；当外部为障碍时I_i＝-E；否则I_i＝0。E是一个很大的正常数(E>>B)，确保了目标(未清扫区域)总是处于神经网络的波峰，障碍总是处于波谷。和[I_i]^-分别表示刺激性和抑制性输入。第i个神经元和第j个神经元的连接权值可以定义为ω_ij＝f(|q_i-q_j|)，|q_i-q_j|代表向量q_i和q_j之间的欧氏距离。f(a)可以是任意单调递减的函数，例如可以定义为：

其中μ和r₀都是正常数，r₀表示感受域，即机器人传感器的探测半径。因此，每个神经元只与小范围区域[0,r₀]内的神经元有直接联系。优选地，本发明只考虑与当前栅格紧邻的8个栅格，因此取r₀＝2，神经元连接示意图如图3所示，第i个神经元的感受域为半径为r₀的圆区域，第i个神经元

步骤6：路径规划算法

在本发明中，机器人采用“弓”字型路径。当机器人未陷入死锁时，采用基于生物激励神经网络模型进行路径规划；当陷入死锁时，采用回溯搜索算法与D*算法规划路径。下面分别进行说明：

1)基于生物激励神经网络模型的路径规划方法

在基于生物激励的神经网络模型中，机器人的路径由状态方程及上一时刻所处的位置决定。给定机器人的当前位置p_c，假设下个时刻的位置为p_n，则p_n按下式确定：

其中，c是一个正常数，表示转角所占的权重，k是邻域的神经元的个数，x_j表示第j个神经元的活性值，y_j是关于角度变化的单调递增函数，定义前一时刻的位置p_p，当前位置p_c，下一位置p_j，则y_j定义为：

其中Δθ_j∈[0,π]是指角度变化值，当机器人走直线时Δθ_j＝0，往回走时Δθ_j＝π，因此Δθ_j可以定义为：

其中，θ_j表示机器人在下一位置的朝向，θ_c表示机器人在当前位置的朝向，atan表示反正切运算。需要注意的是，当机器人遇到障碍物且左右转的角度变化值一样，即存在多个解时，优先选取使机器人右转的解。

当机器人从当前位置到达下一个位置，下一个位置就变成新的当前位置(如果找到下一位置与当前位置一样，机器人将不会移动)。机器人下一时刻的位置根据变化着的环境作适应性地变化。

该算法是的机器人在未知的环境下，朝着活性值最高且转向角最小的未清扫区域移动，并通过激光扫描仪实时建立环境地图，直到完全覆盖整个环境空间。

2)回溯搜索算法与D*算法相结合的规划路径方法

该算法涉及到所述节点队列的创建于维护。首先利用生物激励神经网络使机器人覆盖工作空间，直到陷入死锁。机器人在行走过程中构建空间地图，将检测到的邻域内有未遍历的点作为节点记录下来，并按时间先后顺序保存在节点队列表中，同时剔除已遍历节点。当机器人陷入死锁时，说明局部区域已经遍历完成。这时，将当前点设置为起始点，从节点队列中找到距当前时间最近的节点作为下一个目标点。再根据已经构建的环境地图，由D*算法规划出一条点到点的最短避障路径，当到达目标后，由于目标点邻域内有未遍历区域，机器人退出死锁，此时算法首先更新节点队列，剔除已遍历的节点，加入新的节点，然后按照生物激励神经网络算法遍历该区域。由于D*算法能实时更新启发函数，因此即使在点到点规划过程中遇到未知障碍，也能轻松避障，并以最短路径到达目标点。当节点队列的节点数为0时，说明已经没有未遍历节点，结束遍历工作。

本发明的有益效果是：

本发明综合了生物激励神经网络算法、回溯算法、D*(D Star)算法的优点，不存在局部极小值问题，计算量小，实现简单，能实现机器人在复杂环境下的完全遍历路径规划。

附图说明

图1是本发明方法的步骤流程图；

图2是全局坐标系与局部坐标系示意图；

图3是神经元连接示意图；

图4是基于生物激励神经网络模型的路径规划方法的局部路径图；

图5是图3对应的活性直方图；

图6是本发明在双U型障碍物中的生成路径图。

具体实施方式

下面结合符合和实例对本发明作进一步说明。

如图1为本发明的基于回溯搜索的生物激励完全遍历路径规划方法的流程图，包括以下几个步骤：

步骤1：构建栅格地图

如图2所示，以机器人开始移动前的几何中心所在位置为原点，以机器人开始移动前的指向为Y轴的正方向，顺时针方向旋转90度为X轴的正方向，建立全局坐标系G(X^GO^GY^G)；以移动机器人的几何中心为原点，机器人的运动方向为Y轴的正方向，顺时针方向旋转90度为X轴的正方向，建立移动坐标系L(X^LO^LY^L)。

以精度p将全局坐标系转化成栅格地图，每个栅格的长和宽分别为Δx＝p和Δy＝p，所述p为机器人执行器的工作直径；对扫地机器人来说，p为机器人底盘的直径，取值范围为0.2m到0.4m。

上述栅格坐标系的原点与全局坐标系的原点重合，将环境中已知的静态障碍物以及移动机器人的当前位置对应的栅格坐标设置为：和其中，(x_g，y_g)表示物体与机器人在全局坐标系G中的坐标，(x_s,y_s)表示对应的栅格；移动机器人在全局坐标系下的坐标为(x_r,y_r)，激光传感器安装于机器人的几何中心，探测到前方d处有障碍物，与机器人X轴方向的夹角为α，移动坐标系与全局坐标系的夹角为θ，则该障碍物在全局坐标系下的栅格坐标为：对每个栅格赋值，-1表示无障碍栅格，0表示未知栅格，1表示该栅格被障碍物占据；规定，即使某个栅格只有部分区域被障碍物占据，仍赋值为“1”。

步骤2：构建节点队列

节点定义为邻域内有未遍历的栅格，当前栅格邻域内的未遍历栅格数量定义为：n为与当前栅格相邻的栅格个数，g表示栅格状态；若N_g＞0，则将当前栅格定义为节点，并加入到节点队列，节点队列保存该节点的栅格坐标以及机器人经过该节点的时间。

步骤3：将栅格地图中的每一个栅格作为一个神经元，并初始化每个神经元的活性值为0。

步骤4：判断节点队列是否为空。若节点队列为空，说明已经没有未遍历节点，结束遍历工作；否则需要继续遍历，首先更新节点队列，剔除队列中已遍历的节点，添加新的节点，然后执行步骤5。

步骤5：按时间间隔T读取激光传感器采集的障碍物的测距数据，并将数据转化成栅格坐标，并将全局坐标系的每个神经元(每个神经元对应一个栅格)的活性值按如下公式更新：

其中，x_i表示第i个神经元的状态，即神经元活性；A,B和D是非负常数，A代表衰减率，B和D分别代表神经元状态x_i的上下限，即x_i∈[-D,B]；k是与第i个神经元相邻近的神经元个数；I_i为外部输入，当外部为未清扫区域时I_i＝E；当外部为障碍时I_i＝-E；否则I_i＝0。E是一个很大的正常数(E>>B)，确保了目标(未清扫区域)总是处于神经网络的波峰，障碍总是处于波谷。和[I_i]^-分别表示刺激性和抑制性输入。第i个神经元和第j个神经元的连接权值可以定义为ω_ij＝f(|q_i-q_j|)，|q_i-q_j|代表向量q_i和q_j之间的欧氏距离.f(a)可以是任意单调递减的函数，例如可以定义为：

其中μ和r₀都是正常数，r₀表示感受域，即机器人传感器的探测半径。因此，每个神经元只与小范围区域[0，r₀]内的神经元有直接联系。这里只考虑与当前栅格紧邻的8个栅格，因此取r₀＝2，神经元连接示意图如图3所示，第i个神经元的感受域为半径为r₀的圆区域，第i个神经元。

步骤6：路径规划

该步骤首先判断机器人在当前位置是否陷入死锁。死锁定义为当前位置的相隔为1的邻域内都为障碍或者已遍历。若机器人未陷入死锁，则采用基于生物激励神经网络模型的路径规划方法决策出下一个目标位置；否则，利用回溯搜索算法与D*算法相结合的规划路径方法首先从节点队列中找出距离当前时间最近的节点作为目标位置，然后用D*算法规划出当前位置到目标位置的最短路径，最后控制机器人移动到目标位置。返回步骤4。

图4是由本方法生成的路径，该地图由30*23个栅格组成.机器人默认按照从左到右，从上到下做“弓”字型运动。机器人从S(3,2)出发，将经过点D(14,2)，最终到达终点E(29,8)。

图5表示机器人在D的活性直方图，活性值最高的区域表示检测到的尚未遍历的区域，左边活性值衰减到接近0的区域表示已覆盖的区域，未知区域的活性值保持在0。从图中可以看出该算法在遍历过程中可将障碍物轮廓构建出来。

图6是本发明在双U型障碍物问题下生成的路径，图中，除左下角处的黑色点以外其它的黑色点表示机器人陷入死锁，灰色点表示由回溯搜索算法与D*算法生成的路径。从图中可以看出，机器人的行走路径平滑，转弯少，在陷入死锁时能快速找到一条最短路径逃离到未遍历区域。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，本领域的普通技术人员可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明的精神和范围，本发明的保护范围应以权利要求书所述为准。