[发明专利]一种自适应步长的室内轨迹追踪方法

权利要求书

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

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

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

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

每一步的步长为：其中，S为每一步的步长，N为每一步采集加速度的个数，a为加速度，t为每个加速度的采样时长。

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

则每一步的步长为：

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

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

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

Cnb=cosγcosψ+sinγsinψsinθ-cosγsinψ+sinγcosψsinθ-sinγcosθsinψcosθcosψcosθsinθsinγcosψ-cosγsinψsinθ-sinγsinψ-cosγcosψsinθcosγcosθ;]]>

Cbn=q12+q02-q32-q222(q1q2-q0q3)2(q0q2+q1q3)2(q1q2+q0q3)q22-q32+q02-q122(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q32-q22-q12+q02;]]>

Cbn=(Cnb)T;]]>

其中，为四元数坐标变换矩阵；ψ为航向角，θ为俯仰角，γ为翻滚角；航向姿态角包括航向角、俯仰角、翻滚角。