基于運動學方程的故障診斷方法研究

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

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

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

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

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

圖1 關節角平面Fig.1 Joint angle plan

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

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

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

1 D-H參數與運動學方程

1.1 連桿D-H參數描述

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

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

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

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

1.2 連桿變換和運動學方程

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

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

則相鄰兩關節坐標系間的連桿變換矩陣為

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

2 判斷故障的依據

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

3 試驗驗證

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

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

3.1 基于運動學方程

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

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

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

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

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

3.2 基于余弦定理

由圖3（b）結合余弦定理，可得關節2對應的替代連桿OB的理論長度為

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

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

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

4 仿真分析

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

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

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

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

5 結語

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

[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.