譚彩銘， 張 成， 陳 旭， 梁志偉， 高 翔

（南京郵電大學(xué)a.自動化學(xué)院、人工智能學(xué)院；b.機(jī)器人信息感知與控制研究所，南京 210023）

0 引言

機(jī)器人技術(shù)具有理論與實踐緊密結(jié)合的特點，實驗教學(xué)是機(jī)器人教學(xué)中的重要環(huán)節(jié)。常見的機(jī)器人實驗教學(xué)平臺采用ABB、FANUC、KUKA 等品牌的機(jī)器人，其產(chǎn)品性能優(yōu)良，貼近工業(yè)現(xiàn)場，但機(jī)械結(jié)構(gòu)封閉，系統(tǒng)開放性差，且成本高、占地大、維護(hù)復(fù)雜［1-2］；單純采用計算機(jī)仿真技術(shù)性價比高、安全性好、使用方便，但仿真與實體機(jī)器人的開發(fā)與調(diào)試方法有很大區(qū)別［3-4］；采用仿真與實體機(jī)器人相結(jié)合即虛實結(jié)合的方式，可以一定程度融合兩者的優(yōu)勢，但仍然存在品牌機(jī)器人開放性差、占地大等不足［5-6］。在教學(xué)活動中發(fā)現(xiàn)，學(xué)生在使用品牌機(jī)器人的時候，即便采用虛實結(jié)合的方式，仍存在以下幾點突出問題：①僅能在實驗室開放時間使用；②由于設(shè)備價格昂貴，為避免損壞，學(xué)生不敢輕易嘗試；③人均臺套數(shù)低，需要排隊使用；④需要另外學(xué)習(xí)專用的機(jī)器人仿真軟件，增加了學(xué)習(xí)負(fù)擔(dān)與入門難度。這些大大制約了學(xué)生的學(xué)習(xí)研究進(jìn)程。

基于此，本文將虛實結(jié)合方案與低成本開源方案相結(jié)合，設(shè)計并實現(xiàn)基于Matlab 和低成本開源機(jī)械臂的機(jī)器人實驗教學(xué)平臺，充分發(fā)揮仿真與實體機(jī)器人的優(yōu)勢，彌補(bǔ)品牌機(jī)器人開放性差、成本高、占地大、維護(hù)難等缺陷。Matlab是系統(tǒng)仿真和自動控制領(lǐng)域普遍采用的計算機(jī)輔助設(shè)計工具，不少專業(yè)還開設(shè)Matlab課程［7］，基于Matlab編寫的Robotics 工具箱大大提升了Matlab機(jī)器人編程的效率［8-9］。低成本開源機(jī)械臂由6 個舵機(jī)、若干金屬支架和手爪組成，質(zhì)量約1 kg，最大臂展接近0.5 m。通過逐步實現(xiàn)機(jī)械臂抓取增強(qiáng)學(xué)生對機(jī)械臂驅(qū)動、建模、正逆運動學(xué)、軌跡規(guī)劃、視覺感知、手眼標(biāo)定等機(jī)器人學(xué)中關(guān)鍵概念的理解。設(shè)計基于單個定時器的6 舵機(jī)同步驅(qū)動方法，避免了對單片機(jī)PWM模塊和定時器模塊數(shù)量的依賴；基于標(biāo)志點的手眼標(biāo)定方法，使得‘手’、‘眼’移位后也能快速讓‘眼’正確感知；基于幾何法的逆運動學(xué)快速計算方法，解決常規(guī)逆運動學(xué)方法對5 自由度機(jī)械臂不適用問題。通過這些改進(jìn)，進(jìn)一步提高運行效率降低經(jīng)濟(jì)成本。實驗結(jié)果顯示，Matlab 仿真機(jī)械臂與實物機(jī)械臂同步完成對工件的抓取。

1 平臺總體設(shè)計方案

