本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，特別是涉及一種移動(dòng)機(jī)器人航向角估計(jì)方法。

背景技術(shù)：

移動(dòng)機(jī)器人是一個(gè)集環(huán)境感知、動(dòng)態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)。隨著機(jī)器人性能不斷地完善，移動(dòng)機(jī)器人的應(yīng)用范圍大為擴(kuò)展，不僅在工業(yè)、農(nóng)業(yè)、國(guó)防、醫(yī)療、服務(wù)等行業(yè)中得到廣泛的應(yīng)用，而且在排雷、搜捕、救援、輻射和空間領(lǐng)域等有害與危險(xiǎn)場(chǎng)合得到很好的應(yīng)用。因此，移動(dòng)機(jī)器人技術(shù)已經(jīng)得到世界各國(guó)的普遍關(guān)注。

移動(dòng)機(jī)器人的航向角的精確檢測(cè)是其實(shí)現(xiàn)路線規(guī)劃、導(dǎo)航與避障的前提。傳統(tǒng)的方法存在以下問(wèn)題：1)基于陀螺儀或者編碼器的的航向角估計(jì)方法存在累計(jì)誤差，隨著時(shí)間的推移誤差變得越來(lái)越大；2)基于磁力計(jì)的航向角檢測(cè)方法雖然沒(méi)有累計(jì)誤差，但是其精度會(huì)受到環(huán)境中的鐵磁物質(zhì)的影響，因此存在很大的不確定性。近年來(lái)，一部分研究將陀螺儀與磁力計(jì)進(jìn)行數(shù)據(jù)融合，利用兩種傳感器的互補(bǔ)特性可以得到更為可靠的航向角估計(jì)。但是，他們的共同缺點(diǎn)是沒(méi)有考慮傳感器噪聲的變化與磁力計(jì)失效情況。所以，有必要提出一種新的方法來(lái)提升航向角估計(jì)的精度。

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

為了克服陀螺儀、磁力計(jì)兩種傳感器的不足，同時(shí)考慮傳感器噪聲的變化與磁力計(jì)失效情況，本發(fā)明提出了一種移動(dòng)機(jī)器人航向角估計(jì)方法，包括以下步驟：

(1)利用陀螺儀檢測(cè)移動(dòng)機(jī)器人的航向角速度，利用磁力計(jì)檢測(cè)移動(dòng)機(jī)器人的航向角；

(2)利用狀態(tài)增廣法建立如下的航向角狀態(tài)空間模型

其中，θt為航向角，bg,t為陀螺儀漂移誤差，為增廣航向角向量，t為采樣間隔，為系統(tǒng)轉(zhuǎn)移矩陣，為輸入矩陣，為陀螺儀檢測(cè)的航向角速度，為系統(tǒng)噪聲矩陣，ng,t為陀螺儀內(nèi)在噪聲，wg,t為陀螺儀漂移噪聲，為增廣系統(tǒng)噪聲向量，θm,t為磁力計(jì)檢測(cè)的航向角，為觀測(cè)矩陣，vm,t為磁力計(jì)內(nèi)在噪聲，εm,t為鐵磁物質(zhì)引起的突變?cè)肼?，下?biāo)t表示該變量位于第t個(gè)采樣點(diǎn)，噪聲ng,t、wg,t與vm,t都為高斯白噪聲；

(3)利用抗差自校正卡爾曼濾波進(jìn)行數(shù)據(jù)融合以獲取航向角的最優(yōu)估計(jì)，步驟如下：

(3.1)狀態(tài)預(yù)測(cè)估計(jì)：使用上一時(shí)刻的最優(yōu)估計(jì)來(lái)預(yù)測(cè)下一狀態(tài)：

其中，是在采樣點(diǎn)t-1時(shí)的最優(yōu)估計(jì)，是預(yù)測(cè)估計(jì)，相應(yīng)的預(yù)測(cè)估計(jì)誤差協(xié)方差其中pt-1是最優(yōu)估計(jì)誤差協(xié)方差，是方差的估計(jì)；

(3.2)粗大誤差識(shí)別：磁力計(jì)讀數(shù)被認(rèn)定為粗大誤差，如果滿足以下條件

