基于手眼標(biāo)定算法的移動(dòng)機(jī)器人重定位方法研究

楊明權(quán)，桂輝陽(yáng)，關(guān)宇洋，毛臻炫

（南方電網(wǎng)超高壓輸電公司天生橋局，興義 562400）

0 引言

移動(dòng)機(jī)器人屬于一種可以自動(dòng)完成指定工作執(zhí)行的一種機(jī)器裝置，其有兩種執(zhí)行方式，一是預(yù)先設(shè)定執(zhí)行程序命令，其自動(dòng)運(yùn)行；二是遙控控制運(yùn)行。目前移動(dòng)機(jī)器人已經(jīng)進(jìn)入人們的生活和工作中[1]，例如清潔機(jī)器人、導(dǎo)航機(jī)器人、采摘機(jī)器人等。無(wú)論哪種移動(dòng)機(jī)器人，在工作執(zhí)行時(shí)，均需可靠完成定位，特別是在位置環(huán)境中自主移動(dòng)時(shí)[2]，其需結(jié)合傳感器獲取的周?chē)h(huán)境數(shù)據(jù)完成地圖生成，以此實(shí)現(xiàn)定位。但是，機(jī)器人在自主移動(dòng)過(guò)程中，其會(huì)受到移動(dòng)滑輪、聲音以及環(huán)境等多種因素的影響，導(dǎo)致機(jī)器人的及時(shí)定位精度[3]，存在一定誤差，因此，如果保證移動(dòng)機(jī)器人有效應(yīng)對(duì)環(huán)境變化，實(shí)現(xiàn)其及時(shí)、可靠的定位成為保證機(jī)器人工作執(zhí)行的重要內(nèi)容。重定位指的是地理空間轉(zhuǎn)換的過(guò)程，該轉(zhuǎn)換主要是將程序中邏輯地址向?qū)嶋H環(huán)境中物理地址轉(zhuǎn)換，該轉(zhuǎn)換能夠保證多道程序的同時(shí)運(yùn)行[4]。手眼標(biāo)定是機(jī)器人應(yīng)用領(lǐng)域中一個(gè)重要的內(nèi)容，其決定機(jī)器人任務(wù)執(zhí)行時(shí)，對(duì)于目標(biāo)的定位精度。

基于上述分析，為了提升一定機(jī)器人的定位精度，保證其對(duì)于目標(biāo)的定位效果，文獻(xiàn)[5]和文獻(xiàn)[6]分別基于3D-NDT、深度學(xué)習(xí)與粒子濾波提出相關(guān)定位方法。上述方法均具備定位效果，但是當(dāng)環(huán)境中的障礙物比例較大時(shí)，定位結(jié)果的精度存在一定偏差。因此，本文研究基于手眼標(biāo)定算法的移動(dòng)機(jī)器人重定位方法，該方法以機(jī)器人模型為基礎(chǔ)，完成其手眼標(biāo)定，依據(jù)標(biāo)定結(jié)果，完成移動(dòng)機(jī)器人重定位。

1 移動(dòng)機(jī)器人重定位

1.1 機(jī)器人移動(dòng)模型

針對(duì)移動(dòng)機(jī)器人重定位，本文對(duì)其展開(kāi)全面分析后，為保證最佳的重定位效果[7]，先構(gòu)建機(jī)器人移動(dòng)模型，然后確定機(jī)器人移動(dòng)時(shí)的手眼標(biāo)定模型。以雙輪機(jī)器人為例，驅(qū)使機(jī)器人在指定環(huán)境中移動(dòng)，獲取機(jī)器人在移動(dòng)過(guò)程中的位置，得出機(jī)器人移動(dòng)模型，其如圖1所示。

圖1 機(jī)器人移動(dòng)模型

設(shè)移動(dòng)機(jī)器人的左、右輪的速度分別用? l、? r表示，平移速度和角速度分別用?、w表示，兩者的計(jì)算公式如式(1)、式(2)所示：

式(1)、式(2)中：d表示距離，屬于機(jī)器人中心和輪子之間。

機(jī)器人位置坐標(biāo)和方向的更新公式為如(3)所示：

式(3)中：θ表示位置坐標(biāo)；x和y均表示方向；Δt表示時(shí)間差量。

依據(jù)上述公式完成更新后，需完成左、右兩個(gè)輪的?更新，該更新需依靠概率p完成，其公式如式(4)所示：