實驗平臺構(gòu)成如圖1 所示。下位機(jī)由單片機(jī)構(gòu)成，實現(xiàn)舵機(jī)驅(qū)動以及與上位機(jī)通信。雙目相機(jī)用于感知，增加機(jī)械臂抓取的自動化程度。上位機(jī)使用Matlab編寫雙目相機(jī)測量與機(jī)械臂抓取控制程序。

圖1 機(jī)器人實驗教學(xué)平臺構(gòu)成

基于該平臺實現(xiàn)機(jī)械臂抓取的總體方案流程如圖2 所示，其關(guān)鍵流程包括機(jī)械臂驅(qū)動、機(jī)械臂運動學(xué)建模、手眼標(biāo)定和機(jī)械臂抓取4 個部分，涵蓋了機(jī)器人學(xué)主要知識點。

圖2 平臺機(jī)械臂抓取總體方案流程

2 機(jī)械臂驅(qū)動設(shè)計

舵機(jī)是一種位置（角度）伺服驅(qū)動器，其控制信號為PWM波，周期為20 ms，通過改變脈寬來改變舵機(jī)角度，脈寬和舵機(jī)角度之間呈線性關(guān)系，脈寬在0.5 ～2.5 ms 范圍［10］。開源機(jī)械臂機(jī)械手部分采用TBSNK15 舵機(jī)，角度變化為0° ～180°，其他5 個舵機(jī)均為TBS2701，角度變化為-45° ～225°。這兩種型號的舵機(jī)角度與脈寬的控制關(guān)系見表1。基于該開源機(jī)械臂單片機(jī)，設(shè)計并實現(xiàn)單個定時器的6 舵機(jī)同步PWM波。

表1 脈寬與所用舵機(jī)角度之間的控制關(guān)系

2.1 單片機(jī)硬件電路

如圖3 所示，該開源機(jī)械臂驅(qū)動電路采用IAP15W4K61S4 型STC 單片機(jī)，以IAP 開頭型號的單片機(jī)本身就是仿真器，可直接在線仿真，方便學(xué)生調(diào)試和下載程序。STC 單片機(jī)對之前的51 內(nèi)核進(jìn)行了重大改良并增加了很多片內(nèi)外設(shè)，其性能超越了AT89系列51 單片機(jī)?，F(xiàn)在電腦普遍不安裝串口，使用USB轉(zhuǎn)串口芯片CH341，把電腦的USB 口映射為串口使用，通過該接口實現(xiàn)上位機(jī)和下位機(jī)之間的通信以及單片機(jī)程序下載和在線調(diào)試。6 個舵機(jī)的控制信號分別由單片機(jī)的6 個I/O引腳提供。另外按鍵和指示燈用于人機(jī)交互和狀態(tài)顯示，擴(kuò)展接口用于外接其他設(shè)備。

圖3 驅(qū)動開源機(jī)械臂的單片機(jī)硬件框圖

2.2 基于單個定時器的6 舵機(jī)同步驅(qū)動程序

使用定時器中斷來產(chǎn)生PWM 波。設(shè)第i個舵機(jī)（記為Si）的脈寬為wi，由表1 可知，0.5 ms≤wi≤2.5 ms。如圖4 所示，將整個20 ms 的周期分為8 個2.5ms的小時間段，令S1對應(yīng)的I/O 引腳輸出置為高電平，并設(shè)置定時周期為w1，定時周期到觸發(fā)定時中斷，將S1對應(yīng)的I/O引腳輸出置為低電平，并設(shè)置定時周期為2.5 ms-w1，定時周期到觸發(fā)定時中斷，將S2對應(yīng)的I/ O 引腳輸出置為高電平，并設(shè)置定時周期為w2，以此類推，其中在將最后一個S6對應(yīng)的I/O 引腳輸出置為低電平后，設(shè)置定時周期為7.5 ms -w6，這樣6 個舵機(jī)的一次控制周期之和為20 ms，循環(huán)往復(fù)，這樣就在6 個I/O引腳上分別產(chǎn)生6 個周期為20 ms，脈寬時間不同的PWM波。以上過程，每20 ms的周期里一共有12 次定時器中斷，在任意時刻，當(dāng)某個舵機(jī)輸入為高電平時，其他舵機(jī)輸入必定是低電平。這里將I/O引腳設(shè)置為強(qiáng)推挽輸出以驅(qū)動舵機(jī)。

