基于UWB/INS的单兵班组协同定位算法研究

晁正正，马喜宏，李小燕，吕 辰，朱孟龙

(中北大学 电子测试技术科技重点实验室，山西 太原 030051)

摘 要： 针对单兵作战系统易受室外GPS(Global Positioning System)信号遮挡失锁难以定位与INS自主定位容易发散的问题，提出一种基于单兵之间相互测距信息的EKF(Extended Kalman Filter)班组协同定位算法.利用角速度均方根阈值判别法进行单兵零速修正，UWB(Ultra Wideband)进行相互测距的同时传输协作单元位置估计和协方差信息，通过构造距离量测方程在零速区间协同定位.实测试验对比单兵INS定位算法与基于相互测距信息的EKF班组协同定位算法，试验结果表明：基于UWB/INS的单兵班组协同定位算法明显优于单兵INS算法自主定位，其位置误差减小了一个数量级，有效抑制了单兵INS定位误差累积.该算法可以在我军反恐作战、抢险救灾等极端环境下单兵班组协同定位中使用.

关键词： 单兵协同导航；UWB测距；零速检测；EKF算法；轨迹修正

0 引 言

在未来陆军作战中，一套准确可靠的士兵定位系统可以显著提高军事人员的安全.单兵定位系统应该具有重量轻、体积小、价格便宜等优点，不仅能在户外使用而且在室内定位中仍能提供米级精度.过往士兵携带INS/GPS的组合定位系统因GPS受干扰和作战时不能用从而具有局限性，传统的单兵INS自主定位难以精确获取士兵位置信息.因此提出一种协同定位方法，利用班组间信息交互原则，共享组间单兵测量信息，使各成员获取相比于单兵自主定位精度更高的位置信息.

在协同定位方面，国内外众多学者做了大量研究工作.杨建通将GPS作为辅助导航手段,通过松组合和紧组合的方式来构建导航系统，解决了INS误差随时间积累的问题，缺点是当GPS信号失锁时定位误差容易发散^[1]；穆叡对行人步行时物理规律进行深层探索，并将其应用在定位导航模型中，提升定位导航精度以及利用智能手机中内置的惯性测量单元采集惯性数据，不需要固定惯性测量单元，在多种行为模式下进行了行人室内航迹推算，缺点是INS定位误差随时间累积增大^[2]；Ioannis M.Rekleitis等人在多机器人中首次提出协同导航，利用粒子滤波的同时考虑不同观测信息对协同定位精度的影响，其缺点是机器人所携带GPS容易受到楼层遮挡影响定位精度^[3].以上均不能满足GPS信号受干扰与具有INS累计误差情况下的单兵定位.

针对上述问题，本文提出一种基于相互测距信息EKF的单兵班组协同定位算法，利用鞋式INS系统检测步态信息进行零速修正，UWB进行相互测距的同时传输协作单元位置估计和协方差信息协同定位，提高了两人各自定位精度.

我相信，善于发现欣赏他人之美，会促进人际关系，也会见贤思齐，促使自我不断成长，会让自我的幸福指数提升。这也是我真切关注学生真实内心的一种方式。

1 鞋式惯导和超宽带测距系统

1.1 鞋式惯导系统

鞋式导航算法^[4-6]是基于IMU(Inertial measurement unit)数据提供单兵三维位置、速度和姿态信息.在每一个步态周期，基于零速修正方法来重置IMU漂移，并限制惯导误差增长.该导航算法为基于15维状态量的EKF算法，进而估计IMU传感器的三维位置、速度和姿态，以及加速度计零偏和陀螺仪零偏6种IMU误差状态量.

由于样品前处理添加水的体积和提取时加的乙酸乙酯的量对氯霉素的提取浓度有影响，本实验利用两因素三水平实验，研究这两种因素对实验结果的影响，结果分析如表1所示。

鞋式载体坐标系b 相对于地理坐标系n 的方向余弦矩阵由补偿后的陀螺角速率ω ^b 更新

(1)

鞋式传感器在地理坐标系中的速度v ⁿ 和三维位置x ⁿ 在已知初始载体位置和速度信息下，利用零点补偿过的加计测量值a ^b 得

(2)

x ⁿ (t _k+1 )=x ⁿ (t _k )+v ⁿ (t _k+1 )Δt ,

(3)

|d |=c ·T _TOF.

图1 鞋式惯导系统架构框图

Fig.1 Frame diagram of shoe inertial navigation system

1.2 零速更新

1.2.1 零速检测算法

