利用MEMS-IMU 检测车辆运动状态的自适应方法

胡昊杰，朱 锋，张小红,2

（1. 武汉大学 测绘学院，武汉 430079；2. 地球空间信息技术协同创新中心，武汉 430079）

0 引言

随着自动驾驶、车联网等技术的发展，人们对车辆定位的要求不断提高。定位的精度和稳定性直接影响着自动驾驶汽车的导航、决策以及高精度地图的构建[1]。精确、连续、稳定的定位结果不仅可以为驾驶员自身提供丰富的行车信息，还可以为交管部门、其他驾驶员提供准确的路况信息，提高道路交通的安全性和效率。

传统的组合导航技术采用全球卫星导航系统 （global navigation satellite system， GNSS）与捷联惯性导航系统（strapdown inertial navigation system， SINS）相结合，可以满足上述领域对高精度、高可用性、高可靠性的要求。但受限于高级别惯性测量装置（inertial measurement unit， IMU）昂贵的价格，高精度组合导航系统一般只能用于移动测量等专用领域，难以在民用市场普及。微机电系统（microelectro-mechanical system， MEMS）由于成本较低，可以满足大众的需求。

为了提高车辆的定位精度，在解算过程中常常采用零速修正、速度约束等方法来提高精度，而这些方法在特定的车辆运动条件下使用才能取得更好的效果。这意味着对车辆运动状态的准确检测至关重要，比如判断车辆停止、直行或转弯。现有研究主要通过阈值法进行零速探测：文献[2]通过加速度计X 轴、Y 轴在窗口内的最大值与最小值之差来判断零速；文献[3]利用加速度计方差，并引入航向角进行探测；文献[4]通过加速度计X 轴、Y 轴数据的标准差配合磁力计磁强变化来判断车辆是否静止；文献[5]通过滑动窗口内陀螺和加速度计数据的标准差来判断。但阈值法一般不具有通用性，不同的设备需要确定不同的阈值。文献[6]通过处理样本数据获得标准差（standard deviation，STD）静止概率曲线来实现对阈值的自适应选取；文献[7]通过时频变化，从频域上进行探测。这些方法往往难以满足当前定位技术对实时性和灵活性的需求。在车辆转弯检测方面，文献[8-9]直接将MEMS-IMU 安装在方向盘上检测转向，但不同车型、不同路况条件下，方向盘转动不能真实反映车辆转向情况；文献[4]定义了直线、摆动、曲线、转弯4 种车辆运动状态，并通过陀螺Z 轴数据的均值来检测后2 种状态；文献[5]考虑了IMU 与车辆的安装角问题，采用陀螺3 轴输出判断转弯，但在测试时，主要针对大角度转弯。

本文提出的车辆运动状态检测自适应方法，是利用滑动窗口内的MEMS-IMU 数据提供零速探测阈值，阈值随窗口滑动不断变化，具有良好的灵活性和通用性；利用MEMS-IMU 数据计算航向角判断转弯，并通过速度变化实现阈值的自适应，可检测出小弧段变化及车辆变道的状态。

1 车辆运动状态约束分析

在组合导航系统中，除了GNSS 作为外部观测更新手段外，零速修正、速度约束等方法常用来抑制SINS 误差的快速累积。本节将介绍准确检测车辆运动状态对于使用约束算法取得良好定位效果的重要性。

在城市环境下，停车等待是车辆运动中不可避免的1 种状态。在理想条件下，在车辆停止期间，MEMS-IMU 输出的部分数据应该是零，所以实际输出可认为是 MEMS-IMU 的零偏，从而为传感器在线校正提供辅助。此外，零速探测也是零速修正的关键技术。零速修正可以通过卡尔曼（Kalman）滤波的系统模型建立速度和位置误差之间的关系，进而对SINS 递推的位置误差进行校正[10]。零速修正的约束作用可以通过观测方程体现。根据文献[10]，线性形式的零速修正观测方程可表示为

因此，零速探测对数据补偿、算法校正等具有重要作用。

速度约束是指地面车辆在不发生跳动和侧滑的情况下，侧向速度和垂向速度为零，这可以为组合导航系统提供侧向和垂向的虚拟观测信息。对于MEMS-IMU，速度约束可以显著改善 GNSS 失锁阶段的定位精度[11]。速度更新可表示为

