Dobot機械臂運動軌跡規(guī)劃與控制

唐雯雯，盧 宇，王明芬

（福建師范大學協(xié)和學院，福州 350117）

0 引言

隨著工業(yè)自動化的發(fā)展，機械臂在產(chǎn)業(yè)自動化方面已經(jīng)得到了相當廣泛地應(yīng)用。機械臂在復(fù)雜、枯燥甚至是惡劣環(huán)境下，無論是完成效率以及完成精確性都是人類所無法比擬的，也因此，機械臂在人類的生產(chǎn)和生活中發(fā)揮著越來越重要的作用。機械臂可替代人的繁重勞動以實現(xiàn)生產(chǎn)的機械化和自動化，能在有害環(huán)境下操作以保護人身安全，因而廣泛用于機械制造、冶金、電子、輕工和原子能等行業(yè)。針對目前機器人在各個領(lǐng)域的應(yīng)用情況，機器人的運動規(guī)劃控制已經(jīng)成為了一個重要的研究方向，并具有極大的實用經(jīng)濟價值。但機械臂的控制精度、響應(yīng)時間、系統(tǒng)魯棒性、使用壽命、維護周期等問題都亟待解決。目前國內(nèi)外學者對機械臂控制的研究有很多，但對Dobot Magician這種高精度輕量型智能實訓機械臂的研究幾乎沒有，因此本文對于Dobot機械臂的研究開發(fā)具有較高的市場價值。

本文旨在研究Dobot機械臂的軌跡規(guī)劃問題。首先通過建立機械臂模型并進行正運動學方程求解，再采用數(shù)值法確定機械臂在空間中的活動范圍，使機械臂能夠更好地抓取目標物體；然后利用五次多項式插值法對Dobot機械臂進行運動軌跡規(guī)劃。該研究依據(jù)Dobot機械臂高精度且輕量的性能特點，設(shè)計了一步步功能測試實驗，開展了對Dobot機械臂控制方案的研究[1]。

1 Dobot機械臂正運動學分析

1.1 連桿坐標系的建立

Dobot機械臂是一種四自由度串聯(lián)開鏈式機械臂。為了描述機械臂各連桿之間的相對位置和方向關(guān)系，利用Denavit-Hartenberg參數(shù)建模法建立了關(guān)節(jié)坐標系[2]。其分布情況如圖1～2所示。針對Dobot Magician機械臂的連桿機構(gòu)，其D-H參數(shù)如表1所示。

圖1 機械臂D-H建模連桿坐標系

圖2 連桿關(guān)節(jié)

表1 D-H參數(shù)

1.2 Dobot正運動學求解

正運動學求解是指以Dobot機械臂各個關(guān)節(jié)變量數(shù)值為已知條件，通過齊次變換方程正向求解其末端在空間中的位姿[3]，為了表示方便，令：

由D-H轉(zhuǎn)換法則，得各坐標系轉(zhuǎn)換矩陣分別為

1.3 Dobot機械臂運動軌跡規(guī)劃

在關(guān)節(jié)空間中，這里使用五次多項式插值法進行Dobot機械臂運動軌跡規(guī)劃的研究。其中需要6個約束條件，包括運動段起點和終點的角度、角速度及角加速度。已知機器人某一關(guān)節(jié)i在初始時刻t0的初始角度為θi(0)，終止時刻tf終止角度θi(tf)。假設(shè)這6個約束條件分別為：

設(shè)由這6個約束條件唯一確定的五次多項式的形式為：

其中，系數(shù)a0，a1，...，a5必須滿足以下6個條件，即：

將6個已知的約束條件代入方程，聯(lián)立解得：

因此，由這6個已知條件，可求得五次多項式的系數(shù)，也就是求得了第i關(guān)節(jié)在每一時刻的運動角度、角速度以及角加速度，即得到了第i關(guān)節(jié)的運動軌跡[4]。

2 Matlab仿真

2.1 正運動學結(jié)果驗證

