基于外力矩觀測器的機器人碰撞檢測

張鐵 陳億杰 鄒焱飚

（華南理工大學 機械與汽車工程學院，廣東 廣州 510460）

當工業機器人在工作環境中與人類共享同一空間時，可能會導致意外的碰撞而傷害人類，為了降低這種風險，需要有一種可以檢測碰撞的方法。目前，對工業機器人的碰撞檢測通常分為基于外部傳感器的碰撞檢測和免傳感器的碰撞檢測兩種方法。

基于外部傳感器的碰撞檢測，主要是在機器人結構上安裝額外的傳感器，通過傳感器的實時反饋并結合控制系統實現碰撞檢測，例如，Flacco等［1］在工作環境中加裝視覺傳感器檢測人機距離實現碰撞檢測；Li等［2］在用于固定機器人的底座處安裝力/力矩傳感器并結合機器人動力學模型實現碰撞檢測。然而，上述方法存在以下兩方面的問題：一方面，額外傳感器數據的采集和處理可能會增加控制系統的復雜性；另一方面，為保證工業機器人關節的高剛度，通常沒有配備安裝外部傳感器的結構，且高性能傳感器的成本普遍較高，安裝額外的傳感器會增加工業機器人機械結構的復雜性和硬件成本。

為解決上述問題，免傳感器實現碰撞檢測得到了高度關注，主要分為兩種實現方式：第一種是使用智能算法直接區分碰撞信號和非碰撞信號。例如，Park等［3］提出了基于監督學習的檢測方法，使用支持向量機和卷積神經網絡對硬碰撞和軟碰撞分別進行特征提取，用四維和六維時間序列數據堆棧訓練的六關節機器人實現碰撞檢測。Park等［4］提出了基于無監督異常檢測算法的碰撞檢測方法，僅使用無碰撞運動下的數據訓練模型即可魯棒地檢測各種類型的碰撞。上述方法雖然可以準確識別碰撞信號，實現碰撞檢測，但是往往需要大量的數據進行離線訓練，且智能算法的復雜性可能會降低碰撞檢測實時性。

另一種實現方式是根據機器人的動力學模型和運動狀態來估計外力矩值，設置置信閾值并假定高于閾值的部分是由碰撞引起的［5］。Haddadin等［6］對該方式實現碰撞檢測的整體流程進行了拆分并做了全面概述，將實現過程分為關節外部力矩估計和碰撞識別兩個環節。其中關節外部力矩估計的準確性是影響碰撞檢測精度的主要因素。Zhang等［7］分別建立機器人關節和電機的動力學方程，用兩者計算的差值來估計外力矩，但該方法獲得準確的估計精度需要建立復雜的摩擦力模型。文獻［8-10］中通過引入廣義動量將動力學模型轉化成不含加速度的形式，減少噪聲的干擾，進而將廣義動量的殘差量作為觀測量，構建一階外力觀測器，實現了對關節外力矩的迭代估計。Han等［11］基于模型辨識方法建立高階有限時間觀測器來估計外部時變相互作用力。上述建立觀測器的方法沒有考慮到動力學模型的辨識誤差等影響。在碰撞識別過程中，當外部力矩估計準確時，設置靜態對稱置信閾值［12-14］即可實現碰撞識別。然而，外力矩估計模型并不能保證完全準確，采用靜態對稱閾值的方法會產生誤報。文獻［15-16］中根據外力矩估計值生成動態閾值，具有更好的檢測準確性和魯棒性。

針對動力學模型辨識誤差和電機電流測量誤差導致的外力矩估計不準確以及靜態對稱閾值可能產生誤報的問題，本文提出了一種基于擾動卡爾曼濾波外力矩觀測器的碰撞檢測方法。該方法分為3個步驟。第1步基于擾動原理定義機器人的關節擾動模型，以機器人廣義動量和擾動動力學變量為狀態變量，并結合動力學模型轉化為狀態空間方程。第2步考慮動力學的模型辨識誤差以及電機電流測量過程中引入的噪聲，基于卡爾曼濾波算法進行迭代估計得到最優外力矩觀測值。最后，提出一種隨關節速度變化的時變對稱閾值函數，根據外力矩觀測器的輸出值來實現碰撞檢測，以提高碰撞檢測的靈敏度。

