機械臂外力的時延強跟蹤Kalman估計與自學習控制

淡乾川，崔鳳坤

（1.重慶科創職業學院智能制造學院，重慶 402160；2.山東交通學院，山東 濟南 250357）

1 引言

機械臂在完成抓取、擦拭、去毛刺等接觸式任務時，接觸力的大小和變化不僅影響機械臂末端執行器的定位精度，而且極大程度地決定了任務完成質量和成敗[1]。當接觸力過大時，甚至造成接觸物的損壞。因此研究機械臂與環境交互過程中的接觸力估計問題，具有重要的理論意義、現實意義和安全意義。

在末端執行器上安裝力傳感器是獲取接觸力信息的一般思路，但是安裝力傳感器不僅需要額外的空間成本和經濟成本，而且在力傳感器在極端環境下的測量精度較低。

因此，基于軟件和模型的接觸力估計方法是當前一個研究熱點，常用的估計方法包括狀態觀測器估計[2]、時間延遲估計[3]、神經網絡估計[4]等。狀態觀測器是基于系統內部演化原理，將接觸力視為系統內部的一個狀態，通過狀態觀測得到機械臂與環境交互的接觸力，該方法優點是估計精度相對較高，缺點是需要精確的系統模型。時間延遲估計的前提假設是系統在極短時間的狀態是不變的，這種方法缺點是存在延遲，且估計精度與采樣時間、狀態變換速度等相關。神經網絡估計的優點是不依賴模型，缺點是訓練樣本難以獲得。文獻[5]關于機械臂的力位控制問題，提出了深度強化學習的柔順捕獲控制方法，與阻抗控制比，該方法減小了機械臂捕獲過程的沖擊力，但是該方法需要大量的捕獲訓練。文獻[6]設計了一種非線性擾動觀測器，該觀測器在不測量關節加速度的前提下，可以同時估計出系統摩擦力和接觸力，并在仿真環境下對該理論進行了驗證，該方法的缺點是需要精確的系統模型。文獻[7]基于時延估計獲取了系統的集總動力學參數，從而得到了一種實用的無模型結構，并設計了增益隨誤差自適應調節的控制器，并基于Lyapunov理論證明了其穩定性，但是該方法僅適用于慢變系統，對于快變系統該方法難以跟蹤系統突變使得控制誤差較大。

綜合上述分析，基于狀態觀測器的機械臂接觸力估計需要精準的系統模型，而實際控制系統的精確模型是難以獲得的。針對上述問題，首先建立了機械臂系統的動力學模型，而后提出了無需精確模型的時延強跟蹤卡爾曼估計方法，對機械臂的接觸力進行估計；并針對角位置控制問題，設計了熵-自學習控制方法。最后，通過仿真對這里的接觸力估計和控制方法進行驗證。

2 機械臂動力學建模

首先建立單個關節的驅動模型，并在此基礎上建立機械臂系統的動力學模型。

2.1 關節驅動模型與動力學模型

關節的驅動力矩是電機轉矩經過減速器后獲得的，因此首先建立單個關節的驅動模型，如圖1所示。為了進行符號區分，將圖1中的關節記為關節i，則圖中vi(t)—關節i的電機驅動電壓；i(t)—電機內部電流；Rai—電機內阻；Lai—電機內電感；Bmi—粘滯摩擦系數；Jmi—關節的轉動慣量；Kgi—齒輪傳動比；τmi—關節驅動力矩。

圖1 關節驅動示意圖Fig.1 Schematic Diagram of Joint Drive

根據基爾霍夫定律和轉矩平衡公式，得：

式中：EMi—電動機i的反電勢；Kgi—齒輪傳動比；ηi—電機傳動效率；Ki—電機轉動常數；θi—關節i的轉角。

對于多關節機械臂系統，考慮環境中的擾動力矩τd，依據拉格朗日方程得到系統的動力學方程為：

式中：τm—機械臂系統各關節的力矩矩陣；M(q)—系統的慣性矩陣；q、—關節角、關節角速度、關節角加速度；C(q，)—哥氏力矩和離心力矩；G(q)—重力矩；τd—環境中的干擾力矩，J(q)—雅可比矩陣；Fext—末端執行器的接觸力（矩），記為Fext=[Fx，Fy，Fz]T。在此需要說明的是，機械臂與環境交互過程中，系統所受干擾量無主要因素，可視為由多種因素共同隨機作用的結果，因此將環境干擾τd視為高斯白噪聲。

2.2 四自由度機械臂系統結構

