陳莊浩， 孫亦劼， 蔣文萍， 江永林

(上海應(yīng)用技術(shù)大學(xué) 電氣與電子工程學(xué)院， 上海 201418)

近年來，機械臂在國內(nèi)外已有了較大的發(fā)展，應(yīng)用的領(lǐng)域也很廣泛，但機械臂的控制系統(tǒng)、軌跡規(guī)劃還處在一個快速發(fā)展時期。隨著人工智能、深度學(xué)習(xí)、神經(jīng)網(wǎng)絡(luò)、大數(shù)據(jù)分析等科學(xué)技術(shù)的快速興起，機械臂相關(guān)技術(shù)迅猛發(fā)展。移動機械臂是一個集環(huán)境感知、動態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)[1]。它集中了傳感器技術(shù)、信息處理、電子工程、計算機工程、自動化控制工程以及人工智能等多學(xué)科的研究成果，是機電一體化、計算機科學(xué)完美結(jié)合的典型代表[2]，是目前科學(xué)技術(shù)發(fā)展最活躍的領(lǐng)域之一。隨著移動機械臂性能不斷地完善，其應(yīng)用范圍大幅度的擴展，不僅在工業(yè)、農(nóng)業(yè)、醫(yī)療、服務(wù)等行業(yè)中得到廣泛應(yīng)用，而且在城市安全、國防和空間探測領(lǐng)域等高危場合得到很好的推廣應(yīng)用。隨著科學(xué)技術(shù)的飛速發(fā)展，人類的研究和活動領(lǐng)域已經(jīng)由陸地擴展到海洋和太空。將機械臂搭載到移動平臺上進行空間探測和開發(fā)，是21世紀世界各國探索開發(fā)空間資源的主要手段之一。

本文的移動機械臂將一個六自由度的機械臂安裝于TurtlrBot2機器人之上。通過TurtleBot2機器人的軌跡規(guī)劃，配合六自由度機械臂的手動抓取和自動抓取來實現(xiàn)一系列的功能。

1 移動平臺的硬件系統(tǒng)及其模型

1.1 TurtleBot2移動機器人

TurtleBot2移動機器人是一款帶有開源軟件的低成本個人移動機器人。TurtleBot2機器人本體由YUJIN Kobuki底座，2 200 mA·h的電池和65 W快充和硬件安裝套件組成[3]，如圖1所示。TurtleBot2移動機器人通過與Kinect傳感器與筆記本電腦的組裝，可以運行標準的同步定位和映射(simultaneous localization and mapping，SLAM)算法來構(gòu)建1個地圖，并且可以從1個筆記本電腦或Android的智能手機進行遠程控制并進行軌跡規(guī)劃[4]。

圖1 TurtleBot2 硬件組成Fig.1 Hardware composition of TurtleBot2

1.2 移動機器人模型

TurtleBot2是一種差動式2輪移動機器人，在它的底座上有2個結(jié)構(gòu)相同的驅(qū)動輪，驅(qū)動輪轉(zhuǎn)動為機器人提供驅(qū)使其進行運動的作用力，在驅(qū)動輪前方有1個隨動輪，隨動輪起到支撐的作用，避免機器人翻倒，并不起驅(qū)動作用[5]。移動機器人模型如圖2所示。

圖2 移動機器人模型Fig.2 Mobile robot model

移動機器人的狀態(tài)由2個驅(qū)動輪中點M所在的坐標系的位置和導(dǎo)航角θ來決定。令p=(xyθ)T，q=(vω)T，其中(xy)分別為移動機器人的線速度和角速度。

移動機器人的運動學(xué)方程為

(1)

移動機器人的位姿從p=(xyθ)T移動到位姿qr=(xryrθr)T，移動機器人在新的坐標系Xe-Ye中的坐標為

pe=(xeyeθe)T

(2)

式中，θe=θr-θ。

設(shè)新坐標系Xe-Ye與X-Y之間的夾角為θ。根據(jù)坐標轉(zhuǎn)換公式，可得描述移動機器人位姿的誤差方程為：

(3)

位姿的誤差微分方程為：

(4)

1.3 Kinect傳感器

一臺Kinect上裝載了3個攝像頭，分別為中間的彩色攝影頭，最左邊的是紅外線發(fā)射器和最右邊的紅外線CMOS攝像頭[7]，左右兩邊的鏡頭組成了3D深度傳感器，提供全身3D運動捕捉、人臉識別[6]。Kinect傳感器如圖3所示。

