本發(fā)明涉及一種基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)及其方法，屬于自動控制技術(shù)領(lǐng)域。

背景技術(shù)：

隨著科技進(jìn)步，在工業(yè)生產(chǎn)領(lǐng)域越來越多的應(yīng)用機(jī)械臂完成生產(chǎn)任務(wù)，對其控制性能要求在不斷提高，必須保證其執(zhí)行器、傳感器及其它元件的可靠性與準(zhǔn)確性。特別是航空航天、航海等領(lǐng)域，特定工作環(huán)境對其控制系統(tǒng)的運(yùn)行安全提出了更嚴(yán)格的規(guī)范。但控制系統(tǒng)在運(yùn)行過程中容易造成設(shè)備磨損或遭受外界干擾，而且其力學(xué)執(zhí)行器具有復(fù)雜非線性特點(diǎn)，長期處于工作狀態(tài)極易發(fā)生故障情況。容錯控制策略能夠很好的消除執(zhí)行器故障帶來的不良影響，但是一般的容錯控制方法沒有考慮系統(tǒng)本身具有的動力學(xué)約束，很難達(dá)到穩(wěn)定性和快速性的統(tǒng)一。機(jī)械臂在實(shí)際應(yīng)用過程中其系統(tǒng)控制受限于系統(tǒng)構(gòu)造和驅(qū)動性能，實(shí)現(xiàn)在系統(tǒng)約束情況下快速精確地系統(tǒng)控制也就及其重要。

而且，實(shí)際系統(tǒng)容易受未建模狀態(tài)、未知參數(shù)及外界干擾等不確定因素的影響，不確定系統(tǒng)的控制問題已成為現(xiàn)代控制中一個重要探究方向。在實(shí)際應(yīng)用中，機(jī)械臂系統(tǒng)存在著建模不精確及外界干擾等不確定性因素，是一個典型的強(qiáng)耦合、高度非線性的不確定復(fù)雜系統(tǒng)，因此，在機(jī)械臂控制系統(tǒng)設(shè)計(jì)時還應(yīng)該考慮不確定性因素及系統(tǒng)復(fù)雜的動力學(xué)特性對其控制品質(zhì)帶來的影響，這給機(jī)械臂的容錯控制帶來困難與不便。

解決不確定機(jī)械臂系統(tǒng)控制器設(shè)計(jì)復(fù)雜，故障診斷與容錯系統(tǒng)強(qiáng)耦合、高度非線性的技術(shù)難題，提供一種可滿足實(shí)際運(yùn)行過程中存在的系統(tǒng)約束需求的執(zhí)行器恒偏差故障容錯控制勢在必行。

技術(shù)實(shí)現(xiàn)要素：

為克服不確定機(jī)械臂系統(tǒng)控制器設(shè)計(jì)復(fù)雜，故障診斷與容錯系統(tǒng)強(qiáng)耦合、高度非線性的技術(shù)難題，滿足實(shí)際運(yùn)行過程中存在的系統(tǒng)約束條件，本發(fā)明提供一種基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)及其方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

本發(fā)明一種基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)，該系統(tǒng)包括：

機(jī)械臂執(zhí)行器離散故障模型，采用泰勒級數(shù)展開式建立；

故障診斷(fdd)單元，其基于自適應(yīng)觀測器對故障主動進(jìn)行診斷并估計(jì)故障信息，并將估計(jì)的故障信息引入機(jī)械臂執(zhí)行器離散故障模型，實(shí)現(xiàn)主動容錯；

機(jī)械臂容錯控制器包括軌跡規(guī)劃層和跟蹤控制層，兩層之間交互信息，根據(jù)每層不同的控制目標(biāo)，分別設(shè)計(jì)控制器，其中，所述軌跡規(guī)劃層設(shè)計(jì)軌跡規(guī)劃控制器，規(guī)劃出一條滿足系統(tǒng)約束條件的最優(yōu)參考軌跡；所述跟蹤控制層為提高計(jì)算效率，采用簡便方法設(shè)計(jì)軌跡跟蹤控制器實(shí)現(xiàn)對參考軌跡的追蹤；

