CN112965471B - 一种考虑角速度约束和改进斥力场的人工势场路径规划方法 - Google Patents

Abstract

本发明提供一种考虑角速度约束和改进斥力场的人工势场路径规划方法，属于路径规划领域。所述人工势场路径规划方法根据感知到的障碍物并结合所需的导航精度，实时地更新栅格地图。在每个迭代步中，根据装备与目标点、障碍物的相对位置，计算合力的理想方向。进而结合装备实际的角速度约束、速度信息，生成新的航路点。重复上述步骤，直到装备到达目标点邻域，即完成了路径规划过程。本发明通过引入角速度约束，避免了规划路径中的角度突变，提高了路径的光滑性和运动可行性；通过改进的斥力场设计方式，解决传统人工势场方法中可能出现的目标不可达现象；通过引入角速度和速度信息，将人工势场方法拓展为轨迹规划方法，便于执行后续的轨迹跟踪过程。

Description

一种考虑角速度约束和改进斥力场的人工势场路径规划方法

技术领域

本发明属于路径规划领域，涉及一种考虑角速度约束和改进斥力场的人工势场路径规划方法。

背景技术

人工势场是一种经典的路径规划方法。其基本思想为机器人在环境中的运动，抽象成质点粒子在人造引力场中的运动，其中障碍物对机器人产生斥力，目标点对机器人产生引力，机器人在合力的作用下避开障碍物并抵达目标点。传统的人工势场法具有良好的实时计算效率，在无人潜航器、无人机、机械臂等装备的路径规划问题中已经得到了广泛的引用，但其具有如下明显的缺陷：

(1)陷入局部最优点。复杂障碍场景下的势场设计通常会产生众多的局部最优点，机器人在该类工况下则会陷入局部最优点，无法完成后续的路径规划。

(2)不考虑角速度约束。任何装备的运动都服从特定的运动学或动力学方程，同时在有限的控制输入下，机器人的运动过程中朝向角不可能发生快速的改变。由于传统的人工势场方法不考虑角速度约束，因此得到的路径在实际中执行过程中可能并不可行。

(3)未提供时间信息。路径规划方法分为基于几何的方法以及轨迹规划方法，相比于基于几何的方法，轨迹规划方法在生成一系列航路点的同时还能够提供相应的时间信息。然而人工势场方法本身属于一种基于几何的方法，而非轨迹规划方法，不便于执行后续的轨迹跟踪。

发明内容

为了解决上述技术问题，本发明提出了一种考虑角速度约束和改进斥力场的人工势场路径规划方法。该方法通过提出改进的斥力场设计方式并融合装备的运动学特性，将传统的人工势场路径方法拓展为一种轨迹规划方法，从而为后续轨迹跟踪的执行提供高质量的参考运动轨迹，具有良好的鲁棒性与实时计算效率。

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

一种考虑角速度约束和改进斥力场的人工势场路径规划方法，根据感知到的障碍物并结合所需的导航精度，实时地更新栅格地图。在每个迭代步中，根据装备与目标点、障碍物的相对位置，计算合力的理想方向。进而结合装备实际的角速度约束、速度信息，生成新的航路点。重复上述步骤，直到装备到达目标点邻域，即完成了路径规划过程。本发明的计算流程图如图1所示，包括以下步骤：

步骤1：设定算法参数

设定规划算法中所需的参数，具体包括：出发时间T₀，时间步长ΔT，装备运动速度v_const，装备最大角速度

引力系数K_att，斥力系数K_rep，目标点引力场线性-二次阈值ρ_g，障碍物斥力作用半径ρ_o，斥力分力退化系数n，等效达到误差ε。设定目标点的位置为V(x_g,y_g)，起始点的位置与朝向角分别为G^[0](x^[0],y^[0])和θ^[0]。定义变量ρ(A,B)代表平面内A、B两点间的欧氏距离，定义变量

代表由点B指向点A的单位向量。设定迭代指标k＝0。

步骤2：获取装备的位置及方向信息

测量装备的位置与方向信息。记装备当前的位置为G^[k](x^[k],y^[k])，朝向角为θ^[k]。

步骤3：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，其中1表该栅格内存在障碍需要进行规避(记作障碍栅格)，0代表该栅格无障碍可以通行(记作无障碍)。栅格的尺寸应当根据导航所需的精度、装备尺寸、装备运动速度范围等因素综合确定。

步骤4：根据装备与目标点、障碍物的相对位置，确定合力的理想方向

所述的合力由目标点产生的引力以及障碍物产生的斥力两部分组成。

首先，计算目标点产生的引力场U_att和对应的引力F_att，公式如下：