速度约束中最关键的是速度约束噪声的选取：约束过松，会降低约束的有效性；约束过紧，则反而会使结果变差。车辆在行驶中，发生侧滑及垂向跳动是非常普遍的现象，尤其是在转弯过程和速度较高的情况下[10]更是如此。对于约束噪声，一般会给1 个定值，但这并不适用于所有运动状态。文献[12]通过神经网络训练，结果表明转弯时的约束噪声远大于直线时的约束噪声。所以在车辆直线运动时，可以利用速度约束得到较好的定位结果，但在转弯时，速度约束对结果的提升作用很小。

此外，在车辆穿过高架桥、天桥等卫星信号短时遮挡的环境时，前后几个历元的定位结果可能出现偏差，此时若能保证车辆直线行驶，则可直接利用航向角信息和速度信息朝前进方向递推，获得更为准确的位置结果。

所以车辆转弯的准确检测，可为提升车辆定位结果提供较大辅助。

2 车辆运动状态自适应检测方法

本节将分析车辆运动状态与MEMS-IMU 数据的相关性，说明MEMS-IMU 加速度计、陀螺数据变化分别与车辆零速、转弯具有明显对应关系，并介绍自适应方法的原理及实现。

2.1 零速探测

本文采用加速度计输出作为零速探测的依据。首先选取了1 段车辆动静态交替且区分较为明显的数据段1（Data1），加速度计3 轴输出与车辆是否运动如图1 所示。

图1 Data1 时加速度计3 轴输出变化

从图1 中可以看出，当车辆静止时，加速度计3 轴输出趋于平稳，而在运动时，则波动较大。根据这个特性，本文采用加速度计数据的STD 来判断车辆是否静止。若在窗口时间内有N 组数据，则这N 组数据的某1 轴的STD 可表示为

式中：iA 为第i 个历元的加速度计单轴输出；μ 为对应轴N 组数据的平均值。

对同一时段内加速度计3 轴在动态和静态情况下，每秒计算1 个STD，可得到图2 至图4 的时序对比图。

图2 Data1 时加速度计X 轴动静态STD 对比

图3 Data1 时加速度计Y 轴动静态STD 对比

图4 Data1 时加速度计Z 轴动静态STD 对比

MEMS-IMU 在车上的安装方式为：X 轴朝向车辆右方；Y 轴朝向车辆前方；Z 轴朝向车辆上方。从图2～图4 可以看出，X 轴和Z 轴的STD 有相似的特征：在每1 个独立的静止时段内，STD 整体呈现逐渐增大的趋势，这可能是实验车辆在停车怠速情况下，抖动会逐渐增大导致的；对于不同的静止时段，STD 的均值具有一定差异。这说明临近时间的STD 对阈值确定具有更大的参考价值。Y 轴指向为车辆前进方向，在启动和停止的临界时间段，STD 较不稳定。基于这样的特性，本文提出的零速探测方法，是根据固定长度的滑动窗口内得到的X 轴和Z 轴STD 来计算零速阈值的，阈值可表示为

式中：λS为阈值；n 为窗口长度，这里的窗口指的是用于计算阈值的固定窗口，而式（4）中的窗口N 是用于计算STD 的，本文取N=1，所以在计算阈值的窗口内，每个轴会有n 个STD 值； ai为窗口内的时间顺序排列，每秒计算的STD 值； pi为权重，且 p1≤ p2≤ … ≤pn，以此保证越接近当前历元的STD，对阈值设定的影响越大；k 为放大系数，考虑到图2、图4 中X 轴和Z 轴每秒计算的STD在独立的静止时段内呈增大趋势，k 应大于1，但又不宜过大，根据测试数据的趋势，本文取k=1.3。由于窗口长度是固定的，所以窗口内数据会不断更新。若当前时段内计算的X 轴和Z 轴STD 分别小于2 轴阈值时，则认为当前时段车辆静止。本文的窗口长度n 设计为30 s，故窗口内X 轴和Z 轴的STD 值都有30 个。窗口长度根据测试数据独立静止时段的长度分布及不同长度窗口的实验结果进行设计，结果表明10～50 s 的结果差异很小，但窗口过长会导致结果变差，故将窗口长度固定在30 s。权重可表示为

式中i 为的排序号。

本文以STD 值在窗口内的排序号的平方为依据来设计权重：相比于1 次方，可以增加相近历元的影响；相比于高次方，不会过于突出相近历元，以抑制粗差对阈值计算的影响。

零速探测的结果可用探测率和误判率来评估。探测率表示探测出的零速历元在真实零速历元中所占的比例，误判率表示探测出的零速历元在实际为动态的历元所占的比例。探测率越高，误判率越低，则零速探测的结果越好。

