基于ROS的送餐機器人自主抓取實現

方明輝，曹夢祥，袁薪凱，晏林沖，崔永利，孫一鵬

（1.哈爾濱理工大學 計算機科學與技術學院，黑龍江 哈爾濱 150080；2.哈爾濱北方防務裝備股份有限公司，黑龍江 哈爾濱 150080）

近年來，隨著人工智能、物聯網、5G等技術的不斷發展，機器人的研發與創新都得到了空前的發展與革新。伴隨著智能終端服務化的空前發展，服務型機器人已然在慢慢地融入人們的日常生活。

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

此外，在機器人設計研發中，為了解決各部分的兼容性，需要采用一個統一的平臺，即ROS。該平臺適用于機器人的相關開發研究工作，這個框架提高了機器人各部件之間的耦合度，為這些部件提供了通信架構。除此之外，ROS還包含了大量實用且便捷的工具軟件和軟件包，如QT工具、開發工具RVIZ等。因此，使用ROS開發不僅具有良好的擴展性，還能應用豐富的軟件包來強化機器人的功能[2]。

1 主要任務

本文主要研究如何利用搭載于送餐機器人上的六自由度機械臂與攝像頭，實現機器人自主抓取的功能，從而實現機器人的高度自動化與智能化。

機器人搭載了一個深度視覺攝像頭如圖1所示，和一個普通的1080p的攝像頭，利用ORB_SLAM算法可以高效地建立三維空間模型，并且可以依據建模結果獲得某點的空間坐標值，傳達至機器人主機進行后續處理。

圖1 Kinect深度學習攝像頭

機器人利用六自由度機械臂如圖2所示，完成抓取動作，使用usb與機械臂lsc主控板進行連接，當攝像頭建模后將采集空間坐標參數發送到機器人主機后，由主機轉換為robot_arm/cmdstring格式的topic，這個topic內容為控制命令，最后由robot_arm節點負責將這個topic內容經由usb hid協議發送給機械臂主控板，完成抓取的操作。

圖2 六自由度機械臂

2 六自由度機械臂的二次開發

實際開發過程中，機械臂通過串口與上位機進行通信，通信的有關參數見表1。

表1 通信有關參數

開發中通用的指令格式見表2。其中幀頭表示如果收到連續的2個0x55，就代表有數據包到達，數據長度為參數的數量加2（即N+2），指令表示各種控制信息，參數表示需要補充的一些附加信息。

表2 通用指令格式

2.1 用戶向控制板發送數據

（1）指令名CMD_SERVO_MOVE指令值0x03數據長度值：要轉動的舵機的數量×3+5。

說明：利用指令對任意數量的舵機轉動進行控制。

參數1：要轉動舵機的數量；

參數2：舵機轉動時間的十六進制下的低兩位；

參數3：舵機轉動時間的十六進制下的高兩位；

參數4：轉動舵機的ID號；

參數5：舵機轉動角度位置的十六進制下的低兩位；

參數6：舵機轉動角度位置的十六進制下的高兩位；

參數N，N+1，N+2：與參數4，5，6的意義相同，用來控制其他舵機轉動的角度位置。

比如控制4號和7號舵機在2 500 ms內，4號轉動到2 100的位置，7號轉動到1 700的位置，見表3。

表3 CMD_SERVO_MOVE指令示例

（2）指令名CMD_ACTION_GROUP_RUN指令值0x06數據長度值：5。

說明：使機械臂運行該動作組，并且可以設置該動作組的運行次數，當參數值為0時即表示該動作組可以無限次運行。

參數1：準備運行的動作組的編號；

參數2：動作組執行次數的十六進制下的低兩位；

參數3：動作組執行次數的十六進制下的高兩位。

比如控制4號動作組運行5次，見表4。

表4 CMD_ACTION_GROUP_RUN指令示例

（3）指令名CMD_ACTION_GROUP_STOP指令值0x07數據長度值：2。

說明：中斷當前正在運行的動作組。若當前機械臂沒有執行動作，則無論發送幾次命令都不會對機械臂產生影響。

參數：無參數。

停止當前正在運行的動作組，見表5。

表5 CMD_ACTION_GROUP_STOP指令示例

2.2 控制板向用戶返回數據

指令名CMD_ACTION_GROUP_COMPLETE指令值0x08數據長度值：5。

說明：當機械臂將某個動作組正常執行完畢后（沒有被強行停止運行，而是本身運行的時間到了，自然運行結束會返回該指令）。

參數1：運行動作組的編號；

參數2：動作組準備執行次數的十六進制下的低兩位；

參數3：動作組準備執行次數的十六進制下的高兩位。

比如當9號動作組將要執行8次，自然運行結束后，指令見表6。

表6 CMD_ACTION_GROUP_COMPLETE指令示例

3 基于Tsai-Lenz算法的自主抓取設計

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

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

在整個抓取過程中，主要過程如下[3]：

（1）物體識別：在圖像中檢測到物體。

（2）位姿估計：得到物體在相機坐標系下的位置和姿態。

（3）相機擬定：對物體在相機坐標系下的坐標進行轉換，得到其在機器人坐標系的坐標。

用H表示坐標變換（H指homogeneous matrices齊次變換矩陣）。例如表示i時刻下機械臂（gripper）的坐標變換：

已知多組：

（1）機械臂在機器人坐標系中的坐標：

（2）標定板在相機坐標系中的坐標：

求機械臂和相機之間的位姿關系：

設：

即可得到：

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

在得到機器人坐標系下的物體坐標后，根據坐標進行動作規劃，生成動作指令后并發布到機械臂執行。

4 結束語

本文介紹了一種送餐機器人自主抓取物品的實現方案，從實際角度出發，立足于餐飲行業，將攝像頭與六自由度機械臂的應用進行了結合，可以實現自主抓取食物的功能，具有一定的現實意義和參考價值。