CN110471429A - 基于改进人工势场法的割草机器人实时避障方法 - Google Patents

Abstract

本发明公开了一种基于改进人工势场法的割草机器人避障方法，包括下述步骤：S1、设定割草机器人起点和目标点的位置，同时设定引力势场增益系数和斥力势场增益系数，建立割草机器人周围环境模型；S2、对引力势场函数进行改进；S3、对斥力势场函数进行改进，S5、计算割草机器人所受引力和各个斥力的大小和方向，计算引力和斥力在水平方向和竖直方向上的分量，并求出割草机器人所受的总的势场力大小和方向；S6、割草机器人在改进人工势场法的总势场力的作用下向目标点移动，并更新割草机器人坐标得到割草机器人的规划路径。本发明使得割草机器人能够安全、平稳和高效运行，提高了改进的避障方法的实用性。

Description

基于改进人工势场法的割草机器人实时避障方法

技术领域

本发明属割草机器人的技术领域，具体涉及一种基于改进人工势场法的割草机器人实时避障方法。

背景技术

人工势场法是目前常见的机器人局部路径规划方法之一，其基本思想是通过传感器感知出环境中机器人、障碍物、目标点的位置，障碍物对机器人产生斥力势场，目标点产生引力势场，机器人在引力势场和斥力势场两者产生的复合势场中，搜索总势场下降的路线作为机器人避撞的最优路径。该方法具有结构简单，实时性强，规划路径高效平滑和易于实现底层实时控制等优点，在机器人避障系统的路径规划中得到了广泛的应用。但是经典人工势场法和改进人工势场法仍存在一些问题和不足：障碍物附近目标不可达、在障碍物附近容易产生振荡和机器人易陷入局部极小点等。

通过查阅大量文献资料，针对目标点不可达问题大都采用考虑机器人与目标点之间的欧几里得距离，将其作为距离调节因子对斥力函数进行改进，解决了经典人工势场法目标点周围存在障碍物时，目标不可达的问题。但是，这种形式的斥力势场在保证目标点全局最小的情况下，导致机器人不在目标点附近时，极大的扭曲了斥力势场的形状，使得整个路径规划质量变差，进而使得改进的算法实用性变差，影响机器人的工作效率。

发明内容

本发明是基于经典人工势场法和改进人工势场法的问题和不足，提出一种基于改进人工势场法的割草机器人实时避障方法，不仅解决了机器人在障碍物附近目标不可达、在障碍物附近容易产生振荡和机器人易陷入局部极小值问题，而且使得整个路径规划质量更优，提高了机器人的运行效率，使得改进的算法更具有实用性和高效性。

为了达到上述目的，本发明采用以下技术方案：

本发明提供了一种基于改进人工势场法的割草机器人避障方法，包括下述步骤：

S1、对割草机器人周围进行环境进行感知，实时反馈周围障碍物的距离和角度信息以及机器人自身的位姿信息，建立割草机器人周围环境模型，所述周围环境模型为各个障碍物和目标点相对于割草机器人的位置分布，同时设定引力势场增益系数和斥力势场增益系数；

S2、对引力势场函数进行改进，具体为：

由目标点生成的引力势场函数，其经典函数形式为

式中：ρ(X,X_g)为X和X_g两个位置之间的欧几里得距离，X为割草机器人当前位置，X_g为目标位置，k为大于0的引力势场系数；

当割草机器人与目标点之间的距离很大时，斥力在割草机器人运动控制中几乎不起作用，很容易导致割草机器人与障碍物发生碰撞；故通过增加一个由实际环境决定的范围值d来修改引力势场函数Ut_att(X)，避免机器人与障碍物发生碰撞；当机器人与目标点之间的距离小于d时，引力势场函数形式采用当机器人与目标点之间的距离大于d时，引力势场函数形式采用由此根据实际环境将引力势场分成以上连续的两段，可以很好地解决发生碰撞的问题，并且能够降低引力势场形状的畸变，则改进后引力势场函数具体形式为：

式中：k为引力增益系数，d为环境决定的常数，X(x,y)为割草机器人当前位置，ρ(X,X_g)为割草机器人与目标点之间的欧几里得距离，ρ_o障碍物的影响半径；