圖4 基于單個定時器的6舵機(jī)同步驅(qū)動PWM波產(chǎn)生原理示意圖

為使機(jī)械臂運動更加平滑，在當(dāng)前脈寬和目標(biāo)脈寬之間按固定步長插補(bǔ)若干均勻遞增或遞減的脈寬，依次輸入如圖4 所示的PWM 波產(chǎn)生函數(shù)，這個步長和機(jī)械臂運動速度對應(yīng)。除使用定時器外，也可直接使用單片機(jī)內(nèi)嵌的PWM模塊產(chǎn)生PWM信號。

3 機(jī)械臂運動學(xué)建模

對機(jī)械臂進(jìn)行運動學(xué)建模是進(jìn)行正、逆運動學(xué)計算、軌跡規(guī)劃等過程的基礎(chǔ)。

3.1 DH建模

針對開源機(jī)械臂，采用Denavit Hartenber 提出的DH方法定義連桿坐標(biāo)系［11］，各連桿坐標(biāo)系定義如圖5所示（不包括末端執(zhí)行器）。其中坐標(biāo)系0 作為基座坐標(biāo)系，相對桌面固定。坐標(biāo)系5 通常被稱作末端執(zhí)行器坐標(biāo)系，將其原點設(shè)置在機(jī)械手末端的兩個手指中間。坐標(biāo)系i到坐標(biāo)系i-1 的齊次變換矩陣為

圖5 開源機(jī)械臂連桿坐標(biāo)系定義

上式對應(yīng)的物理變換過程：

（1）沿zi-1軸旋轉(zhuǎn)θi角，使xi-1軸與xi軸平行。（2）沿zi-1軸平移距離di，進(jìn)一步將xi-1軸與xi軸共線。

（3）沿xi-1軸平移距離ai，使坐標(biāo)系i-1 原點與坐標(biāo)系i的原點重合。

（4）沿xi-1軸旋轉(zhuǎn)αi角，使zi-1軸與zi軸共線［12］。

該開源機(jī)械臂DH參數(shù)見表2。

表2 開源機(jī)械臂DH參數(shù)

使用Robotics工具箱9.10 版本Link 函數(shù)建立每個連桿對象，即調(diào)用

式中，sigma 為指定旋轉(zhuǎn)或平移關(guān)節(jié)類型。使用SerialLink函數(shù)建立串聯(lián)機(jī)器人對象，即調(diào)用

式中，L為按序包含串聯(lián)機(jī)器人連桿的數(shù)組。

3.2 關(guān)節(jié)轉(zhuǎn)角數(shù)據(jù)發(fā)送

如圖5 所示，機(jī)械臂處于豎直位置時，所有舵機(jī)處于中間位置，即舵機(jī)的角度?i處于表1 所示的90°。

Matlab仿真機(jī)械臂的關(guān)節(jié)轉(zhuǎn)角θi和實物機(jī)械臂的關(guān)節(jié)轉(zhuǎn)角?i之間存在如下對應(yīng)關(guān)系

由表1 可知，舵機(jī)TBS2701 脈寬tH和舵機(jī)角度?之間的關(guān)系滿足

式中，tH的單位是μs；舵機(jī)角度?的單位是（°）。

Matlab提供對系統(tǒng)串口的支持。運行

scom= serial（‘COM3’，‘BaudRate’，115200）

建立一個串口對象scom，這里串口號為COM3，波特率為115200，運行fopen（scom）打開串口，運行fprintf（scom，str）以文本形式向串口發(fā)送一幀數(shù)據(jù)str，幀格式示例為“#1P1500#2P1500#3P1500#4P1500#5P1500#6P1500”，字符‘#’與‘P’之間的數(shù)字表示舵機(jī)號，字符‘P’后面緊跟的數(shù)據(jù)1500 為該號舵機(jī)的脈寬，可以是500 ～2500 之間的任意數(shù)據(jù)，單位為μs。下位機(jī)接收到數(shù)據(jù)后，驅(qū)動所有舵機(jī)按指令轉(zhuǎn)動。

