基于自控測距的液壓啟閉機(jī)油缸清潔機(jī)器人位姿跟蹤研究

劉曉東,卜曉珊,吳增眾

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

0 引言

目前的油缸清洗方法沒有相應(yīng)的輔助標(biāo)志,難以達(dá)到高效的清洗效果,因此,提出了液壓啟閉機(jī)油缸清潔機(jī)器人。機(jī)器人的定位是當(dāng)前導(dǎo)航領(lǐng)域的一個(gè)主要研究領(lǐng)域,其需要在移動中準(zhǔn)確地感知自身的位置和姿態(tài)[1]。相關(guān)學(xué)者研究了機(jī)器人位姿跟蹤,文獻(xiàn)[2]提出了基于滾動時(shí)域優(yōu)化的位姿跟蹤方法,該方法結(jié)合機(jī)器人運(yùn)動學(xué)模型,采用PD控制方式實(shí)現(xiàn)了機(jī)器人跟蹤參考位姿,并構(gòu)建了閉環(huán)誤差模型。采用滾動時(shí)域跟蹤方法實(shí)時(shí)跟蹤機(jī)器人位姿誤差,并在一個(gè)固定時(shí)域窗口內(nèi)利用誤差參數(shù)迭代處理位姿誤差,結(jié)合參考位姿實(shí)現(xiàn)對機(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è)。為此,提出了基于自控測距的液壓啟閉機(jī)油缸清潔機(jī)器人位姿跟蹤技術(shù)。

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

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

(1)

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

(2)

式中φ表示誤差參量。

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

圖1 位姿跟蹤裝置布局

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

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

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

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

|xoa-xob>x0

(3)

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

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

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

(4)

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

由于液壓啟閉機(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ī)器人移動時(shí)間。

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

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

(6)

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

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

(7)

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

(8)

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

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

(9)

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

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

在機(jī)器人直線移動時(shí),左輪越過一個(gè)清潔物,使其保持直線移動,左輪向前移動一段距離。當(dāng)無清潔物存在時(shí),機(jī)器人的兩個(gè)輪子將盡量保持同樣的速度,而左邊輪子轉(zhuǎn)速要小于右邊輪子轉(zhuǎn)速,這樣機(jī)器人就會朝有清潔物的方向移動。同時(shí),機(jī)器人的運(yùn)動路線也會從直線變?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ù)對機(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)證基于自控測距的液壓啟閉機(jī)油缸清潔機(jī)器人位姿跟蹤研究的合理性,在水電工程液壓啟閉機(jī)油缸仿真平臺上進(jìn)行仿真分析。

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

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

圖3 液壓啟閉機(jī)油缸清潔機(jī)器人整體結(jié)構(gòu)

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

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

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

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

由圖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é)果好壞的評價(jià)指標(biāo),公式為

(16)

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

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

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

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

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

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

由圖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)動趨勢不相符的狀態(tài),但是不影響清潔機(jī)器人末端位姿,其得到的末端位姿坐標(biāo)為(0.5,0.7,0.25),與實(shí)際坐標(biāo)位姿一致。由此說明,所研究方法有效補(bǔ)充了機(jī)器人末端位姿誤差。

4 結(jié)語

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