應用攝像機標定的機械臂軌跡跟蹤控制系統

賈 蓉，鄭秀娟，張金亮

（1.湖北 汽車工業學院電氣與信息工程學院，湖北 十堰 442002；2.武漢科技大學信息科學與工程學院，湖北 武漢 430081）

1 引言

機械臂軌跡跟蹤控制系統能夠實現機械臂的軌跡跟蹤控制，對于提升機械臂的精細化控制程度有很大意義[1]。在提升機械臂控制精度與升級其控制結構的過程中，必須同步進行機械臂軌跡跟蹤控制，才能真正實現作業中控制精度的提升。機械臂軌跡跟蹤控制涉及控制技術、計算機視覺技術、機器人運動學技術、圖像高速處理技術等多種技術學科，控制過程極其復雜[2]。

對于機械臂軌跡跟蹤控制系統的研究，相關研究學者己經取得了一定成績。文獻[3]提出基于視覺圖像轉換技術的機械臂軌跡跟蹤控制系統，通過視覺圖像轉換技術獲取目標位置，結合圖像分析算法計算目標與機械臂的相對位置，通過OPC傳輸給機械臂控制模塊，實現機械臂的軌跡跟蹤控制。該方法的跟蹤控制效率較好，但受目標物體質心投影軌跡不清晰的影響而無法進行信號反饋，存在控制時延過長的問題。文獻[4]提出基于模型預測控制方法的機械臂軌跡跟蹤控制系統。通過歐拉-拉格朗日公式構建機械臂運動動力學模型，應用線性化模型實現機械臂軌跡跟蹤控制的模型中。該方法的控制時延較短，但跟蹤控制準確性較差。文獻[5]提出提出基于改進自適應神經滑模的機械臂軌跡跟蹤控制系統。通過狀態反饋控制系統自適應學習，輸出滑?？刂破餮a償值，實現機械臂軌跡跟蹤控制。該方法信號反饋時效性較好，跟蹤控制效率較差。

針對上述方法存在的問題，提出應用攝像機標定的機械臂軌跡跟蹤控制系統。在系統硬件部分，通過主控單元、UVC攝像頭及壓力檢測傳感器等硬件設計工控模塊，基于前饋—反饋控制結構設計反饋模塊，實現對機械臂軌跡跟蹤信號的采集及反饋。軟件部分，采用攝像機標定算法，通過獲取軌跡跟蹤物體視場的旋轉、平移以及攝像機對應內參數，實現機械臂的軌跡跟蹤控制；應用攝像機標定，通過機械臂已知運動軌跡，將整個運動過程中的數據密化，完成機械臂運動軌跡重現，實現機械臂運動目標的跟蹤控制。

2 機械臂軌跡跟蹤控制系統硬件設計

應用攝像機標定的機械臂軌跡跟蹤控制系統的硬件構成包括工控模塊、反饋模塊[6]。通過工控模塊實現電機工作狀態控制，利用前饋—反饋控制結構設計反饋模塊，對機械臂軌跡跟蹤信號進行反饋。

2.1 設計工控模塊

工控模塊由主控單元、UVC攝像頭、視頻服務器、無線網橋、無刷直流電機、壓力檢測傳感器構成[7]。

其中主控單元負責進行遠程控制端通信、運動控制、圖像信息采集及網絡管理。主要由104/PC-plus工控機構成，選用型號為AMD主板工業嵌入式1815-104工控機[8]。該工控機的具體數據，如表1所示。

表1 工控機的具體數據Tab.1 Specific Data of IPC

視頻服務器選用的型號為威視?？礢D6704，作為一種新型網絡視頻服務器，其采用多種標準編碼算法，包括MJPEG、MPEG2、MPEG4、H.264。該視頻服務器具有強大的兼容性，主要基于Linux操作系統，在FLASH中固定代碼，支持各種網絡協議。