4 基于標(biāo)志點的手眼標(biāo)定

手眼標(biāo)定是使雙目相機(jī)感知的定位信息被機(jī)械臂正確感知的必要環(huán)節(jié)［13］?；跇?biāo)志點的手眼標(biāo)定方法原理，在機(jī)械臂末端固定一個標(biāo)記點，操縱機(jī)械臂運動到若干位置，通過正向運動學(xué)獲得該標(biāo)記點在機(jī)械臂基座坐標(biāo)系下的坐標(biāo)，通過雙目相機(jī)得到這些位置處標(biāo)記點在相機(jī)坐標(biāo)系下的坐標(biāo)，根據(jù)迭代最近點（Iterative Closest Points，ICP）算法求得相機(jī)坐標(biāo)系到基座坐標(biāo)系下的齊次變換矩陣，實現(xiàn)手眼標(biāo)定，為后續(xù)抓取做準(zhǔn)備。

4.1 基于雙目相機(jī)的標(biāo)志點定位

使用的雙目相機(jī)由左、右兩個水平放置的相機(jī)組成，雙目相機(jī)的光軸平行、相機(jī)參數(shù)一致。如圖6 所示，利用針孔模型對相機(jī)進(jìn)行建模，現(xiàn)實世界的空間點光線經(jīng)過光心之后，落在物理成像平面，形成成像點，物理成像平面到光心的距離為焦距f。左、右兩個相機(jī)的光心之間的距離稱為雙目相機(jī)的基線，記為b。設(shè)空間點P在左、右眼的成像點分別為PL和PR，PL和PR在各自的圖像物理坐標(biāo)系的坐標(biāo)分別為（xL，yL）和（xR，yR）。

圖6 雙目相機(jī)的成像模型

定義雙目相機(jī)的坐標(biāo)系O-xcyczc為右眼相機(jī)坐標(biāo)系，P點在相機(jī)坐標(biāo)系z軸坐標(biāo)即為深度Z，根據(jù)三角形相似關(guān)系，可得

整理得

在得到深度的情況下，根據(jù)三角形相似關(guān)系可進(jìn)一步得到P點在相機(jī)坐標(biāo)系O-xcyczc下的坐標(biāo)，用向量表示，滿足

相機(jī)內(nèi)部傳感器在成像平面上對像進(jìn)行采樣和量化，將光線轉(zhuǎn)換成圖像像素。實際通過相機(jī)最終獲得的是一個個像素，每個像素對應(yīng)的物理尺寸是相機(jī)的固有參數(shù)，通過該參數(shù)可得像素對應(yīng)的成像點在圖像物理坐標(biāo)系下的坐標(biāo)［14］，進(jìn)而可由式（1）、（2）得空間點在相機(jī)坐標(biāo)系下的坐標(biāo)。

4.2 基于ICP算法的手眼相對位姿確定

ICP算法是一種點集對點集的配準(zhǔn)算法，多用于點云匹配［15］，將其引入手眼標(biāo)定計算。操縱機(jī)械臂，使與連桿5 固定的標(biāo)記點到達(dá)第i個點，該點記為Pi，通過正向運動學(xué)得到該點Pi在機(jī)械臂基座坐標(biāo)系0系下的坐標(biāo)，記為。通過雙目相機(jī)定位得點Pi在相機(jī)坐標(biāo)系c系下的坐標(biāo)，記為。表示c系原點在0 系下的坐標(biāo)表示c系到0 系的旋轉(zhuǎn)矩陣。根據(jù)ICP算法，通過對下式取最小求解旋轉(zhuǎn)矩陣和平移向這兩個參數(shù)，即

其中，n為標(biāo)記點到達(dá)的位置數(shù)。采用文獻(xiàn)［15］中所述的奇異值分解方法求得和，坐標(biāo)系c系到0 系的齊次變換矩陣為