式(4)中：r1、r2、r3均表示隨機(jī)變量；α、β表示左、右兩個(gè)輪的最小速度值。

1.2 移動(dòng)機(jī)器人的手眼標(biāo)定模型

相機(jī)坐標(biāo)系和基坐標(biāo)系是機(jī)器人具有兩種必備坐標(biāo)系，除此之外還設(shè)有工具坐標(biāo)系，機(jī)器人手眼標(biāo)定的目的則是實(shí)現(xiàn)前兩和坐標(biāo)系之間關(guān)聯(lián)的計(jì)算[8]，并依據(jù)該計(jì)算結(jié)果，向基坐標(biāo)中轉(zhuǎn)換位姿結(jié)果。

設(shè)定視覺(jué)測(cè)量點(diǎn)，并控制一定機(jī)器人使其末端位于該點(diǎn)，以基坐標(biāo)系為參照[10]，獲取位于其內(nèi)的空間點(diǎn)坐標(biāo)，且依據(jù)機(jī)器人手臂的運(yùn)動(dòng)學(xué)參數(shù)完成。如果和兩個(gè)坐標(biāo)之間只存在剛性變換關(guān)聯(lián)，該關(guān)聯(lián)的計(jì)算公式如式(5)所示：

式(5)中：R和T均表示矩陣，前者對(duì)應(yīng)旋轉(zhuǎn)，后者對(duì)應(yīng)平移；并且，前者的性質(zhì)表達(dá)公式如式(6)所示：

式(6)中：I表示單位矩陣。

實(shí)現(xiàn)R和T的估計(jì)，是手眼標(biāo)定的目標(biāo)[11]，則采用最小二乘法完成該目標(biāo)函數(shù)的估計(jì)，其公式如式(7)所示：

式(8)表示F對(duì)T求偏導(dǎo)，其獲取極值的標(biāo)準(zhǔn)為該偏導(dǎo)數(shù)等于0。基于此得出：

如果式(9)的結(jié)果等于0，則可計(jì)算T的結(jié)果，其公式如式(10)所示：

向式(8)中代入式(10)，則目標(biāo)函數(shù)可表示如式(12)所示：

式(13)中：gi和hi分別別表示任意點(diǎn)及其空間點(diǎn)坐標(biāo)的空間變化量，且有結(jié)合式(6)則得出：

機(jī)器人手眼標(biāo)定參數(shù)中，T需滿足式(13)的標(biāo)準(zhǔn)：

式(15)中：求解矩陣的跡用tr(·)表示。

設(shè)S=GHT，采用奇異值分解對(duì)其實(shí)行處理，得出：

式(16)中：U、V均為正交矩陣，R的結(jié)果則為：

基于上述的式(11)和式(17)獲取矩陣R和T的結(jié)果，即得出機(jī)器人的手眼變換矩陣。

1.3 基于深度學(xué)習(xí)多任務(wù)網(wǎng)絡(luò)的移動(dòng)機(jī)器人重定位模型

為精準(zhǔn)完成移動(dòng)機(jī)器人的重定位，本文引入及時(shí)定位和地圖建立技術(shù)（Simultaneous Localization And Mapping，SLAM）并且，針對(duì)不同的任務(wù)需求[12]，采用不同的網(wǎng)絡(luò)結(jié)構(gòu)分支完成多定位任務(wù)的實(shí)現(xiàn)。因此，本文提出基于深度學(xué)習(xí)多任務(wù)的移動(dòng)機(jī)器人重定位模型，該模型由全局位姿回歸和視覺(jué)里程計(jì)兩個(gè)子網(wǎng)絡(luò)組成，分別將上小節(jié)獲取的R和T作為兩個(gè)網(wǎng)絡(luò)的輸入，以此完成移動(dòng)機(jī)器人重定位。

圖2 基于深度學(xué)習(xí)多任務(wù)網(wǎng)絡(luò)的移動(dòng)機(jī)器人重定位模型

模型整體為ResNst30層連體模式，模型的前15個(gè)層主要完成卷積計(jì)算，其將計(jì)算結(jié)果輸入至全連接層中進(jìn)行數(shù)據(jù)的深入特征分析后[13]，由輸出層輸出重定位結(jié)果。在該過(guò)程中，模型的分配權(quán)重計(jì)算公式如式(18)所示：

式(18)中：e表示運(yùn)算函數(shù)；xi表示第i個(gè)描述符；wk和bk分別表示第k層的權(quán)重和可訓(xùn)練參數(shù)。