壓力檢測傳感器選用的型號為微型高精度A301壓力傳感器，其尺寸為（25.4×14×0.208）mm，能夠保持（±3）%之內的線性誤差，測量范圍為（0～540）N。在MCP6004運放中內置了共4個運算放大器[10]，支持的工作電壓為（1.9～6.2）V，輸出輸入均為軌至軌形式，工作靜態電流僅為100μA，帶寬2MHz。壓力檢測中等效壓力的具體計算公式如下式：

2.2 設計反饋模塊

基于前饋—反饋控制結構設計反饋模塊，反饋模塊的主要作用是對機械臂軌跡跟蹤信號進行反饋。反饋模塊的具體結構為反饋控制器、執行器、前饋控制器。反饋模塊的具體運行流程如下：首先向反饋控制器輸入目標信號，接著反饋控制器向執行器發送信號，并向前饋控制器發送信號，經過測量與變送，該信號與執行器輸出信號匯集為被控變量，通過被控變量對目標進行跟蹤，獲取跟蹤信號，并對其進行測量與變送，向系統進行反饋，實現軌跡跟蹤信號的反饋。

3 應用攝像機標定的機械臂軌跡跟蹤控制系統軟件設計

3.1 軌跡跟蹤

通過攝像機標定算法實現機械臂的軌跡跟蹤。通過攝像機標定進行機械臂軌跡跟蹤的具體流程如下：

用下式表示空間中的三維點到成像平面的對應映射單應性關系式：

式中：fx、fy—攝像機對應內參數f的左右坐標；cx、cy—攝像機對應內參數c的左右坐標。

對一個平面進行定義使Z等于0。對旋轉矩陣進行分解，將其分解為三個大小為3×1的向量，則可省略其中一個向量。分解后的旋轉矩陣具體如下式：

對于機械臂軌跡跟蹤物體，可以利用上式對多幅圖像進行計算，從而獲取軌跡跟蹤物體視場的旋轉、平移以及攝像機對應內參數。

3.2 基于直線插補算法的機械臂運動軌跡跟蹤控制

在機械臂運動軌跡跟蹤控制過程中，每個運動坐標點都需要和給定的數據進行比對，進而決定下一步該怎么走。

直線插補算法能夠通過已知坐標點確定運動軌跡，已知曲線上的某些數據，推導出中間點的方法，也稱為“數據點的密化”。

通過直線插補算法對之間的空間進行數據密化，從而形成要求的輪廓軌跡，通過直線插補算法實現機械臂的運動控制。直線插補算法的具體步驟如下：

（1）進行插補準備，具體計算公式如下：

式中：K—插補周期中給定的進給速率數；

ΔL—一個插補周期中的對應進給線段長度；

L—直線段長度；

Δx、Δy、Δz—x、y、z三個坐標軸的位移增量；

xe、ye、ze—終點的坐標軸坐標。

（2）插補計算，具體公式如下：

式中：xi+1、yi+1、zi+1—插補結果坐標點的坐標值；

xi、yi、zi—插補點的坐標值。

據此得出了插補點的坐標值，還原了整體運動軌跡。通過反饋模塊將運動軌跡信息傳輸至工控模塊，實現機械臂軌跡跟蹤控制。

4 實驗測試

4.1 實驗設計

為證明設計的應用攝像機標定的機械臂軌跡跟蹤控制系統的性能，對其進行實驗測試。實驗中采用的機械臂為直線式九自由度機械臂，其立體空間中的工作范圍為（800×800×800)mm，運行速度的最大值為100mm/s。

直線式九自由度機械臂結構，如圖1所示。

圖1 實驗所用機械臂Fig.1 Manipulator Used in Experiment

4.2 實驗方法與步驟

利用應用攝像機標定的機械臂軌跡跟蹤控制系統對實驗機械臂進行軌跡跟蹤控制，在MATLAB軟件搭建實驗平臺，使用上文提到的九自由度機械臂進行圓弧軌跡跟蹤實驗，獲取在機械臂位置增量為（10.52～30.52）mm 的范圍內的控制時延數據以及曲線跟蹤仿真結果作為實驗數據。采用原有的前饋—反饋控制方法、文獻[4-5]方法作為實驗對比方法，驗證機械臂位置增量為（10.52～30.52）mm的范圍內的控制時延及軌跡跟蹤控制準確性，實驗流程圖，如圖2所示。

