一种基于改进人工势场算法的移动机器人路径规划方法与流程

本发明属于移动机器人自主导航领域，特别是一种基于改进人工势场算法的移动机器人路径规划方法。

背景技术：

路径规划是移动机器人自主导航过程中的重要步骤，指的是：移动机器人按照实际情况规划出一条从起始点到目标点的安全无碰撞路径。移动机器人在工作环境中进行路径规划需要达到以下三个标准：(1)所规划的路径能够连接起始点和目标点；(2)所规划的路径必须有效避开环境中的障碍物；(3)所规划的路径在长度，平滑度等方面满足要求。

因为标准人工势场法原理简单、易于实现、计算量小、实时性强，所以其在移动机器人路径规划领域的应用较为广泛。然而，当使用标准人工势场法在室内多障碍物环境中进行路径规划时，经常出现目标不可达问题和陷入局部极小值点问题。目标不可达问题是指：当目标点在障碍物影响范围内时，移动机器人在目标点周围徘徊而不停留在目标点位置。陷入局部极小值点问题是指：当移动机器人在某个非目标点位置受力平衡时，机器人会停止运动而不继续向目标点前进。这两个问题的出现会对移动机器人的整个路径规划过程造成严重影响，并最终导致路径规划操作失败。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种适用于室内多障碍物环境的基于改进人工势场法的移动机器人路径规划方法。包括以下步骤：

s1，根据移动机器人到目标点的距离构造出引力势场函数。

s2，根据移动机器人到目标点的距离以及到障碍物的距离构造出斥力势场函数。

s3，基于移动机器人当前位置对引力势场函数求负梯度得到移动机器人当前受到的吸引力。

s4，基于移动机器人当前位置对斥力势场函数求负梯度得到移动机器人当前受到的排斥力。

s5，利用步骤s3和s4中计算得到的吸引力和排斥力引导移动机器人向目标点前进，包括：

若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置就是目标点，则路径规划完成；

若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置不是目标点，则移动机器人陷入局部极小值点，生成一个强制干扰力使移动机器人逃离该位置影响范围到达新的位置，之后继续执行路径规划操作；

若当前吸引力和当前排斥力的合力不为0，则移动机器人在当前吸引力和当前排斥力共同作用下运动一个步长到达新的位置，之后继续执行步骤s3。

本发明的优点及有益效果如下：

本发明针对使用标准人工势场法在室内多障碍物环境中进行路径规划时经常出现的目标不可达和陷入局部极小值点问题，提出一种基于改进人工势场法的移动机器人路径规划方法。该方法将受到反正切函数处理的移动机器人到目标点的距离引入到标准人工势场法的斥力势场函数中来解决目标不可达问题。其中，反正切函数存在的意义是：减少由于引入移动机器人到目标点的距离而给斥力势场函数带来的形变，进而确保所规划路径的质量不下降。此外，该方法通过以吸引力为基础构造出的强制干扰力打破受力平衡来解决陷入局部极小值点而无法逃离的问题。本发明引入受反正切函数处理的移动机器人到目标点距离以及由吸引力生成的强制干扰力，两个因素协同作用，可以提高移动机器人在室内多障碍物环境中路径规划的成功率，同时也使得移动机器人所规划的路径长度较短。

附图说明

图1是本发明提供优选实施例基于改进人工势场法的移动机器人路径规划实现框架；

图2是吸引力分解示意图；

图3是强制干扰力生成示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

参见图1，本发明的主要步骤为：

s1，初始化相关参数，设置路径规划起始点和目标点；

s2，根据移动机器人到目标点的距离构造出引力势场函数；

s3，根据移动机器人到目标点的距离以及到障碍物的距离构造出斥力势场函数；

s4，基于移动机器人当前位置对引力势场函数求负梯度得到移动机器人当前受到的吸引力；

s5，基于移动机器人当前位置对斥力势场函数求负梯度得到移动机器人当前受到的排斥力；

s6，若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置就是目标点，则路径规划完成。若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置不是目标点，则生成一个强制干扰力使移动机器人逃离该位置影响范围到达新的位置，之后继续执行路径规划操作。若当前吸引力和当前排斥力的合力不为0，则移动机器人在当前吸引力和当前排斥力共同作用下运动一个步长到达新的位置，之后继续执行路径规划操作。

