Dobot機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃與控制

唐雯雯，盧 宇，王明芬

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

0 引言

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

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

1 Dobot機(jī)械臂正運(yùn)動(dòng)學(xué)分析

1.1 連桿坐標(biāo)系的建立

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

圖1 機(jī)械臂D-H建模連桿坐標(biāo)系

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

表1 D-H參數(shù)

1.2 Dobot正運(yùn)動(dòng)學(xué)求解

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

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

1.3 Dobot機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃

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

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

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

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

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

2 Matlab仿真

2.1 正運(yùn)動(dòng)學(xué)結(jié)果驗(yàn)證

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

圖3 初始位姿仿真

經(jīng)正運(yùn)動(dòng)學(xué)求解初始姿態(tài)為：

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

圖4 末端位置仿真

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

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

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

圖5 仿真程序

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

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

3 Dobot機(jī)械臂實(shí)驗(yàn)研究

3.1 實(shí)驗(yàn)研究方案

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

3.2 實(shí)驗(yàn)硬件和軟件

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

圖7 機(jī)械臂控制界面

3.3 點(diǎn)動(dòng)功能研究

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

圖8 機(jī)械臂運(yùn)動(dòng)回零位置

圖9 機(jī)械臂運(yùn)動(dòng)終點(diǎn)位置

3.4 圓弧功能研究

圓弧實(shí)驗(yàn)步驟如下。

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

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

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

圖10 機(jī)械臂回零

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

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

圖12 繪畫效果

3.5 對(duì)弈功能研究

3.5.1 實(shí)驗(yàn)步驟

（1）找到游戲板的頂點(diǎn)。

（2）讓Dobot畫"O"和"X"兩種標(biāo)記。

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

（4）實(shí)施搜索算法。

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

（6）運(yùn)動(dòng)檢測(cè)的實(shí)現(xiàn)。

（7）處理相機(jī)圖片以獲得玩家的動(dòng)作。

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

3.5.2 軟件設(shè)計(jì)原理

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

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

3.5.3 實(shí)驗(yàn)測(cè)試

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

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

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

圖14 界面設(shè)計(jì)

圖15 測(cè)試過(guò)程

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

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

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

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