CN104634345A - 一种自适应步长的室内轨迹追踪方法 - Google Patents

Abstract

本发明涉及一种自适应步长的室内轨迹追踪方法，设定每一步行走的初始速度与结束速度为零，判断是否处于行走状态，如果是，则采集每一步内加速度的个数，根据加速度、加速度的采样时间，计算获得第一步的步长；结合每一步的方向，将每一步进行矢量叠加，获得运动轨迹。为了提高惯性导航定位系统的姿态检测的准确性和减少步长计算误差，从而改善导航定位的精度，基于MEMS惯性传感器的室内行人轨迹追踪，本发明采用自适应的机制可以准确计算每一步的步长，提高导航定位的精度，促进惯性导航定位应用的推广。

Description

一种自适应步长的室内轨迹追踪方法

技术领域

本发明涉及室内定位追踪方法，更具体地说，涉及一种自适应步长的室内轨迹追踪方法。

背景技术

众所周知，GPS等卫星导航定位方式可以在室外卫星信号良好的地方进行导航定位，极大的方便了人们的生活。据统计，人们平均80-90％的时间在室内，70％的移动电话使用和80％的数据连接使用连接来自室内。因此，人们对室内的位置服务(LBS)的需求越来越多，但是由于GPS等卫星导航定位方式无法在室内环境中正常工作，那么如何解决室内定位的问题就成为人们研究的一大热点。

现在，已有大量的技术应用于室内定位，室内定位技术主要分为基站定位和惯导定位两种。基站定位方式主要有：Wi-Fi定位，蓝牙定位及射频定位等。惯导定位方式就是采用基于MEMS技术的惯性传感器，通过运动学定理实现导航定位。由于惯性导航定位方式无需采用额外的基础设施或网络，可以在各种复杂的环境中实现自主定位，因此，惯性导航定位在特定场合有着巨大的应用前景。例如，在公共安全和应急响应。每一个人都想被救援人员精确定位到，大到建筑物的位置，甚至是楼层或者房间号。在这种情况下，基于基站的定位方式由于基础设施或网络遭到破坏，无法实现定位功能，而惯性导航定位方式则不受此影响，从而可以及时有效的实施救援工作，减少人员和财产损失。

影响室内定位技术推广的主要原因是导航定位的精度和定位系统的成本问题。因此，室内定位技术的研究重点是如何提高导航定位的精度和降低定位系统的成本，由于惯性导航定位技术可以实现复杂环境中自主定位，无需额外设施或网络的特点，符合人们对室内定位的精度和成本的要求，所以惯性导航定位成为各大研究机构的研究重点。

发明内容

本发明的目的在于克服现有技术的不足，提供一种准确计算每一步的步长，提高导航定位的精度，促进惯性导航定位应用的推广的自适应步长的室内轨迹追踪方法。

本发明的技术方案如下：

一种自适应步长的室内轨迹追踪方法，设定每一步行走的初始速度与结束速度为零，判断是否处于行走状态，如果是，则采集每一步内加速度的个数，根据加速度、加速度的采样时间，计算获得第一步的步长；结合每一步的方向，将每一步进行矢量叠加，获得运动轨迹。

作为优选，行走状态的判断具体为：预设静态阀值、动态阀值，其中动态阀值大于静态阀值，当陀螺仪输出的每一步的实时角速度小于静态阀值，则表示处于静止状态，大于动态阀值，则表示处于行走状态。

作为优选，当处于静止状态时，角速度大于动态阀值则进入运动状态，角速度大于静态阀值但小于动态阀值则仍处于静止状态；当处于运动状态时，角速度小于静态阀值则进入静止状态，角速度小于动态阀值但大于静态阀值则仍处于运动状态。

作为优选，设定每个加速度的采样时间相等，则每个加速度的位移为其中i＝1、2、3……；S_i为第i段位移；a_i为第i个加速度；t为采样时长；