1 機器人關節受力分析

如圖1所示，在具有垂直關節的n自由度串聯型機器人的運動過程中，若機器人關節發生碰撞，則第i關節所受到的力/力矩包括摩擦力τf，i、關節運動克服自身重力、慣性力等所需力矩τd，i、電機實際的驅動力矩τm，i以及碰撞力等效的力矩τext，i。此時，機器人動力學平衡方程可以表示為

圖1 機器人運動過程中的力/力矩分析Fig.1 Force/torque analysis during robot movement

式中：τf∈Rn×1表示各關節受到的摩擦力矩矢量；τd∈Rn×1表示各關節運動克服自身重力、慣性力等所需的力矩矢量；τm∈Rn×1表示各關節的電機驅動力矩矢量；τext∈Rn×1表示碰撞外力等效至各關節的外力矩矢量。關節完成目標運動所需力矩τd可以等價為

式中：M(q)∈Rn×n表示慣性矩陣；C(q，)∈Rn×n表示科里奧利和離心力矩陣；G(q)∈Rn×1表示重力矢量；q、、分別表示關節轉角、關節速度和關節加速度。

為了實現碰撞檢測，需要能夠實時計算碰撞力等效的力矩值τext，由式（1）可以得到碰撞力等效的力矩計算公式為

由上式可知，計算碰撞力等效的力矩需要知道機器人的關節摩擦力、動力學參數以及機器人運動過程中的關節轉角、關節速度、關節加速度和關節驅動力矩。由于關節摩擦力在高速運動過程中與關節速度之間表現出了近似的線性關系，所以摩擦力模型通常采用庫倫摩擦與粘性摩擦相結合的線性方程描述，即：

式中，fc，i為關節i的庫倫摩擦系數，fv，i為關節i的粘性摩擦系數，S(·)為符號函數。

2 機器人免傳感器碰撞檢測方法

2.1 機器人動力學模型的狀態空間表達式

在得到機器人的動力學參數后，可以根據式（3）實時計算碰撞力的等效力矩τext。然而，工業機器人通常不配備加速度?的信號采集器，無法直接獲得?信號。常用的?信號獲取方法是對速度q?信號進行差分計算，但是這樣會放大信號采集過程中的噪聲，從而導致外力矩估計不準確。為解決上述問題，引入機器人的廣義動量p，具體表達式為

根據機器人動力學的結構可知慣性矩陣M(q)是對稱矩陣，為斜對稱矩陣［8］，可以得到：

結合式（3）、（5）、（6），則在運動過程中受碰撞時的機器人動力學方程式可表示為

在動力學參數辨識過程中，由于動力學的非線性特性以及摩擦模型不準確，導致參數辨識結果存在偏差。考慮動力學參數辨識的誤差，則上式可進一步表示為

機器人與環境發生碰撞產生的外力可以看作機器人系統自由運動時的擾動。由于碰撞力等效的力矩τext是由外界系統產生的，根據擾動原理［18］，關節擾動模型可描述為

式中：ωτ∈Rn×1表示擾動動力學變量；S∈Rn×n和H∈Rn×n表示擾動的系統矩陣；ed～N(0，Σd)表示擾動模型的誤差項，定義為高斯噪聲，其中Σd∈Rn×n表示ed的協方差對角矩陣。

結合式（8）-（9），將擾動動力學變量與機器人廣義動量組合為狀態變量，并定義機器人廣義動量為狀態空間的輸出，建立機器人外力矩觀測系統，其狀態空間方程表示為

