應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)

賈 蓉，鄭秀娟，張金亮

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

1 引言

機(jī)械臂軌跡跟蹤控制系統(tǒng)能夠?qū)崿F(xiàn)機(jī)械臂的軌跡跟蹤控制，對(duì)于提升機(jī)械臂的精細(xì)化控制程度有很大意義[1]。在提升機(jī)械臂控制精度與升級(jí)其控制結(jié)構(gòu)的過(guò)程中，必須同步進(jìn)行機(jī)械臂軌跡跟蹤控制，才能真正實(shí)現(xiàn)作業(yè)中控制精度的提升。機(jī)械臂軌跡跟蹤控制涉及控制技術(shù)、計(jì)算機(jī)視覺(jué)技術(shù)、機(jī)器人運(yùn)動(dòng)學(xué)技術(shù)、圖像高速處理技術(shù)等多種技術(shù)學(xué)科，控制過(guò)程極其復(fù)雜[2]。

對(duì)于機(jī)械臂軌跡跟蹤控制系統(tǒng)的研究，相關(guān)研究學(xué)者己經(jīng)取得了一定成績(jī)。文獻(xiàn)[3]提出基于視覺(jué)圖像轉(zhuǎn)換技術(shù)的機(jī)械臂軌跡跟蹤控制系統(tǒng)，通過(guò)視覺(jué)圖像轉(zhuǎn)換技術(shù)獲取目標(biāo)位置，結(jié)合圖像分析算法計(jì)算目標(biāo)與機(jī)械臂的相對(duì)位置，通過(guò)OPC傳輸給機(jī)械臂控制模塊，實(shí)現(xiàn)機(jī)械臂的軌跡跟蹤控制。該方法的跟蹤控制效率較好，但受目標(biāo)物體質(zhì)心投影軌跡不清晰的影響而無(wú)法進(jìn)行信號(hào)反饋，存在控制時(shí)延過(guò)長(zhǎng)的問(wèn)題。文獻(xiàn)[4]提出基于模型預(yù)測(cè)控制方法的機(jī)械臂軌跡跟蹤控制系統(tǒng)。通過(guò)歐拉-拉格朗日公式構(gòu)建機(jī)械臂運(yùn)動(dòng)動(dòng)力學(xué)模型，應(yīng)用線性化模型實(shí)現(xiàn)機(jī)械臂軌跡跟蹤控制的模型中。該方法的控制時(shí)延較短，但跟蹤控制準(zhǔn)確性較差。文獻(xiàn)[5]提出提出基于改進(jìn)自適應(yīng)神經(jīng)滑模的機(jī)械臂軌跡跟蹤控制系統(tǒng)。通過(guò)狀態(tài)反饋控制系統(tǒng)自適應(yīng)學(xué)習(xí)，輸出滑模控制器補(bǔ)償值，實(shí)現(xiàn)機(jī)械臂軌跡跟蹤控制。該方法信號(hào)反饋時(shí)效性較好，跟蹤控制效率較差。

針對(duì)上述方法存在的問(wèn)題，提出應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)。在系統(tǒng)硬件部分，通過(guò)主控單元、UVC攝像頭及壓力檢測(cè)傳感器等硬件設(shè)計(jì)工控模塊，基于前饋—反饋控制結(jié)構(gòu)設(shè)計(jì)反饋模塊，實(shí)現(xiàn)對(duì)機(jī)械臂軌跡跟蹤信號(hào)的采集及反饋。軟件部分，采用攝像機(jī)標(biāo)定算法，通過(guò)獲取軌跡跟蹤物體視場(chǎng)的旋轉(zhuǎn)、平移以及攝像機(jī)對(duì)應(yīng)內(nèi)參數(shù)，實(shí)現(xiàn)機(jī)械臂的軌跡跟蹤控制；應(yīng)用攝像機(jī)標(biāo)定，通過(guò)機(jī)械臂已知運(yùn)動(dòng)軌跡，將整個(gè)運(yùn)動(dòng)過(guò)程中的數(shù)據(jù)密化，完成機(jī)械臂運(yùn)動(dòng)軌跡重現(xiàn)，實(shí)現(xiàn)機(jī)械臂運(yùn)動(dòng)目標(biāo)的跟蹤控制。

2 機(jī)械臂軌跡跟蹤控制系統(tǒng)硬件設(shè)計(jì)