圖3 Kinect傳感器Fig.3 Kinect sensor

2 機械臂的硬件設(shè)計及其運動參數(shù)

2.1 機械臂的硬件設(shè)計

六自由度機械手臂控制系統(tǒng)的硬件主要由舵機控制電路、TBS2701舵機、無線藍牙模塊、STM32單片機系統(tǒng)、超聲波測距模塊和設(shè)計電路組成。通過超聲波來確定物體與機械臂之間的距離，當滿足一定條件的情況下，微控制器就發(fā)出信號使六路數(shù)字舵機[8]工作抓取目標物。再通過無線藍牙模塊與電腦進行通信，這樣就可以通過人機交互界面來觀察并調(diào)整機械臂的運動狀態(tài)，以完成對目標物的抓取。硬件控制系統(tǒng)框圖如圖4所示。

圖4 機械臂硬件系統(tǒng)方框圖Fig.4 Block diagram of the robotic arm hardware system

2.2 機械臂的運動參數(shù)

通過對機械臂的6個旋轉(zhuǎn)關(guān)節(jié)分別依據(jù)右手法以及D-H 原則來建立各個關(guān)節(jié)的坐標系。將各個連桿的關(guān)系通過D-H參數(shù)法需要用到4個最基本的參數(shù)來進行表達，即αi、ai、di和θi[9]。本文研究的六自由度機械臂連桿參數(shù)見表1。

表1 機械臂連桿參數(shù)Tab.1 Link parameters of manipulator

3 移動機械臂的整體控制

3.1 移動機械臂的避障算法

3.1.1 Dijkstra算法

Dijkstra算法的基本思想：設(shè)置一個集合S存放已經(jīng)找到最短路徑的頂點，S的初始狀態(tài)只包含源點v，對vi∈V-S，假設(shè)從源點v到vi的有向邊為最短路徑。以后每求得一條最短路徑v,…,vk就將vk加入集合S中，并將路徑v,…,vk,vi與原來的假設(shè)相比較，取路徑長度較小者為最短路徑。重復(fù)上述過程，直到集合V中全部頂點加入到集合S中。

3.1.2 DWA算法

在速度空間(v,ω)中采樣多組速度，并模擬這些速度在一定時間內(nèi)的運動軌跡，再通過一個評價函數(shù)對這些軌跡打分，最優(yōu)的速度被選擇出來發(fā)送給移動底座。

首先，建立移動機器人運動學(xué)模型

(5)

然后進行速度采樣。在速度(v,ω)的二維空間中，存在無窮多組速度。但根據(jù)機器人本身的限制和環(huán)境的限制可以將采樣速度控制在一定范圍內(nèi)。

移動機器人受自身最大、最小速度的限制：

vm={v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(6)

移動機器人受電機性能影響：

vd={(v,ω)|v∈[vc-vbΔt,vc+vaΔt]∧

ω∈[ωc-ωbΔt,ωc+ωaΔt]}

(7)

最后是評價函數(shù)：

G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+

γ·velocity(v,ω))

(8)

3.2 移動機械臂的地圖構(gòu)建

移動機器人通過ROS具有在未知環(huán)境中工作和運動的功能，來檢測掃描四周環(huán)境，從而創(chuàng)建出不同環(huán)境下的地圖[10]。圖5(a)為上海應(yīng)用技術(shù)大學(xué)第一學(xué)科樓的某層樓道和移動機器人的實照。通過對樓道的掃描得到圖5(b)所示的掃描圖像。其中凹陷處為樓道兩側(cè)辦公室的門，樓道中間的小點為由電腦主機設(shè)置的障礙物用于避障操作。

圖5 (a)樓道與移動機械臂的實景圖；(b)由Kinect傳感器掃描得到的圖像Fig.5 (a) Real map of the corridor and mobile manipulator, (b) Image scanned by Kinect sensor

3.3 移動機械臂的避障

在使用Python進行避障程序編寫前，要使用到Pygame庫。移動機械臂避障流程圖，如圖6所示。

圖6 移動機械臂避障流程圖Fig.6 Flow chart of mobile manipulator obstacle avoidance

