基于UWB和IMU融合的MAV群精确三维定位

基于UWB和IMU融合的MAV群精确三维定位

注：这篇翻译中的小牛全部替换为无人机 本地化全部替换为定位 百度翻译的问题
李嘉欣1，毕英才1，李坤2，王康丽2，冯琳3，陈本美2
摘要——由微型飞行器等应用驱动（无人机）、无人汽车等，定位解决方案已经成为近十年来的一个活跃的研究课题。近年来，超宽带（UWB）作为一种很有前途的技术应运而生，因为它在室内和室外定位。但是算法只依赖于UWB传感器通常会导致高延迟和低带宽在某些情况下是不可取的，例如控制MAV。
为了缓解这一问题，扩展卡尔曼滤波器（EKF）提出了一种新的惯性测量融合算法单元（IMU）和UWB，实现了80Hz的三维定位准确度显著提高，几乎没有延迟。到验证所提出方法的有效性和可靠性，一个由6个无人机组成的群体在室内展厅。视频和源代码可在
https://github.com/lijx10/uwb-localization

一、 简介

微型飞行器（MAV）群体一直吸引着由于它的潜力，近年来人们对它的兴趣与日俱增各种应用程序的改进，如surveillance[1]，通信中继[2]，3D重建[3]等等。控制一个无人机群的编队，准确定位是最重要的。通常本地化是由全球导航卫星系统（如GPS）实现[4]，或采用高精度光学跟踪系统（如维康）[5] 。标准GPS的精度是米级的，它可以当作为实时运行时，被提升到厘米级带有附加固定基站的动态（RTK）模式。
GPS只能在开阔空间工作，精度高-由于多径效应，卫星信号质量较差。
另一方面，维康系统提供厘米级在室内环境下精确，但需要昂贵的和复杂的设置与多个摄像头。
有越来越多的尝试替代定位-无GPS且价格昂贵的微型飞行器群解决方案维康系统。基于视觉的解决方案尤其令人感兴趣，因为它成本低、重量轻、灵活。在[6]a中-信息估计器是通过引入一致性而发展起来的-基于分布式卡尔曼滤波。使用车载摄像头跟踪标记并测量无人机之间的相对姿势。环境的共同特征是由不同的无人机在[7]中估计了它们之间的相对姿态。
图像首先通过IMU的补偿进行校正基于单应性计算旋转和相对姿态分解。在一个视觉惯性定位系统中是基于IMU测量和AprilTag提出的

1国立大学综合科学与工程研究生院

新加坡大学（NUS）。[email protected]

2国立大学电气与计算机工程系

3新加坡淡马锡实验室。

对apriltag进行了稳健的跟踪和扩展采用卡尔曼滤波（EKF）对两个摄像机进行估计同时做标记姿势。IMU用于改进系统鲁棒性在标记检测丢失的情况下短期的。尽管视觉定位有很多优点，强大的计算能力和适当的照明条件这就限制了它的应用。

如基于无线电的定位系统-序列识别（RFID）[9]，WiFi[10]，Zigbee[11]和超宽带（UWB）[12]，是室内定位。RFID在物流管理中得到了广泛的应用-用于标记和跟踪资产。通过阅读标签预定位置，可确定跟踪位置具有接近度和接收信号强度（RSS）值[13] 。然而，RFID只能在很小的距离内工作使用RSS进行测距只是一个粗略的估计。无线网络Zigbee和Zigbee都配置为无线网络系统。
可以通过评估基于RSS值和已知分布的距离网络节点。WiFi易于使用，成本低廉，并且Zigbee设计为低功耗。WiFi是可能会受到常用移动设备的干扰这会导致仪表水平精度不好足够控制一群无人机了。齐比提高了一定程度上的定位精度[14]，但是仍然很难维持无碰撞的MAV编队室内空间有限。
与上述解决方案相比，UWB是其中之一在准确性、覆盖率方面最有希望的解决方案射程和部署成本。多亏了单曲的缩影芯片收发器[15]，可作为定位系统用于实时机器人应用[16]。两者之间的范围可以通过超宽带的时间差来确定到达（时差）[17]，[18]。同时，高容量数据传输可以用很小的能量来实现功能。由于高带宽，短脉冲波形。此外，这些特征可以帮助信号通过障碍物提高测距精度。因此，超宽带是一种微型飞行器群的高精度定位系统在GPS无效的环境中。

