基於人體運動模型輔助的穿戴式行人導航定位方法和設備與流程
2023-04-23 10:18:21 3
本發明涉及基於人體運動模型輔助的穿戴式行人導航定位方法和設備，屬於個人移動定位技術領域。
背景技術：
行人定位導航作為近些年來民用導航技術發展的主要領域之一，正逐漸被重視和研究。近年來，個人定位設備逐步在民用化市場得到普及，而大多數民用級可攜式設備採用的是gps衛星定位與無線通信技術相結合的定位原理，但是在大型城市、商場、隧道、叢林和山谷等衛星導航信號易受幹擾或屏蔽的應用環境中，該類型設備的導航定位功能將受到嚴重影響。在某些特定行業中，如消防員在執行救援任務時，可能出現匍匐、跳躍、跑步等多種複雜運動模態，所攜帶的導航定位設備容易失效或者定位錯誤，而導致搜救任務失敗，嚴重時甚至將危及使用人員的人身安全。針對無法使用衛星定位和通信定位產品的使用環境，國外廠商對我國技術壟斷，產品價格昂貴。因此，迫切需要研究適應大型城市、商場、隧道、叢林和山谷等複雜應用環境下的針對人體多運動模態的行人導航定位技術，以適應相應的需求。
現有行人導航研究方向主要可分為以下兩類：一是基於各類無線網絡的源頭定位，然而這類方法依賴額外設備(如wifi、藍牙、uwb等)，定位精度受環境影響較大，易受幹擾；二是基於慣性傳感器的行人導航定位，以慣性器件為核心，具有短時高精度和高穩定性的特點，是一種完全自主的導航系統，然而該方案在長時間下誤差會累積而迅速發散。目前，國外個人定位系統主要採用了微慣性/衛星導航實現組合定位，其對衛星導航依賴性較強；美國在基於人體運動學輔助的行人定位算法方面取得了較好的研究進展，已經完成了原型樣機的研製，不過尚沒有向民用領域進行大規模的推廣應用。國內也有多家單位和研究所針對個人微慣性定位算法進行了研究，在系統構造以及傳感器誤差修正等方面開展了富有成效的工作，但總體上的研究水平與國外還具有一定的差距，在基於人體運動學輔助的行人定位算法研究方面尚處於起步階段。其只能實現正常行走模態下的行人定位導航技術，而且多採用了額外設備輔助慣導的方式，既極大增加了成本，又約束了其適用範圍，因此，迫切需要研究一種基於人體運動學建模輔助的定位技術，基於運動學特徵建立人體運動模型，實時判別人體運動模式，實現在沒有衛星導航與無線通信信號下人體多模態的自主實時移動定位。
對於單一自主的慣性導航，低成本的mems捷聯解算高度誤差較大，而且發散速度較快，必須要經過有效的修正，才能確保高度計算精度實際可用。捷聯解算的航向誤差也較大，容易發生波動，導致導航結果與實際路線不符，必須要經過有效的修正，才能提高導航結果的精度。行人在實際運動過程中存在多種運動模態，如快速行走、跑步、上下樓梯、跳躍等，同時，不同的人在運動過程中步態間也存在較大差異，步態的檢測與分辨困難；而現有的國內外研究主要集中在行人的正常行走模態的辨識，對於快速行走、跑步、上下樓梯、跳躍等還鮮有提及。
技術實現要素：
為了解決上述存在的問題，本發明公開了基於人體運動模型輔助的穿戴式行人導航定位方法和設備，提供在gps信號以及無線通訊信號失效時行人的連續導航定位方法，該方法能夠在行人正常步行，快速行走，慢跑，上下樓梯等運動模態下實現姿態，速度和位置解算，滿足複雜應用環境下行人的高精度實時導航定位要求，其具體技術方案如下：
基於人體運動模型輔助的穿戴式行人導航定位方法，其特徵在於包括以下操作步驟:
步驟1：分析人體運動時的步態以及不同運動模態下的慣性傳感器輸出，所述步態為人體運動時足部與地面的接觸情況，所述不同運動模態包括步行、跑步、上下樓梯；
步驟2：建立基於人體運動學模型輔助的零速修正判別模型，所述基於人體運動學模型輔助的零速修正判別模型利用加速度計和陀螺儀的輸出判斷當前時刻是否為足部與地面的接觸時刻；
步驟3：在步驟1和2的基礎上，建立基於地磁輔助捷聯慣導解算航向角誤差模型，利用磁傳感器輸出信息計算當地磁航向角；
步驟4：在步驟1和2的基礎上，建立基於氣壓高度計輔助捷聯慣導解算高度誤差模型，利用氣壓計輸出信息計算當地海拔高度。
所述步驟1中人體運動步態以及不同運動模態具有以下特徵：
在正常行走時，行人的雙腳交替運動，分為四個階段：抬腳、跨步、落地和支撐，兩隻腳交替運動，分別處於不同的時刻，因此，將傳感器件固定於其中一隻腳上，對其進行運動分析，此時，人體足部不能被視為一個質點，在判斷零速時刻時，應對落地、支撐階段作進一步的分析；
當以腳尖上一點為質點時，在落腳階段，腳後跟先與地面接觸，接著以腳後跟為支點整個腳底部逐漸放平直至完全接觸地面，即腳尖與地面接觸，然後，以腳尖為支點，整個腳底部逐漸抬升直至完全離開地面，即腳尖與地面分離，腳尖上質點與地面的實際接觸時間為一個步態期間零速度的時間；
行人在快速行走時，步態周期將比正常行走的周期短，但是兩者足部的運動過程是相似的，在這兩種運動模態下，足部大約有一半的時間是處於抬腳、跨步階段，而相應地，另一隻腳同時處於落地和支撐階段，在大步行走時，足部運動過程仍然是與正常行走時相似，步態周期更長，這兩種運動模態下，足部運動的周期性和對稱性並未發生改變；
在跑步運動模態中，則不存在這種對稱性，當一隻腳的後跟落地時，另一條腿可能還在空中，甚至可能出現兩條腿同時處於空中的狀態，但是，跑步運動模態下一隻腳落地支撐階段的零速時刻與正常行走時是類似的，但其足部與地面的接觸時間更短些；
在上下樓梯運動模態中，在抬腳階段增加了高度信息的變化，這並不影響零速時刻的判斷，落地支撐階段，足部的零速時刻與正常行走是類似的，但是，與正常行走模態不同的是，落地階段不再是腳後跟先接觸地面，而是足部的前端先落地，然後以其為支點，腳底部逐漸放平直至完全與地面接觸，在實際情況中，行人的腳底部可能並不是完全地與地面接觸，通常接觸部分只有腳掌的前大半部分，此時，腳後跟是懸空的狀態；
通過對不同運動模態下imu慣性傳感器件三軸數據信息的採集，能夠得出，在不同的運動模態下，陀螺儀和加速度計的三軸輸出信息與正常行走時相似，仍然具有周期性，行人在快速行走或跑步較劇烈運動模態下，imu慣性傳感器件的輸出變化也更快，此時，imu慣性傳感器件的輸出或用於零速判定的條件會失效；在勻速行走等正常模態下，imu慣性傳感器件的輸出較為平緩；在上下樓梯運動模態下，能夠看出，imu慣性傳感器件的輸出更加平緩，為了保持平衡，人體在足部落地階段需要停留更長的時間。
所述步驟2中建立基於人體運動學模型輔助的零速修正判別模型的算法為：
在人體行走過程中，足部運動包含抬腳、跨步、落地、支撐四個階段，在落地支撐階段存在一段時間足部的速度為零，需要對陀螺儀和加速度計的輸出信息進行判斷，當滿足條件時，即在算法上將速度置零，當檢測到足部為零速度時，採用加速度計的測量值重新求解姿態角，以抑制慣導誤差的發散：
其中，θ、γ依次是俯仰角、橫滾角，依次是加速度計的x、y、z三軸輸出，g是當地的重力加速度值。
所述對陀螺儀和加速度計的輸出信息進行判斷用的判斷方法是：以陀螺儀輸出為主判斷條件、加速度計輸出為輔判斷條件的滑動窗口法，滑動窗口的寬度設為n，n的大小根據不同的步態情況設置為不同的值，先設置如下參數：
stdy＝std(datay,0,1)
stdz＝std(dataz,0,1)
ωxωyωz依次是陀螺儀x、y、z三軸輸出的極大值和極小值之差，a是加速度計x、y、z三軸輸出值平方和開方後的最大值，stdy，stdz依次是加速度計y、z軸方差，axi,ayi,azi分別為加速度計x軸，y軸，z軸滑動窗口內從0到n時刻的輸出值；
為判斷當前時刻是否為零速時刻，取當前時刻後的一小段時間，分別利用加速度計信息和陀螺儀信息來判斷輸出數據是否滿足閾值設置，在不同模態下設置相應的參數，加速度計三軸模值以及方差輸出滿足條件時，人為地將速度置零；陀螺儀的任一軸滿足條件時，速度置零的同時更新姿態角信息，並對誤差進行觀測和補償，以獲得更加精確的姿態和位置信息。
所述步驟3中的建立基於地磁輔助捷聯慣導解算航向角誤差模型的方法為：
將磁傳感器獲取的當地三軸磁數據投影到水平面上，將磁信息從機體系轉換到導航系下，
其中，θ、γ依次是俯仰角、橫滾角，依次是x、y、z三軸地磁信息在導航坐標系下的投影，是地磁信息在機體系下的輸出，是地磁信息在導航系下的輸出，
然後根據投影后水平方向的磁傳感器數據計算磁航向角，三軸磁信息的正負不同，所對應的計算方式也不同：
ψe為待求解的航向角信息；為提高磁航向的精度，事先對磁傳感器進行橢圓標定：首先使用傳統最小二乘橢圓擬合算法求解磁傳感器數據的初始最佳擬合橢圓參數，隨後由初始參數通過實時遞推能夠獲得實時的最佳擬合橢圓參數，橢圓方程為：
ax2+by2+cz2+2dxy+2exz+2fyz+2gx+2hy+2iz＝1，
方程中a,b,c,d,e,f,g,h,i均為橢圓擬合的參數，x,y,z分別對應所擬合三維橢球體的三軸，接著對後續測得的磁傳感器數據(hx,hy)進行坐標平移、旋轉和拉伸，將其映射為正圓坐標(h'x,h'y)；最後通過補償的磁傳感器數據，計算求得磁航向角為由於人體安裝所造成的航向偏差角。
所述步驟4中的建立基於氣壓高度計輔助捷聯慣導解算高度誤差模型的方法為：
利用大氣壓隨水平高度升高按指數律遞減的關係，通過感受行人導航定位設備所處位置的大氣壓的大小，推算所處位置的海拔高度，氣壓高度測量模型為：
氣壓高度(m):
上式中，hpb為所求氣壓高度，t0為傳感器所處環境溫度，psb為傳感器所處環境氣壓值，ps0為海平面氣壓高度值，τ,r均為氣壓高度求解參數。
一種實現基於人體運動模型輔助的穿戴式行人導航定位方法的設備，該設備能夠實現基於人體運動模型輔助的穿戴式行人導航定位方法，該設備內置有imu慣性傳感器件、姿態解算模塊、藍牙模塊和按鈕。
本發明採用以上技術方案與現有技術相比，具有以下技術效果：
本發明在分析人體運動時的步態以及不同運動模態下的慣性傳感器輸出的基礎上，提出零速修正判別算法輔助捷聯慣導解算以修正捷聯解算的速度位置等信息隨時間發散的問題；同時建立地磁航向輔助模型以及氣壓高度輔助模型以修正航向角和高度解算誤差。
本發明很好地解決純慣性解算下速度位置的發散問題，提高無gps和無線通信信號下行人導航定位的精度和可靠性。
附圖說明
圖1是本發明個人導航定位算法解算流程圖，
圖2是本發明人體正常行走模態下陀螺儀和加速度計三軸輸出示意圖，
圖3是本發明人體跑步模態下陀螺儀和加速度計三軸輸出示意圖，
圖4是本發明人體上下樓梯模態下陀螺儀和加速度計三軸輸出示意圖，
圖5是本發明根據加速度計和陀螺儀信息判斷出的零速區間示意圖，
圖6是本發明三軸磁傳感器地磁場測量原理圖，
圖7是本發明純慣性高度通道示意圖，
圖8是本發明二階氣壓阻尼算法示意圖，
圖9是本發明行人實際行走路線圖，
圖10是圖9行人二維軌跡相對位置仿真圖，
圖11是圖9行人三維軌跡相對位置仿真圖。
具體實施方式
下面結合附圖和具體實施方式，進一步闡明本發明。應理解下述具體實施方式僅用於說明本發明而不用於限制本發明的範圍。
基於人體運動模型輔助的穿戴式行人導航定位方法，包括以下操作步驟:
步驟1：分析人體運動時的步態以及不同運動模態下的慣性傳感器輸出，所述步態為人體運動時足部與地面的接觸情況，所述不同運動模態包括步行、跑步、上下樓梯；
步驟2：建立基於人體運動學模型輔助的零速修正判別模型，所述基於人體運動學模型輔助的零速修正判別模型利用加速度計和陀螺儀的輸出判斷當前時刻是否為足部與地面的接觸時刻；
步驟3：在步驟1和2的基礎上，建立基於地磁輔助捷聯慣導解算航向角誤差模型，利用磁傳感器輸出信息計算當地磁航向角；
步驟4：在步驟1和2的基礎上，建立基於氣壓高度計輔助捷聯慣導解算高度誤差模型，利用氣壓計輸出信息計算當地海拔高度。
所述步驟1中人體運動步態以及不同運動模態具有以下特徵：
在正常行走時，行人的雙腳交替運動，分為四個階段：抬腳、跨步、落地和支撐，兩隻腳交替運動，分別處於不同的時刻，因此，將傳感器件固定於其中一隻腳上，對其進行運動分析，此時，人體足部不能被視為一個質點，在判斷零速時刻時，應對落地、支撐階段作進一步的分析；
當以腳尖上一點為質點時，在落腳階段，腳後跟先與地面接觸，接著以腳後跟為支點整個腳底部逐漸放平直至完全接觸地面，即腳尖與地面接觸，然後，以腳尖為支點，整個腳底部逐漸抬升直至完全離開地面，即腳尖與地面分離，腳尖上質點與地面的實際接觸時間為一個步態期間零速度的時間；
行人在快速行走時，步態周期將比正常行走的周期短，但是兩者足部的運動過程是相似的，在這兩種運動模態下，足部大約有一半的時間是處於抬腳、跨步階段，而相應地，另一隻腳同時處於落地和支撐階段，在大步行走時，足部運動過程仍然是與正常行走時相似，步態周期更長，這兩種運動模態下，足部運動的周期性和對稱性並未發生改變；
在跑步運動模態中，則不存在這種對稱性，當一隻腳的後跟落地時，另一條腿可能還在空中，甚至可能出現兩條腿同時處於空中的狀態，但是，跑步運動模態下一隻腳落地支撐階段的零速時刻與正常行走時是類似的，但其足部與地面的接觸時間更短些；
在上下樓梯運動模態中，在抬腳階段增加了高度信息的變化，這並不影響零速時刻的判斷，落地支撐階段，足部的零速時刻與正常行走是類似的，但是，與正常行走模態不同的是，落地階段不再是腳後跟先接觸地面，而是足部的前端先落地，然後以其為支點，腳底部逐漸放平直至完全與地面接觸，在實際情況中，行人的腳底部可能並不是完全地與地面接觸，通常接觸部分只有腳掌的前大半部分，此時，腳後跟是懸空的狀態；
通過對不同運動模態下imu慣性傳感器件三軸數據信息的採集，能夠得出，在不同的運動模態下，陀螺儀和加速度計的三軸輸出信息與正常行走時相似，仍然具有周期性，行人在快速行走或跑步較劇烈運動模態下，imu慣性傳感器件的輸出變化也更快，此時，imu慣性傳感器件的輸出或用於零速判定的條件可能會失效；在勻速行走等正常模態下，imu慣性傳感器件的輸出較為平緩；在上下樓梯運動模態下，能夠看出，imu慣性傳感器件的輸出更加平緩，為了保持平衡，人體在足部落地階段需要停留更長的時間。
根據權利要求2所述的基於人體運動模型輔助的穿戴式行人導航定位方法，其特徵在於所述步驟2中建立基於人體運動學模型輔助的零速修正判別模型的算法為：
在人體行走過程中，足部運動包含抬腳、跨步、落地、支撐四個階段，在落地支撐階段存在一段時間足部的速度為零，需要對陀螺儀和加速度計的輸出信息進行判斷，當滿足條件時，即在算法上將速度置零，當檢測到足部為零速度時，採用加速度計的測量值重新求解姿態角，以抑制慣導誤差的發散：
其中，θ、γ分別是俯仰角、橫滾角，分別是加速度計的x、y、z三軸輸出，g是當地的重力加速度值。
所述對陀螺儀和加速度計的輸出信息進行判斷用的判斷方法是：以陀螺儀輸出為主判斷條件、加速度計輸出為輔判斷條件的滑動窗口法，滑動窗口的寬度設為n，n的大小根據不同的步態情況設置為不同的值，先設置如下參數：
stdy＝std(datay,0,1)
stdz＝std(dataz,0,1)
ωxωyωz分別是陀螺儀x、y、z三軸輸出的極大值和極小值之差，a是加速度計x、y、z三軸輸出值平方和開方後的最大值，stdy，stdz分別是加速度計y、z軸方差，
為判斷當前時刻是否為零速時刻，取當前時刻後的一小段時間，分別利用加速度計信息和陀螺儀信息來判斷輸出數據是否滿足閾值設置，在不同模態下設置相應的參數，加速度計三軸模值以及方差輸出滿足條件時，人為地將速度置零；陀螺儀的任一軸滿足條件時，速度置零的同時更新姿態角信息，並對誤差進行觀測和補償，以獲得更加精確的姿態和位置信息。
所述步驟3中的建立基於地磁輔助捷聯慣導解算航向角誤差模型的方法為：將磁傳感器獲取的當地三軸磁數據投影到水平面上，將磁信息從機體系轉換到導
航系下，
其中，θ、γ分別是俯仰角、橫滾角，分別是x、y、z三軸地磁信息在導航坐標系下的投影，是地磁信息在機體系下的輸出，是地磁信息在導航系下的輸出，
然後根據投影后水平方向的磁傳感器數據計算磁航向角，三軸磁信息的正負不同，所對應的計算方式也不同：
為提高磁航向的精度，事先對磁傳感器進行橢圓標定：首先使用傳統最小二乘橢圓擬合算法求解磁傳感器數據的初始最佳擬合橢圓參數，隨後由初始參數通過實時遞推能夠獲得實時的最佳擬合橢圓參數，橢圓方程為：
ax2+by2+cz2+2dxy+2exz+2fyz+2gx+2hy+2iz＝1，
接著對後續測得的磁傳感器數據(hx,hy)進行坐標平移、旋轉和拉伸，將其映射為正圓坐標(h'x,h'y)；最後通過補償的磁傳感器數據，計算求得磁航向角
所述步驟4中的建立基於氣壓高度計輔助捷聯慣導解算高度誤差模型的方法為：
利用大氣壓隨水平高度升高按指數律遞減的關係，通過感受行人導航定位設備所處位置的大氣壓的大小，推算所處位置的海拔高度，氣壓高度測量模型為：
氣壓高度(m):
本發明設備在實際導航定位過程中，用膠帶將本設備固定於人體足部腳背位置。本設備內置了imu慣性傳感器件，姿態解算模塊，藍牙模塊和按鈕。
如圖1所示，本發明複雜應用環境下基於人體運動模型輔助的可穿戴式行人導航定位算法解算流程有以下步驟：
步驟1，慣性測量單元獲取行人運動時的加速度和角加速度信息，經過誤差補償，陀螺儀數據求解四元數並進行規範化，由四元數得到姿態變換矩陣並將獲取的加速度計信息進行比力轉換，由姿態轉換矩陣求解姿態角，由四元數方程和上一時刻的導航信息求解速度和位置信息並輸出；
步驟2，在姿態解算期間，通過加速度計和陀螺儀的輸出信息判斷當前時刻是否處於零速時刻，若滿足加速度計或陀螺儀判定條件，則對應的標誌位將被置位，若在一段時間內對應的標誌位連續有效，則將當前時刻視為零速時刻，將速度置零的同時，利用加速度計信息修正姿態角。
本發明的具體實施方式如下：
1、分析人體運動時的步態以及不同運動模態下的慣性傳感器輸出
人體運動過程中，雙腳交替運動，以其中一隻為例，足部落地階段可分為四個階段：抬腳、跨步、落地、支撐。將慣性傳感器件置於人體足部，通過分析慣性傳感器輸出信息可得，如圖2所示，足部完全與地面接觸的時間約為0.2-0.4秒之間，這個時間段內應對其進行零速修正。
人體在跑步過程中，可能會出現雙腳同時離地的情況，此時無法進行零速修正。分析跑步模態下的慣性傳感器輸出信息，如圖3所示，加速度計和陀螺儀的信息變化更劇烈，周期變化較短。但是，跑步模態下一隻腳落地支撐階段的零速時刻與正常行走時是類似的，足部完全與地面接觸的時間變短，但這時仍能夠進行零速修正。
人體在上下樓梯上足部的運動與正常行走時類似，分析該模態下的慣性傳感器輸出信息，如圖4所示，可看出加速度計和陀螺儀的信息變化較為緩慢，周期變短，為了保持平衡，人體在足部落地階段需要停留更長的時間。這一階段也可利用零速進行修正。
2、建立基於人體運動學模型輔助的零速修正判別模型
基於人體運動學分析，根據加速度計和陀螺儀的輸出特性設置零速判定條件，如圖5所示，由於運動過程中imu慣性傳感器件的輸出與靜止時刻imu慣性傳感器件的輸出在數值上有較大的差異，為了顯示的方便，將運動過程中imu慣性傳感器件的輸出置為零，即輸出為零的時間段是人體的運動過程，而輸出不為零的時間段為算法判定為零速的時刻。可知單條件判斷可能存在誤判情況，導致零速判斷不準確，而聯合判斷則具有較高的準確性。因此，在行人導航定位方法中，我們使用加速度計和陀螺儀聯合判斷條件來判斷當前時刻是否是零速時刻。
3、建立基於地磁輔助捷聯慣導解算航向角誤差模型
地球是一個大磁體，地球本身以及周圍空間存在的磁場叫「地球磁場」，簡稱「地磁場」，其主要部分是一個偶極場。地面任何一點的磁場的磁感應強度矢量具有一定的大小和方向。如圖6所示，由於地磁的南北極與地理的南北極不完全重合，存在磁偏角。磁偏角即是當地磁場強度矢量的水平投影與正北方向之間的夾角，即磁子午線與地理子午線之間的夾角。
磁傳感器通過測量地球磁場的三維投影來確定載體的航向信息，為了減少外界幹擾對磁強計的精度影響，因此事先要對其進行標定。目前最常用的就是最小二乘橢圓擬合法，該方法在校準過程具有較高的抗突變幹擾能力，可在不增加成本的情況下實現高精度的對準。
4、建立基於氣壓高度計輔助捷聯慣導解算高度誤差模型
如圖7所示為純慣性高度通道模型框架，垂直加速度計測得比力後，經過誤差補償，兩次積分得到高度信息。由於捷聯慣導高度通道誤差較大且發散較快，需要利用外部信息進行高度方向誤差修正以滿足所需精度。如圖8所示為利用大氣傳感器進行高度輔助的二階氣壓阻尼算法模型框架，大氣傳感器利用大氣壓隨高度升高按指數律遞減的關係推算所處位置的海拔高度。其中，選取參數k1＝1.414,k2＝1。
圖10給出了採用本行人導航定位設備進行定位的二維效果圖，圖9是本發明行人實際行走路線圖，測試的地點為學院樓的走廊，測試人員將該設備固定於足部，按照一定的路線開始運動，無論是方形路線還是直線路線，運動軌跡與實際路線基本吻合。圖11為採用本行人導航定位設備進行定位的的三維效果圖，測試地點為學院樓，行走路線為學院走廊l形→下樓→走廊l形→上樓，形成閉合曲線，高度方向未發散，高度方向變化與樓層高度基本吻合，導航效果較好。由試驗可知，該行人導航定位設備滿足複雜情況下行人導航定位精度，適用於工程實踐當中。
本發明方案所公開的技術手段不僅限於上述技術手段所公開的技術手段，還包括由以上技術特徵任意組合所組成的技術方案。
以上述依據本發明的理想實施例為啟示，通過上述的說明內容，相關工作人員完全能夠在不偏離本項發明技術思想的範圍內，進行多樣的變更以及修改。本項發明的技術性範圍並不局限於說明書上的內容，必須要根據權利要求範圍來確定其技術性範圍。