基于多传感器数据融合的导航车定位技术研究

李金生、肖菲、李欣、罗大鹏、董蓓

（中建三局智能技术有限公司，湖北省武汉市 430074）

（中国地质大学（武汉），湖北省武汉市 430074）

摘要：随着定位导航技术的不断发展，人们对获取位置与轨迹导航的需求也在逐渐增加，但室内封闭复杂环境的精确定位仍存在困难。本文主要针对室内封闭环境，基于数据融合技术，利用多个普通定位传感器的数据相互校验与修正，并依靠航迹推算和参考定位算法完成组定位导航实验，完成高精度的定位，实现自主导航车任意两点间的定位导航与任意轨迹的行走，相比于单一的定位算法，航迹和位姿控制精度明显提高。

关键词：室内定位；多传感器；数据融合；组合定位导航；自主导航车

1. 引言

近年以来，随着定位导航技术的不断发展，人们对获取位置与轨迹导航的需求也在逐渐增加。为了提高生活质量和生产效率，家庭服务、仓储物流和工厂自动化等封闭环境中越来越需要精确的室内定位导航技术^[1]。

随着室内活动的增加，室内定位对实现人员、物品和空间的高效管理，有着非常大的实际意义。比如智能扫地机器人，公共场所的安防，工业流水线等都需要安装室内定位系统，用以确保机器人的有效工作和人类的安全作业。但是在室内定位中，由于卫星信号无法穿透墙体的阻挡，导致GPS的信号非常微弱，使定位精度严重降低^[1]。另外，由于室内环境封闭性、复杂性和电子设备等干扰源的影响，使信号在传播时，易发生反射、散射和多径效应^[3]，导致室内定位变得困难。因此，提高室内定位的精度将会是定位与导航方面的热点问题和未来发展的重要方向。

本文针对室内封闭环境研究自主导航车系统。通过多种定位导航和数据融合算法相结合，实现室内任意时刻的定位与预定轨迹的自主导航，解决室内封闭复杂环境中定位困难的问题。

2. 室内定位导航技术

现阶段室内定位主要可以通过蓝牙、无线网络、超宽带、红外线和惯性导航等推算位置^[5]。以下列出了几种国内外具有代表性的室内定位导航技术。

蓝牙定位技术：是一种根据接收信号强度指示的定位方法。该技术的优点在于目前大部分手机终端都配置了蓝牙功能，不需要额外的定位装置。但其信号传播距离一般限制在10m以内，需要布置大量的蓝牙子节点，成本过高，而且蓝牙信号不稳定，抗干扰性能弱，易受四周环境的影响。

红外线定位技术：红外线（Infrared）技术通过发射和接收红外线信号进行定位，定位精度较高。但是红外线不能穿透物体，只可在视距等短距离内传输，定位精度与环境状况有直接的关系，易受到灯光和日光的干扰，系统稳定性差^[6]。

惯性导航技术：惯性导航（Inertial Navigation）通过多种惯性器件采集数据进行位置推算，惯性传感器采集的原始数据含有噪声，且在长时间和远距离定位会出现累计误差，导致精度逐渐下降^[7]。为了得到准确和稳定的位置信息，惯性导航技术正向高精度、小型化方向发展。通常将惯性导航技术与其他定位技术进行数据融合，实现组合定位。

3. 数据融合技术

3.1 自适应加权原理

自适应加权融合是多传感器数据融合中的一种方法，特别适用于多个传感器对同一个参数的测量过程，融合过程中不需要数据的先验知识，而是依靠每一时刻的测量数据，融合得到最佳估计值。算法保证总方差最小的前提下，通过每次测量数据，动态计算每个传感器最合适的加权因子，使加权融合得到的数据最可靠。

自适应加权融合模型示意图如图1所示。

图1 自适应加权融合模型

3.2 自适应加权融合技术优势

本文基于加权融合算法完成的组合定位导航，融合系统主要将参考定位得到的数据与航迹推算的结果进行融合，对位姿进行校准，采用多传感器数据融合的优势在于以下几点：

1. 提高了系统的描述精度。由于单一传感器测量的数据会存在噪声和误差，基于多传感器数据融合能够使同一参数的多种数据相互校准，依靠不同时刻的不同可信度，相应的对加权因子做出调整，可以降低测量误差影响和不确定性。

2. 加强了系统的健壮性和自适应性。当某个传感器损坏时，系统能够基于优良的自组织能力不受影响而正常运行。

3. 增加了空间的感知范围和时间稳定性。数据融合系统根据多源采集的信息互相补偿，能够多方位、多维度的描述环境特征，克服单一传感器感知范围不足的缺点，提高探测能力的全面性和准确度。

4. 组合定位导航算法研究

4.1 整体运动过程分析

结合定位导航算法的研究分析和软硬件系统的设计，可以将导航车系统的整体运行过程划分为5个状态，分别是初始化状态、等待状态、反馈状态、调整状态和行驶状态。下面对每个状态进行详细介绍。