2.2 转弯检测

车辆在行驶过程中包含多种状态，本文将这些状态归为5 种，如图5 所示。

图5 车辆行驶的5 种状态

实际情况是，车辆不可能完全沿直线行驶，难免会有微小摆动，本文把图5（a）和图5（b）这2 种情况都认定为直线行驶。现有研究一般不考虑变道，但针对自动驾驶、车联网等技术，车道级的定位精度是必要的，所以本文把变道与大弧段、转弯一并认定为车辆非直线行驶。

本文采用窗口时间内陀螺输出算得的航向角变化（即航向角变化率）作为判断车辆是否转弯的依据，车辆转弯需满足的条件可表示为

式中： δY为航向角变化率；N 为窗口内的数据总量；gz( i )为陀螺Z 轴方向的输出；dt 为采样间隔；T 为窗口时长；Yδλ 为阈值。

这里选取1 段包含几个转弯路段的数据段2（Data2），角度变化率与车辆是否转弯的关系如图6 所示。

图6 Data2 航向角变化率

从图6 可以看出：航向角变化率快的历元与车辆转弯的历元存在明显对应关系。图6 中2 条横线对应的横坐标为±2 (° ) ⋅s-1，基本区分了不同的角度变化率与是否转弯的关系，所以2 (° ) ⋅s-1可以作为转弯阈值。

然而，对于变道和小角度弧段等情况，不得不考虑车速对转弯角度的影响。变道的目的在于快速地从1 条车道切换至另1 条车道，除堵车外，大多数变道都应在短时间内完成，即不论车速快慢，变道的时间应该是基本相同的。这种不同车速对变道的影响可以通过图7 体现。

图7 车速对变道的影响

假设在相同时间内完成变道，图7（a）车速快于图7（b），α1小于α2，说明较快的车速只需较小的航向角变化即可完成变道。此外，曲线道路往往由于地形原因而把路修得带有一定弧度，一般没有岔路，弯道较缓，车速较快。因此，本文提出的方法针对不同车速设计不同阈值，判断车辆转弯应满足的条件可表示为

式中： λ1、 λ2、 λ3、 λ4为角度变化率 阈 值，λ1＞ λ2＞ λ3＞λ4；v1、v2、v3为速度阈值，v1＜ v2＜v3。由于本文测试路段为城市道路，一般限速最高为60 km ⋅ s-1，故本文最高速度阈值设置为13 m ⋅ s-1，即46.8 km ⋅ h-1。通过分析测试数据，根据车辆航向角变化率的变化情况与是否在变道或在小角度弧段中行驶的关系，设计阈值为： λ1= 2. 0 ，λ2=1 . 5 ，λ3=1 . 0 ，λ4= 0. 7，单 位 为 (° ) ⋅ s-1； v1=5 ，v2=1 0，v3=13，单位为 m ⋅ s-1。

3 实验与结果分析

实验数据采集使用的MEMS-IMU为MPU9250，采用Novatel SPAN-FSAS 高精度组合导航系统采集的数据经IE（inertial explorer）软件计算的紧组合结果作为参考真值。MEMS-IMU 与高精度IMU均固连于架设在车顶行李架的钢板上，各自的载体坐标系轴向一致，运动状态基本相同。数据采集地点为武汉市洪山区，行车轨迹如图8 所示。

图8 实验路线

3.1 零速探测

利用高精度组合系统数据计算的速度结果，认为水平速度小于0.05 m ⋅ s-1的历元是静态，以此得到全时段车辆动静态情况，如图9 所示。

图9 全时段车辆动静态情况

从图9 可以看出，除实验开始和结束时2 段有较长时间的静置外，中间时段频繁出现运动和静止交替出现的情况，真实反映了城市环境中的行车情况。

图10、图11 分别反映了X 轴和Z 轴全时段静态STD 值与按照本文提出的方法不断计算更新的阈值的对比情况。

图10 X 轴静态STD 与阈值对比

图11 Z 轴静态STD 与阈值对比

