[发明专利]一种基于改进人工势场法的无人机三维避障方法

权利要求书

1.一种基于改进人工势场法的无人机三维避障方法，其特征在于，该方法包括以下步骤：

步骤S1、建立三维地形环境模型，初始化人工势场法的参数、无人机初始位置、障碍物位置以及目标点；

步骤S2、计算无人机当前位置的引力和斥力，并对合力进行判断；当合力为0时，转步骤S3，否则，转步骤S4；

步骤S3、当合力为0时，若无人机到达目标点，则结束；否则，采用六边形导向法确定下一步的虚拟目标点，并采用人工势场法对无人机移动至所述虚拟目标点的路径进行规划，当所述无人机移动至所述虚拟目标点后，转步骤S2；

步骤S4、若无人机在当前位置的斥力大于预设倍数的引力时，对斥力场函数进行修正，并转步骤S2；否则，采用人工势场法将无人机移动至下一位置后判断是否到达目标点，若到达目标点，则结束，否则转步骤S1。

2.根据权利要求1所述的一种基于改进人工势场法的无人机三维避障方法，其特征在于，所述步骤S1具体为：

初始化无人机初始位置，目标点位置，障碍物位置，无人机和障碍物之间的安全距离以及无人机到达目标点的预测距离；

初始化人工势场法的引力场参数、斥力场参数以及斥力差值设定值；

初始化六边形导向法对应的最大步长以及迭代次数。

3.根据权利要求1所述的一种基于改进人工势场法的无人机三维避障方法，其特征在于，所述步骤S3中采用六边形导向法确定下一步的虚拟目标点，具体为：

1)以无人机所处的局部极小点为坐标原点，将无人机与目标点连接的直线在水平面的投影作为y轴，取经过原点并垂直于y轴的水平面直线作为x轴；垂直于xoy平面的直线为z轴；

2)当无人机在z轴方向受到的斥力差值Rep_z大于预设斥力差值s且在y轴方向的斥力差值Rep_xy小于预设斥力差值时，以坐标原点为角点构造z轴方向的竖直正六边形；否则，以坐标原点为角点构造水平正六边形；

所述水平正六边形构造过程为：以坐标原点为角点，y轴为对称轴，在xoy平面构造一个以最大步长作为边长的正六边形；

所述竖直正六边形构造过程为：以坐标原点为角点，z轴为对称轴，在zoy平面构造一个以最大步长作为边长的正六边形；

3)将正六边形与当前位置最近的且远离障碍物方向的角点记为虚拟目标点。

4.根据权利要求3所述的一种基于改进人工势场法的无人机三维避障方法，其特征在于，所述步骤S3中采用人工势场法对无人机移动至所述虚拟目标点的路径进行规划具体过程为：

1)调整所述无人机在局部极小点处的初始行进方向；

对于水平正六边形的六边形导向法，定义初始角θ为无人机与x轴的夹角，当初始角θ为正时，无人机顺时针旋转θ_o度；否则，无人机逆时针旋转θ_o度；

对于竖直正六边形的六边形导向法，定义初始角θ为无人机与z轴的夹角，当初始角θ为正时，无人机顺时针旋转θ_o度；否则，无人机逆时针旋转θ_o度；

2)无人机基于变化后初始行进方向的采用人工势场法朝着预设的虚拟目标点移动。

5.根据权利要求4所述的一种基于改进人工势场法的无人机三维避障方法，其特征在于，所述θ_o设置为90。

6.根据权利要求1所述的一种基于改进人工势场法的无人机三维避障方法，其特征在于，所述步骤S4中对斥力场函数进行修正，具体为：

定义斥力场函数U_rep(X)：

其中，D(d(X，X_g))为距离修正因子，其表达式为dⁿ(X，X_g)，d(X，X_g)为无人机到障碍物的距离，n为预设的正实数；k_rep为斥力场常数，d(X，X₀)为无人机到目标点的距离，d₀为障碍物影响辐射半径；

则修正后的斥力函数F_rep(X)表达式为：