將R和T輸入值基于深度學(xué)習(xí)多任務(wù)的移動(dòng)機(jī)器人重定位模型中，模型對(duì)兩個(gè)實(shí)行學(xué)習(xí)訓(xùn)練后，輸出移動(dòng)機(jī)器人的全局位姿結(jié)果[14]。模型在進(jìn)行學(xué)習(xí)訓(xùn)練過(guò)程中，損失函數(shù)直接影響模型的最終重定位結(jié)果，為保證模型對(duì)于多任務(wù)的定位效果，結(jié)合模型的組成結(jié)構(gòu)[15]，采用多任務(wù)損失函數(shù)設(shè)計(jì)，并且在損失函數(shù)中引入懲罰項(xiàng)ψ，保證模型的重定位效果。

R輸入至全局位姿回歸網(wǎng)絡(luò)后，為保證相機(jī)系統(tǒng)坐標(biāo)和實(shí)際機(jī)器人運(yùn)動(dòng)坐標(biāo)之間的一致，將兩者之間的偏差項(xiàng)引入損失函數(shù)中，其計(jì)算公式如式(19)所示：

式(20)～式(23)中：xt和分別表示實(shí)際位置和定位位置；qt和分別表示機(jī)器人實(shí)際位姿和計(jì)算位姿。

將T作為視覺(jué)里程計(jì)子網(wǎng)絡(luò)的輸入，該子網(wǎng)絡(luò)的損失函數(shù)公式如式(24)～式(26)所示：

通過(guò)上述步驟確定兩個(gè)子網(wǎng)絡(luò)的損失函數(shù)后，確定整個(gè)模型的總損失函數(shù)，其公式如式(27)所示：

式(27)中：m表示正常數(shù)；j表示維度；l表示距離平方。獲取總損失函數(shù)后，完成整個(gè)模型的學(xué)習(xí)訓(xùn)練，并輸出最終的機(jī)器人重定位結(jié)果。

3 測(cè)試分析

為衡量本文方法對(duì)移動(dòng)機(jī)器人的重定位效果，展開(kāi)相關(guān)應(yīng)用測(cè)試，采用BR280移動(dòng)抓取機(jī)器人進(jìn)行相關(guān)測(cè)試，測(cè)試在室內(nèi)和室外兩種環(huán)境中進(jìn)行。

該機(jī)器人包含機(jī)械手臂的外形尺寸為40cm×26.2cm×65.8cm，其有2個(gè)4寸電力，2個(gè)1.5寸的萬(wàn)向輪，整體重量為13kg，最大負(fù)載為10kg，單個(gè)電機(jī)的額定功率為150W，最大越障高度為5cm，最大移動(dòng)速度為1m/s，最小轉(zhuǎn)彎半徑為2.2cm，最大爬坡角度為5°；機(jī)械手臂的臂展尺寸為2.8cm，自由度為6，重復(fù)定位精度為±0.5mm。其搭載的相機(jī)相關(guān)參數(shù)詳情如表1所示。

表1 相機(jī)相關(guān)參數(shù)詳情

為測(cè)試本文方法對(duì)于移動(dòng)機(jī)器人手眼標(biāo)定的性能，設(shè)定不同的視覺(jué)測(cè)量點(diǎn)位置，獲取本文方法對(duì)于手眼標(biāo)定的R和T兩種矩陣的坐標(biāo)標(biāo)定結(jié)果，結(jié)果如表2所示。

表2 兩種矩陣的坐標(biāo)標(biāo)定結(jié)果

對(duì)表2測(cè)試結(jié)果實(shí)行分析可知：在不同的視覺(jué)測(cè)量點(diǎn)位置下，本文方法在機(jī)器人處于不同旋轉(zhuǎn)角度和平移方向下，對(duì)于手眼標(biāo)定的R和T兩種矩陣的坐標(biāo)標(biāo)定結(jié)果與設(shè)定的視覺(jué)測(cè)量點(diǎn)位置結(jié)果差異極小，吻合程度極高。因此，本文方法對(duì)于移動(dòng)機(jī)器人的手眼標(biāo)定性能良好，可保證手眼標(biāo)定的R和T兩種矩陣的坐標(biāo)標(biāo)定結(jié)果的可靠性。

為進(jìn)一步衡量本文方法的應(yīng)用性能，采用歐式距離誤差作為評(píng)價(jià)本文方法的重定位性能的指標(biāo)，期望結(jié)果為低于2cm。其公式如式(28)所示。

