[发明专利]一种基于速度势场的无人机避障方法和系统

权利要求书

1.一种基于速度势场的无人机避障方法，其特征在于，包括：

步骤S1、根据无人机的位置和速度、障碍物的位置和速度，检测无人机和障碍物的碰撞可能性；

步骤S2、若无人机与障碍物存在碰撞可能性，计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力；其中，所述引力场是目标点对无人机产生引力的势场，方向从无人机位置指向目标点，使无人机向目标点运动；所述改进的斥力场是根据无人机与目标点之间的位置关系，基于传统人工势场法进行改进，得到的障碍物对无人机产生斥力的势场，控制无人机规避障碍物；速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场，对无人机产生速度势场力，控制无人机规避运动障碍物；

步骤S3、根据步骤S20所述引力、改进的斥力场对无人机产生的斥力和速度势场力，计算无人机第i步所受合力，其中，i取值为正整数；

步骤S4、根据无人机第i步所受合力，计算得到无人机第i步的方向单位角，并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角，控制无人机避开障碍物。

2.根据权利要求1所述的方法，其特征在于，在步骤S1中，按照碰撞锥检测条件，对所述无人机和障碍物的碰撞可能性进行检测；所述碰撞锥检测条件为：

其中，v_r为无人机与障碍物的相对速度矢量，由v_r＝v-v_ob计算得到，v为无人机速度矢量，v_ob为所述障碍物的速度矢量，L_ob为所述无人机到所述障碍物的距离矢量，由L_ob＝p_ob-p计算得到，其中p_ob为所述障碍物的位置，p为所述无人机的位置，d_safe为所述无人机和所述障碍物需要保持的最小安全距离，d_safe＝r_ob+r，其中r_ob为所述障碍物半径，r为所述无人机的最小安全半径。

3.根据权利要求2所述的方法，其特征在于，根据碰撞锥检测条件，按照公式：

计算得到所述改进的斥力场对无人机产生的斥力；

式中：F_rep,cone为所述改进的斥力场对无人机产生的斥力；▽为梯度符号；U_rep,att为根据无人机与目标点之间的位置关系，基于传统人工势场法，得到的改进的斥力场：

式中：U_rep,att为改进的斥力场，η为障碍物斥力因子，p为无人机位置，p_ob为障碍物位置，p_goal为目标点位置，ρ₀为障碍物对无人机运动产生影响的最大距离，ρ(p,p_ob)表示障碍物到无人机的距离，ρ(p，p_ob)＝||p_ob-p||，ρ(p,p_goal)表示目标点到无人机的距离；

λ_cone为根据所述碰撞锥检测条件得到的碰撞锥判断系数；

按照公式：

确定碰撞锥判断系数λ_cone；

其中，v_r为无人机与障碍物的相对速度矢量，由v_r＝v-v_ob计算得到，v为无人机速度矢量，v_ob为所述障碍物的速度矢量，L_ob为所述无人机到所述障碍物的距离矢量，由L_ob＝p_ob-p计算得到，d_safe为所述无人机和所述障碍物需要保持的最小安全距离，d_safe＝r_ob+r，其中r_ob为所述障碍物半径，r为所述无人机的最小安全半径。