式中，系統狀態x=[pn×1ωτ，n×1]T，系統輸入u=τu，n×1，系統狀態轉移矩陣A、系統輸入矩陣B與系統輸出矩陣C分別為

式中，In×n為n×n維的單位矩陣。系統的過程噪聲w表示為w=[eTpeTd]T，即：

由于機器人的關節轉角和關節速度是可測量的，所以系統的輸出定義為可測量的動量，即y=p，測量噪聲v定義為高斯噪聲v～N(0，Σo)，其包括測量關節轉角以及關節速度時引入的噪聲，其中Σo∈Rn×n表示測量噪聲v的協方差對角矩陣。

2.2 基于卡爾曼濾波的外力矩觀測器

為了得到式（10）的最優狀態，從而得到關節外力矩的最優觀測值，采用卡爾曼濾波算法［19］進行最優狀態估計。因為卡爾曼濾波是一個遞推型算法，所以對式（10）進行離散化處理，即：

式中，k為離散系統的時間變量。離散后的系統矩陣Ak，Bk可通過計算系統矩陣的矩陣指數［20］獲得：

式中，Ts為機器人系統的采樣周期，exp(·)為矩陣指數運算的算子。

由于系統的輸出沒有隨時間發生變化，輸出矩陣Ck保持不變，測量噪聲vk的協方差矩陣Rk只與采樣周期Ts相關，則：

過程噪聲wk的協方差對角矩陣Qk通過如下的矩陣指數運算得到：

在k時刻，使用卡爾曼濾波算法得到最優系統狀態xk后，則該時刻的機器人關節外力矩估計值矢量為

對于離散后的系統狀態方程式（10），采用卡爾曼濾波進行最優狀態估計獲得關節外力矩的過程可概括為如下算法

基于卡爾曼濾波的關節外力矩估計算法1 初始化系統狀態和狀態協方差矩陣：x0=0(n+n)×1， P0=In×n；2 在離散系統的每一個采樣周期下（k≥1）；3 根據關節轉角q(k)與速度q?(k)的測量值，更新系統輸出yk=pk=M(q(k))q?(k)；4 采集電機驅動力矩τm(k)的測量值，更新系統輸入uk=τu(k)；5 根據式（12）-（14）計算系統矩陣與協方差矩陣Ak，Bk，Ck，Rk，Qk；6 預測系統狀態和狀態協方差矩陣：x?k=Akxk-1+Bkuk，P?k=AkPk-1Ak T+Qk；7 更新卡爾曼增益：Kk=P?kCk T(CkP?kCk T+Rk)-1；8 使用系統輸出校正系統狀態和狀態協方差矩陣：xk=x?k+Kk(yk-Ckx?k)，Pk=(I-KkCk)P?k；9 根據最優的系統狀態對關節外力矩進行觀測：τ?ext(k)=()[]0n×nH·xk；10 停止觀測。

2.3 基于外力矩觀測器的碰撞檢測

對于工業機器人關節i，在得到觀測器輸出的力矩估計值τext，i后，令其閾值函數為Ri(x)，然后對力矩估計值進行實時判斷。當力矩估計值超出閾值范圍時，則認為超出的部分是由外界碰撞導致的。因此，定義檢測信號Ti來判斷機器人關節是否發生了碰撞，即：

式中，Ti(k)為關節i在k時刻的碰撞檢測信號，τext，i(k)為關節i在k時刻外力矩觀測器的輸出信號，Ri(k)為關節i在k時刻閾值函數的輸出值。Ti(k)=1，表示關節i在第k時刻發生碰撞；Ti(k)=0，表示關節i在k時刻無碰撞發生。

