劉曉東,卜曉珊,吳增眾

(南水北調(diào)中線信息科技有限責(zé)任公司,北京 100070)

0 引言

目前的油缸清洗方法沒有相應(yīng)的輔助標(biāo)志,難以達(dá)到高效的清洗效果,因此,提出了液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人。機(jī)器人的定位是當(dāng)前導(dǎo)航領(lǐng)域的一個(gè)主要研究領(lǐng)域,其需要在移動(dòng)中準(zhǔn)確地感知自身的位置和姿態(tài)[1]。相關(guān)學(xué)者研究了機(jī)器人位姿跟蹤,文獻(xiàn)[2]提出了基于滾動(dòng)時(shí)域優(yōu)化的位姿跟蹤方法,該方法結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)模型,采用PD控制方式實(shí)現(xiàn)了機(jī)器人跟蹤參考位姿,并構(gòu)建了閉環(huán)誤差模型。采用滾動(dòng)時(shí)域跟蹤方法實(shí)時(shí)跟蹤機(jī)器人位姿誤差,并在一個(gè)固定時(shí)域窗口內(nèi)利用誤差參數(shù)迭代處理位姿誤差,結(jié)合參考位姿實(shí)現(xiàn)對(duì)機(jī)器人位姿的實(shí)時(shí)跟蹤;文獻(xiàn)[3]提出了卡爾曼濾波框架下的機(jī)器人位姿跟蹤方法,該方法利用MC處理非高斯噪聲,構(gòu)造基于MC的代價(jià)函數(shù),并使用麥夸爾特法優(yōu)化代價(jià)函數(shù),推導(dǎo)協(xié)方差迭代更新過程,實(shí)現(xiàn)機(jī)器人位姿跟蹤。然而,上述這兩種方法過分依賴機(jī)器人工作環(huán)境,在無相應(yīng)輔助標(biāo)志物條件下,難以實(shí)現(xiàn)自主作業(yè)。為此,提出了基于自控測(cè)距的液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人位姿跟蹤技術(shù)。

1 清潔機(jī)器人位姿跟蹤裝置布局

要充分考慮機(jī)器人在工作過程中位姿不斷變化[4-6],需要使用激光跟蹤儀跟蹤定位位姿。激光跟蹤儀的基本原理是采用球面坐標(biāo)法,在實(shí)際跟蹤過程中,激光跟蹤儀的位置直接影響到跟蹤器與各測(cè)量點(diǎn)之間的相互關(guān)系,從而影響其最終跟蹤準(zhǔn)確性。不合理的布置會(huì)導(dǎo)致測(cè)量誤差增大,甚至導(dǎo)致高精度的激光跟蹤系統(tǒng)失去參考價(jià)值。在機(jī)器人測(cè)量范圍已被確定的情況下,調(diào)節(jié)激光跟蹤儀布置位置,減小其布置位置的不合理所帶來的影響,以達(dá)到更好定位精度,提高機(jī)器人跟蹤精度?；谠撛?激光跟蹤坐標(biāo)系下跟蹤點(diǎn)的坐標(biāo)可表示為:

(1)

式中:α為俯仰角;θ為水平角;r為激光測(cè)量半徑。該坐標(biāo)系下跟蹤誤差可表示為:

(2)

式中φ表示誤差參量。

激光跟蹤器是一種利用激光干涉儀測(cè)量長度的方法,它可以直接追溯到激光的波長。因此,激光跟蹤器的長度測(cè)量比角度測(cè)量的精度要高得多。由于激光跟蹤器測(cè)點(diǎn)的真實(shí)位置坐標(biāo)與其本身的坐標(biāo)系統(tǒng)相關(guān),而采用該方法選取的采集點(diǎn)坐標(biāo)系為機(jī)器人基座坐標(biāo)系,因此,在激光跟蹤器和機(jī)器人基座坐標(biāo)系之間建立對(duì)應(yīng)關(guān)系。由此得到的清潔機(jī)器人位姿跟蹤裝置布局,如圖1所示。

