集群飞行器可完成许多单个飞行器难以解决的任务,而高精度的位置信息是集群飞行器完成各类飞行任务的前提[1-2]。在飞行器的定位中,INS/GPS(Inertial Navigation System/Global Positioning System)组合导航系统得到广泛研究与应用,但是惯性导航误差会随着时间而发散,GPS导航会受到恶劣环境、人为干扰等影响出现大的误差[3]。因此,国内外学者对集群智能体协同定位做了大量研究,包括多AUV(Autonomous Underwater Vehicles)协同定位[4-5]、多移动机器人协同定位[6-7]以及飞行器协同定位[8-10]等方面。在仅利用集群飞行器的INS/GPS导航定位信息的基础上,文献[11]提出一种基于总体最小二乘-卡尔曼滤波算法,很大程度消除了系统误差影响,算法有较好的鲁棒性;文献[12]提出了一种基于联邦滤波的协同定位算法,在长机故障后,很好地抑制了集群编队定位误差快速发散的问题。
在集群飞行器自身导航的基础上,增加集群飞行器之间相互测距和测向的协同定位方法得到了重视。文献[13]提出了一种基于机间相互测距的机群组网协同定位方法,有效地减小了随时间而发散的集群飞行器惯导定位误差;文献[14]提出一种分层式协同导航系统方案,利用僚机与上一层无人机之间的相对测距测向和测速等信息建立协同定位系统模型,通过卡尔曼滤波估计僚机的定位误差,以提高集群飞行器协同定位精度;文献[15]利用飞行器间相对距离、相对角度的测量以及集群飞行器之间的几何特性,通过卡尔曼滤波估计出各个飞行器的位置信息。
而在某些特殊飞行任务中,如集群飞行器对非合作目标进行协同制导与打击时,还需要对非合作目标进行精确定位。文献[16]利用双机对非合作目标进行完整的相对距离与相对角度测量,然后采用非线性迭代获得非合作目标位置。文献[17]提出了不完整测量下的多站无源跟踪算法,利用对目标的纯角度测量,采用最小二乘估计和IMM(Interacting Multiple Model)算法求解得到机动目标的位置信息。
受集群飞行器平台设备的限制,对非合作目标往往只能高精度测向,而不能高精度测距。针对这种情况,本文给出一种利用集群飞行器对非合作目标测向、增加飞行器间的相互测距来实现协同定位的方法,来提高集群飞行器自身和对非合作目标的定位精度。
1 不完整测量与协同定位原理本节给出集群飞行器间相互测距和对非合作目标测向的不完整测量模型,并在简化条件下利用最小二乘估计原理,说明应用这种不完整测量信息可以提高集群飞行器的定位精度,以及对非合作目标的定位精度。
1.1 不完整测量模型如图 1所示,在地面一点建立惯性坐标系,在三维环境中的n个飞行器,分别用Agent 1、Agent 2、…Agent n表示,非合作目标用Target表示,设各个Agent的位置坐标是Xi(xi, yi, zi),非合作目标Target位置坐标为Xt(xt, yt, zt)。假设每个Agent都能正确地感知地面坐标系的3个方向,其自身携带的具有测角功能的传感器测得非合作目标Target相对自身的方位角分别为φ1,φ2,…,φn,俯仰角分别为θ1,θ2,…,θn。并且飞行器可以通过测距传感器测得相互之间的距离分别为d12,d13,…,d(n-1)n。
建立n个Agent对于Target测角的几何模型以及各个Agent之间测距的几何模型,可以表示为
$ \left\{ {\begin{array}{*{20}{l}} {{\varphi _i} = {\rm{arctan}}\frac{{{y_t} - {y_i}}}{{{x_t} - {x_i}}}}\\ {{\theta _i} = {\rm{arctan}}\frac{{{z_t} - {z_i}}}{{\sqrt {{{({x_t} - {x_i})}^2} + {{({y_t} - {y_i})}^2}} }}}\\ {{d_{ij}} = \sqrt {{{({x_i} - {x_j})}^2} + {{({y_i} - {y_j})}^2} + {{({z_i} - {z_j})}^2}} }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i,j = 1,2, \cdots ,n;j \ne i} \end{array}} \right. $ | (1) |
式中:φi为方位角; θi为俯仰角; dij为各飞行器间的距离。
定义方位角、俯仰角、机间距离的观测量Z=[φi θi dij]T,由于传感器测量噪声的存在,则有测量模型:
$ \left[ {\begin{array}{*{20}{l}} {{{\hat \varphi }_i}}\\ {{{\hat \theta }_i}}\\ {{{\hat d}_{ij}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} {{\varphi _i}}\\ {{\theta _i}}\\ {{d_{ij}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{l}} {{v_{{\varphi _i}}}}\\ {{v_{{\theta _i}}}}\\ {{v_{{d_{ij}}}}} \end{array}} \right]\;\:i,j = 1,2, \cdots ,n;j \ne i $ | (2) |
式中:
基于集群飞行器间相互测距以及集群对非合作目标测向信息,在测距拓扑结构图满足刚性条件时,可以基于这些相对测量确定集群飞行器与目标的相对位置。考虑相对测量精度远高于集群飞行器自身绝对定位精度的情况,基于不完整相对测量获得的相对定位精度也远高于集群飞行器的绝对定位精度。这里简化地假设集群各飞行器相对目标的相对定位没有误差,来分析引入相对定位能提升集群飞行器与目标绝对定位精度的原理。
由于集群飞行器自身定位误差的存在,各个飞行器的导航位置为
$ \left\{ {\begin{array}{*{20}{l}} {{{\tilde x}_i} = {x_i} + {v_{{x_i}}}}\\ {{{\tilde y}_i} = {y_i} + {v_{{y_i}}}}\\ {{{\tilde z}_i} = {z_i} + {v_{{z_i}}}} \end{array}\;\:i = 1,2, \cdots ,n} \right. $ | (3) |
式中:vxi、vyi、vzi分别为各个方向上的观测噪声,它们是均值为零且互不相关的高斯白噪声,设各飞行器位置测量精度相同,均方差大小分别为σx2、σy2、σz2。
各飞行器相对目标的位置已知,分别为Δxi、Δyi、Δzi,则有
$ \left\{ {\begin{array}{*{20}{l}} {{x_i} = {x_{\rm{t}}} + \Delta {x_i}}\\ {{y_i} = {y_{\rm{t}}} + \Delta {y_i}\;\:i = 1,2, \cdots ,n}\\ {{z_i} = {z_{\rm{t}}} + \Delta {z_i}} \end{array}} \right. $ | (4) |
设x、y、z方向的测量相互独立,则可仅考虑x方向的协同定位,测量模型可表示为
$ {\tilde x_i} - \Delta {x_i} = {x_{\rm{t}}} + {v_x}\;\:i = 1,2, \cdots ,n $ | (5) |
以等式左端为测量值,利用n次测量,根据最小二乘估计原理,得到目标绝对位置的估计为
$ {\hat x_t} = \frac{1}{n}\sum\limits_{i = 1}^n {({{\tilde x}_i} - \Delta {x_i})} $ | (6) |
且该最小二乘估计的方差为
$ \begin{array}{l} {\rm{Var}}[{{\hat x}_{\rm{t}}}] = \frac{{ {\rm{Var}} [({{\tilde x}_i} - \Delta {x_i})]}}{n} = \frac{{\sigma _x^2}}{n}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i = 1,2, \cdots ,n \end{array} $ | (7) |
然后得到各飞行器x坐标估计及方差:
$ \left\{ {\begin{array}{*{20}{l}} {{{\hat x}_i} = {{\hat x}_{\rm{t}}} + \Delta {x_i}}\\ { {\rm{Var}} [{{\hat x}_i}] = {\rm{Var}} [{{\hat x}_{\rm{t}}}] = \frac{{\sigma _x^2}}{n}\;\:i = 1,2, \cdots ,n} \end{array}} \right. $ | (8) |
可见,目标和集群飞行器的绝对定位精度得到提高,协同测量的飞行器数目n越大,协同定位的精度越高。
2 不完整测量下的协同定位算法文中介绍2种协同定位求解方法:一种为非线性优化求解方法,另一种则将系统线性化后再用最小二乘方法进行求解。
2.1 非线性优化求解1) 非线性优化模型
根据残差平方和最小原理[18-19],构造计算式(1)与式(2)的测量值之间误差一同达到最小的函数。但由于距离和角度量纲不同,先做归一化处理再计算,这样目标函数可以写成
$ \left\{ \begin{align} & {{f}_{d}}=\sum\limits_{i=1}^{n-1}{\sum\limits_{j=i+1}^{n}{\frac{1}{\sigma _{{{d}_{ij}}}^{2}}}}{{[\sqrt{{{({{x}_{i}}-{{x}_{j}})}^{2}}+{{({{y}_{i}}-{{y}_{j}})}^{2}}+{{({{z}_{i}}-{{z}_{j}})}^{2}}}-{{{\hat{d}}}_{ij}}]}^{2}} \\ & {{f}_{\varphi }}=\sum\limits_{i=1}^{n}{\frac{1}{\sigma _{{{\varphi }_{i}}}^{2}}}{{\left( \text{arctan}\frac{{{y}_{\text{t}}}-{{y}_{i}}}{{{x}_{\text{t}}}-{{x}_{i}}}-{{{\hat{\varphi }}}_{i}} \right)}^{2}} \\ & {{f}_{\theta }}=\sum\limits_{i=1}^{n}{\frac{1}{\sigma _{{{\theta }_{i}}}^{2}}}{{\left( \text{arctan}\frac{{{z}_{\text{t}}}-{{z}_{i}}}{\sqrt{{{({{x}_{\text{t}}}-{{x}_{i}})}^{2}}+{{({{y}_{\text{t}}}-{{y}_{i}})}^{2}}}}-{{{\hat{\theta }}}_{i}} \right)}^{2}} \\ & \underset{\begin{smallmatrix} {{x}_{i}},{{y}_{i}},{{z}_{i}},{{x}_{\text{t}}},{{y}_{\text{t}}},{{z}_{\text{t}}} \\ i=1,2,\cdots ,n \end{smallmatrix}}{\mathop{\text{min}}}\,f={{f}_{d}}+{{f}_{\varphi }}+{{f}_{\theta }} \\ \end{align} \right. $ | (9) |
那么求解最优位置问题转化为求解目标函数的最小值Fmin=minf。
2) 初值的选取
目标函数的初值包括多飞行器的位置初值以及非合作目标的位置初值。多飞行器位置初值可以由自身的低精度导航定位给出,初值为Xn0(x10, y10, z10, …, xn0, yn0, zn0)。下面求取非合作目标的位置初值。
在仅利用测向信息的条件下,选取n个飞行器,根据式(1),由第i个飞行器方位角φi和第j个飞行器方位角φj的计算公式可以得到
$ \left\{ \begin{array}{*{35}{l}} {{y}_{ij\_\text{t0}}}=\frac{-{{y}_{i0}}\text{tan}{{\varphi }_{j}}+{{y}_{j0}}\text{tan}{{\varphi }_{j}}+\text{tan}{{\varphi }_{i}}\text{tan}{{\varphi }_{j}}{{x}_{i0}}-\text{tan}{{\varphi }_{i}}\text{tan}{{\varphi }_{j}}{{x}_{j0}}}{\text{tan}{{\varphi }_{i}}-\text{tan}{{\varphi }_{j}}} \\ {{x}_{ij\_\text{t0}}}=\frac{-{{y}_{i0}}+{{y}_{j0}}+{{x}_{i0}}\text{tan}{{\varphi }_{i}}-{{x}_{j0}}\text{tan}{{\varphi }_{j}}}{\text{tan}{{\varphi }_{i}}-\text{tan}{{\varphi }_{j}}} \\ \end{array} \right. $ | (10) |
将xij_t0、yij_t0代入俯仰角θi、θj的计算公式便可以计算出非合作目标zij_t0的值:
$ \begin{array}{*{20}{l}} {{z_{i{j_ - }{\rm{t0}}}} = \frac{1}{2}[{z_i} - {\rm{tan}}{\theta _i}\sqrt {{{({x_{{\rm{t0}}}} - {x_i})}^2} + {{({y_{{\rm{t0}}}} - {y_i})}^2}} + }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {z_j} - {\rm{tan}}{\theta _j}\sqrt {{{({x_{{\rm{t0}}}} - {x_j})}^2} + {{({y_{{\rm{t0}}}} - {y_j})}^2}} ]} \end{array} $ | (11) |
每2个飞行器可以计算出一个非合作目标的位置坐标值,对n个飞行器进行两两计算再取加权平均,这样可以在仅测向时计算得到非合作目标低精度的位置坐标值Xt0(xt0, yt0, zt0)。
将多飞行器的自身导航低精度位置值和非合作目标仅利用测向信息计算得到的位置坐标值,作为已构造的目标函数的初值X0(x10, y10, z10, …, xn0, yn0, zn0, xt0, yt0, zt0),进行迭代计算,当目标函数Fmin=minf最小时,得到各个飞行器与非合作目标更精确的位置解X(x1, y1, z1, …, xn, yn, zn, xt, yt, zt)。
2.2 线性化求解在式(2)中3个观测量的基础上,引入n个Agent分别在惯性坐标系下Ox、Oy、Oz3个方向的自身导航位置观测值的均值,分别定义为x、y、z,这样便有:
$ \left[ {\begin{array}{*{20}{l}} {\bar x}\\ {\bar y}\\ {\bar z} \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} {\bar x}\\ {\bar y}\\ {\bar z} \end{array}} \right] + \left[ {\begin{array}{*{20}{l}} {{v_{\bar x}}}\\ {{v_{\bar y}}}\\ {{v_{\bar z}}} \end{array}} \right] $ | (12) |
式中:
$ \left\{ {\begin{array}{*{20}{l}} {\bar x = \frac{1}{n}({x_{10}} + {x_{20}} + \cdots + {x_{n0}})}\\ {\bar y = \frac{1}{n}({y_{10}} + {y_{20}} + \cdots + {y_{n0}})}\\ {\bar z = \frac{1}{n}({z_{10}} + {z_{20}} + \cdots + {z_{n0}})} \end{array}} \right. $ | (13) |
式中:vx、vy、vz分别为3个方向上位置均值观测噪声, 设它们为均值等于零且互不相关的高斯白噪声,均方差大小分别为
根据式(2)和式(12),可以定义线性化求解过程的观测量Z=[φi θi dij x y z]T,则有
$ \left[ {\begin{array}{*{20}{c}} {{{\hat \varphi }_i}}\\ {\hat \theta }\\ {{{\hat d}_{ij}}}\\ {\bar {\hat x}}\\ {\bar {\hat y}}\\ {\bar {\hat z}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\varphi _i}}\\ {{\theta _i}}\\ {{d_{ij}}}\\ {\bar x}\\ {\bar y}\\ {\bar z} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{v_{{\varphi _i}}}}\\ {{v_{{\theta _i}}}}\\ {{v_{{d_{ij}}}}}\\ {{v_{\bar x}}}\\ {{v_{\bar y}}}\\ {{v_{\bar z}}} \end{array}} \right]\;\:i,j = 1,2, \cdots ,n;j \ne i $ | (14) |
式中:
同非线性优化求解选取初值方法一样,在X0(x10, y10, z10, …, xn0, yn0, zn0, xt0, yt0, zt0)处对式(1)的方程泰勒展开,先进行线性化理。
将方位角方程线性化:
$ \begin{array}{l} {\varphi _i} = {a_{1i}}{x_i} + {a_{2i}}{y_i} + {a_{3i}}{x_t} + {a_{4i}}{y_t} + {a_{5i}}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i = 1,2, \cdots ,n \end{array} $ | (15) |
式中:
$ \left\{ {\begin{array}{*{20}{l}} {{a_{1i}} = \frac{1}{k}({y_{{\rm{t0}}}} - {y_{i0}})}\\ {{a_{2i}} = - \frac{1}{k}({x_{{\rm{t0}}}} - {x_{i0}})}\\ {{a_{3i}} = - \frac{1}{k}({y_{{\rm{t0}}}} - {y_{i0}})}\\ {{a_{4i}} = \frac{1}{k}({x_{{\rm{t0}}}} - {x_{i0}})}\\ {{a_{5i}} = {\rm{arctan}}\frac{{{x_{{\rm{t0}}}} - {x_{i0}}}}{{{y_{{\rm{t0}}}} - {y_{i0}}}}}\\ {k = {{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2}} \end{array}} \right. $ | (16) |
将俯仰角方程线性化:
$ \begin{array}{l} {\theta _i} = {b_{1i}}{x_i} + {b_{2i}}{y_i} + {b_{3i}}{z_i} + {b_{4i}}{x_t} + {b_{5i}}{y_t} + {b_{6i}}{z_t} + {b_{7i}}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i = 1,2, \cdots ,n \end{array} $ | (17) |
式中:
$ \left\{ \begin{array}{l} \begin{array}{*{20}{l}} {{b_{1i}} = \frac{1}{g}({x_{{\rm{t0}}}} - {x_{i0}})({z_{{\rm{t0}}}} - {z_{i0}})}\\ {{b_{2i}} = \frac{1}{g}({y_{{\rm{t0}}}} - {y_{i0}})({z_{{\rm{t0}}}} - {z_{i0}})} \end{array}\\ \begin{array}{*{20}{l}} {{b_{3i}} = - \frac{1}{g}{{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2}}\\ {{b_{4i}} = - \frac{1}{g}({x_{{\rm{t0}}}} - {x_{i0}})({z_{{\rm{t0}}}} - {z_{i0}})} \end{array}\\ \begin{array}{*{20}{l}} {{b_{5i}} = - \frac{1}{g}({y_{{\rm{t0}}}} - {y_{i0}})({z_{{\rm{t0}}}} - {z_{i0}})}\\ {{b_{6i}} = \frac{1}{g}{{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2}} \end{array}\\ \begin{array}{*{20}{l}} {{b_{7i}} = {\rm{arctan}}\frac{{{z_{{\rm{t0}}}} - {z_{i0}}}}{{\sqrt {{{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2}} }}}\\ {g = [{{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2} + }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {{({z_{{\rm{t0}}}} - {z_{i0}})}^2}] \times \sqrt {{{({x_{{\rm{t0}}}} - {x_{i0}})}^2} + {{({y_{{\rm{t0}}}} - {y_{i0}})}^2}} } \end{array} \end{array} \right. $ | (18) |
将机间测距方程线性化:
$ \begin{array}{l} {d_{ij}} = {c_{1i}}{x_i} + {c_{2i}}{y_i} + {c_{3i}}{z_i} + {c_{4i}}{x_j} + {c_{5i}}{y_j} + {c_{6i}}{z_j}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} i,j = 1,2, \cdots ,n;j \ne i \end{array} $ | (19) |
式中:
$ \left\{ \begin{array}{l} \begin{array}{*{20}{l}} {{c_{1i}} = \frac{1}{l}({x_{i0}} - {x_{j0}})}\\ {{c_{2i}} = \frac{1}{l}({y_{i0}} - {y_{j0}})} \end{array}\\ \begin{array}{*{20}{l}} {{c_{3i}} = \frac{1}{l}({z_{i0}} - {z_{j0}})}\\ {{c_{4i}} = - \frac{1}{l}({x_{i0}} - {x_{j0}})} \end{array}\\ \begin{array}{*{20}{l}} {{c_{5i}} = - \frac{1}{l}({y_{i0}} - {y_{j0}})}\\ {{c_{6i}} = - \frac{1}{l}({z_{i0}} - {z_{j0}})} \end{array}\\ l = \sqrt {{{({x_{i0}} - {x_{j0}})}^2} + {{({y_{i0}} - {y_{j0}})}^2} + {{({z_{i0}} - {z_{j0}})}^2}} \end{array} \right. $ | (20) |
可以得到多飞行器与非合作目标的位置坐标X(x1, y1, z1, …, xn, yn, zn, xt, yt, zt)到观测量
$ \mathit{\boldsymbol{\hat Z}} = \mathit{\boldsymbol{AX}} + \mathit{\boldsymbol{B}} $ | (21) |
式中:
$ \mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {{a_{11}}}&{{a_{12}}}&0&0& \cdots &0&0&0&{{a_{31}}}&{{a_{41}}}&0\\ {}&{}&{}&{}& \vdots &{}&{}&{}&{}&{}&{}\\ \cdots &{{a_{1i}}}&{{a_{2i}}}&0& \cdots &0&0&0&{{a_{3i}}}&{{a_{4i}}}&0\\ \cdots &{{b_{1i}}}&{{b_{2i}}}&{{b_{3i}}}& \cdots &0&0&0&{{b_{4i}}}&{{b_{5i}}}&{{b_{6i}}}\\ \cdots &{{c_{1ij}}}&{{c_{2ij}}}&{{c_{3ij}}}& \cdots &{{c_{4ij}}}&{{c_{5ij}}}&{{c_{6ij}}}& \cdots &0&0\\ {}&{}&{}&{}& \vdots &{}&{}&{}&{}&{}&{}\\ \cdots &0&{{C_{1(n - 1)n}}}&{{C_{2(n - 1)n}}}&{{C_{3(n - 1)n}}}&{{C_{4(n - 1)n}}}&{{C_{5(n - 1)n}}}&{{C_{6(n - 1)n}}}&0&0&0\\ {1/n}&0&0&{1/n}& \cdots &{1/n}&0&0&0&0&0\\ 0&{1/n}&0&0&{1/n}& \cdots &{1/n}&0&0&0&0\\ 0&0&{1/n}&0&0&{1/n}& \cdots &{1/n}&0&0&0 \end{array}} \right] $ | (22) |
$ \mathit{\boldsymbol{B}} = \left[ {\begin{array}{*{20}{l}} {{a_{51}}}&{{b_{71}}}&0& \cdots &0&{{a_{5n}}}&{{b_{7n}}}&0& \cdots &0 \end{array}} \right] $ | (23) |
A矩阵中存放式(16)、式(18)、式(20)中泰勒展开后X(x1, y1, z1, …xn, yn, zn, xt, yt, zt)各项的系数,B矩阵中存放俯仰角和方位角的泰勒展开后的常数项,即式(16)和式(18)中的常数项。根据最小二乘法由式(21)可以得到
$ \mathit{\boldsymbol{X}} = {(\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{A}}^{\rm{T}}})^{ - 1}}{\mathit{\boldsymbol{A}}^{\rm{T}}}(\mathit{\boldsymbol{\hat Z}} - \mathit{\boldsymbol{B}}) $ | (24) |
以上便可以通过求解得到各个飞行器以及非合作目标精度更高的位置坐标解为X(x1, y1, z1, …xn, yn, zn, xt, yt, zt)。
3 协同定位仿真结果分析 3.1 精度评估方法采用蒙特卡洛方法进行多次仿真,定位精度根据其解算值同真值的误差的均方根误差(Root Mean Square Error, RMSE)来衡量,xyz三轴的定位精度便可以表示为[20]
$ \left\{ {\begin{array}{*{20}{l}} { RMSE{ _x} = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {\frac{1}{m}} \sum\limits_{t = 1}^m ( {x_{it}} - {x_{it,{\rm{ true}}}}{\rm{ }}} {)^2}}\\ { RMSE{ _y} = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {\frac{1}{m}} \sum\limits_{t = 1}^m {{{({y_{it}} - {y_{it,{\rm{ true}}}})}^2}} } }\\ { RMSE{ _z} = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {\frac{1}{m}} \sum\limits_{t = 1}^t ( {z_{it}} - {z_{it,{\rm{ true}}}}} {)^2}}\\ {i,j = 1,2, \cdots ,n;j \ne i} \end{array}} \right. $ | (25) |
式中:RMSEx、RMSEy、RMSEz为解算值和真值误差的均方根误差;(xit, yit, zit)和(xit,true, yit,true, zit,true)分别为t时刻第i次仿真的解算值和真实值。
3.2 仿真测试初始条件选取3个Agent进行仿真,这些Agent和Target的初始位置以及速度如表 1所示,集群飞行器组以(-200 m/s,-200 m/s,-50 m/s)的速度对Target进行追击,而被追击的Target以(-199 m/s,-199 m/s,-49 m/s)进行逃逸。3个Agent的自身导航精度、测角传感器精度和Agent间测距的传感器探测精度如表 2所示。
飞行器 | 北向/m | 东向/m | 高度/m | 北向速度/(m·s-1) | 东向速度/(m·s-1) | 天向速度/(m·s-1) |
Agent 1 | 1 150 | 1 150 | 1 150 | -200 | -200 | -50 |
Agent 2 | 1 300 | 1 200 | 1 200 | -200 | -200 | -50 |
Agent 3 | 1 300 | 1 300 | 1 150 | -200 | -200 | -50 |
Target | 1 000 | 1 000 | 1 000 | -199 | -199 | -49 |
仿真实验时,设置仿真时间为100 s,采样时间为0.1 s,每个时刻仿真20次, 采用蒙特卡洛的方法统计均方根误差。经过仿真实验,由非线性静态优化求解方法得到的均方根误差结果如图 2所示。
采用蒙特卡洛方法可以统计得到各个方向上的定位精度,均方根误差统计如表 3所示。
飞行器 | 均方根误差/m | ||
x轴方向 | y轴方向 | z轴方向 | |
Agent 1 | 4.186 9 | 4.400 6 | 4.226 8 |
Agent 2 | 4.335 3 | 4.400 8 | 4.216 5 |
Agent 3 | 4.401 0 | 4.412 2 | 4.242 5 |
Target | 4.340 0 | 4.246 6 | 4.412 2 |
在相同的条件下,线性化后再用最小二乘方法求解得到的均方根误差结果与非线性静态优化求解方法得到的均方根误差结果类似,即与图 2类似,但均方根误差大小不同,线性化后求解的均方根误差统计如表 4所示。
飞行器 | 均方根误差/m | ||
x轴方向 | y轴方向 | z轴方向 | |
Agent 1 | 5.524 0 | 5.690 6 | 5.676 9 |
Agent 2 | 5.521 1 | 5.721 1 | 5.679 3 |
Agent 3 | 5.507 5 | 5.704 6 | 5.663 3 |
Target | 5.501 7 | 5.663 7 | 5.704 6 |
相同的条件下,在仅测向时求解得到Target定位均方差结果如图 3所示,此种方法仅可以求解非合作目标的位置,不能提高各个Agent自身的定位精度,各个Agent的定位精度仍为10 m。
统计各个方向上的定位精度,在仅测向时求解的非合作目标均方根误差统计如表 5所示。
由于仿真实验给定的多飞行器自身导航误差精度为10 m,因而以上仿真结果表明:
1) 仅测角求取非合作目标位置时,直接采用多飞行器低精度位置值进行计算,加上测角精度的影响,非合作目标定位误差大约在17~18 m,并且不能提高飞行器自身定位。
2) 在测向的同时加入飞行器间测距的信息后,采用非线性静态优化求解方法提高精度效果最好,能够降低飞行器自身的定位误差到4.2~4.4 m,并且对于非合作目标的定位精度大概为4.2~4.4 m。
3) 线性化求解方法效果略差于非线性化直接求解,但远高于仅测向时的定位精度,可降低多飞行器的定位误差精度到5.5~5.7 m,同时对于非合作目标的定位精度大概为5.5~5.7 m。但线性优化求解方法在运算速度上更优,计算量小,可以减少飞行器组的载荷压力。
4 结论1) 本文研究了集群飞行器在仅能对非合作目标测角的情况下,增加飞行器间测距来进行协同定位的融合处理方法。采用非线性静态优化估计和线性化后最小二乘估计2种求解方法,通过仿真表明,该方法可以提高集群飞行器和非合作目标的定位精度。
2) 该方法可用于在编队飞行的多无人机和利用相对测量提高无人机定位精度;也可用于导弹对非合作目标协同攻击中,在导引头仅能对目标测向时,提高对目标的定位精度。
[1] |
马宗锋, 辛明瑞, 申景诗, 等. 分布式集群空间飞行器综述[J]. 航天器工程, 2013, 22(1): 101-105. MA Z F, XIN M R, SHEN J S, et al. Survey on fractionated spacecraft cluster[J]. Spacecraft Engineering, 2013, 22(1): 101-105. (in Chinese) |
Cited By in Cnki (12) | Click to display the text | |
[2] | ZOU Y, PAGILLA P R, RATLIFF R T. Distributed formation flight control using constraint forces[J]. Journal of Guidance, Control, & Dynamics, 2009, 32(1): 112-120. |
Click to display the text | |
[3] | ALI J, MIRZA M R U B. Performance comparison among some nonlinear filters for[J]. Nonlinear Dynamics, 2010, 61(3): 491-502. |
Click to display the text | |
[4] |
徐博, 白金磊, 郝燕玲, 等. 多AUV协同导航问题的研究现状与进展[J]. 自动化学报, 2015, 41(3): 445-461. XU B, BAI J L, HAO Y L, et al. The research status and progress of cooperative navigation for multiple AUVs[J]. Acta Automatica Sinica, 2015, 41(3): 445-461. (in Chinese) |
Cited By in Cnki (11) | Click to display the text | |
[5] | FALLON M F, PAPADOPOULOS G, LEONARD J J. A measurement distribution framework for cooperative navigation using multiple AUVs[C]//2010 IEEE International Conference on Robotics and Automation (ICRA), Piscataway: IEEE Press, 2010. |
[6] | ROUMELIOTIS S I, BEKEY G A. Distributed multirobot localization[J]. IEEE Transactions on Robotics & Automation, 2002, 18(5): 781-795. |
[7] | ROUMELIOTIS S I, REKLEITIS I M. Propagation of uncertainty in cooperative multirobot localization:Analysis and experimental results[J]. Autonomous Robots, 2004, 17(1): 41-54. |
Click to display the text | |
[8] |
刘俊成, 张京娟, 谭丽芬. 新的导弹协同定位技术[J]. 北京航空航天大学学报, 2012, 38(9): 1149-1153. LIU J C, ZHANG J J, TAN L F. New technique for multi-missile cooperative localization[J]. Journal of Beijing University of Aeronautics and Astronautics, 2012, 38(9): 1149-1153. (in Chinese) |
Cited By in Cnki (6) | Click to display the text | |
[9] |
杜君南, 王融, 熊智, 等.基于相对距离差模型的集群飞行器协同导航方法研究[C]//第十届中国卫星导航年会, 2019: 88-92. DU J N, WANG R, XIONG Z, et al. Research on collaborative navigation method of cluster aircraft based on relative distance difference model[C]//The 10th China Satellite Navigation Conference, 2019: 88-92(in Chinese). |
[10] |
许晓伟, 赖际舟, 吕品, 等. 多无人机协同导航技术研究现状及进展[J]. 导航定位与授时, 2017, 4(4): 1-9. XU X W, LAI J Z, LYU P, et al. A literature review on the research status and progress of cooperation navigation technology for multiple UAVs[J]. Navigation Positioning & Timing, 2017, 4(4): 1-9. (in Chinese) |
Cited By in Cnki (18) | Click to display the text | |
[11] |
闫俊. 基于空战的多机协同定位算法研究[J]. 计算机仿真, 2014, 31(8): 27-31. YAN J. A multi-airplane cooperative detection method for air combat[J]. Computer Simulation, 2014, 31(8): 27-31. (in Chinese) |
Cited By in Cnki (2) | Click to display the text | |
[12] |
刘晓洋, 徐胜红. 一种长机故障情况下机群编队协同定位算法[J]. 兵器装备工程学报, 2019, 40(2): 136-140. LIU X Y, XU S H. Flight formation collaborative localization algorithm under the condition of leader fault[J]. Journal of Ordnance Equipment Engineering, 2019, 40(2): 136-140. (in Chinese) |
Cited By in Cnki | Click to display the text | |
[13] |
刘俊成, 张京娟, 冯培德. 基于相互测距信息的机群组网协同定位技术[J]. 北京航空航天大学学报, 2012, 38(4): 541-545. LIU J C, ZHANG J J, FENG P D. Swarming aircraft collaborative localization based on mutual rangings[J]. Journal of Beijing University of Aeronautics and Astronautics, 2012, 38(4): 541-545. (in Chinese) |
Cited By in Cnki (10) | Click to display the text | |
[14] |
刘晓洋, 徐胜红. 无人机编队分层式协同导航仿真研究[J]. 计算机仿真, 2019, 36(10): 44-48. LIU X Y, XU S H. Simulation research on hierarchical cooperative navigation of UAV formation[J]. Computer Simulation, 2019, 36(10): 44-48. (in Chinese) |
Cited By in Cnki | Click to display the text | |
[15] |
潘瑞鸿, 徐胜红. 基于几何特性的多无人机协同导航算法[J]. 兵器装备工程学报, 2017, 38(10): 55-59. PAN R H, XU S H. Multi-UAV cooperative navigation algorithm based on geometric characteristics[J]. Journal of Ordnance Equipment Engineering, 2017, 38(10): 55-59. (in Chinese) |
Cited By in Cnki (2) | Click to display the text | |
[16] |
于卓静, 孙永荣, 朱云峰, 等. 测角测距信息下的双机协同高精度定位算法[J]. 兵工自动化, 2019, 38(2): 1-5. YU Z J, SUN Y R, ZHU Y F, et al. High precision algorithm of dual-aircraft cooperative locating with angle and distance information[J]. Ordnance Industry Automation, 2019, 38(2): 1-5. (in Chinese) |
Cited By in Cnki | Click to display the text | |
[17] |
张滔.不完全量测下机动目标的多站无源定位跟踪算法[C]//第三十三届中国控制会议, 2014: 696-700. ZHANG T. Maneuvering target tracking algorithm based on multi-station bearing-only passive location with incomplete observation[C]//33rd Chinese Control Conference, 2014: 696-700(in Chinese). |
[18] | ABATZOGLOU T J, MENDEL J M, HARADA G A. The constrained total least squares technique and its applications to harmonic super resolution[J]. IEEE Transactions On Signal Processing, 2002, 39(5): 1070-1087. |
[19] | CHIASSON J, WANG K Y, LI M W, et al. Algebraic methods for nonlinear systems:Parameter identification and state estimation[J]. Current Trends in Nonlinear Systems and Control, 2006, 1-21. |
[20] | WILLMOTT C M K. Advantages of the mean absolute error (MAE) over the root mean square error (RMSE) in assessing average model performance[J]. Climate Research, 2005, 30(1): 79-82. |
Click to display the text |