在實際運動過程中，外力矩觀測器輸出值可能會受到以下因素的影響：（1）電流測量噪聲 由于機器人本身沒有力矩傳感器，關節驅動力矩是由關節電機電流轉換得到的，在采集電流過程中可能存在噪聲；（2）摩擦力誤差 由于目前還沒有準確的摩擦模型能準確擬合實際的摩擦現象，采用的摩擦模型只能近似擬合摩擦力，所以計算摩擦力與實際摩擦力存在誤差。

由于上述誤差的存在，即使機械臂未發生碰撞，外力矩觀測器的輸出信號τ?ext，i也可能不為0。所以根據實際情況提出適用于外力矩觀測器的置信閾值函數可以提高檢測魯棒性。上述的誤差因素中，摩擦力誤差對外力矩估計的準確性影響較大。所用的摩擦模型在中高速運動時對摩擦力有較好的擬合效果而低速時的擬合效果不足，導致外力矩觀測器的輸出在低速運動時會出現較大的偏差。綜合上述分析，提出一種同關節速度變化的閾值模型，該模型可以描述為

式中，C為閾值常數，β為隨速度衰減系數，當關節運動速度較小時表現出較大的閾值，當關節運動速度較大時表現出較小的閾值，Δfv為粘滯摩擦系數誤差量，Δfc為庫倫摩擦系數誤差量。時變閾值模型的參數根據實際實驗結果進行優化。

3 機器人外力估計和碰撞檢測實驗

3.1 實驗平臺與系統

為了驗證所提外力矩觀測器的性能以及碰撞檢測方法的準確性，本文基于GSK-RB08A3型工業機器人實驗平臺進行實驗驗證。機器人系統結構如圖2所示。平臺采用EtherCat總線協議進行通信，使用裝載Kithara實時控制系統的工控機作為上位機進行實時交互。在運動過程中，機器人關節轉角以及電機電流信號由機器人本體的編碼器采樣得到，通過機器人控制柜發送到上位機，并顯示在實時控制系統的交互界面。上位機根據反饋信號計算控制指令，通過機器人控制柜產生驅動信號控制機器人運動。機器人系統的控制和通信周期為1 ms。

圖2 機器人系統Fig.2 Robot system

3.2 動力學模型參數辨識驗證

計算外力矩觀測器的輸出信號需要機器人連桿的動力學參數，在無碰撞條件下進行參數辨識實驗。首先，使用SymPyBotics工具箱［21］建立機器人剛體動力學模型，并得到最小可辨識參數集，然后采用具有周期性的有限傅里葉級數［22］作為激勵軌跡驅動機器人跟蹤軌跡采集關節轉角信號和力矩信號，使用三階巴特沃斯低通濾波器對采樣信號進行濾波。最后基于最小二乘法對剛體動力學參數進行辨識。參數辨識流程具體可參照文獻［23］。

圖3表示機器人前3個關節辨識出的動力學模型參數在激勵軌跡上的性能。其中黑色曲線表示跟蹤激勵軌跡過程中采集的實際測量扭矩，紅色曲線表示通過動力學模型計算的力矩值，藍色曲線表示測量力矩與模型計算力矩之間的誤差，綠色曲線表示關節運動速度曲線。利用整體辨識方法得到的動力學參數，前3個關節的整體力矩預測均方根誤差值分別為2.4、15.3和6.2 N·m。從圖中可以看出，在運動平穩時，即關節速度的絕對值大于0.05 rad/s時，能夠較好地預測關節力矩，此時前3個關節的預測均方根誤差值分別為0.4、4.5和1.6 N·m；然而在速度換向處，即關節速度的絕對值小于等于0.05 rad/s時，會出現較大的偏差，此時前3個關節的預測均方根誤差值分別為15、158和55 N·m。

圖3 參數辨識驗證Fig.3 Parameter identification verification

3.3 外力矩估計方法實驗驗證

