基于工業機器人動力學模型的差分力矩偏差碰撞檢測方法＊

張振宇 鐘佩思 張 超 王祥文 劉 梅 孫鈺雁

（①山東科技大學先進制造技術研究中心，山東 青島 266590；②山東科技大學機械電子工程學院，山東青島 266590；③青島海信日立空調系統有限公司，山東 青島 266510）

隨著機器人技術的發展，工業機器人之間、工業機器人與人之間的協作越來越密切，在協作的過程中難免會發生機器人與入侵物體的碰撞。因此需要準確地進行碰撞檢測，以避免碰撞事故的發生。工業機器人的碰撞檢測可分為使用外部傳感器和不使用外部傳感器的碰撞檢測[1?3]。不使用外部傳感器時，采用基于動力學模型的方法來實現碰撞檢測，通過動力學模型計算相應的預測力矩，將預測力矩與實際力矩的力矩偏差值用于碰撞檢測[4]。機器人的實際力矩是通過電流法來計算關節力矩，通過伺服電機相電流來求得電機實際輸出驅動力矩[5]。采集的機器人實時位置、速度及電機電流等存在噪聲影響，因此有效的濾波方案能夠獲得精確的預測力矩和實際力矩。

基于動力學模型的碰撞檢測方法，是在獲得準確動力學參數的基礎上完成的，因此需要準確的機器人動力學參數，動力學參數的準確性影響著碰撞檢測的精度。現有機器人的動力學參數獲取，通過動力學參數辨識獲得。動力學參數辨識的過程可分為以下5個步驟：（1）建立動力學模型；（2）設計激勵軌跡；（3）采樣與數據處理；（4）動力學參數辨識；（5）驗證辨識模型[6]。從動力學模型辨識過程中能夠看出，設計良好的激勵軌跡，采用準確度更高的動力學參數辨識方案，將獲得接近真實值的機器人動力學參數。通過遺傳算法優化激勵軌跡，但是易陷入局部最優解[7?8]。改進的遺傳算法能夠有效避免陷入局部最優解，并在多個領域應用[9?10]。動力學參數辨識常用的方法是最小二乘法[11]，但是最小二乘法容易受到噪聲的干擾，由此提出遞推最小二乘法、加權最小二乘法等方法來進行動力學參數辨識[12?13]，均需要觀測矩陣求逆，而觀測矩陣存在奇異值情況下無法求逆，效果較差；Urrea C等提出采用線性神經網絡進行機器人動力學參數辨識的方法[14?15]，但神經網絡受學習率的影響較大，學習率過大會導致無法收斂，學習率過小容易陷入局部極值點，而自適應學習率能夠使得神經網絡快速收斂的同時避免陷入局部極值。

本文針對機器人在動力學模型未知的情況下，通過動力學參數辨識，獲得機器人準確的動力學模型，然后通過機器人的動力學模型計算機器人預測力矩，將預測力矩與實際力矩做差獲得力矩偏差，力矩偏差分別向后一階差分、向后二階差分，通過設定的閾值來檢測是否發生碰撞和發生碰撞類型。在動力學參數辨識中，采用基于懲罰函數的自適應遺傳算法優化激勵軌跡，利用自適應學習率的單層神經網絡進行動力學參數辨識。采用力矩偏差差分的碰撞檢測方法不僅實現無外部傳感器碰撞檢測，同時將碰撞的類型區分為有意碰撞和無意碰撞；通過區分碰撞類型，能夠在機器人發生無意碰撞時，人為干預將無意碰撞轉化為有意碰撞，從而避免發生剛性碰撞造成機器人損傷。實驗結果表明，設計的碰撞檢測算法，不需要添加額外的傳感器，降低碰撞檢測成本，能夠準確地將碰撞區分為有意碰撞和無意碰撞，所采用的動力學參數辨識方法，辨識精度較其他方法有所提高，從而提高碰撞檢測的精度。基于工業機器人動力學模型的差分力矩偏差碰撞檢測算法的流程如圖1所示。

圖1 碰撞檢測算法流程圖

