一种基于测距信号的固定翼无人机编队跟踪制导方法与流程

本发明涉及无人机飞行控制技术领域，尤其是一种基于测距信号的固定翼无人机编 队跟踪制导方法。

背景技术：

随着近年来科技水平的不断提高，无人机从最初的目标识别、通信中继到侦查监 视、灾害评估等，其应用领域得到了极大的扩展，未来的应用前景极其广阔。利用无人 机来对地面移动目标进行自动跟踪是无人机的一个重要的应用方向。相对于单架无人 机，多架无人机组成编队对地面目标进行协同跟踪具有更加明显的优势：一是可以扩大 单架无人机传感器的作用范围，降低丢失目标的概率；二是可以提高对目标运动状态的 估计精度，在应用上可以提供更高的灵活性。无人机协同跟踪地面目标制导方法设计的 主要难点在于如何使得多架无人机在自动跟踪地面目标的同时能够有效维持特定编队。

技术实现要素：

发明目的：为解决上述技术问题，本发明提出一种基于测距信号的固定翼无人机编 队跟踪制导方法，该方法能够仅使用距离传感器即实现对地面静止目标的自动协同跟 踪。

技术方案：为实现上述技术效果，本发明提出的技术方案为：

一种基于测距信号的固定翼无人机编队跟踪制导方法，该方法包括步骤：

(1)获取无人机和地面目标的航向信息，根据获取的信息数据构建无人机定距跟 踪地面目标的二维运动学模型：

其中，为无人机i与地面目标间的相对距离变化率；vi为无人机i的速度；χi为 无人机i的视线角，即无人机i速度方向与无人机和目标间连线的夹角；为χi的角变 化率；ωi无人机i的角速度；ρi为无人机i与地面目标间的相对距离；

(2)根据无人机定距跟踪地面目标的二维运动学模型，建立定距跟踪地面静止目 标的单无人机制导模型和无人机编队制导模型；其中，

定距跟踪地面静止目标的单无人机制导模型为：

式中，表示无人机i的航向跟踪制导律输出，表示单无人机制导增益，ρd表 示稳定定距跟踪时无人机与目标间距离；

定距跟踪地面静止目标的无人机编队制导模型包括：

无人机编队单邻居相位控制制导模型：

其中，表示无人机i的编队航向制导律输出，表示编队制导增益，ri,i-1表示无 人机i与其相邻无人机i-1之间的距离，N表示无人机编队中的无人机总数；

无人机编队单邻居速度控制制导模型：

其中，kv表示编队速度制导增益；

无人机编队双邻居相位控制制导模型：

无人机编队双邻居速度控制制导模型：

其中，表示无人机i的编队速度控制制导律输出。

(3)当单架无人机定距跟踪地面静止目标时，选取单无人机制导模型作为无人机 制导策略；

(4)当无人机编队定距跟踪地面静止目标时，根据无人机前、后、左、右四个探 测雷达所探测到的无人机数量，选择各无人机制导策略，包括步骤：

(4-1)定义：若无人机i前方或右方探测雷达探测到的无人机数量非空，则ri,i-1≠0，否则，ri,i-1＝0；若无人机i后方或左方雷达探测到的无人机数量非空，则 ri,i+1≠0，否则，ri,i+1＝0；

(4-2)若ri,i-1≠0且ri,i+1＝0，则采用无人机编队单邻居相位控制制导模型和无人机 编队单邻居速度控制制导模型作为各无人机制导策略；若ri,i-1≠0且ri,i+1≠0，则采用无 人机编队双邻居相位控制制导模型和无人机编队双邻居速度控制制导模型作为各无人 机制导策略；若不满足ri,i-1≠0且ri,i+1＝0或ri,i-1≠0且ri,i+1≠0，则不采用无人机编队跟 踪制导方法。

进一步的，所述无人机的航向信息包括：无人机的位置[xi,yi]^T和无人机航向角ψu； 所述地面目标的航向信息包括：地面目标的位置[xt,yt]^T和地面目标航向角ψt。

