基于ROS的送餐機(jī)器人自主抓取實(shí)現(xiàn)

方明輝，曹夢(mèng)祥，袁薪凱，晏林沖，崔永利，孫一鵬

（1.哈爾濱理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，黑龍江 哈爾濱 150080；2.哈爾濱北方防務(wù)裝備股份有限公司，黑龍江 哈爾濱 150080）

近年來(lái)，隨著人工智能、物聯(lián)網(wǎng)、5G等技術(shù)的不斷發(fā)展，機(jī)器人的研發(fā)與創(chuàng)新都得到了空前的發(fā)展與革新。伴隨著智能終端服務(wù)化的空前發(fā)展，服務(wù)型機(jī)器人已然在慢慢地融入人們的日常生活。

以餐飲行業(yè)為例，從送餐的本質(zhì)上來(lái)講，就是一項(xiàng)重復(fù)性的工作，與很多組裝性的工序相類(lèi)似，這就會(huì)很容易被機(jī)器人代替。就目前市場(chǎng)上送餐機(jī)器人的現(xiàn)狀來(lái)看，其已經(jīng)很好地融入了人們的生活。服務(wù)型機(jī)器人與餐飲行業(yè)的有效碰撞，在為餐飲服務(wù)提供特色的同時(shí)，也引導(dǎo)餐飲行業(yè)的經(jīng)營(yíng)模式走向智能化、高效化。隨著智能硬件高度融入人類(lèi)的生活習(xí)慣，“智能服務(wù)機(jī)器人+餐飲”的模式必將是未來(lái)的發(fā)展趨勢(shì)，送餐機(jī)器人的發(fā)展前景十分明朗?，F(xiàn)在，全國(guó)有大概800多萬(wàn)家餐廳，但是送餐機(jī)器人滲透到餐廳中的比例不到1%，這些數(shù)據(jù)都表明了在未來(lái)“智能服務(wù)機(jī)器人+餐飲”的領(lǐng)域還有很大的發(fā)展空間[1]。

此外，在機(jī)器人設(shè)計(jì)研發(fā)中，為了解決各部分的兼容性，需要采用一個(gè)統(tǒng)一的平臺(tái)，即ROS。該平臺(tái)適用于機(jī)器人的相關(guān)開(kāi)發(fā)研究工作，這個(gè)框架提高了機(jī)器人各部件之間的耦合度，為這些部件提供了通信架構(gòu)。除此之外，ROS還包含了大量實(shí)用且便捷的工具軟件和軟件包，如QT工具、開(kāi)發(fā)工具RVIZ等。因此，使用ROS開(kāi)發(fā)不僅具有良好的擴(kuò)展性，還能應(yīng)用豐富的軟件包來(lái)強(qiáng)化機(jī)器人的功能[2]。

1 主要任務(wù)

本文主要研究如何利用搭載于送餐機(jī)器人上的六自由度機(jī)械臂與攝像頭，實(shí)現(xiàn)機(jī)器人自主抓取的功能，從而實(shí)現(xiàn)機(jī)器人的高度自動(dòng)化與智能化。

機(jī)器人搭載了一個(gè)深度視覺(jué)攝像頭如圖1所示，和一個(gè)普通的1080p的攝像頭，利用ORB_SLAM算法可以高效地建立三維空間模型，并且可以依據(jù)建模結(jié)果獲得某點(diǎn)的空間坐標(biāo)值，傳達(dá)至機(jī)器人主機(jī)進(jìn)行后續(xù)處理。

圖1 Kinect深度學(xué)習(xí)攝像頭

機(jī)器人利用六自由度機(jī)械臂如圖2所示，完成抓取動(dòng)作，使用usb與機(jī)械臂lsc主控板進(jìn)行連接，當(dāng)攝像頭建模后將采集空間坐標(biāo)參數(shù)發(fā)送到機(jī)器人主機(jī)后，由主機(jī)轉(zhuǎn)換為robot_arm/cmdstring格式的topic，這個(gè)topic內(nèi)容為控制命令，最后由robot_arm節(jié)點(diǎn)負(fù)責(zé)將這個(gè)topic內(nèi)容經(jīng)由usb hid協(xié)議發(fā)送給機(jī)械臂主控板，完成抓取的操作。

