[發(fā)明專利]一種基于粒子濾波算法的空間機械臂故障檢測方法

有效

• 申請?zhí)枺?/td>: 201810883670.0
• 申請日: 2018-08-06
• 公開（公告）號: CN108972553B
• 公開（公告）日: 2021-08-13
• 發(fā)明（設(shè)計）人: 高欣;翟林;任澤宇;劉惠禾
• 申請（專利權(quán)）人: 北京郵電大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于粒子濾波算法的空間機械臂故障檢測方法，其特征在于，所述方法步驟包括：

(1)基于空間機械臂動力學模型設(shè)計非線性干擾觀測器，在此基礎(chǔ)上選取李雅普諾夫函數(shù)證明其收斂，具體包括：

使用空間機械臂力矩干擾估計值和力矩干擾實際輸出值的差值對力矩干擾估計值進行修正，因此干擾觀測器設(shè)計為：

其中，d為干擾矢量實際值，為干擾矢量估計值，z為輔助參數(shù)向量，為輔助參數(shù)向量關(guān)于時間的導數(shù)，θ為關(guān)節(jié)角度向量，為關(guān)節(jié)角速度向量，L(θ)、分別是關(guān)于關(guān)節(jié)角度和關(guān)節(jié)角速度的增益矩陣，二者定義如下：

X為可逆矩陣，通過李雅普諾夫函數(shù)所得到的線性矩陣不等式(11)求得；關(guān)節(jié)力矩干擾矢量d可由空間機械臂在關(guān)節(jié)空間的動力學基本方程求得，M^-1(θ)為動力學慣性矩陣的逆矩陣，所述空間機械臂在關(guān)節(jié)空間的動力學基本方程如下：

式(3)中，θ表示關(guān)節(jié)角序列，為n維列向量，由空間機械臂的任務(wù)規(guī)劃和路徑規(guī)劃算法得到；M(θ)∈R^n×n為關(guān)節(jié)空間中的慣性矩陣；為哥氏力和離心力矩陣；τ＝(τ₁,τ₂,···,τ_n)^T為關(guān)節(jié)力矩矢量；

令表達式(1)、(2)、(3)構(gòu)成了非線性干擾觀測器，實際工程中一般很難獲得干擾矢量d的微分先驗知識，且相對于觀測器的動態(tài)特性，干擾的變化是緩慢的，因此假設(shè)

針對所提出的空間機械臂系統(tǒng)，設(shè)計李雅普諾夫函數(shù)為

其中表示觀測器干擾觀測誤差，X^T為式(2)中可逆矩陣X的轉(zhuǎn)置矩陣；

化簡上述觀測器表達式，可得

從而得到

將上述推導結(jié)果帶入李雅普諾夫函數(shù)得到

構(gòu)造不等式

其中Γ0為對稱正定矩陣，則存在Γ的導數(shù)，使得

可見，所用干擾觀測器指數(shù)收斂，收斂精度取決于參數(shù)Γ值，參數(shù)值越大，收斂速度越快，精度越高；

由不等式(8)可見，式中含有非線性項，必須轉(zhuǎn)化為線性矩陣不等式才能求解，令Y＝X^-1，將Y^T和Y分別乘以不等式左右，得：

由于則則上式成立的充分條件為：

Y^T+Y-ζI-Y^TΓY≥0 (10)

其中I為n維單位矩陣，ζ為一個正的常數(shù)，根據(jù)Schur定理，式(10)等價于

因此該不等式的求解是否有效取決于ζ和Γ的值，其值越小，越容易得到有效解，不等式的解即可得到式(2)中的可逆矩陣X；

(2)設(shè)計滑模變結(jié)構(gòu)控制器對干擾觀測器的結(jié)果進行補償，得到空間機械臂的關(guān)節(jié)角度，具體包括：

設(shè)空間機械臂關(guān)節(jié)理想角度為θ_aim，關(guān)節(jié)角的實際值為θ，取跟蹤誤差e＝θ-θ_aim，定義滑模函數(shù)s為：

其中λ為n維對角矩陣，λ_i0,i＝1,2…n，為e的關(guān)于時間的導數(shù)；顯然，當控制系統(tǒng)狀態(tài)保持在滑模面上時，關(guān)節(jié)角跟蹤誤差趨近于零；

為了實現(xiàn)在控制率中不含有加速度項，同時將觀測器輸出反饋到滑模控制器，引入變量則

