本發(fā)明涉及scara機器人關(guān)節(jié)力矩估計方法,特別是一種針對不使用力矩傳感器情況下，以帶噪聲的電機電流信號為基本信息，基于卡爾曼濾波方法的提高關(guān)節(jié)力矩估計精度的方法。

背景技術(shù)：

scara機器人高速、高精度特性的實現(xiàn)依賴于有效的控制算法，而控制算法對關(guān)節(jié)力矩等控制信息的精確度有較高的要求。目前，關(guān)節(jié)力矩有兩種常用的獲取方式，通過力矩傳感器獲取或通過關(guān)節(jié)電機電流換算。

1)通過力矩傳感器獲取關(guān)節(jié)力矩。該方法的主要問題在于成本高以及安裝維護(hù)困難。尤其是對于一些成品機器人，若其設(shè)計階段并未考慮在關(guān)節(jié)處加裝傳感器，則其安裝空間可能并不足夠，同時也可能不具備相應(yīng)的電氣接口。

2)通過關(guān)節(jié)電機電流換算關(guān)節(jié)力矩。該方法的主要問題在于電流是模擬量，存在噪聲，換算得到的關(guān)節(jié)力矩波動較大。目前常用的方法是通過硬件濾波或軟件濾波對讀取到的數(shù)據(jù)進(jìn)行處理。但一些濾波方法的實時性并不能得到保證，從而影響控制算法的實現(xiàn)。

技術(shù)實現(xiàn)要素：

為了在不使用力矩傳感器的情況下提高scara機器人控制算法中所需關(guān)節(jié)力矩信息的精度，本發(fā)明提供一種針對scara機器人關(guān)節(jié)力矩的卡爾曼估計方法。其技術(shù)方案是：

一種scara機器人關(guān)節(jié)力矩的卡爾曼估計方法，包括以下步驟：

步驟1：根據(jù)牛頓歐拉方法建立scara機器人的動力學(xué)模型，獲得各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式；

步驟2：利用多元函數(shù)的一階泰勒展開公式將連續(xù)非線性的關(guān)節(jié)力矩表達(dá)式轉(zhuǎn)化為遞推的線性離散關(guān)節(jié)力矩表達(dá)式；

步驟3：以線性離散化后的各關(guān)節(jié)力矩表達(dá)式為過程方程，并添加相應(yīng)的量測方程，將過程方程和量測方程組合整理為各關(guān)節(jié)力矩的離散時間狀態(tài)空間模型；

步驟4：按照基本卡爾曼濾波方法，結(jié)合關(guān)節(jié)力矩的離散時間狀態(tài)空間模型，確定一個步進(jìn)循環(huán)中需要用到的公式，利用讀取到的關(guān)節(jié)電機電流信號和電機編碼器信號實時估計scara機器人的關(guān)節(jié)力矩。

進(jìn)一步地，步驟1中，在建立scara機器人的動力學(xué)模型時，將所述scara機器人的第三關(guān)節(jié)和第四關(guān)節(jié)合并，形成組合連桿，組合后的關(guān)節(jié)類型為移動關(guān)節(jié)，從而使scara機器人的動力學(xué)模型包括第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)。

進(jìn)一步地，步驟1中，所述各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式為：

式中，a1，a2，a3，a4是由scara機器人動力學(xué)可辨識參數(shù)集中的參數(shù)所組成的系數(shù)項，所述scara機器人動力學(xué)可辨識參數(shù)集由前期參數(shù)辨識工作獲得；f11，f12，f21，f22，f31，f32，p5，jm3為可辨識參數(shù)集中的部分參數(shù)，其中，f11，f21，f31分別表示第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)的庫倫摩擦力系數(shù)，f12，f22，f32分別表示第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)的粘滯摩擦力系數(shù)，p5為組合參數(shù)，jm3為第三關(guān)節(jié)電機的轉(zhuǎn)動慣量；g為重力加速度，這里只表示大小，為-9.81m·s^-2；分別為第一關(guān)節(jié)的角速度和角加速度；分別為第二關(guān)節(jié)的角速度和角加速度；分別為第三關(guān)節(jié)的角速度和角加速度。