选取扩展卡尔曼滤波器状态量为X =[(φ ⁿ )^T(δv ⁿ )^T(δx ⁿ )^T(ε ^b )^T(^b )^T]^T,其中φ ⁿ 为姿态误差角；δv ⁿ 为速度误差；δx ⁿ 为位置误差；ε ^b 为陀螺仪常值漂移；^b 为加速度计零偏值.因此扩展卡尔曼滤波器系统方程为

在每一步态周期中，鞋式惯导系统暂时处于静止状态，速度为零可以作为扩展卡尔曼滤波器中重置状态误差的测量量^[7-9].因此本文提出一种基于陀螺仪模值均方根阈值的零速检测算法.具体方法是：设置固定时间窗口宽度为N 个采样点，在惯性器件输出的每个离散时间t ₁,t ₂,…,t _k-1 ,t _k ,…中计算t _n 时刻陀螺仪模值均方根为

(4)

当S _ωk ≤ThresholdS _ωk 时，即判断为鞋式惯导系统处于零速状态，反之即为运动状态.阈值的选取与惯性器件噪声特性有关，需要实验前对其噪声进行分析.区间长度参数M ₁和M ₂与惯性输出频率有关，试验采用IMU输出频率为100 Hz，经过多次试验测试设定M ₁=M ₂=2,N =5,ThresholdS _ωn =0.1 rad/s.图2 为IMU陀螺仪模值均方根，“”线段为零速片段，可见算法可以检测出步态静止阶段，算法是有效的.

图2 零速检测结果

Fig.2 Zero-speed detection results

UWB传感器测量存在测量误差，本文将其建模为白噪声e _d .试验测试时UWB传感器安装在单兵的右肩部位，INS安装在单兵的右脚.目前还没有对传感器的不同位置进行建模，只能通过增加测量距离噪声的范围来处理.对于本文实验，选择距离测量协方差R _d =1 m².

家长作为孩子的第一任老师，必须要积极发挥教育作用，转变传统的家庭观念，与孩子保持密切的沟通和交流，做到精心呵护和全面关怀，不仅关心子女的生活和学习方面，也要着重关心子女的思想情感和心理健康等方面，重视孩子的健康成长。家庭教育会伴随孩子的一生，应增加家庭教育指导，增强家长的责任意识，提高他们的监护能力。要给孩子提供更多的锻炼机会，发挥其聪明才智，不能凡事都包办，以免打消孩子的积极性。还要正确评价孩子，以正面教育为主，出现错误不要一味批评，做到具体问题具体分析，产生“润物细无声”的教育效果。

许多时候常爱兰都觉得很恍惚，似乎一切都是陌生的。几年下来，这个叫寿包子的男人是让人陌生的，酗酒打架赌博样样都来，把自己与小羽也打得好不了伤疤忘不了痛；而这个小羽呢，却似乎是从生下来就没有过好脾气，从小哭到大，一直到寿包子死掉，然后突然就不哭了。整个人一天到晚也不说什么话；来到岭北镇，这个被人叫麻糍的男人却是个可以随手被人捏被人捣的软麻糍，这个真名叫周财富的男人，不仅没什么财富，却被叫成了麻糍。这一切的一切似乎都陌生得很。

(5)

扩展卡尔曼滤波器的观测量选取为速度误差和距离测量值，即

(6)

式中：

可以看出，误差项是e _x1 在d 方向上的分量.其中x ₁和x ₂是单兵的位置信息.距离量测方程为

1.3 超宽带测距系统

超宽带(UWB)测距系统以其穿透力强、功耗低、抗多径效果好、安全性高的优势，通过发送和接收ns或ns级以下的极窄脉冲来传输数据，从而具有GHz量级的带宽.这使得距离测量在视距条件下具有cm级精度.本文选取TOF原理测距，将发送端发出的数据包和接收回应的时间间隔记为T _TOT,接收端收到数据包和发出回应的时间间隔记为T _TAT,则数据包在空中单向飞行的时间T _TOF和两点距离|d |为

T _TOF=(T _TOT-T _TAT)/2,

(7)

式中：g ⁿ 为只有向下分量的重力矢量；Δt 为积分时间.鞋式惯导系统初始滚转角和俯仰角由静止期间加计粗略校准得到，加速度计和陀螺仪零偏为静采数据均值，假定初始航向角已知.鞋式惯导系统架构和内部流程如图1所示.