預(yù)測故障模型，在軌跡規(guī)劃層，利用mpc算法實(shí)現(xiàn)對故障機(jī)械臂預(yù)測時域內(nèi)最優(yōu)軌跡的規(guī)劃，將得到的規(guī)劃軌跡傳遞給跟蹤控制層；

預(yù)測滑模故障模型，在跟蹤控制層，將滑?？刂埔肽Ｐ皖A(yù)測控制設(shè)計(jì)中，利用跟蹤誤差向量構(gòu)造預(yù)測滑模故障模型，求解優(yōu)化問題得到控制侓，使故障機(jī)械臂系統(tǒng)各關(guān)節(jié)依然快速到達(dá)目標(biāo)位置；

反饋校正機(jī)制補(bǔ)償系統(tǒng)，對實(shí)際系統(tǒng)存在的不確定因素進(jìn)行補(bǔ)償。

本發(fā)明一種基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)的方法，其包括以下步驟：

步驟一、建立執(zhí)行器故障下離散系統(tǒng)模型：針對式(1)所示機(jī)械臂執(zhí)行器恒偏差故障的動力學(xué)模型，建立機(jī)械臂執(zhí)行器恒偏差故障的狀態(tài)空間方程式(2)，采用泰勒級數(shù)展開式對式(2)進(jìn)行離散化處理，得到式(3)，

機(jī)械臂輸入約束

τmin≤τ≤τmax

其中，是關(guān)節(jié)位置向量(速度向量，加速度向量)，m(q)∈r^2×2是正定對稱慣性矩陣，是科里奧利和離心扭矩向量，g(q)∈r²是重力引起的扭矩向量，τ∈r²為機(jī)械臂各關(guān)節(jié)控制力矩向量，假設(shè)各關(guān)節(jié)控制力矩相對獨(dú)立。為未知的恒偏差故障函數(shù)項(xiàng)，為故障函數(shù)，t為故障發(fā)生時間。假設(shè)故障f范數(shù)有界，即||f||≤ρ，ρ為常數(shù)。ω是系統(tǒng)不確定因素。τmax，τmin分別是輸入力矩的上界和下界。

機(jī)械臂執(zhí)行器發(fā)生恒偏差故障的動力學(xué)模型：

機(jī)械臂輸入約束

τmin≤τ≤τmax

其中，是系統(tǒng)狀態(tài)向量，y是系統(tǒng)輸出，p(x1)＝m^-1(q)。

其中，

x(k)表示第k個采樣時刻狀態(tài)量，y(k)表示第k個采樣時刻的輸出量，τ(k)表示第k個采樣時刻的控制輸入量；

在軌跡規(guī)劃層，將式(3)作為預(yù)測故障模型，基于模型預(yù)測控制進(jìn)行理論推導(dǎo)得到優(yōu)化控制量，根據(jù)優(yōu)化控制量求得預(yù)測時域內(nèi)使得性能指標(biāo)最優(yōu)的預(yù)測狀態(tài)值作為跟蹤控制層的期望值。

在跟蹤控制層，將滑?？刂埔肽Ｐ皖A(yù)測控制設(shè)計(jì)中，利用跟蹤誤差向量構(gòu)造預(yù)測滑模故障模型式(4)，求解優(yōu)化問題得到控制侓，使故障機(jī)械臂系統(tǒng)各關(guān)節(jié)依然快速到達(dá)目標(biāo)位置。

令跟蹤誤差向量e(k)＝x(k)-xr(k)，其中，x(k)為當(dāng)前時刻系統(tǒng)狀態(tài)量，xr(k+1)為期望值，xr(k)為下一時刻期望值。

定義則跟蹤誤差動態(tài)方程為

定義滑模函數(shù)為

s(k)＝he(k)

其中，h∈r^m×2n為待設(shè)計(jì)參數(shù)。

由于滑動模態(tài)與系統(tǒng)不確定性和干擾無關(guān)，則預(yù)測滑模故障模型為

步驟二、故障診斷(fdd)單元設(shè)計(jì)：設(shè)計(jì)如下故障觀測器如式(5)，由此估計(jì)故障信息，

