本發(fā)明涉及導航技術,尤指一種導航的方法和終端。
背景技術:
隨著移動通信技術的不斷演進,智能終端越來越多的深入到社會的各個層面,智能終端與人們的購物、娛樂、出行等活動聯(lián)系越來越緊密,其中,智能終端可以為用戶出行時提供路線導航,借助全球定位系統(tǒng)(Global Positioning System,GPS)以及無線網絡,在室外的情況下,往往可以輕松的獲取到位置信息,實現(xiàn)導航等功能。
但是目前應用在智能終端上的GPS或輔助全球定位系統(tǒng)(Assisted GPS,A-GPS)還存在以下的問題:在室內例如建筑物內的情況下,由于GPS系統(tǒng)無法搜索到衛(wèi)星信號,所以在室內的情況下并不能為用戶導航,例如在以下場景中:用戶駕車去某大型商場購物,待購完物后返回車庫,卻忘記了停車的具體位置,這時,由于在GPS或A-GPS系統(tǒng)下終端無法搜索到衛(wèi)星信號來為用戶進行導航,那么,面對偌大的商場地下停車庫,用戶要找到自己的車往往需要花費很大的精力。
技術實現(xiàn)要素:
為了解決上述技術問題,本發(fā)明提供了一種導航的方法和終端,能夠在無法利用衛(wèi)星信號導航的情況下為用戶進行導航,為用戶帶來極大的便利。
為了達到本發(fā)明目的,本發(fā)明提供了一種導航的方法,包括:
當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
根據(jù)所述終端的運動軌跡以及所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。
進一步的,
所述第一位置與所述第二位置之間的路線數(shù)據(jù)通過以下步驟得到:
所述終端從所述第二位置移動到所述第一位置時,執(zhí)行所述獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
執(zhí)行所述根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
執(zhí)行所述根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
將所述終端從所述第二位置移動到所述第一位置的運動軌跡作為所述第一位置與所述第二位置之間的路線數(shù)據(jù)并保存。
進一步的,所述第一位置與所述第二位置之間的路線數(shù)據(jù)是預先存儲到所述終端中的。
進一步的,所述根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡,包括:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除處理,根據(jù)經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌 跡。
進一步的,在所述獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù)之前或之后,所述方法還包括:
獲取所述終端采集的高度數(shù)據(jù);
相應的,在所述根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡之后,所述方法還包括:
根據(jù)所述終端的高度數(shù)據(jù)對所述終端的運動軌跡進行校正。
本發(fā)明提供一種終端,包括:
獲取單元,用于當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
轉換單元,用于根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
確定單元,用于根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
導航單元,用于根據(jù)所述終端的運動軌跡所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。
進一步的,
所述獲取單元,還用于所述終端從所述第二位置移動到所述第一位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
所述轉換單元,還用于根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
所述確定單元,還用于根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
相應的,所述終端還包括:
處理單元,用于將所述終端從所述第二位置移動到所述第一位置的運動軌跡作為所述第一位置與所述第二位置之間的路線數(shù)據(jù)并保存。
進一步的,所述第一位置與所述第二位置之間的路線數(shù)據(jù)是預先存儲到所述終端中的。
進一步的,所述確定單元具體用于:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除處理,根據(jù)經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡。
進一步的,所述獲取單元還用于:獲取所述終端采集的高度數(shù)據(jù);
相應的,所述確定單元具體用于:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除掉,通過分析經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡;
根據(jù)所述終端的高度數(shù)據(jù)對所述終端的運動軌跡進行校正。
本發(fā)明實施例提供的一種導航的方法和終端,當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采 集的運動數(shù)據(jù);根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;根據(jù)所述終端的運動軌跡以及所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。通過本發(fā)明的技術方案,能夠在無法利用衛(wèi)星信號導航的情況下為用戶進行導航,為用戶帶來極大的便利。
本發(fā)明的其它特征和優(yōu)點將在隨后的說明書中闡述,并且,部分地從說明書中變得顯而易見,或者通過實施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點可通過在說明書、權利要求書以及附圖中所特別指出的結構來實現(xiàn)和獲得。
附圖說明
附圖用來提供對本發(fā)明技術方案的進一步理解,并且構成說明書的一部分,與本申請的實施例一起用于解釋本發(fā)明的技術方案,并不構成對本發(fā)明技術方案的限制。
圖1為本發(fā)明實施例提供的一種導航的方法的流程示意圖;
圖2為本發(fā)明實施例中的世界坐標系的示意圖;
圖3為本發(fā)明實施例中的終端坐標系的示意圖;
圖4為本發(fā)明實施例中的MOD函數(shù)的角度映射示意圖;
圖5為本發(fā)明實施例中HDE算法的反饋控制圖;
圖6為本發(fā)明實施例中卡爾曼濾波算法的流程示意圖;
圖7為本發(fā)明實施例中提供的一種終端的結構示意圖。
具體實施方式
為使本發(fā)明的目的、技術方案和優(yōu)點更加清楚明白,下文中將結合附圖對本發(fā)明的實施例進行詳細說明。需要說明的是,在不沖突的情況下,本申請中的實施例及實施例中的特征可以相互任意組合。
在附圖的流程圖示出的步驟可以在諸如一組計算機可執(zhí)行指令的計算機系統(tǒng)中執(zhí)行。并且,雖然在流程圖中示出了邏輯順序,但是在某些情況下,可以以不同于此處的順序執(zhí)行所示出或描述的步驟。
本發(fā)明實施例提供一種導航的方法,基于終端,如圖1所示,該方法包括:
步驟101、當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
步驟102、根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
步驟103、根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
步驟104、根據(jù)所述終端的運動軌跡以及所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。
進一步的,所述第一位置與所述第二位置之間的路線數(shù)據(jù)通過以下步驟得到:
所述終端從所述第二位置移動到所述第一位置時,執(zhí)行所述獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
執(zhí)行所述根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
執(zhí)行所述根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
將所述終端從所述第二位置移動到所述第一位置的運動軌跡作為所述第一位置與所述第二位置之間的路線數(shù)據(jù)并保存。
進一步的,所述第一位置與所述第二位置之間的路線數(shù)據(jù)是預先存儲到所述終端中的。
示例性的,可以是由路測人員攜帶測試儀器對所述第一位置與所述第二位置之間的路線進行具體測量后獲得路線數(shù)據(jù)后存儲到所述終端中。
進一步的,所述根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡,包括:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除處理,根據(jù)經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡。
進一步的,在所述獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù)之前或之后,所述方法還可以包括:
獲取所述終端采集的高度數(shù)據(jù);
相應的,在所述根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡之后,所述方法還包括:
根據(jù)所述終端的高度數(shù)據(jù)對所述終端的運動軌跡進行校正。
本發(fā)明實施例提供的一種導航的方法,當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;根據(jù)所述終端的運動軌跡以及所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。通過本發(fā)明的技術方案,能夠在無法利用衛(wèi)星信號導航的情況下為用戶進行導航,為用戶帶來極大的便利。
為了使本領域技術人員能夠更清楚地理解本發(fā)明提供的技術方案,下面對本發(fā)明提供的技術方案中的一些具體步驟進行詳細說明:
上述實施例中:獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù),該步驟可以具體為:
首先需要進行坐標系的變換:
在導航系統(tǒng)中,由于終端的陀螺儀所采集的數(shù)據(jù)是基于自身坐標系的,所以不能直接應用,必須經過一定的變換后才能使用,即將終端坐標系轉換為世界坐標系。下面首先簡要的介紹世界坐標系以及終端坐標系。
世界坐標系,也稱為全局坐標系,它是一個右手坐標系,該坐標系是恒定不變的,以地球為參考系,具體見圖2,世界坐標系的x軸沿球面水平向東,y軸沿球面水平向北,z軸垂直球面指向天空。
終端坐標系根據(jù)終端屏幕的方向進行定義,如圖3所示,x軸平行終端屏幕向右,y軸平行終端屏幕向左,z軸垂直于終端屏幕指向終端屏幕的朝向,終端坐標系不會因為終端位置的改變而改變。
坐標系的轉換需要三次旋轉,第一次是關于z軸的逆時針旋轉,轉動角度為ψ,令世界坐標系為(E1,E2,E3)繞z軸旋轉后的坐標為(a1,a2,a3)。旋轉的公式為:
第二次是關于y軸的逆時針旋轉,轉動角度為θ,令繞y軸旋轉后的坐標為(b1,b2,b3)。旋轉的公式為:
第三次是關于x軸的逆時針旋轉,轉動角度為ф,令繞y軸旋轉后的坐標為(e1,e2,e3)。旋轉的公式為:
(e1,e2,e3)即為從終端坐標系轉換到世界坐標系下的值,三次旋轉的 公式為:
至此,通過以上過程可以完成終端采集的基于所述終端坐標系的數(shù)據(jù)轉換為基于世界坐標系的數(shù)據(jù)。
上述實施例中:根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡,可以具體為如下內容:
本發(fā)明實施例中確定終端運動軌跡的算法的核心主要包含兩步,第一步計算終端(實際上是指該終端的使用用戶)的步頻,第二步計算終端(實際上是指終端的使用用戶)的步長,獲取到終端運動的步頻以及步長數(shù)據(jù)之后即可得到終端的使用用戶移動的距離數(shù)據(jù)進而可以確定終端的運動軌跡:
步驟一:計算用戶的步頻,這個步驟的基本原理為:用戶的行走過程即為一個加速以及減速的過程,所以從加速度計獲取的數(shù)據(jù)剔除掉重力加速度后,判斷加速度的零點即可判斷用戶運動的開始以及結束。
具體步驟如下:
其中,air為第i個采樣點的總加速度,aix、aiy、aiz分別為加速度計在x y z三個軸上面的加速度值。
計算靜止狀態(tài)下的重力加速度:其中,M為采樣點數(shù),具體由終端的傳感器而定,在探測重力加速度過程中,該終端需保持靜止。
計算去除掉重力加速度后的加速度值:其中C為探測范圍。
具體的,整個計算過程的具體流程如下:
1設置探測參數(shù)并開始;
2初始化探測范圍;
3獲取加速度數(shù)據(jù);
4獲取當?shù)刂亓Γ?/p>
5剔除重力加速度;
6更新探測范圍;
7探測到峰值,尋找跨步結束點:
8探測到跨步結束點,計算該步統(tǒng)計值。
步驟二:計算用戶的步長;
其中,S為步長,k為常數(shù)。
通過以上的算法,系統(tǒng)即可探測出用戶所移動的距離,進而確定終端的運動軌跡。
優(yōu)選的,在本發(fā)明提供的技術方案中,獲取到高度數(shù)據(jù)可以對終端的運動軌跡進一步校正優(yōu)化。在終端中,一般都設置有壓力傳感器,壓力傳感器可以充當氣壓計用于測量海拔高度變化。
在地球大氣層的底層,大氣壓隨著海拔高度上升而降低。我們將在59℉(華氏溫度)時的29.92in.Hg(英寸汞柱)海平面氣壓規(guī)定為標準大氣壓,這個平均值不受時間影響,而受到測量點的地理位置、氣溫和氣流的影響。因此,我們必須了解不同高度的大氣壓。
1個標準大氣壓=14.7psi=76cm.Hg=29.92in.Hg=1.01325bar=1013.25mbar;其中,psi–磅/平方英寸;cm.Hg–厘米汞柱;in.Hg–英寸汞柱;Pa–帕,國際制壓力單位,1Pa=1N/m2;bar–巴,氣壓單位,1bar= 105Pa;mbar–毫巴,1mbar=10-3bar。
可以用下面的公式表示大氣壓與海拔高度之間關系:
其中,P0是標準大氣壓,等于1013.25mbar;Altitude是以米為單位的海拔高度,P是在某一高度的以mbar為單位的氣壓。
根據(jù)上述公式,當高度從海平面上升到海拔11000米高時,大氣壓從1013.25mbar降到230mbar,當高度低于1500米時,大氣壓幾乎呈線性降低,每100米大約降低11.2mbar,即每10米大約降低1.1mbar。為了取得更精確的高度測量數(shù)據(jù),可以在終端中存儲一個大氣壓高度查詢表,根據(jù)壓力傳感器的測量結果,確定對應的海拔高度。
需要說明的是,在本發(fā)明提供的技術方案中,確定終端的運動軌跡時還需要對終端的運動方向做出準確的判斷,具體的,下面詳細說明上述實施例中所述根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向的過程:
本發(fā)明的技術方案中方向判斷主要基于終端中的陀螺儀而實現(xiàn),根據(jù)陀螺儀的工作原理可知,對陀螺儀的瞬時角速度進行不斷積分即可得到終端的轉向角,但是室內環(huán)境受到各種各樣的干擾,例如磁場的干擾,同時受制于陀螺儀本身的精度以及轉向不足效應的影響,終端中內置的陀螺儀中直接獲取的數(shù)據(jù)并不足以用來進行方向判斷。
為了將方向估計誤差限制在一定范圍內,J.Borenstein等人提出了啟發(fā)式消除算法(Heuristic Drift Elimination,HDE)來校正陀螺儀的數(shù)據(jù),HDE算法不僅能減低由于陀螺儀造成的方向誤差,甚至能夠完全消除誤差,HDE算法在幾秒鐘內便能達到穩(wěn)定狀態(tài),使方向誤差基本為零,HDE算法的基本原理基于這樣一個事實情況:通常,建筑物的走廊(墻壁)都是直線,并且和其他的走廊(墻壁)保持垂直或者平行,但是同時也就限制了HDE算法的使用條件,即終端的使用者所走的路徑之間必須為垂直或者是相互平行,目前大部分的建筑物均可達到這一要求,在滿足條件的建筑物中運動時,HDE算法能將由陀螺儀計算的方向逐漸向最靠近的主方向靠近,不斷矯正方向,消除陀螺儀偏差,當終端的使用者轉彎的時候,HDE算法停止矯正,當HDE 算法停止校正時候,陀螺儀的偏差會造成新的偏差。
假設終端的使用者行走的路徑有四個主方向ψ,并且相鄰的主方向之間的夾角為Δ=90°,當用戶將終端豎直放在褲兜中時,將陀螺儀按照終端Y軸轉過的角度校正為終端使用者行進方向,令ε表示陀螺儀偏移,則陀螺儀角速度計算公式可表示為:ωraw=ωtrue+ωd;其中ωraw表示直接從陀螺儀讀出的原始角速度,ωtrue表示實際但是未知的角速度,ωd表示陀螺儀動態(tài)偏移,不可知且緩慢變化。
在詳細描述HDE算法之前,需要定義一個非常重要的子函數(shù)MOD(n,Δ)。MOD(n,Δ)是一個求模函數(shù),它的作用是求兩個數(shù)n和Δ的余數(shù),它的具體計算公式如下:
如圖4所示,說明了MOD的角度映射,MOD的作用是將任意大小的角度映射到(0,Δ)的區(qū)間內,(0,Δ)的區(qū)間又均分為兩個子空間R和L,當△=90°時,(0,Δ)和坐標系的第一象限重合,MOD(n,90°)將任意角度n(大于360°或者為負數(shù)等)映射到R和L區(qū)間,MOD的重要性在于,在有四個主方向的ψ=90° 180° 270° 360°時,凡是在所有主方向左側的任意角度θ都可映射到區(qū)間L,凡是在所有主方向右側的任意角度θ都可映射到區(qū)間R,在主方向右側是指在ψ和ψ-△/2的角度,在主方向左側是指在ψ和ψ+△/2的角度,MOD函數(shù)使得沿任意主方向行走都能變?yōu)檠?°角行走。
HDE算法是一個反饋控制系統(tǒng),如圖5所示為HDE算法的反饋過程,其步驟如下:
1、當終端的使用者走直線路徑時,ωtrue=0,此時陀螺儀的輸出為ωd,ωd與二元控制器I-Controller的輸出Ii(初始情況下Ii=0),相加得到ωd+Ii,之后ωd+Ii被送往數(shù)值積分器,積分結果為行進方向的相對變化角ψi;
2、在第一次迭代之后,反饋控制系統(tǒng)形成回路,并將ψi做z-1處理得到ψi-1,并送給MOD函數(shù),z-1表示延遲一個采樣間隔,公式ψ*i=MOD(ψi-1,Δ)表 示ψi-1經MOD函數(shù)處理之后得到映射到(0,△)的ψ*i;
3、將ψ*i和固定值фset=△/2比較,得到誤差信號
4、Ei會被送到二元控制器I-Controller,I-Controller僅根據(jù)Ei的符號調節(jié)輸出。當Ei的符號為正時,(ψi-1偏向主方向右側),積分器Ii增加固定的小增量ic;當Ei符號為負時,(ψi-1偏向主方向左側,積分器Ii減少ic,下面公式說明了二元控制器的控制過程;
需要說明的是,HDE算法在直線上可以很好的消除陀螺儀的偏移誤差,但是在轉彎過程中HDE算法會停止,在這個過程中誤差有可能累積,所以,本發(fā)明提供的技術方案中,在HDE算法的基礎上使用卡爾曼濾波算法,對曲線路徑以及轉彎處的陀螺儀偏移誤差進行校正,卡爾曼濾波的基本思想如下:
利用上一時刻的估計,再加上實時得到的量測來進行實時估計。卡爾曼濾波是一種遞推線性最小方差估計,因為上一時刻的估計是利用上一時刻和以前的量測得到的,所以這種遞推的實時估計就是利用所有的量測數(shù)據(jù)得到的;其次,卡爾曼濾波把被估計量作為系統(tǒng)的狀態(tài),用系統(tǒng)狀態(tài)方程來描述狀態(tài)的轉移過程,因此,各時刻之間的狀態(tài)相關函數(shù),就可以根據(jù)狀態(tài)方程的轉移特性來描述,解決非平穩(wěn)隨機過程估計的困難??柭鼮V波的最優(yōu)準則與線性最小方差估計一樣,每一時刻的估計都使估計均方誤差最小。由于采用了狀態(tài)方程,進行卡爾曼濾波的系統(tǒng)必須是線性的;要用卡爾曼濾波來估計非線性系統(tǒng)的狀態(tài),必須對系統(tǒng)進行線性化。
卡爾曼濾波算法:
設隨機線性離散系統(tǒng)的方程(先不考慮控制作用)為:
Z(k)=HX(k)+V(k)
上式中,X(k)是系統(tǒng)的n維狀態(tài)向量,Z(k)是系統(tǒng)的m維觀測序列,W(k) 是p維系統(tǒng)過程噪聲序列,V(k)是m維觀測噪聲序列,ф是系統(tǒng)的n×n維狀態(tài)轉移矩陣,Γ是n×p維噪聲輸入矩陣,H是m×n維觀測矩陣。
關于系統(tǒng)過程噪聲和觀測噪聲的統(tǒng)計特性,我們假定如下:
其中,Q(k)是系統(tǒng)過程噪聲W(k)的p×p維對稱非負定方差矩陣,R(k)是系統(tǒng)觀測噪聲V(k)的m×m維對稱正定方差陣,而δkj是Kronecker-δ函數(shù)。
上述離散系統(tǒng)卡爾曼濾波的計算流程為:
計算狀態(tài)一步預測:
狀態(tài)估計:
濾波增益矩陣:K(k)=P(k|k-1)=ΦP(k-1)HT[HP(k|k-1)HT+R(k)]-1; (3)
一步預測誤差方差陣:P(k|k-1)=ΦP(k-1)ΦT+ΓQ(k-1)ΓT; (4)
估計誤差方差陣:P(k)=[I-K(k)H]P(k|k-1); (5)
以上五個公式即為隨機線性離散系統(tǒng)卡爾曼濾波基本方程。只要給定初值,根據(jù)k時刻的觀測值Z(k),就可以遞推計算得k時刻的狀態(tài)估計值。在一個濾波周期內,從卡爾曼濾波在使用信息和觀測信息的先后順序來看,卡爾曼濾波具有兩個明顯的信息更新過程:時間更新過程和觀測更新過程。公式(3)說明了根據(jù)k-1時刻的狀態(tài)估計預測k時刻狀態(tài)的方法,公式(4)對這種預測的質量做了定量描述。公式(3)和公式(4)的計算中僅使用了與系統(tǒng)的動態(tài)特性有關的信息,如狀態(tài)一步轉移矩陣、噪聲輸入陣、過程噪聲方差陣。從時間的推移過程來看,該兩式將時間從k-1時刻推進至k時刻,描述了卡爾曼濾波的時間更新過程。公式(2)、公式(3)及公式(5)用來計算對時間更新值的修正量,該修正量由時間更新的質量優(yōu)劣P(k|k-1)、觀測信息的質量優(yōu)劣(R(k))、觀測與狀態(tài)的關系(H)以及具體的觀測信息Z(k)所確定, 所有這些方程圍繞一個目的,即正確、合理地利用觀測Z(k),所以這一過程描述了卡爾曼濾波器的觀測更新過程。
本發(fā)明的技術方案中的卡爾曼濾波算法可用方框圖表示,如圖6所示,從圖中可以明顯看出卡爾曼濾波算法具有兩個計算回路:增益計算回路和濾波計算回路。其中增益計算回路是獨立計算的,濾波計算回路依賴于增益計算回路。
判斷終端的移動方式為直線或拐彎的算法如下:
1、初始情況下,陀螺儀沿Y軸轉動的角度設置為終端使用者當前的行走方向,對一次腳步的開始點以及結束點的陀螺儀Y軸讀數(shù)進行積分得到增量角△ψi;
2、在直線路徑可以直接使用HDE算法,在曲線路徑上使用卡爾曼濾波器處理陀螺儀獲取的數(shù)據(jù);
下面的公式中給出了如何判斷直線曲線。其中Th是一個常量閾值,ψi-1和ψi-2是當前腳步i的前兩步i-1和i-2的估計方向,T=0表示沒有拐彎,T=1表示左轉,T=-1表示右轉。
3、在直線上,HDE算法的補償公式如下:
其中,Ii表示HDE在第i步的補償值,c是一個很小的常量增量,用于方向誤差的補償,n是在一個探測腳步內的采樣數(shù)。
4、在轉彎以及曲線過程中,使用卡爾曼濾波器處理陀螺儀獲取的數(shù)據(jù),通過對角速度進行積分,便能得到轉向的角度值。
5、最后,該終端的使用者的行進方向由如下公式來表示:
ψi=Δψi+ψi-1+P
其中,為直線路徑上每一個探測腳步的I值的平均值。
綜上所述,本領域技術人員可以根據(jù)上述內容實現(xiàn)本發(fā)明的技術方案。
本發(fā)明實施例提供一種終端10,如圖7所示,該終端10包括:
獲取單元11,用于當終端需要從第一位置移動到第二位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
轉換單元12,用于根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
確定單元13,用于根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
導航單元14,用于根據(jù)所述終端的運動軌跡所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。
進一步的,
所述獲取單元11,還用于所述終端從所述第二位置移動到所述第一位置時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);
所述轉換單元12,還用于根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);
所述確定單元14,還用于根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;
相應的,所述終端10還可以包括:
處理單元15,用于將所述終端從所述第二位置移動到所述第一位置的運動軌跡作為所述第一位置與所述第二位置之間的路線數(shù)據(jù)并保存。
進一步的,所述第一位置與所述第二位置之間的路線數(shù)據(jù)是預先存儲到 所述終端中的。
進一步的,所述確定單元14具體用于:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除處理,根據(jù)經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡。
進一步的,所述獲取單元11還用于:獲取所述終端采集的高度數(shù)據(jù);
相應的,所述確定單元12具體用于:
將所述轉換后的運動數(shù)據(jù)中加速度數(shù)據(jù)中重力方向的數(shù)據(jù)剔除掉,通過分析經過剔除處理后得到的加速度數(shù)據(jù)的零值計算所述終端運動的頻率;
根據(jù)所述經過剔除處理后得到的加速度數(shù)據(jù)中水平方向的數(shù)據(jù)計算獲得所述終端運動的步長;
根據(jù)所述頻率以及所述步長計算所述終端的移動距離數(shù)據(jù):
根據(jù)所述轉換后的運動數(shù)據(jù)中的角速度數(shù)據(jù)以及方向矯正算法確定所述終端的運動方向;
根據(jù)所述移動距離數(shù)據(jù)以及所述終端的運動方向確定所述終端的運動軌跡;
根據(jù)所述終端的高度數(shù)據(jù)對所述終端的運動軌跡進行校正。
本實施例用于實現(xiàn)上述各方法實施例,本實施例中各個單元的工作流程和工作原理參見上述各方法實施例中的描述,在此不再贅述。
本發(fā)明實施例提供的一種終端,當終端需要從第一位置移動到第二位置 時,獲取所述終端運動時所述終端基于所述終端自身坐標系采集的運動數(shù)據(jù);根據(jù)預設轉換算法將所述運動數(shù)據(jù)轉換為基于世界坐標系的運動數(shù)據(jù);根據(jù)所述轉換后的運動數(shù)據(jù)以及預設算法確定所述終端的運動軌跡;根據(jù)所述終端的運動軌跡以及所述第一位置與所述第二位置之間的路線數(shù)據(jù)引導所述終端從所述第一位置移動到所述第二位置。通過本發(fā)明的技術方案,能夠在無法利用衛(wèi)星信號導航的情況下為用戶進行導航,為用戶帶來極大的便利。
雖然本發(fā)明所揭露的實施方式如上,但所述的內容僅為便于理解本發(fā)明而采用的實施方式,并非用以限定本發(fā)明。任何本發(fā)明所屬領域內的技術人員,在不脫離本發(fā)明所揭露的精神和范圍的前提下,可以在實施的形式及細節(jié)上進行任何的修改與變化,但本發(fā)明的專利保護范圍,仍須以所附的權利要求書所界定的范圍為準。