驅(qū)控一體7DOF機械臂位置控制技術(shù)研究*

陳 罡,吳 菁,金貴陽,嚴(yán) 楠

(1.浙江機電職業(yè)技術(shù)學(xué)院 自動化學(xué)院,浙江 杭州 310053;2.寧波職業(yè)技術(shù)學(xué)院 機電工程學(xué)院,浙江 寧波 315800)

0 引 言

7DOF機械臂具有冗余自由度機構(gòu)特性及良好的運動靈活性,能夠克服關(guān)節(jié)限位,完成復(fù)雜避障任務(wù),因此,7DOF機械臂被廣泛應(yīng)用于醫(yī)療、工廠作業(yè)、救援、核電站輻射探測等多種復(fù)雜多變場景中。

7DOF機械臂是一個多輸入多輸出、非線性、強耦合的復(fù)雜系統(tǒng),在其運動過程中,存在諸如摩擦力、重力矩及慣性矩等外部擾動的影響,因此,系統(tǒng)存在參數(shù)攝動、外界干擾及未建模動態(tài)等不確定性[1-3]。

常規(guī)比例積分微分(PID)、模型預(yù)測控制、線性二次調(diào)節(jié)器等線性算法過于依賴精確模型,不能滿足高非線性情況下,機械臂系統(tǒng)快速、高精度的軌跡跟蹤性能要求。神經(jīng)網(wǎng)絡(luò)、模糊邏輯和自適應(yīng)等非線性控制算法都有非線性函數(shù)的擬合能力,可以用來處理不確定性。

DIETRICH A[4]、YANG Chen-guang[5]、HE Wei[6]、眭東亮[7]采用模糊邏輯、神經(jīng)網(wǎng)絡(luò)、滑模變結(jié)構(gòu)控制和自適應(yīng)等智能控制算法,對機械臂多電機伺服系統(tǒng)進(jìn)行了控制,解決了實際過程中的機械臂參數(shù)不確定問題。

這些非線性方法雖然可解決外部擾動未知、參數(shù)不確定和未建模動態(tài)等問題,但機械臂在控制過程中干擾信號的強度是未知的,仍會引起系統(tǒng)的抖振現(xiàn)象,故采用觀測器的方法對機械臂外部擾動進(jìn)行估計,并做出補償,將有利于降低控制系統(tǒng)的增益,獲得更好的軌跡跟蹤性能。

楊柳[8]提出了一種基于干擾觀測器的軌跡跟蹤魯棒控制算法,提升了四旋翼無人機控制過程中的抗干擾性能。鮮斌等人[9]利用擾動觀測器,估計和補償了地面效應(yīng)帶來的擾動,實現(xiàn)了對降落過程中的無人機的精確控制。

此外,擾動觀測器還被廣泛地用于永磁同步電機控制[10,11]、自主船舶協(xié)同路徑跟蹤控制[12]、機器人控制[13]中。但上述文獻(xiàn)中觀測器的觀測誤差是漸進(jìn)收斂的,存在速度較慢的問題。

為此,有必要引入一種非線性擾動觀測器(nonlinear disturbance observer-based, NDOB),以提高對機械臂外部未知擾動及建模不確定性的快速精確估計能力;將其與滑模變結(jié)構(gòu)控制結(jié)合,可以提高機械臂的軌跡跟蹤性能[14，15]。

但傳統(tǒng)滑模變結(jié)構(gòu)控制中存在符號函數(shù),容易引起抖振,在一定程度上降低了機械臂的軌跡跟蹤性能。冒建亮等人[16]設(shè)計了一種快速連續(xù)非奇異終端滑膜控制策略(fast-continuous nonsingular terminal sliding mode control, FNTSMC),采用連續(xù)可導(dǎo)滑模面,可有效削弱系統(tǒng)抖振現(xiàn)象,其性能優(yōu)于傳統(tǒng)滑模變結(jié)構(gòu)控制策略。

