本發(fā)明屬于機器人技術(shù)領(lǐng)域，特別涉及一種機器人定位方法。

背景技術(shù)：

機器人定位主要是指機器人通過外部傳感器獲取相關(guān)信息經(jīng)過處理來確定自身及目標位姿的過程。現(xiàn)有的技術(shù)方案有里程計法、慣性導航法、衛(wèi)星導航法、磁導航法等。其中，里程計法(odo)是使用最為廣泛的定位方法。在移動機器人車輪上裝有光電編碼器，通過對車輪轉(zhuǎn)動的記錄來實現(xiàn)對機器人的位姿跟蹤。但此方法定位為航位推算法，是一個累加過程，其測量誤差與車輛行駛路程長度有關(guān)，隨著移動距離的增加，定位精度逐漸下降。慣性導航法利用慣性器件(加速度計和陀螺儀)來測量機器人本身的加速度以及角速度，并結(jié)合給定的初始條件，經(jīng)過積分、解算得到每一時刻機器人的速度、位置、姿態(tài)等參數(shù)。但導航定位誤差會隨著時間積累，精度逐漸下降，所以慣性導航法不適合長時間的精確定位。衛(wèi)星導航通過在機器人上安裝gnss移動站接收機，在另一已知點上安裝gnss基站，移動站gnss接收基站gnss的差分改正數(shù)據(jù)，為機器人提供高精度的位置、速度等信息來完成導航。但其具有局限性，即在衛(wèi)星信號受到遮擋或干擾時無法提供有效的定位信息，不能滿足導航具備完全自主性的要求。磁導航通過磁傳感器檢測磁場變化，從而進行定位導航。但此方法可變性和可維護性差，易受環(huán)境的影響。

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

本發(fā)明的目的是：提供一種結(jié)合gnss和odo兩傳感器各自優(yōu)點，揚長避短，基于gnss/odo的組合定位方法，避免gnss定位信息由于遮擋或信號失鎖時無法實現(xiàn)定位的問題，同時避免了odo定位導致的累積誤差問題，提高定位的可靠性。

本發(fā)明的技術(shù)方案是：一種基于gnss/odo的機器人雙輪差速定位方法，包括以下步驟：

a.初始化機器人，利用gnss獲取初始時刻機器人的全局位姿信息；

設(shè)當前時刻為n，該時刻機器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示機器人在全局坐標系中的平面坐標位置，θg(n)表示機器人的航向角；

b.依據(jù)n時刻機器人的全局位姿信息，根據(jù)公式(1.1)中雙輪里程計該歷元里的里程變化對機器人n+1時刻的全局位姿信息進行預測；

其中：l1、l2分別表示雙輪里程計測量到的機器人左、右輪每歷元的里程變化量，k1、k2分別為機器人左、右輪編碼器矯正系數(shù)；l為機器人兩輪中心輪距；

dθg為機器人在相鄰兩時刻轉(zhuǎn)過的角度，(dxl,dyl)為機器人在本體坐標系下平面位置：

c.判斷當前時刻是否為設(shè)定的gnss數(shù)據(jù)修正時刻，如果是，則執(zhí)行d步驟；如果不是，則執(zhí)行e步驟；

d.根據(jù)步驟b中預測得到的n+1時刻的全局位姿信息，進行卡爾曼濾波“預測-修正”，卡爾曼組合濾波器設(shè)計如下：

系統(tǒng)狀態(tài)變量和系統(tǒng)方程分別如公式(1.3)和(1.4)所示，

xn＝[δxδyδθδk1δk2δψ]^t(1.3)

xn+1＝φxn+wn(1.4)

式中：[δx,δy,δθ]為步驟b中雙輪里程計的位置和航向誤差；δk1、δk2為左、右兩輪標定因子誤差；δψ為安裝誤差；系統(tǒng)噪聲為wt～n(0,q)；系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣φ為：

其中，

f13＝-[dxl(n)·sinθg(n)+dyl(n)·cosθg(n)]

f14＝l1/2·[l1·k1·cosθg(n)/l-sinθg(n)]

f15＝-l2/2·[l2·k2·cosθg(n)/l+sinθg(n)]

f23＝dxl·cosθg(n)-dyl·sinθg(n)

f24＝l1/2·[l1·k1·sinθg(n)/l+cosθg(n)]

f25＝-l2/2·[l2·k2·sinθg(n)/l-cosθg(n)]

f34＝-l1/l

f35＝l2/l