Pygame作為一個游戲模塊具備了繪制圖形、顯示圖片、動畫效果、外設(shè)交互、播放聲音和碰撞檢測等功能。仿真中使用小球代替小車。通過get_rect()得到小球的位置，返回一個surface對象，進入循環(huán)，進行碰撞檢測[11]。當小球與障礙物之間的距離小于障礙物半徑時，會發(fā)生碰撞。所以此時需要提前修改軌跡。將障礙物視作一個為10 cm的圓，當小球與障礙物之間距離小于80 cm時，速度為[3,2.5cos(t) cm/s]。程序通過改變surface對象的位置實現(xiàn)小球的運動。在小球避開障礙物后小球?qū)⒒氐皆瓉淼倪\動軌跡上。Python上的仿真示例，如圖7所示。

圖7 小球在Python上的避障仿真Fig.7 Obstacle avoidance simulation of small ball on Python

在完成前提工作，創(chuàng)建地圖后，把機器人放置在地圖的某一點上。在rviz中選擇2D Pose Estimate和選擇機器人的初始位姿，調(diào)用避障程序。機器人會向前運動3 m，并且避開路線上的障礙物。具體實際避障情況如圖8所示。

圖8 Turtlebot2移動機器人的避障(a)移動機械臂位于起始位置；(b)移動機械臂避開障礙物；(c)移動機械臂運動到障礙物3 m后處Fig.8 Obstacle avoidance for TurtleBot2 mobile robot

3.4 移動機械臂的軌跡規(guī)劃和自動抓取

3.4.1 移動機械臂的軌跡規(guī)劃

軌跡規(guī)劃實際上是對機械臂在空間內(nèi)的運動進行一次規(guī)劃[12]。本文使用3次多項式插值方法。 3次多項式插值法中每一小段路徑通過之前的3次多項式求解可以得出下一段路徑中的初始位置并用一組關(guān)節(jié)角進行表示[13]。需要確定的是插入的點的期望位置，速度以及所用的時間。先選取若干中間點處的期望位置、速度和時間來進行3次多項式的驗證，如表2所示。

表2 3次多項式插值法參數(shù)Tab.2 Cubic polynomial interpolation parameters

以下由MATLAB來解決3次多項式的解，如圖9所示。

圖9 MATLAB的三次多項式解的仿真Fig.9 The simulation of cubic polynomial of MATLAB

圖10 機械臂自動抓取主程序流程圖Fig.10 Main program flow chart of manipulator automatic capture

3.4.2 移動機械臂的自動抓取

六自由度機械臂系統(tǒng)功能比較復(fù)雜，在對機械臂進行分析時，將復(fù)雜的系統(tǒng)分為了對多個模塊進行封裝的方式來進行編程[14]。這里僅對主程序進行講解，程序流程圖如圖10所示。程序首先完成初始化過程，然后一直循環(huán)主程序的內(nèi)容，看是否有來自串口數(shù)據(jù)的指令，如果需要重新編輯動作，則運動調(diào)試程序，獲取動作組指令，存儲數(shù)據(jù)，隨后檢測超聲波是否有檢測到目標位置，如果存在需要抓取的物體，分析處理動作組指令，并控制舵機執(zhí)行指令。

在遙控器的控制下，實現(xiàn)了手動控制和自動控制2個模式下的抓取物塊實現(xiàn)。手動模式下，可以實現(xiàn)對機械臂各個關(guān)節(jié)的控制，完成抓取并通過上位機觀察各個關(guān)節(jié)的轉(zhuǎn)角。自動模式下，通過超聲波的距離檢測判斷是否進行抓取，執(zhí)行調(diào)試時寫入的動作組，完成了自動模式下的自動抓取。圖11所示為機械臂通過超聲波測距模塊自動抓取小物塊過程。

圖11 機械臂自動抓取過程Fig.11 Process of manipulator automatic capture

4 結(jié) 語

本文將六自由度機械臂安裝在差動式兩輪移動機器人(Turtlebot2)之上，通過對移動機器人的運動學(xué)模型建立并進行了仿真，以及對Dijkstra算法和DWA算法的分析，設(shè)計了一種由Kinect傳感器、TurtleBot2移動機器人和筆記本電腦組成的移動工作站的運動控制，包括基本的路徑規(guī)劃、地圖構(gòu)建和避障運動的實現(xiàn)。搭載上六自由度機械臂通過超聲波測距模塊實現(xiàn)的對物體的位置判斷，能夠自動并準確地定位物體位置并進行抓取、放置到指定區(qū)域的操作。由于本文實現(xiàn)的是移動機器人與機械臂的協(xié)同運動控制，因此即便是移動機械臂與物塊間存在一定的距離，也可以通過以上功能實現(xiàn)對物體遠距離的定位并移動到物塊附近，抓取并放置到人工指定的區(qū)域。