圖1 位姿跟蹤裝置布局

從圖1可以看出,為了最大限度地減少跟蹤誤差,激光跟蹤儀應(yīng)盡可能地設(shè)置在最優(yōu)配置和最靠近基座的位置,減少激光跟蹤儀的分布位置對(duì)測(cè)量精度的影響。

2 清潔機(jī)器人位姿跟蹤方法研究

2.1 基于自控測(cè)距法的機(jī)器人位姿跟蹤

在機(jī)器人位姿跟蹤方法中[7-8],為了解決無相應(yīng)幅值標(biāo)志物條件下的非系統(tǒng)誤差問題,使用自控測(cè)距法跟蹤位姿。根據(jù)機(jī)器人在清洗過程中各個(gè)傳感器的誤差特征,采用一種融合加速度計(jì)的自控測(cè)距方法[9]。該方法根據(jù)測(cè)量的實(shí)際位置和加速度計(jì)測(cè)量的位置對(duì)比結(jié)果跟蹤與調(diào)整位姿,可表示為

|xoa-xob>x0

(3)

式中:xoa表示測(cè)量的位置;xob表示加速度計(jì)測(cè)量的位置;x0表示閾值。為克服運(yùn)動(dòng)目標(biāo)對(duì)姿態(tài)估計(jì)的影響,采用激光測(cè)距儀測(cè)量清潔機(jī)器人位置,機(jī)器人在動(dòng)態(tài)障礙物中的運(yùn)行示意圖如圖2所示。

圖2 機(jī)器人在動(dòng)態(tài)障礙物中的運(yùn)行示意圖

根據(jù)運(yùn)行示意圖可以獲取機(jī)器人大概運(yùn)行的軌跡,設(shè)待運(yùn)動(dòng)特征點(diǎn)的坐標(biāo)為(ai,bi),則對(duì)每一個(gè)特征點(diǎn)可做出如下判斷:

(4)

式中L表示誤差感知范圍邊長。由于機(jī)器人的姿態(tài)跟蹤存在著一定的感知范圍,為了避免在距離較近的背景點(diǎn)發(fā)生誤跟蹤,應(yīng)適當(dāng)?shù)卦龃笃湎秶?盡量減少運(yùn)動(dòng)目標(biāo),以免影響其誤差[10]。

由于液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人在清潔過程中,位姿變化非常復(fù)雜,需要結(jié)合機(jī)器人瞬時(shí)姿態(tài)值,才能獲取機(jī)器人位姿信息[7]。當(dāng)清潔機(jī)器人的收集處理器采用吸附再生法降解凈化處理油時(shí),俯仰軸姿態(tài)角計(jì)算公式為

(5)

式中:S1(t)、S2(t)分別表示機(jī)器人在初始清理位置和當(dāng)前清理位置;L12表示兩次清潔位置之間的距離;t表示機(jī)器人移動(dòng)時(shí)間。

清潔機(jī)器人在清潔時(shí),機(jī)器人前進(jìn)位移由超聲傳感器的測(cè)量值確定,其計(jì)算公式為

x(t)=[S3(t)+L3]cosθ(t)-L4sinθ(t)

(6)

式中:S3(t)表示傳感器測(cè)量的位置;L3、L4分別表示傳感器與機(jī)器人中心、底盤距離[11]。

在確定俯仰軸姿態(tài)角和機(jī)器人前進(jìn)位移的前提下,構(gòu)建機(jī)器人狀態(tài)位移矩陣:

(7)

式中η表示機(jī)器人在前進(jìn)位移下的移動(dòng)頻率。在該頻率下,計(jì)算機(jī)器人輸入控制矩陣:

(8)

基于此,構(gòu)建的清潔機(jī)器人位姿跟蹤數(shù)學(xué)模型為

X(t)=F(t)+H(t)

(9)

