基于零加速修正的智能手机惯性传感器室内定位方法与流程

本发明属于室内定位技术领域，特别是涉及一种基于零加速修正的智能手机惯性传感器室内定位方法。

背景技术：

随着社会经济的快速发展，人们对基于位置服务(locationbasedservices,lbs)的需求日益增加。内置gps芯片的智能手机可以便捷地在户外实现定位功能。然而，对于室内环境，特别是大型室内环境，如图书馆、商场、写字楼等，gps信号受建筑物的影响，信号强度较低，无法提供相应的定位服务。因此如何在复杂室内环境中利用智能手机实现人员定位成为近年来的研究热点。

随着智能手机的普及与功能完善，有关学者们尝试利用智能手机内置器件实现室内定位功能。wifi定位技术是目前常用的室内定位技术，但此技术需要用户提前预知wifi热点的分布情况，环境wifi热点布局的任何改变都会对定位性能产生影响。基于惯性传感器的定位技术可以应用在无gps信号的室内环境中，目前主要有行人航位推算(pedestriandeadreckoning,pdr)技术和捷联惯性导航(strapdowninertialnavigation,sin)技术。pdr技术的核心问题是如何确定行人步长。法国geoloc实验室的renaudinv等人利用杂交滤波器标定行人步长；hefeng-tao等人提出基于模糊逻辑的步长估计算法；天津大学的汪少初等人提出了根据步行中腰部运动特点的步长估计算法。由于pdr算法导航精度通常为0.3％～10％，该技术过于依赖步长估计算法，因此具有较大的局限性。sin技术则是基于加速度和角速度数据信号积分的方法，该方法对惯性传感器的精度依赖性较强。对于智能手机内置惯性传感器件，由于成本原因，相应测量精度较低，利用惯性传感器件测量数据积分获得的轨迹信息易受到累积误差的影响。

零速修正是利用平台的短暂静止状态对定位数据进行修正，可以解决惯性定位方法的累积误差问题。采用零速修正的位置系统通常是利用一个外置的传感器进行静止状态检测，例如将惯性传感器件置于行人脚底，通过检测行人步行时脚着地期间的短暂停止期作为零速状态，将此状态信息提供给定位系统以，实现对累积误差的修正，然而智能手机不适用于置于鞋底的方式。

技术实现要素：

为了解决上述问题，本发明的目的在于提供一种基于零加速修正的智能手机惯性传感器室内定位方法。

为了达到上述目的，本发明提供的基于零加速修正的智能手机惯性传感器室内定位方法包括按顺序进行的下列步骤：

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

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

(3)根据步骤(2)获得的行人是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下姿态估计矩阵的s3阶段；

(4)利用步骤(3)得到的姿态估计矩阵实现三轴加速度和三轴角速度由载体坐标系向惯性坐标系的转换，并对地理坐标系的速度向量和位置向量进行更新，得到行人位置的s4阶段。

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

在步骤(3)中，所述的根据步骤(2)获得的行人是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下姿态估计矩阵的方法是：当判定行人处于零加速状态时，利用零加速姿态估计方法得到此时的姿态估计矩阵；当判定行人处于非零加速状态时，利用三轴角速度数据和前一时刻的姿态估计矩阵来更新当前时刻的姿态估计矩阵，其关键是计算得到初始时刻的姿态估计矩阵；假设起始状态是静止的，初始时刻的姿态估计矩阵能够通过零加速姿态估计方法获得。

在步骤(4)中，所述的利用步骤(3)得到的姿态估计矩阵实现三轴加速度和三轴角速度由载体坐标系向惯性坐标系的转换，并对地理坐标系的速度向量和位置向量进行更新，得到行人位置的方法是：行人处于零加速状态条件时，将三轴加速度置为零，当前时刻的速度向量应该等于前一时刻的速度向量；行人处于非零加速状态条件时，利用姿态估计矩阵将载体坐标系下转换到惯性坐标系下的三轴加速度数据乘以采样时间加上前一时刻的速度向量来更新当前时刻的速度向量，然后将速度向量乘以采样时间加上前一时刻位置得到当前时刻的位置向量，重复上述过程即可得到行人位置。

本发明提供的基于零加速修正的智能手机惯性传感器室内定位方法利用智能手机内置的惯性传感器实现室内定位的算法是在不外接惯性传感器件的情况下，直接利用智能手机的惯性传感器数据进行零加速状态检测，并在零加速状态时对定位系统进行状态估计与修正，实现定位的方法，可以有效地解决无gps信号区域的人员定位问题。

附图说明

图1为本发明提供的基于零加速修正的智能手机惯性传感器室内定位方法流程图。

图2为零加速检测结果。

图3为相关坐标系间关系。

图4为户外实验结果。

图5为室内实验结果。

具体实施方式