每一步的步长为： $S=\frac{1}{2}\×((2N-1)\×a_1+(2N-3)\×a_2+...+a_N))\×t^2,$ 其中，S为每一步的步长，N为每一步采集加速度的个数，a为加速度，t为每个加速度的采样时长。

作为优选，将加速度变换为导航坐标系后，加速度变换为

则每一步的步长为：

其中，s为每一步的步长，s_东为每一步的东向位移，s_北为每一步的北向位移，分别为东向加速度与北向加速度，flag为每一步采集加速度的个数，t为每个加速度的采样时长。

作为优选，加速度计输出的加速度通过四元数坐标变换矩阵变换为导航坐标系

作为优选，每一步的方向由航向姿态角决定，航向姿态角由如下方程组得出：

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

$C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right];$

$C_b^n={\left(C_n^b\right)}^T;$

其中， $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]$ 为四元数坐标变换矩阵；ψ为航向角，θ为俯仰角，γ为翻滚角；航向姿态角包括航向角、俯仰角、翻滚角。

作为优选，四元数坐标变换矩阵由陀螺仪输出的角速度采用四元数微分方程计算得出，具体为：

$\frac{\mathrm{dQ}}{\mathrm{dt}}=\frac{1}{2}Q\⊗\mathrm{\ω};$

其中，Q(q₀,q₁,q₂,q₃)为四元数，ω为陀螺仪输出的角速度，为四元数乘法，计算得到四元数坐标变换矩阵 $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right].$

作为优选，每一步内的加速度的个数的确定，具体为：实时检测陀螺仪输出的角速度，如果当前时刻的角速度小于静态阀值，将采样标志位置1，等待下一时刻的角速度；当角速度大于动态阀值时，采样标志位加1，直到角速度再次小于静态动态阀值，结束当前一步的加速度的个数计算。

作为优选，陀螺仪的X轴固定为前进方向，Y轴的数据分量用于判断行走状态。

本发明的有益效果如下：可以有效解决步长计算误差，

为了提高惯性导航定位系统的姿态检测的准确性和减少步长计算误差，从而改善导航定位的精度，基于MEMS惯性传感器的室内行人轨迹追踪，本发明提出一种自适应的步长计算方法，采用自适应的机制可以准确计算每一步的步长，提高导航定位的精度，促进惯性导航定位应用的推广。

针对姿态检测的准确性问题，采用双阈值姿态检测提高姿态检测的正确性；针对由于积分运算引入的积分漂移误差，根据运动学规律采用ZUPT(Zero Velocity Update，零速校正)复位静止状态的速度的方法，可以有效的减少积分漂移误差。既提高姿态检测的准确性，又提高了步长计算的准确性和稳定性，可以很大程度上改善室内惯性导航定位的精度，对加快惯性导航定位的应用有一定的参考价值。

附图说明

图1是本发明的原理示意图；

图2是加速度采样个数的确定流程；

图3是实施本发明的硬件原理图。

具体实施方式

以下结合附图及实施例对本发明进行进一步的详细说明。

本发明所述的方法，首先，对于MEMS惯性传感器陀螺仪的原始数据，采用双阈值的方法，准确判定行人所处的运动状态，用于后续的步长计算；

然后，采集MEMS惯性传感器加速度计的原始数据，通过坐标变化矩阵，将载体(行人)坐标系的三轴加速度转换到导航坐标系，并自动记录每一步的加速度采样个数，作为自适应步长计算的标志因子，提高步长计算的准确性。

本发明提供一种自适应步长的室内轨迹追踪方法，设定每一步行走的初始速度与结束速度为零，判断是否处于行走状态，如果是，则采集每一步内加速度的个数，根据加速度、加速度的采样时间，计算获得第一步的步长；结合每一步的方向，将每一步进行矢量叠加，获得运动轨迹。