进一步的，所述基于测距信号的固定翼无人机编队跟踪制导方法还包括采用双曲正 切函数限制各个制导模型输出，经过双曲正切函数限幅后的定距跟踪地面静止目标的单 无人机制导模型为：

经过双曲正切函数限幅后的无人机编队单邻居相位控制制导模型为：

经过双曲正切函数限幅后的无人机编队单邻居速度控制制导模型为：

经过双曲正切函数限幅后的无人机编队双邻居相位控制制导模型为：

经过双曲正切函数限幅后的无人机编队双邻居速度控制制导模型为：

式中，是无人机的最大偏航角速率，它与无人机的最大横滚角φmax有关，其关 系为：

Δvmax为无人机的最大速率变化允许值，vmax和vstall分别为固定翼无人机的最大速度 和标称速度，且vi-1+Δvmax≤vmax且vi-1-Δvmax≥vstall。

有益效果：与现有技术相比，本发明具有以下优势：

(1)实现了单架无人机standoff跟踪地面静止目标的自动制导。

(2)实现了多无人机standoff跟踪地面静止目标的编队自动制导，并能够使得无 人机在跟踪圆周上均匀分布。

(3)仿真试验表明本发明提出的新型编队跟踪策略可以很好的对地面静止目标进 行跟踪，具有工程应用价值。

附图说明

图1是无人机编队跟踪地面目标几何关系图。

图2是本发明飞控/制导计算机控制信号处理板组成图。

图3是无人机雷达示意图。

图4(a)是无人机编队应用相位制导跟踪静止目标轨迹示意图；图4(b)是无人 机编队应用相位制导跟踪静止目标无人机与目标距离示意图；图4(c)是无人机编队应 用相位制导跟踪静止目标无人机间距示意图。

图5(a)是无人机编队应用速度制导跟踪静止目标轨迹示意图；图5(b) 是无人机编队应用速度制导跟踪静止目标各无人机速度示意图；图5(c)是无 人机编队应用速度制导跟踪静止目标无人机与目标距离示意图；图5(d)是无 人机编队应用速度制导跟踪静止目标无人机间距示意图。

图6是无人机编队跟踪地面三目标轨迹示意图。

图7(a)是六自由度仿真编队中各无人机飞行高度示意图；图7(b)是六自由度 仿真编队中各无人机俯仰角示意图；图7(c)是六自由度仿真编队中各无人机滚转角示 意图；图7(d)是六自由度仿真编队中各无人机间距示意图。

具体实施方式

下面结合附图和实施例对本发明作更进一步的说明。

1、固定翼无人机飞控/制导计算机设计及机载测距雷达

a.本实施例所用的飞控/制导计算机DC/DC变换将27V输入电压转变为+5V输出，提供 数字电路工作；最大输出电流为1A，工作温度-45～+60℃。控制信号处理板包含串口、 PWM(脉冲宽度调制)输入输出和CPU模块。CPU采用MPC565，对输入输出的信息进行处理、 运算、控制等。串行NVRAM(非易失性随机访问存储器),频率为40MHz，容量为2Mb,允 许无限次擦除。低电压保护电路可在掉电时自动保护数据，防止在规定电压范围以外时 写入数据。飞控/制导计算机的引导程序支持程序装载和程序运行2种工作模式。当超级 终端主机DB9接头中的8脚和4脚连接时，即运行程序装载模式，否则就运行程序运行模 式。在运行程序装载模式时,首先将本发明可执行程序通过XMODEM(串口通信中异步文 件传输)协议下载到主板的SRAM(静态随机存取存储器)中，同时保存到主板上的FLASH 中，开始执行用户应用程序。在运行用户程序运行模式时，引导程序把可执行程序从 FLASH(闪存)中读取到主板的SRAM(静态随机存取存储器)中，并开始执行用户程序。 操作步骤：把用于用户程序装载的串口接头插在J1上；编写可执行二进制文件程序；打 开WINDOWS的超级终端，定义超级终端的属性有每秒位数115200，数据位为8，奇偶校验 无且停止位为1；上电后出现菜单MENU,按X键选择XMODEM；在超级终端上不断出现“§” 符号，则主板在请求超级终端发送用户可执行程序；点击超级终端上的菜单：传送->发 送文件。选择使用XMODEM协议，然后点击“浏览”选择程序可执行文件，点击发送；按 R键直接执行程序。本实施例中飞控/制导计算机控制信号处理板组成如图2所示。