1 機器人動力學模型建立與線性化

1.1 動力學模型建立

建立n連桿機器人的動力學模型為

1.2 動力學模型線性化

由于標準動力學模型具有非線性強耦合的特點，直接利用該模型進行動力學參數辨識異常困難[16]，因此將機器人的動力學模型進行線性化，可表示為：

借助Python環境下的SymPyBotic工具箱求解機器人的動力學最小參數矩陣和機器人各關節力矩表達式。

2 基于懲罰函數的自適應遺傳算法優化激勵軌跡

2.1 激勵軌跡選擇

機器人運行激勵軌跡時達到的位姿越多，測得動力學參數信息越全面，動力學參數辨識的精度就會越高。Swevers J使用有限傅里葉級數對機器人進行激勵軌跡設計，方便易用的同時，能夠保證較好的辨識效果，被廣泛使用，且傅里葉級數能夠減少噪聲對參數精度的影響[18]。第i關節傅里葉級數公式為

式中：i是機器人連桿的編號；q0i是關節的初始位置；M是傅里葉級數；ai,k、bi,k為正弦項和余弦項的系數；ω=2πf為基角頻率。為使得關節初始位置變化平穩，設q0i為式（8）。

根據式（7）獲得機器人關節i的角速度和角加速度為式（9）和式（10）。

為使得激勵軌跡運動范圍充滿整個運動空間，達到最好激勵效果，在對激勵軌跡進行參數優化時，優化目標函數為觀測矩陣最小條件數[19]。條件數是線性方程組解對輸入輸出誤差敏感性的測量，條件數越大表明敏感性越差，因此將觀測矩陣條件數最小作為優化目標函數。

