基于神经网络与自调节卡尔曼滤波的超宽带定位算法研究

古玉锋　杜雨洁　王育阳　李昆鹏　黎程山

摘要：提出了一種基于神经网络与自调节卡尔曼滤波的超宽带（UWB）定位算法，以改善目前某三线自动驾驶轨道交通系统车辆定位精度不够高的现状。使用UWB标签和基站采集大量标签与各个基站的距离信息及对应标签的实际位置训练神经网络。在实时定位阶段，标签与各个基站的距离信息经网络发送至集中控制中心的服务器，通过优化后的神经网络得出实时的UWB定位标签的位置，对实时得到的标签位置使用自调节卡尔曼滤波以进一步提高精度。根据实车运行情况设计了一组包含斜道、直道和弯道的UWB标签移动轨迹进行仿真，并搭建UWB定位系统，设计标签的行驶轨迹，对神经网络与自调节卡尔曼滤波结合的UWB定位算法进行实验验证。结果表明：神经网络与自调节卡尔曼滤波结合的定位算法最大定位误差为223.58 mm，平均定位误差为43.16 mm，定位误差均方根值为42.06 mm。提出的神经网络与自调节卡尔曼滤波结合的定位算法相较于三点定位算法、卡尔曼滤波算法和神经网络算法，具有精度高、实时性好及稳定性高的优点，能够满足目前该三线轨道交通的定位要求。

关键词：超宽带；神经网络；自调节卡尔曼滤波；定位算法

中图分类号：U285.2

DOI：10.3969/j.issn.1004-132X.2023.12.013

Study on UWB Positioning Algorithm Based on Neural Networks and Self-adjusting Kalman Filters

GU Yufeng DU Yujie WANG Yuyang LI Kunpeng LI Chengshan

Abstract： A UWB positioning algorithm was proposed based on neural networks and self-adjusting Kalman filters for improving the positioning accuracy of current a certain three-line automatic driving rail transport system vehicles. The UWB tags and base stations were used to collect large amount of distance information between tags and various base stations and collect the actual locations of the corresponding tags， and the neural network was trained. The distance information between the tags and various base stations was sent to the centralized control center server through the network during the real-time positioning stage， and the real-time locations of the UWB positioning tags were obtained by the optimized neural network. The self-adjusting Kalman filter was used to improve the accuracy of the real-time tag positions furtherly. A set of UWB tag moving trajectories containing inclines， straight paths， and curves were designed for simulation based on the actual vehicle operation， and a UWB positioning system was built， the moving trajectories of the tags were designed， the UWB positioning algorithm combining the neural network and self-adjusting Kalman filter was verified through experiments. The results show that the maximum positioning error of the positioning algorithm combining neural network and self-adjusting Kalman filter is as 223.58 mm， and the average positioning error is as 43.16 mm， and the root mean square value of the positioning errors is as 42.06 mm. The positioning algorithm proposed combining the neural network and self-adjusting Kalman filter has the advantages of higher accuracy， better real-time performance and stability compared with the three-point positioning algorithm， Kalman filtering algorithm， and neural network algorithm， and the current positioning requirements of the three-line rail transports may be fulfilled.

Key words： ultra-wide band（UWB）; neural network; self-adjusting Kalman filter; positioning algorithm

0 引言

高精度定位是自动驾驶轨道交通的基本要求［1-2］。传统的全球定位系统（global positioning system， GPS）和北斗卫星导航系统在平坦空旷地带定位精度较高，但在隧道及室内定位精度显著下降，甚至出现没有定位信息的情况［3-5］。超宽带（ultra-wide band， UWB）技术具有传输速度快、硬件成本低、系统功耗小及穿透能力强的优点，十分适合于高精度定位［6-7］。UWB定位主要由UWB标签和UWB基站两部分构成，定位标签和定位基站通过飞行时间（time of flight， TOF）测距法测得距离信息，将至少三个以上的距离信息进行解算得到定位标签的位置［8-9］。TOF测距方法属于双向测距技术，受硬件设备和测距环境的影响，一般会产生硬件设备误差、非视距环境误差和多径效应误差［10-11］，这些误差会导致UWB定位的精度下降，因此，需要提出一种高效简便的算法以减少上述误差对定位结果的影响。