為驗證所提外力矩觀測器的準確性和有效性，使用與參數辨識相同的激勵軌跡進行仿真評估。應用提出的擾動卡爾曼濾波觀測器估計外力矩值，并與文獻［8］中的廣義動量觀測器的性能進行對比。為了使所提外力觀測器具有較優的性能，本文通過仿真實驗反復進行調整，協方差矩陣參數設置為對角矩陣且均為常數，具體參數值如表1所示。廣義動量觀測器的增益矩陣設置為Ko=diag(20，20，20，20，20，20)。

表1 擾動卡爾曼濾波觀測器中協方差矩陣參數Table 1 Parameters of covariance matrix in disturbance Kalman filter observer

以關節1和關節4為例分析外力矩觀測器的性能。圖4表示無碰撞時兩種不同觀測器的輸出值對比曲線圖。圖中藍色曲線表示廣義動量觀測器的輸出值，紅色曲線表示擾動卡爾曼濾波觀測器的輸出值，黑色曲線表示關節速度。

圖4 無碰撞下的外力矩觀測器輸出對比Fig.4 Comparison of the output of the external torque observer without collision

從圖中可以看出，與廣義動量觀測器的輸出值相比，擾動卡爾曼濾波觀測器的輸出值關節1的平均輸出誤差值減小了2.46 N·m，精度提高了65.56%，關節4的平均輸出誤差值減小了0.75 N·m，精度提高了50.14%，說明在無碰撞情況下，所提觀測器的精度優于廣義動量觀測器。然而，在關節速度發生變化的過程中，兩種觀測器的估計誤差都增大，主要的原因是摩擦模型在低速時無法準確擬合摩擦力的真實情況。為了更詳細的進行比較，采用均方根誤差作為評價指標，量化兩種觀測器的估計效果。表2表示無碰撞時兩種外力矩觀測器的輸出均方根誤差值。表中給出了機器人6個關節的觀測器輸出均方根誤差值，具體的機器人關節圖可以參考圖2。從表2中可以看出，擾動卡爾曼濾波觀測器整體上有更小的均方根誤差，力矩估計精度提高了52.03%，說明所提的外力矩觀測器具有更高的精度。

表2 無碰撞下的外力矩觀測器輸出均方根誤差值Table 2 Root mean square error of the output of the external torque observer without collision

3.4 免傳感器碰撞檢測實驗

將所提的外力矩觀測器用于實時監控關節外力矩變化，以機器人第1關節為例分析碰撞檢測方案的有效性。在實驗過程中，機器人跟蹤直線運動軌跡，做一次往返運動。圖5表示用于碰撞檢測實驗的運動軌跡圖，起點位姿P1為［720，-300，640，180°，0，0］終點位姿P4為［525，730，640，180°，0，0］。一次往返運動即從點P1經過P2，P3到達P4，再從P4經過P3，P2回到點P1后停止。

圖5 用于碰撞檢測實驗的仿真軌跡Fig.5 Simulation trajectories for collision detection experiments

為了提高碰撞檢測靈敏度，降低碰撞的誤報率，本文通過反復對閾值函數的參數進行調整，設置碰撞閾值函數的參數為：C=15，β=25，Δfv=1.57，Δfc=1.01。在實驗過程中，為了更好的觀測外力矩觀測器的輸出曲線，在機器人發生碰撞后不中斷機器人原有運動，完整運行整個直線運動軌跡。

在實驗中，首先讓機器人在無碰撞下跟蹤直線運動軌跡，得到外力矩觀測器的輸出值。圖6表示無碰撞運動過程中關節1的外力矩觀測器輸出值與閾值曲線，從圖中可以看出，在機器人啟動和換向階段，不準確的摩擦模型以及一些非線性特性會造成觀測器輸出值發生突變，此時的碰撞閾值也會跟著發生突變，防止發生碰撞誤報。

圖6 無碰撞下外力矩觀測器輸出曲線Fig.6 Output curve of external torque observer without collision