室内行人导航定位算法的精度之所以不高的主要原因之一就是由于步长计算的误差导致的。室内行人导航定位就是计算每一的步长和方向，对每一步的步长和方向进行矢量叠加，得到行人运动轨迹的路线图。如何提高步长计算的准确性，对改善室内行人惯性导航定位算法的精度有着重要的意义。所以，如何准确的检测出行走过程中的每一步和计算每一步的步长成为提高定位的精度的关键。

针对姿态检测准确性问题，本发明通过双阈值检测方法进行判断，采用双阈值检测方法能够弥补单阈值算法的不足：检测到数据大于静态阈值，就判定行人处于运动状态，但是实际情况有可能是行人仍处于静止状态，只是从静止状态向运动动态转变，并没有处于运动状态，这个时候行人的实际速度仍然是零。但是采用单阈值的方法就判定此时行人处于运动状态的话，就要采用步长计算算法对步长进行计算，这样每一步步长的开始点就出现误差，导致步长计算的误差不断累积，严重影响室内行人定位系统的精度。

采用双阈值检测方法，增加一个动态阈值，双阈值分别为：静态阈值和动态阈值，排除处于两者之间的原始数据，可以提高姿态检测的准确性，为后续的自适应步长的计算提供准确的状态信息。

行走状态的判断具体为：预设静态阀值、动态阀值，其中动态阀值大于静态阀值。当陀螺仪输出的每一步的实时角速度小于静态阀值，则表示处于静止状态，大于动态阀值，则表示处于行走状态。

当处于静止状态时，角速度大于动态阀值则进入运动状态，角速度大于静态阀值但小于动态阀值则仍处于静止状态；当处于运动状态时，角速度小于静态阀值则进入静止状态，角速度小于动态阀值但大于静态阀值则仍处于运动状态。

本发明所采用的MEMS惯性传感器成本低，与商用的MEMS惯性测量单元价格相差百倍。通过实验得知：本发明的行走检测准确率能够达到98％，行走误差能控制在行走距离的10％以下。对于实现低成本、高精度的MEMS惯性传感器定位系统来说，本发明提供了一种良好的解决思路，具有一定的使用价值和推广价值。

针对步长计算精度的问题，本发明提出自适应的步长计算方法，可以适应行人不同的行走速度，准确计算步长。具体为：首先，如果行人处于运动状态，记录每一步的采样的加速度的个数记为N。然后，对每一步的行走过程根据采样点数划分为flag段位移，根据行走规律，每一步的初始和结束速度为零，所以第一段位移的初始速度为零，在足够小的采样时间t内，可以认为加速度保持不变，，则每个加速度的位移为其中i＝1、2、3……；S_i为第i段位移；a_i为第i个加速度；t为采样时长；如，为第一段的位移。后续每一段位移的计算的初始速度均不为零，根据每一步的采集数据的个数N采用数学归纳的方法，可以得到每一步的步长计算为：

$S=\frac{1}{2}\×((2N-1)\×a_1+(2N-3)\×a_2+...+a_N))\×t^2,$ 其中，S为每一步的步长，N为每一步采集加速度的个数，a为加速度，t为每个加速度的采样时长。

通过每一步的采样的加速度的个数N就可以计算出每一步的步长，而且不仅可以降低步长计算的复杂性，通过采样的加速度的个数N的不同适应不同的行走速度，这种自适应的机制大大提高了算法的准确性和稳定性。

如图1所示，本发明基于MEMS惯性传感器进行室内定位，MEMS 3-轴陀螺仪的原始数据一方面通过四元数微分方程计算得出四元数坐标变换矩阵和航向姿态角。另一方面MEMS3-轴陀螺仪的原始数据通过双阈值姿态检测方法进行行走状态检测。MEMS 3-轴加速度计的原始数据通过四元数坐标变换矩阵由载体坐标系变换为导航坐标系，导航坐标系的加速度通过自适应步长计算方法得到行人的行走轨迹图，实现行人的室内导航定位。

具体步骤如下：

