基于MEMS传感器的道面坡度提取方法

摘要

一种基于MEMS传感器的道面坡度提取方法。其包括数据采集、零加速状态检测、旋转四元数估计以及位置解算与坡度提取等步骤。在不增加额外传感器的基础上，本发明采用MEMS传感器实现对道面坡度提取的方法。该方法仅利用智能手机的MEMS传感器，通过零加速状态检测的方法实现对累积误差的修正，并利用重力加速度方向不变性，通过载体坐标系中的重力加速度矢量方向的变化间接地实现了道面坡度信息提取。实验结果表明，本发明算法可以准确地提取道面坡度信息并实现对道面坡度信息的定位。

权利要求

1.一种基于MEMS传感器的道面坡度提取方法，其特征在于，所述的基于MEMS传感器的道面坡度提取方法包括按顺序进行的下列步骤：

(1)利用智能手机内置的三轴加速度计、三轴陀螺仪和三轴磁力计在内的传感器采集小车运动数据，得到载体坐标系下三轴加速度、三轴角速度和三轴磁分量的S1阶段；

(2)对步骤(1)得到的载体坐标系下三轴加速度和三轴角速度进行零加速检测，以判定小车是否处于零加速状态的S2阶段；

(3)根据步骤(2)得到小车是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下旋转四元数估计值的S3阶段；

(4)利用步骤(3)得到的旋转四元数估计值实现三轴加速度和三轴角速度在载体坐标系与惯性坐标系之间的转换，并对地理坐标系的速度向量和位置向量进行更新，得到小车位置，同时进行坡度提取的S4阶段；

在步骤(2)中，所述的对步骤(1)得到的载体坐标系下三轴加速度和三轴角速度进行零加速检测，以判定小车是否处于零加速状态的方法是：在零加速状态时，三轴加速度计的输出应为重力加速度的值，而三轴陀螺仪的输出应为零，分别计算三轴加速度和三轴角速度范数，设置阈值来判别当前时刻范数是否满足上述零加速状态下的条件。

2.根据权利要求1所述的基于MEMS传感器的道面坡度提取方法，其特征在于，在步骤(3)中，所述的根据步骤(2)得到的小车是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下旋转四元数估计值的方法是：若小车处于零加速状态，则根据零加速状态的特点对旋转四元数初始化；若小车处于非零加速状态，则根据前一时刻的旋转四元数进行旋转四元数更新。

3.根据权利要求1所述的基于MEMS传感器的道面坡度提取方法，其特征在于，在步骤(4)中，所述的利用步骤(3)得到的旋转四元数估计值实现三轴加速度和三轴角速度在载体坐标系与惯性坐标系之间的转换关系，并对地理坐标系的速度向量和位置向量进行更新，同时进行坡度提取，得到小车位置的方法是：小车处于零加速状态条件时，将三轴加速度置为零，当前时刻的速度向量应该等于前一时刻的速度向量；小车处于非零加速状态条件时，利用旋转四元数估计值将三轴加速度由载体坐标系转换到惯性坐标系，将惯性坐标系下三轴加速度乘以采样时间加上前一时刻的速度向量来更新当前时刻的速度向量，然后将速度向量乘以采样时间加上前一时刻位置得到当前时刻的位置向量，重复上述过程即可得到小车位置；提取坡度的方法是：在坡度测量的过程中，地理坐标系是不动的，导航坐标系只是相对于地理坐标系平移，因此在两个坐标系中的重力加速度向量gn是不变化的；对于载体坐标系，在坡度测量的过程中是存在相对姿态的变化，即载体坐标系相对导航坐标系发生旋转，而且这个角度的变化是与平台所接触面的坡度有关，因此，在载体坐标系中重力加速度向量gb(t)的方向变化量均反映着平台所接触面的坡度变化情况。

说明书

基于MEMS传感器的道面坡度提取方法

技术领域

[0001] 本发明属于坡度提取技术领域，特别是涉及一种基于MEMS传感器的道面坡度提取方法。

背景技术