以四自由度機械臂系統為研究對象，該機械臂由4個關節和3 個連桿組成，其中關節1 和關節2 為同軸心關節，如圖2 所示。圖中x1y1z1～x4y4z4分別為關節1～關節4的固連坐標系，θ1～θ4分別為關節1～關節4的轉角，L1～L3分別為連桿長度。連桿1質量m1=4.40kg，連桿2 質量m2=2.30kg，連桿3 質量m3=1.70kg；連桿1轉動慣量Jm1=0.110kg·m2，連桿2轉動慣量Jm2=0.250kg·m2，連桿3轉動慣量Jm3=0.230kg·m2。

圖2 四自由度機械臂系統Fig.2 Manipulator System of 4 Freedom of Degree

根據圖2所示的四自由度機械臂系統結構，得到該機械臂系統的DH參數，如表1所示。αi—連桿間扭曲角度；ai—連桿長度。

表1 機械臂系統DH參數Tab.1 D-H Parameters of Manipulator System

3 基于時延卡爾曼的外力估計

3.1 機械臂系統的時延模型

在機械臂的實際控制中，由于外界擾動、系統非線性、機理分析不全面等原因的影響，難以獲得準確的系統模型。本節為了對機械臂接觸力進行估計，基于時延估計理論［8］，使用系統的時間延遲信號對當前參數進行估計。為了簡化表達，令τext=JT(q)Fext、H(q，，)=C(q，)+G(q)，則將式（2）形式進行簡化，并表達為時變形式：

將系統的采樣時間記為ts，則力矩矩陣τext在前一采樣時刻t-ts的值為：

用式（3）減式（4），并整理得：

以采樣時間ts為時間分度，即t-ts時刻記為k-1時刻，t時刻記為k時刻。則式（6）可以轉化為遞推形式，即：

針對式（7），狀態量選擇為x(k)=τext(k)，控制量選擇為u(k)=[d(k)，H(k)-H(k-1)]T，并以x(k)作為觀測量，得到機械臂系統的時延狀態模型為：

式中：A=I—系統矩陣；B=[M I]—控制矩陣；w(k) —系統噪聲；C=I—觀測矩陣；v(k)—觀測噪聲。

3.2 接觸力的強跟蹤Kalman估計

由于機械臂系統存在一定的非線性、模型不準確等問題，標準卡爾曼濾波的參數估計精度難以保證[9]。為了解決此問題，本文提出了強跟蹤Kalman濾波算法，其核心思想是：通過引入一個漸消因子，強行使估計殘差保持正交，從而提高參數估計的精度。強跟蹤Kalman濾波的狀態修正方法為：

強跟蹤Kalman濾波的核心思想是，通過設計校正系數Lk，使得估計殘差強制正交，即：

當系統模型準確且不存在系統非線性等問題時，式（10）自然成立，此時強跟蹤Kalman濾波退化為標準Kalman濾波；當系統模型不準確或存在系統非線性等問題時，通過上式可以強行使估計殘差保持正交，從而獲得較高的參數估計精度。

對式（10）求解需使用基于非線性規劃的梯度下降法，該方法計算量巨大，不具備實用性。因此使用次優漸消因子：

其中，ek定義為：

式中：V0，k—估計殘差協方差矩陣；ρ—遺忘因子；—誤差協方差矩陣；β≥1—弱化因子。

基于機械臂系統的時延狀態模型和強跟蹤Kalman 濾波算法原理，得到狀態估計步驟如下：

（1）預測，包括狀態預測、誤差協方差矩陣預測、系統噪聲預測等，即

式中：Mk-1—k-1時刻的Markov隨機場，ek-2—高斯噪聲。

（2）新息計算，依據測量值得到新息為：

式中：γk—新息；yk—k時刻的觀測值。

（3）狀態更新與協方差更新為：

（4）基于強跟蹤Kalman濾波的接觸力估計值為：

反復迭代（1）～（4），可實時得到接觸力∕接觸力矩估計值Fext。

分析上述基于時延系統和強跟蹤Kalman 濾波的接觸力估計過程可知，在外力估計的推導過程中，沒有引入具體的機械臂結構信息和參數，因此該方法不僅僅局限于圖2的四自由度機械臂系統，對其他結構機械臂系統的外力估計同樣具有適用性。

4 自學習控制

經常使用的機械臂力∕位控制方法包括阻抗控制、反步控制等，但是這些方法的控制精度跟模型精度密切相關。但是如前文所述，機械臂系統存在不確定性和非線性，因此系統模型是不夠準確的。為了保證較高的力∕位控制精度，本節基于RBF神經網絡的自學習能力提出了自學習控制方法。

4.1 RBF神經網絡