基于神经网络与自调节卡尔曼滤波的超宽带定位算法研究——古玉锋 杜雨洁 王育阳等

[SM（]中国机械工程 第34卷 第12期 2023年6月下半月[SM）]

在UWB的应用研究中，爱尔兰公司Decawave是全球唯一的超宽带定位芯片供应商，目前广泛使用的DW1000即是该公司在2014研发的产品［12］。文献［13］提出了一种基于三边定位算法的位置解算方法，三边定位算法受噪声、多径效应等因素影响，测距不可避免地存在误差，导致3条圆曲线不会相交于一点，误差较大。文献［14］利用三边定位原理解算位置坐标，结合卡尔曼滤波算法提高定位精度，但该方法应用场景有限且定位精度改善效果不明显。文献［15］通过卡尔曼滤波提高测距数据的精度，减少测距数据的噪声，当系统的运动状态发生变化时，滤波精度下降明顯。文献［16］提出了一种基于传统卡尔曼滤波提高精度的方法，但是传统卡尔曼滤波普遍存在跟踪能力不强和自适应能力差的问题。文献［17］利用神经网络提升UWB室内定位系统在非视距情况下的抗干扰能力，从而提高定位精度。文献［18］提出了一种基于遗传神经网络的煤矿井下定位算法，在非视距环境下具有较好的定位效果。神经网络不用建立准确的数学模型就可以进行计算，但也正因如此，神经网络不能有效过滤噪声。文献［19］提出了一种融合UWB+PDR（pedestrian dead reckoning）的室内定位方法。文献［20］提出了一种融合视觉里程计与全球卫星导航系统-惯性导航系统（global navigation satellite system-inertial navigation system，GNSS-INS）的低成本车道级定位方法。多技术融合的室内定位技术能融合多种定位方法的优点，弥补单项定位算法的不足，实现更高精度的室内定位。

由上述文献可知，传统的定位方法和滤波方式很难满足目前该三线轨道交通的定位要求。为此，本文提出了一种更新速度较快、成本较低、精度较高的基于神经网络与自调节卡尔曼滤波的UWB定位算法：运用UWB标签和基站采集大量标签与各个基站的距离信息及对应标签的实际位置信息训练神经网络，将实时的距离信息发送至集中控制中心的服务器上，计算得到实时定位标签的位置，最后通过仿真与实验验证定位算法。

1 神经网络与自调节卡尔曼滤波结合的UWB定位算法

本文采用基于到达时间（time of arrival， TOA）的对称双向双边测距（symmetric double-sided two-way ranging， SDS-TWR）算法，通过轮询的方式测得三个UWB基站与标签的距离。由标签广播请求信号Poll，记录发送Poll的时刻。Poll包含自身的标识码及其中一个基站的ID，在其通信范围内的UWB基站接收到请求信号Poll后，记录接收信号的时刻，并提取其中包含基站的ID，提取出的基站ID和自身的ID进行比较，若一致，则发送消息Resp，并记录发送Resp的时刻。Resp包含其自身的标识码，在Resp通信范围内的UWB标签接收到Resp后，记录接收信号的时刻。紧接着发送消息Final，并记录发送Final的时刻。Final包含其自身的标识码、UWB标签发送Poll的时刻、接收Resp的时刻、发送Final的时刻及其中一个基站的ID。在其通信范围内的UWB基站接收到Final后，记录接收信号的时刻，并提取其中包含基站的ID，提取出的基站ID和自身的ID进行比较，若一致，将接收到的UWB标签发送Poll的时刻、接收Resp的时刻、发送Final的时刻和UWB基站记录的接收Poll的时刻、发送Resp的时刻、接收Final的时刻结合起来解算距离信息［21-23］。对称双向双边测距算法原理如图1所示，其中：

（1）

式中，T^p为UWB标签和基站之间的飞行时间；tpt为UWB标签发送Poll时刻；tpr为UWB基站接收Poll时刻；trt为UWB基站发送Resp时刻；trr为UWB标签接收Resp时刻；tft为UWB标签发送Final时刻；tfr为UWB基站接收Final时刻。

由UWB标签和基站之间的飞行时间T^p可