式中，01×3=［00 0］T。

5 機(jī)械臂抓取

根據(jù)手眼標(biāo)定可得相機(jī)坐標(biāo)系和機(jī)械臂基座坐標(biāo)系的齊次變換矩陣，由雙目相機(jī)定位可得被抓取目標(biāo)在相機(jī)坐標(biāo)系下的坐標(biāo)，經(jīng)坐標(biāo)變換可得被抓取目標(biāo)在基座坐標(biāo)系下的坐標(biāo)。控制機(jī)械臂自主運動，使機(jī)械手順利抓取目標(biāo)，該過程主要涉及逆運動學(xué)和軌跡規(guī)劃兩個過程。

5.1 基于幾何法的逆向運動學(xué)

逆向運動學(xué)是給定機(jī)器人末端執(zhí)行器相對基座坐標(biāo)系的期望位姿，求機(jī)器人能夠到達(dá)預(yù)期位姿的關(guān)節(jié)變量［16］。對于5 自由度機(jī)械臂（不包括末端執(zhí)行器），由于其自由度數(shù)不足6，不能以任意姿態(tài)逼近同一目標(biāo)位置，會出現(xiàn)逆運動學(xué)解不存在的情況。根據(jù)機(jī)械臂抓取物體的實際過程，提出一種基于幾何方法的運動學(xué)逆解快速計算方法。如圖7 所示，坐標(biāo)系5 的原點O5在基座坐標(biāo)系下的坐標(biāo)用極坐標(biāo)和高度來描述，其極坐標(biāo)長度記為r，極坐標(biāo)角度記為ψ，高度記為h。

圖7 基于幾何法的開源機(jī)械臂運動學(xué)逆解求解原理示意圖

顯然，ψ由關(guān)節(jié)轉(zhuǎn)角θ1唯一確定，抓取過程中，令機(jī)械手平面與豎直方向保持60°去抓取物體，這樣關(guān)節(jié)轉(zhuǎn)角θ5確定，且坐標(biāo)系3 的原點O3在基座坐標(biāo)系下的坐標(biāo)確定，O3高度和極坐標(biāo)長度分別為

O1O3長度的確定，由于O1O2和O2O3長度確定，根據(jù)余弦定理求得ΔO1O2O3和ΔO1AO3各內(nèi)角的大小，可得∠O2O1A、∠O1O2O3和∠O2O3O4。根據(jù)DH模型對關(guān)節(jié)轉(zhuǎn)角的定義，取其中一組解：θ1= ψ，θ2=∠O2O1A，θ3=∠O1O2O3-π，θ4=π-∠O2O3O4，θ5= -π/2。要使得存在運動學(xué)逆解，需滿足O1O3≤O1O2+O2O3。

5.2 軌跡規(guī)劃

抓取過程中，令機(jī)械臂末端依次經(jīng)過以下7 個位置：初始位置、第1 抬起位置、目標(biāo)位置上方、目標(biāo)位置（此處控制機(jī)械手夾取）、第2 抬起位置、放置位置上方、放置位置（此處控制機(jī)械手放開）。

對抓取過程中依次經(jīng)過的7 個位置，根據(jù)5.1 節(jié)所述逆向運動學(xué)算法計算得到關(guān)節(jié)角度，再進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃得到相鄰位置間的關(guān)節(jié)角度序列。在Matlab編程時，使用jtrag函數(shù)進(jìn)行關(guān)節(jié)空間下軌跡規(guī)劃，調(diào)用

式中：q0 為初始關(guān)節(jié)角度；qf為終止關(guān)節(jié)角度；step 為過程包含的時間步數(shù)。該函數(shù)使用五次多項式進(jìn)行插值規(guī)劃軌跡，默認(rèn)初始和終止時關(guān)節(jié)速度和加速度的值均為零。

6 實驗驗證