基于上述研究與分析,筆者首先采用ARM+DSP+FPGA硬件架構(gòu),設(shè)計驅(qū)控一體式控制器控制方案,保證控制器有足夠的計算性能;然后,針對冗余機械臂的不確定因素問題,將NDOB與FNTSMC相結(jié)合,提出一種NDOB-FNTSMC復(fù)合控制策略(其中,外界擾動中的可觀測部分采用非線性擾動觀測器來消除,不可觀測及系統(tǒng)建模不確定部分采用快速連續(xù)非奇異終端滑膜控制器進(jìn)行補償,以提高機械臂位置控制系統(tǒng)的收斂速度,削弱抖振現(xiàn)象);最后,通過實驗,驗證驅(qū)控一體式控制方案和復(fù)合控制策略的有效性,以滿足7DOF機械臂高速高精伺服控制及響應(yīng)要求。

1 驅(qū)控一體式控制器控制方案

為提升7DOF工業(yè)機械臂控制器的性能,結(jié)合ARM、DSP和FPGA各自的優(yōu)點,筆者設(shè)計了新型7DOF機械臂驅(qū)控一體控制方案[17],如圖1所示。

圖1 新型7DOF機械臂驅(qū)控一體控制方案

由圖1可知:ARM負(fù)責(zé)高級運動路徑規(guī)劃和更高級的算法集成,實現(xiàn)與PC機、示教器、視頻和觸摸屏等外設(shè)的連接擴展;

DSP接受來自ARM的控制指令,完成7路電機的位置、速度環(huán)控制,同時承擔(dān)伺服控制高級算法的實現(xiàn);

FPGA負(fù)責(zé)7路電機的電流環(huán)控制和外圍接口信號的采集,實現(xiàn)傳感器采集數(shù)據(jù)之間的通訊、DSP之間的通訊和外圍擴展信號間的通訊,7路電機空間矢量脈寬調(diào)制(SVPWM)的生成,并根據(jù)指令控制對應(yīng)電機的運動。

控制器對許多信號的檢測處理都是在FPGA模塊中實現(xiàn)的,信號監(jiān)測后進(jìn)行相應(yīng)的運算處理,自動檢測更新,以達(dá)到實時控制的效果。外部通訊通過中斷執(zhí)行,DSP與FPGA模塊進(jìn)行數(shù)據(jù)交換,直接讀取相應(yīng)寄存器中的處理完成的數(shù)據(jù),參與控制,減少通訊等待時間,有效減少了DSP的處理時間,大大提高了控制系統(tǒng)伺服帶寬[18]。

因此,采用雙核微處理器(ARM+DSP)和FPGA模塊結(jié)合,實現(xiàn)7路電機的實時控制是一個有效的解決手段。

2 驅(qū)控一體式控制器

基于ARM+DSP+FPGA多電機協(xié)調(diào)伺服控制采用電流環(huán)、速度環(huán)和位置環(huán)3級反饋控制,其架構(gòu)如圖2所示。

圖2 電機3環(huán)伺服控制架構(gòu)

從圖2中可以看出:當(dāng)機械臂示教器或控制器發(fā)出運動指令后,由ARM模塊通過運動學(xué)正解逆解、直線軌跡規(guī)劃、圓弧軌跡規(guī)劃等方法,生成運動軌跡的參考位置指令信號θr;DSP模塊承擔(dān)伺服系統(tǒng)位置環(huán)和速度環(huán)的控制算法實現(xiàn),生成參考電流ir;電流環(huán)控制在FPGA內(nèi)部硬件電路中完成,通過產(chǎn)生的SVPWM驅(qū)動信號,完成對功率模塊的開關(guān)控制,實現(xiàn)對多電機的運動控制。

電機實際位置信號通過電機位置編碼器讀取,速度反饋信號通過單位時間內(nèi)位置值的變化量求取,電機實際電流值通過電流采樣芯片采集。

FPGA模塊讀取伺服電機信息、電壓和電流等信息,檢測、監(jiān)控伺服電機,通過片外設(shè)備接口(external peripheral interface, EPI)總線送給DSP模塊,進(jìn)行數(shù)據(jù)運算處理。