設關節i的傅里葉軌跡參數向量Ri=[ai,1,bi,1,...,則所有關節的激勵軌跡參數向量R=軌跡采樣點數為K個，非線性優化問題可表示為：

式中：qimax是關節i的位置限制；是關節i的速度限制；是關節i的加速度限制。

式中：tstart、tend為軌跡起始時刻和結束時刻。

2.2 基于懲罰函數的自適應遺傳算法原理

通過增加懲罰函數，能夠將約束問題轉化為無約束問題。因此在遺傳算法中加入懲罰函數以增大適應度，進行排列后，將增大后的個體剔除。自適應機制將交叉概率和變異概率自適應調整，如圖2所示。

圖2 基于懲罰函數的自適應遺傳算法原理圖

（1）初始種群：隨機生成初始種群。

（2）適應度函數：適應度函數是評價個體好壞的標準，是非負的值。在動力學參數辨識中，采用觀測矩陣的最小條件數作為適應度函數。

（3）懲罰函數：根據動力學參數辨識的需要，設計動態懲罰函數。如：

式中：λ是懲罰因子；α是動態偏差因子（不滿足約束條件的解偏離約束差值的平均值）；b為經驗常數；t(R)是個體滿足線性條件和非線性條件的個數。

（4）選擇：選擇是從父代種群中選出優化的個體來產生下一代，采用最佳保留選擇，因為條件數作為適應度函數，因此條件數越大則適應度越小，因此選擇適應度函數值低的個體，排除適應度函數值高的個體。

（5）交叉與變異：交叉與變異的概率有固定概率和動態概率2 種方式，動態概率是能夠隨著算法的迭代概率也隨之變化。采用動態概率，以實現交叉概率與變異概率的自適應調整。概率的調整函數為

其中：k1、k2分別為交叉和變異的經驗常數，則k1=1、k2=0.5。fmin、fmean分別為種群的適應度函數的最小值和平均值。

交叉選擇算術交叉，變異采用高斯變異。

3 采用自適應學習率的單層神經網絡實現動力學參數辨識

3.1 實驗數據采集與處理

采集的位置、速度及電流信號存在的噪聲將對參數辨識結果產生影響，且加速度是由速度差分獲得，差分會成比例放大噪聲，同時為了防止經過濾波后的數據產生相位延時，采用零相位濾波以避免產生延時。具體濾波方案如下：

（1）位置：巴特沃夫斯低通濾波+零相位濾波。

（2）速度：巴特沃夫斯低通濾波+零相位濾波。

（3）加速度：巴特沃夫斯低通濾波+零相位濾波+RLOESS平滑。

（4）力矩：巴特沃夫斯低通濾波+零相位濾波+RLOESS平滑。

3.2 神經網絡算法

單層神經網絡通過1個輸入層計算后獲得輸出層，通過對每個輸入進行加權求和（w），再通過激活函數f(x)得到對應的輸出，如圖3所示。

圖3 單層神經網絡

將觀測矩陣的每1行作為單層神經網絡的輸入，輸出為每一行對應的力矩值。更新的權重W為辨識的參數，激活函數為f(x)=x。

權重更新方法為

式中：dw為權重增量；β為超參數；α為學習率；e為實際力矩值與預測力矩值的差值；κ為經驗常數；X為觀測矩陣的某一行。

將學習率依據實際力矩與辨識后的預測力矩值的差值進行自適應調整。加入自適應學習率，能夠提高神經網絡的學習效果防止陷入局部最優解，同時能夠避免無法收斂。

4 基于差分力矩偏差的碰撞檢測算法

4.1 碰撞檢測算法原理

通過辨識的動力學參數獲得機器人預測力矩，利用機器人驅動電流信號計算實際力矩，將預測力矩與采集的實時力矩做差，獲得力矩偏差。

依據辨識機器人的動力學參數，根據式（16）能夠計算出機器人預測力矩。

式中：τe為 摩擦力矩；為機器人的力矩。

將預測力矩與實際力矩τreal做差獲得力矩偏差：

根據圖4a所示的力矩偏差變化圖能夠看出，在有意碰撞是入侵物體有準備地與機器人發生碰撞，接觸緩慢且緩和，機器人力矩偏差變化也會緩慢，而無意碰撞力矩變化較為急劇。采用差分方程能夠獲得力矩偏差的變化趨勢。差分方程分為向前差分、向后差分、中心差分，因為要在機器人碰撞瞬間能夠快速檢測碰撞，采用向后差分方程。

f(xk?1)、f(xk)、f(xk+1)一階向后差分方程為：

因此，力矩偏差的一階向后差分公式可表示為：

依據式（20）獲得力矩偏差的一階向后差分。如圖4b為一階力矩偏差差分變化圖。因為差分方程能夠放大噪聲，因此采取無延時濾波處理。從圖中看出，有意碰撞一階力矩偏差值變化較為平緩，而無意碰撞變化較為明顯，能夠有效區分出有意碰撞與無意碰撞。考慮到碰撞力矩較小，引起的力矩偏差較小，因此將力矩偏差進行二階力矩偏差差分，從而獲得更為準確的碰撞檢測時刻。

對于f(xk?1)、f(xk)、f(xk+1)則二階向后差分方程為：

因此，力矩偏差的二階向后差分公式可表示為

式（21）、（23）在t?1<0,t?2<0時，設?τ(t?1)、?τ(t?2) 等于?τ(t)。

依據式（23）獲得力矩偏差的二階差分。如圖4c所示，能夠看出在無意碰撞階段力矩偏差二階差分變化較為明顯，但在初始與結束階段，變化也較為明顯，因此需結合一階力矩偏差差分進行力矩碰撞檢測。

圖4 力矩偏差及一階二階差分

通過設置合理的置信區間，當力矩偏差、一階力矩偏差差分、二階力矩偏差差分超出置信區間則認為機器人發生有意碰撞或無意碰撞，此時機器人采取相應的動作，防止碰撞力進一步增大，從而避免損失。

5 動力學參數辨識與碰撞檢測實驗

5.1 動力學參數辨識實驗

利用6R工業機器人進行相關激勵軌跡的設計，采用基于懲罰函數的自適應遺傳算法優化激勵軌跡，優化后的激勵軌跡關節位置、關節速度、關節加速度如圖5所示。從圖中能夠看出均滿足約束條件，且速度與加速度的初始和結束均為零，滿足激勵軌跡優化的目標要求，能夠有效實現6R機器人在關節空間內達到多種位姿，從而能夠獲得準確的機器人動力學參數。

圖5 激勵軌跡位置、速度、加速度

將6R機器人運行激勵軌跡后采集的數據進行濾波處理后，利用自適應學習率的單層神經網絡進行動力學參數辨識。為了驗證辨識參數的準確性，隨機生成滿足約束的1條軌跡，將辨識力矩與實際力矩進行對比，并利用均方根誤差來對比辨識力矩和實際力矩型之間誤差進行描述，其表達式為：

辨識力矩與實際力矩的對比如圖6所示。各關節的力矩均方根誤差如表1所示。

表1 各關節誤差

圖6 辨識力矩與實際力矩對比

對比發現，采用自適應學習率的單層神經網絡進行工業機器人動力學參數辨識，辨識精度通過均方根誤差能夠看出有所提高。同時也證明采用基于懲罰函數的自適應遺傳算法優化激勵軌跡有效。得到的動力學參數能夠滿足碰撞檢測的需要。將獲得的動力學參數應用于碰撞檢測算法。

5.2 差分力矩偏差碰撞檢測實驗

利用辨識的動力學參數，結合獲取的實際位置、實際速度、預測位置、預測速度、電機的電流信號及差分獲得的實際加速度、預測加速度計算出實際力矩與預測力矩。將實際力矩與預測力矩加入碰撞檢測算法中，利用6R工業機器人開展碰撞檢測實驗。首先將6R工業機器人運行多次，采集無碰撞狀態下的位置、速度信號，從而獲得無碰撞狀態下的實際力矩信息，從而獲得該機器人的碰撞置信區間，如表2所示。碰撞狀態檢測條件如表3所示。依據表2和表3結合差分力矩偏差碰撞檢測算法對6R工業機器人的各個關節分別進行碰撞實驗。碰撞過程如圖7所示，為機器人6 個關節在不同狀態下的碰撞，關節1為大阻力無意碰撞狀態，關節2為微阻力無意碰撞狀態，關節3為小阻力有意碰撞狀態，關節4為小阻力無意碰撞狀態，關節5為大阻力有意碰撞狀態，關節6為小阻力有意碰撞狀態

圖7 碰撞過程

表2 關節置信區間

表3 碰撞檢測條件

圖8～13為碰撞檢測算法的實驗結果，圖8為大阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖9為微阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖10為小阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞；圖11為微阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞；圖12為小阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖13為小阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞。因此驗證碰撞算法能夠對于有意碰撞與無意碰撞能夠進行有效檢測和區分，區分精度高，不需要在機器人外部添加額外的傳感器。

圖8 關節1碰撞檢測

圖9 關節2碰撞檢測

圖10 關節3碰撞檢測

圖11 關節4碰撞檢測

圖12 關節5碰撞檢測

圖13 關節6碰撞檢測

6 結語

本文提出一種基于機器人動力學模型的差分力矩偏差碰撞檢測算法，能夠有效檢測并將碰撞區分為有意碰撞和無意碰撞，從而使機器人在協同作業時，發生無意碰撞，人為能夠將無意碰撞轉化為有意碰撞，觸發相應的后退機制，避免發生剛性碰撞造成設備損傷，且重新規劃路線避免停機造成損失；人為無法干預時觸發整機停止機制，避免損傷加劇。與人發生碰撞時，能夠有效檢測為有意碰撞，從而觸發后退機制，避免對人體造成損傷。采用基于懲罰函數的自適應遺傳算法優化激勵軌跡，在避免算法陷入局部最優解的同時，能夠快速獲得最優的激勵軌跡參數。自適應學習率單層神經網絡較加權最小二乘法和遞推最小二乘法辨識精度較高，從而使得機器人的動力學參數辨識得更為準確，提高了碰撞檢測算法中預測力矩的準確性。實驗結果表明，所設計算法能夠有效進行碰撞的檢測與區分，滿足實際生產的需要。