CN112146650B - 一种用于无人蜂群协同导航的构型优化方法 - Google Patents

Abstract

本发明公开了一种用于无人蜂群协同导航的构型优化方法，属于导航技术领域。该方法利用无人蜂群协同信息进行无人飞行器所用卫星导航系统的卫星几何构型优化；结合各蜂群无人飞行器所用卫星导航系统可用导航卫星几何构型特点，以及蜂群无人飞行器间相对位置关系，建立了协同辅助信息多重筛选机制，有效利用几何位置较佳的飞行器协同辅助信息建立组合定位模型，降低系统几何精度因子，优化系统几何构型。该方法能够有效提高无人蜂群协同导航的精度和可靠性。

Description

一种用于无人蜂群协同导航的构型优化方法

技术领域

本发明涉及一种用于无人蜂群协同导航的构型优化方法，属于导航技术领域。

背景技术

在无人蜂群导航过程中，无人蜂群工作在恶劣条件下时，其自身卫星导航系统的可见卫星较少、几何构型差，定位精度可能无法满足协同导航需求，此时无人蜂群导航的可靠性将降低。协同导航技术利用多飞行器间的传感器信息，实现多飞行器间的信息协同，可用于改善系统几何构型，降低系统精度因子。但是受导航卫星几何分布的影响，部分几何构型较差的飞行器对系统精度因子的降低效果将欠佳，从而影响无人蜂群导航过程中的整体导航精度。因此，可对无人蜂群的协同导航信息进行筛选处理，选择几何位置较佳的协同辅助信息，提高各蜂群无人飞行器的导航精度。

现有方法通常直接利用所有可用的外部辅助信息，信息量大，计算复杂，从而造成了无人蜂群系统计算机资源的浪费。更重要的是，如果直接利用所有外部辅助信息，缺少对辅助信息的筛选机制，也无法确保无人蜂群导航精度的实际改善效果。

发明内容

为解决现有技术存在缺少辅助信息筛选机制、无法确保系统几何构型的实际优化效果等问题，本发明提出了一种用于无人蜂群协同导航的构型优化方法。

本发明为解决其技术问题采用如下技术方案：

一种用于无人蜂群协同导航的构型优化方法，包括如下步骤：

步骤1：令当前时刻为t，无人蜂群各成员根据自身的机载惯导系统，获取自身在地理坐标系下的位置矢量，并将其转换至地球固联直角坐标系中；

步骤2：无人蜂群的n个成员分别通过自身的机载卫星导航接收机接收导航卫星信号，从而确定当前各自能用导航卫星的个数，并通过导航卫星星历获取当前时刻每个成员的所有能用导航卫星在地球固联直角坐标系的位置矢量，以及所有能用导航卫星的高度角和方位角；

步骤3：无人蜂群的n个成员根据自身的机载无线电协同通信范围，确定各成员的能用协同成员，得到各成员的能用协同成员集合；

步骤4：针对无人蜂群任意一个成员k，k＝1,2,…,n，计算成员k的能用导航卫星数和能用协同成员数之和为w_k＝m_k+n_k，其中m_k为成员k的能用导航卫星数，n_k为成员k的能用协同成员数，并判断成员k的能用导航卫星数和能用协同成员数之和是否大于预设的阈值，若w_k＞q_n，则成员k的机载导航系统的导航观测信息足够，成员k的导航系统转入较佳协同区域划分模式，执行步骤5；否则成员k的导航系统进入观测信息不足告警模式，将该成员观测信息不足的信息输出至飞行管理系统，并将该成员从其它成员的能用协同成员集合中去除，同时该成员的导航系统转步骤24；所述q_n为预设好的阈值；

步骤5：成员k以自身为中心，单位1为半径，建立一个标准球体及标准球体坐标系；所有导航卫星不管其距离成员k的远近，一律投影到标准球体的球面；所述标准球体坐标系的基本大圆为当前时刻地平圈，原点为正北点，坐标为卫星方位角α和卫星高度角β，表示为(α,β)；在标准球体上表示卫星位置的α和β，与该卫星相对成员k的方位角和高度角相对应；卫星的α即为卫星投影点的方位角，卫星的β即为卫星投影点的高度角；

步骤6：判断成员k的能用协同成员集合O_k是否为空集，若是，则进入无协同信息告警模式，并转至步骤24；否则，令集合O_k中各协同成员编号为i,i＝1,2,…,n_k，n_k为成员k的能用协同成员个数，执行步骤7；所述无协同信息告警模式为：将成员k无能用协同信息的告警信息输出至飞行管理系统；

步骤7：根据步骤1获得的各能用协同成员位置计算成员k的能用协同成员i,i＝1,2,…,n_k相对成员k的方位角

和高度角θ_i；

步骤8：成员k以自身为中心，建立标准球体投影面平面直角坐标系，该坐标系X轴指向东向，Y轴指向北向，取值范围为x²+y²≤1；所述标准球体投影面平面直角坐标系上某一点坐标(x,y)与步骤5所述标准球体坐标系上点的坐标(α,β)的转换关系为将标准球体坐标系上的点投影至成员k的当前水平面上，某点在两种坐标系下的位置转换式为