二。传感器设置

在我们的MAV swarm应用中，多个UWB传感器

设置时域制造（如图1所示）
作为双向测距系统。6个超宽带传感器固定在展览厅作为锚。锚大约包围着一个长方体，我们建立一个锚的坐标系精确测量它们之间的距离和角度。

我们的主播设在新加坡樟宜展览中心

如图2和（1）所示，使用西北向上（NWU）协调。详情见第五节。图2，定位算法可以达到最大的性能在凸壳内部，这是在传感器安置社区。

图2：樟宜展览中心超宽频锚点布置图，

新加坡。
p Ax0 = 0,p Ay0 = 0,p Az0 = 0
p Ax1 = 14.6,p Ay1 = 0,p Az1 = 0
p Ax2 = 14.6,p Ay2 = 25.5,p Az2 = 0
p Ax3 = 0,p Ay3 = −1,p Az3 = 5.3
p Ax4 = 0,p Ay4 = 26.6,p Az4 = 5.3
p Ax5 = 17.4,p Ay5 = 10.1,p Az5 = 5.3

双向测距系统，也称为时间-到达（TOA）系统，配置有时分多址（TDMA）和码分多址（CDMA）。每个MAV都配备了有一个UWB传感器，表示为移动，它在独特的频道。在每个时隙中，每个移动设备启动一个向主播发送消息，主播立即响应。

因为所有超宽带传感器的时间都同步到皮秒的精度，每个mobile(可移动的-》无人机)都能计算出与它交谈的锚的距离，根据他们的时间接收对方的超宽带脉冲。(可移动的-》无人机)探测以循环的方式，即每6个时隙，一个移动设备将探测所有6个锚，并获得相应的距离测量r k与测量不确定度σrk。在我们的设置中，每台手机每台可获得80个测量值每秒（80hz）

三、VANILLA EKF FOR UWB

典型的基于TOA的算法包括多维标度（MDS）、加权最小二乘法、三边测量法等。
这些方法至少需要3次测量来确定3D位置，但在典型的UAB？？？设置中，每部手机每次接收一次距离测量，这意味着任何三个测量值都是用不同的时间戳。另一方面，基于EKF和粒子滤波等方法能够一次测量一次。但是，没有其他信息，他们通常是在一些运动假设下像等速模型，这会导致小牛的延迟定位。在普通EKF中，状态向量由位置和

（2）中定义的速度。在等速模型下，

时间更新过程在（3）中定义。
其中控制输入u k−1和输入增益B k−1为

零。系统矩阵Ak可由定常速度假设导出，而过程噪声协方差Q(k-1)在假设加速度为

是高斯分布N（0，σa）的情况下导出。在本文中，我们设置σa=0.125经验。T是每个

EKF迭代间的时间间隔。
将锚的位置表示为p Ax，p Ay，p Az预计无人机和锚之间的距离为，
测量矩阵hk由一阶定义（6）的泰勒展开式
，EKF的测量更新定义如下，其中测量值表示为rk和测量噪声协方差R k=σr 2（σr的平方）
σr表示测量值一对无人机和锚之间的不确定性的UWB传感器报告值。
方程（2-8）定义了用于UWB定位的常规EKF，在等速假设下。当传感器具有低加速率，即香草型EKF性能良好，但在应用中可以观察到严重的延迟-如第五节所示。

四、UWB和IMU融合
！！！！！！！！！！！！！！！！！！！！

为了解决延迟和低带宽问题

由等速假设带来的，融合了加速器-IMU提供的信息是一个理想的解决方案。然而，低成本商用IMU的加速度测量非常noisy。此外，加速度偏差为不稳定，即其偏差受许多因素影响，包括温度、工作时间、机械振动等。
直接整合来自IMU的加速度可能导致甚至比等速假设的结果更糟。