S3、对斥力势场函数进行改进，具体为：

添加旋转力的改进斥力势场函数，具体形式为：

式中：η为斥力势场增益系数，ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离，ρ_o为障碍物的影响范围，m为调节因子的指数；

这种形式的斥力势场在保证目标点全局最小的情况下，导致机器人不在目标点附近时，极大的扭曲了斥力势场的形状，且仍存在机器人陷入局部极小值的问题；

为此，引入考虑割草机器人本体半径的调节因子改进斥力势场函数，使得机器人不在目标点附近时，最小化扭曲障碍物势场，同时保证机器人在目标点在目标点取全局最小，则改进斥力势场函数具体形式为：

式中：η为斥力增益系数，ρ_o障碍物的影响半径，R为机器人半径，X_o(x_o,y_o)为障碍物位置，X_g(x_g,y_g)为目标点位置，ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离，ρ(X,X_g)为机器人与目标点之间的欧几里得距离；

S4、基于改进的引力势场函数和改进的斥力势场函数，得到割草机器人所受的引力和斥力以及总势场函数和总势场力函数，具体为：

改进后的人工势场法的总势场函数为改进后的引力势场函数和斥力势场函数之和；若割草机器人周围存在多个障碍物时，则总的势场函数等于引力势场函数与各斥力势场函数之和；割草机器人所受引力等于引力势场函数的负梯度及其所受斥力等于斥力势场函数的负梯度；改进后的人工势场法的总势场力函数等于所得到的总的势场函数的负梯度；

S5、计算割草机器人所受引力和斥力在水平方向和竖直方向上的分量，并求出割草机器人所受的总的势场力与水平方向的夹角θ，所述夹角θ为割草机器人的航向角；机器人所受势场力水平方向上的分量为在竖直方向上的分量为则割草机器人所受的总的势场力与水平方向的夹角为

式中：θ为割草机器人的航向角，F_attx(X)、F_atty(X)分别为机器人所受引力在水平方向和竖直方向上的分量，F_repxi(X)、F_repyi(X)(i＝1,2,L,n)分别为机器人所受斥力在水平方向和竖直方向上的分量，F_totalx(X)、F_totaly(X)分别为机器人所受势场力水平方向和竖直方向上的分量；

S6、割草机器人在改进人工势场法的总势场力的作用下向目标点移动，并更新割草机器人坐标，当割草机器人未到达目标点时，则在合力作用下继续运行；当割草机器人到达目标点时，则停止运行，由此，得到割草机器人的规划路径。

作为优选的技术方案，在步骤S1中，由搭载在割草机器人上的激光雷达和GPS/IMU对割草机器人周围进行环境感知，实时反馈周围障碍物的距离和角度信息以及机器人自身的位姿信息，为割草机器人提供环境信息进行路径规划，从而实现机器人的实时避障。

作为优选的技术方案，步骤S2中，

由于引力函数等于引力势场函数的负梯度，则改进后的引力函数的具体形式如下：

式中：k为引力增益系数，d为环境决定的常数，X(x,y)为机器人当前位置，ρ(X,X_g)为机器人与目标点之间的距离，ρ_o为障碍物的影响半径。

作为优选的技术方案，步骤S3中，

与引力函数定义相同，斥力函数等于斥力势场函数的负梯度，则斥力函数具体形式如下：

式中：

作为优选的技术方案，步骤S4中，所述割草机器人所受的总势场函数和势场力，具体为：

改进后的人工势场法的总势场函数为

改进后的人工势场法的总势场力函数为

式中：n为障碍物的数量。

作为优选的技术方案，还包括下述步骤：

设定割草机器人的移动步长l，更新机器人坐标：

式中：x(k+1)为割草机器人在k+1时刻的水平方向的坐标，x(k)为机器人在k时刻的水平方向的坐标，y(k+1)为机器人在k+1时刻的竖直方向的坐标，y(k)为机器人在k时刻的竖直方向的坐标，l为机器人的移动步长，θ为割草机器人所受的总的势场力与水平方向的夹角θ。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明通过将现有模型的引力势场函数分成连续的两段，使机器人在两段内分别采用不同的引力公式。当机器人与目标点之间的距离很大时，引力也随之变得很大，引力起主要作用，斥力在机器人运动控制中几乎不起作用，很容易导致机器人与障碍物发生碰撞。采用引力势场函数分段设计，可以很好地解决考虑路径规划偏差时机器人在障碍物环境中面临的发生碰撞的风险。

