基于運動學(xué)方程的故障診斷方法研究

劉 鵬 ，張 偉 ，王 強 ，季毅巍

（1.遼寧石油化工大學(xué) 信息與控制工程學(xué)院，撫順 113000；2.中國科學(xué)院 沈陽自動化研究所 機器人國家重點實驗室，沈陽 110016；3.上海微小衛(wèi)星工程中心，上海 200050）

常見的機械臂故障診斷方法是利用觀測器（組）產(chǎn)生殘差集，依據(jù)某種準(zhǔn)則或閾值對殘差進(jìn)行估計和決策。對于控制理論與計算機技術(shù)，其實質(zhì)性進(jìn)步表現(xiàn)在使觀測器的故障診斷技術(shù)能夠合理地應(yīng)用于復(fù)雜過程中，需要考慮建模誤差的非線性系統(tǒng)和時變系統(tǒng)，然而就機械臂的故障診斷而言，大多數(shù)使用理論值與真實值之間的偏差，對偏差進(jìn)行仿真分析。機械臂的故障主要分為傳感器故障和執(zhí)行器故障。

傳感器故障，是在控制系統(tǒng)中用來檢測系統(tǒng)參數(shù)的傳感器發(fā)生恒增益變化、卡死或恒偏差，而不能準(zhǔn)確測量信息或者出現(xiàn)偏差，實際表現(xiàn)為測量值與真實值之間的偏差超過了系統(tǒng)可以接受的閾值。

執(zhí)行器故障，是控制系統(tǒng)中用來執(zhí)行控制命令的執(zhí)行器發(fā)生恒增益變化、卡死或恒偏差從而不能準(zhǔn)確地執(zhí)行命令，實際表現(xiàn)為執(zhí)行器的動作輸出不符合它的輸入命令。在機械臂故障診斷中，執(zhí)行器指的是機械臂系統(tǒng)中控制關(guān)節(jié)的伺服電機，而傳感器指的是機械臂系統(tǒng)中測量、檢測關(guān)節(jié)角位移的碼盤，具體如圖1所示。

圖1 關(guān)節(jié)角平面Fig.1 Joint angle plan

現(xiàn)有的機械臂故障診斷，主要是通過電機的碼盤或轉(zhuǎn)速計等傳感器測量各個關(guān)節(jié)的轉(zhuǎn)動的實際角度，然后再用理想模型觀測器測量理論角度，通過不同時刻實際值與理論值的關(guān)系設(shè)計閾值來檢測和識別故障，在機械臂故障診斷領(lǐng)域這種方法的研究報告和文獻(xiàn)已有很多[1-6]。這些研究均建立在動力學(xué)基礎(chǔ)之上，忽略了機械臂運動學(xué)方程的重要性，未用到機械臂的D-H參數(shù)及運動學(xué)方程，未利用機械臂末端位置相對于基坐標(biāo)在空間上的傳遞關(guān)系。

顯然，由于機械臂抖動的存在，無論傳感器測量值與真實值之間的差值怎么變化，都將產(chǎn)生一個很小的偏差，導(dǎo)致機械臂運動學(xué)末端理論位置與實際位置產(chǎn)生微小的不同，而這個不同與機械臂每個連桿的長度是分不開的。然而，產(chǎn)生偏差的原因有很多，例如建模不準(zhǔn)確、控制對象的參數(shù)在執(zhí)行任務(wù)中發(fā)生突變等。

在此，基于運動學(xué)故障診斷的角度，運用運動學(xué)方程及余弦定理，把機械臂當(dāng)作一個整體，通過對機械臂末端位置的測量值與真實值之間的偏差，利用殘差仿真圖間的關(guān)系來識別故障，完善了以前故障診斷方法中的不足，同時對機械臂傳感器進(jìn)行診斷。

1 D-H參數(shù)與運動學(xué)方程

1.1 連桿D-H參數(shù)描述

機械臂系統(tǒng)通過一系列關(guān)節(jié)連接組成，關(guān)節(jié)通過一個個連桿進(jìn)行連接。把坐標(biāo)系固定在機械臂每一個連桿機構(gòu)中，將它們坐標(biāo)系間的相對位置和方向用齊次變換矩陣來描述，如果要得到機械臂末端關(guān)節(jié)的位置姿態(tài)，首先必須給每個關(guān)節(jié)都定義一個坐標(biāo)系，再計算后一關(guān)節(jié)相對于前一關(guān)節(jié)的坐標(biāo)變換矩陣，最后將其叉乘到一起，得到末端相對于基坐標(biāo)的位姿，連桿到連桿的坐標(biāo)變換關(guān)系如圖2所示。