應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)的硬件構(gòu)成包括工控模塊、反饋模塊[6]。通過(guò)工控模塊實(shí)現(xiàn)電機(jī)工作狀態(tài)控制，利用前饋—反饋控制結(jié)構(gòu)設(shè)計(jì)反饋模塊，對(duì)機(jī)械臂軌跡跟蹤信號(hào)進(jìn)行反饋。

2.1 設(shè)計(jì)工控模塊

工控模塊由主控單元、UVC攝像頭、視頻服務(wù)器、無(wú)線網(wǎng)橋、無(wú)刷直流電機(jī)、壓力檢測(cè)傳感器構(gòu)成[7]。

其中主控單元負(fù)責(zé)進(jìn)行遠(yuǎn)程控制端通信、運(yùn)動(dòng)控制、圖像信息采集及網(wǎng)絡(luò)管理。主要由104/PC-plus工控機(jī)構(gòu)成，選用型號(hào)為AMD主板工業(yè)嵌入式1815-104工控機(jī)[8]。該工控機(jī)的具體數(shù)據(jù)，如表1所示。

表1 工控機(jī)的具體數(shù)據(jù)Tab.1 Specific Data of IPC

視頻服務(wù)器選用的型號(hào)為威視?？礢D6704，作為一種新型網(wǎng)絡(luò)視頻服務(wù)器，其采用多種標(biāo)準(zhǔn)編碼算法，包括MJPEG、MPEG2、MPEG4、H.264。該視頻服務(wù)器具有強(qiáng)大的兼容性，主要基于Linux操作系統(tǒng)，在FLASH中固定代碼，支持各種網(wǎng)絡(luò)協(xié)議。

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

2.2 設(shè)計(jì)反饋模塊

基于前饋—反饋控制結(jié)構(gòu)設(shè)計(jì)反饋模塊，反饋模塊的主要作用是對(duì)機(jī)械臂軌跡跟蹤信號(hào)進(jìn)行反饋。反饋模塊的具體結(jié)構(gòu)為反饋控制器、執(zhí)行器、前饋控制器。反饋模塊的具體運(yùn)行流程如下：首先向反饋控制器輸入目標(biāo)信號(hào)，接著反饋控制器向執(zhí)行器發(fā)送信號(hào)，并向前饋控制器發(fā)送信號(hào)，經(jīng)過(guò)測(cè)量與變送，該信號(hào)與執(zhí)行器輸出信號(hào)匯集為被控變量，通過(guò)被控變量對(duì)目標(biāo)進(jìn)行跟蹤，獲取跟蹤信號(hào)，并對(duì)其進(jìn)行測(cè)量與變送，向系統(tǒng)進(jìn)行反饋，實(shí)現(xiàn)軌跡跟蹤信號(hào)的反饋。

3 應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)軟件設(shè)計(jì)

3.1 軌跡跟蹤

通過(guò)攝像機(jī)標(biāo)定算法實(shí)現(xiàn)機(jī)械臂的軌跡跟蹤。通過(guò)攝像機(jī)標(biāo)定進(jìn)行機(jī)械臂軌跡跟蹤的具體流程如下：

用下式表示空間中的三維點(diǎn)到成像平面的對(duì)應(yīng)映射單應(yīng)性關(guān)系式：

式中：fx、fy—攝像機(jī)對(duì)應(yīng)內(nèi)參數(shù)f的左右坐標(biāo)；cx、cy—攝像機(jī)對(duì)應(yīng)內(nèi)參數(shù)c的左右坐標(biāo)。

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

對(duì)于機(jī)械臂軌跡跟蹤物體，可以利用上式對(duì)多幅圖像進(jìn)行計(jì)算，從而獲取軌跡跟蹤物體視場(chǎng)的旋轉(zhuǎn)、平移以及攝像機(jī)對(duì)應(yīng)內(nèi)參數(shù)。

3.2 基于直線插補(bǔ)算法的機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制

在機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制過(guò)程中，每個(gè)運(yùn)動(dòng)坐標(biāo)點(diǎn)都需要和給定的數(shù)據(jù)進(jìn)行比對(duì)，進(jìn)而決定下一步該怎么走。

直線插補(bǔ)算法能夠通過(guò)已知坐標(biāo)點(diǎn)確定運(yùn)動(dòng)軌跡，已知曲線上的某些數(shù)據(jù)，推導(dǎo)出中間點(diǎn)的方法，也稱為“數(shù)據(jù)點(diǎn)的密化”。