其中，表示x(k)的估計(jì)值，表示y(k)的估計(jì)值，表示故障矩陣f(k)的估計(jì)值，l表示適當(dāng)維數(shù)的觀測器增益矩陣，選擇合適的增益矩陣l設(shè)計(jì)故障估計(jì)觀測器得到故障信息估計(jì)值

步驟三、反饋校正機(jī)制補(bǔ)償系統(tǒng)設(shè)計(jì)：將得到的當(dāng)前時刻實(shí)際值與和過去時刻預(yù)測值作比較，得到預(yù)測誤差，之后通過所得誤差值對未來時刻的預(yù)測值進(jìn)行反饋校正，能很好的對系統(tǒng)不確定性進(jìn)行補(bǔ)償；

步驟四、雙層結(jié)構(gòu)容錯控制器設(shè)計(jì)：運(yùn)用分層優(yōu)化控制思想，軌跡規(guī)劃層考慮目標(biāo)位置、系統(tǒng)約束等條件，設(shè)計(jì)滿足系統(tǒng)條件的最優(yōu)軌跡，跟蹤控制層實(shí)現(xiàn)對最優(yōu)軌跡的精確跟蹤。

進(jìn)一步地,采用模型預(yù)測控制算法設(shè)計(jì)上層的軌跡規(guī)劃控制器。

根據(jù)系統(tǒng)自身的物理約束條件和人為要求的目標(biāo)位置，為跟蹤控制層設(shè)計(jì)出一條包含機(jī)械臂各關(guān)節(jié)的位置和速度的最優(yōu)參考軌跡。

在模型預(yù)測控制框架下，構(gòu)造性能指標(biāo)函數(shù)式(6)，求出控制時域內(nèi)確保系統(tǒng)輸出最優(yōu)的控制量，由此計(jì)算出機(jī)械臂下一時刻最優(yōu)參考軌跡。

ey(k+j)＝y(tǒng)(k)-y(k|k-j)j＝1,2,…,p

yopt(k+j)＝y(tǒng)p(k+j)+ey(k+j)j＝1,2,…,p

τmin≤τ(k+j-1|k)≤τmaxj＝1,…,m

τ(k+j-1|k)＝τ(k+m-1|k)m<j≤p

其中，y(k)是k時刻系統(tǒng)實(shí)際值，y(k|k-j)是過去時刻對k時刻預(yù)測值，ey(k+j)是預(yù)測輸出誤差，yp(k+j)是模型預(yù)測輸出，yopt(k+j)是機(jī)械臂的校正輸出，τ(k+j-1|k)是待優(yōu)化控制變量，qset是關(guān)節(jié)目標(biāo)位置向量，p和m分別是預(yù)測時域和控制時域，qi，ri(qi＞0，ri＞0)分別是自定義的誤差加權(quán)系數(shù)和力矩加權(quán)系數(shù)。

進(jìn)一步地,采用預(yù)測滑模控制算法設(shè)計(jì)下層的軌跡跟蹤控制器。

構(gòu)造性能指標(biāo)函數(shù)式(7)，求出控制時域內(nèi)確保系統(tǒng)輸出最優(yōu)的控制侓，并將計(jì)算的第一個分量τ(k|k)作用于機(jī)械臂：

e(k)＝x(k)-xr(k)

s(k)＝he(k)

es(k+j)＝s(k)-s(k|k-j)j＝1,2,…,p

其中，x(k)是k時刻系統(tǒng)狀態(tài)量，xr(k)是約束預(yù)測規(guī)劃控制器傳遞給軌跡跟蹤控制器在下一時刻要跟蹤的期望，e(k)為跟蹤誤差向量，s(k)是滑模函數(shù)，表示k時刻滑模預(yù)測值，s(k|k-j)是過去時刻對k時刻滑模預(yù)測值，es(k+j)是預(yù)測誤差，sp(k+j)是預(yù)測滑模輸出，是機(jī)械臂校正輸出，τ(k+j-1|k)是待優(yōu)化控制變量，p和m分別是預(yù)測時域和控制時域；qi，ri(qi＞0，ri＞0)分別是自定義的誤差加權(quán)系數(shù)和力矩加權(quán)系數(shù)。