進(jìn)一步地，所述步驟2具體包括：

步驟21：對所述各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式在k時刻分別進(jìn)行多元函數(shù)一階泰勒展開，可得到線性化的關(guān)節(jié)力矩遞推公式：

式中，τ1,k，τ2,k，τ3,k分別表示k時刻第一關(guān)節(jié)，第二關(guān)節(jié)，第三關(guān)節(jié)的關(guān)節(jié)力估計矩值；τ1,k+1，τ2,k+1，τ3,k+1分別表示k+1時刻第一關(guān)節(jié)，第二關(guān)節(jié)，第三關(guān)節(jié)的關(guān)節(jié)力矩估計值；分別為第一關(guān)節(jié)的角速度變化量、角加速度變化量，δq2，分別為第二關(guān)節(jié)的角度變化量，角速度變化量、角加速度變化量；分別為第三關(guān)節(jié)的角速度變化量、角加速度變化量；表示第一關(guān)節(jié)力矩表達(dá)式對第一關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；表示第二關(guān)節(jié)力矩表達(dá)式對第一關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；表示第三關(guān)節(jié)力矩表達(dá)式對第三關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；

步驟22：進(jìn)一步將以上線性表達(dá)式改寫可得：

τ1,k+1＝τ1,k+bu1,ku1,k+w1,k

τ2,k+1＝τ2,k+bu2,ku2,k+w2,k

τ3,k+1＝τ3,k+bu3,ku3,k+w3,k

式中，

其中

其中

其中

w1,k，w2,k，w3,k為過程噪聲。

進(jìn)一步地，步驟3中，將過程方程和量測方程組合整理為各關(guān)節(jié)力矩的離散時間狀態(tài)空間模型具體包括：

第一關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ1,k+1＝τ1,k+bu1,ku1,k+w1,k

y1,k＝τ1,k+v1,k

第二關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ2,k+1＝τ2,k+bu2,ku2,k+w2,k

y2,k＝τ2,k+v2,k

第三關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ3,k+1＝τ3,k+bu3,ku3,k+w3,k

y3,k＝τ3,k+v3,k

式中，v1,k，v2,k，v3,k為量測噪聲。

進(jìn)一步地，步驟4中，所述按照基本卡爾曼濾波方法，結(jié)合關(guān)節(jié)力矩的離散時間狀態(tài)空間模型，確定一個步進(jìn)循環(huán)中需要用到的公式具體包括：

步驟41：設(shè)線性系統(tǒng)的離散時間狀態(tài)空間模型為：

xk+1＝fkxk+bukuk+bwkwk

yk＝ckxk+vk

式中，xk為狀態(tài)向量；yk為量測向量；uk為輸入向量；wk為過程噪聲；vk為量測噪聲；fk，buk，bwk，ck為對應(yīng)的系數(shù)矩陣；下標(biāo)中k表示k時刻；

步驟42：對于形如上式所示的狀態(tài)空間模型，可按照如下方程完成一個步進(jìn)循環(huán)的卡爾曼濾波估計：

式中，wk和vk分別是過程噪聲和量測噪聲的協(xié)方差矩陣，和pt分別是t時刻的驗前均值和方程，和pt′分別是t時刻的驗后均值和方程，kt是t時刻的增益，和pt+1分別是t+1時刻的驗前均值和方程。

相比現(xiàn)有技術(shù)，本發(fā)明益處在于：該方法考慮了機器人一側(cè)的特性，對不使用傳感器的scara機器人進(jìn)行關(guān)節(jié)力矩信息估計時，將電機電流信息和機器人自身的動力學(xué)因素綜合考慮，將電機電流信息與動力學(xué)模型結(jié)合起來對關(guān)節(jié)力矩進(jìn)行估計，比起單純應(yīng)用硬件濾波或軟件濾波更深入考慮了實際的應(yīng)用背景，達(dá)到了提高力矩估計精度的目的，同時由于使用的是一種遞推的估計方法，保證了機器人控制的實時性要求。

附圖說明

圖1為本實施例所涉及的scara機器人示意圖。

圖中所示：1-基座；2-第一關(guān)節(jié)臂；3-第二關(guān)節(jié)臂；4-第三關(guān)節(jié)和第四關(guān)節(jié)的組合關(guān)節(jié)臂。