式中，x^-t为第t次卡尔曼滤波状态的预测值;x^t-1为第t-1次卡尔曼滤波状态的最优估计值;x^t为第t次卡尔曼滤波状态的最优估计值;B为控制输入矩阵;ut-1为第t-1次卡尔曼滤波的输入;P-t为第t次卡尔曼滤波真实值与预测值之间的协方差矩阵;Pt-1为第t-1次卡尔曼滤波真实值与最优估计值之间的协方差矩阵;Pt为第t次卡尔曼滤波真实值与最优估计值之间的协方差矩阵;Q为系统噪声协方差;H为观测矩阵;R为观测噪声协方差;Kt为第t次卡尔曼滤波增益;zt第t次卡尔曼滤波的观测量；I为单位矩阵。

首先对状态进行预测，然后预测协方差，最后进行状态更新及协方差更新，如此循环计算［26-27］。

在本文中，状态转移矩阵F为1×1的矩阵，因此可看作为一个数，F的值F不是一个定值，因为定位坐标X的实际值在某一个时间段会增大、减小或者不变。F值需要根据对应的实际情况作出调整。若坐标值逐渐变大，则F>1；若坐标值不变，则F=1；若坐标值减小，则F<1。

F值的确定方法是：初始的F值设定为1。设Δk=xk－1－zk（k>1） ，其中xk－1为定位坐标X第k-1次的卡尔曼滤波预测值，zk为定位坐标X第k次的观测值。若k>10，则计算Δk－9～Δk这10个数的平均值，记为k；若k≤10，则计算Δ2～Δk的平均值，记为k。对k进行合理分段，并赋予其相应的F值，用于第k次卡尔曼滤波。对大量的实验数据进行归纳整理，可得F和k之间的函数关系，用分段函数F=f（k） 表示。k>0时，F<1，且随着k的增大，F值逐渐减小；k<0时，F>1，且随着k的减小，F值逐渐增大。如此自适应地调节F值，拓展了卡尔曼滤波的使用环境，提高了卡尔曼滤波的滤波精度，得到了更加精准的定位坐标值，此方法即为自调节卡尔曼滤波方法。

2 模拟轨迹仿真

为了验证本文提出的基于神经网络与自调节卡尔曼滤波结合的UWB定位算法，本文在5.5 m×5.5 m定位区域中设计一组包含了斜道、直道和弯道的UWB标签移动轨迹，该轨迹有效地涵盖了项目轨道铺设过程中可能会遇到的直线行驶、车道变换及弯道情况，且该图形中弯道的最小曲率半径大于实际轨道的运营场景。图形的走向充分考虑了磁浮车的运营场景，是项目组根据运营场景综合研判的设计结果。如果在该图形中能满足定位要求，那么实际的运营场景也是可靠的。假设将A、B、C三个UWB基站分别放在（0，0）、（6000，0）mm、（3000，6000）mm三个位置，计算出UWB标签到每个基站的距离信息以及对应标签的实际位置，对计算出的距离信息加上一组噪声，将模拟出的初始距离信息用三点定位算法、卡尔曼滤波算法、神经网络算法和基于神经网络与自调节卡尔曼滤波结合的定位算法分别进行计算，得到的结果如图4所示。

本文采用实际位置与算法计算位置的距离作为定位误差，如下式所示：

（10）

式中，ei为第i次定位误差；（xi，yi）为第i次待定位点的真实坐标；（x^i，y^i）为第i次待定位点的计算坐标。

不同算法的计算误差如表1所示。由表1可知，本文提出的定位算法虽然仍有定位误差，但是与三点定位算法、卡尔曼滤波算法和神经网络算法相比较，基于神经网络与自调节卡尔曼滤波结合的定位算法具有更高的定位精度。

3 车道识别实验

为了更好地验证神经网络与自调节卡尔曼滤波结合的UWB定位算法，本文搭建了实验平台进行实验验证。

