鐘浩翔, 陳亞, 王殿君, 朱亞東, 焦向東, 高易佳

(北京石油化工學(xué)院 機(jī)械工程學(xué)院,北京 102600)

泊車機(jī)器人是一種重載移動(dòng)機(jī)器人,是立體車庫的關(guān)鍵執(zhí)行設(shè)備[1-2]。在進(jìn)行泊車作業(yè)時(shí),若機(jī)器人位姿信息出現(xiàn)偏差會(huì)導(dǎo)致汽車無法被搬運(yùn)到指定位置,甚至使用戶財(cái)產(chǎn)安全受到威脅[3-4]。

目前移動(dòng)機(jī)器人室內(nèi)定位常用的方法有SLAM、視覺里程計(jì)、RFID和慣性單元等[5-8]。其中慣性單元因其對(duì)環(huán)境依賴性低、低成本等優(yōu)點(diǎn)成為移動(dòng)機(jī)器人室內(nèi)定位較常采用的方法,但零點(diǎn)漂移是慣性單元的主要問題,導(dǎo)致無法長時(shí)間獨(dú)立定位[9-10]。陳映秋等將慣性單元固定于車輪中央,利用慣性單元與車輪一起旋轉(zhuǎn)的運(yùn)動(dòng)特性,提取出運(yùn)動(dòng)速度信息,從慣導(dǎo)安裝方式和算法設(shè)計(jì)層面實(shí)現(xiàn)了慣性單元的獨(dú)立導(dǎo)航[11],但慣導(dǎo)單元零點(diǎn)漂移誤差無法消除,使得該定位方法無法保證定位系統(tǒng)精度。劉艷嬌等將慣性單元與視覺傳感器進(jìn)行結(jié)合,將慣性單元數(shù)據(jù)緊密關(guān)聯(lián)到圖像跟蹤過程,提供精確的短期運(yùn)動(dòng)約束和較好的初始梯度方向信息,有效地解決了零漂問題[12],但泊車機(jī)器人工作環(huán)境屬于無人環(huán)境,光線不足問題使得視覺傳感器并不適用。Wang等提出了一種利用RFID和慣性單元進(jìn)行室內(nèi)定位的方法[13],估計(jì)軌跡主要由RFID確定,在RFID信號(hào)不穩(wěn)定時(shí)采用慣性單元進(jìn)行定位,但由于基于RFID的定位方法定位誤差較大,且車庫中包括無線通訊設(shè)備在內(nèi)的電子設(shè)備會(huì)嚴(yán)重影響基于RFID的定位系統(tǒng)精度。因此需要對(duì)泊車機(jī)器人的定位方法進(jìn)行進(jìn)一步研究。

首先對(duì)雙舵輪泊車機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)分析,設(shè)計(jì)了一種融合慣導(dǎo)、里程計(jì)、運(yùn)動(dòng)學(xué)航位推算數(shù)據(jù)的定位算法,建立了泊車機(jī)器人定位系統(tǒng),并進(jìn)行了機(jī)器人定位試驗(yàn)。

1 泊車機(jī)器人運(yùn)動(dòng)學(xué)模型

泊車機(jī)器人輪系布局如圖1所示。該輪系結(jié)構(gòu)由對(duì)角布置的舵輪、兩個(gè)從動(dòng)萬向輪以及中部兩個(gè)彈簧減震輪組成,每個(gè)舵輪能夠獨(dú)立進(jìn)行驅(qū)動(dòng)和轉(zhuǎn)向。

圖1 泊車機(jī)器人輪系結(jié)構(gòu)Fig.1 Wheel structure of a parking robot

以慣性坐標(biāo)系N(X,O,Y)為基準(zhǔn),建立車體坐標(biāo)系B(x,o,y)、舵輪坐標(biāo)系D(xo,o,yo),如圖2所示。

圖2 曲線運(yùn)動(dòng)位姿狀態(tài)Fig.2 Pose status of curved motion

設(shè)左前方舵輪的速度為Va,與xo軸正方向夾角為θa,右后方舵輪的速度為Vb,與xo軸正方向夾角為θb,車體中心速度為Vo,與xo軸正方向夾角為θ,Vox和Voy為Vo在xo、yo方向速度分量。φ為車體坐標(biāo)系與慣性坐標(biāo)系X軸夾角。L為兩舵輪沿y方向的前后距離,W為兩舵輪沿x方向的左右間距,α為定值,表達(dá)式為