可以发现，当装备与目标点的距离小于目标点引力场线性-二次阈值ρ_g，引力线性变化；当大于该阈值，引力为一常数。传统人工势场法中引力场固定为二次函数的形式，当装备与目标点相对距离过大时，引力场数值过大，可能产生数值不稳定现象。而本专利中的分段处理方式可以有效避免这类数值不稳定现象的出现。

然后，计算由障碍物产生的斥力场和对应的斥力。对于栅格地图中的第j个障碍栅格，记其中心位置为

则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下：

可以发现，只有当装备与障碍栅格的距离小于障碍物斥力作用半径ρ_o时，装备才会受到来自障碍物的斥力影响。斥力F_rep,j具体由两个分力组成，其中F_rep1,j的方向由障碍栅格中心位置指向装备，F_rep2,j的方向由装备指向目标点。相比于传统人工势场仅依靠装备与障碍物的相对位置设计斥力场的方式，该改进的斥力场设计方式可以解决传统方法中的目标不可达现象。

记环境中一共有S个障碍栅格，则装备受到的合力为

在图2中，给出了仅有一个目标栅格情况下合力的构成方式。记

的方向为

则

即为当前迭代步中合力的理想方向。

步骤5：使用角速度约束修正合力方向

根据时间步长ΔT、装备最大角速度

计算相邻两个迭代步中装备朝向角最大改变量

定义如下两个变量：

Δθ^[k]＝θ^[k]-θ^[k+1] (7)

则装备在下一迭代步的朝向角增量应由如图3所示的饱和函数确定。为方便后续算法的说明，记图3中所示的饱和关系为：

根据式(9)计算的朝向角增量和步骤1中设定的装备运动速度，计算(k+1)时刻装备的位置与朝向角，计算公式如下：

步骤6：算法终止条件判断

更新迭代指标k＝k+1。若满足ρ(G^[k],V)≤ε，代表装备已经抵达目标点附近，迭代结束；否则，返回步骤2继续迭代。

本发明的有益效果为：本发明通过引入角速度约束，能够避免规划路径中的角度突变，提高了路径的光滑性和运动可行性；通过改进的斥力场设计方式，能够解决传统人工势场方法中可能出现的目标不可达现象；通过引入角速度和速度信息，将人工势场方法拓展为轨迹规划方法，便于执行后续的轨迹跟踪过程。

附图说明

图1为本发明的计算流程图。

图2为本发明的由引力和斥力合成合理的示意图。

图3为本发明的由角速度约束导致相邻迭代步中朝向角的变化饱和。

图4为本发明实施例中障碍环境。

图5为本发明实施例中障碍环境对应的人工势场。

图6(a)为本发明实施例中计算得到的路径；图6(b)为图6(a)标注①的局部放大图，图6(c)为图6(a)标注②的局部放大图；

图7(a)为本发明实施例中计算得到的朝向角变化历程；图7(b)为图7(a)标注①的局部放大图，图7(c)为图7(a)标注②的局部放大图.

图8(a)为本发明实施例中计算得到的角速度变化历程；图8(b)为图8(a)标注①的局部放大图，图8(c)为图8(a)标注②的局部放大图。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

考虑一无人船在图4所示的500m×400m的水域中的路径规划问题。目标点的位置设置为V(400m,200m)，起始点的位置与朝向角分别设定为G^[0](10m,150m)与θ^[0]＝0deg。

一种考虑角速度约束和改进斥力场的人工势场路径规划方法，包括以下步骤：

步骤1：设定算法参数

设定规划算法中所需的参数，具体包括：出发时间T₀＝0s，时间步长ΔT＝0.5s，无人船运动速度v_const＝1m/s，无人船最大角速度

引力系数K_att＝15，斥力系数K_rep＝5，目标点引力场线性-二次阈值ρ_g＝500m，障碍物斥力作用半径ρ_o＝20m，斥力分力退化系数n＝2，等效达到误差ε＝2·ΔT·v_const＝1m。设定目标点的位置为V(400m,200m)。设定迭代指标k＝0。

步骤2：获取无人船位置及方向信息

测量无人船的位置与方向信息。记无人船当前的位置为G^[k](x^[k],y^[k])，朝向角为θ^[k]。在k＝0的迭代步中，有x^[k]＝10m、y^[k]＝150m以及θ^[k]＝0deg，即无人船在T₀时刻的初始位姿。

步骤3：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，综合导航所需的精度、无人船尺寸、无人船运动速度等因素，确定栅格尺寸为2m×2m。其所对应的势场如图5所示。

步骤4：根据无人船与目标点、障碍物的相对位置，确定合力的理想方向

首先计算目标点产生的引力场U_att和对应的引力F_att如下

然后，计算由障碍物产生的斥力场和对应的斥力。对于栅格地图中的第j个障碍栅格，记其中心位置为

则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下

记环境中一共有S个障碍栅格，则无人船受到的合力为

记

的方向为

则

即为当前迭代步中合力的理想方向。