步骤1、加速度计和陀螺仪组成的测量单元，固定在行人的脚上，采集行走过程中的加速度和角速度四元数坐标变换矩阵由陀螺仪输出的角速度采用四元数微分方程计算得出，具体为：

$\frac{\mathrm{dQ}}{\mathrm{dt}}=\frac{1}{2}Q\⊗\mathrm{\ω};$

其中，Q(q₀,q₁,q₂,q₃)为四元数，ω为陀螺仪输出的角速度，为四元数乘法，计算得到四元数坐标变换矩阵 $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right].$

步骤2、每一步的方向由航向姿态角决定，航向姿态角由如下方程组得出：

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

$C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right];$

其中， $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]$ 为四元数坐标变换矩阵；ψ为航向角，θ为俯仰角，γ为翻滚角；航向姿态角包括航向角、俯仰角、翻滚角。

步骤3、加速度计输出的加速度通过四元数坐标变换矩阵变换为导航坐标系(东-北-天坐标系)的加速度，

步骤4、将采集到的角速度原始数据ω和变换后的加速度αⁿ，采用双阈值检测方法进行行走姿态的判断。若角速度ω小于静态阀值ω_s，则说明行人处于静止状态，若角速度ω大于动态阈值ω_d，则说明行人处于行走状态，并记录每一步的加速度的采样个数flag。

步骤5、判断每一步采样的加速度的个数，采用采样标志位flag记录加速度采样个数，根据采样标志位flag进一步计算每一步的步长，具体推导过程如下：

每一步的初始时刻和结束时刻的速度均为零，根据采样标志位flag将每一步位移划分为flag段，每一段位移的时间均相等，为采样周期T_s，本实施例中，T_s＝0.02s，则采样周期为50Hz。

将加速度变换为导航坐标系后，加速度变换为其中，东向位移如下：

...

北向位移的计算与东向位移同理。

将北向位移和东向位移合成，即可得到每一步的步长，结果如下：

其中，s为每一步的步长，s_东为每一步的东向位移，s_北为每一步的北向位移，分别为东向加速度与北向加速度，flag为每一步采集加速度的个数，t为每个加速度的采样时长。

步骤6、重复步骤3、步骤4，计算行走过程中每一步的步长，即可得到行走轨迹图，实现定位功能。

上述步骤5中，每一步内的加速度的个数的确定，具体为：实时检测MEMS 3-轴陀螺仪输出的角速度，如果当前时刻的角速度小于静态阀值，将采样标志位置1，等待下一时刻的角速度；当角速度大于动态阀值时，采样标志位加1，直到角速度再次小于静态动态阀值，结束当前一步的加速度的个数计算。本实施例中，MEMS 3-轴陀螺仪的X轴固定为前进方向，Y轴的数据分量用于判断行走状态，如图2所示。

首先，判定MEMS 3-轴陀螺仪Y轴的原始数据是否小于静态阈值，小于的话说明此时行人处于的脚与地面是相对静止的状态，并记采样标志位flag等于1。

然后，进一步判定MEMS 3-轴陀螺仪Y轴的下一时刻的数据是否大于动态阈值，大于的话说明此时行人处于行走状态，每得出一个这样的点，采样标志位flag加1，直到行人从行走状态变为静止状态为止。此时，根据每一步的采样标志位flag采用ZUPT自适应步长计算方法得到当前一步的步长。

最后，依次类推，不断计算每一步的步长并进行矢量叠加，得到行人的轨迹图，实现室内定位追踪功能。

本实施例的硬件实现，如图3所示，数据采集单元采用的是MPU91509-轴传感器，集成3-轴加速度计、3-轴陀螺仪和3-轴磁力计于一体的9-轴传感器芯片，采集行人的自身运动的角速度和加速度。控制单元MCU采用的是意法半导体公司的STM32F103单片机。2.4G无线发射单元和2.4G无线接收单元采用的是蓝牙主从收发一体芯片。接收单元将采集的数据通过USB转串口芯片CH340t传输给电脑，进行数据的后端处理(Matlab处理)。