α=arctan(W/L)

(1)

為了使機(jī)器人兩舵輪間不產(chǎn)生內(nèi)應(yīng)力,雙舵輪運(yùn)動(dòng)模型滿足如下約束條件

Vasinθa=Vbsinθb

(2)

根據(jù)兩舵輪實(shí)際速度及方向角可推導(dǎo)泊車機(jī)器人中心位置的速度大小為

(3)

機(jī)器人中心速度Vo與xo方向的夾角θ為

(4)

機(jī)器人在平面內(nèi)繞某一點(diǎn)做圓周運(yùn)動(dòng),根據(jù)舵輪的速度,可確定其旋轉(zhuǎn)半徑R及機(jī)器人角速度ω:

(5)

(6)

綜合以上推導(dǎo),設(shè)泊車機(jī)器人初始位姿為(x0,y0,φ0),單個(gè)控制周期時(shí)間為t,則機(jī)器人于慣性坐標(biāo)系位姿可表示為:

(7)

2 多傳感器組合定位模塊

定位功能是泊車機(jī)器人的核心功能,是機(jī)器人完成存/取車任務(wù)的前提。泊車機(jī)器人要求定位系統(tǒng)對(duì)車庫改造小,獨(dú)立工作能力強(qiáng),且定位精度達(dá)到±25 mm[14]。本文提出的定位模塊將慣導(dǎo)與里程計(jì)組合進(jìn)行位姿推算,并利用卡爾曼濾波技術(shù)[15],將其與運(yùn)動(dòng)學(xué)航位推算結(jié)果進(jìn)行數(shù)據(jù)融合,從而提高定位精度。最終結(jié)合二維碼的絕對(duì)位姿信息對(duì)定位結(jié)果進(jìn)行定期校正。

(8)

式中:c=cos,s=sin。

設(shè)Δt時(shí)間內(nèi)兩個(gè)舵輪的轉(zhuǎn)向角度保持不變,前后舵輪運(yùn)動(dòng)軌跡近似為短直線,則兩舵輪移動(dòng)里程分別為ΔS1、ΔS2,進(jìn)而可求得車體中心移動(dòng)里程ΔS為

(9)

車體中心于導(dǎo)航坐標(biāo)系X方向、Y方向的移動(dòng)里程ΔSX和ΔSY可表示為

(10)

設(shè)第i時(shí)刻,泊車機(jī)器人的位置為[X(i),Y(i),Z(i)],第i+1時(shí)刻,慣性單元測量的姿態(tài)角為[θ(i+1),φ(i+1),φ(i+1)],從第i時(shí)刻到第i+1時(shí)刻泊車機(jī)器人幾何中心移動(dòng)里程為ΔS(i+1),則第i+1時(shí)刻,泊車機(jī)器人的位置[X(i+1),Y(i+1),Z(i+1)]為

(11)

工控機(jī)可接收PMAC運(yùn)動(dòng)控制卡獲取的相應(yīng)電機(jī)編碼器數(shù)據(jù)及慣導(dǎo)數(shù)據(jù),換算后得出前、后舵輪移動(dòng)里程ΔS1、ΔS2,舵輪方向θ1、θ2及機(jī)器人姿態(tài)角(θ,φ,φ),對(duì)數(shù)據(jù)進(jìn)行上述計(jì)算,可得出基于慣導(dǎo)/里程計(jì)信息推算的機(jī)器人位姿。工控機(jī)同樣可以通過提取的前、后舵輪的速度(V1,V2)和角度(θ1,θ2)進(jìn)行基于運(yùn)動(dòng)學(xué)的航位推算,然后采用卡爾曼濾波的數(shù)據(jù)融合方法將兩種定位結(jié)果進(jìn)行融合,充分利用短時(shí)間內(nèi)慣性單元的高精度航向角檢測和里程計(jì)的準(zhǔn)確位置推算的特點(diǎn),結(jié)合運(yùn)動(dòng)學(xué)推算位姿實(shí)時(shí)性較強(qiáng)的優(yōu)勢,得到機(jī)器人的最優(yōu)位姿。