[0002] 由惯性测量单元(inertial measurement unit,IMU)组成的惯性导航系统(inertial navigation system,INS)可以精确地定位车辆的水平位置，但对高度和坡度的测量需额外的辅助传感器提供辅助参数。在车辆行驶过程中若遇到上下坡以及不同坡度的高架桥路面时，导航系统便有可能产生较大的估计偏差。如何较为精确地实现对高度位置和道面坡度的解算，引起业界学者的广泛关注。

[0003] 国内外道路坡度识别的方法可从实现原理上分为三类：基于全球定位系统(global positioning system,GPS)数据的方法、基于车辆动力学模型的方法和基于加速度估计的方法。基于GPS数据的方法，其测量精度取决于GPS的有效使用范围，且易受遮挡物的影响。基于车辆动力学模型的方法是建立车辆与道路坡度之间的动力学关系模型，其对传感设备精度要求较高。基于加速度估计的方法是利用加速度传感器估计车辆的俯仰角，并提取道路坡度信息。基于加速度估计的方法采用的加速度传感器大多是便携式微电子机械系统(micro-electromechanical system,MEMS)传感器。该类传感器采集的数据精度较低，噪声干扰相对较大，且在解算过程中需要对加速度进行积分处理，引入了累积误差的问题，因此，此类方法通常需额外的辅助传感器提供辅助参数。J.Jang等利用轮速传感器采集的车辆速度数据，并据此解算出轨迹信息，Y.Son等采用气压计传感器来获得道路的绝对高度信息，张海等采用里程仪通过建模得到坡度，A.Norrdine等利用置于足底的传感器检测行人行走过程中脚触地时短暂的零速状态来实现对累积误差的修正。

发明内容

[0004] 为了解决上述问题，本发明的目的在于提供一种基于MEMS传感器的道面坡度提取方法。

[0005] 为了达到上述目的，本发明提供的基于MEMS传感器的道面坡度提取方法包括按顺序进行的下列步骤：

[0006] (1)利用智能手机内置的三轴加速度计、三轴陀螺仪和三轴磁力计在内的传感器采集小车运动数据，得到载体坐标系下三轴加速度、三轴角速度和三轴磁分量的S1阶段；

[0007] (2)对步骤(1)得到的载体坐标系下三轴加速度和三轴角速度进行零加速检测，以判定小车是否处于零加速状态的S2阶段；

[0008] (3)根据步骤(2)得到小车是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下旋转四元数估计值的S3阶段；

[0009] (4)利用步骤(3)得到的旋转四元数估计值实现三轴加速度和三轴角速度在载体坐标系与惯性坐标系之间的转换，并对地理坐标系的速度向量和位置向量进行更新，得到小车位置，同时进行坡度提取的S4阶段。

[0010] 在步骤(2)中，所述的对步骤(1)得到的载体坐标系下三轴加速度和三轴角速度进行零加速检测，以判定小车是否处于零加速状态的方法是：在零加速状态时，三轴加速度计的输出应为重力加速度的值，而三轴陀螺仪的输出应为零，分别计算三轴加速度和三轴角速度范数，设置阈值来判别当前时刻范数是否满足上述零加速状态下的条件。

[0011] 在步骤(3)中，所述的根据步骤(2)得到的小车是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下旋转四元数估计值的方法是：若小车处于零加速状态，则根据零加速状态的特点对旋转四元数初始化；若小车处于非零加速状态，则根据前一时刻的旋转四元数进行旋转四元数更新。

[0012] 在步骤(4)中，所述的利用步骤(3)得到的旋转四元数估计值实现三轴加速度和三轴角速度在载体坐标系与惯性坐标系之间的转换关系，并对地理坐标系的速度向量和位置向量进行更新，同时进行坡度提取，得到小车位置的方法是：小车处于零加速状态条件时，将三轴加速度置为零，当前时刻的速度向量应该等于前一时刻的速度向量；小车处于非零加速状态条件时，利用旋转四元数估计值将三轴加速度由载体坐标系转换到惯性坐标系，将惯性坐标系下三轴加速度乘以采样时间加上前一时刻的速度向量来更新当前时刻的速度向量，然后将速度向量乘以采样时间加上前一时刻位置得到当前时刻的位置向量，重复上述过程即可得到小车位置。提取坡度的方法是：在坡度测量的过程中，地理坐标系是不动的，导航坐标系只是相对于地理坐标系平移，因此在两个坐标系中的重力加速度向量gn是不变化的。对于载体坐标系，在坡度测量的过程中是存在相对姿态的变化，即载体坐标系相对导航坐标系发生旋转，而且这个角度的变化是与平台所接触面的坡度有关，因此，在载体坐标系中重力加速度向量gb(t)的方向变化量均反映着平台所接触面的坡度变化情况。