通過(guò)直線插補(bǔ)算法對(duì)之間的空間進(jìn)行數(shù)據(jù)密化，從而形成要求的輪廓軌跡，通過(guò)直線插補(bǔ)算法實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制。直線插補(bǔ)算法的具體步驟如下：

（1）進(jìn)行插補(bǔ)準(zhǔn)備，具體計(jì)算公式如下：

式中：K—插補(bǔ)周期中給定的進(jìn)給速率數(shù)；

ΔL—一個(gè)插補(bǔ)周期中的對(duì)應(yīng)進(jìn)給線段長(zhǎng)度；

L—直線段長(zhǎng)度；

Δx、Δy、Δz—x、y、z三個(gè)坐標(biāo)軸的位移增量；

xe、ye、ze—終點(diǎn)的坐標(biāo)軸坐標(biāo)。

（2）插補(bǔ)計(jì)算，具體公式如下：

式中：xi+1、yi+1、zi+1—插補(bǔ)結(jié)果坐標(biāo)點(diǎn)的坐標(biāo)值；

xi、yi、zi—插補(bǔ)點(diǎn)的坐標(biāo)值。

據(jù)此得出了插補(bǔ)點(diǎn)的坐標(biāo)值，還原了整體運(yùn)動(dòng)軌跡。通過(guò)反饋模塊將運(yùn)動(dòng)軌跡信息傳輸至工控模塊，實(shí)現(xiàn)機(jī)械臂軌跡跟蹤控制。

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

4.1 實(shí)驗(yàn)設(shè)計(jì)

為證明設(shè)計(jì)的應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)的性能，對(duì)其進(jìn)行實(shí)驗(yàn)測(cè)試。實(shí)驗(yàn)中采用的機(jī)械臂為直線式九自由度機(jī)械臂，其立體空間中的工作范圍為（800×800×800)mm，運(yùn)行速度的最大值為100mm/s。

直線式九自由度機(jī)械臂結(jié)構(gòu)，如圖1所示。

圖1 實(shí)驗(yàn)所用機(jī)械臂Fig.1 Manipulator Used in Experiment

4.2 實(shí)驗(yàn)方法與步驟

利用應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)對(duì)實(shí)驗(yàn)機(jī)械臂進(jìn)行軌跡跟蹤控制，在MATLAB軟件搭建實(shí)驗(yàn)平臺(tái)，使用上文提到的九自由度機(jī)械臂進(jìn)行圓弧軌跡跟蹤實(shí)驗(yàn)，獲取在機(jī)械臂位置增量為（10.52～30.52）mm 的范圍內(nèi)的控制時(shí)延數(shù)據(jù)以及曲線跟蹤仿真結(jié)果作為實(shí)驗(yàn)數(shù)據(jù)。采用原有的前饋—反饋控制方法、文獻(xiàn)[4-5]方法作為實(shí)驗(yàn)對(duì)比方法，驗(yàn)證機(jī)械臂位置增量為（10.52～30.52）mm的范圍內(nèi)的控制時(shí)延及軌跡跟蹤控制準(zhǔn)確性，實(shí)驗(yàn)流程圖，如圖2所示。

圖2 實(shí)驗(yàn)方法流程圖Fig.2 Flow Chart of Experimental Method

4.3 實(shí)驗(yàn)結(jié)果分析

在機(jī)械臂工作范圍內(nèi)選?。?00×200×200）mm的立體空間，讓其進(jìn)行圓弧軌跡實(shí)驗(yàn)，機(jī)械臂在范圍為（200×200×200）mm 的立體空間內(nèi)作隨機(jī)圓弧軌跡運(yùn)動(dòng)，A為運(yùn)動(dòng)路徑的起始點(diǎn)，B為運(yùn)動(dòng)終點(diǎn)。其運(yùn)動(dòng)路徑，如圖3所示。所設(shè)計(jì)應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)是在原有前饋—反饋控制系統(tǒng)的基礎(chǔ)上進(jìn)行改進(jìn)，所以將應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)和前饋—反饋控制系統(tǒng)、文獻(xiàn)[4-5]系統(tǒng)進(jìn)行對(duì)比實(shí)驗(yàn)，得到軌跡跟蹤路線圖，如圖4所示。

圖3 機(jī)械臂運(yùn)行軌跡Fig.3 Trajectory of Manipulator

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