初期研究主要围绕MOOC的特点、MOOC对于信息共享和教育公平的意义、MOOC环境下图书馆及图书馆员的应对策略等宏观内容展开笼统分析。随后，研究内容逐步扩展到对MOOC环境下图书馆是否应进行功能重构、应如何开展功能重构，图书馆利用MOOC开展服务创新的路径与措施，MOOC课程制作周期及图书馆在MOOC课程制作过程中的作用，MOOC学习规律及图书馆在MOOC学习过程中的作用，MOOC学习者学习行为的搜集、研判与利用，MOOC平台在线交流技术支持与优化，MOOC发展与推广过程中的政策支持与法律环境完善等问题进行深入剖析。

(8)

虽然比较文学有一定的难度，但是，因为它特有的开放的视野和比较的意识，的确能帮助选修华文和文学课程以及双文化课程的学生们有效地阅读，多元地思考，从而提升他们从不同文化场域进行文学欣赏和理性思考的能力。

图3 DW1000静态测距

Fig.3 DW1000 static ranging

2 UWB/INS的单兵班组协同定位

UWB测距系统可测量在检测范围内单兵之间的距离.用于单兵之间相互通信的估计位置和协方差矩阵信息与测距信息相关，而距离信息又可作为扩展卡尔曼滤波器中的附加测量提高单兵班组定位精度^[10-13].

在扩展卡尔曼滤波中形成一个单兵的距离测量方程时，需要协作单兵的位置信息，但只知道其估计值误差协方差为其单兵之间真正的距离可以用关于的一阶形式表示

(9)

由于滤波器观测量只在鞋式惯导检测静止时才能获取，并且观测噪声与具体运动和测距值噪声相关.因此在时刻t _k 进行扩展卡尔曼滤波更新时，若检测状态为静止，则触发零速修正的扩展卡尔曼滤波算法进行时间更新和量测更新；如检测状态为运动状态，则只进行时间更新.

(10)

其中总量测协方差为单兵协同单元预测协方差P _x1 在视线向量上的投影和UWB传感器噪声协方差之和，即

这个夜晚对甲洛洛来说太不寻常了，他几度大起大落，几度悲喜交集，最后几乎找到了四十年前单相思的那种无法制控的激情和绝望。他躺在床上一动不动，当晨光映亮窗户上那一朵褪色的大牡丹时，他终于想透了：其实年岁只能左右身体，根本把控不到情感，如果有谁觉着年老而无法涉足情爱，那一定是这个人的身子活着，而心死了。

(11)

1.2.2 零速更新

通过距离测距1 m验证DW1000测距系统的精度，实验数据分析如图3所示，其噪声水平(标准差)为3.55 cm.最大测距值与最小测距值之差为19 cm，最大测距值与均值之差为9.08 cm，最小测距值与均值之差为9.92 cm.达到芯片测距精度10 cm以内，其精度满足单兵协同定位时测距条件.

3 试验及结果分析

3.1 硬件介绍

本文INS系统由ADXL354B的MEMS三轴加速度计和单个CRM100及包含两个CRM200的三轴陀螺仪及PNI公司的磁传感器RM3100模块构成.三轴加速度计属于低噪声密度和低功耗，支持±2 g和±4 g范围，本文选用±4 g量程.CRM100/200具有小尺寸、低功耗及输出信号稳定的特点.陀螺仪采用了贴片形式的LCC封装，零偏稳定性为24°/h，角度随机游走为0.28°/h，选用±300°/s量程 ^[14].RM3100磁传感器模块的2个X /Y 轴磁传感器，±8gauss的量测和26 nT的分辨率保证了航向测量的精确性.UWB测距系统采用DW1000芯片，其符合IEEE802.15.4-2011超宽带标准，最小误差在10 cm 以内.DW1000的最远传输距离为450 m，具有芯片功耗低可双向测距等特点，满足单兵协同需求，硬件模块如图4所示.

图4 INS系统和UWB测距系统

Fig.4 INS system and UWB ranging system

3.2 试验验证

试验地点为中北大学14号楼后侧，选取如图5所示曲线路线行走.试验由两人分别右脚携带鞋式INS系统与右肩携带UWB测距装置进行测试.第二单元距离第一单元左前方6 m，第一单元所处为原点.开始点和停止点在几厘米以下认为相同.每个单元携带高精度GPS定位模块实现单兵真实轨迹定位.两单元均静止10 s,然后沿曲线路线同时行走，通过Honor手机APP统计两单元行走步数，并与零速检测算法检测结果对比，试验结果表明算法能够准确检测出两个单元行走步数.结果如表1所示.

图5 协同单元行走示意图

Fig.5 Walking diagram of collaborative unit

表1 行走计步统计

Tab.1 Statistics of walking steps

