[发明专利]一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法

权利要求书

1.一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法，其特征在于，其操作步骤具体如下：

1.1、判断无人机是否到达目的地，如到达目的地，则无人机编队及其避障控制系统操作结束；

如未到达目的地，则执行步骤1.2；

其中，在判断无人机是否到达目的地之前，先将其处于初始化无人机状态；

1.2、判断无人机是否为领航者，如是领航者，则执行步骤1.3中的领航者编队控制；

如是跟随者，则执行步骤1.4中的跟随者编队控制和步骤1.5中的跟随者避障控制；

1.3、领航者编队控制调用有限时间一致性算法得到领航者编队控制输入，随即执行步骤1.9；

其中，在领航者编队控制调用有限时间一致性算法得到领航者编队控制输入之前，先获取目标点的位置信息；

定义a₀(t)、V₀(t)以及X₀(t)＝[x₀,y₀]^T表示领航者的加速度、速度以及位姿，定义a_g(t)、V_g(t)以及X_g(t)＝[x_g,y_g]^T表示目标点g的加速度、速度以及位姿；领航者控制输入设计为：

其中，sig(x)为符号函数；0＜p₁＜1，c₁、c₂、p₁和p₂为调节参数，其可调节算法的收敛速度；

1.4、跟随者编队控制首先调用获取其他跟随者的位姿信息，然后再调用有限时间一致性算法产生跟随者编队控制输入，并执行步骤1.9；

其中，跟随者编队控制在调用有限时间一致性算法产生跟随者编队控制输入中，获取的是与之有通信关系的其他跟随者的位姿信息；

定义V_i(t)、V_j(t)、V₀(t)以及X_i(t)、X_j(t)、X₀(t)分别表示跟随者i、j和虚拟领航者的速度以及位姿，定义表示跟随者i和j相对于虚拟领航者的期望相对位姿；跟随者的编队控制输入设计为：

其中，i,j∈{1,2,…,n}，0＜p₁＜1，p₁,p₂,b₁,b₂,b₃,b₄∈R为调节参数，其可调节算法的收敛速度；

1.5、调用改进后的人工势场方法，然后执行步骤1.6和步骤1.7；

所述的改进后的人工势场方法首先需要构建人工势场；对障碍物仅构建斥力场，对无人机同时构建斥力场和引力场：

第一，构建一个以障碍物s为势场中心，以r_o为半径的圆形障碍物斥力场；若无人机i进入障碍物的势场影响范围内，即无人机与障碍物的距离小于最小安全距离，则无人机i受障碍物s的斥力场作用而产生规避加速度a_uo(i,s,t)，该加速度可驱使无人机i远离障碍物s；为应对快速移动的障碍物，引入一个垂直于障碍物s移动方向的辅助牵引加速度a_vuo(i,s,t)，利用该加速度辅助无人机i快速远离障碍物s；

第二，构建一个以无人机j为势场中心，以为半径的圆形无人机斥力场；同时，在斥力场上叠加一个引力场，该势场的影响范围为以无人机j为势场中心，以为半径的圆外；若无人机i进入无人机j的斥力场影响范围内，即无人机i与无人机j之间的距离小于则无人机i受无人机j的斥力场作用而产生加速度a_uu(i,j,t)，该加速度迫使无人机i远离无人机j；若上述两机的距离大于则无人机i受无人机j的引力场作用而产生加速度a_uu(i,j,t)，该加速度迫使无人机i接近无人机j；

1.6、跟随者避障控制首先获取其他跟随者的位姿信息，再判断其是否处于其他跟随者势场内，如处于其他跟随者势场内，则产生机间加速度，然后执行步骤1.8；

其中，跟随者避障控制在产生机间加速度中，获取的是与之有通信关系的其他跟随者位姿信息；

跟随者的无人机间加速度设计为：

其中，为跟随者j的斥力场最大作用半径，为引力场最小作用半径，d_uu(i,j)表示跟随者i和跟随者j之间的相对距离，和为加速度调节参数；

1.7、跟随者避障控制同时感知无人机周围的障碍物，判断其是否处于障碍物势场内，如处于障碍物势场内，则产生规避加速度及辅助牵引加速度，然后执行步骤1.8；

定义X_i(t)＝[x_i,y_i]^T以及X_s(t)＝[x_s,y_s]^T表示跟随者i和障碍物s的位姿，跟随者的规避加速度设计为：

其中，表示跟随者i与障碍物s之间的相对距离，r_o(s)表示障碍物s的势场范围，r_i^d表示无人机i的探测范围；和为规避加速度可调参数；

定义v_i(t)表示跟随者i的速度，v_s(t)表示障碍物s的速度，l₁表示跟随者i与障碍物s之间的连线，l₂表示障碍物s为端点沿其速度方向的射线，θ表示将l₂逆时针旋转至与l₁重合的角度，α表示跟随者i和障碍物s两者移动方向之间的相对角度；跟随者的辅助牵引加速度设计为：

其中，θ∈[0,2π],α∈[0,2π]，旋转角α和θ均为逆时针旋转；为辅助牵引加速度调节参数；R(φ)为旋转矢量矩阵，

辅助牵引加速度的方向与θ有关；当0≤θ＜180°时，辅助牵引加速度的方向为沿障碍物s的速度方向逆时针旋转π/2；否则，辅助牵引加速度的方向为沿障碍物s的速度方向顺时针旋转π/2；

1.8、步骤1.6中的机间加速度和1.7中的规避加速度及辅助牵引加速度通过叠加产生加速度融合，从而进行跟随者避障控制输入，并执行步骤1.9；

定义单无人机在空域中受障碍物势场及各无人机势场影响而产生多个加速度，将各加速度叠加后即为无人机所受的合加速度；所述跟随者的避障控制输入设计为：

其中，a_uo(i,s,t)为跟随者i在障碍物s斥力场影响下的规避加速度；a_vuo(i,s,t)为跟随者i在障碍物s斥力场影响下的辅助牵引加速度，a_uu(i,j,t)为跟随者i在跟随者j势场影响下的加速度；

1.9、将步骤1.3，1.4和1.8中的各输入融合产生无人机编队与避障控制协议，后执行步骤1.10；

具体的如式所示：

其中，u₀(t)为虚拟领航者的控制输入，u_i(t)为跟随者的控制输入；

b₁、b₂、b₃、b₄、以及为各项加速度调节参数；

1.10、更新无人机状态，执行步骤1。