RBF神經網絡由輸入層、隱藏層、輸出層共三層結構組成，其中輸入神經元到隱藏神經元為非線性映射，隱藏神經元到輸出神經元一般使用線性激活方式[10]。以末端執行器的三軸接觸力誤差、三維位置跟蹤誤差作為RBF神經網絡的輸入量，以4個關節控制力矩τm為輸出量。按照上述設定，構造的自學習神經網絡結構，如圖3所示。

圖3 自學習網絡結構Fig.3 Self-Learning Network Structure

圖3中e1～e3定義為三軸接觸力誤差，e4～e6定義為三維位置誤差，O1～O4為4個關節的控制力矩。輸入神經元向隱藏神經元的信號傳遞使用高斯函數激活，則自學習網絡的信號傳遞為：

式中：e=(e1，e2，…，e6)T—輸入向量；φ(e)—隱藏神經元輸出；ci、σi—高斯分布的均值和標準差；w—隱藏神經元與輸出神經元的權值；Om—輸出力矩。

4.2 基于熵的自學習控制網絡

在經典RBF神經網絡中，隱藏神經元數量一般使用經驗公式確定，具有較大的自主性且過度依賴專家經驗；參數ci、σi一般使用K-means算法迭代產生，但是由于初值設定的主觀性使得參數的收斂速度較慢。為了解決上述問題，使用熵聚類的方法確定隱藏神經元數量和參數ci、σi的參數值。

對于N個M維向量v=(v1，v2，…vN)，任意兩個向量vi、vj之間的熵值定義為：

式中：Sij—向量vi、vj之間的相似度；Dij—向量vi、vj之間的距離；α=-ln(0.5)—曲率，其中，—所有向量的平均距離；Eij—向量vi、vj之間的熵值。某一向量vi在向量整體v中的熵值Ei為：

在上述參量定義的基礎上，基于熵聚類［11］的隱藏神經元數量及參數確定方法為：

（2）以Ecm=min(Ei)為優化目標確定m個聚類中心vcm；

（3）設定一個相似度閾值b，當向量vi與聚類中心vcm的相似度Sij＞b，則將vi歸為類vcm，形成類集合Vm；

（4）設定一個數量閾值λ=0.05N，當類集合Vm中的向量數量≥λ，則vcm可以作為聚類中心，且V=V-Vm；當類集合Vm中的向量數量＜λ，則將vcm進行標記，不允許vcm作為聚類中心；

（5）重復（2）～（4），直至V中向量數量＜λ時算法結束。

根據v=(v1，v2，…，vN)的聚類結果，聚類的類別數量即為隱藏神經元數量，高斯分布均值ci為類Vi中各元素均值、σi為類Vi中各元素的標準差。圖3中權值w使用梯度下降法進行迭代訓練，誤差函數設置為末端執行器的三軸接觸力誤差與三維位置誤差之和，即：

基于梯度下降法的傳遞權值w迭代方法為：

式中：ηw—自學習網絡的學習因子；αw—網絡的動量因子；Δw(k)=w(k)-w(k-1)。

分析上述基于熵-RBF神經網絡的自學習控制方法，其輸入信息為機械臂的接觸力誤差和位置誤差，輸出為各關節控制力矩，從熵-RBF自學習網絡的角度講，其對機械臂結構沒有特殊的結構要求；從控制力矩計算的角速講，計算過程沒有對機械臂結構的特殊設定。因此，基于熵-RBF神經網絡的自學習控制方法不僅僅適用于圖2的四自由度機械臂系統，對其他結構機械臂系統的外力估計同樣適用。

5 仿真與分析

5.1 仿真情景設置

以2.2 節介紹的四自由度機械臂系統為實驗對象，使用SolidWorks軟件建立其仿真模型，將機械臂仿真模型導入到Matlab中進行接觸力估計和控制策略驗證。為了模擬環境干擾和系統不確定性，在測量數據中加入功率為0.05的白噪聲，并在系統參數中加入5%不確定性。

參考機械臂末端執行器的實際工作過程，末端執行器與物體接觸時首先接觸力不斷增大，而后保持一個恒定的接觸力，作業完畢后接觸力逐漸減小。模擬上述過程，將期望的末端執行器接觸力變化過程設置為，如圖4所示。

圖4 末端執行器接觸力Fig.4 Contact Force of End Effector

根據末端執行器的任務需要，關節角1和關節角3的期望軌跡設置為：θ1=θ3=1+0.2 sin(4t)；關節2 和關節4 的期望軌跡設置為：θ2=θ4=-0.2 cos(4t)。4 個關節的初始角位置設置為（0.3rad，-1.0rad，0.3rad，-1.0rad）。

5.2 接觸力估計精度對比與分析

為了對這里基于時延強跟蹤Kalman算法的接觸力估計精度進行分析，同時使用基于標準Kalman濾波的時延方法和本文方法對接觸力進行估計，如圖5所示。