與現(xiàn)有技術(shù)相比，本發(fā)明有益效果是：

本發(fā)明在保證機(jī)械臂具有足夠動力性能的同時，具有一定的自主運(yùn)動能力；本發(fā)明通過自適應(yīng)觀測器對系統(tǒng)執(zhí)行器恒偏差故障進(jìn)行檢測，得到故障估計(jì)信息代替故障信號實(shí)現(xiàn)主動容錯；利用反饋校正機(jī)制有效地克服了機(jī)械臂難以精確建模和內(nèi)部動態(tài)不穩(wěn)定的特性，使系統(tǒng)具有強(qiáng)魯棒性，提高軌跡跟蹤精度；機(jī)械臂容錯控制器包括軌跡規(guī)劃層和跟蹤控制層，每層根據(jù)不同的控制目標(biāo)，分別基于不同方法設(shè)計(jì)控制器，兩層之間交互信息，共同作用，能夠滿足系統(tǒng)各種要求，更好地處理輸入力矩約束條件以及抑制系統(tǒng)不確定性，實(shí)現(xiàn)復(fù)雜系統(tǒng)的容錯控制目標(biāo)過程層次分明。

附圖說明

圖1是本發(fā)明雙層結(jié)構(gòu)容錯控制的結(jié)構(gòu)圖；

圖2是本發(fā)明雙層結(jié)構(gòu)容錯控制的故障真實(shí)值和估計(jì)值對比圖；

圖3是本發(fā)明雙層結(jié)構(gòu)容錯控制的故障系統(tǒng)關(guān)節(jié)1期望位置和軌跡曲線圖；

圖4是本發(fā)明雙層結(jié)構(gòu)容錯控制的故障系統(tǒng)關(guān)節(jié)2期望位置和軌跡曲線圖；

圖5是本發(fā)明雙層結(jié)構(gòu)容錯控制的故障系統(tǒng)關(guān)節(jié)控制輸入力矩圖。

具體實(shí)施方式

為更直觀了解本發(fā)明，結(jié)合附圖給出該發(fā)明的詳細(xì)說明。

如圖1所示，本發(fā)明基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)，其利用泰勒級數(shù)展開式建立執(zhí)行器恒偏差故障下機(jī)械臂執(zhí)行器離散故障模型，以減小計(jì)算量,提高計(jì)算效率，提高實(shí)時性。

機(jī)械臂容錯控制器包括軌跡規(guī)劃層和跟蹤控制層，兩層之間交互信息，根據(jù)每層不同的控制目標(biāo)，分別設(shè)計(jì)控制器，對問題更有針對性，其中，軌跡規(guī)劃層基于mpc控制綜合質(zhì)量高及良好的處理約束的能力的優(yōu)點(diǎn)，設(shè)計(jì)軌跡規(guī)劃控制器，規(guī)劃出一條滿足系統(tǒng)約束條件的最優(yōu)參考軌跡。以目標(biāo)位置、約束條件和機(jī)械臂當(dāng)前時刻的狀態(tài)信息為輸入?yún)?shù)，設(shè)計(jì)相應(yīng)的性能指標(biāo)函數(shù)，滿足系統(tǒng)約束條件。跟蹤控制層為提高計(jì)算效率，采用簡便方法設(shè)計(jì)軌跡跟蹤控制器實(shí)現(xiàn)對參考軌跡的追蹤。

基于自適應(yīng)觀測器設(shè)計(jì)離散系統(tǒng)的故障診斷(fdd)單元，以基于自適應(yīng)觀測器對故障主動進(jìn)行診斷并估計(jì)故障信息，將故障估計(jì)信息代入機(jī)械臂離散故障模型，實(shí)現(xiàn)主動容錯。

根據(jù)分層優(yōu)化結(jié)構(gòu)分別設(shè)計(jì)預(yù)測故障模型和預(yù)測滑模故障模型。在軌跡規(guī)劃層，預(yù)測故障模型利用mpc算法實(shí)現(xiàn)對故障機(jī)械臂預(yù)測時域內(nèi)最優(yōu)軌跡的規(guī)劃，將得到的規(guī)劃軌跡傳遞給跟蹤控制層；