2、针对人工势场法的存在局部极小值和障碍物周边振荡问题，一些文献通过斥力势场乘以一个不小于0的多项式为机器人到目标点距离的平方。这种形式的斥力势场在保证目标点全局最小的情况下，导致机器人不在目标点附近时，极大的扭曲了斥力势场的形状。引入考虑割草机器人半径的调节因子改进斥力势场函数，使得机器人不在目标点附近时，最小化扭曲障碍物势场，同时保证机器人在目标点在目标点取全局最小。除此之外，改进的斥力势场函数有助于解决机器人在障碍物附近容易产生振荡和陷入局部极小值问题。

3、本发明的改进人工势场法不仅解决了机器人在障碍物附近目标不可达、在障碍物附近容易产生振荡和机器人易陷入局部极小值问题，而且使得整个路径规划质量更优，提高了机器人的运行效率，使得改进的算法更具有实用性和高效性。

附图说明

图1为本发明割草机器人周围环境模型；

图2为本发明割草机器人在障碍物环境中所受势场力示意图；

图3为本发明仅改进斥力势场的人工势场法得到的规划路径；

图4为本发明改进人工势场法得到的规划路径；

图5为基于改进人工势场法的割草机器人实时避障方法流程图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

人工势场法的机器人与目标点之间的距离直接决定了引力势场函数或引力大小。当机器人与目标点之间的距离很大时，引力势场函数或引力也随之变得很大，换句话说，引力起主要作用，斥力在机器人运动控制中几乎不起作用，很容易导致机器人与障碍物发生碰撞。为此，将引力势场函数根据实际环境分成连续的两段，可以很好地解决这个问题，并且能够降低引力势场形状的畸变；引入考虑割草机器人半径的调节因子改进斥力势场函数，使得机器人不在目标点附近时，最小化扭曲障碍物势场形状，同时保证机器人在目标点在目标点取全局最小，从而解决了机器人在障碍物附近目标不可达、在障碍物附近容易产生振荡和易陷入局部极小点等问题，使得机器人能够安全、平稳和高效运行，提高了改进的算法的实用性。

如图4所示，本实施例一种基于改进人工势场法的割草机器人实时避障方法，其实施步骤为：

在同一坐标系下，如图1所示，设定割草机器人起点和目标点的位置以及引力增益系数、斥力增益系数和障碍物的影响半径等参数，采用二维激光雷达和GPS/IMU进行环境感知，实时反馈周围障碍物的距离和角度信息以及机器人自身的位姿信息建立机器人周围的环境模型。割草机器人本体采用四轮独立驱动，可实现差速转向。

设置割草机器人起始位置为X_o(x_o,y_o)，X_g(x_g,y_g)为目标点位置，X(x,y)为机器人当前位置。通过对激光雷达和GPS/IMU获得的原始数据进行预处理，将极坐标系下的数据信息变换到直角坐标系下，可得到机器人在直角坐标系下的周围障碍物具体位置信息X_o(x_o,y_o)和自身位姿信息。

如图2所示，机器人在障碍物环境中所受势场力示意图，对割草机器人与目标点之间的引力进行受力分析和设计。由环境感知得到障碍物坐标为X_o(x_o,y_o)，X_g(x_g,y_g)为目标点位置，X(x,y)为机器人当前位置。

在步骤S2中，所述优化设计的引力势场函数和引力函数具体为：

改进后的引力函数，如下式所示：

式中：k为引力增益系数，d为环境决定的常数，X(x,y)为机器人当前位置，ρ(X,X_g)为机器人与目标点之间的欧几里得距离，ρ_o障碍物的影响半径。

进一步的，步骤S2中，计算割草机器人与目标点之间的引力，以及引力在水平方向和竖直方向上的分量。计算机器人和目标点之间连线与水平方向(即X轴方向)之间的夹角为α，则引力在水平方向的分量大小为F_attx(X)＝F_att(X)cosα，在竖直方向上的分量大小为F_atty(X)＝F_att(X)sinα。