以雙輪里程計差速解算的位置信息[xg(n+1),yg(n+1)]與gnss差分定位輸出的位置信息[xgnss(n+1),ygnss(n+1)]作為卡爾曼濾波器的量測信息，此時的量測方程為：

式中，觀測方程ε(t)為系統(tǒng)觀測噪聲矢量，為n(0,r)的高斯白噪聲過程；并轉(zhuǎn)到f步驟；

e.僅進行d步驟中的卡爾曼濾波預測，而不做修正，并轉(zhuǎn)到f步驟；

f.輸出濾波后的機器人的全局位姿信息；

i.令n＝n+1，轉(zhuǎn)到步驟b，進行下一循環(huán)。

有益效果：利用本發(fā)明，機器人的定位、定向精度有顯著提高，彌補了單一導航系統(tǒng)的缺陷，為機器人的自主導航提供了保障，相比于高精度傳感器，大大減少了機器人成本，加快了建圖效率，使得機器人能夠更廣泛使用。

附圖說明

圖1為本發(fā)明中所用坐標與參數(shù)示意圖；

圖2為本發(fā)明gnss/odo組合定位的原理圖；

圖3為本發(fā)明步驟流程圖。

具體實施方式

下面結(jié)合附圖并舉實施例，對本發(fā)明進行詳細描述。

參見附圖，一種基于gnss/odo的機器人雙輪差速定位方法，包括以下步驟：

a.初始化機器人，利用gnss獲取初始時刻機器人的位姿信息；

設(shè)當前時刻為n，該時刻機器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示機器人在全局坐標系中的平面坐標位置，θg(n)表示機器人的航向角；令l1、l2分別表示雙輪里程計測量到的機器人左、右輪每歷元的里程變化量，則機器人在相鄰兩時刻轉(zhuǎn)過的角度dθg和機器人本體坐標系下平面位置(dxl,dyl)變化分別為

其中，k1、k2分別為機器人左、右輪編碼器矯正系數(shù)(編碼器安裝在里程計上，用于記錄每輪的脈沖數(shù)，然后計算每輪行走的距離)；l為機器人兩輪中心輪距；

b.利用步驟a中g(shù)nss獲得的n時刻機器人全局位姿信息，根據(jù)公式(1.2)中雙輪里程計該歷元里的里程變化對機器人n+1時刻的全局位姿信息進行預測；

c.判斷當前時刻是否為設(shè)定的gnss數(shù)據(jù)修正時刻，如果是，則執(zhí)行d步驟；如果不是，則執(zhí)行e步驟；

d.根據(jù)步驟b中預測得到的n+1時刻的全局位姿信息，進行卡爾曼濾波“預測-修正”，卡爾曼組合濾波器設(shè)計如下：

系統(tǒng)狀態(tài)變量和系統(tǒng)方程分別如公式(1.3)和(1.4)所示，

xn＝[δxδyδθδk1δk2δψ]^t(1.3)

xn+1＝φxn+wn(1.4)

式中：[δx,δy,δθ]為步驟b中雙輪里程計的位置和航向誤差；δk1、δk2為左、右兩輪標定因子誤差；δψ為安裝誤差；系統(tǒng)噪聲為wt～n(0,q)；系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣φ為：

其中，

f13＝-[dxl(n)·sinθg(n)+dyl(n)·cosθg(n)]

f14＝l1/2·[l1·k1·cosθg(n)/l-sinθg(n)]

f15＝-l2/2·[l2·k2·cosθg(n)/l+sinθg(n)]

f23＝dxl·cosθg(n)-dyl·sinθg(n)

f24＝l1/2·[l1·k1·sinθg(n)/l+cosθg(n)]

f25＝-l2/2·[l2·k2·sinθg(n)/l-cosθg(n)]

f34＝-l1/l

f35＝l2/l

以雙輪里程計差速解算的位置信息[xg(n+1),yg(n+1)]與gnss差分定位輸出的位置信息[xgnss(n+1),ygnss(n+1)]作為卡爾曼濾波器的量測信息，此時的量測方程為：

式中，觀測方程ε(t)為系統(tǒng)觀測噪聲矢量，為n(0,r)的高斯白噪聲過程；并轉(zhuǎn)到f步驟；

e.僅進行d步驟中的卡爾曼濾波預測，而不做修正，并轉(zhuǎn)到f步驟；

f.輸出n+1時刻機器人的位姿信息；

i.轉(zhuǎn)到步驟b，進行下一循環(huán)。