圖2 機械臂連桿變換關(guān)系示意Fig.2 Manipulator connecting rod transformation relationship diagram

機械臂各連桿都可以用圖2中的 4個參數(shù)ai，di，αi，θi來描述。 其中，ai和 αi描述連桿本身特征；di和θi描述相鄰兩連桿之間的關(guān)系。對旋轉(zhuǎn)關(guān)節(jié)來說，θi為變量，其他3個參數(shù)不變；對移動關(guān)節(jié)來說，di為變量，另外3個參數(shù)不變。這種用連桿參數(shù)描述機構(gòu)運動關(guān)系的規(guī)則稱為Denavit-Hartenberg方法，簡稱D-H方法。

設(shè)固連在基座的坐標(biāo)系為坐標(biāo)系｛O｝，與連桿固連的坐標(biāo)系為｛i｝，則連桿參數(shù)定義為αi-1為zi-1與zi之間的夾角，繞xi軸旋轉(zhuǎn)；ai-1為zi-1到zi之間的距離，沿著xi軸正方向；θi為xi-1與xi之間的夾角，繞 zi-1軸旋轉(zhuǎn)；di為 xi-1到 xi之間的距離，沿著 zi-1軸正方向。 ai-1非負(fù)；di，αi-1，θi的值可正、可負(fù)。

1.2 連桿變換和運動學(xué)方程

相鄰連桿坐標(biāo)系之間的連桿變換與 ai，di，αi，θi這4個連桿參數(shù)有關(guān)。如果所有連桿坐標(biāo)系都已經(jīng)被規(guī)定，那么相鄰兩連桿之間的相對關(guān)系便可以用2個旋轉(zhuǎn)和2個平移這4個基本子變換建立起來，即：①rot（zi-1，θi），繞 zi-1軸轉(zhuǎn)動 θi角度；②trans（zi-1，di），沿 zi-1軸移動距離 di；③trans（xi，ai-1），沿xi-1軸移動距離 ai-1；④rot（zi-1，αi-1），繞 xi-1軸轉(zhuǎn)動αi-1角度。

由此可知，連桿i相對于i-1的位姿可以用變換矩陣i-1Ti描述。由于這些基本子變換都是相對于基坐標(biāo)進(jìn)行變換的，根據(jù)“從左到右”原則，得到坐標(biāo)系｛Oi-1｝到｛Oi｝的連桿變換矩陣為

則相鄰兩關(guān)節(jié)坐標(biāo)系間的連桿變換矩陣為

由齊次變換矩陣的乘法可知

2 判斷故障的依據(jù)

機械臂的每個關(guān)節(jié)都與2個連桿相連 （除去2個端點），如圖1所示。假設(shè)把連桿的另外兩端相連接，利用余弦定理計算出替代連桿的理論值。然而，在機械臂運動時，每個關(guān)節(jié)測量角度的傳感器（碼盤）包含了一個均值為零的正態(tài)分布的噪聲信息ξ，ξ∈N（0，σ2），用含有噪聲的角度測量值以及余弦定理計算理論值，與通過運動學(xué)方程計算出的真實值進(jìn)行對比，構(gòu)造殘差。利用多個殘差間的關(guān)系特性來進(jìn)行故障診斷，由于對各個關(guān)節(jié)角度的測量相互獨立，所以各個關(guān)節(jié)間的測量噪聲互不相關(guān)，但是對故障做出甄別的程度以及采取的措施與采用的傳感器數(shù)量及配置密切相關(guān)。

3 試驗驗證

根據(jù)上述故障診斷原理，利用現(xiàn)有的實驗室資源，以平面型三連桿機械臂為例說明這種故障診斷方法，如圖3所示。圖中，機械臂的每個關(guān)節(jié)都具有1個測量關(guān)節(jié)角度的傳感器 （碼盤）和1個執(zhí)行器（舵機），各連桿長度為L1=1.8 m，L2=1.2 m，L3=1.6m；原點O與關(guān)節(jié)1重合，點C為機械臂末端手的位置，點A和點B分別對應(yīng)于關(guān)節(jié)2和關(guān)節(jié)3，OB和AC都是虛擬連桿，OC為機械臂工作空間的半徑。

圖3 機械臂傳感器配置及坐標(biāo)系Fig.3 Sensor collocation sketch and coordinates of manipulator

3.1 基于運動學(xué)方程