步骤9：根据步骤2获得的各卫星高度角，成员k从其所有能用导航卫星中选择4颗高度角最低的卫星，并根据步骤8获得的两种坐标系的转换公式，得到该4颗导航卫星在标准球体投影面平面直角坐标系上的位置，令其位置为点a(x_a,y_a)、点b(x_b,y_b)、点c(x_c,y_c)和点d(x_d,y_d)，根据点a,b,c,d的坐标利用最小二乘法拟合得到一个圆，令其为拟合圆e，圆心e坐标为(x_e,y_e)，圆半径为r_e，表达式为(x-x_e)²+(y-y_e)²＝r_e ²；

步骤10：根据步骤9所得拟合圆e的圆心坐标(x_e,y_e)，以点e为圆心，构造两个同心圆，外圆半径为r_ex，内圆半径为r_in，两圆之间的区域构成一个圆环，圆环区域范围为r_in ²≤(x-x_e)²+(y-y_e)²≤r_ex ²，其中，外圆半径

内圆半径

(x_i,y_i)为导航卫星i,i＝1,2,…,m_k根据步骤8所述转换公式获得的在标准球体投影面平面直角坐标系上的位置坐标；该圆环区域为较差协同区域，不考虑在该区域内选择辅助飞行器，在该圆环外部或内部区域，选择辅助成员；

步骤11：根据步骤10所得圆环区域的位置，确定较佳协同区域为圆环内部区域还是圆环外部区域，根据射线eo的方程y＝y_e/x_e·x与步骤10所得外圆方程(x-x_e)²+(y-y_e)²＝r_ex ²和内圆方程(x-x_e)²+(y-y_e)²＝r_in ²，分别计算射线eo与外圆的交点坐标m(x_m,y_m)，射线eo与内圆的交点坐标n(x_n,y_n)，并转换得到点m、n在标准球体坐标系下的位置坐标m(α_m,β_m)和n(α_n,β_n)，若α_m＝α_n且β_m＞90-β_n，选择圆环外部区域为较佳协同区域，否则，选择圆环内部区域为较佳协同区域；

步骤12：根据系统所设定的高度角间隔

将步骤11所述较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2；所述第i，i＝1,2…,n个协同子区域为，该空间内任意协同成员在标准球体坐标系下的位置

满足如下要求：

且第i个协同子区域比第i+1个协同子区域具有更高的优先选择顺序；

步骤13：根据步骤2获得的卫星位置计算成员k的几何精度因子GDOP，将GDOP与预设好的限值l_dop比较，若GDOP>l_dop，执行步骤14，否则，转步骤24；

步骤14：对成员k进行辅助记录信息初始化，成员k当前辅助成员数量为s_k，初始的s_k＝0，成员k当前辅助成员集合为U_k，U_k初始为空集，初始观测系数矩阵

初始观测量协方差阵

其中，H_k为卫星观测系数矩阵，

为m_k阶单位矩阵，σ_s为导航卫星伪距标准差；

步骤15：对成员k进行辅助成员增选，按步骤12所述优先顺序依次从第i个协同子区域对所有能用协同成员进行筛选，初始i＝1，从第1个协同子区域开始筛选，定义成员k的较佳协同成员集合U_k，U_k初始为空集；

步骤16：判断成员k的能用协同成员j,j＝1,2,…,n_k是否处于第i个协同子区域内，若是，将协同成员j加入成员k的较佳协同成员集合U_k，否则不加入较佳协同成员集合U_k；

步骤17：判断较佳协同成员集合U_k中协同成员的个数u，若u＝1，成员k选择该成员作为辅助成员，转步骤22，若u＞1，对集合U_k中u个协同成员进行二次筛选，执行步骤18；若u＝0，不在该协同子区域选择辅助成员，转步骤19；

步骤18：根据步骤11所得点m的方位角α_m确定最佳方位角γ，当选择圆环外部区域为较佳协同区域时，令γ＝α_m，当选择圆环内部区域为较佳协同区域时，若0≤α_m＜180，令γ＝α_m+180，若180≤α_m＜360，令γ＝α_m-180；成员k从其较佳协同成员集合U_k中选择一个方位角

j＝1,2,…,u最接近最佳方位角γ，即

最小的协同成员作为辅助成员，其中u为较佳协同成员集合U_k中协同成员的个数；

步骤19：判断i是否等于协同子区域个数n，若i不等于n，令i＝i+1，成员k从下一个协同子区域筛选辅助成员，转步骤16，否则，成员k从其能用协同成员集合O_k中随机选择一个成员作为辅助成员，执行步骤20；

步骤20：将选择的辅助成员加入辅助成员集合V_k中，并在能用协同成员集合O_k中删除该成员，同时s_k＝s_k+1，令选择的辅助成员编号为j，计算成员k的协同辅助观测系数矩阵g^j，并更新在当前辅助条件下的观测系数矩阵