由于機(jī)器人的清潔速度較快,因此在清潔期間,z方向的位移對(duì)于機(jī)器人正常工作狀態(tài)下的方向偏差較小,但是x方向上的位移變化較大。因此,可忽略z向的定位問題,僅探討x方向的定位問題。

2.2 機(jī)器人末端位姿誤差補(bǔ)償

在機(jī)器人直線移動(dòng)時(shí),左輪越過一個(gè)清潔物,使其保持直線移動(dòng),左輪向前移動(dòng)一段距離。當(dāng)無清潔物存在時(shí),機(jī)器人的兩個(gè)輪子將盡量保持同樣的速度,而左邊輪子轉(zhuǎn)速要小于右邊輪子轉(zhuǎn)速,這樣機(jī)器人就會(huì)朝有清潔物的方向移動(dòng)。同時(shí),機(jī)器人的運(yùn)動(dòng)路線也會(huì)從直線變?yōu)榍€,在經(jīng)過清潔物后,機(jī)器人的末端位姿發(fā)生變化,即產(chǎn)生誤差。為了補(bǔ)償機(jī)器人的末端位姿誤差,采用神經(jīng)網(wǎng)絡(luò)的真實(shí)輸出和預(yù)期輸出的平均誤差作為模型的適應(yīng)性函數(shù),可表示為

(10)

N=(m+1)×j+(j+1)×n

(11)

式中:m表示輸入節(jié)點(diǎn)數(shù);n表示輸出節(jié)點(diǎn)數(shù);j表示隱藏層節(jié)點(diǎn)數(shù)。

為了最小化位姿誤差,設(shè)定閾值ε需滿足如下公式:

f≤ε

(12)

僅考慮上述誤差無法達(dá)到精準(zhǔn)跟蹤機(jī)器人位姿,因此在上述研究的基礎(chǔ)上,分析誤差參數(shù)對(duì)機(jī)器人位姿跟蹤的影響,從而調(diào)整機(jī)器人狀態(tài)轉(zhuǎn)移矩陣,實(shí)現(xiàn)清潔機(jī)器人位姿跟蹤數(shù)學(xué)模型修正,則調(diào)整后的機(jī)器人狀態(tài)位移矩陣為

(13)

式中Δφ表示誤差參量包含的誤差值。

聯(lián)立上述公式,得到機(jī)器人末端位姿誤差補(bǔ)充公式:

(14)

將補(bǔ)償誤差后的機(jī)器人末端位姿帶入式(9),替換原來的機(jī)器人狀態(tài)位移矩陣,得到更加準(zhǔn)確的清潔機(jī)器人位姿跟蹤數(shù)學(xué)模型:

X(t)=V(t)+H(t)

(15)

至此完成機(jī)器人末端位姿誤差補(bǔ)償,并且得到了精度更高的機(jī)器人位姿跟蹤模型。通過誤差補(bǔ)償,提高了機(jī)器人位姿跟蹤效果。

3 仿真分析

為了驗(yàn)證基于自控測(cè)距的液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人位姿跟蹤研究的合理性,在水電工程液壓?jiǎn)㈤]機(jī)油缸仿真平臺(tái)上進(jìn)行仿真分析。

3.1 實(shí)驗(yàn)裝置及內(nèi)容

液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人整體結(jié)構(gòu),如圖3所示。

圖3 液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人整體結(jié)構(gòu)

清潔機(jī)器人工作原理:當(dāng)清潔機(jī)器人啟動(dòng)時(shí),兩組固定塊的電動(dòng)連接軸開始工作,收集板在電動(dòng)連接軸滾動(dòng)的情況下自動(dòng)閉合,同時(shí)采用平衡模塊收集機(jī)器人的運(yùn)行數(shù)據(jù),實(shí)時(shí)調(diào)整機(jī)器人運(yùn)動(dòng)平衡。當(dāng)清潔機(jī)器人停止工作時(shí),兩組收集板在電動(dòng)連接軸下自動(dòng)打開,通過電動(dòng)連接軸實(shí)時(shí)調(diào)整收集面積。通過進(jìn)油管中的收集處理器降解凈化處理油污,并通過排油管排出。