[0013] 本文利用智能手机内置的MEMS传感器实现道面坡度提取，是在不外接惯性传感器件的情况下，直接利用智能手机的惯性传感器数据进行零加速状态检测，并在零加速状态时对定位系统进行状态估计与修正，实现高度解算，同时利用重力场的不变性，通过其在坡度测量中相对姿态的变化，实现了对道面坡度的提取。

附图说明

[0014] 图1为本发明提供的基于MEMS传感器的道面坡度提取方法流程图。

[0015] 图2为相关坐标系间关系。

[0016] 图3为坡度提取原理图。

[0017] 图4为实验场景模型。

[0018] 图5为实验结果。

具体实施方式

[0019] 下面结合附图和具体实施例对本发明提供的基于MEMS传感器的道面坡度提取方法进行详细说明。

[0020] 如图1所示，本发明提供的基于MEMS传感器的道面坡度提取方法包括按顺序进行的下列步骤：

[0021] (1)利用智能手机内置的三轴加速度计、三轴陀螺仪和三轴磁力计在内的传感器采集小车运动数据，得到载体坐标系下三轴加速度、三轴角速度和三轴磁分量的S1阶段：

[0022] 智能手机内置三轴加速度计、三轴陀螺仪和三轴磁力计等传感器，在智能手机置于小车上并随小车一起运动时，利用手机端Matlab通过IP与电脑端Matlab互联，可以实时将三轴加速度、三轴角速度和三轴磁分量采集到电脑中。各传感器采集的数据将以载体坐标系数据的形式输出。

[0023] (2)对步骤(1)得到的载体坐标系下三轴加速度和三轴角速度进行零加速检测，以判定小车是否处于零加速状态的S2阶段：

[0024] 零加速状态就是小车的平动加速度和转动加速度均为零的状态，即小车处于受力平衡态。虽然此时的速度可能不为零，但此时载体坐标系与导航坐标系间的位置关系相对固定，可以进行旋转四元数初始化，从而实现对惯性器件累积误差的修正。

[0025] 在零加速状态时，理论上三轴加速度计的输出为重力加速度的值，三轴陀螺仪的输出为零，但由于噪声的存在，在进行零加速检测时，需要对三轴加速度计采集的载体坐标系的加速度向量ab(t)＝[axt ayt azt]T和三轴陀螺仪采集的角速度向量ωb(t)＝[ωxt ωyt ωzt]T进行范数阈值判定。

[0026] 利用加速度向量ab(t)进行加速度检测：

[0027]

[0028] 其中||·||表示向量的范数，thamin与thamax为加速度向量ab(t)的范数的上、下阈值。

[0029] 利用角速度向量ωb(t)进行角速度检测：

[0030]

[0031] 其中，thωmax为角速度向量ωb(t)的范数的阈值。

[0032] 如上所述，同时满足以上两种条件时判定为零加速状态，即：

[0033] T(t)＝T1(t)×T2(t)  (3)

[0034] (3)根据步骤(2)获得的小车是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下旋转四元数估计值的S3阶段：

[0035] 当步骤(2)判定小车处于零加速状态时，利用零加速姿态估计方法得到此时的旋转四元数估计；当步骤(2)判定小车处于零加速状态，则根据零加速状态的特点对旋转四元数初始化；若判定小车处于非零加速状态，则根据前一时刻的旋转四元数进行旋转四元数更新。

[0036] 当小车处于零加速状态时，其在平衡力作用下保持运动状态不变，此时载体坐标系与导航坐标系之间的位置关系保持不变，因此可以利用传感器采集的数据直接实现对旋转四元数的初始化，从而消除前期累积误差的影响。