從而使得中不包含加速度項；

在控制率設(shè)計中，為確保在存在干擾時，系統(tǒng)軌跡依舊能靠近滑模表面，應(yīng)滿足滑模函數(shù)與滑模函數(shù)的微分之積小于等于零，即將其展開可得：

因此滑模控制器設(shè)計為：

其中，α為元素均為正數(shù)的對角矩陣，sgn為符號函數(shù)；

通過在控制率中加入對力矩干擾的補償來降低切換增益，從而抑制抖振；

(3)使用粒子濾波算法中對傳感器測量結(jié)果進行評估，計算空間機械臂達到目標關(guān)節(jié)角的概率，結(jié)合干擾觀測器曲線和計算得到的概率值，綜合評價故障的發(fā)生。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，使用粒子濾波算法中對傳感器測量結(jié)果進行評估，計算空間機械臂達到目標關(guān)節(jié)角的概率；

根據(jù)空間機械臂使用可靠性控制系統(tǒng)狀態(tài)空間模型：

其中，θ_i為第i個控制周期機械臂關(guān)節(jié)角的實際值；y_i為第i個控制周期傳感器的測量值；x_i為第i個控制周期的控制策略，可知要計算空間機械臂的使用可靠度則需要獲得θ_i的后驗概率分布θ_i|y_i，采用如下的正態(tài)分布模型作為空間機械臂關(guān)節(jié)角度傳感器觀測誤差波動：

v_i～N(μ₀,Λ₀)

其中，μ₀為預設(shè)的空間機械臂關(guān)節(jié)角測量誤差的期望值，Λ₀為預設(shè)的空間機械臂關(guān)節(jié)角測量誤差的方差；

采用和兩個集合來對θ_i的分布進行描述；其中是與θ_i相同形式的一組樣本數(shù)據(jù)，即粒子，N_s為粒子數(shù)量；是粒子對應(yīng)的權(quán)值，表示粒子與θ_i的相關(guān)程度；

粒子濾波算法源于蒙特卡洛思想，即以某事件出現(xiàn)的頻率來指代該事件的概率，基于粒子濾波算法實現(xiàn)空間機械臂使用可靠度計算分為以下5個步驟：

1)初始狀態(tài)：隨機產(chǎn)生N_s個粒子，依次計算每個粒子z_j到目標初始角度的距離，其中j＝1,2…N_s，然后得到所有粒子的幾何中心位置；

2)預測階段：根據(jù)狀態(tài)轉(zhuǎn)移方程，每一個粒子得到一個預測粒子，求取每個粒子與測量位置相差的距離dist，并根據(jù)傳感器測量的先驗概率密度求得粒子權(quán)重ω_i，之后將權(quán)值歸一化；

3)重采樣：重要性采樣法引入一個已知的、容易采樣的重要性概率密度函數(shù)從中生成采樣粒子，利用這些隨機樣本的加權(quán)和來逼近后驗濾波概率密度根據(jù)粒子權(quán)重對粒子進行篩選，篩選過程中，去掉權(quán)重較小的粒子，復制權(quán)值較大的粒子；計算所有粒子幾何中心位置，得到與實際值的位置誤差；

4)計算到達目標角度的概率：根據(jù)空間機械臂任務(wù)精度要求，計算精度范圍內(nèi)粒子占總粒子個數(shù)的比例，空間機械臂到達目標角度概率計算方法如下：

其中，P{θ_i1≤θ_i≤θ_i2}為到達關(guān)節(jié)目標位置的概率，θ_i1≤θ_i≤θ_i2為預設(shè)的精度范圍，θ_i1為精度范圍的下限值，θ_i2為精度范圍的上限值；

5)濾波：將重采樣后的粒子帶入狀態(tài)轉(zhuǎn)移方程得到新的預測粒子，即步驟2)；

計算所得的結(jié)果為空間機械臂在每個控制周期達到目標角度的概率，當關(guān)節(jié)角不符合精度范圍且干擾觀測曲線出現(xiàn)異常時，即可綜合定位故障發(fā)生的關(guān)節(jié)和時間；當干擾觀測曲線未出現(xiàn)明顯異常，但使用可靠度持續(xù)較低時，可判定為關(guān)節(jié)角傳感器故障，所述方法可在一定力矩干擾的情況下使機械臂繼續(xù)可靠運行。