和观测量协方差阵

步骤21：计算成员k在当前辅助条件下的几何精度因子GDOP，其表达式为

其中，P为成员k在当前辅助条件下的权重矩阵，

trace()表示求矩阵的迹；

步骤22：将GDOP与预设好的限值l_dop比较，若GDOP>l_dop，转步骤15，否则，执行步骤23；

步骤23：成员k利用其辅助成员的协同信息，与自身卫星导航系统的卫星观测信息进行组合，进行定位解算。

步骤24：判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤1，整个无人蜂群协同导航系统进行下一时刻的构型优化。

步骤7中所述计算成员k能用协同成员i，i＝1,2,…,n_k相对成员k的方位角

和高度角θ_i的具体方法为：

步骤7.1：计算成员k与能用协同成员i在当前地理坐标系下X，Y，Z轴的差值Δx_i、Δy_i、Δz_i，其表达式为：

其中，

为从地球坐标系到地理坐标系的转换矩阵，x_ni、y_ni、z_ni分别为能用协同成员i在地球固连直角坐标系下X、Y、Z轴的坐标，x_I、y_I、z_I分别为成员k在地球固连直角坐标系下X、Y、Z轴的坐标；

步骤7.2：计算协同成员i相对成员k的方位角

的主值ψ_i，其表达式为

步骤7.3：计算协同成员i相对成员k的方位角

其表达式为

步骤7.4：计算协同成员i相对成员k的高度角θ_i，其表达式为

θ_i＝arctan(Δx_i/Δyi)。

步骤12中所述根据系统所设定的高度角间隔

将较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2的具体方法为：

步骤12.1：计算协同子区域个数n，选择圆环外部区域为较佳协同区域时，协同子区域数

选择圆环内部区域为较佳协同区域时，协同子区域数

其中，int()表示对实数取整；

步骤12.2：计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1和β_i2，选择圆环外部区域为较佳协同区域时，令

选择圆环内部区域为较佳协同区域时，令

步骤12.3：计算第i，i＝1,2…,n个协同子区域的方位角临界值α_i1和α_i2，选择圆环外部区域为较佳协同区域时，在标准球体投影面平面直角坐标系中，根据第i个协同区域的高度角临界值

建立以原点o为圆心，

为半径的圆，判断其与外圆是否有两个交点，若有，计算两个圆的交点g_i、h_i的位置坐标，并转换为标准球体坐标系下位置坐标

令

否则，令α_i1＝0、α_i2＝360；选择圆环内部区域为较佳协同区域时，令α_i1＝0、α_i2＝360。

所述步骤16中判断成员k的能用协同成员j,j＝1,2,…,n_k是否处于第i个协同子区域内的具体方法为：根据步骤7得到的协同成员j的方位角

和高度角θ_j，将其与第i个协同子区域方位角角临界值α_i1、α_i2和高度角临界值β_i1、β_i2比较，若

且β_i1＜θ_j＜β_i2，则协同成员j的位置处于第i个协同子区域内，否则协同成员j的位置不处于第i个协同子区域内。

所述步骤20中计算成员k的协同辅助观测矩阵g^j，具体为：

其中，

分别为地球固连直角坐标系下成员k到辅助成员j的距离矢量对于X、Y、Z轴的方向余；

所述步骤20中成员k更新在当前辅助条件下的观测系数矩阵

和观测量协方差阵

具体为：

其中，

为前一次循环已有辅助条件下的观测系数矩阵，

为前一次循环已有辅助条件下观测量协方差阵，σ^j为辅助成员j与成员k间的测距标准差。

本发明的有益效果如下：

1、本发明考虑了协同导航系统中各飞行器可用导航卫星的几何分布情况，在获得导航卫星的方位及高度信息的基础上，根据蜂群飞行器的相对位置选择辅助飞行器，能有效改善导航卫星与辅助飞行器共同组成的几何构型。

2、本发明考虑了不同辅助飞行器对系统几何精度因子降低效果的差异，建立了协同辅助信息多重筛选机制，充分利用几何位置较佳的辅助飞行器信息，使无人蜂群系统的导航精度有较大提高，克服了原有方法缺少信息筛选机制造成导航精度提高效果不明显的不足。

3、本发明适用性广，不受协同辅助信息种类和数量的限制。

附图说明

图1是本发明的流程图。

图2(a)是选择圆环外部区域为较佳协同区域时协同子区域划分示意图。

图2(b)是选择圆环内部区域为较佳协同区域时协同子区域划分示意图。

图3是蜂群无人飞行器飞行航迹图。

图4是无人飞行器1采用本发明方法后筛选得到的1架辅助飞行器的编号图。

图5是导航时间第1600s时，无人飞行器1的能用导航卫星和辅助飞行器的几何分布图。

图6是无人飞行器1未采用优化方法、采用传统优化方法和采用本发明优化方法后的几何精度因子GDOP变化曲线对比图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

如图1所示，本实施例提供一种用于无人蜂群协同导航的构型优化方法，具体如下所示：