在跟蹤控制層，跟蹤控制層不考慮目標(biāo)位置和約束條件等外部需求，采用盡可能使簡便易行的控制策略，預(yù)測滑模故障模型將滑?？刂埔肽Ｐ皖A(yù)測控制設(shè)計(jì)中，利用跟蹤誤差向量構(gòu)造預(yù)測滑模故障模型，基于預(yù)測滑?？刂频膬?yōu)良控制性能和強(qiáng)魯棒性，設(shè)計(jì)軌跡跟蹤控制器，實(shí)現(xiàn)對規(guī)劃軌跡的快速準(zhǔn)確跟蹤,求解優(yōu)化問題得到控制侓，使故障機(jī)械臂系統(tǒng)各關(guān)節(jié)依然快速到達(dá)目標(biāo)位置。。

考慮實(shí)際系統(tǒng)存在的不確定因素，利用反饋校正機(jī)制補(bǔ)償系統(tǒng)補(bǔ)償系統(tǒng)不確定因素的影響，提高系統(tǒng)強(qiáng)魯棒性。

這種雙層結(jié)構(gòu)的容錯控制能很好地處理存在的系統(tǒng)約束條件，具有強(qiáng)魯棒性，能有效解決復(fù)雜機(jī)械臂執(zhí)行器恒偏差故障問題，保障整個系統(tǒng)的穩(wěn)定性和控制性能。

本發(fā)明基于雙層結(jié)構(gòu)的機(jī)械臂執(zhí)行器故障容錯控制系統(tǒng)的關(guān)鍵處理方法及過程如下：

步驟一、建立系統(tǒng)故障模型：

一類具有不確定因素的兩關(guān)節(jié)機(jī)械臂系統(tǒng)，系統(tǒng)執(zhí)行器恒偏差故障時，根據(jù)lagrange一euler方程建立機(jī)械臂故障動力學(xué)模型

機(jī)械臂輸入約束

τmin≤τ≤τmax

其中，是關(guān)節(jié)位置向量(速度向量，加速度向量)，m(q)∈r^2×2是正定對稱慣性矩陣，是科里奧利和離心扭矩向量，g(q)∈r²是重力引起的扭矩向量，τ∈r²為機(jī)械臂各關(guān)節(jié)控制力矩向量，假設(shè)各關(guān)節(jié)控制力矩相對獨(dú)立。為未知的恒偏差故障函數(shù)項(xiàng)，為故障函數(shù)，t為故障發(fā)生時間。假設(shè)故障f范數(shù)有界，即||f||≤ρ，ρ為常數(shù)。ω是系統(tǒng)不確定因素。τmax，τmin分別是輸入力矩的上界和下界。

為了減小機(jī)械臂強(qiáng)耦合、高度非線性帶來的難度，針對式(2)所示機(jī)械臂執(zhí)行器恒偏差故障的狀態(tài)空間方程，采用泰勒級數(shù)展開的方式進(jìn)行離散，得到式(3)，使模型既能準(zhǔn)確描述機(jī)械臂動態(tài)特性，又能減少計(jì)算量，快速實(shí)現(xiàn)控制算法。式(3)為預(yù)測故障模型。

機(jī)械臂系統(tǒng)執(zhí)行器發(fā)生恒偏差故障的狀態(tài)空間方程

機(jī)械臂輸入約束

τmin≤τ≤τmax

其中，是系統(tǒng)狀態(tài)向量，y是系統(tǒng)輸出，p(x1)＝m^-1(q)。

其中，

x(k)表示第k個采樣時刻狀態(tài)量，y(k)表示第k個采樣時刻的輸出量，τ(k)表示第k個采樣時刻的控制輸入量。為保證預(yù)測模型的準(zhǔn)確性，增強(qiáng)控制系統(tǒng)的魯棒性，控制器將得到的實(shí)時的系統(tǒng)狀態(tài)信息引入機(jī)械臂狀態(tài)空間模型，減小模型失配的影響。

