驅控一體7DOF機械臂位置控制技術研究*

陳 罡,吳 菁,金貴陽,嚴 楠

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

0 引 言

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

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

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

DIETRICH A[4]、YANG Chen-guang[5]、HE Wei[6]、眭東亮[7]采用模糊邏輯、神經網絡、滑模變結構控制和自適應等智能控制算法,對機械臂多電機伺服系統進行了控制,解決了實際過程中的機械臂參數不確定問題。

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

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

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

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

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

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

1 驅控一體式控制器控制方案

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

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

由圖1可知:ARM負責高級運動路徑規劃和更高級的算法集成,實現與PC機、示教器、視頻和觸摸屏等外設的連接擴展;

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

FPGA負責7路電機的電流環控制和外圍接口信號的采集,實現傳感器采集數據之間的通訊、DSP之間的通訊和外圍擴展信號間的通訊,7路電機空間矢量脈寬調制(SVPWM)的生成,并根據指令控制對應電機的運動。

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

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

2 驅控一體式控制器

基于ARM+DSP+FPGA多電機協調伺服控制采用電流環、速度環和位置環3級反饋控制,其架構如圖2所示。

圖2 電機3環伺服控制架構

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

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

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

3 機械臂多電機協同控制策略

基于ARM+DSP+FPGA的機械臂多電機協同控制策略,筆者根據采樣周期和控制環時間的長短,對多電機控制的順序進行排序和分類,并合理規劃多電機控制時序。

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

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

Switch (Count)

{

Case 0: process State0;break;

Case 1: process State1;break;

Case 2: process State2;break;

Case 3: process State3;break;

}

每種控制狀態對應不同序列組合的電機控制和操作,完成對應狀態下各電機的任務,滿足7DOF機械臂伺服系統的要求。

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

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

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

電機各信號監測正常后,進入空閑等待,完成狀態0周期內任務,等待下一個中斷到來。

根據狀態0的任務,筆者設計機械臂7個電機控制時序的轉換表,如表1所示。

表1 多電機控制時序轉換

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

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

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

4 機械臂位置控制策略

4.1 機械臂動力學模型

對于n自由度的串聯性機械臂來說,其動力學模型可由拉格朗日動力學方程推導而來:

(1)

為便于后續控制律的運用,式(1)可以被表示為另一種非線性的標準系統方程:

(2)

4.2 非線性擾動觀測器

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

(3)

(4)

(5)

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

(6)

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

(7)

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

4.3 滑?？刂坡?/h3>

針對7DOF機械臂每個關節控制系統,筆者給出了誤差定義:

e=x-xd

(8)

式中:xd—機械臂給定的期望關節角參考信號。

機械臂連續快速非奇異終端滑模面函數為:

(9)

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

筆者將式(9)的一階導數代入式(1),可獲得系統的等效控制律:

(10)

為了使系統快速收斂,此處引入趨近律:

(11)

式中:0

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

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

(12)

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

u=ue+ur

(13)

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

圖4 機械臂位置控制策略

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

5 實驗及結果分析

5.1 實驗平臺

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

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

圖5 實驗測試平臺

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

表2 電機參數

表2中,列出了功率、極對數、轉動慣量、定子電阻、定子電感、摩擦系統、額定轉速等參數。

5.2 控制系統性能測試

為了驗證機械臂驅控一體控制器的性能,筆者對機械臂關節2電機進行了電機位置控制,如圖6所示。其中,圖6(a)為機械臂關節2電機位置控制響應曲線、速度響應和電機電流Iq的變化曲線,圖6(b)為位置控制達到穩態后的誤差曲線。

圖6 關節2電機位置控制

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

在機械臂末端空載狀態下,筆者對關節2電機進行速度控制實驗,如圖7所示。其中,圖7(a)為機械臂關節2電機的速度響應曲線、對應控制電流的響應曲線,圖7(b)為速度響應誤差曲線。

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

圖7 關節2電機速度控制

在機械臂末端空載狀態下,筆者對關節6電機進行恒力矩控制實驗,如圖8所示。其中,圖8(a)為機械臂關節6電機恒力矩控制實驗對應的電機電流、速度響應曲線,圖8(b)為關節6電機恒轉矩控制誤差變化曲線。

圖8 關節6電機恒轉矩控制

據圖8可以看出:電機電流能在瞬間達到設定值,達到穩態后,電機電流誤差較小,基本上都在-0.005 A～0.005 A之間,具有很好的恒力矩控制性能。

5.3 機械臂動態性能實驗

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

5.3.1 機械臂PID位置控制實驗

筆者在機械臂末端抓取5kg載荷狀態下,進行PID位置控制實驗,得到第4軸和第6軸電機負載狀態下PID補償控制的位置響應實驗曲線,如圖9所示。

圖9 帶負載關節4、6 PID位置控制

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

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

筆者在機械臂末端抓取5kg載荷狀態下,進行NDOB-FNTSMC位置控制實驗,如圖10所示。其中,圖10(a)為關節4NDOB-FNTSMC位置控制響應曲線,圖10(b)為關節6NDOB-FNTSMC位置控制響應曲線。

圖10 關節4、6 NDOB-FNTSMC位置控制

筆者得到了第4軸和第6軸電機負載狀態下NDOB-FNTSMC位置控制響應實驗曲線。

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

6 結束語

采用基于ARM+DSP+FPGA的硬件架構,筆者開發了7DOF機械臂驅控一體式控制器,實現了對機械臂的多電機協同控制;設計了基于非線性擾動觀測器的快速連續非奇異終端滑模復合控制策略;通過實驗,對驅控一體控制器方案和復合控制策略的性能進行了驗證。

研究結論如下:

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

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

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

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

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