实验系统原理如图5所示，由3个UWB基站、1个UWB标签、4根网线、1个工业交换机、1个UWB定位服务器和5个电源模块组成。UWB基站和标签均由EVB1000模块、STM32F103控制器、天线及电源模块组成，实物照片如图6所示。STM32F103控制器通过SPI接口和EVB1000模块连接；STM32F103控制器通过控制EVB1000模块中的DW1000芯片实现测距；天线和EVB1000模块通过SMA接口连接，天线发挥发射和接收电磁波的作用；电源模块给设备供电。测距终端设备可以通过软件配置为基站或者标签。UWB基站将其坐标值和基站到标签的距离信息通过网络发送给定位服务器；UWB定位服务器采集数据，运用本文的算法处理数据，并显示结果。

UWB定位服务器的软件界面如图7所示，该软件实时显示标签的车道信息和定位信息。实验中，对变道轨迹进行验证之前需要先对UWB测距系统进行布置，实验总体布局如图8所示。

实验为三车道，依次为高速车道、中速车道和低速车道。车辆只能在相邻的轨道之间进行变道。车辆行驶轨迹如图9所示，a～b为车辆由停车位进入低速车道的轨迹；b～c、j～k为车辆在低速车道行驶的轨迹；c～d为车辆由低速车道到中速车道变道的行驶轨迹；d～e、h～i为车辆在中速车道行驶的轨迹；e～f为车辆由中速车道到高速車道变道的行驶轨迹；f～g为车辆在高速车道行驶的轨迹；g～h为车辆由高速车道到中速车道变道的行驶轨迹；i～j为车辆由中速车道到低速车道变道的行驶轨迹；k～l为车辆由低速车道进入停车位的行驶轨迹。车辆在a～l移动的过程中，每隔50 cm记录一次UWB标签的实际位置，UWB定位服务器记录原始数据，并将原始数据经本文的算法处理后显示在服务器上，部分实验数据如表2所示。

由表2和表3可知：神经网络与自调节卡尔曼滤波结合的定位算法具有精度高、实时性好及稳定性高的优点，满足本文三线轨道交通的定位要求。

4 结语

（1）本文提出了一种神经网络与自调节卡尔曼滤波结合的UWB定位算法。采用基于到达时间（TOA）的对称双向双边测距（SDS-TWR）方法采集UWB标签到各基站之间的距离信息后，使用本文提出的算法进行处理。

（2）设计了一组包含斜道、直道和弯道的UWB标签移动轨迹，该轨迹能够模拟实际车辆运行中的所有情况。模拟实际采集的距离信息，使用三点定位算法、卡尔曼滤波算法、神经网络算法和本文提出的定位算法分别对仿真结果的距离信息进行处理。结果表明，神经网络与自调节卡尔曼滤波结合的定位算法的最大定位误差为147.31 mm，平均定位误差为48.59 mm，定位误差均方根值为30.28 mm。

（3）搭建了实验平台，对本文提出的定位算法进行了实验验证。结果表明，神经网络与自调节卡尔曼滤波结合的定位算法的最大定位误差为223.58 mm，平均定位误差为43.16 mm，定位误差均方根值为42.06 mm。神经网络与自调节卡尔曼滤波结合的定位算法具有精度高、实时性好及稳定性高的优点，满足本文三线自动驾驶轨道交通的定位要求。

參考文献：

［1］JIANG Shengchuan， ZHAO Cong， ZHU Yifan， et al. A Practical and Economical Ultra-wideband Base Station Placement Approach for Indoor Autonomous Driving Systems［J］. Journal of Advanced Transportation， 2022， 12（8）：168-174.

［2］朱冰，陶晓文，赵健，等.智能汽车两阶段UWB定位算法［J］. 交通运输工程学报，2021，21（2）：256-266.

ZHU Bing，  TAO Xiaowen，  ZHAO Jian， et al. Two-stage   UWB  Positioning  Algorithm  for Smart Cars［J］. Journal of Traffic and Transportation Engineering， 2021， 21（2）：256-266.

［3］LIAO Z， ZHENG Z， LI Y. Implementation and Improvement of Indoor Wearable UWB/INS Integration Positioning Method［J］. Remote Sensing and Spatial Information Sciences， 2022， XLVI-3/W1-2022：111-117.

［4］ZHANG Chengwei， ZHANG Na， CHEN Bingqian. Research and Application on Cooperative Positioning Technology in Substation Project Based on BDS and UWB［J］. Journal of Physics：Conference Series， 2022， 2260（1）：1560-1572.

