[发明专利]一种多无人机的动态编队控制方法

权利要求书

1.一种多无人机的动态编队控制方法，其特征在于：包括以下几个步骤：

步骤一：队形保持方法；

(1)建立地面坐标系XOY；

建立地面坐标系XOY，其中X轴代表东向位置，Y轴代表北向位置，ML和MF分别表示编队长机和僚机，Ψ_L和Ψ_F分别表示长机和僚机的航迹偏角，MV表示虚拟结构设定的僚机，简称为虚拟僚机，L和a分别表示虚拟僚机对长机期望的距离和观测角，L_l和a_l分别表示实际僚机对长机的距离和观测角，存在的通信网络延迟为Δt，MVH和MFH分别表示虚拟僚机和实际僚机在Δt移动后的位置，MFH表示长机MF在通信网络延迟Δt内飞行的剖面距离，MVH表示虚拟僚机MV在通信网络延迟Δt内飞行的剖面距离；

(2)建立各个关系公式；

设定长机和僚机在通信周期内移动距离均为l，根据长机、僚机和虚拟僚机的几何关系建立长机ML和MVH的位置关系公式：

xMVH=xML-Lcos(ψL-a)+lcosψLyMVH=yML-Lsin(ψL-a)+lsinψL---(1)]]>

其中，x_MVH、y_MVH、x_ML、y_ML分别表示考虑通信延迟的虚拟僚机的东向位置、北向位置、长机的东向位置、北向位置，L表示虚拟僚机对长机期望的距离，Ψ_L表示长机的航迹偏角，a表示虚拟僚机对长机期望的观测角，l表示长机僚机在通信周期内移动距离；

建立僚机MF和MFH的位置关系公式：

xMFH=xMF+lcosψFyMFH=yMF+lsinψF---(2)]]>

其中，x_MFH、y_MFH、x_MF、y_MF分别表示考虑通信延迟的实际僚机的东向位置、北向位置、实际僚机的东向位置、北向位置，Ψ_F表示僚机的航迹偏角；公式(2)减公式(1)可以建立MVH和MFH的跟踪误差关系公式：

ex=xMFH-xMVH=xMF+lcosψF-xML+Lcos(ψL-a)-lcosψLey=yMFH-yMVH=yMF+lsinψF-yML+Lsin(ψL-a)-lsinψL---(3)]]>

其中，e_x、e_y分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差；对MVH和MFH的跟踪误差关系公式(3)进行求导，得到：

e·x=VFcosψF-lωFsinψF-VLcosψL-LωLsin(ψL-a)+lωLsinψLe·y=VFsinψF+lωFcosψF-VLsinψL+LωLcos(ψL-a)-lωLcosψL---(4)]]>

其中，V_L、V_F分别表示长机和僚机的速度，ω_L、ω_F分别表长机和僚机的偏航角速度，分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数，l表示长机僚机在通信周期内移动距离，L表示僚机对虚拟长机期望的距离，Ψ_L表示长机的航迹偏角，a表示虚拟长机期望的观测角，Ψ_F僚机的航迹偏角；

将地面坐标系的e_x、e_y投影到僚机的速度坐标系：

e‾x=excosψF+eysinψFe‾y=-exsinψF+eycosψF---(5)]]>

其中，e_x、e_y分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差，分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影；

对变换坐标后的公式(5)进行求导，得到：

e‾·x=e·xcosψF-exsinψF-e·ysinψF-eycosψFe‾·y=e·xsinψF+excosψF+e·ycosψF-eysinψF---(6)]]>

其中分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数，分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影的导数；

联立公式(3)、公式(4)和公式(6)得到：

e‾·x=VF-VLcos(ψL-ψF)-LωLsin(ψL-ψF-a)+lωLsin(ψL-ψF)+ωFe‾ye‾·y=lωF-VLsin(ψL-ψF)+LωLcos(ψL-ψF-a)-lωLcos(ψL-ψF)-ωFe‾x---(7)]]>

当僚机的偏航角速度和速度指令为公式(8)所示时，公式(7)可以化简为公式(9)：

VF=VLcos(ψL-ψF)+LωLsin(ψL-ψF-a)-lωLsin(ψL-ψF)-k1e‾xωF=(VLsin(ψL-ψF)-LωLcos(ψL-ψF-a)+lωLcos(ψL-ψF)-k2e‾y)/l---(8)]]>

其中，k₁，k₂表示实际编队过程中待调整的控制律系数；

e‾·x=-k1e‾x+ωFe‾ye‾·y=-k2e‾y-ωFe‾x---(9)]]>