具體實施方式

下面將結(jié)合附圖和具體實施例對本發(fā)明做進(jìn)一步的說明。

一種scara機器人關(guān)節(jié)力矩的卡爾曼估計方法，包括以下步驟：

步驟1：根據(jù)牛頓歐拉方法建立scara機器人的動力學(xué)模型，獲得各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式；

步驟2：利用多元函數(shù)的一階泰勒展開公式將連續(xù)非線性的關(guān)節(jié)力矩表達(dá)式轉(zhuǎn)化為遞推的線性離散關(guān)節(jié)力矩表達(dá)式；

步驟3：以線性離散化后的各關(guān)節(jié)力矩表達(dá)式為過程方程，并添加相應(yīng)的量測方程，將過程方程和量測方程組合整理為各關(guān)節(jié)力矩的離散時間狀態(tài)空間模型；

步驟4：按照基本卡爾曼濾波方法，結(jié)合關(guān)節(jié)力矩的離散時間狀態(tài)空間模型，確定一個步進(jìn)循環(huán)中需要用到的公式，利用讀取到的關(guān)節(jié)電機電流信號和電機編碼器信號實時估計scara機器人的關(guān)節(jié)力矩。

具體而言，步驟1中，在建立scara機器人的動力學(xué)模型時，如圖1所示，所述的scara機器人包括基座1、第一關(guān)節(jié)臂2、第二關(guān)節(jié)臂3、第三關(guān)節(jié)和第四關(guān)節(jié)的組合關(guān)節(jié)臂4。第四關(guān)節(jié)的旋轉(zhuǎn)運動會對第三關(guān)節(jié)的移動造成一定量的同方向耦合，考慮到第四關(guān)節(jié)的旋轉(zhuǎn)運動對scara機器人主要的水平面定位運動不產(chǎn)生影響，為合理簡化動力學(xué)方程，將所述scara機器人的第三關(guān)節(jié)和第四關(guān)節(jié)合并，形成組合連桿，組合后的關(guān)節(jié)類型為移動關(guān)節(jié)，從而使scara機器人的動力學(xué)模型包括第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)。

具體而言，利用牛頓歐拉方法計算關(guān)節(jié)力矩的算法由兩部分組成。第一部分是對每個連桿應(yīng)用牛頓-歐拉方程，從第一連桿1到第n連桿向外迭代計算連桿的速度和加速度。第二部分是從第n連桿到第1連桿向內(nèi)迭代計算出連桿間的相互作用力和力矩以及關(guān)節(jié)驅(qū)動力矩。此外，還需對考慮電機轉(zhuǎn)子所需的力矩以及克服摩擦的力矩。最終總的關(guān)節(jié)力矩表達(dá)式包含驅(qū)動力矩，電機轉(zhuǎn)子力矩和摩擦力矩三部分，因此，步驟1中，所述各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式為：

式中，a1，a2，a3，a4是由scara機器人動力學(xué)可辨識參數(shù)集中的參數(shù)所組成的系數(shù)項，所述scara機器人動力學(xué)可辨識參數(shù)集由前期參數(shù)辨識工作獲得；f11，f12，f21，f22，f31，f32，p5，jm3為可辨識參數(shù)集中的部分參數(shù)，其中，f11，f21，f31分別表示第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)的庫倫摩擦力系數(shù)，f12，f22，f32分別表示第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)的粘滯摩擦力系數(shù)，p5為組合參數(shù)，jm3為第三關(guān)節(jié)電機的轉(zhuǎn)動慣量；g為重力加速度，這里只表示大小，為-9.81m·s^-2；分別為第一關(guān)節(jié)的角速度和角加速度；分別為第二關(guān)節(jié)的角速度和角加速度；分別為第三關(guān)節(jié)的角速度和角加速度。

具體而言，所述步驟2具體包括：

步驟21：對所述各關(guān)節(jié)的連續(xù)非線性力矩表達(dá)式在k時刻分別進(jìn)行多元函數(shù)一階泰勒展開，可得到線性化的關(guān)節(jié)力矩遞推公式：