b.机载测距雷达使用1000米超声波测距雷达，并带有相应的地面站。该型雷达测量 范围为5-1000m，使用高度在0-18000m之间，可在-10-+60℃环境下使用，防护等级IP54。 有±135°的探测范围，测量精度在±1m，分辨率1m，全机重量≤200g，尺寸：80×40 ×60mm，输入电压5V或9-24VDC，通讯接口使用RS232方式。可选择连续测量，单次测量， 指令控制三种模式使用。无人机搭载4个机载雷达示意如图3所示。

2、无人机建模

无人机飞行控制系统由内回路(稳定回路)和外回路(制导回路)构成，在本发明 中认为内回路已经设计完成，能够很好的响应外回路给出的制导指令。在理想情况下， 执行standoff跟踪任务的无人机应保持固定高度和转弯半径,同时围绕被跟踪对象做圆 周运动，因此通常可以仅考虑固定高度上的二维制导问题。记ρ为无人机与目标的相对 距离，ρ∈[0,∞]。无人机编队跟踪地面目标几何关系如图1所示。

考察由式(1)描述的无人机二维质点运动学模型：

其中，下标i为编队中无人机的编号，ψ表示无人机航向角，v为无人机速度，ω为 角速度控制输入，为航向角变化率，为无人机在二维平面x轴方向的速度分量，为无人机在二维平面y轴方向的速度分量。若以[xt,yt]^T表示目标位置，则有

制导方法的设计是使当t→∞时，使得ρi→ρd，ρd为期望距离。

根据以上几何关系，无人机跟踪地面目标的二维模型可以表示为如下所示的极坐标 形式：

式(3)中，为相对距离变化率，χi为视线角，为视线角变化率，状态变量由 [x,y,ψ]^T变为[ρ,χ,v]^T。而且，当无人机的速度为常量，与χi可互相决定。当时，

或分别表示无人机顺时针与逆时针的运动状态。

3、单架无人机跟踪制导方法设计

当跟踪地面目标时，无人机有顺时针、逆时针两种飞行状态，为便于分析，在本发 明中仅采用顺时针飞行方式，逆时针方式可按同样的方法进行分析处理。

本发明的制导方法应用于静止的地面目标。

当地面目标静止时，制导方法设计为：

其中：制导增益为且ρd为稳定定距跟踪时无人机与目标间距离。vi为 编号为i的无人机的速度。

分析无人机动力学模型(3)在制导方法(4)作用下，无人机跟踪静止目标的闭环 系统稳定性：

首先提出如下李雅普诺夫方程：

其中：Lc为单机时的李雅普诺夫方程，可知Lc≥0，且仅当在时，有Lc＝0。对Lc求导得：

其中：为单机跟踪时李雅普诺夫Lc的导数。

代入式(3)，可得：

再将(4)代入有：

显然，当时，对于任何χi，都有而且当且仅当时，有设 R为实数集，当Lc＝0时，可得ρi＝ρd，且在S域中，仅有点可使Lc＝0。根据拉萨尔不变性原理，是闭环系统的渐近平衡点。

4无人机编队跟踪制导方法设计

令ri,i-1((i±n)∈[1…N]，)表示两相邻无人机i和i-1间的直线距离，则 (ψi,i-1＝ψi-ψi-1)。在平衡状态时，ri,i-1等于期望圆的内接多边形弦长。

4.1无人机编队单邻居相位控制制导方法

对于编队中编号为i的无人机，建立如下相位控制制导方法：

