[發明專利]一種自適應步長的室內軌跡追蹤方法

有效

• 申請號: 201510042490.6
• 申請日: 2015-01-28
• 公開（公告）號: CN104634345B
• 公開（公告）日: 2018-02-16
• 發明（設計）人: 莫冰;朱金林;楊逸純;曾顯彬;毛文輝;蔡鈞
• 申請（專利權）人: 華僑大學;福州曲直電子科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 廈門市首創君合專利事務所有限公司35204
• 代理人: 張松亭,楊鍇

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;]]>

其中，為四元數坐標變換矩陣；ψ為航向角，θ為俯仰角，γ為翻滾角；航向姿態角包括航向角、俯仰角、翻滾角。