3 機械臂多電機協(xié)同控制策略

基于ARM+DSP+FPGA的機械臂多電機協(xié)同控制策略,筆者根據(jù)采樣周期和控制環(huán)時間的長短,對多電機控制的順序進(jìn)行排序和分類,并合理規(guī)劃多電機控制時序。

在整個多電機運動控制周期中,筆者將7個電機控制劃分為4個狀態(tài),即根據(jù)計數(shù)器的遞增,設(shè)置如下4種狀態(tài):

Count=(Count++)%4;//每次中斷計數(shù)加1,0～3循環(huán)

Switch (Count)

{

Case 0: process State0;break;

Case 1: process State1;break;

Case 2: process State2;break;

Case 3: process State3;break;

}

每種控制狀態(tài)對應(yīng)不同序列組合的電機控制和操作,完成對應(yīng)狀態(tài)下各電機的任務(wù),滿足7DOF機械臂伺服系統(tǒng)的要求。

7DOF機械臂電機控制時序如圖3所示。

圖3 機械臂7個電機控制時序圖

從圖3可以看出:在第1個采樣周期內(nèi),電機1、3、5、7進(jìn)入工作狀態(tài),電機2、4和6處于空閑狀態(tài);電機1和5在DSP模塊中進(jìn)行位置環(huán)和速度環(huán)的閉環(huán)控制運算;電機3和7在DSP模塊中只進(jìn)行速度環(huán)的閉環(huán)控制運算。

電機各信號監(jiān)測正常后,進(jìn)入空閑等待,完成狀態(tài)0周期內(nèi)任務(wù),等待下一個中斷到來。

根據(jù)狀態(tài)0的任務(wù),筆者設(shè)計機械臂7個電機控制時序的轉(zhuǎn)換表,如表1所示。

表1 多電機控制時序轉(zhuǎn)換

注:P.V.—在DSP模塊位置環(huán)和速度環(huán)的運算;I.—在FPGA模塊中的電流環(huán)運算。

從表1中可知:在每個運動周期(state0～state3),機械臂7路電機完成4次電流采樣、2次速度采樣和1次位置采樣,因此,控制系統(tǒng)速度環(huán)的頻率是電流環(huán)的1/2,位置環(huán)的頻率是電流環(huán)的1/4。

由此可見,采用上述控制時序比順序控制有更多的帶寬,控制系統(tǒng)具有更高的控制性能。

4 機械臂位置控制策略

4.1 機械臂動力學(xué)模型

對于n自由度的串聯(lián)性機械臂來說,其動力學(xué)模型可由拉格朗日動力學(xué)方程推導(dǎo)而來:

(1)

為便于后續(xù)控制律的運用,式(1)可以被表示為另一種非線性的標(biāo)準(zhǔn)系統(tǒng)方程:

(2)

4.2 非線性擾動觀測器

擾動觀測器是機器人控制中很重要的一種手段。

(3)

(4)

(5)

基于以上趨近律,代入式(1),可得:

(6)

為了觀察機械臂系統(tǒng)中的未知擾動,結(jié)合式(2),非線性擾動觀測器將被設(shè)計為:

(7)

式中:o(x,z)—增益向量,也是給定的非線性函數(shù)。

4.3 滑?？刂坡?/h3>

針對7DOF機械臂每個關(guān)節(jié)控制系統(tǒng),筆者給出了誤差定義:

e=x-xd

(8)

式中:xd—機械臂給定的期望關(guān)節(jié)角參考信號。

機械臂連續(xù)快速非奇異終端滑模面函數(shù)為:

(9)

式中:β>0,1<γ<2,用sat飽和函數(shù)替代常用的sign分段函數(shù)可以有效削弱控制輸入的抖振現(xiàn)象。

筆者將式(9)的一階導(dǎo)數(shù)代入式(1),可獲得系統(tǒng)的等效控制律:

(10)

為了使系統(tǒng)快速收斂,此處引入趨近律:

(11)

式中:0

由此,可得基于趨近律的控制輸入為:

ur=h(x)-1(k1s-k2sign(s)p)

(12)

結(jié)合式(10,12),可得最終的控制律為:

u=ue+ur

(13)

綜合上述研究內(nèi)容,可得到機械臂位置的控制策略,如圖4所示。

圖4 機械臂位置控制策略

圖4中,采用非線性擾動觀測器,對模型不確定性及未知外部擾動進(jìn)行估計,并采用快速連續(xù)非奇異終端滑膜控制器,實現(xiàn)對機械臂系統(tǒng)的軌跡跟蹤控制。

5 實驗及結(jié)果分析

5.1 實驗平臺

7DOF機械臂實驗測試平臺如圖5所示。

圖5中,實驗平臺采用驅(qū)控一體式控制器,通過多控制端口控制機械臂驅(qū)動電機;采用高性能ARM+DSP+FPGA芯片設(shè)計的控制平臺和接口電路,對7路不同的電機統(tǒng)一控制;并通過智能功率模塊(intelligent power module, IPM),實現(xiàn)對7DOF機械臂多路伺服電機一體式驅(qū)動,完成多電機的一體控制和驅(qū)動,并實時檢測電機的動態(tài)跟蹤特性。

圖5 實驗測試平臺

機械臂7路伺服電機的參數(shù)如表2所示。

表2 電機參數(shù)

表2中,列出了功率、極對數(shù)、轉(zhuǎn)動慣量、定子電阻、定子電感、摩擦系統(tǒng)、額定轉(zhuǎn)速等參數(shù)。

5.2 控制系統(tǒng)性能測試

為了驗證機械臂驅(qū)控一體控制器的性能,筆者對機械臂關(guān)節(jié)2電機進(jìn)行了電機位置控制,如圖6所示。其中,圖6(a)為機械臂關(guān)節(jié)2電機位置控制響應(yīng)曲線、速度響應(yīng)和電機電流Iq的變化曲線,圖6(b)為位置控制達(dá)到穩(wěn)態(tài)后的誤差曲線。

圖6 關(guān)節(jié)2電機位置控制

從圖6中可以看出:在設(shè)定值為8 000狀態(tài)下,穩(wěn)態(tài)誤差不超過0.35,說明機械臂電機位置控制性能良好。

在機械臂末端空載狀態(tài)下,筆者對關(guān)節(jié)2電機進(jìn)行速度控制實驗,如圖7所示。其中,圖7(a)為機械臂關(guān)節(jié)2電機的速度響應(yīng)曲線、對應(yīng)控制電流的響應(yīng)曲線,圖7(b)為速度響應(yīng)誤差曲線。

從圖7中曲線變化情況可以得出:在速度控制模式下,電機速度能快速地達(dá)到穩(wěn)定狀態(tài),達(dá)到穩(wěn)態(tài)后,速度誤差在0附近變化,波動在1%以內(nèi),速度變化較小,具有很好的控制精度。

圖7 關(guān)節(jié)2電機速度控制

在機械臂末端空載狀態(tài)下,筆者對關(guān)節(jié)6電機進(jìn)行恒力矩控制實驗,如圖8所示。其中,圖8(a)為機械臂關(guān)節(jié)6電機恒力矩控制實驗對應(yīng)的電機電流、速度響應(yīng)曲線,圖8(b)為關(guān)節(jié)6電機恒轉(zhuǎn)矩控制誤差變化曲線。

圖8 關(guān)節(jié)6電機恒轉(zhuǎn)矩控制

據(jù)圖8可以看出:電機電流能在瞬間達(dá)到設(shè)定值,達(dá)到穩(wěn)態(tài)后,電機電流誤差較小,基本上都在-0.005 A～0.005 A之間,具有很好的恒力矩控制性能。

5.3 機械臂動態(tài)性能實驗

針對機械臂在運動中存在摩擦力、重力矩等外擾動的影響,筆者分別進(jìn)行機械臂PID位置控制、NDOB-FNTSMC位置控制實驗,以研究機械臂的動態(tài)響應(yīng)性能。

