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

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

【技术实现步骤摘要】
一种考虑角速度约束和改进斥力场的人工势场路径规划方法
本专利技术属于路径规划领域，涉及一种考虑角速度约束和改进斥力场的人工势场路径规划方法。
技术介绍
人工势场是一种经典的路径规划方法。其基本思想为机器人在环境中的运动，抽象成质点粒子在人造引力场中的运动，其中障碍物对机器人产生斥力，目标点对机器人产生引力，机器人在合力的作用下避开障碍物并抵达目标点。传统的人工势场法具有良好的实时计算效率，在无人潜航器、无人机、机械臂等装备的路径规划问题中已经得到了广泛的引用，但其具有如下明显的缺陷：(1)陷入局部最优点。复杂障碍场景下的势场设计通常会产生众多的局部最优点，机器人在该类工况下则会陷入局部最优点，无法完成后续的路径规划。(2)不考虑角速度约束。任何装备的运动都服从特定的运动学或动力学方程，同时在有限的控制输入下，机器人的运动过程中朝向角不可能发生快速的改变。由于传统的人工势场方法不考虑角速度约束，因此得到的路径在实际中执行过程中可能并不可行。(3)未提供时间信息。路径规划方法分为基于几何的方法以及轨迹规划方法，相比于基于几何的方法，轨迹规划方法在生成一系列航路点的同时还能够提供相应的时间信息。然而人工势场方法本身属于一种基于几何的方法，而非轨迹规划方法，不便于执行后续的轨迹跟踪。
技术实现思路
为了解决上述技术问题，本专利技术提出了一种考虑角速度约束和改进斥力场的人工势场路径规划方法。该方法通过提出改进的斥力场设计方式并融合装备的运动学特性，将传统的人工势场路径方法拓展为一种轨迹规划方法，从而为后续轨迹跟踪的执行提供高质量的参考运动轨迹，具有良好的鲁棒性与实时计算效率。为了达到上述目的，本专利技术采用的技术方案为：一种考虑角速度约束和改进斥力场的人工势场路径规划方法，根据感知到的障碍物并结合所需的导航精度，实时地更新栅格地图。在每个迭代步中，根据装备与目标点、障碍物的相对位置，计算合力的理想方向。进而结合装备实际的角速度约束、速度信息，生成新的航路点。重复上述步骤，直到装备到达目标点邻域，即完成了路径规划过程。本专利技术的计算流程图如图1所示，包括以下步骤：步骤1：设定算法参数设定规划算法中所需的参数，具体包括：出发时间T0，时间步长ΔT，装备运动速度vconst，装备最大角速度引力系数Katt，斥力系数Krep，目标点引力场线性-二次阈值ρg，障碍物斥力作用半径ρo，斥力分力退化系数n，等效达到误差ε。设定目标点的位置为V(xg,yg)，起始点的位置与朝向角分别为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：根据装备与目标点、障碍物的相对位置，确定合力的理想方向所述的合力由目标点产生的引力以及障碍物产生的斥力两部分组成。首先，计算目标点产生的引力场Uatt和对应的引力Fatt，公式如下：可以发现，当装备与目标点的距离小于目标点引力场线性-二次阈值ρg，引力线性变化；当大于该阈值，引力为一常数。传统人工势场法中引力场固定为二次函数的形式，当装备与目标点相对距离过大时，引力场数值过大，可能产生数值不稳定现象。而本专利中的分段处理方式可以有效避免这类数值不稳定现象的出现。然后，计算由障碍物产生的斥力场和对应的斥力。对于栅格地图中的第j个障碍栅格，记其中心位置为则由该栅格产生的斥力场Urep,j和对应的斥力Frep,j如下：可以发现，只有当装备与障碍栅格的距离小于障碍物斥力作用半径ρo时，装备才会受到来自障碍物的斥力影响。斥力Frep,j具体由两个分力组成，其中Frep1,j的方向由障碍栅格中心位置指向装备，Frep2,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：设定算法参数设定规划算法本文档来自技高网...

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

【技术特征摘要】
1.一种考虑角速度约束和改进斥力场的人工势场路径规划方法，其特征在于，根据感知到的障碍物并结合所需的导航精度，实时地更新栅格地图；在每个迭代步中，根据装备与目标点、障碍物的相对位置，计算合力的理想方向；进而结合装备实际的角速度约束、速度信息，生成新的航路点；重复上述步骤，当装备到达目标点邻域时完成路径规划过程；包括以下步骤：
步骤1：设定算法参数
设定规划算法中所需的参数，所述参数包括：出发时间T0，时间步长ΔT，装备运动速度vconst，装备最大角速度引力系数Katt，斥力系数Krep，目标点引力场线性-二次阈值ρg，障碍物斥力作用半径ρo，斥力分力退化系数n，等效达到误差ε；设定目标点的位置为V(xg,yg)，起始点的位置与朝向角分别为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：根据装备与目标点、障碍物的相对位置，确定合力的理想方向
所述的合力由目标点产生的引力以及障碍物产生的斥力两部分组成；
首先，计算目标点产生的引力场Uatt和对应的引力Fa...

【专利技术属性】
技术研发人员：王昕炜，刘洁，彭海军，陈飙松，张盛，李云鹏，吕琛，
申请(专利权)人：大连理工大学，
类型：发明
国别省市：辽宁;21