对割草机器人与障碍物之间的斥力进行受力分析和设计。

如图3所示，步骤S3中，所述添加旋转力的改进斥力势场函数具体为：

式中：η为斥力势场增益系数；ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离；ρ_o为障碍物的影响范围，m为调节因子的指数。

进一步的，步骤S3中，所述改进的斥力势场函数和斥力函数具体为：

式中：η为斥力增益系数，ρ_o障碍物的影响半径，R为割草机器人本体半径，X_o(x_o,y_o)为障碍物位置，X_g(x_g,y_g)为目标点位置，ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离，ρ(X,X_g)为机器人与目标点之间的欧几里得距离。

改进后的斥力函数为：

式中：

更进一步的，步骤S3中，计算机器人与各障碍物之间的斥力，求出总的斥力以及其在水平方向和竖直方向上的分量。计算机器人和各障碍物之间连线与水平方向(即X轴方向)之间的夹角为β_i，则斥力在水平方向的分量大小为F_repxi(X)＝F_repi(X)cosβ_i，在竖直方向上的分量大小为F_repyi(X)＝F_repi(X)sinβ_i。

进一步的，步骤S4中，所述割草机器人所受的总势场和势场力，具体为：

改进后的人工势场法的总势场函数为

改进后的人工势场法的总势场力函数为

式中：n为障碍物的数量。

进一步的，步骤S5中，计算引力与各斥力在水平方向和竖直方向上的分量和以及合力与水平方向的夹角θ。合力在水平方向上的分量为在竖直方向上的分量大小为合力与水平方向的夹角为

式中：θ为割草机器人的航向角，F_attx(X)、F_atty(X)分别为机器人所受引力在水平方向和竖直方向上的分量，F_repxi(X)、F_repyi(X)(i＝1,2,L,n)分别为机器人所受斥力在水平方向和竖直方向上的分量，F_totalx(X)、F_totaly(X)分别为机器人所受势场力水平方向和竖直方向上的分量。

如图4所示，在步骤S6中，设定割草机器人的移步长l，更新机器人坐标：

x(k+1)＝x(k)+lcosθ

y(k+1)＝y(k)+lsinθ

割草机器人在总势场力的作用下向目标点移动，当机器人未到达目标点时，则在合力作用下继续运行；当机器人到达目标点时，则停止运行。由此，得到了满足机器人要求的规划路径。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

1.基于改进人工势场法的割草机器人避障方法，其特征在于，包括下述步骤：

S1、对割草机器人周围进行环境进行感知，实时反馈周围障碍物的距离和角度信息以及机器人自身的位姿信息，建立割草机器人周围环境模型，所述周围环境模型为各个障碍物和目标点相对于割草机器人的位置分布，同时设定引力势场增益系数和斥力势场增益系数；

S2、对引力势场函数进行改进，具体为：

由目标点生成的引力势场函数，其经典函数形式为

式中：ρ(X,X_g)为X和X_g两个位置之间的欧几里得距离，X为割草机器人当前位置，X_g为目标位置，k为大于0的引力势场系数；

当割草机器人与目标点之间的距离很大时，斥力在割草机器人运动控制中几乎不起作用，很容易导致割草机器人与障碍物发生碰撞；故通过增加一个由实际环境决定的范围值d来修改引力势场函数Ut_att(X)，避免机器人与障碍物发生碰撞；当机器人与目标点之间的距离小于d时，引力势场函数形式采用当机器人与目标点之间的距离大于d时，引力势场函数形式采用由此根据实际环境将引力势场分成以上连续的两段，可以很好地解决发生碰撞的问题，并且能够降低引力势场形状的畸变，则改进后引力势场函数具体形式为：

式中：k为引力增益系数，d为环境决定的常数，X(x,y)为割草机器人当前位置，ρ(X,X_g)为割草机器人与目标点之间的欧几里得距离，ρ_o障碍物的影响半径；

S3、对斥力势场函数进行改进，具体为：

添加旋转力的改进斥力势场函数，具体形式为：

式中：η为斥力势场增益系数，ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离，ρ_o为障碍物的影响范围，m为调节因子的指数；

