非線性回歸機(jī)器人運(yùn)動參數(shù)估計(jì)方法*

鄭 杰, 張 華, 肖宇峰

(西南科技大學(xué) 信息工程學(xué)院,四川 綿陽 621000)

0 引 言

機(jī)器人運(yùn)動參數(shù)估計(jì)是機(jī)器人研究中尤為關(guān)鍵的問題，對于機(jī)器人的導(dǎo)航、定位、路徑規(guī)劃以及目標(biāo)跟蹤等具有十分重要的意義。為解決此問題，研究人員提出了多種方案：文獻(xiàn)[1～6]的方法均存在累積誤差，因而不適用于全局運(yùn)動參數(shù)估計(jì)。文獻(xiàn)[7，8]基于信標(biāo)的方法雖然適用于全局位姿估計(jì)，但該方法要求在環(huán)境中放置信標(biāo)，運(yùn)用場景受到限制，尤其在室外環(huán)境。目前，較為可行的方案是通過安裝激光雷達(dá)估計(jì)機(jī)器人[9，10]的運(yùn)動參數(shù)方法精度高，且不需要對環(huán)境做任何改動，而成為該領(lǐng)域的研究熱點(diǎn)。文獻(xiàn)[11]提出角度直方圖(angle histogram)算法將三維搜索問題轉(zhuǎn)換成3個一維搜索問題，大大降低了計(jì)算量，但算法要求環(huán)境具有明顯的線段特征，且精度受到直方圖分辨率的影響。文獻(xiàn)[12]提出的迭代最近點(diǎn)(iterative closest point，ICP)算法精度和魯棒性均高于基于特征的對應(yīng)方法。但激光測量數(shù)據(jù)多，導(dǎo)致ICP算法的計(jì)算量大，收斂速度慢。此外，ICP算法與其他基于對應(yīng)的方法一樣具有局部最小值問題。文獻(xiàn)[13]在ICP算法的基礎(chǔ)上，提出了IDC(iterative dual correspondence)算法。其優(yōu)勢在于能夠處理任意角度的旋轉(zhuǎn)且收斂速度比ICP快。但在實(shí)際應(yīng)用中，IDC算法的計(jì)算量仍然比較大[10]。

本文提出一種非線性回歸的機(jī)器人運(yùn)動參數(shù)估計(jì)方法,通過實(shí)驗(yàn)驗(yàn)證本文算法能夠?qū)崟r(shí)準(zhǔn)確地對機(jī)器人運(yùn)動參數(shù)進(jìn)行估計(jì)，并與ICP算法進(jìn)行了比較。

1 運(yùn)動模型分析

系統(tǒng)坐標(biāo)系建立如圖1所示，假設(shè)OX0Y0為世界坐標(biāo)系，OXY為機(jī)器人坐標(biāo)系，OXLYL為激光雷達(dá)坐標(biāo)系。本文機(jī)器人采用兩輪驅(qū)動模型，形式圖1。

(1)

(2)

(3)

結(jié)合線速度矢量在相關(guān)坐標(biāo)系統(tǒng)上的投影，可將式(3)表示為

(4)

2 運(yùn)動參數(shù)估計(jì)模型轉(zhuǎn)換

將上述微分公式的差分項(xiàng)dx和dy代入相對運(yùn)動方程(3)中[14]，得到公式

(5)

3 運(yùn)動參數(shù)估計(jì)

Aiω+Biv+Di=0

(6)

但在實(shí)際測量過程中，由于激光雷達(dá)所反饋的數(shù)據(jù)含有噪聲，并且該噪聲的特性的計(jì)算非常困難，因此，在這里假設(shè)測量結(jié)果中出現(xiàn)的噪聲為符合高斯分布的隨機(jī)噪聲序列ξ，則測量公式表示為

(7)

zi=HiXi+di+ξi

(8)

(9)

其中,卡爾曼增益為Ki=ATPiHT(HPiHT+R)-1。進(jìn)而進(jìn)行進(jìn)一步的誤差估計(jì)。

4 特殊情況分析

1)機(jī)器人沿著平坦的墻壁移動

2)機(jī)器人在圓形地形中旋轉(zhuǎn)