考虑李雅普诺夫函数：

V=e‾x2+e‾y2V·=2e‾xe‾·x+2e‾ye‾·y---(10)]]>

公式(9)代入公式(10)可得

V·=2e‾xe‾·x+2e‾ye‾·y=-2(k1e‾x2+k1e‾y2)<0---(11)]]>

由李雅普诺夫定律，对于给定的偏差采用公式(8)的速度和角速度指令，编队队形偏差收敛为0，使得误差表达式收敛为0；

最终的编队过载指令和编队速度指令为：

n1*=ωFVFV1*=VF---(12)]]>

步骤二：避障方法；

地面障碍物在空间中服从均匀分布，M为无人机当前位置，V为无人机速度矢量，r为障碍物的威胁范围，R为无人机距离障碍物的距离，为保证无人机不进入威胁区，无人机可以沿左侧航迹MA飞行也可以沿右侧航迹MB飞行，设距无人机最近的威胁源为圆心O，理想威胁回避航迹为与圆O外切且与速度矢量V相切的两圆O_l、O_r的圆弧MA和MB，定义切点A、B为威胁回避导航点；

转弯半径应用解三角形的方法求取，在ΔOO_lM中应用余弦定理得：

cos(∠OMOl)=sin(b)=R2+Rl2-(Rl+r)22RRl---(13)]]>

得出无人机威胁回避的转弯半径：

Rl=R2-r22(r+Rsinb)---(14)]]>

Rr=R2-r22(r-Rsinb)---(15)]]>

相应的侧向加速度的表达式为：

ar=V2Rr,]]>al=V2Rl---(16)]]>

其中a_r、a_l分别表示无人机沿圆弧MB、MA进行避障所需要的侧向加速度；

最终的避障过载指令

n2*=aroral---(17)]]>

最终避障速度指令选择与长机速度对齐指令

V2*=VL---(18)]]>

其中V_L表示长机的速度；

步骤三：基于行为的编队过程；

(1)行为分解过程：将任务行为分解为三个互相平行的子行为：异常情况处理，队形保持方法和威胁回避方法，同时为每个子行为赋予相应的重要性，用权值表示；队形保持方法中无人机通过无线通信网络获得长机的信息，具体包括长机速度V_L、偏航角速度ω_L和航迹偏角Ψ_L；无人机通过自身本机导航设备获得导航信息，具体包括速度V_F、角速度ω_F和偏角Ψ_F，按照步骤一公式(8)和公式(12)生成编队过载指令和编队速度指令避障方法中无人机通过光学传感器获取障碍物的信息包括：无人机距离障碍物的距离R，速度矢量与无人机与障碍物连线的夹角为b，按照步骤二公式(17)和公式(18)生成避障过载指令和避障速度指令异常情况处理是指飞行器在执行飞行任务时，遇到通信链路中断等无法维持编队的原因，无人机飞控计算机进入异常处理状态，异常处理要保证无人机按照单机最优的策略执行任务或飞离搜索区域到达指定位置准备降落，异常处理在执行任务中优先级最高，无人机按照单机最优的策略执行任务需要的过载指令和速度指令分别为

(2)控制实现过程指无人机根据运动模型和当前运动状态，加权计算无人机实际的控制指令，进而输入执行机构：无人机的偏航通道产生方向舵偏，从而实现对无人飞行器编队的运动控制；

根据步骤一和步骤二中的队形保持方法和避障方法，各个子行为的过载指令和速度指令获得后按照公式(17)进行加权：

n*=wn0n0*+wn1n1*+wn2n2*]]>

V*=wV0V0*+wV1V1*+wV2V2*---(17)]]>

其中w_n0+w_n1+w_n2＝1.0，w_V0+w_V1+w_V2＝1.0

其中，w_n0表示异常处理时过载指令权值、w_V0表示异常处理时速度指令权值、w_n1表示队形保持时过载指令权值、w_V1表示队形保持时速度指令权值、w_n2表示避障时过载指令权值、w_V2表示避障时速度指令权值；

1)符合异常处理的条件：w_n0＝w_V0＝1.0，w_n1＝w_V1＝w_n2＝w_V2＝0.0，这时队形保持和避障方法已经不再有效，无人机采取单机飞行的方式执行任务，或者沿指定导航点进入降落阶段；

2)未符合异常处理条件时，队形保持方法与避障方法有效，按照光学传感器是否发现障碍物进行权值计算：

其中，w_n2，w_V2的取值取决于地面障碍物尺度与障碍物威胁范围的比值，w_n1＝1.0-w_n2，w_V1＝1.0-w_V2。