下面结合附图和具体实施例对本发明提供的基于零加速修正的智能手机惯性传感器室内定位方法进行详细说明。

如图1所示，本发明提供的基于零加速修正的智能手机惯性传感器室内定位方法包括按顺序进行的下列步骤：

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

智能手机内置三轴加速度计、三轴陀螺仪和三轴磁力计等传感器，在行人携带智能手机行走时，利用手机端matlab通过ip与电脑端matlab互联，可以实时将三轴加速度、三轴角速度和三轴磁分量采集到电脑中。各传感器采集的数据将以载体坐标系数据的形式输出。

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

零加速检测是对零速检测的拓展。零速状态要求平台的加速度与速度同时为零，利用行人携带的智能手机检测出其在地理坐标系下加速度与速度同为零的状态是不实际的，因此无法获得充足的零速状态信息，以实现对累积误差的修正。零加速状态实质上是智能手机处于力平衡态，是平台的平动加速度和转动加速度均为零的状态，虽然此时的速度可能不为零，但也可利用此时的姿态估计来实现对惯性器件累积误差的修正。

根据智能手机中传感器的特点可知，在零加速状态时，三轴加速度计的输出为重力加速度的值，而三轴陀螺仪的输出应为零。在无外部传感器的情况下，携带智能手机的行人零加速状态可利用惯性器件输出的加速度数据和角速度数据来判定。

利用三轴加速度计输出进行加速度模值检测(accelerationmagnitudedetector，amd)：

其中，||·||表示向量的2范数，th1max和th1min分别为加速度检测的上、下阈值，ab(t)＝[axtaytazt]^t为三轴加速度计输出的在载体坐标系下表示的加速度向量。

利用三轴陀螺仪输出进行角速度模值检测(angularratemagnitudedetector，armd)：

其中，ωb(t)＝[ωxtωytωzt]^t为三轴陀螺仪输出的在载体坐标系下表示的角速度向量，th2max为角速度检测的阈值。

如上所述，零加速状态应该是平动加速度和转动加速度均为零，即d1(t)和d2(t)同时成立时的状态：

zad(t)＝d1(t)×d2(t)(3)

根据部分实际数据进行的amd，armd和zad检测结果如图2所示。由图2可见，采用zad检测确定的零速状态数量虽然较少，但是检测结果更加准确。

(3)根据步骤(2)获得的行人是否处于零加速状态的判定结果，得到零加速状态或非零加速状态下姿态估计矩阵的s3阶段：

当步骤(2)判定行人处于零加速状态时，利用零加速姿态估计方法得到此时的姿态估计矩阵；当步骤(2)判定行人处于非零加速状态时，利用三轴角速度数据和前一时刻的姿态估计矩阵来更新当前时刻的姿态估计矩阵。假设起始状态是静止的，初始时刻的姿态估计矩阵可以通过零加速姿态估计方法获得。

当平台处于零加速状态时，需要对平台的姿态进行估计，即需要获得载体坐标系与惯性坐标系间的位置关系，也就是对惯性坐标系到载体坐标系的转换矩阵ci^b(t)进行估计。

根据图3定义轴角关系，惯性坐标系绕某单位向量r(t)＝[rxtrytrzt]^t逆时针旋转θ(t)角得到载体坐标系。根据rodrigues旋转公式可得：

其中i为三阶单位矩阵，k(t)为该单位向量r(t)的斜对称矩阵：

因为转换矩阵ci^b(t)是酉矩阵，所以由载体坐标系到惯性坐标系的转换矩阵：

零加速姿态估计就是要获得载体坐标系与惯性坐标系间的关系，即获得初始的转换矩阵由公式(4)至(6)可知，转换矩阵由单位向量r(t)和旋转角θ(t)所确定。

在静止状态下，利用三轴加速度计和三轴磁力计可以分别获得三轴加速度和三轴磁分量在载体坐标系的向量gb(t)＝[gxtgytgzt]^t和nb(t)＝[nxtnytnzt]^t，在惯性坐标系下相应的向量为gi＝[00-g]^t和ni＝[hxhyhz]^t。惯性坐标系中的磁分量向量可以根据国际地磁参考场(internationalgeomagneticreferencefield，igrf)获得。

如前所述，向量gb(t)和nb(t)均是向量gi和ni绕单位向量r(t)逆时针旋转θ(t)角获得的，因此单位向量r(t)在重力加速度矢量gb(t)和gi的对称面上，该平面的法向量为：

m1(t)＝[gxtgytgzt+g]^t(7)

同理，单位向量r(t)同时在磁分量矢量nb(t)和ni的对称面上，该平面的法向量为：

m2(t)＝[nxt-hxnyt-hynzt-hz]^t(8)

所以，单位向量r(t)在公式(7)和公式(8)所确定的两平面的交线上，该交线的方向向量为：

rl(t)＝m1(t)×m2(t)(9)

