【信息科学与控制工程 】
一种长机故障情况下机群编队协同定位算法
刘晓洋a,徐胜红b
(海军航空大学 a.研究生管理大队； b.控制工程系， 山东 烟台 264001)
摘要 ：提出了一种基于联邦滤波的协同定位方案，采用两级卡尔曼滤波结构，在相对导航坐标系和僚机惯导位置输出转换的基础上建立系统数学模型，最后采用联邦滤波算法进行定位误差估计；仿真结果表明：基于联邦滤波的协同定位算法能够有效抑制长机故障后机群编队定位误差迅速发散的情况。
关键词 ：机群编队；定位误差；协同导航；卡尔曼滤波
面对现代战争日益复杂的作战环境，机群编队协同作战这种新型空战模式受到了越来越多的关注[1]。协同定位作为协同作战的关键技术之一，其定位精度的高低直接影响整个编队协同作战效果。目前，机群协同定位主要采用高低精度搭配的主从式结构，然而在编队执行任务时长机很可能在敌方的干扰作用下失去与僚机的通信，无法为编队协同定位提供高精度的定位信息，导致机群定位效果不佳。因此，如何在长机故障情况下实现协同定位已成为急需解决的重要问题。
针对机群编队缺少误差有界绝对导航信息的问题，文献[2]提出了一种基于CEC与捷联惯性导航(SINS)组合的相对导航算法，提高了平台的导航精度，弥补了SINS误差随航时积累的不足，但该算法无法满足动平台编队协同作战的要求。文献[3]在无GPS条件下，提出了一种基于机间相互测距信息并利用几何图形平移旋转来估计各飞机导航误差的方法。该方法有效延缓了惯导位置误差的发散速度，但没有考虑飞机高度信息，只是在平面内进行了机群协同定位，且该算法需要机群队形的先验知识，有较高的局限性。文献[4]通过编队成员INS位置信息和机间测距信息作差得到量测值来提高各成员INS的位置精度，但该方法建立的模型维数过高，且各成员两两之间均要求通信，因此对计算机和数据链的要求较高。本文在对多平台INS输出信息进行最优综合实现INS误差修正[5]的基础上，提出了一种分布式机群编队协同定位算法，在保证长机失联前后编队协同导航系统模型变动较小的情况下抑制编队整体定位误差发散。
“当过一天兵，一辈子都是军人！”谢清森表示，只要国家有需要，任何时候，这些退伍老兵都会响应号召，奔赴第一线，这是一个兵的职责，也是七兵堂的宗旨：“育精兵铁卫，保稷泰民安”。
1 问题描述与方案设计
协同导航系统定位算法结构如图1所示，长机搭载精度较高的INS/GPS组合导航系统，其他成员设为僚机，除搭载较低精度的INS之外，均配备有光电测距测角传感器等相对导航设备，用以测量与长机之间相对距离和方位等相对信息。长机通过机载数据链广播高精度导航信息，为僚机提供量测信息。当长机因被击落等原因与编队失去联系后，僚机无法获得高精度的量测信息，若仅依靠搭载的低精度INS则定位误差会迅速发散。
那血痕反倒让诸葛玉眼前一亮，只见他微笑着收回了手中的剑，对着赵白说：“我的剑脏了，我从来不用一把脏剑比武，我们约定十年后再战吧！”
图1 协同导航系统定位算法结构
此时，可选择其中的某一架僚机作为相对导航参考点，视其为伪长机。伪长机的选取可依据与其他无人机的距离关系，假设僚机i 与僚机j 之间的直线距离为d ij ，则僚机i 与编队其他成员的距离和D i 可以表示为
D i =min(D 1,D 2,…,D n )
选定伪长机后，其他僚机一方面以伪主机为基准点测量相对距离和相对方位等相对导航信息作为僚机卡尔曼滤波器的量测信息，另一方面通过机载数据链向伪长机提供通过僚机卡尔曼滤波器得到的状态估计。伪长机与长机类似，通过机载数据链为僚机提供量测信息，不同之处在于，除了提供量测信息外，伪长机卡尔曼滤波器还反馈其状态估计给僚机用于重置僚机状态。自此，整个机群编队构成了一个卡尔曼滤波闭环系统。为了更加简洁地说明算法，本文以3机为例建立系统模型。
2 系统数学模型
2.1 僚机部分系统方程
选取地理坐标系为导航坐标系(n 系)，僚机 的INS误差状态方程为
(1)
式中，状态量X i 为
ε bxi ε byi ε bzi ε rxi ε ryi ε rzi ▽xi ▽yi ▽zi ]T
系统噪声W i 为
其中，δλ i 、δφ i 、δh i 为三维位置误差，δv Ei 、δv Ni 、δv Ui 为三维速度误差，ε bxi 、ε byi 、ε bzi 为3个陀螺仪随机常值漂移，φ Ei 、φ Ni 、φ Ui 为平台坐标系与导航坐标系之间误差产生的3个平台失准角，ε rxi 、ε ryi 、ε rzi 为3个陀螺仪一阶马尔科夫漂移，▽xi 、▽yi 、▽zi 为3个加速度计一阶马尔科夫漂移。
僚机i 状态方程为自身INS与伪长机INS误差状态方程的叠加，即
(2)
如图1所示，在导航坐标系(n 系)中，僚机i 量测值Z i 是由僚机i 通过数据链收到的伪长机INS信息与自身INS信息和僚机测量的相对导航信息依次相减得到的，即
（2）适当提前“未知数、变量的使用”“代数式的运算”尤其是“代数式的运算”最早开始年级；适当推后代数思维知识主题最晚结束年级．中国代数思维课程内容阶段跨度是六国中最小的国家，所属4个知识主题均只属一个阶段，连续性较弱；然而，代数思维作为联系“算术”与“代数”的桥梁，在澳大利亚、英国、美国、南非均横跨小学、初中、高中3个阶段，适当延长中国整个代数思维课程内容的设置时间显得很有必要．
(3)
和
可分别看作由伪长机和僚机真实位置
和各自INS系统解算误差δp g 、δp i 两部分组成。僚机测量的相对导航信息也存在误差，因此也可把
看成由真实值和误差值两部分组成，即
(4)
这样可以保证式(13)的约束能反馈给僚机，抑制僚机滤波状态估计误差发散。
诗人在第2小节想要表达的乡愁是自己对新婚妻子的无比思念，并通过“乡愁是一张窄窄的船票”体现出来。如图2所示，“乡愁是船票”乍看起来也是典型的“A为B”隐喻结构，其中目标域为抽象的“乡愁”，而要理解这一抽象概念的语义我们只能通过具体的源域“船票”来实现。但是，当我们进行由“船票”向“乡愁”的跨域映射理解时，我们却无法直接获得“乡愁”的语义解释，因为源域“船票”不是隐喻的喻体，而是转喻的喻体。所以，对“船票”作为转喻喻体的解读就成为理解这一隐转喻结构的关键。如前所述，转喻映射是在ICM框架内发生的，因此，要知道“船票”具体指代什么就需要弄清楚它所在的ICM。
(5)
由于系统模型是在n 系下建立的,而相对导航的测量是以机体坐标系(b 系)为基准，因此需要对相对导航坐标转换。在b 系中，僚机测量的相对导航信息存在误差，通过激光传感器测得的相对距离、仰角和方位角均可分为真实值与误差值两部分，将如图2所示沿着b 系三个坐标轴方向分解，有
b 系到n 系的变换矩阵[6]为
(6)
式中，Z G =0，V G =0，H G = [I 3×303×15I 3×303×15I 3×303×15]。
图2 僚机相对速度(位置)测量示意图
假设相对导航测量误差比较小，则有cos(ω α )≈1、cos(ω β )≈1、sin(ω α )≈1、sin(ω β )≈1，忽略高阶小量，即φ v ω α ≈0、φ v ω β ≈0、φ α ω β ≈0，式(6)可以写成以下形式：
(7)
式中,
除了上述问题，现实中典型的弄虚作假现象还有扶贫领域虚报统计数据，搞数字脱贫；工作中欺上瞒下、报喜不报忧；党员干部学历造假、年龄造假、档案造假……这些问题，严重妨碍了党的路线、方针、政策的贯彻执行，也许能蒙混一时，但蒙混不了一世，迟早会被拆穿。
(8)
式中，γ 、θ 、φ 分别为僚机真实横滚、俯仰和航向角，由于惯性导航系统解算的姿态角误差极小，用惯性导航系统解算出的姿态角可以近似替代真实姿态角，即
(9)
则由式(7)可以得到相对导航在n 系的误差值为
Economic Policy Uncertainty and Growth Divergence between State-owned Investment and Private Investment in China
(10)
时间更新
(11)
结合式(4)、(5)、(10)和(11)，僚机i 量测方程为
Z i (t )=H i (t )X i (t )+V i (t )
(12)
式中,
2.2 伪长机部分系统模型
伪长机滤波器的状态量是各编队成员状态的组合
由INS的误差服从正态分布特性可知，多套INS系统在同一位置工作时通过对其输出加权平均可以提高定位精度。
(13)
其中，
和
表示编队成员各自的SINS解算误差，
和
表示权重。由于本文中僚机i 与伪长机g 搭载的SINS性能相同，因此
涂鸦期是1岁半到4岁左右的幼儿在没有明确的绘画目的的情况下，以游戏的形式，不受或少受视觉控制，随意进行的画线活动。如，在绘本《跟着线走》《跟着线走过房子》《跟着线环游世界》中，作者用一根线创作出简单明了、细节丰富、富有想象、充满童趣的一系列画面，教师可以以此作为幼儿绘画涂鸦活动的媒介，用深受幼儿喜爱的故事游戏形式，引导幼儿带着“画笔”一起畅游在“线条”的奇妙世界里。
依据式(13)可以构造伪长机的量测方程:
式中，i，t分别表示上市公司和年份，t-1表示滞后一期；Year，Quarter和Industry分别为控制年份、季度与行业的哑变量；Ece为企业家信心指数，衡量的是企业家对下一季度的经济预期，因此以滞后一期的企业家信心指数作为解释变量进行实证研究；资产负债率Lev、净资产收益率Roe、现金持有Cash、资产规模Size、经营性现金流Cfo为公司的财务指标；管理费用Adm和第一大股东持股比例Top1为公司的治理因素。这些均为影响上市公司投资效率的控制变量。
Z G =H G ·X G +V G
(14)
式中，各种误差值均假定为期望是零的高斯白噪声过程，方差分别为
和
3 滤波算法
机群编队协同导航系统的联邦滤波器由僚机和伪长机滤波器两部分组成，滤波算法的运行流程如图3所示。
图3 滤波算法流程
1) 状态重置
利用伪长机滤波器状态估计重置各僚机滤波器的状态量，即
(15)
结合式(3)和式(4)可得僚机状态量与量测值的关系为
2) 僚机滤波器时间更新和量测更新
这段历史让NIH前院长哈罗德·瓦默斯（Harold Varmus）对2025年的目标持有谨慎态度。他说：“没有人否认对阿尔茨海默病的研究亟须取得进展，但是我希望不要对其限定日期。”
各僚机部分滤波器独立地进行时间更新和量测更新。
INS输出的三维位置信息为经、纬、高，相对导航传感器输出的三维位置信息为三个方向的距离，常规单位是m ，为了与相对导航距离信息量纲匹配，对INS输出的经度和纬度进行了转换
(16)
(17)
量测更新
(18)
(19)
选作伪长机的僚机i 所对应的D i 应满足以下关系：
建立平等的师生关系，实现师生的有效对话，以合作探索为方式，人人参与竞争合作。这样的课堂氛围即使是学困生也会积极参与到教学活动之中。老师的一个微笑，同学的一阵掌声都可能使任何一个学生敢于在课堂上发表自己的见解。久而久之，学困生也会积极主动地去学习。
在购买肉时，如果想一次多买点，可以在买时让营业员把肉按要求分成若干份，比如一家4口人，如果平均每人100克肉一餐，在买时就按400克一份包装，买回家后放在冰箱的速冻保存，要吃时取一份出来就是400克，平均每人1/4，这样就可以比较准确地控制食物的量。这样也可以避免由于冷冻食品反复解冻和冷冻而影响口感。
(20)
3) 信息融合
利用联邦卡尔曼滤波中公共状态估计的融合算法[7]，将各僚机部分滤波器得到的伪长机状态估计和其对应的协方差阵融合，得到除长机部分滤波器以外的全局估计。
(21)
(22)
其中
和P ij 表示滤波器i 中成员j 部分的状态估计及其对应的协方差矩阵，下同。
4) 伪长机部分波器量测更新
伪长机部分滤波器不进行时间更新，状态一步预测和协方差一步预测值由僚机滤波器状态估计提供，并且伪长机状态部分由融合后的结果替换。
(23)
(24)
由于长机部分滤波器不进行时间更新，因此其无需估计均方误差。这样主滤波器只需计算简化后的滤波增益和状态估计，即使主滤波器包含所有状态也能保证较低的计算量。
(25)
(26)
4 仿真分析
为了验证本文提出算法的有效性，进行了计算机数字仿真分析。在仿真计算中惯性导航和相对导航设备相关参数如表1所示。飞行航迹包括起飞、爬升、转向、匀速直线运动和降落等状态，仿真时间1 000 s。以北向为例，某一次仿真实验中3架飞机INS解算误差和滤波估计误差如图4所示。
电费：根据设备运行管理经验，每例检查一般消耗电费10元左右（含待机电费分摊），根据设备使用率提高，单位消耗电费将逐渐降低，本次分析按照单位电费10元测算。
表1 INS和相对导航传感器的相关误差参数
图4为长机故障后编队各成员仅依靠自身惯性导航系统和采用本文所提协同导航算法后的北向定位误差。可以看出利用所提算法，各成员定位误差明显小于仅依赖INS时的误差，即协同定位误差的发散速度明显小于独立定位时发散速度。但图4仅为INS误差有正有负时的情况，若所有编队成员INS误差均为正或负，则最后结果只能说明协同定位使得各成员位置估计误差圆半径介于INS解算误差圆半径的最大值与最小值之间。即便如此，对于机群来说，其整体定位误差也优于各成员独自定位。
图4 编队各成员北向INS解算和滤波估计误差
由图4还可以看出，各成员协同定位估计误差曲线基本重叠，说明通过协同定位算法各成员位置估计误差大小极为接近。图5协同定位误差曲线局部放大进一步说明了这个结论。表2列出了仿真最后一步3架飞机INS解算误差值和协同定位估计误差值，直观印证了上述结果。
1）覆盖系统。填埋作业区的日覆盖采用0.3mm浸塑布进行覆盖，完成阶段性填埋的区域采用优质1.0 mm HDPE膜进行中间覆盖。中间覆盖采用优质HDPE膜可以减少后期的维护工作量，减少臭气外逸和雨水进入；中间覆盖表面压载沙袋，并用绳网呈网格状整齐连接，防止在台风或暴雨天气时造成沙袋位移，进而导致位移部位的膜面破损，还可以呈现出规则的美感。此外，沙袋为柔性材料，不会对1.0 mm HDPE膜造成损伤；中间覆盖的边界，如永久道路、垃圾转运而临时修建的砖渣路要采用锚固沟的方式进行锚固；为防止垃圾对1.0 mmHDPE膜的穿刺，垃圾表面须先覆盖0.3mm浸塑布。
本文以3架飞机为例进行算法仿真验证，由于INS解算误差有正有负，随着编队成员数量增加，算法抑制定位误差发散的效果会越来越显著。
图5 编队各成员北向滤波估计误差局部图
表2 编队各成员INS解算和估计误差
5 结论
1) 在建立协同导航系统模型的基础上，提出了基于同性能INS误差特性的机群编队协同定位算法。
2) 该算法对原有主从式协同导航模型改动较小，使机群编队故障前后能够迅速反应，保证了编队系统的稳定性。
3) 计算机仿真结果表明各成员定位估计误差为所有INS输出误差均值，算法起到了抑制编队整体定位误差发散的作用。
参考文献：
[1] 潘瑞鸿,徐胜红.基于几何特性的多无人机协同导航算法[J].兵器装备工程学报,2017(10):55-59.
[2] 林伟,邹俊锋,占明锋.CEC/SINS组合的相对导航与定位算法[J].海军工程大学学报,2011,23(1):51-57.
[3] 刘俊成,张京娟,冯培德.基于相互测距信息的机群组网协同定位技术[J].北京航空航天大学学报,2012,38(4):541-545.
[4] 冯培德,谢淑香,张京娟.机群组网定位的一种新途径[J].北京航空航天大学学报,2006,32(11):1263-1267.
[5] 张共愿,程咏梅,程承,等.基于相对导航的多平台INS误差联合修正方法[J].航空学报,2011,32(2):271-280.
[6] 周姜滨,袁建平,罗建军,等.基于联邦滤波的SINS/GPS全组合导航系统研究[J].系统仿真学报,2009,21(6):1562-1564.
[7] CARLSON N A.Federated square root filter for decentralized parallel processors[J].Aerospace & Electronic Systems IEEE Transactions on,1990,26(3):517-525.
Flight Formation Collaborative Localization Algorithm Under the Condition of Leader Fault
LIU Xiaoyanga， XU Shenghongb
(a.Graduate Management Team; b.Department of Control Engineering, Naval Aeronautical University, Yantai 264001, China)
Abstract : A cooperative positioning scheme based on federated filtering was proposed. A two-stage Kalman filter structure was adopted, and a system mathematical model was established based on the conversion of the relative navigation coordinated system and the INS position output of the followers. Finally, the federal filtering algorithm was used to estimate the positioning error. The simulation results show that the cooperative positioning algorithm based on federated filtering can effectively suppress the rapid divergence of formation positioning errors after leader fault.
Key words : flight formation; localization error; collaborative localization; Kalman filtering
本文引用格式 ：刘晓洋,徐胜红.一种长机故障情况下机群编队协同定位算法[J].兵器装备工程学报，2019,40(2):136-140.
Citation format :LIU Xiaoyang，XU Shenghong.Flight Formation Collaborative Localization Algorithm Under the Condition of Leader Fault[J].Journal of Ordnance Equipment Engineering,2019,40(2):136-140.
中图分类号 ：V249.3
文献标识码： A
文章编号： 2096-2304(2019)02-0136-05
收稿日期 ：2018-09-17；
修回日期： 2018-10-27
基金项目 ：国家自然科学基金项目(61203168)；总装预研基金项目(9140A090401)
作者简介 ：刘晓洋(1993—)，男 ，硕士研究生，主要从事飞行器综合导航研究。
通讯作者 ：徐胜红(1974—)，男 ，副教授，博士，主要从事导航和制导与控制研究，E-mail:xusirsohu@sohu.com。
doi: 10.11809/bqzbgcxb2019.02.028
(责任编辑 杨继森)
标签：机群编队论文; 定位误差论文; 协同导航论文; 卡尔曼滤波论文; 海军航空大学研究生管理大队论文; 海军航空大学控制工程系论文;