步骤5：使用角速度约束修正合力方向

根据时间步长ΔT＝0.5s、无人船最大角速度

计算相邻两个迭代步中无人船朝向角最大该变量Δθ_max＝10deg。定义如下两个变量

Δθ^[k]＝θ^[k]-θ^[k+1] (17)

则无人船在下一迭代步的朝向角增量应由如图3所示的饱和函数确定。为方便后续算法的说明，记图3中所示的饱和关系为

根据式(9)计算的朝向角增量和步骤1中设定的无人船运动速度，可以计算(k+1)时刻无人船的位置与朝向角如下

步骤6：算法终止条件判断

更新迭代指标k＝k+1。若满足ρ(G^[k],V)≤1m，代表无人船已经抵达目标点附近，迭代结束；否则，返回步骤2继续迭代。

根据上述步骤，在图6～图8与分别绘制了本方法(图例中IAPF)计算得到的路径、朝向角、角速度，并与传统人工势场方法(图例中BAPF)进行了对比。其中本方法消耗了496s，传统人工势场方法消耗了501s。此外，可以发现，本方法相比于传统方法明显的避免了障碍物局部的位置以及朝向角振荡(即生成的路径具有更佳的光滑性)，且施加的角速度约束得到了严格的满足。

本发明在经典人工势场法的基础上，通过引入角速度约束，并构造改进的斥力场，发展了一种改进的人工势场路径规划方法。角速度约束的施加令生成的路径更为光滑，避免了传统人工势场方法中突然的角度变化与振荡；得益于改进的斥力场，避免了传统人工势场方法中的目标不可达性；此外，由于引入了时间信息，这将人工势场这一传统的基于几何的方法拓展为了一种轨迹规划方法，便于后续的轨迹跟踪执行。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。

Claims (1)

1.一种考虑角速度约束和改进斥力场的人工势场路径规划方法，其特征在于，根据感知到的障碍物并结合所需的导航精度，实时地更新栅格地图；在每个迭代步中，根据装备与目标点、障碍物的相对位置，计算合力的理想方向；进而结合装备实际的角速度约束、速度信息，生成新的航路点；重复上述步骤，当装备到达目标点邻域时完成路径规划过程；包括以下步骤：

步骤1：设定算法参数

设定规划算法中所需的参数，所述参数包括：出发时间T₀，时间步长ΔT，装备运动速度v_const，装备最大角速度

引力系数K_att，斥力系数K_rep，目标点引力场线性-二次阈值ρ_g，障碍物斥力作用半径ρ_o，斥力分力退化系数n，等效达到误差ε；设定目标点的位置为V(x_g,y_g)，起始点的位置与朝向角分别为G^[0](x^[0],y^[0])和θ^[0]；定义变量ρ(A,B)代表平面内A、B两点间的欧氏距离，定义变量

代表由点B指向点A的单位向量；设定迭代指标k＝0；

步骤2：获取装备的位置及方向信息

测量装备的位置与方向信息；记装备当前的位置为G^[k](x^[k],y^[k])，朝向角为θ^[k]；

步骤3：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，其中1代表该栅格内存在障碍，需要进行规避，记作障碍栅格；0代表该栅格无障碍可以通行，记作无障碍；

步骤4：根据装备与目标点、障碍物的相对位置，确定合力的理想方向

所述的合力由目标点产生的引力以及障碍物产生的斥力两部分组成；

首先，计算目标点产生的引力场U_att和对应的引力F_att，公式如下：

当装备与目标点的距离小于目标点引力场线性-二次阈值ρ_g，引力线性变化；当大于该阈值，引力为一常数；

然后，计算由障碍物产生的斥力场和对应的斥力；对于栅格地图中的第j个障碍栅格，记其中心位置为

则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下：

只有当装备与障碍栅格的距离小于障碍物斥力作用半径ρ_o时，装备才会受到来自障碍物的斥力影响；斥力F_rep,j由两个分力组成，其中F_rep1,j的方向由障碍栅格中心位置指向装备，F_rep2,j的方向由装备指向目标点；

记环境中一共有S个障碍栅格，则装备受到的合力为

步骤5：使用角速度约束修正合力方向

根据时间步长ΔT、装备最大角速度

计算相邻两个迭代步中装备朝向角最大改变量

定义如下两个变量：

Δθ^[k]＝θ^[k]-θ^[k+1] (7)

则装备在下一迭代步的朝向角增量由饱和函数确定，其饱和关系为：

根据公式(9)计算的朝向角增量和步骤1中设定的装备运动速度，计算k+1时刻装备的位置与朝向角，计算公式如下：

步骤6：算法终止条件判断

更新迭代指标k＝k+1；若满足ρ(G^[k],V)≤ε，代表装备已经抵达目标点附近，迭代结束；否则，返回步骤2继续迭代。

Images