在Matlab環(huán)境下，借助機械臂工具箱Robotics Toolbox建立了四自由度機械臂模型。4個參數(shù)值共同確定了末端執(zhí)行器的位姿。通過驅(qū)動圖對應(yīng)的4個滑塊即改變了機械臂末端執(zhí)行器的位置。Dobot初始姿態(tài)如圖3所示，當機械臂關(guān)節(jié)角q1=0，q2=0，q3=0，q4=0時，機器人末端位置為［343.05 0143.0］，單位為cm，X-Y-Z固定角坐標系的回轉(zhuǎn)角、俯仰角和偏轉(zhuǎn)角分別為[0 0 90]，單位為°[5]。

圖3 初始位姿仿真

經(jīng)正運動學求解初始姿態(tài)為：

Dobot末端姿態(tài)如圖4所示，當機械臂關(guān)節(jié)角q1=π∕6，q2=π∕6，q3=π∕6，q4=π∕6時，機器人末端位置為[154.048，111.922，112.856]，單位為cm，X-Y-Z固定角坐標系的回轉(zhuǎn)角、俯仰角和偏轉(zhuǎn)角分別為[90-36-90]，單位為°。經(jīng)正運動學求解初始姿態(tài)為[6]：

圖4 末端位置仿真

矩陣演算結(jié)果中的姿態(tài)矩陣和位置矩陣均與Matlab中Dobot機械臂仿真模型的結(jié)果一致，從而驗證了正運動學分析的正確性[7]。

2.2 關(guān)節(jié)空間規(guī)劃

假設(shè)各個關(guān)節(jié)運動時間間隔為5 s，各個關(guān)節(jié)初始和終止的角速度都為0，運動時間為40 s；利用五次多項式插值法的jtraj函數(shù)進行軌跡規(guī)劃，分別得到了機械臂從p1到p2各個關(guān)節(jié)的角度、角速度及角加速度與時間的關(guān)系曲線[8]。仿真程序如圖5所示。

圖5 仿真程序

矩陣q每一行表示在此時間采樣點上各關(guān)節(jié)的關(guān)節(jié)角度；qd和qdd每一行表示相應(yīng)關(guān)節(jié)的角速度與角加速度，初始和終止時刻的角速度及角加速度默認為0。init_ang和targ_ang分別表示機械臂各關(guān)節(jié)初始位置角度和終止位置角度；分別繪制了各個關(guān)節(jié)的角度、角速度及角加速度隨時間變化的曲線。由圖6易知，各關(guān)節(jié)角度值從起點到終點的變化曲線呈現(xiàn)五次多項式曲線變化，驗證了仿真參數(shù)的合理性和有效性。

圖6 機械臂末端執(zhí)行器坐標變化曲線

3 Dobot機械臂實驗研究

3.1 實驗研究方案

本研究通過對實體機械臂的控制接口及通信協(xié)議的分析，用python語言實現(xiàn)了Dobot機械臂的二次開發(fā)。由于Dobot機械臂底層代碼并不開源，進行二次開發(fā)、控制算法相關(guān)實驗研究時通過串口通信技術(shù)來接入PC機板控制器利用通信協(xié)議和Dobot控制器進行信息交互，對機械臂的運動控制進行了實驗研究[9]。

3.2 實驗硬件和軟件

此實驗硬件由計算機、Dobot機械臂、控制器、傳感器等組成整個控制算法實驗系統(tǒng)。上位機運行Visual Studio Code軟件，pyQt提供了Dobot各項功能的操作界面（圖7）。上位機與Dobot控制器建立了串口通訊、提供了用戶操作界面。Dobot控制器接收到上位機發(fā)送的控制命令，與機械臂之間建立了位置閉環(huán)控制，從而驅(qū)動Dobot機械臂的運動。

圖7 機械臂控制界面

3.3 點動功能研究

為深刻理解機械臂的軌跡規(guī)劃以及各部分的運動關(guān)系，在利用Matlab仿真運動之后，規(guī)劃了機械臂的末端沿設(shè)定的特定軌跡運動過程。首先輸入指令機械臂移動到指定坐標（x，y，z），并且獲取機械臂姿態(tài)檢測是否移動到（x，y，z），試驗結(jié)果顯示控制算法效果較為理想，機械臂可到達運動范圍內(nèi)任意指定的位置。

圖8 機械臂運動回零位置

圖9 機械臂運動終點位置

3.4 圓弧功能研究

圓弧實驗步驟如下。