[0037] 根据图2定义的坐标系间关系，载体坐标系到导航坐标系的变换可以看作坐标系绕导航坐标系定义的某单位向量un(t)＝[uxt uyt uzt]T逆时针旋转θnt角度得到导航坐标系。因此，载体坐标系下三轴加速度计输出的重力加速度向量gb(t)＝[gxt gyt gzt]T和三轴磁力计输出的磁场向量hb(t)＝[mxt myt mzt]T也可绕单位向量un(t)逆时针旋转θnt角度得到导航坐标系的重力分量gn＝[0 0 -g]T和磁场分量hn＝[hx hy hz]T，其中g为当地重力加速度，hx、hy和hz为当地磁力线在地理坐标系中各轴向的分量，可由国际地磁参考场(international geomagnetic reference field，IGRF)获得。由此可知，单位向量un(t)一定位于重力分量gn和重力加速度向量gb(t)之间的对称面上，该对称面的法向量为：

[0038] r1(t)＝[gxt gyt gzt+g]T  (4)

[0039] 同理，单位向量un(t)也一定落在导航坐标系下的磁场分量hn与载体坐标系下的磁场向量hb(t)之间的对称面上，相应对称面的法向量为：

[0040] r2(t)＝[mxt-hx myt-hy mzt -hz]T  (5)

[0041] 所以，单位向量un(t)位于式(4)和(5)定义的两平面的交线上，该交线的方向向量：

[0042] ul(t)＝r1(t)×r2(t)  (6)

[0043] 由此可得方向向量ul(t)在导航坐标系中各轴向的分量：

[0044] ulxt＝gyt(mzt-hz)-(gyt+g)(myt-hy)  (7)

[0045] ulyt＝(gzt+g)(mxt-hx)-gxt(mzt-hz)  (8)

[0046] ulzt＝gxt(myt-hy)-gyt(mxt-hx)  (9)

[0047] 对方向向量ul(t)进行归一化计算：

[0048]

[0049] 即可求得单位向量un(t)的三个分量uxt、uyt和uzt。

[0050] 利用单位向量un(t)和旋转角度θnt可构成旋转四元数：

[0051]

[0052] 其中，i，j和k是虚数单位。显然范数||qn(t)||＝1。

[0053] 假设载体坐标系中的向量rb(t)＝[xbt ybt zbt]T在导航坐标系中的对应向量为rn(t)＝[xnt ynt znt]T，定义p(·)为将三维向量映射为旋转四元数的函数，则有：

[0054] p(rb(t))＝xbti+ybtj+zbtk  (12)

[0055] p(rn(t))＝xnti+yntj+zntk  (13)

[0056] 式(12)和(13)中旋转四元数间关系为：

[0057]

[0058] 其中， 是旋转四元数qn(t)的共轭。式(14)中的运算服从四元数乘法运算法则。

[0059] 对式(14)进行逆运算可获得由导航坐标系向载体坐标系的映射关系：

[0060]

[0061] 将导航坐标系下的重力分量gn和载体坐标系下的重力分量gb(t)代入式(15)并整理得：

[0062] -gxt/g＝(1-cosθnt)uxtuzt-uytsinθnt  (16)

[0063] -gyt/g＝(1-cosθnt)uytuzt+uxtsinθnt  (17)

[0064]

[0065] 由式(18)可得：

[0066]

[0067] 将式(19)代入式(17)并整理得：

[0068]

[0069] 根据式(19)和(20)即可确定出旋转角度θnt所在的象限及其值。

[0070] 利用获得的单位向量un(t)的各个分量uxt、uyt、uzt与旋转角度θnt，由式(11)可确定出旋转四元数qn(t)。

[0071] 当小车处于非零加速状态时，三轴加速度计的输出将不影响载体坐标系与导航坐标系之间的位置关系，而影响载体坐标系与导航坐标系之间的位置关系的是三轴陀螺仪的输出，即三轴陀螺仪采集的角速度向量ωb(t)影响载体坐标系与导航坐标系之间的位置关系。因此，需要利用角速度向量ωb(t)实现对旋转四元数的更新。

[0072] 如前所述，单位向量un(t)和旋转角度θnt均是在导航坐标系定义的，因此，由单位向量un(t)和旋转角度θnt所确定的旋转四元数qn(t)也是在导航坐标系下定义的。对旋转四元数qn(t)的更新需要利用导航坐标系的角速度向量ωn(t)。根据式(14)可得：