A、 增广状态向量

为了解决偏差问题，我们增加了状态向量在所有三个轴上都有加速度偏差，即建立一个增广状态向量
。加速度测量[a x，a y，a z]T用作（3）的输入。因此

定义了系统矩阵A、输入增益B k−1和u k−1在（10-12）。
假设IMU读数被高斯噪声损坏（corrupt），我们用τa来测量IMU噪声。同样，τb用于测量估计加速度偏差。因此，过程噪声可以定义如下。
测量更新几乎与（8）相同，除了将线性化测量矩阵替换为（14）
。B、 实施细节
到目前为止，上述带有增广状态向量的EKF是完整的。来自IMU的加速度被融合到UWB中测量作为控制输入，而加速度偏差被估计为状态向量的一部分。但在普拉克-超宽带传感器的读数不稳定。在大多数情况下在这种情况下，测量的距离r k精确到几厘米，但偶尔报告的距离是完全错误，例如离真值几米远。此外，IMU的读数非常嘈杂意味着过度依赖IMU是不切实际的。通常我们倾向于更依赖于超宽带的读数调整协方差矩阵Q和R，其代价是超宽带读数的跳变将导致突变估计的位置。为了缓解这些问题，我们计算了差异在预测距离
和实际UWB之间测量值r k，
。如果dk超过某个阈值，例如2m，定位结果x k被丢弃。

五、 实验

首先，第三节的香草EKF和第四节中的融合EKF是在配备有了维康系统，才有了真正的真值
这两种算法的性能可以量化评价的。如图4和图5所示
，很明显融合EKF显示出明显更好的准确性和低延迟。fusion EKF部署在6辆MAV上
在樟宜展览馆上演了一场灯展新加坡中心。这个灯光秀是两个展览的仪式，即无人系统
亚洲2017和旋翼机亚洲2017。
MAV平台如图3所示。
飞行控制员是Pixhawk的增强版本。像磁强计、IMU、气压计、稳压电源等传感器供应商集成在一个紧凑的电路中。这个UWB传感器是由时域公司time domain。如第二节所述
UWB传感器配置为双向测距

TDMA和CDMA模式。它们提供距离测量-以大约80Hz，典型精度为10cm。加速度测量值来自频率为50赫兹。一个upboard(UP board是一款信用卡尺寸大小，采用最新平板电脑技术并具有高性能和低功耗特点的一款板卡。UP board搭载英特尔® 凌动™ X5 - Z8350处理器(代号Cherry Trail)，配备1GB/2GB/4GB DDR3L RAM和16GB/32GB/64GBeMMC。)up板，即商业化的英特尔原子系列的微型计算机，安装在MAV上以托管任务管理和定位算法。总计MAV的重量为800g，包括一个2200mAh 3电池、大功率LED阵列和旋翼保护器。
在维康室和展厅的测试中6个MAV执行由多主体(multiagent)splines(样条,曲线)的轨迹生成算法[19]。MAV的最大速度为1.2m/s，并且最大加速度为2m/s2。在实验中在本文中，只使用了其中一个mav，而另5个只在执行的路径上不同。第五节中的mav定位依赖于融合EKF算法。

A、 维康试验

在装有维康系统的房间里，超宽带在地面和天花板上放置锚固件。根据维康房间的大小限制，锚之间最大距离水平方向约为6m，并且垂直方向2.5m。其实这样的设置与附近锚会显著降低超宽带定位系统的精度。测量距离的误差为如果UWB传感器在最大范围内，大概几百米大约会产生10厘米误差。无论传感器之间的距离如何，只要距离在最大工作范围内，误差都处于同一水平
因此，在紧凑的设置中测量误差和测量距离较大使三角测量不那么精确。在这么小的设置下在维康房间，表演并不理想，但仍然足够让MAV执行一条路径。执行预定义路径的结果如所示图4和图5。