（1）準備工作。打開電源，檢查機械臂指示燈，若是綠色則能正常工作。

（2）開啟下位機，開啟上位機控制軟件運行系統(tǒng)，并啟動畫畫模式。

（3）執(zhí)行回零功能，如圖10所示。為保證在工作范圍內(nèi)完成繪畫，選擇合適的姿態(tài)落筆到紙上。如圖11所示。

圖10 機械臂回零

圖11 調(diào)整機械臂筆尖位置

（4）設(shè)置畫畫的速度，打開pyCharm軟件，配置加速度選項，根據(jù)快慢調(diào)節(jié)，推薦參數(shù)50，輸入筆下壓的距離2 mm。運行繪畫設(shè)計程序，機械臂將沿著給定的期望位置運動，畫畫效果如圖12所示。機械臂最終的繪畫效果圖的關(guān)鍵位置點均與設(shè)想位置基本一致，在圖畫輪廓的轉(zhuǎn)折處變化相對平滑。而繪畫過程中接觸力不好控制，這一點仍需改進[10]。

圖12 繪畫效果

3.5 對弈功能研究

3.5.1 實驗步驟

（1）找到游戲板的頂點。

（2）讓Dobot畫"O"和"X"兩種標記。

（3）創(chuàng)建游戲環(huán)境。

（4）實施搜索算法。

（5）實現(xiàn)狀態(tài)差分。

（6）運動檢測的實現(xiàn)。

（7）處理相機圖片以獲得玩家的動作。

（8）處理當前的狀態(tài)，以找到最佳棋步[11]。

3.5.2 軟件設(shè)計原理

首先系統(tǒng)通過圖像處理技術(shù)校準棋盤，然后辨別兩種棋子的形狀，將棋子位置傳給求解函數(shù)并返回棋盤狀態(tài)，從而定位棋子位置，提取出當前棋局信息。設(shè)置玩家下棋過程的10 s倒計時，從差異最大的區(qū)域捕捉玩家的下棋動作。程序從當前狀態(tài)需要模擬了每個可能的動作，為了推進游戲的狀態(tài)，使用搜索算法實現(xiàn)了游戲中機械臂下棋策略的分析。該算法不斷比較一個啟發(fā)式數(shù)，作為游戲的結(jié)果。最終通過PC機發(fā)送相應(yīng)的指令控制機械臂執(zhí)行落子動作[12]。

圖13 系統(tǒng)軟件設(shè)計流程

3.5.3 實驗測試

（1）使用帶有筆執(zhí)行器的Dobot Magician在游戲板上畫"X"標記，使用攝像頭檢測并接收棋盤的狀態(tài)。通過在視覺檢測中給出的狀態(tài)、內(nèi)部游戲邏輯和Dobot之間進行交流。

（2）設(shè)置了游戲環(huán)境（棋盤）和游戲棋盤的內(nèi)部表示，與視覺傳感器有關(guān)。然后，將每個單元內(nèi)的每個頂點的一般偏移值定義為在每個游戲單元中創(chuàng)建一個"X"所需的4個坐標。

（3）測試人工智能機器人棋手能否識別出平局狀態(tài)、勝局狀態(tài)和敗局狀態(tài)，并且不做進一步的動作。還將使用每局所犯的錯誤，錯誤的定義是Dobot玩家沒有立即獲勝或沒有阻止人類玩家獲勝。

圖14 界面設(shè)計

圖15 測試過程

4 結(jié)束語

（1）本文首先求解出了Dobot機械臂的正運動學解析式，然后采用五次多項式插值法實現(xiàn)了Dobot機械臂的軌跡規(guī)劃。并且通過Matlab編程，實驗驗證了正運動學分析結(jié)果的正確性和五次多項式插值算法的有效性。

（2）基于python語言對Dobot機械臂進行了二次開發(fā)，設(shè)計了圖形化界面實現(xiàn)了對Dobot機械臂的實時操作控制。

（3）本文最終融合了AI算法，結(jié)合圖像處理技術(shù)，設(shè)計了界面化的智能人機對弈系統(tǒng)。實驗結(jié)果為Dobot機械臂軌跡規(guī)劃的研究打下了基礎(chǔ)，為該類機械臂在智能應(yīng)用中的實際控制提供了新思路。