圖2 六自由度機(jī)械臂

2 六自由度機(jī)械臂的二次開(kāi)發(fā)

實(shí)際開(kāi)發(fā)過(guò)程中，機(jī)械臂通過(guò)串口與上位機(jī)進(jìn)行通信，通信的有關(guān)參數(shù)見(jiàn)表1。

表1 通信有關(guān)參數(shù)

開(kāi)發(fā)中通用的指令格式見(jiàn)表2。其中幀頭表示如果收到連續(xù)的2個(gè)0x55，就代表有數(shù)據(jù)包到達(dá)，數(shù)據(jù)長(zhǎng)度為參數(shù)的數(shù)量加2（即N+2），指令表示各種控制信息，參數(shù)表示需要補(bǔ)充的一些附加信息。

表2 通用指令格式

2.1 用戶(hù)向控制板發(fā)送數(shù)據(jù)

（1）指令名CMD_SERVO_MOVE指令值0x03數(shù)據(jù)長(zhǎng)度值：要轉(zhuǎn)動(dòng)的舵機(jī)的數(shù)量×3+5。

說(shuō)明：利用指令對(duì)任意數(shù)量的舵機(jī)轉(zhuǎn)動(dòng)進(jìn)行控制。

參數(shù)1：要轉(zhuǎn)動(dòng)舵機(jī)的數(shù)量；

參數(shù)2：舵機(jī)轉(zhuǎn)動(dòng)時(shí)間的十六進(jìn)制下的低兩位；

參數(shù)3：舵機(jī)轉(zhuǎn)動(dòng)時(shí)間的十六進(jìn)制下的高兩位；

參數(shù)4：轉(zhuǎn)動(dòng)舵機(jī)的ID號(hào)；

參數(shù)5：舵機(jī)轉(zhuǎn)動(dòng)角度位置的十六進(jìn)制下的低兩位；

參數(shù)6：舵機(jī)轉(zhuǎn)動(dòng)角度位置的十六進(jìn)制下的高兩位；

參數(shù)N，N+1，N+2：與參數(shù)4，5，6的意義相同，用來(lái)控制其他舵機(jī)轉(zhuǎn)動(dòng)的角度位置。

比如控制4號(hào)和7號(hào)舵機(jī)在2 500 ms內(nèi)，4號(hào)轉(zhuǎn)動(dòng)到2 100的位置，7號(hào)轉(zhuǎn)動(dòng)到1 700的位置，見(jiàn)表3。

表3 CMD_SERVO_MOVE指令示例

（2）指令名CMD_ACTION_GROUP_RUN指令值0x06數(shù)據(jù)長(zhǎng)度值：5。

說(shuō)明：使機(jī)械臂運(yùn)行該動(dòng)作組，并且可以設(shè)置該動(dòng)作組的運(yùn)行次數(shù)，當(dāng)參數(shù)值為0時(shí)即表示該動(dòng)作組可以無(wú)限次運(yùn)行。

參數(shù)1：準(zhǔn)備運(yùn)行的動(dòng)作組的編號(hào)；

參數(shù)2：動(dòng)作組執(zhí)行次數(shù)的十六進(jìn)制下的低兩位；

參數(shù)3：動(dòng)作組執(zhí)行次數(shù)的十六進(jìn)制下的高兩位。

比如控制4號(hào)動(dòng)作組運(yùn)行5次，見(jiàn)表4。

表4 CMD_ACTION_GROUP_RUN指令示例

（3）指令名CMD_ACTION_GROUP_STOP指令值0x07數(shù)據(jù)長(zhǎng)度值：2。

說(shuō)明：中斷當(dāng)前正在運(yùn)行的動(dòng)作組。若當(dāng)前機(jī)械臂沒(méi)有執(zhí)行動(dòng)作，則無(wú)論發(fā)送幾次命令都不會(huì)對(duì)機(jī)械臂產(chǎn)生影響。

參數(shù)：無(wú)參數(shù)。

停止當(dāng)前正在運(yùn)行的動(dòng)作組，見(jiàn)表5。