根據(jù)運動學(xué)方程公式，計算機械臂末端手相對于基坐標(biāo)的位姿，求其替代連桿OC長度，具體方法如下：

根據(jù)式（1）和式（2），三連桿機械臂末端手相對于基坐標(biāo)的位姿為

因此得到機械臂工作空間半徑替代連桿OC長度為

同理，由于每個測量角度的傳感器包含了一個均值為零、正態(tài)分布的噪聲信息 ξ，ξ∈N（0，σ2），因此實際計算得到的機械臂工作空間半徑虛擬連桿OC的表達(dá)式為

綜上所述，機械臂工作空間半徑替代連桿OC的長度為

3.2 基于余弦定理

由圖3（b）結(jié)合余弦定理，可得關(guān)節(jié)2對應(yīng)的替代連桿OB的理論長度為

由于測量角度的傳感器包含了一個均值為零、正態(tài)分布的噪聲信息 ξ2，ξ2∈N（0，σ2），故實際計算得到的虛擬連桿OB的長度為

用替代連桿長度的平方來替代連桿的長度，則替代連桿OB的殘差為

由所得的關(guān)節(jié)2和關(guān)節(jié)3所對應(yīng)的虛擬連桿OB，AC以及機械臂工作空間半徑虛擬連桿OC，即可判斷出機械臂每個關(guān)節(jié)的故障。

4 仿真分析

將各數(shù)據(jù)帶入方程中，用Matlab軟件進(jìn)行仿真分析。仿真結(jié)果如圖4、圖5所示。圖4表明機械臂各個關(guān)節(jié)都正常時，每個關(guān)節(jié)所對應(yīng)的虛擬連桿殘差及工作空間半徑殘差的關(guān)系，都可以看做是均值為零的噪聲序列。

圖4 正常時虛擬連桿殘差及工作空間半徑殘差的關(guān)系Fig.4 Normal virtual link residual and work space radius residual relationship

圖5 虛擬連桿OB，AC殘差及工作空間半徑OC殘差的關(guān)系Fig.5 Relationship between the virtual link residual OB，AC and the working radius residual OC

由圖5（a）可見，機械臂末端超出了可接受區(qū)域，表示發(fā)生了故障，比較上面2個虛擬連桿的殘差關(guān)系，極有可能是虛擬連桿OB所對應(yīng)的關(guān)節(jié)2發(fā)生了故障。同理，圖5（b）表明極有可能是虛擬連桿OC對應(yīng)的關(guān)節(jié)3發(fā)生了故障。對于圖5（c），機械臂末端超出了可接受區(qū)域，表示發(fā)生了故障，但是關(guān)節(jié)2、關(guān)節(jié)3所對應(yīng)的虛擬連桿誤差均值都基本為零，并沒有太大變化。這種情況說明第2關(guān)節(jié)發(fā)生了故障（由于其前一個連桿長度為零，并具備與參考點相重合的特性，因而不能直接判斷，只能通過間接方法判斷）。

5 結(jié)語

本文提出了一種基于機械臂運動學(xué)方程的故障診斷方法。以三連桿機械臂為例，利用運動學(xué)方程和余弦定理，將殘差的設(shè)計與機械臂末端位置以及連桿長度的變化相聯(lián)系；將2種不同計算方法做對比，分析殘差；最后利用Matlab軟件對其進(jìn)行仿真分析。從仿真圖中可以看出故障所在，完善了大部分基于動力學(xué)的故障診斷方法中沒有利用運動學(xué)方程以及連桿長度的不足。

[1]Visinsky M L，Cavallaro J R，Walker I D.Robotic fault detection and fault tolerance：a survey[J].Reliability Engineering and System Safety，1994，46（2）：139-158.

[2]Rolf Isermann.Model based fault detection and diagnosis methods[C]//Proceedings of the American ControlConferences.Seattle Washington：[s.l.]，1995：1605-1609.

[3]Visinsky M L.Fault detection and fault tolerance in robotics[C]//Proceedings of NASA Space Operations Applications Research Symposium.Houston，Texas：[s.l.]，1991：262-271.

[4]Visinsky M L，Cavallaro J R，Walker I D.Adaptive fault detection and tolerance for robots[C]//First World Automation Conference.1994：205-210.

[5]Horak D T.Failure detection in dynamic systems with modeling errors[J].Journal of Guidance Control and Dynamics，1988，11（6）：508-516.

[6]Horak D T.Experimental estimation of modeling errors in dynamic systems[J].AIAA Journal of Guidance Control and Dynamics，1989，12（5）：653-658.