上述实施例仅是用来说明本发明，而并非用作对本发明的限定。只要是依据本发明的技术实质，对上述实施例进行变化、变型等都将落在本发明的权利要求的范围内。

Claims (10)

1.一种自适应步长的室内轨迹追踪方法，其特征在于，设定每一步行走的初始速度与结束速度为零，判断是否处于行走状态，如果是，则采集每一步内加速度的个数，根据加速度、加速度的采样时间，计算获得第一步的步长；结合每一步的方向，将每一步进行矢量叠加，获得运动轨迹。

2.根据权利要求1所述的自适应步长的室内轨迹追踪方法，其特征在于，行走状态的判断具体为：预设静态阀值、动态阀值，其中动态阀值大于静态阀值，当陀螺仪输出的每一步的实时角速度小于静态阀值，则表示处于静止状态，大于动态阀值，则表示处于行走状态。

3.根据权利要求2所述的自适应步长的室内轨迹追踪方法，其特征在于，当处于静止状态时，角速度大于动态阀值则进入运动状态，角速度大于静态阀值但小于动态阀值则仍处于静止状态；当处于运动状态时，角速度小于静态阀值则进入静止状态，角速度小于动态阀值但大于静态阀值则仍处于运动状态。

4.根据权利要求1所述的自适应步长的室内轨迹追踪方法，其特征在于，设定每个加速度的采样时间相等，则每个加速度的位移为其中i＝1、2、3……；S_i为第i段位移；a_i为第i个加速度；t为采样时长；

每一步的步长为： $S=\frac{1}{2}\×((2N-1)\×a_1+(2N-3)\×a_2+...+a_N))\×t^2,$ 其中，S为每一步的步长，N为每一步采集加速度的个数，a为加速度，t为每个加速度的采样时长。

5.根据权利要求4所述的自适应步长的室内轨迹追踪方法，其特征在于，将加速度变换为导航坐标系后，加速度变换为

则每一步的步长为：

其中，s为每一步的步长，s_东为每一步的东向位移，s_北为每一步的北向位移，分别为东向加速度与北向加速度，flag为每一步采集加速度的个数，t为每个加速度的采样时长。

6.根据权利要求5所述的自适应步长的室内轨迹追踪方法，其特征在于，加速度计输出的加速度通过四元数坐标变换矩阵变换为导航坐标系

7.根据权利要求1所述的自适应步长的室内轨迹追踪方法，其特征在于，每一步的方向由航向姿态角决定，航向姿态角由如下方程组得出：

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

$C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right];$

$C_b^n={\left(C_n^b\right)}^T;$

其中， $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]$ 为四元数坐标变换矩阵；ψ为航向角，θ为俯仰角，γ为翻滚角；航向姿态角包括航向角、俯仰角、翻滚角。

8.根据权利要求6或7任一项所述的自适应步长的室内轨迹追踪方法，其特征在于，四元数坐标变换矩阵由陀螺仪输出的角速度采用四元数微分方程计算得出，具体为：

$\frac{\mathrm{dQ}}{\mathrm{dt}}=\frac{1}{2}Q\⊗\mathrm{\ω};$

其中，Q(q₀,q₁,q₂,q₃)为四元数，ω为陀螺仪输出的角速度，为四元数乘法，计算得到四元数坐标变换矩阵 $C_b^n=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right].$

9.根据权利要求1所述的自适应步长的室内轨迹追踪方法，其特征在于，每一步内的加速度的个数的确定，具体为：实时检测陀螺仪输出的角速度，如果当前时刻的角速度小于静态阀值，将采样标志位置1，等待下一时刻的角速度；当角速度大于动态阀值时，采样标志位加1，直到角速度再次小于静态动态阀值，结束当前一步的加速度的个数计算。

10.根据权利要求1所述的自适应步长的室内轨迹追踪方法，其特征在于，陀螺仪的X轴固定为前进方向，Y轴的数据分量用于判断行走状态。