表5 CMD_ACTION_GROUP_STOP指令示例

2.2 控制板向用戶(hù)返回?cái)?shù)據(jù)

指令名CMD_ACTION_GROUP_COMPLETE指令值0x08數(shù)據(jù)長(zhǎng)度值：5。

說(shuō)明：當(dāng)機(jī)械臂將某個(gè)動(dòng)作組正常執(zhí)行完畢后（沒(méi)有被強(qiáng)行停止運(yùn)行，而是本身運(yùn)行的時(shí)間到了，自然運(yùn)行結(jié)束會(huì)返回該指令）。

參數(shù)1：運(yùn)行動(dòng)作組的編號(hào)；

參數(shù)2：動(dòng)作組準(zhǔn)備執(zhí)行次數(shù)的十六進(jìn)制下的低兩位；

參數(shù)3：動(dòng)作組準(zhǔn)備執(zhí)行次數(shù)的十六進(jìn)制下的高兩位。

比如當(dāng)9號(hào)動(dòng)作組將要執(zhí)行8次，自然運(yùn)行結(jié)束后，指令見(jiàn)表6。

表6 CMD_ACTION_GROUP_COMPLETE指令示例

3 基于Tsai-Lenz算法的自主抓取設(shè)計(jì)

送餐機(jī)器人通過(guò)Kinect攝像頭和六自由度機(jī)械臂結(jié)合，基于手眼標(biāo)定法完成自主抓取工作。手眼標(biāo)定法根據(jù)攝像頭固定的位置可分為兩種類(lèi)型，一種是攝像頭固定在機(jī)械臂之上，稱(chēng)為眼在手上（eye-in-hand），另一種是攝像頭固定在機(jī)械臂之外，稱(chēng)為眼在手外（eye-to-hand）。該機(jī)器人由于攝像頭與機(jī)械臂分離，所以采用了eye-to-hand方式。

Tsai-Lenz算法是手眼標(biāo)定中最常見(jiàn)的算法之一，其通常用于eye-in-hand的手眼固定形式，但是同樣也能夠用于eye-to-hand的情況，具有成熟可用的工具包，該方法借助標(biāo)定板，通過(guò)求解線(xiàn)性方程的方法求取手眼關(guān)系。

在整個(gè)抓取過(guò)程中，主要過(guò)程如下[3]：

（1）物體識(shí)別：在圖像中檢測(cè)到物體。

（2）位姿估計(jì)：得到物體在相機(jī)坐標(biāo)系下的位置和姿態(tài)。

（3）相機(jī)擬定：對(duì)物體在相機(jī)坐標(biāo)系下的坐標(biāo)進(jìn)行轉(zhuǎn)換，得到其在機(jī)器人坐標(biāo)系的坐標(biāo)。

用H表示坐標(biāo)變換（H指homogeneous matrices齊次變換矩陣）。例如表示i時(shí)刻下機(jī)械臂（gripper）的坐標(biāo)變換：

已知多組：

（1）機(jī)械臂在機(jī)器人坐標(biāo)系中的坐標(biāo)：

（2）標(biāo)定板在相機(jī)坐標(biāo)系中的坐標(biāo)：

求機(jī)械臂和相機(jī)之間的位姿關(guān)系：

設(shè)：

即可得到：

根據(jù)Tsai-Lenz算法即可求解X，其核心等式如下：

在得到機(jī)器人坐標(biāo)系下的物體坐標(biāo)后，根據(jù)坐標(biāo)進(jìn)行動(dòng)作規(guī)劃，生成動(dòng)作指令后并發(fā)布到機(jī)械臂執(zhí)行。

4 結(jié)束語(yǔ)

本文介紹了一種送餐機(jī)器人自主抓取物品的實(shí)現(xiàn)方案，從實(shí)際角度出發(fā)，立足于餐飲行業(yè)，將攝像頭與六自由度機(jī)械臂的應(yīng)用進(jìn)行了結(jié)合，可以實(shí)現(xiàn)自主抓取食物的功能，具有一定的現(xiàn)實(shí)意義和參考價(jià)值。