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

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

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

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

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

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

圖1 碰撞檢測算法流程圖

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

1.1 動力學模型建立

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

1.2 動力學模型線性化

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

借助Python環(huán)境下的SymPyBotic工具箱求解機器人的動力學最小參數(shù)矩陣和機器人各關節(jié)力矩表達式。

2 基于懲罰函數(shù)的自適應遺傳算法優(yōu)化激勵軌跡

2.1 激勵軌跡選擇

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

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

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

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

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

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

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

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

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

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

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

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

（3）懲罰函數(shù)：根據(jù)動力學參數(shù)辨識的需要，設計動態(tài)懲罰函數(shù)。如：

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

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

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

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

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

3 采用自適應學習率的單層神經(jīng)網(wǎng)絡實現(xiàn)動力學參數(shù)辨識

3.1 實驗數(shù)據(jù)采集與處理

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

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

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

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

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

3.2 神經(jīng)網(wǎng)絡算法

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

圖3 單層神經(jīng)網(wǎng)絡

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

權重更新方法為

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

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

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

4.1 碰撞檢測算法原理

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 動力學參數(shù)辨識與碰撞檢測實驗

5.1 動力學參數(shù)辨識實驗

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

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

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

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

表1 各關節(jié)誤差

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

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

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

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

圖7 碰撞過程

表2 關節(jié)置信區(qū)間

表3 碰撞檢測條件

圖8～13為碰撞檢測算法的實驗結(jié)果，圖8為大阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖9為微阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖10為小阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞；圖11為微阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞；圖12為小阻力無意碰撞檢測，從圖中能夠看到檢測碰撞為無意碰撞；圖13為小阻力有意碰撞檢測，從圖中能夠看到檢測碰撞為有意碰撞。因此驗證碰撞算法能夠?qū)τ谟幸馀鲎才c無意碰撞能夠進行有效檢測和區(qū)分，區(qū)分精度高，不需要在機器人外部添加額外的傳感器。

圖8 關節(jié)1碰撞檢測

圖9 關節(jié)2碰撞檢測

圖10 關節(jié)3碰撞檢測

圖11 關節(jié)4碰撞檢測

圖12 關節(jié)5碰撞檢測

圖13 關節(jié)6碰撞檢測

6 結(jié)語

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