圖5 接觸力估計結果Fig.5 Expect Joint Angle Position

由圖5可以直觀看出：（1）基于時延標準Kalman算法和時延強跟蹤Kalman 算法都能夠對接觸力進行有效估計；（2）強跟蹤Kalman估計精度高于標準Kalman估計精度，尤其在接觸力變化規律突變的瞬間，這種精度優勢更加明顯。為了深入分析不同方法的估計誤差，同時使用文獻［12］基于柔順控制的估計方法，統計上述3種估計方法的誤差分布概率，如圖6所示。

圖6 估計誤差分布概率Fig.6 Distribution Probability of Estimation Error

由圖6可知：（1）三種估計方法的估計誤差均服從0均值的正態分布，說明三種方法都是有效的；（2）基于時延強跟蹤Kalman估計的接觸力估計誤差主要集中在區間[-0.2N，0.2N]內，基于時延標準Kalman估計的估計誤差主要集中在[-0.6N，0.6N]，文獻[12]柔順控制的估計誤差主要集中在[-0.4N，0.4N]；從分布密度的集中度看，方法在0誤差位置的概率密度極大，遠高于另外兩種估計方法。這是因為估計方法使用了強跟蹤Kalman 濾波算法，該算法通過合理設置漸消因子，使估計殘差強制正交，實現了參數的高精度估計。上述實驗結果和分析充分表明了時延強跟蹤Kalman估計方法在機械臂接觸力估計中的精確性。

5.3 控制精度對比分析

本節對基于熵的自學習控制網絡的控制精度進行驗證，為了對改進點的有效性進行驗證，同時使用標準RBE神經網絡、熵-RBF神經網絡、文獻[13]變阻尼阻抗控制進行機械臂關節空間軌跡控制，其中標準RBF 隱藏層神經元數量使用專家經驗確定，參數ci、σi使用K-means算法迭代產生。為了保證公平，接觸力估計均使用時延強跟蹤Kalman 濾波的估計結果。由于關節1 和關節3、關節2和關節4的期望軌跡一致，且關節1和3、關節2和4 的跟蹤誤差相差不大，在此只給出關節1 和關節3 的跟蹤結果，如圖7所示。

圖7 角位置跟蹤控制Fig.7 Tracking Control of Angle Position

分析圖7可以看出：（1）以相對跟蹤誤差＜5%為收斂條件，關節1中熵-自學習控制網絡的收斂時間為0.23s，變阻尼阻抗控制的收斂時間為0.54s，自學習控制網絡的收斂時間為1.24s；關節2中熵-自學習控制網絡的收斂時間為0.38s，變阻尼阻抗控制的收斂時間為0.78s，自學習控制網絡的收斂時間為1.52s；（2）統計兩種控制器對關節1和關節2的絕對跟蹤誤差均值，關節1中熵-自學習控制網絡的絕對誤差均值為0.0024rad，變阻尼阻抗控制的絕對誤差均值為0.145rad，自學習控制網絡的絕對誤差均值為0.0324rad；關節2 中熵—自學習控制網絡的絕對誤差均值為0.0031rad，變阻尼阻抗控制的絕對誤差均值為0.122rad，自學習控制網絡的絕對誤差均值為0.0358rad。

上述實驗結果表明，在對關節1和關節2角位置的跟蹤控制中，基于熵—自學習控制網絡的收斂時間和跟蹤誤差均遠小于自學習控制網絡、變阻尼阻抗控制，這是因為標準自學習控制網絡中隱藏神經元數量依賴個人經驗設定，且參數ci、σi的訓練質量跟初值設置關系較大；變阻尼阻抗控制只有阻尼這一個參數隨控制工況自適應變化；而熵-自學習控制網絡中隱藏神經元數量的設置、參數ci和σi的設置完全依賴聚類結果，完全擺脫了對個人經驗的依賴，且熵—自學習網絡能夠隨控制工況自適應調整控制參數，因此熵—自學習控制網絡的跟蹤性能遠遠好于自學習控制網絡、變阻尼阻抗控制的跟蹤性能。

6 結論

研究了機械臂系統與環境交互過程中的接觸力估計方法和關節軌跡控制方法，提出了基于時延強跟蹤Kalman濾波的接觸力軌跡方法和基于熵—自學習控制網絡的軌跡跟蹤方法。經驗證得出以下結論：

（1）強跟蹤Kalman 通過設置漸消因子，使估計殘差強行正交，有效提高了參數估計精度；

（2）基于熵聚類方法確定自學習網絡的隱藏神經元數量和參數值，可以有效提高自學習控制網絡的跟蹤收斂時間和跟蹤精度。