从图10（a）、图11（a）可以看出，在900 s和1 200 s 左右有2 段时间的STD 很大，X 轴达到0.5 m ⋅ s-1左右，Z 轴达到0.7 m ⋅ s-1左右，这里为了更好地体现细节而把图中的Y 轴范围适当缩小。并且前面静置段的STD 也并不是很平稳。这是由于实验起始段停车静置时，实验人员不在车上， 但调试设备、取放东西等对车辆造成了轻微影响，导致STD 值波动；静置完成后，人员先后上车，开关车门与上车会对X 轴、Z 轴数据产生影响，导致STD 大幅上升。从图10（b）、图11（b）的阈值来看，这样的数据抖动基本被平滑了。这是可以接受的，因为即使这些历元车速的确为零，但考虑到零速状态可能用于数据补偿，使用这些历元的数据并不是好的选择。而当车辆在道路上正常行驶，动静交替时，可以看到阈值变化趋势与静态STD 基本相符，且一般情况下阈值略大于STD。

本文零速探测的结果首先统计了实验全时段，得到91.91%的探测率和9.92%的误判率；考虑到驾驶员实际开车时一般不会有起始段长时间的静止，本文又统计了图7 中从T 时刻至实验结束的结果，得到94.25%的探测率和10.32%的误判率。统计如表1 所示。

表1 零速探测统计结果

表1 中：探测成功历元数A 为探测出的历元中实际为静止的历元数；实际静止历元数B 为实际中静止的历元数；探测静止历元数C 为探测出静止的历元数。探测率D 和误判率E 可表示为

3.2 转弯检测

首先给出采用固定阈值进行转弯检测的结果，如图12 所示。

图12 固定阈值的转弯检测结果

从图12 中可以看出，对于直角弯、掉头、环岛以及部分弧段，固定阈值算法可以检测到，但对于图12 中圈出的包含角度缓变曲线的路段，固定阈值算法并不能检测成功。

根据式（7）添加速度条件后，依速度自适应阈值进行弯道检测的结果如图13 所示。

图13 根据速度自适应阈值的转弯检测结果

从图13 中可以看出，整个行车路线中较为明显的弯道及弧段已全部检测出。同时部分直道上也新增了被检测出的轨迹点。实际上这些轨迹可能包含着变道信息，这也是固定阈值算法难以检测出的。以图13 中的圆圈标记为例，放大后的轨迹投影在谷歌地球上如图14 所示。

车辆在这1 段道路上先后行驶过2 次，中间路段2 条轨迹分离，从投影的轨迹来看，车辆行驶在2 条车道上时，可以检验出轨迹分离与合并处的坐标点，这证明本文的算法对变道的检测是可行的。

图14 变道示意图

此外，本文以图12 中上方标记处的路段为例，说明根据不同速度设置阈值是如何影响转弯检测的。此路段包含了速度阈值划分的所有速度区间，整体上车速较快。首先给出此路段的速度变化情况，如图15 所示，图例速度的单位为 m ⋅ s-1。

图15 示例路段速度变化情况

添加不同个数的速度分段阈值后，检测结果对比如图16 所示。

图16 不同速度阈值对转弯检测的影响

图16（a）为不加速度限制的固定阈值算法，无法检测出这个带弧线路段的任何弯曲部分，说明初始阈值对小角度弧段的检测能力很弱。图16（b）相较于图16（a）在速度大于5 m ⋅ s-1时添加1 个阈值，检测出了2 个明显弧段，结合图15 可以看到，这2 个弧段的速度都在13 m ⋅ s-1以上，说明最初的阈值设置对于这2 个弧段略高，在非低速情况下可适当降低阈值。图16（c）相较于图16（b），在速度大于10 m ⋅ s-1时添加1 个阈值，多检测出了1 个弧段和1 个变道。这个弧段与之前已经检测出的2 个相比，角度更小，所以需要更小的阈值；而检测出的变道路段的速度处在新增阈值的这个速度段内，说明这个速度段和阈值的设计是合理的。图16（d）相较于图16（c），在速度大于13 m ⋅ s-1时添加1 个阈值，由于此路段的转弯已经全被检测出，所以图16（d）相较于图16（c）无明显差异。由此看来，根据速度改变阈值对于检测小角度弧段、变道是很有必要的。

4 结束语

本文提出了根据滑动窗口内的加速度计数据在线确定阈值的零速探测方法，以及根据速度自适应阈值进行转弯检测的方法。首先说明了车辆运动状态的准确检测对提升定位算法结果的重要性，然后分析了IMU 数据与车辆运动状态之间的关系，表明加速度计输出与车辆是否静止、陀螺航 向轴数据与车辆转弯具有明显联系，并介绍了算法的设计。测试结果表明零速探测方法具有较高成功率，探测率为94.25%，误判率为10.32%；转 弯检测算法可检测出车辆行驶中绝大部分转弯、小弧段变化及变道情况，并分析了不同速度及对应阈值对转弯检测的影响。