在機器人運行過程中，用金屬棒敲擊機器人關節模擬外部碰撞。模擬碰撞實驗過程中，啟動系統定時器，以顯示機器人系統檢測到發生碰撞的時間。圖7表示碰撞下關節1的外力矩觀測器輸出值和閾值曲線。從圖中可以看出，在沒有發生碰撞的時間段，觀測器的輸出曲線與無碰撞運動過程中的曲線相似。碰撞實際發生在2 575 ms處，外力矩值約為-3.54 N·m，此時閾值上、下限為［3.41，-3.41］N·m，而此時機器人系統沒有檢測到碰撞發生，在經過18ms后，即2 593 ms機器人系統發出碰撞警報。為了測試碰撞檢測方法的實用性，分別在有無碰撞下重復運行直線軌跡各200次。在無碰撞過程中，機器人發出3次誤報，在啟動和換向階段時分別產生2次和1次誤報，誤報率為1.5%。在有碰撞實驗過程中，系統均成功檢測到碰撞的發生，包括在關節換向階段，實際發生碰撞的時間與系統發出警報的時間平均相差約35 ms，即系統的平均延時約為35 ms。

為了進一步分析時變閾值在碰撞檢測靈敏度中的作用，分別在碰撞檢測中使用靜態對稱閾值與時變對稱閾值進行人機碰撞對比實驗。在該實驗過程中，機器人檢測到碰撞后立即執行急停指令，以展示兩種不同閾值函數在實際碰撞情況下的靈敏度。實時外力矩值與閾值曲線如圖8所示，圖8（a）表示時變對稱閾值下的碰撞檢測，在1 965 ms左右發生人機碰撞，機器人系統在2 017 ms成功檢測并執行急停，檢測延遲約52 ms；圖8（b）表示靜態對稱閾值下的碰撞檢測，碰撞實際發生在約2 181 ms，機器人在2 305 ms成功檢測并執行急停，檢測延遲約124 ms；從上述實驗對比結果可得，與靜態閾值對比，時變閾值下的碰撞檢測延時縮短了58.06%，說明了時變閾值函數在碰撞檢測的靈敏度更高。

圖8 不同閾值函數下的碰撞檢測對比Fig.8 Comparison of collision detection under different threshold functions

4 結語

為了降低動力學模型的參數辨識誤差以及電機電流的測量誤差等因素影響外力矩估計的準確性，提高碰撞檢測的精度。本文設計了一種擾動卡爾曼濾波外力矩觀測器。該觀測器基于擾動原理，將外部碰撞等效的外力矩作為擾動項，定義了關節擾動模型，并引入機器人的廣義動量構建狀態空間方程式。建立狀態空間方程過程中，考慮動力學模型的參數辨識誤差以及電機電流的測量誤差，并基于卡爾曼濾波算法進行迭代估計，得到最優外力矩觀測值，從而實現免傳感器的最優外力矩估計。與基于廣義動量構建的一階觀測器在無碰撞實驗中進行對比，外力矩估計精度提高了52.03%。

為了提高碰撞檢測的靈敏度并降低碰撞誤報率，本文通過分析可能影響外力矩觀測器輸出值的因素，提出一種隨關節速度變化的時變閾值函數用于檢測碰撞，該閾值函數具有更好的魯棒性。以6自由度串聯型關節工業機器人為實驗對象，笛卡爾空間直線軌跡為運動軌跡，驗證了基于擾動卡爾曼濾波外力矩觀測器的碰撞檢測方法。實驗結果表明，碰撞實驗過程中，系統的平均延時約為35 ms，相比于靜態對稱閾值函數，時變對稱閾值函數在檢測延遲上縮短了58.06%，提高了碰撞檢測的靈敏度。利用該方法進行碰撞檢測，無碰撞實驗的誤報率為1.5%，主要是由于動力學模型的中摩擦模型在啟動和換向階段精度下降導致誤報的發生。為了繼續降低誤報的發生，將在后續工作中對摩擦模型進行進一步研究。