5.3.1 機械臂PID位置控制實驗

筆者在機械臂末端抓取5kg載荷狀態(tài)下,進(jìn)行PID位置控制實驗,得到第4軸和第6軸電機負(fù)載狀態(tài)下PID補償控制的位置響應(yīng)實驗曲線,如圖9所示。

圖9 帶負(fù)載關(guān)節(jié)4、6 PID位置控制

圖9(a)為關(guān)節(jié)4PID位置控制響應(yīng)曲線,圖9(b)為關(guān)節(jié)6PID位置控制響應(yīng)曲線。機械臂關(guān)節(jié)4、6位置動態(tài)響應(yīng)基本一致,兩者超調(diào)量相近;關(guān)節(jié)6做俯仰動作,關(guān)節(jié)6電機速度和電流波動比關(guān)節(jié)4明顯。

5.3.2 機械臂NDOB-FNTSMC位置控制實驗

筆者在機械臂末端抓取5kg載荷狀態(tài)下,進(jìn)行NDOB-FNTSMC位置控制實驗,如圖10所示。其中,圖10(a)為關(guān)節(jié)4NDOB-FNTSMC位置控制響應(yīng)曲線,圖10(b)為關(guān)節(jié)6NDOB-FNTSMC位置控制響應(yīng)曲線。

圖10 關(guān)節(jié)4、6 NDOB-FNTSMC位置控制

筆者得到了第4軸和第6軸電機負(fù)載狀態(tài)下NDOB-FNTSMC位置控制響應(yīng)實驗曲線。

對比圖(9，10)的實驗曲線可以看出:采用NDOB-FNTSMC,起始電流較大,系統(tǒng)響應(yīng)更快,能提前0.25 s到達(dá)控制位置,穩(wěn)態(tài)時超調(diào)量減少,檢測到的關(guān)節(jié)電機速度波動較小,電流波動也較小,性能相比圖9的常規(guī)PID控制提升了10%～15%,一體式控制器伺服驅(qū)動性能良好。

6 結(jié)束語

采用基于ARM+DSP+FPGA的硬件架構(gòu),筆者開發(fā)了7DOF機械臂驅(qū)控一體式控制器,實現(xiàn)了對機械臂的多電機協(xié)同控制;設(shè)計了基于非線性擾動觀測器的快速連續(xù)非奇異終端滑模復(fù)合控制策略;通過實驗,對驅(qū)控一體控制器方案和復(fù)合控制策略的性能進(jìn)行了驗證。

研究結(jié)論如下:

(1)基于ARM+DSP+FPGA硬件架構(gòu)的7DOF機械臂驅(qū)控一體控制方案具有較高的計算性能,從計算分析可知,控制系統(tǒng)速度環(huán)的頻率是電流環(huán)的1/2,位置環(huán)的頻率是電流環(huán)的1/4,相比于順序控制獲得了更多的帶寬;

(2)根據(jù)機械臂空載實驗,機械臂關(guān)節(jié)的位置控制、速度控制、恒力矩控制最終的誤差都在1%以內(nèi),獲得了較好的控制性能;

(3)基于非線性擾動觀測器的快速連續(xù)非奇異終端滑模控制策略,解決了模型不確定和未知擾動的干擾問題,削弱控制系統(tǒng)的抖振現(xiàn)象,確保機械臂軌跡跟蹤誤差能在限定時間內(nèi)收斂至特定區(qū)域,相比PID控制策略,超調(diào)量、系統(tǒng)響應(yīng)速度等性能提高了10%～15%,具有更好的位置動態(tài)響應(yīng)性能,能較好的滿足機械臂運動控制要求。

7DOF機械臂軌跡跟蹤控制系統(tǒng)的設(shè)計是工業(yè)控制領(lǐng)域一個實用的研究課題,筆者主要解決模型不確定和未知擾動下的有限時間位置跟蹤問題。

在未來的研究過程中,筆者還將對機械臂軌跡跟蹤控制系統(tǒng)的時延、執(zhí)行器飽和等問題展開進(jìn)一步的研究。