圖8 所示為實驗場景。將機(jī)械臂和雙目相機(jī)固定在桌面。雙目相機(jī)內(nèi)參數(shù)已知，首先進(jìn)行手眼標(biāo)定，在機(jī)械臂連桿5 固定一個白色標(biāo)記點，基于Matlab 通過串口發(fā)送關(guān)節(jié)角度數(shù)據(jù)，控制機(jī)械手到達(dá)7 個位置，根據(jù)第4 節(jié)說述的手眼標(biāo)定方法獲得相機(jī)坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的齊次變換矩陣。桌面上有一工件，通過雙目相機(jī)對工件進(jìn)行定位，代入得到該工件在機(jī)械臂基座坐標(biāo)系0 系下的坐標(biāo)。根據(jù)第5 節(jié)所述的機(jī)械臂抓取方法對工件進(jìn)行抓取，軌跡規(guī)劃所得關(guān)節(jié)角度數(shù)據(jù)同時發(fā)送給Matlab 仿真機(jī)械臂和實物機(jī)械臂，控制兩者同步運動。如圖9、10 所示，二者的運動軌跡一致，實物機(jī)械臂成功地抓取工件，其中圖10 繪制了仿真機(jī)械臂末端的運行軌跡。

圖8 實驗平臺機(jī)械臂抓取實驗場景

圖9 實物機(jī)械臂抓取過程

圖10 仿真機(jī)械臂抓取過程

在實驗時，雙目相機(jī)捕獲白色標(biāo)記點和工件是通過人工鼠標(biāo)點擊得到的，由于工件體積較大，捕獲的圖像上工件的位置會影響最終定位準(zhǔn)確度。視覺感知方法上還可以持續(xù)改進(jìn)，如進(jìn)行標(biāo)記點與工件的自動識別、工件三維建模等，進(jìn)一步提高抓取的智能化程度。

7 結(jié)語

本文設(shè)計了一種虛實結(jié)合的低成本開源機(jī)器人實驗教學(xué)平臺，基于Matlab軟件建立仿真機(jī)械臂，通過一系列流程產(chǎn)生機(jī)械臂運動數(shù)據(jù)，控制開源機(jī)械臂和仿真機(jī)械臂運動，實驗結(jié)果顯示，仿真機(jī)械臂和機(jī)械臂運動一致，成功實現(xiàn)對工件的抓取。通過將虛實結(jié)合方案與開源方案有機(jī)結(jié)合，充分發(fā)揮仿真與機(jī)器人的優(yōu)勢，彌補(bǔ)品牌機(jī)器人開放性差、成本高、占地大、維護(hù)難等缺陷。此外，特別設(shè)計了單個定時器的6 舵機(jī)同步驅(qū)動程序、標(biāo)志點的手眼標(biāo)定方法以及幾何法的快速逆運動學(xué)解算方法，進(jìn)一步提高運行效率降低了經(jīng)濟(jì)成本。

通過將Matlab 和低成本開源機(jī)械臂結(jié)合，使學(xué)生在Matlab編程時具有針對性，促進(jìn)學(xué)生對機(jī)器人學(xué)的真正理解，比如使用Link函數(shù)建模時，必須要理解DH模型的含義，才能準(zhǔn)確建模并模擬實物機(jī)械臂。通過模擬工業(yè)場景常見的機(jī)械臂抓取過程，明確正逆運動學(xué)、軌跡規(guī)劃、手眼標(biāo)定等是實現(xiàn)機(jī)械臂抓取不可或缺的環(huán)節(jié)，加深學(xué)生對機(jī)器人學(xué)相關(guān)概念的理解。

該平臺可開展的機(jī)器人學(xué)實驗包括DH 建模、正逆運動學(xué)、軌跡規(guī)劃、單片機(jī)編程、雙目相機(jī)定位、手眼標(biāo)定、機(jī)械臂抓取等。該平臺也可用于設(shè)計開放性實驗，如自主設(shè)計正逆運動學(xué)算法，對復(fù)雜形態(tài)的物體進(jìn)行抓取，進(jìn)行目標(biāo)物體識別以便進(jìn)一步提高機(jī)械臂抓取智能化。