主控通过SPI模式将INS系统中三轴加计和三轴陀螺数据采集存储到SD卡中，UWB测距系统通过串口接到笔记本上存储数据.本文研究了INS算法和基于UWB/INS的零速修正EKF单兵协同导航算法.下面将比较不同算法的导航定位结果.

图6 INS解算轨迹

Fig.6 INS solving trajectory

图7 协同导航

Fig.7 Collaborative navigation

从图6 中可以看出，仅使用INS的解决方案，受其累计误差和航向角偏差影响，两个单兵单元轨迹发散很快，轨迹与真实行走路线相差甚远.而由图7 所知，使用基于UWB/INS的零速修正EKF协作导航定位算法解算，通过使用零速检测算法检测零速区间，结合UWB测距信息进行EKF的量测更新，解算轨迹与真实两单兵单元行走轨迹基本一致，证明了协同导航算法的有效性.

若按右幅7-2#裂缝产状及发展趋势拟合，则裂缝将从右幅7-3#边缘通过，具有渗透条件。右幅7-3#于5月27日浇筑，右幅7-2#于5月31日开始开挖，具有渗透时间差。从图7所见的水泥浆充填情况分析，趋势拟合是合理的。

通过图8 和图9、表2 可知，通过协同导航，图6 中东向和北向位置误差显著降低，第一单元和第二单元位置误差分别从11.58 m和15.19 m降低到1.36 m和2.18 m，其误差减少一个数量级，表明协同导航能有效抑制INS自主导航定位的累计误差.

图8 第一单元位置误差

Fig.8 Location error of the first unit

图9 第二单元位置误差

Fig.9 Location error of the second unit

表2 两单元位置误差均值距离

Tab.2 Mean distance of position errors of the two units

对比图6 和图7，基于UWB/INS的单兵单元协同导航定位结果显著改善的3个原因为:

1) 因为两个单兵单位一前一后几乎总是在视线之内.因此，单元之间的距离测量常常是可用的.

国内外蓄热氧化换向流动反应器的研究，主要包括数学模型及验证、试验研究、反应机理简化、反应动力学、蓄热床层阻力、甲烷转化率、反应器运行稳定性、蓄热材料热疲劳及反应器设计方法等方面，不一而足。而通过不同研究的时间区间，亦不难得出该项技术的研究历程，如图2所示。

2) 位置估计的准确性随路径而变化.有时第二单元的精度要比第一单元的精度好，几十秒后，第一单元具有最高的精确度.因此，这些单兵单元可以相互支持.

3) 通过零速修正以及距离信息的约束，可以克服单兵轨迹误差漂移及有效修正航向角误差.

4 结束语

本文通过建立鞋式惯导系统方程，提出了一种基于陀螺仪模值均方根阈值的零速检测方法，相比其他检测算法，具有简单高效、减少主观标定和阈值调整的优点.通过分析UWB测距原理，提出一种基于UWB/INS的零速修正EKF算法，量测方程结合协作单元位置及距离协方差从而进行EKF更新.试验结果表明，两个单兵单元轨迹解算结果显著提高，位置误差能减小一个数量级，有效地抑制了单个单兵独立解算的发散性，表明该算法在单兵协同导航定位中具有一定参考价值.

在未来，为了得到更加优良的单兵协同导航结果，可以进行进一步研究：① 引入更真实的超宽带测距传感器误差模型，使模型结合IMU和超宽带天线之间的距离模型；② 增加积分阶数(目前使用的是一阶欧拉积分)；③ 将GPS(户外使用)和磁力仪集成到现有导航滤波器中；④ 当单兵行走时间较长时，补偿地球自转；⑤ 对比引入多个协作单元后的效果.

参考文献：

[1] 杨建通.基于MIMU/GPS紧组合的单兵导航算法研究[D].哈尔滨：哈尔滨工程大学，2016.

[2] 穆叡.基于惯性传感器的行人室内定位导航[D].成都：电子科技大学，2018.

[3] Rekleitis I M,Dudek G,Milios E E.Multi-robot cooperative localization:a study of trade-offs between efficiency and accuracy[C].International Conference on Intelligent Robots and Systems.Lausanne,Switzerland: IEEE,2002: 2690-2695.

[4] 张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统[J].中国惯性技术学报，2011，19(3)：253-256.

Zhang Jinliang,Qin Yongyuan,Mei Chunbo.Shoe-mounted personal navigation system based on MEMS inertial technology[J].Journal of Chinese Inertial Technology,2011,19(3)：253-256.(in Chinese)

[5] 崔潇，秦永元，周琪，等.鞋式个人导航系统算法和试验研究[J].测控技术，2013，32(3)：138-142.

