本發(fā)明屬于慣性導(dǎo)航領(lǐng)域，更具體地，涉及一種行人定位與軌跡跟蹤方法和系統(tǒng)。

背景技術(shù)：

隨著科學(xué)技術(shù)的快速發(fā)展和城市化的快速推進(jìn)，定位技術(shù)在人們生活中變得越發(fā)重要，人們對定位產(chǎn)品、功能和服務(wù)的需求加大，從而催生了龐大的市場。傳統(tǒng)定位技術(shù)基本上都是基于gps、無線網(wǎng)絡(luò)(wifi、zigbee、uwb等)，但在無gps信號的室內(nèi)，以及沒有無線網(wǎng)絡(luò)的地方，這種定位方式不能使用。因此基于慣性導(dǎo)航傳感器的行人定位與軌跡跟蹤技術(shù)逐漸興起，其最大的特點(diǎn)是在無基礎(chǔ)固聯(lián)設(shè)施的已知或未知的環(huán)境下能夠?qū)崿F(xiàn)自主導(dǎo)航和實時定位。常規(guī)的基于慣性導(dǎo)航傳感器的行人定位裝置通常僅采用加速度計和陀螺儀，然后配合姿態(tài)解算以及零速檢測算法，進(jìn)行三維軌跡推算與定位。但是這種方案，由于陀螺儀的漂移，導(dǎo)致航向角最終會發(fā)散，在長時間導(dǎo)航定位情況下，定位會失敗。同時，常規(guī)的基于慣性導(dǎo)航傳感器的定位方案，有時還會采用對行人步長估計的方式進(jìn)行定位，這種方案依賴特定人員的平均步長，因而對人員的適應(yīng)性較差，僅適用于特定人員的定位場景，大大降低了基于慣性導(dǎo)航傳感器的行人定位裝置的應(yīng)用范圍。

由此可見，現(xiàn)有技術(shù)存在無法長時間導(dǎo)航、適應(yīng)性較差和應(yīng)用范圍窄的技術(shù)問題。

技術(shù)實現(xiàn)要素：

針對現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種行人定位與軌跡跟蹤方法和系統(tǒng)，由此解決現(xiàn)有技術(shù)存在無法長時間導(dǎo)航、適應(yīng)性較差和應(yīng)用范圍窄的技術(shù)問題。

為實現(xiàn)上述目的，按照本發(fā)明的一個方面，提供了一種行人定位與軌跡跟蹤方法，包括：

(1)接收行人的加速度信息、角速度信息，對加速度信息和角速度信息進(jìn)行濾波和閾值分割，得到速度狀態(tài)信息，速度非0時利用角速度信息得到姿態(tài)信息，速度為0時利用加速度信息修正角速度信息得到姿態(tài)信息；

(2)接收磁力計信息，對磁力計進(jìn)行橢圓校正后，再利用姿態(tài)信息進(jìn)行傾角補(bǔ)償，得到磁力航向角；

(3)將磁力航向角和姿態(tài)信息中的航向角融合，得到融合航向角，結(jié)合姿態(tài)信息和融合航向角，對加速度積分得速度信息，速度信息經(jīng)過零速校正后，再次積分得到水平方向上的位移和垂直方向的高度，最后通過位移和高度進(jìn)行行人定位與軌跡跟蹤。

進(jìn)一步的，步驟(1)包括：

(1-1)對加速度信息和角速度信息進(jìn)行濾波和閾值分割，得到速度狀態(tài)信息；

(1-2)速度非0時利用角速度信息更新四元數(shù)，利用四元數(shù)得到姿態(tài)信息；速度為0時，采用比例積分，利用加速度信息修正角速度信息的漂移，采用修正后角速度信息更新四元數(shù)，進(jìn)而得到姿態(tài)信息。

進(jìn)一步的，融合航向角為：

其中，0≤i≤n，ci為i時刻的磁力航向角，yi為i時刻的姿態(tài)信息中的航向角，hi為i時刻的融合航向角，ei為i時刻的航向角偏移，kp為比例系數(shù)，ki為積分系數(shù)。

進(jìn)一步的，步驟(3)包括：

(3-1)將磁力航向角和姿態(tài)信息中的航向角融合，得到融合航向角；

(3-2)速度信息經(jīng)過零速校正后，水平方向上的位移可由水平方向速度直接積分得出，垂直方向上的速度需進(jìn)一步通過上、下樓判斷，進(jìn)行高度補(bǔ)償后，得到垂直方向的高度；

(3-3)利用位移和高度進(jìn)行行人定位與軌跡跟蹤。

按照本發(fā)明的另一方面，提供了一種行人定位與軌跡跟蹤系統(tǒng)，包括：加速度計、陀螺儀、磁力計和控制模塊，

加速度計用于接收行人的加速度信息，傳輸至控制模塊；

陀螺儀用于接收行人的角速度信息，傳輸至控制模塊；

所述磁力計用于接收行人的磁力計信息，傳輸至控制模塊；