讓機(jī)器人圍繞自身坐標(biāo)系的原點(diǎn)旋轉(zhuǎn)，而機(jī)器人四周的環(huán)境是一個圓柱體，其軸線垂直于OXY平面，并穿過機(jī)器人坐標(biāo)系原點(diǎn)。

在這種情況下，運(yùn)動方程(4)具有形式f(x,y,t)=x2+y2-R2=0，經(jīng)過計(jì)算，可以得到關(guān)于ω,v公式：0·ω+2yv=0。與前一種情況相似，將得到方程的解為v=0,ω是任何實(shí)數(shù)。因此，這種情況下無法確定角速度。

3)機(jī)器人向平坦的墻壁上移動

(10)

5 實(shí)驗(yàn)結(jié)果與分析

在實(shí)驗(yàn)室走廊搭建實(shí)驗(yàn)環(huán)境。以TurtleBot2為實(shí)驗(yàn)平臺(如圖2(a)所示)搭載Hokuyo URG—04LX激光雷達(dá)，獲得實(shí)驗(yàn)環(huán)境的激光雷達(dá)數(shù)據(jù)并進(jìn)行建模。實(shí)驗(yàn)中，機(jī)器人的移動環(huán)境是一個封閉的矩形空間，如圖2(b)顯示了RVIZ中激光雷達(dá)的一幀圖像。

圖2 實(shí)驗(yàn)平臺實(shí)物圖及激光雷達(dá)數(shù)據(jù)

機(jī)器人的運(yùn)動由平移和旋轉(zhuǎn)組成，根據(jù)上述的運(yùn)動參數(shù)估計(jì)方法，對獲得的激光雷達(dá)數(shù)據(jù)進(jìn)行處理，最終得到機(jī)器人的運(yùn)動參數(shù)。圖3為上述方法估計(jì)的線速度和角速度。

圖3 運(yùn)動參數(shù)估計(jì)值

圖3表明：線速度和角速度的估計(jì)值快速收斂，但在函數(shù)f(x,y)的拐點(diǎn)處獲得的數(shù)據(jù)具有不連續(xù)性，這是由于在拐點(diǎn)處函數(shù)偏導(dǎo)數(shù)不存在所致。通過對激光雷達(dá)的掃描點(diǎn)設(shè)置閾值，以去除這些點(diǎn)的影響，可以極大程度地提高估計(jì)的收斂速度及準(zhǔn)確度。

圖4中將運(yùn)動參數(shù)估計(jì)方法所估計(jì)的值與運(yùn)動固有參數(shù)進(jìn)行對比。從圖中可看出，相比于原始數(shù)據(jù)，所提出方法的估計(jì)值具有較高的收斂精度，且收斂時(shí)間短。

圖4 參數(shù)對比

圖5是在不同運(yùn)動情況下，通過本文算法估計(jì)后的機(jī)器人運(yùn)動位置坐標(biāo)與ICP算法結(jié)果的對比。從圖中可以看到：該算法與ICP算法具有相似的精度。

圖5 機(jī)器人運(yùn)動的坐標(biāo)

通過將機(jī)器人運(yùn)動位姿情況與地形數(shù)據(jù)整合，得到實(shí)際機(jī)器人運(yùn)動軌跡如圖6所示。從圖中可以看出：在不同的地形環(huán)境以及不同的運(yùn)動情況下，該算法都能夠平滑地估計(jì)出機(jī)器人的運(yùn)動參數(shù)。

圖6 獲得的機(jī)器人的地形和運(yùn)動軌跡

6 結(jié)束語

本文通過建立與分析線速度和角速度估計(jì)的數(shù)學(xué)模型，轉(zhuǎn)換參數(shù)估計(jì)求解模型，并利用遞推最小二乘的方法與卡爾曼濾波來求解參數(shù)與消除誤差。本文方法精度高、運(yùn)算速率快。延遲時(shí)間為對一個點(diǎn)多次掃描的積累所花費(fèi)的時(shí)間，但由于現(xiàn)代測距儀允許獲得高頻(例如75 Hz)的掃描，與獲得幾次掃描相關(guān)的延遲將是可忽略的，因此，能夠?qū)崟r(shí)估計(jì)機(jī)器人運(yùn)動參數(shù)。最后將本文方法在實(shí)際的機(jī)器人系統(tǒng)中進(jìn)行實(shí)驗(yàn)驗(yàn)證，結(jié)果驗(yàn)證了所提出方法的有效性。