移動機械臂整體控制研究

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

(上海應用技術大學 電氣與電子工程學院， 上海 201418)

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

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

1 移動平臺的硬件系統及其模型

1.1 TurtleBot2移動機器人

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

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

1.2 移動機器人模型

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

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

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

移動機器人的運動學方程為

(1)

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

pe=(xeyeθe)T

(2)

式中，θe=θr-θ。

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

(3)

位姿的誤差微分方程為：

(4)

1.3 Kinect傳感器

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

圖3 Kinect傳感器Fig.3 Kinect sensor

2 機械臂的硬件設計及其運動參數

2.1 機械臂的硬件設計

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

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

2.2 機械臂的運動參數

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

表1 機械臂連桿參數Tab.1 Link parameters of manipulator

3 移動機械臂的整體控制

3.1 移動機械臂的避障算法

3.1.1 Dijkstra算法

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

3.1.2 DWA算法

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

首先，建立移動機器人運動學模型

(5)

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

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

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

(6)

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

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

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

(7)

最后是評價函數：

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

γ·velocity(v,ω))

(8)

3.2 移動機械臂的地圖構建

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

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

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

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

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

3.4 移動機械臂的軌跡規劃和自動抓取

3.4.1 移動機械臂的軌跡規劃

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

表2 3次多項式插值法參數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 移動機械臂的自動抓取

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

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

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

4 結 語

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