其中，为编队制导增益，ri,i-1编号分别为i与i-1的两架无人机间的距离，N为编 队的无人机数量，ρi与ρi-1分别为编号为i和i-1的无人机与地面目标间的距离。

在制导方法(8)的作用下，若则ri,i-1会逐渐趋近于2ρdsinπ/N。

分析无人机模型(3)在制导方法(8)的作用下，无人机编队跟踪静止目标时系统 的稳定性。

首先，提出李雅普诺夫方程为：

显然，Lψ≥0。当且仅当无人机编队在圆周上均匀分布时，Lψ＝0成立。

对李雅普诺夫方程(9)求导：

代入(8)式，得：

因为当且仅当无人机编队在圆周上均匀分布时，成立。所以， 可得在制导方法(8)作用下，无人机编队跟踪静止目标系统稳定，且相邻无人机间距 会逐渐趋近于2ρdsinπ/N。

4.2无人机编队单邻居速度控制制导方法

对于编队中编号为i的无人机，建立如下速度控制制导方法：

其中kv为编队制导增益，ri,i-1编号分别为i与i-1的两架无人机间的距离，N为编队 的无人机数量，ρi与ρi-1分别为编号为i和i-1的无人机与地面目标间的距离，vi与vi-1分 别为编号为i和i-1的无人机的速度。

在制导方法(11)的作用下，若kv＞0，则ri,i-1会逐渐趋近于2ρdsinπ/N。

在无人机跟踪目标做圆周运动时，无人机的速度与航向角速率有如下关系：

代入制导方法(8)，则有：

令根据4.1中结论，无人机编队跟踪静止目标系统稳定，且相邻无人机 间距会逐渐趋近于2ρdsinπ/N。

4.3无人机编队双邻居相位控制制导方法

对于编队中编号为i的无人机，建立如下相位控制制导方法：

其中为编队制导增益，ri,i-1与ri,i+1分别表示编号为i的无人机与其两架相邻无人机 之间的距离，在稳定状态时，ri,i-1等于ri,i+1。

在制导方法(14)的作用下，若无人机会逐渐稳定在其相邻两架无人机之 间的位置。

分析无人机模型(3)在制导方法(14)的作用下，无人机编队跟踪静止目标时系 统的稳定性。

首先，提出李雅普诺夫方程为：

显然，Lψ≥0。当且仅当无人机编队在圆周上均匀分布时，Lψ＝0成立。

对李雅普诺夫方程(15)求导：

代入(14)式，得：

因为当且仅当无人机编队在圆周上均匀分布时，成立。所以， 可得在制导方法(14)作用下，无人机编队跟踪静止目标系统稳定，且无人机会逐渐稳 定在其相邻两架无人机之间的位置。

4.4无人机编队双邻居速度控制制导方法

对于编队中编号为i的无人机，建立如下速度控制制导方法：

其中kv为编队制导增益，ri,i-1编号分别为i与i-1的两架无人机间的距离，vi与vi-1分 别为编号为i和i-1的无人机的速度。

在制导方法(17)的作用下，若kv＞0，无人机会逐渐稳定在其相邻两架无人机之间的 位置。

根据式(18)和制导方法(14)，可得：

令根据4.3中结论，无人机编队跟踪静止目标系统稳定，且无人机会逐 渐稳定在其相邻两架无人机之间的位置。

5无人机编队跟踪制导方法选择策略设计

5.1制导方法策略选择

每架无人机都装有前后左右四个雷达，如图3所示。假定雷达探测距离满足rsensor＞2ρd并且探测角度则在四个雷达作用下，无人机机身360°的范围都可被探测。

定义：其中和分别表示四个 距离探测雷达所能探测到的无人机数量。所以，Ni表示编号为i的无人机所能探测到的 无人机数量总和。制导方法选择策略为：

