專利名稱：基于運動量測信息的個人定位方法及裝置的制作方法
技術(shù)領域：
本發(fā)明涉及導航與定位領域，尤其涉及一種基于運動量測信息的個人定位方法及
直O(jiān)
背景技術(shù)：
導航與定位技術(shù)是現(xiàn)代社會的重要支撐性基礎技術(shù)之一。目前主要的定位技術(shù) 按照原理分為如下幾種GPS定位；慣性導航定位(INS)、GPS/INS融合定位以及射頻識別 (RFID)定位等。它們各有優(yōu)缺點。GPS定位是目前使用最廣泛的定位方式。它的特點是定位 精度較好，使用范圍廣，定位可靠。但缺點是存在著信號遮擋問題，在城市建筑內(nèi)，林蔭道等 很多場合無法提供定位信息。慣性導航定位(INS)是根據(jù)慣性測量單元的數(shù)據(jù)，并采用時 間積分的方法來獲得軌跡，具有不受外界信號干擾，環(huán)境適應性強的優(yōu)點。但缺點是具有誤 差累積效應。因此實用中往往需要將慣性導航定位和GPS定位融合在一起使用，以定期修 正慣導的累積誤差。然而精度較高的慣性測量傳感器十分昂貴，低成本的慣性測量單元又 誤差較大，都無法直接在個人導航系統(tǒng)中應用。射頻識別(RFID)技術(shù)主要適合室內(nèi)環(huán)境， 而且也需要預先布置好大量的射頻定位設備才能工作，成本較高，目前尚處于實驗階段。除了以上提及的定位技術(shù)之外，與本發(fā)明有一定相關(guān)性的是步數(shù)計數(shù)器。它通過 檢測人體垂直軸上的加速度變化脈沖來確定走步，并進行計數(shù)。但它只能計數(shù)，沒有任何定 位功能。在個人定位的情況下，具有使用環(huán)境多變，運動方式多變的特點。前面提及的技術(shù) 都無法完全滿足要求。

發(fā)明內(nèi)容
為了克服現(xiàn)有技術(shù)方法精度不高，可靠性和環(huán)境適應性差的問題，本發(fā)明提供了 一種基于運動量測信息的個人導航定位方法和裝置，本發(fā)明能有效實現(xiàn)使用者在各種環(huán)境 下的可靠定位；具有攜帶便捷、使用方便、環(huán)境適應性好的特點。本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的—種基于運動量測信息的個人定位方法，該方法包括以下步驟(1)標定運動模型參數(shù)；(2)接收來自傳感器的數(shù)據(jù)；(3)確定當前朝向以及實際運動方向和朝向的差角；(4)確定當前的實際運動方向和步頻；(5)由步頻根據(jù)運動模型確定對應的步長；(6)由實際運動方向和步長推算當前位置。(7)在能獲得GPS信號時，利用GPS定位信息修正定位結(jié)果和對應運動模型參數(shù)。一種用于基于運動量測信息的個人定位裝置，它包括運動傳感裝置、人工及GPS 定位信息輸入裝置、信息處理和數(shù)據(jù)存儲裝置、定位導航結(jié)果輸出裝置。其中，所述運動傳感裝置、人工及GPS定位信息輸入裝置、定位導航結(jié)果輸出裝置分別與信息處理和數(shù)據(jù)存 儲裝置相連。本發(fā)明的有益效果是本發(fā)明通過對行動人員運動狀態(tài)的量測，通過運動分類和 運動模型來推算運動距離與方向，并最終確定行動人員的當前位置。同時它將運動模型和 GPS結(jié)合起來，利用GPS來修正定位誤差和模型參數(shù)誤差?？朔藗鹘y(tǒng)導航定位器在GPS信 號缺失時無法定位的缺點，又具有較高的定位精度。該發(fā)明的裝置具有輕巧便攜，抗干擾性 強，可靠性和精度較高的技術(shù)特點。