由圖4可知，在九自由度機(jī)械臂圓弧軌跡跟蹤實(shí)驗(yàn)過(guò)程中，原有的前饋—反饋控制系統(tǒng)在跟蹤前期得到的跟蹤路徑與實(shí)際路徑較為一致，在跟蹤后期由于未能及時(shí)獲取誤差坐標(biāo)點(diǎn)，出現(xiàn)了跟蹤偏差較大的問(wèn)題；而所提方法得到的軌跡與機(jī)械臂的實(shí)際運(yùn)行軌跡較為一致，解決了原有方法存在的問(wèn)題，體現(xiàn)了其良好的跟蹤準(zhǔn)確性。而文獻(xiàn)[4]方法得到的軌跡具有一定的誤差，與設(shè)計(jì)圓弧軌跡相比有所偏離，文獻(xiàn)[5]方法在軌跡跟蹤過(guò)程中存在缺失，沒(méi)有得到一個(gè)完整的圓弧軌跡，且與設(shè)定軌跡的偏離誤差較大。

收集軌跡跟蹤路徑數(shù)據(jù)，得到軌跡跟蹤控制準(zhǔn)確性，如圖5所示。

圖5 軌跡跟蹤控制準(zhǔn)確性對(duì)比實(shí)驗(yàn)結(jié)果Fig.5 Experimental Results of Trajectory Tracking Control Accuracy Comparison

根據(jù)圖5可知，在機(jī)械臂35min運(yùn)動(dòng)時(shí)間內(nèi)，原有的前饋—反饋控制系統(tǒng)對(duì)機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制的準(zhǔn)確度平均值為83%，文獻(xiàn)[4]方法對(duì)機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制的準(zhǔn)確度平均值為69%，文獻(xiàn)[5]方法對(duì)機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制的準(zhǔn)確度平均值為63%，而所設(shè)計(jì)系統(tǒng)對(duì)機(jī)械臂運(yùn)動(dòng)軌跡跟蹤控制的準(zhǔn)確度平均值為93%。所設(shè)計(jì)系統(tǒng)應(yīng)用攝像機(jī)標(biāo)定準(zhǔn)確計(jì)算出了機(jī)械臂運(yùn)動(dòng)軌跡坐標(biāo)點(diǎn)，并通過(guò)反饋系統(tǒng)傳遞給控制模塊，因此能實(shí)現(xiàn)機(jī)械臂運(yùn)動(dòng)目標(biāo)的準(zhǔn)確跟蹤控制。

在機(jī)械臂位置增量為（10.52～20.52）mm的范圍內(nèi)，應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)對(duì)比系統(tǒng)的控制時(shí)延對(duì)比實(shí)驗(yàn)結(jié)果，如表2所示。

表2（10.52～20.52）mm范圍內(nèi)控制時(shí)延對(duì)比實(shí)驗(yàn)結(jié)果Tab.2 Comparative Experimental Results of Control Delay in the Range of（10.52～20.52）mm

根據(jù)表2結(jié)果可知，應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)的控制時(shí)延遠(yuǎn)低于文獻(xiàn)對(duì)比系統(tǒng)及原有的前饋—反饋控制系統(tǒng)。這是因?yàn)樗O(shè)計(jì)系統(tǒng)在前饋—反饋控制結(jié)構(gòu)的基礎(chǔ)上引入了直線插補(bǔ)算法，能夠快速求解運(yùn)動(dòng)控制插補(bǔ)點(diǎn)坐標(biāo)值并傳輸至工控模塊，有效降低了控制時(shí)延，提升了控制效率。

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

為提升機(jī)械臂軌跡跟蹤控制的準(zhǔn)確性及效率，提出應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)設(shè)計(jì)，設(shè)計(jì)工控模塊由主控單元、UVC 攝像頭、視頻服務(wù)器壓力檢測(cè)傳感器構(gòu)成；引入前饋-反饋控制結(jié)構(gòu)實(shí)現(xiàn)控制系統(tǒng)的快速反饋。軟件部分通過(guò)攝像機(jī)標(biāo)定算法實(shí)現(xiàn)機(jī)械臂的軌跡跟蹤，引入直線插補(bǔ)算法得出目標(biāo)物體準(zhǔn)確坐標(biāo)值，實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制。實(shí)驗(yàn)結(jié)果表明，應(yīng)用攝像機(jī)標(biāo)定的機(jī)械臂軌跡跟蹤控制系統(tǒng)能夠有效提升機(jī)械臂軌跡跟蹤控制的準(zhǔn)確性，通過(guò)直線插補(bǔ)算法實(shí)現(xiàn)了控制時(shí)延的降低。