图5：（a-c）MAV沿x，y，z轴的位置。（d）仔细观察（b）。可以观察到维康测量和融合EKF估计几乎为零，而香草EKF估计与地面之间存在延迟真值是0.5秒。

根据位置和速度沿x，y，z方向（NWU）的曲线，融合EKF有准确度明显高于香草EKF。在特别是香草型EKF的超调问题因为速度不变的假设。
此外，与精度问题相比，延迟在香草EKF上展出是致命的。之间的延迟估计位置和维康测量大约500毫秒
在图5中，它的大小足以使控制器发散小牛队的。最大和平均定位误差如表1所示。虽然很难得到IMU的基本加速度偏差真值，估计偏差的范围和趋势。
在图4中似乎是合理的。基于定位结果，我们可以安全地假设估计偏差大致正确。另一个现象是融合EKF，估计的位置和速度下降很明显在68秒左右。那是因为小牛着陆了在当时的地面上，这一打击（hit）引入了一个巨大的来自IMU的加速度测量。由于融合EKF的系统矩阵依赖于加速度测量，这种异常输入会导致估计错误。但一般来说，这种现象不会引起任何问题，因为它只发生在着陆或小牛撞到什么东西上。
B、 樟宜展览中心演出

利用fusion EKF，MAV群的性能是在新加坡樟宜展览中心举行2017亚洲无人驾驶系统和旋翼机仪式亚洲2017。在表演中，设置了UWB锚在图2中所示的更大的空间中。如前所述

如上所述，超宽带传感器之间的空间越大，估计就越稳定和准确。事实上，小牛在展厅里可以在没有明显运动的情况下盘旋。
但在多个MAV操作的情况下，其他困难出现了。
1 首先，12个超宽带传感器在满功率下工作时电磁波环境变得更复杂了，以及成千上万的WiFi设备。
2 其次，在MAV编队中，MAV阻断其他UWB的信号。超宽带测距的本质要求任何一对传感器之间的视线环境。任何这种阻塞可能会导致完全不正确的读数。（恍然大悟，多无人机编队的话 当其中一个无人机出现在另一个无人机和基站连线的中间的时候，不是就出现了NLOS情况了吗！而且在我们的日常生产生活当中，一个UWB室内定位系统应该总是需要考虑NLOS情况的 因为你无法保证什么意外的因素会导致NLOS情况的出现，所以我们的系统要考虑这些因素才能设计的更加鲁棒）
这两个原因都导致了较大的测距误差频繁的测量跳跃。在我们提出的融合EKF中，显然不正确的超宽带读数被更新的阈值所抛弃。在阈值失效的情况下，与加速度的积分保持估计的位置避免偏离真值太多。
如图6所示，

图6：（a）x-y平面上的轨迹。（b-d）沿x，y，z轴的MAV位置。（e）IMU估计的加速度偏差的。