步骤(1)：令当前时刻为t，无人蜂群各成员根据自身的机载惯导系统，获取自身在地理坐标系下的位置矢量，并将其转换至地球固联直角坐标系中；

步骤(2)：无人蜂群的n个成员分别通过自身的机载卫星导航接收机接收导航卫星信号，从而确定当前各自能用导航卫星的个数，并通过导航卫星星历获取当前时刻每个成员的所有能用导航卫星在地球固联直角坐标系的位置矢量，以及所有能用导航卫星的高度角和方位角；

步骤(3)：无人蜂群的n个成员根据自身的机载无线电协同通信范围，确定各成员的能用协同成员，得到各成员的能用协同成员集合；

步骤(4)：针对无人蜂群任意一个成员k，k＝1,2,…,n，计算成员k的能用导航卫星数和能用协同成员数之和为w_k＝m_k+n_k，其中m_k为成员k的能用导航卫星数，n_k为成员k的能用协同成员数，并判断成员k的能用导航卫星数和能用协同成员数之和是否大于预设的阈值，若w_k＞q_n，则成员k的机载导航系统的导航观测信息足够，成员k的导航系统转入较佳协同区域划分模式，执行步骤(5)；否则成员k的导航系统进入观测信息不足告警模式，将该成员观测信息不足的信息输出至飞行管理系统，并将该成员从其它成员的能用协同成员集合中去除，同时该成员的导航系统转步骤(24)；所述q_n为预设好的阈值；本实施例中q_n＝4；

步骤(5)：成员k以自身为中心，单位1为半径，建立一个标准球体及标准球体坐标系；所有导航卫星不管其距离成员k的远近，一律投影到标准球体的球面；所述标准球体坐标系的基本大圆为当前时刻地平圈，原点为正北点，坐标为卫星方位角α和卫星高度角β，表示为(α,β)；在标准球体上表示卫星位置的α和β，与该卫星相对成员k的方位角和高度角相对应；卫星的α即为卫星投影点的方位角，卫星的β即为卫星投影点的高度角；

步骤(6)：判断成员k的能用协同成员集合O_k是否为空集，若是，则进入无协同信息告警模式，并转至步骤(24)；否则，令集合O_k中各协同成员编号为i,i＝1,2,…,n_k，n_k为成员k的能用协同成员个数，执行步骤(7)；所述无协同信息告警模式为：将成员k无能用协同信息的告警信息输出至飞行管理系统；

步骤(7)：根据步骤(1)获得的各能用协同成员位置计算成员k的能用协同成员i,i＝1,2,…,n_k相对成员k的方位角

和高度角θ_i，包括以下子步骤：

(7.1)计算成员k与能用协同成员i在当前地理坐标系下X，Y，Z轴的差值Δx_i、Δy_i、Δz_i，其表达式为：

其中，

为从地球坐标系到地理坐标系的转换矩阵，x_ni、y_ni、z_ni分别为能用协同成员i在地球固连直角坐标系下X、Y、Z轴的坐标，x_I、y_I、z_I分别为成员k在地球固连直角坐标系下X、Y、Z轴的坐标；

(7.2)计算协同成员i相对成员k的方位角

的主值ψ_i，其表达式为

(7.3)计算协同成员i相对成员k的方位角

其表达式为

(7.4)计算协同成员i相对成员k的高度角θ_i，其表达式为

θ_i＝arctan(Δx_i/Δy_i)

步骤(8)：成员k以自身为中心，建立标准球体投影面平面直角坐标系，该坐标系X轴指向东向，Y轴指向北向，取值范围为x²+y²≤1；所述标准球体投影面平面直角坐标系上某一点坐标(x,y)与步骤(5)所述标准球体坐标系上点的坐标(α,β)的转换关系为将标准球体坐标系上的点投影至成员k的当前水平面上，某点在两种坐标系下的位置转换式为

步骤(9)：根据步骤(2)获得的各卫星高度角，成员k从其所有能用导航卫星中选择4颗高度角最低的卫星，并根据步骤(8)获得的两种坐标系的转换公式，得到该4颗导航卫星在标准球体投影面平面直角坐标系上的位置，如图2(a)和图2(b)所示，令其位置坐标为点a(x_a,y_a)、点b(x_b,y_b)、点c(x_c,y_c)和点d(x_d,y_d)，根据点a,b,c,d的位置坐标利用最小二乘法拟合得到一个圆，令其为拟合圆e，圆心e坐标为(x_e,y_e)，圆半径为r_e，表达式为(x-x_e)²+(y-y_e)²＝r_e ²；

步骤(10)：根据步骤(9)所得拟合圆e的圆心坐标(x_e,y_e)，以点e为圆心，构造两个同心圆，外圆半径为r_ex，内圆半径为r_in，两圆之间的区域构成一个圆环，圆环区域范围为r_in ²≤(x-x_e)²+(y-y_e)²≤r_ex ²，其中，外圆半径

内圆半径