［5］DJOSIC S， STOJANOVIC I， JOVANOVIC M， et al. Multi-algorithm UWB-based Localization Method for Mixed LOS/NLOS Environments［J］. Computer Communications， 2022， 181：365-373.

［6］LIU Qingzhi， YIN Zhendong， ZHAO Yanlong， et al. UWB LOS/NLOS Identification in Multiple Indoor Environments Using Deep Learning Methods［J］. Physical Communication， 2022， 52（6）：101695.1-101695.11.

［7］ZHANG Yuekui， CHU Yunxia， FU Yunfang， et al. UWB Positioning Analysis and Algorithm Research［J］. Procedia Computer Science， 2022， 198：466-471.

［8］HU Xiaohao， LUO Zai， JIANG Wensong. AGV Localization System Based on Ultra-wideband and Vision Guidance［J］. Electronics， 2020， 9（3）：448-462.

［9］LIU Chang， BAI Fengshan， WU Chunsheng. A Joint Positioning Algorithm of TDOA and TOF Based on Ultra-wideband［J］. Journal of Physics：Conference Series， 2021， 2031（1）：012039.

［10］ARDIANSYAH M， GDE D N， HYOJEONG H，et al. A Decision Tree-based NLOS Detection Method for the UWB Indoor Location Tracking Accuracy Improvement［J］. International Journal of Communication Systems， 2019， 32（13）：e3997.

［11］FAN Qigao， SUN Biwen， WU Yaheng. Tightly Coupled Model for Indoor Positioning Based on UWB/INS［J］. International Journal of Computer Science Issues （IJCSI）， 2015， 12（4）：11-16.

［12］PANG Liang， CHEN Xiao， XUE Zhi， et al. A Novel Range-free Jammer Localization Solution in Wireless Network by Using PSO Algorithm［C］∥International Conference of Pioneering Computer Scientists， Engineers and Educators. Singapore：Springer， 2017：198-211.

［13］张子明，王从庆.基于超宽带技术的室内定位系统应用开发［J］. 单片机与嵌入式系统应用 ，2021，21（4）：76-80.

ZHANG Ziming， WANG Congqing. Application Development of Indoor Positioning System Based on Ultra-wideband Technology［J］. Microcontrollers & Embedded Systems， 2021， 21（4）：76-80.

［14］劉永昌，龚元明.基于卡尔曼滤波的超宽带定位技术应用［J］. 软件工程，2021，24（7）：24-27.

LIU Yongchang， GONG Yuanming. Application of Ultra-wideband Positioning Technology Based on Kalman Filter［J］. Software Engineering， 2021， 24（7）：24-27.

［15］杨玉钊，郑良广.卡尔曼滤波在列车防撞预警系统中的应用［J］. 电子设计工程，2020， 28（18）：56-59.

YANG Yuzhao， ZHENG Liangguang. Application of Kalman Filter in Train Collision Avoidance Warning System［J］. Electronic Design Engineering， 2020， 28（18）：56-59.

［16］任昊誉，郭晨霞，杨瑞峰.卡尔曼滤波提高UWB测距精度研究［J］. 电子测量技术，2021，44（18）：111-115.

REN Haoyu， GUO Chenxia， YANG Ruifeng. Kalman Filter Improves UWB Ranging Accuracy Research［J］. Electronic Measurement Technology， 2021， 44（18）：111-115.

［17］薛阳阳，沈重.基于GA-BP神经网络的超宽带室内定位［J］. 海 南 大 学 学 报（自 然 科 学 版），2021，39（3）：235-241.

XUE Yangyang， SHEN Zhong. Ultra-wideband Indoor Positioning Based on GA-BP Neural Network［J］. Journal of Hainan University（Natural Science）， 2021， 39（3）：235-241.

［18］逄明祥，王善培，李 乾，等.一种基于遗传神经网络的煤矿井下定位算法［J］. 实验室研究与探索，2021，40（4）：8-12.

PANG Mingxiang， WANG Shanpei， LI Qian， et al. A Coal Mine Underground Positioning Algorithm Based on Genetic Neural Network［J］. Research and Exploration in Laboratory， 2021，40（4）：8-12.