初始化状态：对多传感器进行初始化，设定地图坐标原点的初始值，进入等待状态；

等待状态：导航车停止运动，判断是否收到上位机软件发送的64位编码数据，如果没有收到数据，继续保持等待状态；否则，对数据进行解析得到指令，如果指令为获取导航车此刻的位姿（x，y，θ），则停止数据解析过程，进入反馈状态，完成后继续进入等待状态，如果指令为设置导航车下一目标点，则继续解析得到航迹坐标，并进入调整状态；

反馈状态：将导航车当前的位姿（x，y，θ）反馈给上位机软件，然后根据命令进入其他状态；

调整状态：根据此刻位姿信息和目标点坐标，应用几何知识，计算并判断当前位置与目标位置的距离是否小于等于设定的理想误差值，如果小于等于这个设定值，表明导航车已到达目标位置，执行反馈，然后进入等待状态；否则依据两点间距离计算每次的动态行驶距离S，并继续计算导航车航向正对目标点方向所需的航向偏转角度，依靠陀螺仪进行角度旋转，并更新航向角θ，然后进入行驶状态；

行驶状态：向前行驶，并判断行驶的距离是否达到S，如果大于等于S，对多传感器测量的航向偏移角进行数据融合，得到最优值，然后通过这一阶段行驶的距离和航向偏移角进行航迹推测，得到初步计算值（x，y，θ）然后应用参考定位算法结合已知地图信息，对位置坐标（x，y）进行校准，更新导航车的位姿（x，y，θ），执行反馈，并进入调整状态；否则，继续保持行驶状态。

导航车的整体运动状态示意图如图2所示。

图2 自主导航车运动状态图

导航车整体运动状态中的S参数说明：

S参数是更新一次坐标中动态行驶的距离，它也是判断当前点与目标点的距离大小的参数，通过动态的S参数可以确定是否到达目标点。它在导航车运行过程中与控制精度，定位精度有很大的关系，初始值由理论依据和实际调试情况确定。

理论分析中，S参数的值越小，则系统精度越高，S无限小，代表时刻在更新坐标与位姿校正，对提高精度有帮助，但是实际调试过程中，发现如果S过小，则导致导航车由于惯性的影响，不能立刻停止，从而导致向前滑动，使得测量的行驶距离误差累计次数增多，并且由于S过小则各个传感器的变化量变小，意味着最小测量分度值变小，需要各个传感器提供更高的精度，所以要合理的限制S的大小；如果S过大，则表明需要在一段长距离之后，才能更新坐标并校正自身姿态，累计误差会加剧。所以合理的选取S参数的初始值，对导航车的精度有重大的影响。

经过调试，S参数选取的初始值为20cm，导航车运行效果较好。

4.2 定位导航算法流程

在导航车的整体运动过程中，当接收到新的目标点后，通过定位导航算法与数据融合完成一次定位导航。如图3所示，展示了一次定位导航算法的程序设计图。

图3 定位导航算法流程图

4.3 融合与位姿校准过程

导航车在更新坐标前时，通过多传感器进行数据融合，完成位姿的校准。其中位姿的校准分为位置坐标(x，y)和航向角θ两部分的校准。

在航迹推测中，主要依靠霍尔编码器记录行驶的距离和陀螺仪测出的角度进行位置的初步估算，然后在更新坐标前，如果霍尔编码器计算的角度与电子罗盘测量的角度在设定的合理阈值内，则将两个角度信息与陀螺仪测量的角度进行加权融合，得到最优航向角θ；

定位过程中需要用到红外测距传感器时，多个红外测距传感器需要进行自适应加权融合，得到可信度高的距离信息。结合导航车的车身长度，宽度和已知地图信息，应用参考定位算法结合航迹推算的结果，得到最优位置坐标(x，y)；

至此，经过航迹推算与参考定位结合，并加入数据融合与位姿校准算法，完成导航车的组合定位导航。如图4所示，说明了本系统中所有的传感器数据融合完成位姿校准的过程。

图4 融合及位姿校准示意图

4.4 系统测试与分析

4.4.1 行走轨迹效果

测试并验证验证系统的导航功能，对自主导航车进行预定轨迹导航测试，在软件中规划导航车的航迹路线，使导航车按照预定轨迹进行自主定位导航，行走出相应的轨迹。

为了清晰的演示自主导航车行走轨迹效果，分别进行长方形，对角线，任意弧形轨迹测试，在每次导航过程中，更新并实时发送导航车的当前位置坐标。

测试条件为灯光充足且柔和的夜间，测试场地是四周为50mm高的围墙，2.4*1.2米的长方形木板上。

1. 长方形轨迹

对于长方形轨迹，导航车接收4个点，分别是（150，0），（150，50），（0,50），（0,0），如图5所示，展示了导航车行走长方形轨迹时更新的坐标点效果图。