(x_i,y_i)为导航卫星i,i＝1,2,…,m_k根据步骤8所述转换公式获得的在标准球体投影面平面直角坐标系上的位置坐标；该圆环区域为较差协同区域，不考虑在该区域内选择辅助飞行器，在该圆环外部或内部区域，选择辅助成员；

步骤(11)：根据步骤(10)所得圆环区域的位置，确定较佳协同区域为圆环内部区域还是圆环外部区域，根据射线eo的方程y＝y_e/x_e·x与步骤10所得外圆方程(x-x_e)²+(y-y_e)²＝r_ex ²和内圆方程(x-x_e)²+(y-y_e)²＝r_in ²，分别计算射线eo与外圆的交点坐标m(x_m,y_m)，射线eo与内圆的交点坐标n(x_n,y_n)，并转换得到点m、n在标准球体坐标系下的位置坐标m(α_m,β_m)和n(α_n,β_n)，若α_m＝α_n且β_m＞90-β_n，选择圆环外部区域为较佳协同区域，如图2(a)所示，否则，选择圆环内部区域为较佳协同区域，如图2(b)所示；

步骤(12)：根据系统所设定的高度角间隔

将步骤(13)所述较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2；所述第i，i＝1,2…,n个协同子区域为，该空间内任意协同成员在标准球体坐标系下的位置

满足如下要求：

且第i个协同子区域比第i+1个协同子区域具有更高的优先选择顺序；本实施中

为10度；其中，将较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2包括以下子步骤：

(12.1)计算协同子区域个数n，选择圆环外部区域为较佳协同区域时，协同子区域数

选择圆环内部区域为较佳协同区域时，协同子区域数

其中，int()表示对实数取整；

(12.2)计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1和β_i2，选择圆环外部区域为较佳协同区域时，令

选择圆环内部区域为较佳协同区域时，令

(12.3)计算第i，i＝1,2…,n个协同子区域的方位角临界值α_i1和α_i2，选择圆环外部区域为较佳协同区域时，在标准球体投影面平面直角坐标系中，根据第i个协同区域的高度角临界值

建立以原点o为圆心，

为半径的圆，判断其与外圆是否有两个交点，若有，计算两个圆的交点g_i、h_i的位置坐标，并转换为标准球体坐标系下位置坐标

令

否则，令α_i1＝0、α_i2＝360；选择圆环内部区域为较佳协同区域时，令α_i1＝0、α_i2＝360；

步骤(13)：根据步骤(2)获得的卫星位置计算成员k的几何精度因子GDOP，将GDOP与预设好的限值l_dop比较，若GDOP>l_dop，执行步骤(14)，否则，转步骤(24)；

步骤(14)：对成员k进行辅助记录信息初始化，成员k当前辅助成员数量为s_k，初始的s_k＝0，成员k当前辅助成员集合为U_k，U_k初始为空集，初始观测系数矩阵

初始观测量协方差阵

其中，H_k为卫星观测系数矩阵，

为m_k阶单位矩阵，σ_s为导航卫星伪距标准差；

步骤(15)：对成员k进行辅助成员增选，按步骤(12)所述优先顺序依次从第i个协同子区域对所有能用协同成员进行筛选，初始i＝1，从第1个协同子区域开始筛选，定义成员k的较佳协同成员集合U_k，U_k初始为空集；

步骤(16)：根据步骤(7)得到的协同成员j,j＝1,2,…,n_k的方位角

和高度角θ_j，将其与第i个协同子区域方位角角临界值α_i1、α_i2和高度角临界值β_i1、β_i2比较，若

且β_i1＜θ_j＜β_i2，则协同成员j的位置处于第i个协同子区域内，将协同成员j加入成员k的较佳协同成员集合U_k，否则不加入较佳协同成员集合U_k；

步骤(17)：判断较佳协同成员集合U_k中协同成员的个数u，若u＝1，成员k选择该成员作为辅助成员，转步骤(22)，若u＞1，对集合U_k中u个协同成员进行二次筛选，执行步骤(18)；若u＝0，不在该协同子区域选择辅助成员，转步骤(19)；

步骤(18)：根据步骤(11)所得点m的方位角α_m确定最佳方位角γ，当选择圆环外部区域为较佳协同区域时，令γ＝α_m，当选择圆环内部区域为较佳协同区域时，若0≤α_m＜180，令γ＝α_m+180，若180≤α_m＜360，令γ＝α_m-180；成员k从其较佳协同成员集合U_k中选择一个方位角

j＝1,2,…,u最接近最佳方位角γ，即

最小的协同成员作为辅助成员，其中u为较佳协同成员集合U_k中协同成员的个数；

步骤(19)：判断i是否等于协同子区域个数n，若i不等于n，令i＝i+1，成员k从下一个协同子区域筛选辅助成员，转步骤(16)，否则，成员k从其能用协同成员集合O_k中随机选择一个成员作为辅助成员，执行步骤(20)；

步骤(20)：将选择的辅助成员加入辅助成员集合V_k中，并在能用协同成员集合O_k中删除该成员，同时s_k＝s_k+1，令选择的辅助成员编号为j，计算成员k的协同辅助观测系数矩阵g^j，其表达式为