［19］李景文，韦晶闪，周俊芬，等.融合 UWB+PDR 的室内定位方法改进［J］. 测绘通报，2022（3）：36-40．

LI Jingwen， WEI Jingshan， ZHOU Junfen， et al. Improved Indoor Positioning Method Based on UWB+PDR［J］. Bulletin of Surveying and Mapping， 2022（3）：36-40.

［20］朱贵冬，林钢鑫，田震华，等.融合视觉里程计与GNSS-INS的低成本车道级定位方法［C］∥第十三届中国卫星导航年会.北京，2022：28-32.

ZHU Guidong， LIN Gangxin， TIAN Zhenhua， et al. A Low-cost Lane-level Positioning Method Combining Visual Odometer and GNSS-INS［C］∥Proceedings of the 13th China Satellite Navigation Annual Conference.Beijing，2022：28-32.

［21］王春琦，冯大权，何春龙，等.基于UWB的增强非对称双边双向测距算法研究［J］. 南昌航空大学学报（自然科学版），2019，33（1）：66-73.

WANG Chunqi， FENG Daquan， HE Chunlong， et al. Research on Enhanced Asymmetric Bilateral Bidirectional Ranging Algorithm Based on UWB［J］. Journal of Nanchang Hangkong University（Natural Sciences）， 2019， 33（1）：66-73.

［22］孙宏伟，曹雪虹，焦良葆，等.DS-TWR算法室内定位批量测距系统的优化研究［J］. 电信科学，2022，38（1）：73-82.

SUN Hongwei， CAO Xuehong， JIAO Liangbao， et al. Optimization Study of Indoor Positioning Batch Ranging System of DS-TWR Algorithm［J］. Telecommunications Science， 2022， 38（1）：73-82.

［23］高健，陆阳，李庆巧，等.采用三次通信的TOF与TDOA联合定位算法［J］. 电子测量与仪器学报，2020，34（3）：66-73.

GAO Jian， LU Yang， LI Qingqiao， et al. TOF and TDOA Joint Positioning Algorithm Using Tertiary Communication［J］. Journal of Electronic Measurement and Instrumentation， 2020， 34（3）：66-73.

［24］张月霞，刘冲.基于神经网络和路侧单元指纹的车辆定位方法［J］. 计算机仿真，2022，39（4）：114-118.

ZHANG Yuexia， LIU Chong. Vehicle Positioning Method Based on Neural Network and Roadside Unit Fingerprint［J］. Computer Simulation， 2022， 39（4）：114-118.

［25］李志祥，丁绪星，陈兴盛，等.基于深度神经网络的WiFi室内定位算法研究［J］. 安徽师范大学学报（自然科学版），2022，45（2）：131-138.

LI Zhixiang， DING Xuxing， CHEN Xingsheng， et al. Research on WiFi Indoor Positioning Algorithm Based on Deep Neural Network［J］. Journal of Anhui Normal University（Natural Science）， 2022， 45（2）：131-138.

［26］葛淼，张亚东，李科宏，等.基于滑模控制的卡尔曼滤波在列车定位中的研究［J］. 铁道标准设计，2019，63（5）：148-152.

GE Miao， ZHANG Yadong， LI Kehong， et al. Research on Kalman Filter Based on Sliding Mode Control in Train Positioning［J］. Railway Standard Design， 2019， 63（5）：148-152.

［27］王宪伦，孙旭祥，丁文壮，等.基于卡尔曼滤波的机器人力控制［J］. 机械制造与自动化，2021，50（4）：159-161.

WANG Xianlun， SUN Xuxiang， DING Wenzhuang， et al. Robot Force Control Based on Kalman Filtering［J］. Machine Building & Automation， 2021， 50（4）：159-161.

（编辑 王艳丽）

作者简介：

古玉锋，男，1978年生，副教授。研究方向为机械系统动力学及其控制、汽车电控主动悬架系统、现代汽车無人驾驶系统、现代汽车及工程机械底盘系统控制与匹配。发表论文14篇。E-mail：guyufeng@chd.edu.cn。

收稿日期：2022-11-09

基金项目：国家自然科学基金（52205249）；陕西省自然科学基础研究计划（2022JQ-434）