定位结果是正确的接近参考值。虽然没有办法在这样的公开演出中得到真值，但是定位准确度可通过视频在 https://www.youtube.com/watch?v=1id49danIK4&feature=youtu.be 视频里观看
六、 结论
本文提出了一种基于EKF的算法融合UWB传感器和IMU的测量值。同时对IMU的位置、速度和偏差进行了估计。维康系统的实验证明我们的方法的稳定性和准确性明显优于香草EKF。更重要的是，定位算法的延迟几乎被消除，而香草EKF的估计延迟是不可接受的。在此基础上，利用本文提出的定位算法，在室内展厅内进行了多MAV灯光表演。
REFERENCES
[1] N. Nigam, S. Bieniawski, I. Kroo, and J. Vian, “Control of multiple UAVs for persistent surveillance: algorithm and flight test results,”IEEE Transactions on Control Systems Technology, vol. 20, no. 5, pp.1236–1251, 2012.
[2] S. Hayat, E. Yanmaz, and R. Muzaffar, “Survey on unmanned aerial vehicle networks for civil applications: a communications viewpoint,”IEEE Communications Surveys & Tutorials, vol. 18, no. 4, pp. 2624–2661, 2016.
[3] M. Haris, T. Watanabe, L. Fan, M. R. Widyanto, and H. Nobuhara,“Superresolution for UAV images via adaptive multiple sparse representation and its application to 3-D reconstruction,” IEEE Transactions on Geoscience and Remote Sensing, vol. 55, no. 7, pp. 4047–4058,July 2017.
[4] X. Dong, Y. Zhou, Z. Ren, and Y. Zhong, “Time-varying formation control for unmanned aerial vehicles with switching interaction topologies,” Control Engineering Practice, vol. 46, pp. 26–36, 2016.
[5] A. Kushleyev, D. Mellinger, C. Powers, and V. Kumar, “Towards a swarm of agile micro quadrotors,” Autonomous Robots, vol. 35, no. 4,pp. 287–300, 2013.
[6] T. Nägeli, C. Conte, A. Domahidi, M. Morari, and O. Hilliges,“Environment-independent formation flight for micro aerial vehicles,”in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 1141–1146.
[7] E. Montijano, E. Cristofalo, D. Zhou, M. Schwager, and C. Saguees,“Vision-based distributed formation control without an external positioning system,” IEEE Transactions on Robotics, vol. 32, no. 2, pp.339–351, 2016.
[8] M. Neunert, M. Bloesch, and J. Buchli, “An open source, fiducial based, visual-inertial motion capture system,” in Information Fusion(FUSION), 2016 19th International Conference on. IEEE, 2016, pp.1523–1530.
[9] J. Zhou and J. Shi, “RFID localization algorithms and applications review,” Journal of intelligent manufacturing, vol. 20, no. 6, pp. 695–707, 2009.
[10] A. Polo, F. Viani, E. Giarola, G. Oliveri, P. Rocca, and A. Massa,“Semantic wireless localization enabling advanced services in museums,” in Antennas and Propagation (EuCAP), 2014 8th European Conference on. IEEE, 2014, pp. 443–446.
[11] N. Watthanawisuth, A. Tuantranont, and T. Kerdcharoen, “Design of mobile robot for real world application in path planning using ZigBee localization,” in Control, Automation and Systems (ICCAS), 2014 14th International Conference on. IEEE, 2014, pp. 1600–1603.
[12] A. Alarifi, A. Al-Salman, M. Alsaleh, A. Alnafessah, S. Al-Hadhrami,M. A. Al-Ammar, and H. S. Al-Khalifa, “Ultra wideband indoor positioning technologies: Analysis and recent advances,” Sensors,vol. 16, no. 5, p. 707, 2016.
[13] A. R. J. Ruiz, F. S. Granja, J. C. P. Honorato, and J. I. G. Rosas, “Accurate pedestrian indoor navigation by tightly coupling foot-mounted IMU and RFID measurements,” IEEE Transactions on Instrumentation and Measurement, vol. 61, no. 1, pp. 178–189, 2012.
[14] L. Yut, Q. Fei, and Q. Geng, “Combining Zigbee and inertial sensors
for quadrotor UAV indoor localization,” IEEE, pp. 1912–1916, 2013.
[15] decaWave. [Online]. Available: https://www.decawave.com/
[16] S. Krishnan, P. Sharma, Z. Guoping, and O. H. Woon, “A UWB based localization system for indoor robot navigation,” in Ultra-Wideband,2007. ICUWB 2007. IEEE International Conference on. IEEE, 2007,pp. 77–82.
[17] J. Xu, M. Ma, and C. L. Law, “Position estimation using UWB TDOA measurements,” in Ultra-wideband, the 2006 IEEE 2006 International Conference on. IEEE, 2006, pp. 605–610.
[18] Q. Fan, B. Sun, Y. Sun, Y. Wu, and X. Zhuang, “Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB,” The Journal of Navigation, pp. 1–19, 2017.
[19] S. Lai, K. Wang, H. Qin, J. Q. Cui, and B. M. Chen, “A robust online path planning approach in cluttered environments for micro rotorcraft drones,” Control Theory and Technology, vol. 14, no. 1, pp. 83–96,2016.