步骤s2的具体实施过程为：

根据移动机器人到目标点的距离构造出引力势场函数uatt，具体公式如下：

上式中，k表示吸引增益系数。表示移动机器人到目标点的距离。其中，(xr,yr)表示移动机器人的世界坐标；(xg,yg)表示目标点的世界坐标。

步骤s3的具体实施过程为：

根据移动机器人到目标点的距离以及到障碍物的距离构造出斥力势场函数urep具体公式如下：

上式中，η表示排斥增益系数。表示移动机器人到障碍物的距离。其中，(xo,yo)表示障碍物的世界坐标。ρo表示障碍物的最大影响距离。表示反正切限制函数。n表示距离调节系数。

步骤s4的具体实施过程为：

s41：根据移动机器人当前位置对引力势场函数uatt求负梯度获得移动机器人当前受到的吸引力f′att，具体公式如下：

f′att＝-grad(uatt)＝-k·(x′r-xg)i-k·(y′r-yg)j

上式中，(x′r,y′r)表示移动机器人当前位置的世界坐标。i和j分别表示沿着世界坐标系横轴正方向以及纵轴正方向的单位向量。

s42：求得吸引力f′att的大小是方向是由移动机器人指向目标点。

步骤s5的具体实施过程为：

s51：根据移动机器人当前位置对斥力势场函数urep求负梯度获得移动机器人当前受到的吸引力f′rep，具体公式如下：

其中，f′x,rep以及f′y,rep的具体表达分别如下：

上两式中，表示当前移动机器人到目标点的距离。表示当前移动机器人到障碍物的距离。

s52：求得排斥力f′rep的大小是方向是由移动机器人指向目标点。

步骤s6的具体实施过程为：

s61：若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置就是目标点，则路径规划完成；

s62：若当前吸引力和当前排斥力的合力为0并且移动机器人当前位置不是目标点，则移动机器人陷入局部极小值点。此时依次执行(1)～(8)；

(1)保存移动机器人当前位置(即局部极小值点位置)在世界坐标系xwowyw中的坐标，记为a；

(2)以当前移动机器人的位置为原点，以当前吸引力f′att的方向偏移(包括左偏和右偏)θ为横轴正方向，建立直角坐标系xnonyn。其中，θ为取值范围是的随机角度。本实施例中按照右偏θ为横轴正方向，建立直角坐标系xnonyn。向左偏同理。

(3)在直角坐标系xnonyn中，将吸引力f′att沿着横纵坐标轴分解，得到两个分量吸引力，分别记为f′x,att和f′y,att。其中，f′x,att的坐标表示为(|f′att|cosθ,0)，f′y,att的坐标表示为(0,|f′att|sinθ)。

(4)利用随机系数对f′x,att和f′y,att进行修改，得到两个新的分量吸引力，和的坐标表示为(α|f′att|cosθ,0)，的坐标表示为(0,β|f′att|sinθ)。其中，α和β均表示取值范围是[0.5,1.5]的随机系数。注意，两个随机系数不能同时取相同的值；

(5)将与合成，得到一个新的吸引力，记为该吸引力是移动机器人逃离局部极小值点的关键，称为强制干扰力。其在直角坐标系xnonyn中的坐标表示为(α|f′att|cosθ,β|f′att|sinθ)。

(6)根据在直角坐标系xnonyn中的坐标表示计算出其在世界坐标系中的坐标表示具体公式如下：

上式中，∠f′att表示当前吸引力f′att在世界坐标系中的偏转角度。

(7)求得强制干扰力在世界坐标系中的大小为偏转角度为

(8)保持强制干扰力的大小和偏转角度不变，使移动机器人在该力以及所受吸引力和排斥力共同作用下运动5个步长，到达某个位置点，并将该位置点的世界坐标记为b。此时，不考虑强制干扰力的影响，计算出移动机器人在b位置所受到的合力。如果该合力与向量的夹角大于则移动机器人已经跳出局部极小值点。之后，撤销强制干扰力，移动机器人继续执行路径规划操作。如果该合力与向量的夹角小于等于则此强制干扰力作废，跳回到(2)重新生成强制干扰力。

s63：若当前吸引力和当前排斥力的合力不为0，则移动机器人沿着合力方向运动一个步长到达新的位置，之后继续执行路径规划操作。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。