由此可得方向向量rl(t)在惯性坐标系各轴向的分量：

rlxt＝gyt(nzt-hz)-(gzt+g)(nyt-hy)(10)

rlyt＝(gzt+g)(nxt-hx)-gxt(nzt-hz)(11)

rlzt＝gxt(nyt-hy)-gyt(nxt-hx)(12)

对方向向量rl(t)进行归一化计算：

即可求得单位向量r(t)的三个分量rxt，ryt，rzt。

考虑重力加速度向量在载体坐标系和惯性坐标系间的关系：

将公式(4)、(5)、(6)代入公式(14)，整理得：

-gxt/g＝[1-cosθ(t)]rxtrzt+rytsinθ(t)(15)

-gyt/g＝[1-cosθ(t)]rytrzt-rxtsinθ(t)(16)

-gzt/g＝[1-cosθ(t)]rz²t+cosθ(t)(17)

由公式(17)可得：

将公式(18)代入公式(16)并整理可得：

根据公式(18)和(19)即可确定出旋转角θ(t)所在的象限及其值。

利用上述获得的单位向量r(t)的三个分量rxt，ryt，rzt以及旋转角θ(t)，由公式(4)和(6)可得载体坐标系到惯性坐标系的姿态估计矩阵

当行人处于非零加速状态时，利用三轴角速度数据和前一时刻的姿态估计矩阵来更新当前时刻的姿态估计矩阵。将由载体坐标系到惯性坐标系的转换矩阵更新为：

其中i为三阶单位矩阵，β(t)为用于定义方向的小角度的斜对称矩阵：

其中dt为采样时间间隔，ωxt、ωyt和ωzt分别为三轴陀螺仪在时刻t的三个输出，构成当前时刻的角速度向量ωb(t)。

(4)利用步骤(3)得到的姿态估计矩阵实现三轴加速度和三轴角速度由载体坐标系向惯性坐标系的转换，并对地理坐标系的速度向量和位置向量进行更新，得到行人位置的s4阶段：

假设起始状态是由静止状态开始的，则地理坐标系下的加速度向量an(0)、速度向量vn(0)和位置向量pn(0)均为零向量，根据零加速姿态估计可获得初始时刻的姿态估计矩阵

在平台运动过程中，三轴加速度计输出的载体坐标系下的加速度向量ab(t)需要变换为地理坐标系下的加速度向量an(t)。由于地理坐标系与惯性坐标系下的加速度向量相等，即an(t)＝ai(t)，所以：

显然，地理坐标系下的加速度向量an(t)是三轴加速度计输出的加速度向量ab(t)经过变换且消除重力影响后的当前时刻的三轴加速度向量。

速度向量的更新方程为：

vn(t+dt)＝vn(t)+an(t)×dt(23)

其中，vn(t)为当前时刻的速度向量，vn(t+dt)为速度向量的更新值。当平台处于零加速状态时，加速度an(t)为零向量，则公式(23)变为：

vn(t+dt)＝vn(t)(24)

位置向量的更新方程为：

pn(t+dt)＝pn(t)+vn(t)×dt(25)

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

实验结果

本发明人将分别利用实际采集的行人步行三轴加速度、三轴角速度和三轴磁力计数据验证来本发明方法的有效性。

实验平台采用iphone6plus智能手机，其内置的三轴加速度计和三轴陀螺仪是invensense公司的mpu-6700集成的，三轴磁力计采用的是akm半导体公司的ak8963c。利用手机端matlab通过ip与电脑端matlab互联，实现传感器数据的采集与处理。数据采样率设置为100hz。零加速检测中的加速度检测阈值为th1min＝9.2和th1max＝10.4，角速度检测阈值为th2max＝0.2。

为了检验本发明方法的有效性，首先进行户外定位实验，利用gps定位数据检验本发明方法的有效性。在户外手持智能手机进行了“z”形运动。将零加速修正的轨迹、未进行零加速修正的轨迹和gps轨迹相比较，如图4所示。由图4可见，零加速修正的轨迹较好地重现了gps轨迹，具有较好的定位精度，而未进行零加速修正的轨迹明显地出现了轨迹发散现象，无法提供准确的定位信息。

下面在室内进行类似实验，由于没有gps轨迹，根据室内走廊的固定路线行走，相应的零加速修正的轨迹、未进行零加速修正的轨迹和实际轨迹的比较结果如图5所示。由图5可见，在无gps信号的室内环境，采用本发明方法也可以有效地重现行走轨迹，实现对行人的精确定位。而未进行零加速修正时，轨迹明显存在发散现象，使得定位的结果不可用。

比较图4与图5可见，经过零加速修正的轨迹均能够很好地逼近实际轨迹，而不经过零加速修正的轨迹随着时间的推移，明显远离实际位置，无法实现对行人定位的目的。