图5 长方形轨迹效果图

从图中数据和线条可以看出，自主导航车基本实现了长方形轨迹的行走，其中在两条长边上行进到轴坐标为100时，误差由于累积变大，但是经过位姿校准以后，逐渐回归到正常位置。

2. 对角线轨迹

对于对角线轨迹，导航车接收4个特征点，分别是（200，80），（200，0），（0,80），（0,0），如图6所示，展示了导航车行走对角线轨迹时更新的坐标点效果图。

图6 对角线轨迹效果图

由图中可以看出，自主导航车基本实现了对角线轨迹的行走，且误差较小，说明了数据融合算法的有效性，也验证了组合定位导航算法的可靠性。

3. 任意弧形轨迹

对于任意弧形轨迹，控制仿真软件将弧形拆分成的多个小线段，导航车分别接收这些离散的点，图7展示了导航车行走弧形轨迹时更新的坐标点效果图。

图7 弧形轨迹效果图

由弧形轨迹联合测试图和轨迹坐标效果图可以看出，导航车运行的轨迹基本和软件航迹规划的路线一致，实现了任意弧形轨迹的行走。

由行走轨迹效果图可以看出，导航车能够按照航迹规划的路线完成长方形、对角线和任意弧形轨迹的行走。并且每两点间的距离是不一样的，它代表了行走时坐标点更新的距离，也验证了前文中提到的重要参数S的动态性，即更新一次坐标中动态调整的行驶距离，参数S在接近目标点时会逐渐的减小，直到小于设定值，说明到达目标点。

4.4.2 准确性和稳定性测试

自主导航车系统能够进行任意轨迹的行走后，对系统的准确性和稳定性测试。对自主导航车进行5次测试，在同一初始点（0，0）出发行走到不同目标点，对此结果进行误差和稳定性分析。测试条件为灯光充足且柔和的夜间，测试场地是四周为50mm高的围墙，2.4*1.2米的长方形木板上。测试结果如表1。

表1 自主导航车测试结果

测试结果表明，对自主导航车进行的5次测试都完成了预设目标，基本实现了定位导航过程，导航车能够在设定好目标点后，通过自主定位导航实时调整运动状态，直至到达目标点，且坐标误差保持在8cm范围内，稳定性较强。

4.4.3 组合定位导航实验分析

为了说明本文提出的组合定位导航方式的有效性，对本系统设计针对性实验，分别测试自主导航车依靠航迹推算定位和组合定位导航两种方式的结果并进行误差比较，其中航迹推算用霍尔编码器和陀螺仪记录距离和角度，组合定位导航使用多传感器进行数据融合，与距离和角度信息相互校准。

导航车在两种方式的基础上分别行驶5次由（0，0），（150，0），（150，50），（0，50）四个点组成的长方形的轨迹，最后回到（0，0）坐标点。测试条件为灯光充足且柔和的夜间，测试场地是四周为50mm高的围墙，2.4*1.2米的长方形木板上。测试结果如表2。

表2 航迹推算和组合定位导航方式测试结果

由表中多次测试数据可以看出，组合定位导航方式与单一的航迹推算定位比较，误差普遍减小4~5cm，最后的定位导航精度维持在8cm范围内。说明了本文提出的由航迹推算定位和参考定位相结合，并加入数据融合算法与位姿校准的组合定位导航方式精度提高。

5. 总结与展望

本文通过多传感器的数据融合，实现自主导航车系统的多个普通定位传感器的数据相互校验与修正，完成高精度的定位，并依靠航迹推算和参考定位相结合的方式完成了组合定位导航相关实验，实现了自主导航车任意两点间的定位导航与任意轨迹的行走，证明了系统的稳定性和准确性，且相比于单一的航迹推算定位，精度明显提高。

参考文献

[1] Ni L M, Liu Y, Lau Y C, et al. LANDMARC: Indoor Location Sensing Using Active RFID[J]. Wireless Networks, 2004, 10(6):701-710.

[2] 赵延鹏. 时伟, 艾明曦. 室内行人航迹推算/超声波组合定位融合算法[J]. 中南大学学报(自然科学版), 2016, 47(5):1588-1598.

[3] 王珊. 基于惯性传感器和WiFi联合室内定位方法的研究与实现[D]. 南京邮电大学, 2015.

[4] 陆振宇. 自主导航移动机器人的设计[D]. 南昌大学, 2013.

[5] DENG, Zhongliang, Yanpei, et al. Situation and Development Tendency of Indoor Positioning[J]. China Communications, 2013, 10(3):42-55.

[6] 王玉峰. 自主移动机器人地图构建、探索及定位研究[D]. 大连理工大学, 2004.

[7] 贾子庆. 基于WLAN指纹和惯性测量的室内定位系统设计与研究[D]. 哈尔滨工业大学, 2014.