圖1是本發(fā)明的構(gòu)成原理圖；圖2是本發(fā)明的裝置結(jié)構(gòu)示意圖；圖3是本發(fā)明的方法總體流程圖。
具體實施例方式一、本發(fā)明的原理如圖1所示，本發(fā)明基于運動量測信息，結(jié)合人體運動模型的方法來計算行進距 離，判斷方向，確認地點高度，最后輸出導航解決方案。本方法的原理包括以下部分①傳感 信息輸入部分輸出運動的三軸線加速度值與三軸角速度值，高度值變化值，當前朝向值， GPS定位值等；②運動分類及定位部分接收傳感信息輸入部分輸入的信息后，根據(jù)設定的 步頻閾值，判別運動類型，獲得運動運動步長和運動方向值，并進行位置計算；③數(shù)據(jù)誤差 修正部分為運動分類及定位部分提供模型和定位數(shù)據(jù)修正；④標定處理部分為運動分 類部分提供各種運動模型的模型參數(shù)和分類判別閾值。1.傳感器數(shù)據(jù)輸入部分該部分一般由如下傳感器構(gòu)成三軸加速度和角速度 計、三軸電子羅盤、高度計等。以佩戴于人體腰部的導航裝置中心為原點，垂直向上為Z軸， 水平向右和向前分別為X軸和Y軸建立人體坐標系。該部分輸出數(shù)據(jù)為人體坐標系三個軸 上的線性加速度和角速度，以及當前人體坐標系相對導航坐標系(正北)的朝向和當前高 度。其中導航坐標系以某一已知位置為原點，正東和正北分別為X和Y軸，高度方向為Z軸 建立，分別用Xnav、Ynav和Znav表示。當有GPS信號時，也可以將該信號作為定位信息輸出。當 有現(xiàn)場的電子地圖時，也可以在電子地圖中人工輸入當前位置作為導航起始點或定位點。2.運動分類及定位部分該部分包含兩個部分運動類別判定算法和位置推算算 法。根據(jù)實際情況，將人體運動分為走路和跑步兩個大類。人體生理學研究表明，人體運動 時步頻和運動速度存在著一定的關(guān)系。運動類別判定算法是采用了如下的一個人體運動模 型f = C0Vbo( 1 )公式(1)中，f為步頻，ν為運動速度，C0和IDci為兩個待定的系數(shù)。設1為每步的 步長，由步頻f、速度ν和步長1之間的相互關(guān)系有ν = l*f (2)結(jié)合公式⑴和公式⑵，可得I = C;m°fWbo=Cfb(3)
6
其中: 公式(3)就是所采用的運動模型，它代表了步長和步頻的關(guān)系。當模型參數(shù)C和b 通過標定確定后，由垂直軸測得的線性加速度脈沖周期可以獲得步頻f，從而由公式(3)可 以得到步長1。對于行走和跑步兩種不同的運動類型，通過標定可以分別確定它們對應的模型參 數(shù)，分別為(cw，bw)和(Cpb》。如何區(qū)分兩者是靠一個步頻閾值&來確定的
( 4 )公式(4)中，1.9444m/s的速度是根據(jù)一般行走和跑步之間的速度來確定的， 1. 9444m/s相當于7km/h的速度。因此當檢測到使用者當前的步頻大于ft時，就判斷為跑 步，而小于ft時則判為走路。每種狀態(tài)分別對應不同的運動模型參數(shù)。當前的朝向Ψ —般由電子羅盤傳感器給出。陀螺儀給出的角速度經(jīng)過時間積分 后也能獲得當前的朝向改變，然而該方法受角速度漂移的影響較大。在系統(tǒng)中我們將從陀 螺儀獲得的朝向改變只用于一至二個行走周期的短時間，用于和羅盤輸出的角度改變進行 比較。當兩者的差別超過一個閾值時，判定為電子羅盤受周圍鐵器磁場的影響出現(xiàn)了方向 偏移，此時以陀螺儀積分獲得的角度變化量代替電子羅盤獲得的角度變化量。當人體向正前方運動時，運動方向就是當前朝向。而當人體后退、側(cè)移時，則運動 方向與朝向成一定角度。后退時，運動方向與當前朝向成180°差角；側(cè)移時，運動方向與 朝向成90°或-90°差角。在實際中，這個差角是通過檢測兩個水平軸上線性加速度峰值 之比來獲得的。設在本周期中人體坐標系中X和Y軸上的加速度值峰值分別為ax和ay，那 么在X-Y平面內(nèi)人體朝向與實際運動方向的差角θ為:θ = arctan (ax/ay) (5)由于加速度計誤差等原因，所獲得的差角θ在-180° 180°之間?？梢源_定如 下四種情況(a)若θ e [-45°，45° ]，則認為是向前行進，θ =0;(b)若θ e (45° 135° ]，則認為是向右側(cè)方行走，θ =90° ；(c)若θ e (-45° -135° ]，則認為是向 左側(cè)方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，則認 為是向后行進，θ =180°。因此實際的運動方向由當前朝向Ψ和差角θ的和得到。當爬坡或者爬樓時，在X-Y平面上的實際有效步長會縮短，但同時會在Z方向上有 位移。設坡度角為Φ，則X-Y方向上和Z方向上的實際步長為lx_y = Icos Φ , Iz = Isin Φ (6)爬坡狀態(tài)較難依靠傳感器信息確定，需要依賴電子地圖或者人工確定。樓梯和上 下坡的角度Φ可以事先人工輸入，也可以由電子地圖獲知。某一特定實現(xiàn)方案中，樓梯的 坡度可以根據(jù)多數(shù)情況預先確定一個初始值。在沒有任何坡度信息的情況下，可以直接以 氣壓高度計的輸出作為當前高度。 在運動分類與定位模塊中，位置推算算法接收來自于運動類別判定算法輸出的運 動步長和運動方向信息，并進行累積推算。設當前為第K步，航向角(與正北方向夾角)為 ψ，有 Xk = Xk-AlrySinv
yk = y.^+l^cos Ψ (7)zk = Ζη+1ζ3.數(shù)據(jù)誤差修正部分該部分接收來自GPS定位或者人工定位的信息，為運動分 類和定位部分提供可靠的導航結(jié)果和運動模型參數(shù)修正。該部分有兩個功能①定位誤差 修正；②運動模型參數(shù)修正。當有GPS信息時，它無疑比基于純運動模型的定位結(jié)果更加可靠，因此因該充分 利用。當前GPS的定位結(jié)果和位置推算結(jié)果的差值可以作為定位誤差修正值，直接修正當 前的位置推算結(jié)果。設由GPS獲得的當前時刻定位信息與之前某時刻GPS的定位信息之差為Δ Sgps,對 應該時間段內(nèi)來自于運動分類和定位模塊獲得的定位結(jié)果之差為Δ Sm，對應該段時間內(nèi)總 共行走了 N步，可以求出運動類型判定算法中的步長修正值ΔΙ。Δ 1可由公式(8)求出& = -A(8)
N因此修正后的步長為模型運動步長1和步長修正值Δ 1的和。只要有有效GPS信 號就可以隨時修正定位誤差。而模型參數(shù)的修正只有在如下條件都滿足時才進行①當前 存在有效GPS信號或者人工路標等外部定位輔助；②用于計算ASepsW這段時間中，步頻變 化很小，而且運動類別沒有發(fā)生變化；③用于計算ASeps的這段時間中，使用者的運動軌跡 是近似直線，即運動方向變化在一個很小范圍之內(nèi)；④用于計算ASeps的這段時間中，運動 步數(shù)N大于某一個閾值Nt。否則模型參數(shù)修正值都設定為0。4.標定處理模塊部分在運動模型中，每個不同使用者的參數(shù)(Cw，bw)和((；，br) 是不同的，在使用前需要通過標定過程來獲得。在一段長度為L的路程中，使用者分別用慢 走和快走兩種方式走完全程，總步數(shù)和用時為(N1, T1)和(N2, T2)，由公式(3)可得
, Ig(JV2ZiV1)K= 二二、(9)
lg(i^r2 /N2T1) Cw =
—LIN1 _ LIN2 ~ (NlZTl)b^ = (N2ZT2)b^同理，分別用慢跑和快跑兩種方式完成全程L，總步數(shù)和用時分別為(N/，T1 和(N2'，T2')，由公式(3)可得IlgwT2V^T1,) (10) Cr —.…ιιηη ,、 ~ (-/.τ t/T ,、br
ZZiV1' 一 LIN2' {Νλ Τλ')κ ~ (N2 T21Y 總長度L可以事先測得，也可以由在運動中直接由GPS等外部定位輔助獲得。并 由如下公式計算運動分類頻率閾值, _ Cw ft-+11.9444^+1 +Cr ^+11.9444^+1
Λ= 2 (11；經(jīng)過標定后的運動模型參數(shù)(Cw，bw)、(Cr, br)和頻率閾值ft都存放在裝置的系統(tǒng) 存儲器中，作為該使用者的參數(shù)。也可以每個使用者在使用前根據(jù)自己的不同ID號調(diào)用屬 于自己的運動模型參數(shù)，以確保參數(shù)正確。
8
如圖3所示，本發(fā)明基于運動量測信息的個人定位方法，包括以下步驟1、標定運動模型參數(shù)在開始定位之前，先采用本發(fā)明所提出的運動參數(shù)標定方法進行運動模型參數(shù)標 定每個不同使用者的參數(shù)(Cw，bw)和((；，br)是不同的。對步行模型參數(shù)標定得方法如 下在一段長度為L的路程中，使用者分別用慢走和快走兩種方式走完全程，總步數(shù)和用時 為(N1J1)和(N2，T2)，由公式(9)可得出走路模型參數(shù)。同理，分別用慢跑和快跑兩種方式 完成全程L，總步數(shù)和用時分別為(N/，T1')和(N2'，T2')，由公式(10)得出跑步模型 參數(shù)。由公式(11)可得出運動分類頻率閾值。經(jīng)過標定后的運動模型參數(shù)(Cw，bw)、(Cr, br)以及計算得到的運動分類步頻閾值ft都存放在裝置的系統(tǒng)非易失性存儲器中，作為該 使用者的運動模型參數(shù)。也可以每個使用者在使用前根據(jù)自己的不同ID號調(diào)用屬于自己 的運動模型參數(shù)，以確保參數(shù)正確。2、接收來自傳感器的數(shù)據(jù)傳感器數(shù)據(jù)一般來自如下傳感器三軸加速度和角速度計，三軸電子羅盤，高度計 等。以佩戴于人體腰部的導航裝置中心為原點，垂直向上為Z軸，水平向右和向前分別為X 軸和Y軸建立人體坐標系。該模塊的輸出數(shù)據(jù)為人體坐標系三個軸上的線性加速度和角速 度，以及當前人體坐標系相對導航坐標系(正北)的朝向和當前高度。其中導航坐標系以 某一已知位置為原點，正東和正北分別為X和Y軸，高度方向為Z軸建立，分別用Xnav、Ynav 和Znav表示。當有GPS信號時，也可以將該定位信號作為導航起始點。當有現(xiàn)場的電子地 圖時，也可以在電子地圖中人工輸入當前位置作為導航起始點或定位點。3、確定當前朝向以及實際運動方向和朝向的差角當前的朝向Ψ由電子羅盤傳感器給出。從陀螺儀獲得的朝向改變只用于一至二 個行走周期的短時間，用于和羅盤輸出的角度改變進行比較。當兩者的差別超過一個閾值 時，判定為電子羅盤受周圍鐵器磁場的影響出現(xiàn)了方向偏移，此時以陀螺儀積分獲得的角 度變化量代替電子羅盤獲得的角度變化量。當人體向正前方運動時，運動方向就是當前朝向。而當人體后退、側(cè)移時，則運動 方向與朝向成一定角度。后退時，運動方向與當前朝向成180°差角；側(cè)移時，運動方向與 朝向成90°或-90°差角。在實際中，這個差角是通過檢測兩個水平軸上線性加速度峰值 之比來獲得的。設人體坐標系中X和Y軸上的加速度值峰值分別為ax和ay，那么在X-Y平 面內(nèi)人體朝向與實際運動方向的差角θ為θ = arctan (ax/ay)由于加速度計誤差等原因，所獲得的差角θ在-180° 180°之間。可以確定如 下四種情況(a)若θ e [-45°，45° ]，則認為是向前行進，θ =0;(b)若θ e (45° 135° ]，則認為是向右側(cè)方行走，θ =90° ；(c)若θ e (-45° -135° ]，則認為是向 左側(cè)方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，則認 為是向后行進，θ = 180°。4、確定當前的實際運動方向和步頻實際的運動方向由當前朝向Ψ和差角θ的和得到。由垂直軸測得的線性加速度 脈沖周期可以獲得步頻f。5、由步頻根據(jù)運動模型確定對應的步長
對于行走和跑步兩種不同的運動類型，它們對應的模型參數(shù)分別為(Cw，bw)和 (Cr, br) 0如何區(qū)分兩者是靠一個步頻閾值ft來確定的。因此當檢測到使用者當前的步頻 大于ft時，就判斷為跑步，而小于或等于ft時則判為走路。每種狀態(tài)分別對應不同的運動 模型參數(shù)。并由公式(3)確定對應的步長。6、由實際運動方向和步長推算當前位置當爬坡或者爬樓時，在X-Y平面上的實際有效步長會縮短，但同時會在Z方向上有 位移。設坡度角為Φ，則X-Y方向上和Z方向上的實際步長為lx_y = Icos Φ , Iz = Isin Φ爬坡狀態(tài)較難依靠傳感器信息本身確定，需要依賴電子地圖或者人工確定。樓梯 和上下坡的角度Φ可以事先人工輸入，也可以由電子地圖獲知。某一特定實現(xiàn)方案中，樓 梯的坡度可以根據(jù)多數(shù)情況預先確定一個初始值。位置推算算法接收來自于運動類別判定算法輸出的運動步長和運動方向信息，并 進行累積推算。設當前為第K步，航向角(與正北方向夾角)為Ψ，則在導航坐標系下的坐 標為
‘Xk= Xk-i+lx-y\yk=y+LyCOSl//
. W1H當走完下一步后，又重新執(zhí)行步驟2)至6)。如此往復，由(Χ(1，％. Z0)開始，可以不 斷獲得(X1, I1, Z1)，(x2, y2. Z2)，(x3, y3，z3)…(xn，yn，zn)，并最終得到當前定位和全部運動 軌跡。7、當有GPS信息或者人工定位信息輸入時，執(zhí)行定位修正以及運動模型參數(shù)修正 量計算，否則，將所有校正量設為0，隨后通過定位及運動模型參數(shù)修正步驟將修正量加到 對應參量中去。當前GPS的定位結(jié)果和位置推算結(jié)果的差值可以直接作為定位誤差修正值。步長 誤差修正量則可由下面的方法計算。設由GPS獲得的當前定位信息與之前某一次的定位信 息之差為ASeps，對應該時間段內(nèi)來自于運動分類和定位模塊獲得的定位結(jié)果之差為Δ Sm， 對應該段時間內(nèi)總共行走了 N步，可以求出運動類型判定算法中的步長修正值Δ 1。Δ1可 由公式⑶求出Al=dsOPS-^m
N只要有有效GPS信號就可以隨時修正定位誤差。而模型參數(shù)的修正只有在如下條 件都滿足時才進行①當前存在有效GPS信號或者人工路標等外部定位輔助；②用于計算 ASgps的這段時間中，步頻變化很小，而且運動類別沒有發(fā)生變化；③用于計算ASeps的這 段時間中，使用者的運動軌跡是近似直線，即運動方向變化在一個很小范圍之內(nèi)；④用于計 算ASeps的這段時間中，運動步數(shù)N大于某一個閾值Nt。否則模型參數(shù)修正值都設定為0。如圖2所示，本發(fā)明用于基于運動量測信息的個人定位裝置，它包括運動傳感裝 置、人工及GPS定位信息輸入裝置、信息處理和數(shù)據(jù)存儲裝置、定位導航結(jié)果輸出裝置。其 中，所述運動傳感裝置、人工及GPS定位信息輸入裝置、定位導航結(jié)果輸出裝置分別與信息 處理和數(shù)據(jù)存儲裝置相連。具體如下
1、運動傳感裝置運動傳感裝置包括三軸加速度、三軸陀螺儀、三軸電子羅盤、氣壓高度計。其中， 三軸加速度計提供運動加速度；三軸陀螺儀提供運動角速度；三軸電子羅盤提供航向和姿 態(tài)角；氣壓高度計提供當前高度信息。運動傳感器裝置采集使用者的各種運動狀態(tài)和姿態(tài) 信息，提供給信息處理和數(shù)據(jù)存儲裝置。2、人工及GPS定位信息輸入裝置GPS信息可以是普通民用GPS信息，也可以是經(jīng)過差分站獲得的較高精度的DGPS 定位信息。人工及GPS定位信息輸入裝置可以是鍵盤或者是觸摸屏或者屏幕上顯示的軟鍵 盤等。所有這些信息也輸入到信息處理和數(shù)據(jù)存儲裝置中。3、信息處理和數(shù)據(jù)存儲裝置信息處理裝置包括由嵌入式微處理器或者微控制器等構(gòu)成的中央處理單元，它可 以是ARM、MIPS或者DSP等嵌入式微處理器，也可以是低端的MCS-51單片機等微控制器。數(shù) 據(jù)存儲裝置包括有內(nèi)存或非易失性存儲器件等構(gòu)成的程序和數(shù)據(jù)存儲空間。信息處理裝置 負責接收運動傳感裝置和人工及GPS定位信息輸入裝置的信息，處理得到導航定位結(jié)果以 及當前運動狀態(tài)和參數(shù)等信息。數(shù)據(jù)存儲裝置存放程序和數(shù)據(jù)，以及模型運動參數(shù)等。處 理結(jié)果輸出到定位導航結(jié)果輸出裝置中。4、定位導航結(jié)果輸出裝置該裝置提供了處理過后的個人定位數(shù)據(jù)輸出和顯示的功能。它具有有多種方式輸 出一種方式是通過標準接口如RS-232/RS-422/RS-485，或者USB等將個人的定位信息記 錄輸出到相應的主設備如計算機；也可以有LCD屏幕輸出，它可以將人行走的距離，方向， 高度，實時反映在顯示有電子地圖的屏幕上，使使用者清晰了解自己所處位置與環(huán)境；也可 以具有無線發(fā)射模塊，提供將定位數(shù)據(jù)通過無線信號發(fā)射出去的功能，使指揮部實時了解 所有當前應急響應人員的位置。并不是所有這些輸出裝置都是必須的，可以只具有其中的 某一種或者某兩種方式。下面詳細描述本發(fā)明的具體實施例，本發(fā)明的目的和效果將變得更加明顯。實施例1一種基于運動量測信息的個人定位方法，包括以下步驟1)標定運動模型參數(shù)在本例中，L= 100m, N1 = 196，T1 = 158. 05s, N2 = 138，T2 = 60. 98s,因此得 b _ ^(N2ZN1) _lg(138/196)Q5g33
w IgCiV1T2/TV2^) lg[(l 96 X 60.98) /(138 χ 158.05)]. LfNl ^ 100/196 (N1ZTl)b^ ~ (196/158.05)°
同理，分別用慢跑和快跑兩種方式完成全程L，(N/，T1')和(N2'，T2')分別 Cw =-1T- =-r-j^rr = 0.4500
Lw 」 w …λ* ζ οα/1 η 、0.5833
為(100,39. 28)和(75，23. 45)，由公式(10)可得(Cr，br)。在本例中，計算可得 br= WW) =_MW_= 1.2609
IgCiV1T2ViV2T1') lg[(l 00 X 23.45)/(75 X 39.28)]
LZN1' _ 100/100 (N1'/T1')"- ~ (100/39.28)1 Cr =….二、=________…細=0.3078
11
接下來根據(jù)公式(4)計算運動分類步頻閾值ft。在本例中，f _.. Cw ^+11.9444ft-+1 +Cr ^L9444^1 _ . ^99
2 一 .經(jīng)過標定后的運動模型參數(shù)(Cw，bw)、(Cr, br)以及計算得到的運動分類步頻閾值 ft都存放在裝置的系統(tǒng)非易失性存儲器中，作為該使用者的運動模型參數(shù)。也可以每個使 用者在使用前根據(jù)自己的不同ID號調(diào)用屬于自己的運動模型參數(shù)，以確保參數(shù)正確。2)接收來自傳感器的數(shù)據(jù)在本例中，設起始點位置μΤν， Γν，ΖΓ")為(100，33，0. 6)，單位為米。3)確定當前朝向以及實際運動方向和朝向的差角在本例中，ax = 0. 06g, ay = 0. 15g，其中g(shù)為重力加速度。由公式(5)可以算得 θ = arctan(ax/ay) =21.8°，顯然 θ G [-45°，45° ]，因此最后確定 θ =0。表明當 時使用者在向前走。4)確定當前的實際運動方向和步頻在本例中，當前朝向ψ =25°，θ = 0，表明使用者在向北偏東25°方向運動。 由垂直軸測得的線性加速度脈沖周期可以獲得步頻f，本例子中f = 2. 149Hz。5)由步頻根據(jù)運動模型確定對應的步長在本例中，f = 2. 149Hz < ft = 2. 3899Hz,因此判斷為走路狀態(tài)。而后根據(jù)公式 ⑶算出對應步長。本例中/ = Cw/" =0.45χ2.149?！?833 = 0.7031米。6)由實際運動方向和步長推算當前位置在本例中，坡度角φ = 0，步長lx_y = 1 = 0. 7031，Iz = 0，航向角Ψ = 25°，因 此有X1 = x0+lx_ysin￥ = 100+0. 7031 X sin25° = 100.2971yi = y0+lx_ycos￥ = 33+0. 7031Xcos25° = 33.6372Z1 = z0+lz = 0. 6+0 = 0. 6當走完下一步后，又重新執(zhí)行步驟2)至6)，并獲得(x2，y2.z2)。如此往復，可以不 斷獲得(X3，y3，Z3)，(x4, y4, Z4)…(xn，yn, zn)，并最終得到當前定位和全部運動軌跡。7)當有GPS信息或者人工定位信息輸入時，執(zhí)行定位修正以及運動模型參數(shù)修正 量計算步驟，否則，將所有校正量設為0。隨后通過定位及運動模型參數(shù)修正步驟將修正量 加到對應參量中去。在本例中，如上條件都滿足，N = 266 > Nt = 100，因此可以進行校正。在這段時 刻中，設 ASgps = 120. 35m，Δ Sm = 115. 78，因此有
「 A7 ASops-ASm 120.35-115.78Al = ~—-- =-= 0.0172
N266因此，校正后的步長為1 = 1+Δ 1 = 0. 7031+0. 0172 = 0. 7203 米。實施例2一種用于基于運動量測信息的個人定位裝置，它包括1)運動傳感裝置，包括三軸加速度計提供運動加速度；三軸陀螺儀提供運動角 速度；三軸電子羅盤提供航向和姿態(tài)角；氣壓高度計提供當前高度信息。并不是所有這些傳感器都是必須的，在某些實現(xiàn)案例中，也可能沒有陀螺儀，或者采用單軸的電子羅盤和陀 螺儀等，也可能沒有高度計。運動傳感器裝置采集使用者的各種運動狀態(tài)和姿態(tài)信息，提供 給息處理和數(shù)據(jù)存儲裝置。2)人工及GPS定位信息輸入裝置，GPS定位信息輸入裝置可以是固定設計在系統(tǒng) 處理板上，也可以由通訊接口通過線纜和系統(tǒng)相連接。GPS信息可以是普通民用GPS信息， 也可以是經(jīng)過差分站獲得得較高精度的DGPS定位信息。人工信息輸入裝置可以是鍵盤或 者是觸摸屏或者屏幕上顯示的軟鍵盤等。所有這些信息也輸入到息處理和數(shù)據(jù)存儲裝置 中。3)信息處理和數(shù)據(jù)存儲裝置，信息處理裝置包括由嵌入式微處理器或者微控制器 等構(gòu)成的中央處理單元，它可以是ARM、MIPS或者DSP等嵌入式微處理器，也可以是低端的 MCS-51單片機等微控制器。數(shù)據(jù)存儲裝置包括有內(nèi)存或非易失性存儲器件等構(gòu)成的程序和 數(shù)據(jù)存儲空間。信息處理裝置負責接收運動傳感裝置和人工及GPS定位信息輸入裝置的信 息，運行由本發(fā)明所提出的方法設計的程序，得到導航定位結(jié)果以及當前運動狀態(tài)和參數(shù) 等信息。數(shù)據(jù)存儲裝置存放程序和數(shù)據(jù)，以及模型運動參數(shù)等。處理結(jié)果輸出到定位導航 結(jié)果輸出裝置中。4)定位導航結(jié)果輸出裝置，輸出電路與核心處理器相連，提供了處理過后的個 人定位數(shù)據(jù)輸出和顯示的功能。它具有有多種方式輸出一種方式是通過標準接口如 RS-232/RS-422/RS-485，或者USB等將個人的定位信息記錄輸出到相應的主設備如計算 機；也可以有LCD屏幕輸出，它可以將人行走的距離，方向，高度，實時反映在顯示有電子地 圖的屏幕上，使使用者清晰了解自己所處位置與環(huán)境；也可以具有無線發(fā)射模塊，提供將定 位數(shù)據(jù)通過無線信號發(fā)射出去的功能，使指揮部實時了解所有當前應急響應人員的位置。 并不是所有這些輸出裝置都是必須的，可以只具有其中的某一種或者某兩種方式。
1權(quán)利要求
一種基于運動量測信息的個人定位方法，其特征在于，該方法包括以下步驟(1)標定運動模型參數(shù)。(2)接收來自傳感器的數(shù)據(jù)。(3)確定當前朝向以及實際運動方向和朝向的差角。(4)確定當前的實際運動方向和步頻。(5)由步頻根據(jù)運動模型確定對應的步長。(6)由實際運動方向和步長推算當前位置。(7)當有GPS信息或者人工定位信息輸入時，可以執(zhí)行定位修正，否則定位修正量為0。在同時滿足一定特定條件時，還可以進行運動模型參數(shù)修正量計算，否則模型參數(shù)修正值設定為0。通過定位及運動模型參數(shù)修正步驟將修正量加到對應系統(tǒng)參量中去。
2.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(1) 具體為每個不同使用者的行走運動模型參數(shù)(Cw，bw)和跑步運動模型參數(shù)(C,，b,)是不同 的。對步行模型參數(shù)標定的方法如下在一段長度為L的路程中，使用者分別用慢走和快走 兩種方式走完全程，總步數(shù)和用時為(N1, T1)和(N2，T2)，由下式可得出模型參數(shù) 對跑步運動模型參數(shù)標定的方法如下分別用慢跑和快跑兩種方式完成全程L，總步 數(shù)和用時分別為(N/，T1')和(N2'，T2')，由下式得出模型參數(shù) 由下式可得出運動分類頻率閾值
3.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(2) 具體為傳感器數(shù)據(jù)來自如下傳感器三軸加速度、三軸陀螺儀、三軸電子羅盤、氣壓高度 計。以佩戴于人體腰部的導航裝置中心為原點，垂直向上為Z軸，水平向右和向前分別為X 軸和Y軸建立人體坐標系。傳感器數(shù)據(jù)為人體坐標系三個軸上的線性加速度和角速度，以 及當前人體坐標系相對導航坐標系的朝向和當前高度。其中導航坐標系以某一已知位置為 原點，正東和正北分別為X和Y軸，高度方向為Z軸建立，分別用Xnav、Ynav和Znav表示。當有 GPS信號時，也可以將該定位信號作為導航起始點。當有現(xiàn)場的電子地圖時，也可以在電子 地圖中人工輸入當前位置作為導航起始點或定位點。
4.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(3) 具體為當前的朝向Ψ由電子羅盤傳感器給出。從陀螺儀獲得的朝向改變只用于一至二個 行走周期的短時間，用于和羅盤輸出的角度改變進行比較。當兩者的差別超過一個閾值時，判定為電子羅盤受周圍鐵器磁場的影響出現(xiàn)了方向偏移，此時以陀螺儀積分獲得的角度變 化量代替電子羅盤獲得的角度變化量。當人體向正前方運動時，運動方向就是當前朝向。而 當人體后退、側(cè)移時，則運動方向與朝向成一定角度。后退時，運動方向與當前朝向成180° 差角；側(cè)移時，運動方向與朝向成90°或-90°差角。在實際中，這個差角是通過檢測兩個 水平軸上線性加速度峰值之比來獲得的。設人體坐標系中X和Y軸上的加速度值峰值分別 為\和 ，那么在X-Y平面內(nèi)人體朝向與實際運動方向的差角θ為:θ = arctan (ax/ay)差角θ在-180° 180°之間?？梢源_定如下四種情況(a)若θ e [-45°， 45° ]，則認為是向前行進，θ =0;(b)若θ e (45° 135° ]，則認為是向右側(cè)方行走， θ =90° ；(c)若θ e (-45° -135° ]，則認為是向左側(cè)方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，則認為是向后行進，θ = 180°。
5.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(4) 具體為實際的運動方向由當前朝向Ψ和差角θ的和得到，由垂直軸測得的線性加速度脈 沖周期獲得步頻f。
6.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(5) 具體為當檢測到使用者當前的步頻f大于步頻閾值ft時，就判斷為跑步，由公式/ = CJh' 確定步長。而使用者當前的步頻f小于或等于步頻閾值ft時，則判為走路，由公式/ = CJk 確定步長。
7.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(6) 具體為當爬坡或者爬樓時，在X-Y平面上的實際有效步長會縮短，但同時會在Z方向上有 位移。設坡度角為Φ，則X-Y方向上和Z方向上的實際步長為lx_y = Icos Φ , Iz = Isin Φ爬坡狀態(tài)較難依靠傳感器信息本身確定，需要依賴電子地圖或者人工確定。樓梯和上 下坡的角度Φ可以事先人工輸入，也可以由電子地圖獲知。某一特定實現(xiàn)方案中，樓梯的 坡度可以根據(jù)多數(shù)情況預先確定一個初始值。位置推算算法接收來自于運動類別判定算法輸出的運動步長和運動方向信息，并進行 累積推算。設當前為第K步，航向角(與正北方向夾角)為Ψ，則在導航坐標系下的坐標 為Xk = Xk-i+lx-ysin￥Yk = Yk-1 + lx-yCOS ΨZk — Zk-l + lz當走完下一步后，又重新執(zhí)行步驟2)至7)，并獲得(x2，y2.z2)。如此往復，可以不斷獲 得(x3，y3，z3)，(x4, y4，z4)…(xn，yn，zn)，并最終得到當前定位和全部運動軌跡。
8.根據(jù)權(quán)利要求1所述基于運動量測信息的個人定位方法，其特征在于，所述步驟(7) 具體為當前GPS的定位結(jié)果和位置推算結(jié)果的差值可以直接作為定位誤差修正值。步長 誤差修正量則可由下面的方法計算設由GPS獲得的當前定位信息與之前某一次的定位信 息之差為ASeps，對應該時間段內(nèi)來自于運動分類和定位方法獲得的定位結(jié)果之差為Δ Sm， 對應該段時間內(nèi)總共行走了 N步，可以求出運動類型判定算法中的步長修正值Δ 1。Δ1可 由下式求出AI ^ASgps-ASm N只要有有效GPS信號就可以隨時修正定位誤差。而模型參數(shù)的修正只有在如下條件都 滿足時才進行①當前存在有效GPS信號或者人工路標等外部定位輔助；②用于計算ASeps 的這段時間中，步頻變化很小，而且運動類別沒有發(fā)生變化；③用于計算ASeps的這段時 間中，使用者的運動軌跡是近似直線，即運動方向變化在一個很小范圍之內(nèi)；④用于計算 ASgps的這段時間中，運動步數(shù)N大于某一個閾值Nt。否則模型參數(shù)修正值都設定為0。
9. 一種用于基于運動量測信息的個人定位裝置，其特征在于，它包括運動傳感裝置、 人工及GPS定位信息輸入裝置、信息處理和數(shù)據(jù)存儲裝置、定位導航結(jié)果輸出裝置。其中， 所述運動傳感裝置、人工及GPS定位信息輸入裝置、定位導航結(jié)果輸出裝置分別與信息處 理和數(shù)據(jù)存儲裝置相連。
全文摘要
本發(fā)明公開了一種基于運動量測信息的個人定位方法及裝置，本發(fā)明通過對行動人員運動狀態(tài)的量測，通過運動分類和運動模型來推算運動距離和方向，并最終確定行動人員的當前位置；同時它將運動模型和GPS結(jié)合起來，利用GPS來修正定位誤差和模型參數(shù)誤差。本發(fā)明克服了傳統(tǒng)導航定位器在GPS信號缺失時無法定位的缺點，又具有較高的定位精度。本發(fā)明的裝置具有輕巧便攜，抗干擾性強，可靠性和精度較高的技術(shù)特點。
文檔編號G01C21/04GK101907467SQ201010248929
公開日2010年12月8日 申請日期2010年8月6日 優(yōu)先權(quán)日2010年8月6日
發(fā)明者劉濟林, 王嘉鋒, 項志宇 申請人:浙江大學