其中，為新息序列，是vm,t方差的估計(jì)；

(3.3)狀態(tài)最優(yōu)估計(jì)：如果θm,t不是粗大誤差，則卡爾曼增益kt為

因此可得到最優(yōu)估計(jì)

其相應(yīng)的最優(yōu)估計(jì)誤差協(xié)方差為如果θm,t是粗大誤差，則

kt＝[00]′

(3.4)噪聲方差估計(jì)：計(jì)算三種相關(guān)函數(shù)與如下

其中，n為窗口長(zhǎng)度系數(shù)，與分別為對(duì)應(yīng)到與的最新的n個(gè)可用磁力計(jì)讀數(shù)的采樣點(diǎn)序號(hào)，為二次后向差分磁力計(jì)讀數(shù)序列，然后可以得到噪聲方差估計(jì)

本發(fā)明與現(xiàn)有技術(shù)相比，提供了一種更為精確且可靠的航向角估計(jì)方法，其優(yōu)點(diǎn)在于：1)陀螺儀的漂移誤差能夠被較好的補(bǔ)償，同時(shí)利用磁力計(jì)消除了航向角估計(jì)的累計(jì)誤差；2)濾波器系數(shù)隨著傳感器噪聲的變化而變化，進(jìn)一步提升了估計(jì)精度；3)錯(cuò)誤的磁力計(jì)讀數(shù)能被識(shí)別并隔離，進(jìn)一步提升了估計(jì)可靠性。

附圖說(shuō)明

圖1為本發(fā)明流程圖

圖2為本發(fā)明實(shí)驗(yàn)結(jié)果圖

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖與具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。

如圖1所示，本發(fā)明的具體實(shí)施步驟如下：

(1)利用陀螺儀檢測(cè)移動(dòng)機(jī)器人的航向角速度，利用磁力計(jì)檢測(cè)移動(dòng)機(jī)器人的偏航角；

(2)利用狀態(tài)增廣法建立如下的航向角狀態(tài)空間模型

其中，θt為航向角，bg,t為陀螺儀漂移誤差，為增廣航向角向量，t為采樣間隔，為系統(tǒng)轉(zhuǎn)移矩陣，為輸入矩陣，為陀螺儀檢測(cè)的航向角速度，為系統(tǒng)噪聲矩陣，ng,t為陀螺儀內(nèi)在噪聲，wg,t為陀螺儀漂移噪聲，為增廣系統(tǒng)噪聲向量，θm,t為磁力計(jì)檢測(cè)的航向角，為觀測(cè)矩陣，vm,t為磁力計(jì)內(nèi)在噪聲，εm,t為鐵磁物質(zhì)引起的突變?cè)肼?。下?biāo)t表示該變量位于第t個(gè)采樣點(diǎn)，噪聲ng,t、wg,t與vm,t都為高斯白噪聲。

(3)利用抗差自校正卡爾曼濾波進(jìn)行數(shù)據(jù)融合以獲取航向角的最優(yōu)估計(jì)，步驟如下：

(3.1)狀態(tài)預(yù)測(cè)估計(jì)：使用上一時(shí)刻的最優(yōu)估計(jì)來(lái)預(yù)測(cè)下一狀態(tài)：

其中，是在采樣點(diǎn)t-1時(shí)的最優(yōu)估計(jì)，是預(yù)測(cè)估計(jì)。相應(yīng)的預(yù)測(cè)估計(jì)誤差協(xié)方差其中pt-1是最優(yōu)估計(jì)誤差協(xié)方差，是方差的估計(jì)；

(3.2)粗大誤差識(shí)別：磁力計(jì)讀數(shù)被認(rèn)定為粗大誤差，如果滿足以下條件

其中，為新息序列，是vm,t方差的估計(jì)；

(3.3)狀態(tài)最優(yōu)估計(jì)：如果θm,t不是粗大誤差，則卡爾曼增益kt為

因此可得到最優(yōu)估計(jì)

其相應(yīng)的最優(yōu)估計(jì)誤差協(xié)方差為如果θm,t是粗大誤差，則

kt＝[00]′