在軌跡規(guī)劃層，將式(3)作為預(yù)測故障模型，基于模型預(yù)測控制進(jìn)行理論推導(dǎo)得到優(yōu)化控制量，根據(jù)優(yōu)化控制量求得預(yù)測時域內(nèi)使得性能指標(biāo)最優(yōu)的預(yù)測狀態(tài)值作為跟蹤控制層的期望值。

在跟蹤控制層，將滑?？刂埔肽Ｐ皖A(yù)測控制設(shè)計(jì)中，利用跟蹤誤差向量構(gòu)造預(yù)測滑模故障模型，求解優(yōu)化問題得到控制侓，使故障機(jī)械臂系統(tǒng)各關(guān)節(jié)依然快速到達(dá)目標(biāo)位置。

記x(k)為當(dāng)前時刻系統(tǒng)狀態(tài)量，xr(k+1)為期望值，xr(k)為下一時刻期望值。令跟蹤誤差向量e(k)＝x(k)-xr(k)，并定義

則跟蹤誤差動態(tài)方程為

定義滑模函數(shù)為

s(k)＝he(k)

其中，h∈r^m×2n為待設(shè)計(jì)參數(shù)。

由于滑動模態(tài)與系統(tǒng)不確定性和干擾無關(guān)，則預(yù)測滑模故障模型為

步驟二、故障診斷(fdd)單元設(shè)計(jì)：

基于容錯控制的需要，利用自適應(yīng)觀測器綜合性能好、能很好的抑制系統(tǒng)不確定因素等優(yōu)點(diǎn)，設(shè)計(jì)如下故障觀測器如式(5)，由此估計(jì)故障信息。

其中，表示x(k)的估計(jì)值，表示y(k)的估計(jì)值，表示故障矩陣f(k)的估計(jì)值，l表示適當(dāng)維數(shù)的觀測器增益矩陣。選擇合適的增益矩陣l基于觀測器誤差動態(tài)方程進(jìn)行理論推導(dǎo)得到故障信息估計(jì)值

步驟三、反饋校正機(jī)制：

對于軌跡規(guī)劃層的預(yù)測故障模型，首先檢測當(dāng)前k時刻的系統(tǒng)實(shí)際值y(k)和過去時刻預(yù)測值y(k|k-p)之間的誤差，將所得誤差和k+p時刻的預(yù)測滑模值y(k+p)相加得到k+p時刻的閉環(huán)預(yù)測輸出值

其中，ey(k)＝y(tǒng)(k)-y(k|k-p)，σi為校正系數(shù)，一般來說0＜σi＜1。

對于跟蹤控制層的預(yù)測滑模故障模型，首先計(jì)算得到k時刻滑模函數(shù)當(dāng)前值s(k)和過去時刻預(yù)測值s(k|k-p)之間的偏差，之后通過所得偏差值對k+p時刻的預(yù)測滑模值s(k+p)進(jìn)行反饋校正，則k+p時刻的閉環(huán)校正預(yù)測滑模輸出

其中，es(k)＝s(k)-s(k|k-p)。hi為校正系數(shù)，一般來說0＜hi＜1。

采用反饋校正機(jī)制能很好的補(bǔ)償系統(tǒng)不確定性，提升系統(tǒng)控制性能。

步驟四、雙層結(jié)構(gòu)容錯控制器設(shè)計(jì)

運(yùn)用雙層結(jié)構(gòu)優(yōu)化控制思想，整個容錯控制系統(tǒng)由軌跡規(guī)劃層和跟蹤控制層組成，根據(jù)每層不同的控制目標(biāo)，分別設(shè)計(jì)控制器。

①軌跡規(guī)劃控制器

采用模型預(yù)測控制算法設(shè)計(jì)上層軌跡規(guī)劃控制器，根據(jù)系統(tǒng)自身的物理約束條件和人要求的目標(biāo)位置，為跟蹤控制層設(shè)計(jì)出一條包含機(jī)械臂各關(guān)節(jié)的位置和速度的最優(yōu)參考軌跡。

