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

权利要求书

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继续迭代。