Cui Xiao,Qin Yongyuan,Zhou Qi,et al.Algorithm and test research on shoe-mounted personal navigation system[J].Measurement & Control Technology,2013,32(3)：138-142.(in Chinese)

[6] 陈泽，吴美平，陈昶昊.基于距离信息的行人协同导航研究[J].导航与控制，2017，16(1)：5-10.

Chen Ze,Wu Meiping,Chen Changhao.The research of cooperative pedestrian navigation based on distance information[J].Navigation and Control,2017,16(1)：5-10.(in Chinese)

[7] 韩勇强，田晓春.基于足部安装MIMU的行人导航系统设计[J].导航定位与授时，2018，5(1)：22-27.

Han Yongqiang,Tian Xiaochun.Design of pedestrian navigation system based on foot mounted MIMU[J].Navigation Positioning and Timing,2018,5(1)：22-27.(in Chinese)

[8] 田晓春，陈家斌，尚剑宇，等.基于自包含传感器的单兵导航系统设计[J].导航定位与授时，2017，4(5)：54-59.

Tian Xiaochun,Chen Jiabin,Shang Jianyu,et al.Design of individual soldier navigation system based on self-contained sensors[J].Navigation Positioning and Timing,2017,4(5)：54-59.(in Chinese)

[9] 许晓伟，赖际舟，吕品，等.多无人机协同导航技术研究现状及进展[J].导航定位与授时，2017，4(4)：1-9.

Xu Xiaowei,Lai Jizhou,Lv Pin,et al.A literature review on the research status and progress of cooperative navigation technology for multiple UAVs[J].Navigation Positioning and Timing,2017,4(4)：1-9.(in Chinese)

[10] Rantakokko J,Strömbäck P,Andersson P.Foot-and knee-mounted INS for firefighter localization[C].Ion International Technical Meeting.USA: ResearchGate,2014: 230-239.

[11] Rantakokko J，Händel P,Fredholm M，et al.User requirements for localization and tracking technology: A survey of mission-specific needs and constraints[C].International Conference on Indoor Positioning & Indoor Navigation.Zurich,Switzerland: IEEE,2010: 110-119.

[12] Berefelt F,Boberg B,Nygärds,et al.Collaborative GPS/INS navigation in urban environment[J].Ion National Technical Meeting,2004,5(2)：120-125.

[13] Olsson F,Rantakokko J,Nygards J.Cooperative localization using a foot-mounted inertial navigation system and ultrawideband ranging[C].International Conference on Indoor Positioning & Indoor Navigation.Busan,South Korea: IEEE,2015:122-131.

[14] 吕辰.基于MARG无人机姿态测量系统的实时融合算法研究[D].太原：中北大学，2018.

Research on Cooperative Location Algorithm of Individual Team Based on UWB /INS

CHAO Zhengzheng，MA Xihong，LI Xiaoyan，LV Chen，ZHU Menglong

(Science and Technology on Electronic Test & Measurement Laboratory,North University of China,Taiyuan 030051,China)

Abstract : Aiming at the problem that the individual combat system was easy to lose lock because GPS signal was shaded outdoor and INS autonomous positioning was easy to diverge,an EKF team cooperative positioning algorithm based on ranging information between individual soldiers was proposed.Root mean square threshold test of angular velocity was used to perform single soldier zero speed correction.when performing mutual ranging,UWB transmits the cooperative unit position estimation and covariance information,through constructing the distance measurement equation,single soldier cooperatively located in the zero-speed interval.The measured results were compared with the individual INS positioning algorithm and the EKF team cooperative positioning algorithm based on mutual ranging information.The experimental results show that single team cooperative positioning algorithm is obviously better than the individual INS algorithm,the error is reduced by an order of magnitude,effectively suppressing the accumulation of individual INS positioning errors.The algorithm can be used in the cooperative positioning of individual teams in extreme environments,which conclude fight against terrorism and rescue and relief work.

Key words : individual soldier cooperative navigation; UWB ranging; zero speed detection; EKF algorithm; trajectory correction

中图分类号： V249.32

文献标识码： A

doi： 10.3969/j.issn.1671-7449.2019.05.007

文章编号: 1671-7449(2019)05-0406-06

收稿日期： 2019-03-21

基金项目： 国家自然科学基金资助项目(51375463)

作者简介： 晁正正(1993-)，男，硕士生，主要从事地磁导航与组合导航方面的研究.

标签：单兵协同导航论文;  UWB测距论文;  零速检测论文;  EKF算法论文;  轨迹修正论文;  中北大学电子测试技术科技重点实验室论文;