在模型預(yù)測控制框架下，構(gòu)造性能指標(biāo)函數(shù)式(6)，求出控制時域內(nèi)確保系統(tǒng)輸出最優(yōu)的控制量，由此計(jì)算出機(jī)械臂下一時刻最優(yōu)參考軌跡。

ey(k+j)＝y(tǒng)(k)-y(k|k-j)j＝1,2,…,p

yopt(k+j)＝y(tǒng)p(k+j)+ey(k+j)j＝1,2,…,p

τmin≤τ(k+j-1|k)≤τmaxj＝1,…,m

τ(k+j-1|k)＝τ(k+m-1|k)m<j≤p

其中，y(k)是k時刻系統(tǒng)實(shí)際值，y(k|k-p)是過去時刻對k時刻預(yù)測值，ey(k+j)是預(yù)測輸出誤差，yp(k+j)是模型預(yù)測輸出，yopt(k+j)是機(jī)械臂的校正輸出，τ(k+j-1|k)是待優(yōu)化控制變量，qset是關(guān)節(jié)目標(biāo)位置向量，p和m分別是預(yù)測時域和控制時域，qi，ri(qi＞0，ri＞0)分別是自定義的誤差加權(quán)系數(shù)和力矩加權(quán)系數(shù)。

由于存在系統(tǒng)輸入約束，將性能指標(biāo)函數(shù)優(yōu)化問題式(6)通過公式變形化成標(biāo)準(zhǔn)的含約束條件的二次規(guī)劃問題式，該式可以求解得到控制時域內(nèi)確保系統(tǒng)輸出最優(yōu)的數(shù)值解，由此獲得使得性能最優(yōu)的預(yù)測狀態(tài)值xopt(k)作為下一時刻要跟蹤的參考軌跡。為了保證系統(tǒng)控制性能要求，依然采用滾動優(yōu)化策略，軌跡規(guī)劃控制器只將下一時刻的預(yù)測輸出值xopt(k+1)傳遞給軌跡跟蹤控制器作為其參考軌跡xr(k+1)，兩層共同作用，實(shí)現(xiàn)機(jī)械臂對目標(biāo)位置的精準(zhǔn)跟蹤。到下一采樣時刻，軌跡規(guī)劃控制器重新設(shè)計(jì)一條最優(yōu)軌跡。

②軌跡跟蹤控制器

若得到機(jī)械臂運(yùn)動軌跡和初始狀態(tài)以及執(zhí)行器故障估計(jì)信息，可直接求出系統(tǒng)力矩，但這種方法要求精確的系統(tǒng)模型，且無法對擾動進(jìn)行補(bǔ)償，系統(tǒng)容易發(fā)散。基于預(yù)測滑?？刂屏己玫聂敯粜?，采用預(yù)測滑?？刂扑惴ㄔO(shè)計(jì)下層軌跡跟蹤控制器，抑制系統(tǒng)不確定因素，實(shí)現(xiàn)對上層最優(yōu)參考軌跡的追蹤，使得實(shí)際輸出與參考軌跡偏差最小。

構(gòu)造性能指標(biāo)函數(shù)式(7)，求出控制時域內(nèi)確保系統(tǒng)輸出最優(yōu)的控制侓，并將計(jì)算的第一個分量τ(k|k)作用于機(jī)械臂。

e(k)＝x(k)-xr(k)

s(k)＝he(k)

es(k+j)＝s(k)-s(k|k-j)j＝1,2,…,p

其中，x(k)是k時刻系統(tǒng)狀態(tài)量，xr(k)是約束預(yù)測規(guī)劃控制器傳遞給軌跡跟蹤控制器在下一時刻要跟蹤的期望，e(k)為跟蹤誤差向量，s(k)是滑模函數(shù)，表示k時刻滑模預(yù)測值，s(k|k-j)是過去時刻對k時刻滑模預(yù)測值，es(k+j)是預(yù)測誤差，sp(k+j)是預(yù)測滑模輸出，是機(jī)械臂校正輸出，τ(k+j-1|k)是待優(yōu)化控制變量，p和m分別是預(yù)測時域和控制時域；qi，ri(qi＞0，ri＞0)分別是自定義的誤差加權(quán)系數(shù)和力矩加權(quán)系數(shù)。