圖2 實驗方法流程圖Fig.2 Flow Chart of Experimental Method

4.3 實驗結果分析

在機械臂工作范圍內選?。?00×200×200）mm的立體空間，讓其進行圓弧軌跡實驗，機械臂在范圍為（200×200×200）mm 的立體空間內作隨機圓弧軌跡運動，A為運動路徑的起始點，B為運動終點。其運動路徑，如圖3所示。所設計應用攝像機標定的機械臂軌跡跟蹤控制系統是在原有前饋—反饋控制系統的基礎上進行改進，所以將應用攝像機標定的機械臂軌跡跟蹤控制系統和前饋—反饋控制系統、文獻[4-5]系統進行對比實驗，得到軌跡跟蹤路線圖，如圖4所示。

圖3 機械臂運行軌跡Fig.3 Trajectory of Manipulator

圖4 軌跡跟蹤控制路徑Fig.4 Trajectory Tracking Control Path

由圖4可知，在九自由度機械臂圓弧軌跡跟蹤實驗過程中，原有的前饋—反饋控制系統在跟蹤前期得到的跟蹤路徑與實際路徑較為一致，在跟蹤后期由于未能及時獲取誤差坐標點，出現了跟蹤偏差較大的問題；而所提方法得到的軌跡與機械臂的實際運行軌跡較為一致，解決了原有方法存在的問題，體現了其良好的跟蹤準確性。而文獻[4]方法得到的軌跡具有一定的誤差，與設計圓弧軌跡相比有所偏離，文獻[5]方法在軌跡跟蹤過程中存在缺失，沒有得到一個完整的圓弧軌跡，且與設定軌跡的偏離誤差較大。

收集軌跡跟蹤路徑數據，得到軌跡跟蹤控制準確性，如圖5所示。

圖5 軌跡跟蹤控制準確性對比實驗結果Fig.5 Experimental Results of Trajectory Tracking Control Accuracy Comparison

根據圖5可知，在機械臂35min運動時間內，原有的前饋—反饋控制系統對機械臂運動軌跡跟蹤控制的準確度平均值為83%，文獻[4]方法對機械臂運動軌跡跟蹤控制的準確度平均值為69%，文獻[5]方法對機械臂運動軌跡跟蹤控制的準確度平均值為63%，而所設計系統對機械臂運動軌跡跟蹤控制的準確度平均值為93%。所設計系統應用攝像機標定準確計算出了機械臂運動軌跡坐標點，并通過反饋系統傳遞給控制模塊，因此能實現機械臂運動目標的準確跟蹤控制。

在機械臂位置增量為（10.52～20.52）mm的范圍內，應用攝像機標定的機械臂軌跡跟蹤控制系統對比系統的控制時延對比實驗結果，如表2所示。

表2（10.52～20.52）mm范圍內控制時延對比實驗結果Tab.2 Comparative Experimental Results of Control Delay in the Range of（10.52～20.52）mm

根據表2結果可知，應用攝像機標定的機械臂軌跡跟蹤控制系統的控制時延遠低于文獻對比系統及原有的前饋—反饋控制系統。這是因為所設計系統在前饋—反饋控制結構的基礎上引入了直線插補算法，能夠快速求解運動控制插補點坐標值并傳輸至工控模塊，有效降低了控制時延，提升了控制效率。

5 結束語

為提升機械臂軌跡跟蹤控制的準確性及效率，提出應用攝像機標定的機械臂軌跡跟蹤控制系統設計，設計工控模塊由主控單元、UVC 攝像頭、視頻服務器壓力檢測傳感器構成；引入前饋-反饋控制結構實現控制系統的快速反饋。軟件部分通過攝像機標定算法實現機械臂的軌跡跟蹤，引入直線插補算法得出目標物體準確坐標值，實現機械臂的運動控制。實驗結果表明，應用攝像機標定的機械臂軌跡跟蹤控制系統能夠有效提升機械臂軌跡跟蹤控制的準確性，通過直線插補算法實現了控制時延的降低。