这种形式的斥力势场在保证目标点全局最小的情况下，导致机器人不在目标点附近时，极大的扭曲了斥力势场的形状，且仍存在机器人陷入局部极小值的问题；

为此，引入考虑割草机器人本体半径的调节因子改进斥力势场函数，使得机器人不在目标点附近时，最小化扭曲障碍物势场，同时保证机器人在目标点在目标点取全局最小，则改进斥力势场函数具体形式为：

式中：η为斥力增益系数，ρ_o障碍物的影响半径，R为机器人半径，X_o(x_o,y_o)为障碍物位置，X_g(x_g,y_g)为目标点位置，ρ(X,X_o)为机器人当前位置X和障碍物位置X_o之间的欧几里得距离，ρ(X,X_g)为机器人与目标点之间的欧几里得距离；

S4、基于改进的引力势场函数和改进的斥力势场函数，得到割草机器人所受的引力和斥力以及总势场函数和总势场力函数，具体为：

改进后的人工势场法的总势场函数为改进后的引力势场函数和斥力势场函数之和；若割草机器人周围存在多个障碍物时，则总的势场函数等于引力势场函数与各斥力势场函数之和；割草机器人所受引力等于引力势场函数的负梯度及其所受斥力等于斥力势场函数的负梯度；改进后的人工势场法的总势场力函数等于所得到的总的势场函数的负梯度；

S5、计算割草机器人所受引力和斥力在水平方向和竖直方向上的分量，并求出割草机器人所受的总的势场力与水平方向的夹角θ，所述夹角θ为割草机器人的航向角；机器人所受势场力水平方向上的分量为在竖直方向上的分量为则割草机器人所受的总的势场力与水平方向的夹角为

式中：θ为割草机器人的航向角，F_attx(X)、F_atty(X)分别为机器人所受引力在水平方向和竖直方向上的分量，F_repxi(X)、F_repyi(X)(i＝1,2,L,n)分别为机器人所受斥力在水平方向和竖直方向上的分量，F_totalx(X)、F_totaly(X)分别为机器人所受势场力水平方向和竖直方向上的分量；

S6、割草机器人在改进人工势场法的总势场力的作用下向目标点移动，并更新割草机器人坐标，当割草机器人未到达目标点时，则在合力作用下继续运行；当割草机器人到达目标点时，则停止运行，由此，得到割草机器人的规划路径。

2.根据权利要求1所述基于改进人工势场法的割草机器人避障方法，其特征在于，在步骤S1中，由搭载在割草机器人上的激光雷达和GPS/IMU对割草机器人周围进行环境感知，实时反馈周围障碍物的距离和角度信息以及机器人自身的位姿信息，为割草机器人提供环境信息进行路径规划，从而实现机器人的实时避障。

3.根据权利要求1所述基于改进人工势场法的割草机器人避障方法，其特征在于，步骤S2中，

由于引力函数等于引力势场函数的负梯度，则改进后的引力函数的具体形式如下：

式中：k为引力增益系数，d为环境决定的常数，X(x,y)为机器人当前位置，ρ(X,X_g)为机器人与目标点之间的距离，ρ_o为障碍物的影响半径。

4.根据权利要求1所述基于改进人工势场法的割草机器人避障方法，其特征在于，步骤S3中，

与引力函数定义相同，斥力函数等于斥力势场函数的负梯度，则斥力函数具体形式如下：

式中：

5.根据权利要求4所述基于改进人工势场法的割草机器人避障方法，其特征在于，步骤S4中，所述割草机器人所受的总势场函数和势场力，具体为：

改进后的人工势场法的总势场函数为

改进后的人工势场法的总势场力函数为

式中：n为障碍物的数量。

6.根据权利要求1所述基于改进人工势场法的割草机器人避障方法，其特征在于，还包括下述步骤：

设定割草机器人的移动步长l，更新机器人坐标：

式中：x(k+1)为割草机器人在k+1时刻的水平方向的坐标，x(k)为机器人在k时刻的水平方向的坐标，y(k+1)为机器人在k+1时刻的竖直方向的坐标，y(k)为机器人在k时刻的竖直方向的坐标，l为机器人的移动步长，θ为割草机器人所受的总的势场力与水平方向的夹角θ。