[发明专利]一种基于改进型人工势场法的多无人机编队避障方法

权利要求书

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

步骤1：对引力增益系数k_a、斥力增益系数k_r、无人机安全距离区间[d_min,d_max]进行初始化；d_min表示无人机安全距离最小值，d_max表示无人机安全距离最大值；

步骤2：初始化无人机当前位置坐标p_current，目标点位置为q₀，障碍物数量为N，障碍物的影响范围为ρ₀；

步骤3：无人机获取与编队内其他无人机之间距离d，判断d是否处于无人机安全距离区间[d_min,d_max]，计算无人机间的作用力F_uav(p)，

步骤4：通过无人机上的双目视觉模块获得障碍物的点云数据，对点云数据进行椭球拟合，拟合出障碍物的椭球体方程；

步骤5：无人机P的坐标为(x_p,y_p,z_p)，无人机P与第i个障碍物之间的距离D_poi(p,o_i)修正为点到椭圆的最短距离，其中椭圆的方程可由椭球体得到；

步骤6：计算无人机与障碍物的相对速度，引入斥力函数，得到改进后的斥力场函数F_rep(p)；

步骤7：获得无人机所受的合力；

步骤8：计算下一个航迹点p_next；

步骤9：按无人机所受合力的方向运动到下一个航迹点p_next；

步骤10：保存无人机当前的位置坐标，并将当前无人机的位置设置为起点坐标，p_current＝p_next；

步骤11：通过无人机与目标点的位置来判断无人机是否已经到达目标点，如果无人机没有到达目标点，则返回步骤3继续执行，直到运动至目标点。

2.根据权利要求1所述的方法，其特征在于，步骤3包括：

无人机间的作用力F_uav(p)为：

其中k_a为引力增益系数，k_r为斥力增益系数，为梯度。

3.根据权利要求2所述的方法，其特征在于，步骤5包括：

设定由障碍物椭球体得到的椭圆方程为：

其中，(x₀,y₀)为椭圆的中心点坐标，A,B为椭圆的焦距，(x，y)为椭圆上任意一点的坐标。

4.根据权利要求3所述的方法，其特征在于，步骤5还包括：离无人机P最近的椭圆上的点的坐标为(x₂,y₂)，通过求解如下方程组得到：

无人机P与第i个障碍物之间的距离D_poi(p,o_i)为：

5.根据权利要求4所述的方法，其特征在于，步骤6包括：

以障碍物与无人机的相对速度v_poc在无人机前进方向分量v_poc作为判断依据，只有当v_poc0时，才考虑相对速度的影响；改进后的斥力势能函数U_rep(p,v_po)如下：

其中，ξ₁,ξ₂为斥力系数，ρ₀为障碍物影响范围。

斥力函数F_rep(p)为：

X_pro表示无人机p相对于障碍物o的位置的单位向量。

6.根据权利要求5所述的方法，其特征在于，步骤7包括：

目标点对无人机产生的引力为F_att(p)，按如下公式获得无人机所受的引力：

引力势能函数U_att(p)如下：

目标点对无人机产生的引力F_att(p)为：

其中，k_a表示引力势能场的增益系数，D_pt(p,t)表示无人机所在的p点到目标t点的欧氏距离；

无人机所受的合力F为：

其中F为无人机所受的合力，F_att(p)表示目标点对无人机产生的引力，F_rep(p)表示障碍物对无人机产生的斥力，F_uav(p)表示编队其他无人机对无人机产生的作用力。