所述控制模塊用于利用加速度信息、角速度信息得到速度狀態(tài)信息與姿態(tài)信息，利用磁力計信息得到磁力航向角，將磁力航向角和姿態(tài)信息中的航向角融合，得到融合航向角，利用位移和高度進(jìn)行行人定位與軌跡跟蹤。

進(jìn)一步的，控制模塊包括步態(tài)子模塊、融合子模塊和導(dǎo)航子模塊。

進(jìn)一步的，步態(tài)子模塊用于對加速度信息和角速度信息進(jìn)行濾波和閾值分割，得到速度狀態(tài)信息，通過對加速度積分得到速度信息，速度信息經(jīng)過零速校正后，水平方向上的位移可由水平方向速度直接積分得出，垂直方向上的速度需進(jìn)一步通過上、下樓判斷，進(jìn)行高度補(bǔ)償后，得到垂直方向的高度。

進(jìn)一步的，融合子模塊用于在行人速度非0時利用角速度信息得到姿態(tài)信息，行人速度為0時利用加速度信息修正角速度信息進(jìn)而得到姿態(tài)信息，對磁力計進(jìn)行橢圓校正后，利用姿態(tài)信息對其進(jìn)行傾角補(bǔ)償?shù)玫酱帕较蚪牵瑢⒋帕较蚪呛妥藨B(tài)信息中的航向角融合，得到融合航向角。

進(jìn)一步的，導(dǎo)航子模塊用于利用位移和高度進(jìn)行行人定位與軌跡跟蹤。

總體而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，能夠取得下列有益效果：

(1)速度非0時，加速度信息受外力的影響，并不能較準(zhǔn)確的反應(yīng)姿態(tài)信息，此時利用加速度信息和角速度信息獲取姿態(tài)信息的話，將降低姿態(tài)信息的解算精度；速度為0時，加速度信息可以較準(zhǔn)確的反應(yīng)姿態(tài)信息，此時用加速度信息來修正角速度信息的偏移，能夠有效地提高姿態(tài)信息的解算精度，本發(fā)明速度非0時利用角速度信息得到姿態(tài)信息，速度為0時利用加速度信息修正角速度信息進(jìn)而得到姿態(tài)信息，本發(fā)明的姿態(tài)信息精度高。

(2)本發(fā)明將磁力航向角和姿態(tài)信息中的航向角融合，得到的融合航向角，是特征級的融合，由于磁力計信息容易受干擾，若在數(shù)據(jù)層，將磁力計信息與加速度信息、角速度信息一起融合計算姿態(tài)角，易將磁力計信息的誤差擴(kuò)散至俯仰角與橫滾角，從而降低姿態(tài)信息的精度，而采取本發(fā)明特征級融合方案后，在不降低俯仰角與橫滾角精度的前提下，能夠有效地用磁力計信息來改善融合航向角的漂移，從而進(jìn)一步提高融合航向角的精度。

(3)本發(fā)明不依賴于特定人員的步行平均步長，算法計算量小，可滿足大部分應(yīng)用的要求，本發(fā)明可以長時間導(dǎo)航、適應(yīng)性較強(qiáng)且應(yīng)用范圍寬。

(4)優(yōu)選的，速度信息經(jīng)過零速校正后，水平方向上的位移可由水平方向速度直接積分得出，垂直方向上的速度需進(jìn)一步通過上、下樓判斷，再融合高度補(bǔ)償，以補(bǔ)償高度的漂移，這樣在保證二維定位精度的情況下提升三維定位的精度。

附圖說明

圖1是本發(fā)明實施例提供的一種行人定位與軌跡跟蹤方法的流程圖；

圖2是本發(fā)明實施例1提供的一種行人定位與軌跡跟蹤系統(tǒng)的結(jié)構(gòu)圖；

圖3是本發(fā)明實施例1提供的一種行人定位與軌跡跟蹤系統(tǒng)的結(jié)構(gòu)設(shè)計和拓?fù)潢P(guān)系；

圖4是本發(fā)明實施例1提供的姿態(tài)解算算法流程圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實施例，對本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個實施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。

如圖1所示，一種行人定位與軌跡跟蹤方法，包括：

(1)接收行人的加速度信息、角速度信息，對加速度信息和角速度信息進(jìn)行濾波和閾值分割，得到速度狀態(tài)信息，速度非0時利用角速度信息得到姿態(tài)信息，速度為0時利用加速度信息修正角速度信息得到姿態(tài)信息；

(2)接收磁力計信息，對磁力計進(jìn)行橢圓校正后，再利用姿態(tài)信息進(jìn)行傾角補(bǔ)償，得到磁力航向角；

(3)將磁力航向角和姿態(tài)信息中的航向角融合，得到融合航向角，結(jié)合姿態(tài)信息和融合航向角，對加速度積分得速度信息，速度信息經(jīng)過零速校正后，再次積分得到水平方向上的位移和垂直方向的高度，最后通過位移和高度進(jìn)行行人定位與軌跡跟蹤。

