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

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

0 引言

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

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

1 平臺總體設計方案

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

圖1 機器人實驗教學平臺構成

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

圖2 平臺機械臂抓取總體方案流程

2 機械臂驅動設計

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

表1 脈寬與所用舵機角度之間的控制關系

2.1 單片機硬件電路

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

圖3 驅動開源機械臂的單片機硬件框圖

2.2 基于單個定時器的6 舵機同步驅動程序

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

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

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

3 機械臂運動學建模

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

3.1 DH建模

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

圖5 開源機械臂連桿坐標系定義

上式對應的物理變換過程：

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

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

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

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

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

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

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

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

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

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

Matlab仿真機械臂的關節(jié)轉角θi和實物機械臂的關節(jié)轉角?i之間存在如下對應關系

由表1 可知，舵機TBS2701 脈寬tH和舵機角度?之間的關系滿足

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

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ù)字表示舵機號，字符‘P’后面緊跟的數(shù)據(jù)1500 為該號舵機的脈寬，可以是500 ～2500 之間的任意數(shù)據(jù)，單位為μs。下位機接收到數(shù)據(jù)后，驅動所有舵機按指令轉動。

4 基于標志點的手眼標定

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

4.1 基于雙目相機的標志點定位

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

圖6 雙目相機的成像模型

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

整理得

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

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

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

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

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

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

5 機械臂抓取

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

5.1 基于幾何法的逆向運動學

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

圖7 基于幾何法的開源機械臂運動學逆解求解原理示意圖

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

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

5.2 軌跡規(guī)劃

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

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

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

6 實驗驗證

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

圖8 實驗平臺機械臂抓取實驗場景

圖9 實物機械臂抓取過程

圖10 仿真機械臂抓取過程

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

7 結語

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

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

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