3.2 機(jī)器人實(shí)際運(yùn)動(dòng)軌跡及相關(guān)數(shù)據(jù)分析

使用RAM計(jì)算機(jī)作為仿真平臺(tái),利用Matlab工具箱得到機(jī)器人實(shí)際運(yùn)動(dòng)軌跡,如圖4所示。

圖4 機(jī)器人實(shí)際運(yùn)動(dòng)軌跡

由圖4可知,該機(jī)器人正在執(zhí)行清潔工作時(shí)的精準(zhǔn)位姿數(shù)據(jù)如圖5所示。

圖5 機(jī)器人精準(zhǔn)位姿數(shù)據(jù)

圖5所示的機(jī)器人末端位姿坐標(biāo)為(0.5,0.7,0.25)。為了定量分析機(jī)器人位姿跟蹤性能,使用方均根誤差作為實(shí)驗(yàn)結(jié)果好壞的評(píng)價(jià)指標(biāo),公式為

(16)

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

分別使用文獻(xiàn)[2]、文獻(xiàn)[3]和所研究方法對(duì)比分析機(jī)器人跟蹤軌跡,對(duì)比結(jié)果如圖6所示。

圖6 3種方法機(jī)器人跟蹤軌跡對(duì)比分析

由圖6可知,使用文獻(xiàn)[2]、文獻(xiàn)[3]的機(jī)器人運(yùn)動(dòng)軌跡與實(shí)際軌跡不一致,其中文獻(xiàn)[2]方法下的機(jī)器人運(yùn)動(dòng)軌跡與實(shí)際軌跡差別最大;使用所研究方法機(jī)器人運(yùn)動(dòng)軌跡與實(shí)際軌跡基本一致,該軌跡僅在x方向0～10 m和50～70 m位置處出現(xiàn)較小的誤差,使用所研究方法的y方向位置不固定。

對(duì)于機(jī)器人清潔工作下的機(jī)器人位姿數(shù)據(jù)跟蹤結(jié)果,如圖7所示。

圖7 3種方法機(jī)器人位姿數(shù)據(jù)跟蹤結(jié)果對(duì)比分析

由圖7可知,使用文獻(xiàn)[2]機(jī)器人末端位姿與實(shí)際位姿不一致,末端位姿坐標(biāo)為(0.2,0.8,1.0);使用文獻(xiàn)[3]機(jī)器人末端位姿與實(shí)際位姿不一致,末端位姿坐標(biāo)為(-1.1,0.9,1.6);使用所研究方法雖然在前一關(guān)節(jié)發(fā)生了與實(shí)際運(yùn)動(dòng)趨勢(shì)不相符的狀態(tài),但是不影響清潔機(jī)器人末端位姿,其得到的末端位姿坐標(biāo)為(0.5,0.7,0.25),與實(shí)際坐標(biāo)位姿一致。由此說明,所研究方法有效補(bǔ)充了機(jī)器人末端位姿誤差。

4 結(jié)語

本文所提出的自控測(cè)距方法,使用激光儀布置了清潔機(jī)器人位姿跟蹤裝置,結(jié)合自控測(cè)距方法和誤差補(bǔ)償,精準(zhǔn)跟蹤液壓?jiǎn)㈤]機(jī)油缸清潔機(jī)器人位姿,解決傳統(tǒng)方法位姿跟蹤精度低的問題。仿真結(jié)果表明:該方法在保證跟蹤實(shí)時(shí)性前提下,精準(zhǔn)跟蹤了機(jī)器人運(yùn)動(dòng)軌跡,末端跟蹤誤差為0,實(shí)現(xiàn)了機(jī)器人位姿的精準(zhǔn)跟蹤。