其中，

分别为地球固连直角坐标系下成员k到辅助成员j的距离矢量对于X、Y、Z轴的方向余弦；

根据协同辅助观测系数矩阵g^j更新在当前辅助条件下的观测系数矩阵

和观测量协方差阵

具体为：

其中，

为前一次循环已有辅助条件下的观测系数矩阵，

为前一次循环已有辅助条件下观测量协方差阵，σ^j为辅助成员j与成员k间的测距标准差；

步骤(21)：计算成员k在当前辅助条件下的几何精度因子GDOP，其表达式为

其中，P为成员k在当前辅助条件下的权重矩阵，

trace()表示求矩阵的迹；

步骤(22)：将GDOP与预设好的限值l_dop比较，若GDOP＞l_dop，转步骤(15)，否则，执行步骤(23)；

步骤(23)：成员k利用其辅助成员的协同信息，与自身卫星导航系统的卫星观测信息进行组合，进行定位解算。

步骤(24)：判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤(1)，整个无人蜂群协同导航系统进行下一时刻的构型优化。

为了验证本发明所提出的构型优化方法用于优化蜂群无人飞行器几何构型的有效性，进行数字仿真分析。仿真中采用的无人蜂群中无人飞行器数量为10架，飞行器间相对距离测量精度为5米。图3是蜂群无人飞行器飞行航迹；图4是无人飞行器1采用本发明方法后筛选得到的1架辅助飞行器的编号；图5是导航时间为1600s时，无人飞行器1的能用导航卫星和辅助飞行器的几何分布图；图6是无人飞行器1未采用优化方法、采用传统优化方法和采用本发明优化方法后的几何精度因子GDOP变化曲线对比图；表1是无人飞行器1在3600s导航时间内未采用优化方法、采用传统优化方法和采用本发明优化方法所得协同飞行器辅助下的卫星导航系统的位置误差均方差。

表1

由图4可以看出，采用本实例所提出的一种用于无人蜂群协同导航的构型优化方法后，蜂群无人飞行器可根据自身卫星导航系统导航卫星几何构型的变化选择不同的辅助飞行器；由图5可以看出，采用本实例所提出的一种用于无人蜂群协同导航的构型优化方法后，无人飞行器1所有能用导航卫星和辅助飞行器构成的几何分布构型较好，能够有效改善系统的几何构型；由图6可以看出，采用本实例所提出的一种用于无人蜂群协同导航的构型优化方法后，无人蜂群导航系统的几何精度因子在各阶段都有较大改善；由表1可以看出，采用本实例所提出的一种用于无人蜂群协同导航的构型优化方法后，利用筛选得到的飞行器协同信息辅助卫星导航系统能降低位置误差，有效提高无人蜂群系统的导航精度；此外，采用本发明能够适应无人蜂群在飞行过程中构型变化引起的几何精度因子的不断变化，具有良好的应用价值。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种用于无人蜂群协同导航的构型优化方法，其特征在于，包括如下步骤：

步骤1：令当前时刻为t，无人蜂群各成员根据自身的机载惯导系统，获取自身在地理坐标系下的位置矢量，并将其转换至地球固联直角坐标系中；

步骤2：无人蜂群的n个成员分别通过自身的机载卫星导航接收机接收导航卫星信号，从而确定当前各自能用导航卫星的个数，并通过导航卫星星历获取当前时刻每个成员的所有能用导航卫星在地球固联直角坐标系的位置矢量，以及所有能用导航卫星的高度角和方位角；

步骤3：无人蜂群的n个成员根据自身的机载无线电协同通信范围，确定各成员的能用协同成员，得到各成员的能用协同成员集合；

步骤4：针对无人蜂群任意一个成员k，k＝1,2,…,n，计算成员k的能用导航卫星数和能用协同成员数之和为w_k＝m_k+n_k，其中m_k为成员k的能用导航卫星数，n_k为成员k的能用协同成员数，并判断成员k的能用导航卫星数和能用协同成员数之和是否大于预设的阈值，若w_k＞q_n，则成员k的机载导航系统的导航观测信息足够，成员k的导航系统转入较佳协同区域划分模式，执行步骤5；否则成员k的导航系统进入观测信息不足告警模式，将该成员观测信息不足的信息输出至飞行管理系统，并将该成员从其它成员的能用协同成员集合中去除，同时该成员的导航系统转步骤24；所述q_n为预设好的阈值；

步骤5：成员k以自身为中心，单位1为半径，建立一个标准球体及标准球体坐标系；所有导航卫星不管其距离成员k的远近，一律投影到标准球体的球面；所述标准球体坐标系的基本大圆为当前时刻地平圈，原点为正北点，坐标为卫星方位角α和卫星高度角β，表示为(α,β)；在标准球体上表示卫星位置的α和β，与该卫星相对成员k的方位角和高度角相对应；卫星的α即为卫星投影点的方位角，卫星的β即为卫星投影点的高度角；