進(jìn)一步的，步驟(1)包括：

(1-1)對加速度信息和角速度信息進(jìn)行濾波和閾值分割，得到速度狀態(tài)信息；

(1-2)速度非0時利用角速度信息更新四元數(shù)，利用四元數(shù)得到姿態(tài)信息；速度為0時，采用比例積分，利用加速度信息修正角速度信息的漂移，采用修正后角速度信息更新四元數(shù)，進(jìn)而得到姿態(tài)信息。

進(jìn)一步的，融合航向角為：

其中，0≤i≤n，ci為i時刻的磁力航向角，yi為i時刻的姿態(tài)信息中的航向角，hi為i時刻的融合航向角，ei為i時刻的航向角偏移，kp為比例系數(shù)，ki為積分系數(shù)，t1與t2分別為比例限幅幅值與積分限幅幅值。

進(jìn)一步的，步驟(3)包括：

(3-1)將磁力航向角和姿態(tài)航向角融合，得到融合航向角；

(3-2)速度信息經(jīng)過零速校正后，水平方向上的位移可由水平方向速度直接積分得出，垂直方向上的速度需進(jìn)一步通過上、下樓判斷，進(jìn)行高度補(bǔ)償后，得到垂直方向的高度；

(3-3)利用位移和高度進(jìn)行行人定位與軌跡跟蹤。

實施例1

如圖2所示，本發(fā)明實施例1提供一種行人定位與軌跡跟蹤系統(tǒng)的結(jié)構(gòu)示意圖，其主要由電源、加速度計、陀螺儀、磁力計、控制模塊以及藍(lán)牙組成，其連接方式是：電源與加速度計、陀螺儀、磁力計、控制模塊以及藍(lán)牙連接，控制模塊與加速度計、陀螺儀、磁力計以及藍(lán)牙雙向連接。

如圖3所示，本發(fā)明實施例1提供的一種行人定位與軌跡跟蹤系統(tǒng)的結(jié)構(gòu)設(shè)計和拓?fù)潢P(guān)系：優(yōu)選的，磁力計為三軸磁力計，加速度計為三軸加速度計，陀螺儀為三軸陀螺儀，三軸磁力計經(jīng)磁力計校正后，計算磁力航向角；三軸加速度計與三軸陀螺儀，通過ahrs姿態(tài)解算，不僅解算出姿態(tài)角還解算出世界坐標(biāo)系下的行人步行的加速度；姿態(tài)角包括航向角、俯仰角與橫滾角，姿態(tài)解算出的航向角與磁力航向角做融合后，更新世界坐標(biāo)系下的行人步行加速度；利用三軸陀螺儀與世界坐標(biāo)系下的行人步行加速度，通過步態(tài)提取，對行人步行狀態(tài)進(jìn)行分割，分割結(jié)果反饋至ahrs姿態(tài)解算以及用于零速校正；經(jīng)過零速校正后，水平方向上的位移可由水平方向速度直接積分得出，垂直方向上的速度需進(jìn)一步通過上、下樓判斷，進(jìn)行高度補(bǔ)償后，再計算垂直方向的高度。

如圖4所示，姿態(tài)解算采取四元數(shù)法進(jìn)行計算，首先初始化四元數(shù)q，當(dāng)步態(tài)判斷為非零速時，采用陀螺儀更新四元數(shù)；而當(dāng)步態(tài)判斷為零速時，采用pi(比例積分)算法，利用加速度計修正陀螺儀的漂移，并采用修正后的陀螺儀更新四元數(shù)，最后通過四元數(shù)解算三個姿態(tài)角：姿態(tài)航向角y、滾轉(zhuǎn)角r和俯仰角p，利用磁力計計算磁力航向角c，而航向角融合分為實時融合和定時融合兩個階段。實時融合階段使用磁力計來實時修正航向角，定時融合階段定期采用融合后的航向角更新四元數(shù)，以使陀螺儀的積分漂移定期清零。

本發(fā)明與現(xiàn)有技術(shù)相比，無需基礎(chǔ)固聯(lián)設(shè)施，沒有外界的輔助設(shè)備，采用全自主式定位方式，能夠方便地用于行人定位與軌跡跟蹤；而在相同的硬件方案中，采用改進(jìn)的算法結(jié)構(gòu)，不依賴于特定人員的步行平均步長，算法計算量小，可滿足大部分應(yīng)用的要求。

采用在特征級融合航向角的算法，在有效地改善航向角漂移的同時，并沒有影響俯仰角以及橫滾角的解算精度，并利用零速判斷的結(jié)果反饋至ahrs姿態(tài)解算，避免加速度在受外力干擾的情況下，依然用于修正陀螺儀的零漂，從而進(jìn)一步提高了姿態(tài)角的解算精度。

采用零速校正方式，不僅對水平速度做了二次平滑估計，對于其垂直方向利用步行速度特征，做了可變參的二次平滑估計，進(jìn)一步的提高了本裝置的三維定位精度。

本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。