[0073]

[0074] 在时刻t旋转四元数qn(t)的变化率为：

[0075]

[0076] 因此，相应的旋转四元数qn(t)更新为：

[0077]

[0078] (4)利用步骤(3)得到的旋转四元数估计值实现三轴加速度和三轴角速度在载体坐标系与导航坐标系之间转换的旋转四元数，并对地理坐标系的速度向量和位置向量进行更新，同时进行坡度提取的S4阶段：

[0079] 根据获得的旋转四元数qn(t)和传感器采集的数据可以进行平台位置的解算和道面坡度信息的提取。假设平台由静止状态开始，则在初始状态下，导航坐标系的加速度向量an(0)、速度向量vn(0)和位置向量pn(0)均为零向量，根据此时的零加速可以获得旋转四元数qn(0)。

[0080] 在平台运动过程中，平台所获得的加速度向量ab(t)为载体坐标系中的测量值。在进行坡度测量过程中需要将加速度向量ab(t)进行坐标变换，并用重力加速度向量gn进行补偿，从而获得导航坐标系形式的加速度向量an(t)。参考式(14)可得：

[0081]

[0082] 速度向量的更新方程为：

[0083] v(t+dt)＝v(t)+an(t)dt  (25)

[0084] 其中，v(t)为当前时刻的速度向量，v(t+dt)为速度的更新值。当载体处于零加速状态时，加速度向量an(t)为零向量，则式(25)变为：

[0085] v(t+dt)＝v(t)  (26)

[0086] 位置向量的更新方程为：

[0087] p(t+dt)＝p(t)+v(t)dt  (27)

[0088] 其中，p(t)为当前时刻的位置向量，p(t+dt)为位置向量的更新值。

[0089] 根据图2所示的坐标系间的关系可知，在坡度测量的过程中，地理坐标系是不动的，导航坐标系只是相对于地理坐标系平移，因此在两个坐标系中的重力分量gn是不变化的。对于载体坐标系，在坡度测量过程中存在相对姿态的变化，即载体坐标系相对导航坐标系发生旋转，而且这个角度的变化是与平台所接触面的坡度有关，因此，在载体坐标系中重力加速度向量gb(t)的方向变化量均反映着平台所接触面的坡度变化情况，如图3所示。

[0090] 假设测量起始时平台由水平面静止状态出发，则起始时刻载体坐标系中的重力加速度向量gb(0)为：

[0091]

[0092] 在时刻t，载体坐标系的重力加速度向量gb(t)为

[0093]

[0094] 重力加速度向量gb(t)与重力加速度向量gb(0)间的夹角即为平台在时刻t所处位置的坡度：

[0095]

[0096] 实验结果

[0097] 本发明人将分别利用实际采集的小车运动三轴加速度、三轴角速度和三轴磁分量数据来验证本发明方法的有效性。

[0098] 实验采用的智能手机为iPhone7，利用其内置的三轴加速度计、三轴陀螺仪和三轴磁力计进行数据采集。利用手机端Matlab通过IP与电脑端Matlab互联，以实现传感器数据的采集与处理。数据采样率设置为100Hz。零加速检测的加速度阈值为thamin＝8.0和thamax＝10.8，角速度检测阈值为thωmax＝1.3。

[0099] 为检验本发明方法的有效性，实验场地选择在可实测角度与尺寸的楼梯上进行，场景模型如图4所示。将智能手机置于小车顶部，手推小车在实验大楼内的楼梯上进行下降四层楼梯的运动，模拟小车经过连续坡度路面的情况，将零加速修正的轨迹与楼梯的真实轨迹相比较，并提取其坡度信息，如图5所示。

[0100] 由图5(a)可见，由于小车与智能手机的实验平台尺寸的原因，其在楼梯下降拐角处的轨迹是弧形，零加速修正的轨迹较好地重现了真实轨迹，具有较好的定位精度。由图5(b)可见，小车在楼梯平台上水平移动时坡度角为0°，当小车从一个台阶平台边缘下降到另一个台阶平台上的过程中，小车先从水平状态偏转到垂直状态，然后再逐渐从垂直状态恢复为水平状态，坡度从0°减小至-90°，然后再从-90°恢复为0°，具有较好的精度。