步骤6：判断成员k的能用协同成员集合O_k是否为空集，若是，则进入无协同信息告警模式，并转至步骤24；否则，令集合O_k中各协同成员编号为i,i＝1,2,…,n_k，n_k为成员k的能用协同成员个数，执行步骤7；所述无协同信息告警模式为：将成员k无能用协同信息的告警信息输出至飞行管理系统；

步骤7：根据步骤1获得的各能用协同成员位置计算成员k的能用协同成员i,i＝1,2,…,n_k相对成员k的方位角

和高度角θ_i；

步骤8：成员k以自身为中心，建立标准球体投影面平面直角坐标系，该坐标系X轴指向东向，Y轴指向北向，取值范围为x²+y²≤1；所述标准球体投影面平面直角坐标系上某一点坐标(x,y)与步骤5所述标准球体坐标系上点的坐标(α,β)的转换关系为将标准球体坐标系上的点投影至成员k的当前水平面上，某点在两种坐标系下的位置转换式为

步骤9：根据步骤2获得的各卫星高度角，成员k从其所有能用导航卫星中选择4颗高度角最低的卫星，并根据步骤8获得的两种坐标系的转换公式，得到该4颗导航卫星在标准球体投影面平面直角坐标系上的位置，令其位置为点a(x_a,y_a)、点b(x_b,y_b)、点c(x_c,y_c)和点d(x_d,y_d)，根据点a,b,c,d的坐标利用最小二乘法拟合得到一个圆，令其为拟合圆e，圆心e坐标为(x_e,y_e)，圆半径为r_e，表达式为(x-x_e)²+(y-y_e)²＝r_e ²；

步骤10：根据步骤9所得拟合圆e的圆心坐标(x_e,y_e)，以点e为圆心，构造两个同心圆，外圆半径为r_ex，内圆半径为r_in，两圆之间的区域构成一个圆环，圆环区域范围为r_in ²≤(x-x_e)²+(y-y_e)²≤r_ex ²，其中，外圆半径

内圆半径

为导航卫星i,i＝1,2,…,m_k根据步骤8所述转换公式获得的在标准球体投影面平面直角坐标系上的位置坐标；该圆环区域为较差协同区域，不考虑在该区域内选择辅助飞行器，在该圆环外部或内部区域，选择辅助成员；

步骤11：根据步骤10所得圆环区域的位置，确定较佳协同区域为圆环内部区域还是圆环外部区域，根据射线eo的方程y＝y_e/x_e·x与步骤10所得外圆方程(x-x_e)²+(y-y_e)²＝r_ex ²和内圆方程(x-x_e)²+(y-y_e)²＝r_in ²，分别计算射线eo与外圆的交点坐标m(x_m,y_m)，射线eo与内圆的交点坐标n(x_n,y_n)，并转换得到点m、n在标准球体坐标系下的位置坐标m(α_m,β_m)和n(α_n,β_n)，若α_m＝α_n且β_m＞90-β_n，选择圆环外部区域为较佳协同区域，否则，选择圆环内部区域为较佳协同区域；

步骤12：根据系统所设定的高度角间隔

将步骤11所述较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2；所述第i，i＝1,2…,n个协同子区域为，该空间内任意协同成员在标准球体坐标系下的位置

满足如下要求：

且第i个协同子区域比第i+1个协同子区域具有更高的优先选择顺序；

步骤13：根据步骤2获得的卫星位置计算成员k的几何精度因子GDOP，将GDOP与预设好的限值l_dop比较，若GDOP>l_dop，执行步骤14，否则，转步骤24；

步骤14：对成员k进行辅助记录信息初始化，成员k当前辅助成员数量为s_k，初始的s_k＝0，成员k当前辅助成员集合为U_k，U_k初始为空集，初始观测系数矩阵

初始观测量协方差阵

其中，H_k为卫星观测系数矩阵，

为m_k阶单位矩阵，σ_s为导航卫星伪距标准差；

步骤15：对成员k进行辅助成员增选，按步骤12所述优先选择顺序依次从第i个协同子区域对所有能用协同成员进行筛选，初始i＝1，从第1个协同子区域开始筛选，定义成员k的较佳协同成员集合U_k，U_k初始为空集；

步骤16：判断成员k的能用协同成员j,j＝1,2,…,n_k是否处于第i个协同子区域内，若是，将协同成员j加入成员k的较佳协同成员集合U_k，否则不加入较佳协同成员集合U_k；

步骤17：判断较佳协同成员集合U_k中协同成员的个数u，若u＝1，成员k选择该成员作为辅助成员，转步骤22，若u＞1，对集合U_k中u个协同成员进行二次筛选，执行步骤18；若u＝0，不在该协同子区域选择辅助成员，转步骤19；

步骤18：根据步骤11所得点m的方位角α_m确定最佳方位角γ，当选择圆环外部区域为较佳协同区域时，令γ＝α_m，当选择圆环内部区域为较佳协同区域时，若0≤α_m＜180，令γ＝α_m+180，若180≤α_m＜360，令γ＝α_m-180；成员k从其较佳协同成员集合U_k中选择一个方位角