依據(jù)該公式獲取本文方法在不同的障礙物比例下，本文方法在室內(nèi)和室外兩種環(huán)境中的DEL(I)測(cè)試結(jié)果，如圖3所示。

圖3 兩種環(huán)境中的歐式距離誤差測(cè)試結(jié)果

對(duì)圖3測(cè)試結(jié)果實(shí)行分析后可知：在兩種環(huán)境中，本文方法隨著障礙物占據(jù)比例的逐漸增加，DEL(I)指標(biāo)結(jié)果也存在一定差異，但是該結(jié)果均在期望標(biāo)準(zhǔn)下，最高DEL(I)結(jié)果為1.7cm左右，最低DEL(I)結(jié)果為0.5cm左右。因此，本文方法的重定位性能良好，能夠保證重定位的精準(zhǔn)結(jié)果。

為衡量本文方法對(duì)于移動(dòng)機(jī)器人重定位的應(yīng)用效果，機(jī)器人按照360度的軌跡實(shí)行移動(dòng)，該移動(dòng)軌跡上沒(méi)有障礙物。并在移動(dòng)軌跡上，設(shè)定4個(gè)目標(biāo)標(biāo)定點(diǎn)，采用本文方法對(duì)該標(biāo)定點(diǎn)實(shí)行重定位，獲取重定位結(jié)果，如圖4所示。

圖4 目標(biāo)位置的移動(dòng)標(biāo)定結(jié)果

對(duì)圖4測(cè)試結(jié)果實(shí)行分析后可知：在機(jī)器人的圓形移動(dòng)軌跡中，本文方法應(yīng)用后能夠完成機(jī)器人整個(gè)移動(dòng)軌跡的重定位，并且其中4個(gè)目標(biāo)標(biāo)定點(diǎn)的重定位結(jié)果吻合。因此，本文方法的重定位效果良好，能夠?qū)崟r(shí)定位機(jī)器人的移動(dòng)位置。

為測(cè)試本文方法對(duì)移動(dòng)機(jī)器人重定位的應(yīng)用效果，將實(shí)驗(yàn)采用機(jī)器人分別置于室外和室內(nèi)環(huán)境中，在兩種環(huán)境中隨機(jī)設(shè)定障礙物，并在場(chǎng)景中標(biāo)定指定的移動(dòng)軌跡，獲取本文方法的重定位結(jié)果，如圖5所示。

圖5 不同環(huán)境中的機(jī)器人重定位結(jié)果

對(duì)圖5測(cè)試結(jié)果實(shí)行分析可知：在室外環(huán)境中，指定的移動(dòng)軌跡中存在交叉現(xiàn)象，即便如此，本文方法依據(jù)可靠完成指定移動(dòng)軌跡的重定位，沒(méi)有發(fā)生無(wú)法定位的情況；在室內(nèi)環(huán)境中，指定移動(dòng)軌跡存在多個(gè)房間中，本文方法依據(jù)能夠精準(zhǔn)完成移動(dòng)軌跡的標(biāo)定，并且沒(méi)有受到障礙物的影響。因此，本文方法具有極好的機(jī)器人移動(dòng)重定位效果，可應(yīng)用在不同的環(huán)境中。

3 結(jié)語(yǔ)

重定位是機(jī)器人在移動(dòng)過(guò)程中完成自主導(dǎo)航的重要部分，其依據(jù)相機(jī)標(biāo)定系統(tǒng)獲取的環(huán)境圖像信息為依據(jù)，確定機(jī)器人在場(chǎng)景中的位置。為保證機(jī)器人移動(dòng)重定位的精度，本文研究基于手眼標(biāo)定算法的移動(dòng)機(jī)器人重定位方法，該方法結(jié)合手眼標(biāo)定模型以及深度學(xué)習(xí)多任務(wù)網(wǎng)絡(luò)完成重定位。經(jīng)測(cè)試：本文所提方法的應(yīng)用性能良好，可靠完成機(jī)器人的手眼標(biāo)定，獲取其旋轉(zhuǎn)和平移矩陣，得出標(biāo)定位置；并且定位結(jié)果的歐式距離誤差較小，滿足應(yīng)用需求，除此之外，機(jī)器人在移動(dòng)過(guò)程中的重定位結(jié)果可靠，精準(zhǔn)完成標(biāo)定點(diǎn)的重定位；且適用于不同的環(huán)境中，應(yīng)用性良好。