(3.4)噪聲方差估計(jì)：θm,t的差分序列包括陀螺儀和磁力計(jì)的噪聲，因此噪聲的統(tǒng)計(jì)特性可以通過(guò)差分序列的統(tǒng)計(jì)分析得出?？紤]以下式子：

θm,t+1＝θt+1+vm,t+1+εm,t+1

θm,t＝θt+vm,t+εm,t

上式相減可得

從而可得

上式中包含陀螺儀和磁力計(jì)的誤差項(xiàng)。為了消除bg,t，引入后向差分算子得到：

假設(shè)在所有采樣點(diǎn)εm,t＝0，也就是說(shuō)不存在鐵磁干擾，將會(huì)是三個(gè)獨(dú)立的滑動(dòng)平均過(guò)程wg,t-1，ng,t-ng,t-1，vm,t+1-2vm,t+vm,t-1之和，因此，也是一個(gè)滑動(dòng)平均過(guò)程，它的階數(shù)等于三個(gè)中較高階的階數(shù)。

求取上式兩邊隨機(jī)過(guò)程的相關(guān)函數(shù)可得

其中，表示數(shù)學(xué)期望。使用有限記憶方法，計(jì)算在采樣點(diǎn)t時(shí)三種相關(guān)函數(shù)與如下

但是如果存在鐵磁干擾，假設(shè)在采樣點(diǎn)集合時(shí)磁力計(jì)的讀數(shù)是不可用的，則對(duì)和來(lái)說(shuō)，可用磁力計(jì)讀數(shù)的采樣點(diǎn)集合為：

進(jìn)而，在采樣點(diǎn)t時(shí)三種相關(guān)函數(shù)與可寫(xiě)為

其中，n為窗口長(zhǎng)度系數(shù)，與分別為對(duì)應(yīng)到與的最新的n個(gè)可用磁力計(jì)讀數(shù)的采樣點(diǎn)序號(hào)，為二次后向差分磁力計(jì)讀數(shù)序列，然后可以得到噪聲方差估計(jì)

采用本專利方法需要注意如下問(wèn)題：首先，準(zhǔn)確的噪聲方差初始值和需要提前獲取，因?yàn)檫@些初始值對(duì)粗大誤差識(shí)別很關(guān)鍵。其次，噪聲方差應(yīng)當(dāng)是隨時(shí)間緩慢變化的，可以保證在一段較長(zhǎng)時(shí)間里的和的計(jì)算是準(zhǔn)確的。最后，εm,t在多數(shù)情況下應(yīng)該是等于0的，否則將沒(méi)有足夠可用的磁力計(jì)讀數(shù)來(lái)校正基于陀螺儀的航向角估計(jì)結(jié)果。

使用輪式移動(dòng)機(jī)器人對(duì)本發(fā)明實(shí)施例進(jìn)行測(cè)試，時(shí)間間隔為0.2秒，其結(jié)果如圖2所示。首先，由于在濾波過(guò)程中增加了噪聲方差估計(jì)，所以濾波器的參數(shù)會(huì)隨著噪聲方差的變化而變化，從而保證了濾波精度；其次，隨著鐵磁物質(zhì)的出現(xiàn)，磁力計(jì)讀數(shù)發(fā)生很大的偏差，傳統(tǒng)方法如卡爾曼濾波則會(huì)跟蹤錯(cuò)誤的磁力計(jì)讀數(shù)，使濾波結(jié)果變得極不可靠，而本專利方法中提出的抗差自校正卡爾曼濾波能夠隔離錯(cuò)誤的磁力計(jì)讀數(shù)，雖然濾波誤差會(huì)在短期內(nèi)增大，但是當(dāng)磁力計(jì)讀數(shù)恢復(fù)正常時(shí)，濾波誤差能夠迅速變小，證明本專利方法有著較強(qiáng)的可靠性。

提供以上實(shí)施例僅僅是為了描述本發(fā)明的目的，而并非要限制本發(fā)明的范圍。本發(fā)明的范圍由所附權(quán)利要求限定。不脫離本發(fā)明的精神和原理而做出的各種等同替換和修改，均應(yīng)涵蓋在本發(fā)明的范圍之內(nèi)。