j＝1,2,…,u最接近最佳方位角γ，即

最小的协同成员作为辅助成员，其中u为较佳协同成员集合U_k中协同成员的个数；

步骤19：判断i是否等于协同子区域个数n，若i不等于n，令i＝i+1，成员k从下一个协同子区域筛选辅助成员，转步骤16，否则，成员k从其能用协同成员集合O_k中随机选择一个成员作为辅助成员，执行步骤20；

步骤20：将选择的辅助成员加入辅助成员集合V_k中，并在能用协同成员集合O_k中删除该成员，同时s_k＝s_k+1，令选择的辅助成员编号为j，计算成员k的协同辅助观测系数矩阵g^j，并更新在当前辅助条件下的观测系数矩阵

和观测量协方差阵

步骤21：计算成员k在当前辅助条件下的几何精度因子GDOP，其表达式为

其中，P为成员k在当前辅助条件下的权重矩阵，

trace()表示求矩阵的迹；

步骤22：将GDOP与预设好的限值l_dop比较，若GDOP>l_dop，转步骤15，否则，执行步骤23；

步骤23：成员k利用其辅助成员的协同信息，与自身卫星导航系统的卫星观测信息进行组合，进行定位解算；

步骤24：判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤1，整个无人蜂群协同导航系统进行下一时刻的构型优化。

2.根据权利要求1所述的一种用于无人蜂群协同导航的构型优化方法，其特征在于，步骤7中所述计算成员k能用协同成员i，i＝1,2,…,n_k相对成员k的方位角

和高度角θ_i的具体方法为：

步骤7.1：计算成员k与能用协同成员i在当前地理坐标系下X，Y，Z轴的差值Δx_i、Δy_i、Δz_i，其表达式为：

其中，

为从地球坐标系到地理坐标系的转换矩阵，x_ni、y_ni、z_ni分别为能用协同成员i在地球固连直角坐标系下X、Y、Z轴的坐标，x_I、y_I、z_I分别为成员k在地球固连直角坐标系下X、Y、Z轴的坐标；

步骤7.2：计算协同成员i相对成员k的方位角

的主值ψ_i，其表达式为

步骤7.3：计算协同成员i相对成员k的方位角

其表达式为

步骤7.4：计算协同成员i相对成员k的高度角θ_i，其表达式为

θ_i＝arctan(Δx_i/Δy_i)。

3.根据权利要求1所述的一种用于无人蜂群协同导航的构型优化方法，其特征在于，步骤12中所述根据系统所设定的高度角间隔

将较佳协同区域划分成n个协同子区域，并计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1、β_i2和方位角临界值α_i1、α_i2的具体方法为：

步骤12.1：计算协同子区域个数n，选择圆环外部区域为较佳协同区域时，协同子区域数

选择圆环内部区域为较佳协同区域时，协同子区域数

其中，int()表示对实数取整；

步骤12.2：计算第i，i＝1,2…,n个协同子区域的高度角临界值β_i1和β_i2，选择圆环外部区域为较佳协同区域时，令

选择圆环内部区域为较佳协同区域时，令

步骤12.3：计算第i，i＝1,2…,n个协同子区域的方位角临界值α_i1和α_i2，选择圆环外部区域为较佳协同区域时，在标准球体投影面平面直角坐标系中，根据第i个协同区域的高度角临界值

建立以原点o为圆心，

为半径的圆，判断其与外圆是否有两个交点，若有，计算两个圆的交点g_i、h_i的位置坐标，并转换为标准球体坐标系下位置坐标

令

否则，令α_i1＝0、α_i2＝360；选择圆环内部区域为较佳协同区域时，令α_i1＝0、α_i2＝360。

4.根据权利要求1所述的一种用于无人蜂群协同导航的构型优化方法，其特征在于，所述步骤16中判断成员k的能用协同成员j,j＝1,2,…,n_k是否处于第i个协同子区域内的具体方法为：根据步骤7得到的协同成员j的方位角

和高度角θ_j，将其与第i个协同子区域方位角临界值α_i1、α_i2和高度角临界值β_i1、β_i2比较，若

且β_i1＜θ_j＜β_i2，则协同成员j的位置处于第i个协同子区域内，否则协同成员j的位置不处于第i个协同子区域内。

5.根据权利要求1所述的一种用于无人蜂群协同导航的构型优化方法，其特征在于，所述步骤20中计算成员k的协同辅助观测系数矩阵g^j，具体为：

其中，

分别为地球固连直角坐标系下成员k到辅助成员j的距离矢量对于X、Y、Z轴的方向余；

所述步骤20中成员k更新在当前辅助条件下的观测系数矩阵

和观测量协方差阵

具体为：

其中，

为前一次循环已有辅助条件下的观测系数矩阵，

为前一次循环已有辅助条件下观测量协方差阵，σ^j为辅助成员j与成员k间的测距标准差。

Images