对于编号为i(i∈[1,...N])的无人机，若则否则，若则若都不满足，则ri,i-1:＝0。若则否则， 若则若都不满足，则ri,i+1:＝0。综上，若ri,i-1≠0且ri,i+1＝0，使用4.2 或4.3的制导方法；若ri,i-1≠0且ri,i+1≠0，使用4.4或4.5的制导方法。若以上两种情况都 不满足，则无法使用编队控制制导方法。

5.2制导方法输出限幅

由于需要限制制导输出的最大值，的取值需要满足同时，kv的取值必须 注意无人机的最大速度和标称速度。本实施例中，采用双曲正切函数限制制导输出。由 于双曲正切函数的单调有界性，输出使用双曲正切函数限幅后，不会影响系统的稳定性。 制导方法(4)、(8)、(11)、(14)和(17)在限幅后可写为如下式(19)-(23)：

其中vi-1+Δvmax≤vmax且vi-1-Δvmax＞vstall。

是无人机的最大偏航角速率，它与无人机的最大横滚角有关，其关系为：

其中Δvmax为无人机的最大速率变化允许值，vmax和vstall分别为固定翼无人机的最大速 度和标称速度。

5功能与性能验证

为了验证本发明提出的基于测距雷达无人机编队跟踪地面静止目标架构和制导方 法设计的合理性与有效性，以4架无人机编队为例，针对静止目标的跟踪问题进行仿真 验证。

5.1Dubins模型仿真

在仿真开始时，无人机参数设定为：

巡航速度：40m/s。最大速度：46m/s。标称速度：34m/s。跟踪半径：400m。最 大横滚角：30°。最大偏航角速率：0.1(rad/s)。雷达探测距离：900m。雷达探测范 围：100°。

无人机与目标初始状态设定为：

a.目标

●位置:(1000,1000)

●航向角:无

b.无人机(#1)

●位置:(0,2000)

●航向角:20°

c.无人机(#2)

●位置:(2000,2000)

●航向角:50°

d.无人机(#3)

●位置:(0,0)

●航向角:100°

e.无人机(#4)

●位置:(2000,0)

●航向角:100°

(1)相位控制

仿真参数设置为：无人机及目标轨迹如图4(a)所示。无人 机与目标间的距离如图4(b)所示。无人机间间距如图4(c)所示。

(2)速度控制

设定如上，kv＝0.24。无人机及目标轨迹如图5(a)所示。无人机速度如图5(b) 所示。无人机与目标间的距离如图5(c)所示。无人机间间距如图5(d)所示。

(3)仿真性能分析

由图4和图5可看出，本发明中的制导方法可以对地面静止目标进行稳定跟踪。目 标与无人机间距离，无人机间间距都有很好的收敛特征。单邻居与双邻居制导方法的切 换时间为135s，225s，132s和330s。其中，在速度控制下有更好的跟踪轨迹，在相位 控制下，无人机速度可以保持恒定。

5.2六自由度实时仿真

相较于Dubins仿真，在六自由度非线性仿真中可以观察到无人机的高度与姿态变 化。在本节，采用了样例无人机在数字实时仿真系统中进行仿真，飞行数据使用真实飞 行测试数据。仿真结果有高可靠性。

样例无人机的参数为：机体重量：21kg。翼展：3.8m。巡航速度：40m/s。机翼面 积：1.75m²。机翼平均弦长0.56m。攻角：1.55°。

数字实时仿真系统由样例无人机模型，遥测软件，执行机构和2D/3D展示软件构成。

无人机编队对地面三个静止目标进行跟踪，为使仿真结果接近真实飞行，传感器噪 声设置为：距离传感器信号距离变化率传感器信号仿真时间 持续3000s，2D轨迹如图6所示。

无人机编队高度，俯仰角，滚转角和无人机间距离分别如图7(a)、图7(b)、图7 (c)、图7(d)所示。

图6表明无人机非线性六自由度模型可以在考虑传感器噪声的情况下成功进行对目 标的跟踪任务。此外，从图7可看出，制导输出对飞机的高度和纵向姿态的影响在可接 受范围内。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员 来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也 应视为本发明的保护范围。