在不考慮約束的情況下，根據(jù)求解優(yōu)化問題(7)得到預(yù)測滑?？刂频目刂苼?/p>

u＝-(ω^tqω+r)^-1ω^tqφ

根據(jù)滾動優(yōu)化的思想，只選取當(dāng)前時刻優(yōu)化控制u的第一個分量為當(dāng)前時刻的最優(yōu)控制量τ(k)＝[i0…0]u作為實(shí)際控制輸入作用于故障后的機(jī)械臂系統(tǒng)。在下一時刻，預(yù)測時域同時向前推進(jìn)一步，并以新得到的測量值作為初始條件，重新優(yōu)化計(jì)算得到τ(k+1)，實(shí)現(xiàn)滾動優(yōu)化。這種方式可以有效適應(yīng)系統(tǒng)的最新情況，降低了對系統(tǒng)模型精度的要求，使控制能夠保持實(shí)際的最優(yōu)。

若系統(tǒng)執(zhí)行器發(fā)生恒偏差故障且存在系統(tǒng)不確定因素，考慮系統(tǒng)輸入約束條件，構(gòu)建容錯控制系統(tǒng)，驗(yàn)證本發(fā)明的有效性。

圖2為執(zhí)行器f1故障及故障估計(jì)的診斷結(jié)果，圖中顯示1.5s時機(jī)械臂第1關(guān)節(jié)的執(zhí)行器發(fā)生恒偏差故障，此時f1＝3，表明設(shè)計(jì)的自適應(yīng)故障觀測器能夠有效估計(jì)故障大小。

從圖3—圖4所示的機(jī)械臂關(guān)節(jié)輸出軌跡能夠看出，采用雙層結(jié)構(gòu)控制方法設(shè)計(jì)的機(jī)械臂容錯控制系統(tǒng)在對系統(tǒng)不確定性方面表現(xiàn)出了較好的性能，而且能夠使機(jī)械臂快速運(yùn)動到達(dá)指定目標(biāo)位置。當(dāng)系統(tǒng)執(zhí)行器發(fā)生恒偏差故障時，機(jī)械臂第1關(guān)節(jié)的運(yùn)動輸出軌跡發(fā)生劇烈抖動，然后在短時間內(nèi)重新實(shí)現(xiàn)對目標(biāo)位置的跟蹤，如圖3所示；機(jī)械臂第2關(guān)節(jié)的運(yùn)動輸出軌跡波動不大，如圖4所示，說明該容錯控制方案能很好的對故障進(jìn)行補(bǔ)償，滿足容錯控制系統(tǒng)的性能要求。

圖5顯示，機(jī)械臂系統(tǒng)在啟動階段，控制輸入力矩快速變化，很快趨于穩(wěn)定。在系統(tǒng)發(fā)生故障后，也能夠做出相應(yīng)動作，且整個過程都在輸入約束范圍內(nèi)。根據(jù)仿真實(shí)驗(yàn)圖，可以看出結(jié)合了基于模型預(yù)測控制的機(jī)械臂容錯控制和基于預(yù)測滑模控制的機(jī)械臂容錯控制方法來設(shè)計(jì)的基于雙層結(jié)構(gòu)控制的容錯控制方法對處理執(zhí)行器可能發(fā)生故障情況下實(shí)現(xiàn)對機(jī)械臂系統(tǒng)的控制問題是可行且有效的，在解決控制輸入約束問題時具有良好的能力，同時在抑制系統(tǒng)不確定性、快速性及穩(wěn)定性方面也都有不錯的表現(xiàn)。

以上所述具體實(shí)施例，對本發(fā)明的結(jié)構(gòu)及技術(shù)實(shí)施作了詳盡的分析，但并非用以限定本發(fā)明。在不脫離本發(fā)明精神和原則之內(nèi)，可對其進(jìn)行進(jìn)一步研究，豐富發(fā)明內(nèi)容。因此，本發(fā)明保護(hù)范圍應(yīng)以權(quán)利要求為準(zhǔn)。