基於慣性傳感器的行人導航裝置和方法
2023-04-23 14:49:41 1
基於慣性傳感器的行人導航裝置和方法
【專利摘要】本發明公開了基於慣性傳感器的行人導航裝置，包括微處理器以及分別與之連接的三軸加速度傳感器、三軸陀螺儀、三軸磁力計和無線通信模塊，所述微處理器接收三軸加速度傳感器、三軸陀螺儀和三軸磁力計採集的數據，並通過無線通信模塊將這些數據上傳至PC機上。本發明還公開了基於前述裝置的行人導航方法，包括步伐階段檢測算法、腳體方位估計算法和擴展卡爾曼濾波算法。本發明在保證精確度的同時極大限度地降低了計算的複雜度，在實際環境中不必消耗大量硬體功耗也能保證實時性的要求。
【專利說明】基於慣性傳感器的行人導航裝置和方法
【技術領域】
[0001] 本發明屬於導航【技術領域】，特別涉及了基於慣性傳感器的行人導航裝置和方法。
【背景技術】
[0002] 隨著數據業務與多媒體業務數據的快速增長，高精度行人導航系統的需求 日益劇增，擁有這樣導航系統的人們可以輕而易舉的享受到眾多基於位置的服務 (Location-Based Services, LBS)。然而，由於城市叢林環境的影響，GPS信號在很大程度 上會被摩天大樓阻擋或是進行反射，導致最終不能直接通過GPS進行定位與導航，因此基 於低成本微電機系統（Micro Electro-Mechanic System, MEMS)的解決方案漸漸進入了人 們的視線。基於MEMS的行人慣性導航系統使用的是包含在自身的一組慣性傳感器，如加速 度傳感器，陀螺儀傳感器和磁力傳感器，不需要與外界進行通信，就可以估算和推測出行人 的運動軌跡。像這樣的慣性導航系統擁有體積小、成本低、高隱蔽性和高獨立性等優點。因 此慣性導航系統可以在GPS信號條件不好的環境中發揮自身的優勢。然而，慣性導航系統 自身有很多缺點，最嚴重的就是誤差會隨著時間累計，例如在用陀螺儀測量角速度時，由於 陀螺儀自身的一些特性，存在著零點漂移等現象，所以需要使用各種算法來不斷進行修正 和處理，消除累計誤差。並且由於精度的限制，一般的商業陀螺儀傳感器不能測量大範圍的 角速度值，而當行人在行走時甚至上下樓梯時，瞬時角速度值會遠大於其量程。
[0003] 目前已經有學者對基於MEMS的慣性導航系統進行研究，為了克服系統自身的缺 點並提高系統的精度，有些研究者用使用了計步器用於統計步伐數，同時基於這個統計的 步伐數再計算出平均步伐長度，就可以推算和估測出行走的距離和軌跡，但是計步器不能 區分出不同的步伐狀態，比如跑步、踏步等，當行人行走速度快時，該方法會漏估計，當行人 行走速度慢時，該方法會多估計，效果不是很理想。有人使用了一種比較複雜的類似之前 的計步器的方法，他們將一個兩軸加速度傳感器和一個兩軸陀螺安置在行人的腳上，通過 神經網絡的方法用加速度傳感器估計出步伐長度，並加入改進的卡爾曼濾波技術，來計算 出行人行走的軌跡，然而這種方法計算複雜度高，不能進行實時的導航。還有的研究者使用 超聲波和射頻進行輔助導航，然後由於他們要求收端和發端之間的傳播路徑上不能受到阻 擋，所以在地形不好的情況下會受到極大的限制。
[0004] 因此，在當前國內外的研究結果中，大多數算法的精度仍然不是很高，並且有些並 沒有考慮到計算複雜度對實時性的影響。
【發明內容】
[0005] 為了解決【背景技術】提出的技術問題，本發明旨在提供基於慣性傳感器的行人導航 裝置和方法，在保證精確度的同時極大限度地降低了計算的複雜度，在實際環境中不必消 耗大量硬體功耗也能保證實時性的要求。
[0006] 為了實現上述技術目的，本發明的技術方案為：
[0007] 基於慣性傳感器的行人導航裝置，包括微處理器以及分別與之連接的三軸加速度 傳感器、三軸陀螺儀、三軸磁力計和無線通信模塊，所述微處理器接收三軸加速度傳感器、 三軸陀螺儀和三軸磁力計採集的數據，並通過無線通信模塊將這些數據上傳至PC機上。
[0008] 本發明還包括基於上述基於慣性傳感器的行人導航裝置的行人導航方法，將基於 慣性傳感器的行人導航裝置固定在待測行人的腳上，三軸加速度傳感器、三軸陀螺儀和三 軸磁力計將採集到的數據通過無線通信模塊實時上傳至PC機上，再將這些數據經數據融 合算法計算出行人在三維空間中的運動軌跡，並將其在屏幕上實時顯示；所述數據融合算 法包括步伐階段檢測算法、腳體方位估計算法以及擴展卡爾曼濾波算法，利用三軸陀螺儀 採集的數據檢測出步伐階段，根據步伐階段以及三軸加速度傳感器、三軸陀螺儀、三軸磁力 計採集的數據估計出腳體方位，最後經過擴展卡爾曼濾波算法計算出行人的導航數據。
[0009] 其中，步伐檢測算法是將步伐階段分為依次循環的中間站立階段、腳尖離地階段、 擺動階段和腳跟觸地階段；
[0010] 當第個採樣點及其後續T -1個採樣點的和角速度均滿足：
[0011] COjb < COtJ I1 ^ i ^ I1+ T -I (1)
[0012] 則表示從I1個採樣點至其後續T-I個採樣點，行人處於中間站立階段，其中，
【權利要求】
1. 基於慣性傳感器的行人導航裝置，其特徵在於：包括微處理器以及分別與之連接的 三軸加速度傳感器、三軸陀螺儀、三軸磁力計和無線通信模塊，所述微處理器接收三軸加速 度傳感器、三軸陀螺儀和三軸磁力計採集的數據，並通過無線通信模塊將這些數據上傳至 PC機上。
2. 基於權利要求1所述基於慣性傳感器的行人導航裝置的行人導航方法，其特徵在 於：將基於慣性傳感器的行人導航裝置固定在待測行人的腳上，三軸加速度傳感器、三軸陀 螺儀和三軸磁力計將採集到的數據通過無線通信模塊實時上傳至PC機上，再將這些數據 經數據融合算法計算出行人在三維空間中的運動軌跡，並將其在屏幕上實時顯示；所述數 據融合算法包括步伐階段檢測算法、腳體方位估計算法以及擴展卡爾曼濾波算法，利用三 軸陀螺儀採集的數據檢測出步伐階段，根據步伐階段以及三軸加速度傳感器、三軸陀螺儀、 三軸磁力計採集的數據估計出腳體方位，最後經過擴展卡爾曼濾波算法計算出行人的導航 數據。
3. 根據權利要求2所述基於慣性傳感器的行人導航裝置的行人導航方法，其特徵在 於：所述步伐檢測算法是將步伐階段分為依次循環的中間站立階段、腳尖離地階段、擺動階 段和腳跟觸地階段； 當第h個採樣點及其後續t-1個採樣點的和角速度均滿足： w jb < w TH | ^ i ^ ij+ t -1 (1) 則表示從^個採樣點至其後續T-l個採樣點，行人處於中間站立階段，其中，
，cobxi、cobyi、cobzi為第i個採樣點三軸上的角速度分量，wTH |i2 ^i^i2+t-1 (2) 則表示從i2個採樣點至其後續T-1個採樣點，行人處於腳尖離地階段； 當第i3個採樣點及其後續T-1個採樣點的俯仰角均滿足： 0ib > 0bH| " 彡i彡 "+T-1 (3) 則表示從"個採樣點至其後續T-1個採樣點，行人處於擺動階段，其中，0^表示第i個採樣點腳的俯仰角； 當第i4個採樣點及其後續T-1個採樣點的俯仰角均滿足： 9,b < 9 b._11i4 ^i^i4+t-1 (4) 則表示從i4個採樣點至其後續T-1個採樣點，行人處於腳跟觸地階段。
4. 根據權利要求3所述基於慣性傳感器的行人導航裝置的行人導航方法，其特徵在 於：估計腳體方位的具體方法包括行人處於中間站立階段時的腳體方位估計和行人處於非 中間站立階段時的腳體方位估計； 所述中間站立階段時的腳體方位估計的步驟： (1) 利用三軸加速度傳感器測得的三軸上的加速度值計算出腳體的俯仰角en，進而得 到腳體圍繞xn軸旋轉en角度的旋轉四元數qen; (2) 利用三軸加速度傳感器測得的三軸上的加速度值計算出腳體的橫滾角f*%進而得 到腳體圍繞zn軸旋轉 < 角度的旋轉四元數f/ ; (3) 利用三軸磁力計測得的三軸上的磁場強度值計算出腳體的偏航角Vn，進而得到腳 體圍繞Yn軸旋轉Vn角度的旋轉四元數qVn; (4) 將q0n、？/、qVn依次相乘得到腳體方位四元數qf(rat。 所述非中間站立階段時的腳體方位估計的步驟： (1) 利用三軸陀螺儀測得三軸上的瞬時角速度值以及前一個採樣時刻的腳體方 位四元數^根據式（5)求出當前時刻的腳體方位的微分四元數
0表示四元數乘法運算； (2) 建立微分方程，得到當前時刻的腳體方位四元數qf(Krt:
上式中，Ts為採樣時間。
5.根據權利要求4所述基於慣性傳感器的行人導航裝置的行人導航方法，其特徵在 於：擴展卡爾曼濾波算法的具體步驟為： (1) 設置狀態矢量、卡爾曼增益以及均方差的初值，其中狀態矢量為13維，分別為導航 坐標系下三個方向的角度、位置、速度的測量值以及腳體方位四元數與它們本身真實值的 偏差； (2) 計算13維狀態矢量的狀態轉移矩陣，預測t時刻的狀態矢量和先驗均方差； (3) 基於零速度更新算法，當行人處於中間站立階段時，計算出t時刻的卡爾曼增益， 並根據卡爾曼增益計算出狀態矢量的偏差，根據卡爾曼增益和狀態矢量的偏差更新狀態矢 量和後驗均方差。
【文檔編號】G01C21/16GK104406586SQ201410729335
【公開日】2015年3月11日 申請日期:2014年12月4日 優先權日:2014年12月4日
【發明者】周亮, 周暄承, 陳建新, 王磊, 魏昕, 董義, 薛雯, 蘇宇 申請人:南京郵電大學