車庫中以一定距離間隔布置二維碼標(biāo)簽,它們包含的信息包括自身ID號(hào)以及當(dāng)前二維碼傳感器中心與標(biāo)簽中心的偏移量(ΔX,ΔY,Δθ),其中ID號(hào)與導(dǎo)航坐標(biāo)系中坐標(biāo)(X,Y)綁定,則掃描二維碼能夠提取到機(jī)器人的位姿(X-ΔX,Y-ΔY,Δθ),利用該數(shù)據(jù)對(duì)數(shù)據(jù)融合結(jié)果進(jìn)行校正,達(dá)到定期清除累計(jì)誤差的目的。定位模塊軟件流程如圖3所示。

圖3 導(dǎo)航定位模塊軟件流程Fig.3 Software flow of navigation and positioning module

3 定位實(shí)驗(yàn)

3.1 定位系統(tǒng)

利用團(tuán)隊(duì)自主研發(fā)的雙舵輪泊車機(jī)器人樣機(jī)對(duì)定位模塊進(jìn)行實(shí)驗(yàn)驗(yàn)證,機(jī)器人樣機(jī)如圖4所示。

圖4 雙舵輪泊車機(jī)器人樣機(jī)Fig.4 Prototype of the double steering-wheel parking robot

樣機(jī)采用研華PPC-6120作為上位機(jī),泰道公司Clipper運(yùn)動(dòng)控制卡作為下位機(jī),構(gòu)成開放式控制器硬件系統(tǒng),保證機(jī)器人對(duì)控制信號(hào)的實(shí)時(shí)響應(yīng)。實(shí)驗(yàn)場地為平坦水泥路面,令泊車機(jī)器人直行6.65 m,橫移2.5 m,倒退6.65 m,模擬從車廳取車后存入車位的運(yùn)動(dòng)過程。誤差標(biāo)定設(shè)備采用美國FARO公司的激光跟蹤儀,將激光靶球置于泊車機(jī)器人中心,實(shí)時(shí)記錄機(jī)器人在導(dǎo)航坐標(biāo)系中的坐標(biāo)位置。實(shí)驗(yàn)環(huán)境如圖5所示。

3.2 定位結(jié)果

設(shè)定泊車機(jī)器人移動(dòng)速度為0.3 m/s,分別應(yīng)用運(yùn)動(dòng)學(xué)航位推算定位方法和多傳感器組合定位方法進(jìn)行了泊車機(jī)器人定位試驗(yàn)。采用運(yùn)動(dòng)學(xué)航位推算定位方法得到的定位結(jié)果如圖6和表1所示,采用多傳感器組合定位方法得到的定位結(jié)果如圖7和表2所示。

表1 運(yùn)動(dòng)學(xué)航位推算定位方法實(shí)驗(yàn)數(shù)據(jù)Tab.1 Experimental data of kinematic dead reckoning positioning method

表2 多傳感器組合定位方法實(shí)驗(yàn)數(shù)據(jù)Tab.2 Experimental data of multi-sensor combined positioning method

圖6 運(yùn)動(dòng)學(xué)航位推算定位方法實(shí)驗(yàn)結(jié)果Fig.6 Experimental results on kinematic dead reckoning positioning method

圖7 多傳感器組合定位方法實(shí)驗(yàn)結(jié)果Fig.7 Experimental results on multi-sensor combined positioning method

3.3 對(duì)比分析

從表2可知,采用運(yùn)動(dòng)學(xué)航位推算定位方法的泊車機(jī)器人平均定位偏差為176.46 mm,從表3可知,同樣實(shí)驗(yàn)條件下,采用多傳感器組合定位方法的泊車機(jī)器人平均定位偏差為14.04 mm,相比前者降低了92%,定位精度有了較大提高,滿足泊車機(jī)器人定位精度±25 mm的技術(shù)要求。

4 結(jié)論

1) 建立了雙舵輪泊車機(jī)器人的運(yùn)動(dòng)學(xué)模型,提出了融合慣導(dǎo)、里程計(jì)、運(yùn)動(dòng)學(xué)航位推算的多傳感器組合導(dǎo)航定位方法。

2) 定位試驗(yàn)結(jié)果表明,采用本文提出的多傳感器組合定位方法的泊車機(jī)器人定位精度明顯提高,能夠滿足泊車機(jī)器人技術(shù)要求。

3) 所構(gòu)建的泊車機(jī)器人定位系統(tǒng)為機(jī)器人存取車功能研究奠定了基礎(chǔ)。