式中，τ1,k，τ2,k，τ3,k分別表示k時刻第一關(guān)節(jié)，第二關(guān)節(jié)，第三關(guān)節(jié)的關(guān)節(jié)力估計矩值；τ1,k+1，τ2,k+1，τ3,k+1分別表示k+1時刻第一關(guān)節(jié)，第二關(guān)節(jié)，第三關(guān)節(jié)的關(guān)節(jié)力矩估計值；分別為第一關(guān)節(jié)的角速度變化量、角加速度變化量，δq2，分別為第二關(guān)節(jié)的角度變化量，角速度變化量、角加速度變化量；分別為第三關(guān)節(jié)的角速度變化量、角加速度變化量；表示第一關(guān)節(jié)力矩表達(dá)式對第一關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；表示第二關(guān)節(jié)力矩表達(dá)式對第一關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；表示第三關(guān)節(jié)力矩表達(dá)式對第三關(guān)節(jié)角速度求偏導(dǎo)數(shù)，其余類同；

步驟22：進(jìn)一步將以上線性表達(dá)式改寫可得：

τ1,k+1＝τ1,k+bu1,ku1,k+w1,k

τ2,k+1＝τ2,k+bu2,ku2,k+w2,k

τ3,k+1＝τ3,k+bu3,ku3,k+w3,k

式中，

其中：

其中

其中

w1,k，w2,k，w3,k為過程噪聲。

具體而言，步驟3中，將過程方程和量測方程組合整理為各關(guān)節(jié)力矩的離散時間狀態(tài)空間模型具體包括：

第一關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ1,k+1＝τ1,k+bu1,ku1,k+w1,k

y1,k＝τ1,k+v1,k

第二關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ2,k+1＝τ2,k+bu2,ku2,k+w2,k

y2,k＝τ2,k+v2,k

第三關(guān)節(jié)對應(yīng)的狀態(tài)空間模型：

τ3,k+1＝τ3,k+bu3,ku3,k+w3,k

y3,k＝τ3,k+v3,k

式中，v1,k，v2,k，v3,k為量測噪聲。

具體而言，步驟4中，所述按照基本卡爾曼濾波方法，結(jié)合關(guān)節(jié)力矩的離散時間狀態(tài)空間模型，確定一個步進(jìn)循環(huán)中需要用到的公式具體包括：

步驟41：設(shè)線性系統(tǒng)的離散時間狀態(tài)空間模型為：

xk+1＝fkxk+bukuk+bwkwk

yk＝ckxk+vk

式中，xk為狀態(tài)向量；yk為量測向量；uk為輸入向量；wk為過程噪聲；vk為量測噪聲；fk，buk，bwk，ck為對應(yīng)的系數(shù)矩陣；下標(biāo)中k表示k時刻；

步驟42：對于形如上式所示的狀態(tài)空間模型，可按照如下方程完成一個步進(jìn)循環(huán)的卡爾曼濾波估計：

式中，wk和vk分別是過程噪聲和量測噪聲的協(xié)方差矩陣，和pt分別是t時刻的驗前均值和方程，和pt′分別是t時刻的驗后均值和方程，kt是t時刻的增益，和pt+1分別是t+1時刻的驗前均值和方程。

綜上所述，本發(fā)明提供的估計方法首先根據(jù)牛頓歐拉方法建立scara機器人的動力學(xué)模型，獲得一組關(guān)節(jié)力矩的連續(xù)非線性表達(dá)式；然后利用多元函數(shù)的一階泰勒展開，將關(guān)節(jié)力矩表達(dá)式離散線性化；接著將關(guān)節(jié)力矩的離散線性表達(dá)式作為過程方程，并添加相應(yīng)的量測方程，每個關(guān)節(jié)對應(yīng)的兩個方程組合并整理后可得到該關(guān)節(jié)力矩的離散時間狀態(tài)空間模型；最后結(jié)合狀態(tài)空間模型和卡爾曼濾波方法，利用每個控制周期讀取到的電機電流信號和電機編碼器信號，可以實時地對關(guān)節(jié)力矩進(jìn)行估計，且實時性和估計精度都能滿足機器人實際控制的要求。