王中立,李麗宏
(太原理工大學(xué)信息工程學(xué)院,太原 030024)
隨著科技的進(jìn)步以及經(jīng)濟(jì)的發(fā)展,移動(dòng)機(jī)器人需要不僅能在結(jié)構(gòu)化環(huán)境下定點(diǎn)作業(yè),還能在非結(jié)構(gòu)化環(huán)境下非定點(diǎn)作業(yè),比如,深海探測(cè)、地質(zhì)災(zāi)害救援和軍事目標(biāo)追蹤等非結(jié)構(gòu)化環(huán)境,從而代替人類完成危險(xiǎn)任務(wù)以及到達(dá)人類不能到達(dá)的區(qū)域。普通的輪式移動(dòng)機(jī)器人很難適應(yīng)非結(jié)構(gòu)化環(huán)境,由于足式機(jī)器人僅需滿足一定的離散落足點(diǎn)即可步行移動(dòng),因此足式機(jī)器人能更好地適應(yīng)非結(jié)構(gòu)化環(huán)境。
腿部是足式機(jī)器人準(zhǔn)確行走以及適應(yīng)非結(jié)構(gòu)化環(huán)境的關(guān)鍵,由于足式機(jī)器人的姿態(tài)受到腿部狀態(tài)影響,因此腿部是決定機(jī)器人姿態(tài)的關(guān)鍵因素[1]。為了更好地獲取足端位置,本文利用高精度雙軸傳感器SCA1000測(cè)量腿部各個(gè)關(guān)節(jié)旋轉(zhuǎn)角度,實(shí)現(xiàn)對(duì)足式機(jī)器人足端精確定位。然而足式機(jī)器人工作環(huán)境多是非結(jié)構(gòu)的戶外環(huán)境,受各種因素影響,傾角傳感器測(cè)量誤差大,再加上足式機(jī)器人長(zhǎng)時(shí)間行走后,測(cè)量誤差累計(jì),導(dǎo)致足式機(jī)器人腿部?jī)A角傳感器測(cè)量不準(zhǔn)確,難以根據(jù)傾角傳感器信號(hào)利用運(yùn)動(dòng)學(xué)模型分析腿部狀態(tài)。所以,如何處理腿部?jī)A角傳感器信號(hào)是現(xiàn)如今足式機(jī)器人研究領(lǐng)域的熱點(diǎn)和難點(diǎn)。
現(xiàn)如今已有很多學(xué)者對(duì)提高傾角傳感器測(cè)量精度方面進(jìn)行了大量的研究,例如:朱月青等研究了溫度對(duì)傾角傳感器的影響,通過利用BP神經(jīng)網(wǎng)絡(luò)方法融合傾角傳感器輸出信號(hào),將傳感器的測(cè)量誤差控制在2%以內(nèi)[2];邵曉敏等研究了傾角傳感器輸入和輸出的關(guān)系,利用最小二乘法對(duì)傳感器進(jìn)行信號(hào)處理,將傾角傳感器輸出信號(hào)的誤差控制在1.7%以內(nèi)[3]等等。盡管以上研究在某種程度上提高了傾角傳感器的測(cè)量精度,但信號(hào)處理精度仍然偏低。為了進(jìn)一步提高測(cè)量精度,本文先利用卡爾曼濾波算法對(duì)傾角傳感器信號(hào)進(jìn)行濾波預(yù)處理,再采用徑向基函數(shù)RBF(Radial Basis Function)神經(jīng)網(wǎng)絡(luò)模型對(duì)濾波預(yù)處理的信號(hào)進(jìn)行溫度補(bǔ)償,而對(duì)于RBF神經(jīng)網(wǎng)絡(luò)隱含層基函數(shù)中心值的選取,則利用具有并行尋優(yōu)特征和自適應(yīng)揮發(fā)系數(shù)的蟻群聚類算法,更改信息量,使算法能夠搜索全局最優(yōu)解,提高了RBF網(wǎng)絡(luò)的訓(xùn)練速度以及魯棒性。
足式機(jī)器人腿部主要包括大腿、小腿和支撐腳盤,系統(tǒng)結(jié)構(gòu)如圖1所示。傾角傳感器安裝位置如圖所示,其主要測(cè)量大腿和小腿的旋轉(zhuǎn)角度值,通過得到的角度值再根據(jù)腿部運(yùn)動(dòng)學(xué)方程可以得到足端狀態(tài),也可以聯(lián)合多條腿的角度值來估計(jì)機(jī)器人位姿。足端估計(jì)原理如圖2所示。
圖1 足式機(jī)器人腿部結(jié)構(gòu)
圖2 定位足端原理框圖
SCA1000是采用三維MEMS(Micro Electro Mechanical System)技術(shù)開發(fā)的高精度雙軸傾角測(cè)量傳感器,能夠測(cè)量相對(duì)于水平面的傾斜角度以及俯仰角度,同時(shí)也能測(cè)量所處環(huán)境的溫度。內(nèi)部主要組成包括:硅敏感電容傳感器、存儲(chǔ)器、溫度傳感器、A/D轉(zhuǎn)換器、感應(yīng)元件、信號(hào)調(diào)理電路、濾波電路和SPI通信接口等[4]。該傳感器可以分別通過X通道、Y通道測(cè)量?jī)A斜加速度、俯仰加速度,測(cè)得的信號(hào)既可以是數(shù)字信號(hào)也可以是模擬信號(hào),最終通過SPI通信接口傳輸?shù)娇刂破?同時(shí)也可以輸出溫度信號(hào)。
傾角傳感器內(nèi)部具有對(duì)加速度敏感的質(zhì)量塊,懸臂彈簧把質(zhì)量塊固定在敏感器上,并且一對(duì)電極板置于質(zhì)量塊的上下方,等似于一個(gè)電容,該電容是一種極距變化電容。當(dāng)傾角傳感器發(fā)生傾斜時(shí),由于重力作用,傾角傳感器中質(zhì)量塊發(fā)生偏移,導(dǎo)致兩片電極板與質(zhì)量塊之間的距離發(fā)生相對(duì)變化,因此,電容發(fā)生變化,最終導(dǎo)致電壓變化。電壓值可以反映加速度值,再通過相應(yīng)的計(jì)算得到角度值。
傾角傳感器的角度輸出可以利用模擬量電壓值或者數(shù)字量進(jìn)行轉(zhuǎn)換。
電壓到角度轉(zhuǎn)換公式:
(1)
式中:Offset是在0°輸出的電壓值;Sen是靈敏度;Vout是芯片輸出的模擬量。
數(shù)字量到角度轉(zhuǎn)換公式:
(2)
式中:Dout是X或Y通道的數(shù)字輸出量;Dout@0°是數(shù)值抵消量(一般傾角為0°時(shí),輸出1 024數(shù)字量);Sen是芯片靈敏度。
SCA1000內(nèi)部溫度傳感器測(cè)量溫度值為8 bit數(shù)據(jù),既可以作為內(nèi)在補(bǔ)償,也可以作為外部補(bǔ)償,轉(zhuǎn)換公式:
(3)
式中:Counts是讀取的溫度量;T是轉(zhuǎn)換后的溫度量,單位是 ℃。
假設(shè)一個(gè)研究對(duì)象是隨機(jī)線性離散系統(tǒng),那么其卡爾曼濾波狀態(tài)方程表示為:
X(k)=S(k)X(k-1)+B(k)U(k)+W(k)
(4)
式中:X(k)是研究對(duì)象的狀態(tài)向量;S(k)是狀態(tài)轉(zhuǎn)移矩陣;B(k)是輸入控制矩陣;U(k)是控制向量;W(k)可以看作均值為0,協(xié)方差為σk的正態(tài)分布的過程噪聲。
觀測(cè)方程可表示為:
Z(k)=H(k)X(k)+V(k)
(5)
式中:Z(k)是研究對(duì)象的觀測(cè)向量;H(k)是觀測(cè)矩陣;V(k)為觀測(cè)噪聲矩陣。
卡爾曼濾波器的遞推過程主要分為2部分:預(yù)測(cè)和更新。在預(yù)測(cè)部分時(shí),卡爾曼濾波器利用上一狀態(tài)的估計(jì)值來估計(jì)當(dāng)前狀態(tài)的值;在更新部分時(shí),卡爾曼濾波器會(huì)利用當(dāng)前狀態(tài)的觀測(cè)值來補(bǔ)償預(yù)測(cè)階段獲得的預(yù)測(cè)值,使新獲得的估計(jì)值更為精確[5-7]。
假設(shè)控制信號(hào)向量U(k)=0,則狀態(tài)預(yù)測(cè)方程為:
X(k|k-1)=S(k)X(k-1|k-1)
(6)
預(yù)測(cè)誤差協(xié)方差估計(jì)為:
(7)
式中:Q(k)是系統(tǒng)噪聲協(xié)方差陣。
更新方程為:
K(k)=P(k|k-1)HT(k)[H(k)P(k|k-1)HT(k)+R(k)]-1
(8)
式中:R(k)為測(cè)量噪聲協(xié)方差陣。
狀態(tài)估計(jì)為:
X(k|k)=X(k|k-1)+K(k)[Z(k)-H(k)X(k|k-1)]
(9)
最優(yōu)解在更新k狀態(tài)下所對(duì)應(yīng)的誤差協(xié)方差為:
P(k|k)=[1-K(k)H(k)]P(k|k-1)
(10)
由上式可知,除了給定濾波初值X(0|0)和P(0|0)以及k時(shí)刻的觀測(cè)值Z(k)之外,還需要根據(jù)實(shí)際應(yīng)用調(diào)整Q和R的取值,Q和R可以通過對(duì)象模型和采集離線數(shù)據(jù)獲得,最后通過遞推計(jì)算得出k時(shí)刻的狀態(tài)估計(jì)值X(k|k)和誤差協(xié)方差P(k|k)。
圖3 卡爾曼濾波前后傾角信號(hào)誤差對(duì)比圖
本文針對(duì)SCA1000高精度雙軸傾角傳感器輸出的傾角信號(hào)進(jìn)行卡爾曼濾波,在濾波時(shí),先將SCA1000傾角傳感器固定在三軸精密轉(zhuǎn)臺(tái)上,連續(xù)采集150組輸出信號(hào)和傾角傳感器輸出信號(hào),并計(jì)算傾角傳感器輸出信號(hào)的平均值和方差,分別作為濾波初值X(0|0)和P(0|0)。Q是濾波參數(shù)且反映的是對(duì)象模型噪聲方差,當(dāng)模型確定時(shí),可以把此參數(shù)定位極小值;R是測(cè)量方差,可以通過采集數(shù)據(jù)求得,本文為了得到良好的濾波效果,根據(jù)經(jīng)驗(yàn)確定Q的取值為1×10-6,R的取值為0.001 5。
本文利用MatLab 2014b仿真軟件進(jìn)行卡爾曼濾波仿真分析,利用軟件中設(shè)計(jì)的濾波器處理采集數(shù)據(jù),得到傾角誤差波形圖如圖3所示。
從圖3可知,經(jīng)過卡爾曼濾波后傾角信號(hào)中的隨機(jī)噪聲明顯減少,提高了傾角信號(hào)的穩(wěn)定性和精確度。為了進(jìn)一步提高傾角傳感器測(cè)量精度,再把濾波信號(hào)作為建立的RBF神經(jīng)網(wǎng)絡(luò)樣本數(shù)據(jù)。
RBF神經(jīng)網(wǎng)絡(luò)是包含輸入層、隱含層和輸出層的三層前饋單隱層網(wǎng)絡(luò),利用非線性基函數(shù)的線性組合來實(shí)現(xiàn)映射關(guān)系[8],其結(jié)構(gòu)如圖4所示。
圖4 RBF神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)
輸入層僅起到信號(hào)傳輸?shù)淖饔?輸入層和隱含層之間的連接權(quán)值為1;隱含層神經(jīng)元的激活函數(shù)為徑向基函數(shù),它是中心點(diǎn)徑向?qū)ΨQ且衰減的非負(fù)線性函數(shù),此函數(shù)局部響應(yīng)函數(shù)采用非線性優(yōu)化策略;輸出層是對(duì)輸入模式做出的響應(yīng),對(duì)線性權(quán)值進(jìn)行調(diào)整,采用線性化策略,學(xué)習(xí)速度較快[9-11]。本文中RBF神經(jīng)網(wǎng)絡(luò)隱含層節(jié)點(diǎn)的徑向基函數(shù)選用高斯函數(shù):
(11)
式中:σ為基函數(shù)的擴(kuò)展常數(shù)或?qū)挾取?/p>
設(shè)輸入樣本為X=(x1,x2,…,xn),單輸出為y,隱含層節(jié)點(diǎn)輸出為D=(d0,d2,…,dm),隱含層節(jié)點(diǎn)到輸出節(jié)點(diǎn)的連接權(quán)重為W=(w1,w2,…,wm),當(dāng)高斯函數(shù)作為隱含層節(jié)點(diǎn)的激勵(lì)函數(shù)時(shí),RBF神經(jīng)網(wǎng)絡(luò)的單輸出Y表示為:
(12)
式中:m是隱含層節(jié)點(diǎn)數(shù);wi是第i個(gè)隱含層節(jié)點(diǎn)與輸出層節(jié)點(diǎn)連接權(quán)值;‖·‖是歐幾里得范數(shù);Ci是第i個(gè)隱含層節(jié)點(diǎn)中心參數(shù)。
4.2.1 基于蟻群算法的RBF神經(jīng)網(wǎng)絡(luò)訓(xùn)練
在處理大樣本數(shù)據(jù)時(shí),一般聚類算法,如K均值聚類算法,存在聚類慢、易陷入局部最優(yōu)問題中,難以獲得最優(yōu)結(jié)果,因此,本文采用蟻群算法,蟻群算法是人們?cè)谟^察蟻群覓食行為時(shí)受到啟發(fā)而提出的。螞蟻在覓食過程中,會(huì)在經(jīng)過的地方留下一些物質(zhì),這些物質(zhì)就是信息素[12]。而路徑上信息素的多少,會(huì)決定同一群螞蟻行進(jìn)的方向,它們通過比較路徑上信息素濃度大小,選擇濃度大的方向前進(jìn),同時(shí),也會(huì)在該路徑上留下自己的信息素。相同時(shí)間內(nèi),路徑越短,則被越多的螞蟻訪問,所以,后續(xù)的螞蟻選擇該路徑的可能性越大,因此,該路徑就是到達(dá)目標(biāo)食物的最佳路徑[13-14]。利用蟻群算法進(jìn)行聚類分析時(shí),聚類中心為目標(biāo)食物源,把數(shù)據(jù)看作具有不同屬性的螞蟻,聚類過程就等同于螞蟻尋找食物。其聚類詳細(xì)過程[15]如下:
①基本參數(shù)初始化:樣本數(shù)量為N,樣本屬性為m,聚類半徑為r,誤差允許值為ξ0,參考概率為p。
②求出任意兩樣本間的歐式距離Lij:
Lij=‖xi-xj‖,i,j=1,2,…,N
(13)
③對(duì)路徑上的信息素τij進(jìn)行初始化可以依據(jù)如下公式:
(14)
④xi和xj之間的概率為:
(15)
如果值pij(t)不少于預(yù)先設(shè)定值p0,則把xi歸屬于xj類,否則為不同的兩個(gè)類。
若誤差ξ<ξ0,則停止,否則繼續(xù)。
⑥得到新的聚類中心與各樣本間的距離Lij,改變信息素的揮發(fā)系數(shù):
(16)
式中:ρ為揮發(fā)系數(shù),ρ的最小值為ρmin;t為時(shí)間,則t、t-1時(shí)刻的揮發(fā)系數(shù)分別為ρ(t)、ρ(t-1)。調(diào)整信息素:
式中,S為信息強(qiáng)度,是一個(gè)常數(shù)。
⑦重復(fù)進(jìn)行步驟④~步驟⑥直到滿足預(yù)設(shè)誤差為止,最后得到聚類中心以及聚類數(shù)。
4.2.2RBF神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)及樣本數(shù)據(jù)處理
通過蟻群算法得到的聚類中心作為隱含層單元中心值,然后通過最小二乘法來直接求出隱含層到輸出層的連接權(quán)值。
利用RBF神經(jīng)網(wǎng)絡(luò)處理傾角傳感器輸出信號(hào)時(shí),應(yīng)確定輸入層以及輸出層節(jié)點(diǎn)個(gè)數(shù)。由于傾角傳感器測(cè)量時(shí)易受溫度影響,因此,把傾角傳感器輸出的溫度信號(hào)和經(jīng)過濾波處理的傾角信號(hào)作為RBF網(wǎng)絡(luò)輸入量,確定輸入層節(jié)點(diǎn)個(gè)數(shù)為2個(gè)。由于需得到精確的傾角傳感器輸出信號(hào),所以把傾角信號(hào)作為輸出,即輸出層為單輸出。
為了控制RBF神經(jīng)網(wǎng)絡(luò)規(guī)模以及確保其訓(xùn)練的準(zhǔn)確性和不出現(xiàn)飽和狀況,對(duì)采集的溫度數(shù)據(jù)和濾波后的傾角信號(hào)進(jìn)行歸一化處理,使樣本數(shù)據(jù)落在區(qū)間[0,1]之內(nèi),歸一化公式如下:
Xi=(di-dimax)/(dimin-dimax)
(17)
式中:Xi是樣本數(shù)據(jù)的歸一化;di是樣本數(shù)據(jù)的原始數(shù)據(jù);dimin,dimax分別是數(shù)據(jù)的最小值、最大值。
把SCA1000傾角傳感器放在三軸精密轉(zhuǎn)臺(tái)上,通過改變外界溫度和轉(zhuǎn)臺(tái)角度,采集包括三軸精密轉(zhuǎn)臺(tái)的傾角信號(hào)以及傾角傳感器的溫度信號(hào)、傾角信號(hào),從采集數(shù)據(jù)中隨機(jī)選取50組數(shù)據(jù)。首先,對(duì)采集的傾角傳感器的傾角信號(hào)進(jìn)行卡爾曼濾波處理,然后再隨機(jī)選取30組溫度數(shù)據(jù)和濾波后的傾角傳感器輸出信號(hào)經(jīng)過歸一化后,用于RBF神經(jīng)網(wǎng)絡(luò)訓(xùn)練,其余20組數(shù)據(jù)用作測(cè)試樣本。
利用MATLAB 2014b中的神經(jīng)網(wǎng)絡(luò)工具箱來訓(xùn)練RBF神經(jīng)網(wǎng)絡(luò),根據(jù)經(jīng)驗(yàn)值以及補(bǔ)償精度要求,設(shè)置基于蟻群算法的RBF神經(jīng)網(wǎng)絡(luò)各參數(shù)如下:r=0.4,ξ0=0.003,ρmin=0.3,S=25,p0=0.5。建立一個(gè)隱含層單元中心點(diǎn)個(gè)數(shù)為6個(gè),目標(biāo)誤差為1×10-6的RBF神經(jīng)網(wǎng)絡(luò)。再利用其余20組數(shù)據(jù)測(cè)試訓(xùn)練好的RBF神經(jīng)網(wǎng)絡(luò)。經(jīng)過25次訓(xùn)練達(dá)到目標(biāo)訓(xùn)練精度,神經(jīng)網(wǎng)絡(luò)誤差收斂曲線如圖5所示。
圖5 RBF神經(jīng)網(wǎng)絡(luò)誤差收斂曲線
從50組數(shù)據(jù)中隨機(jī)選取10組數(shù)據(jù),然后對(duì)卡爾曼濾波后的數(shù)據(jù)與先通過卡爾曼濾波再進(jìn)行基于蟻群聚類算法的RBF神經(jīng)網(wǎng)絡(luò)溫度補(bǔ)償?shù)臄?shù)據(jù)誤差相比較,得到如表1所示的信號(hào)誤差比較結(jié)果。
表1 實(shí)驗(yàn)數(shù)據(jù)誤差比較
由表1得出,經(jīng)過卡爾曼濾波預(yù)處理后的傾角傳感器測(cè)量誤差較大,不能達(dá)到良好的信號(hào)處理效果,為了達(dá)到較高的測(cè)量精度,再把溫度信號(hào)和預(yù)處理的傾角信號(hào)作為基于蟻群聚類算法的RBF神經(jīng)網(wǎng)絡(luò)的輸入信號(hào),對(duì)傾角信號(hào)進(jìn)行溫度補(bǔ)償,能把傾角測(cè)量誤差控制在0.75%以內(nèi)。
足式機(jī)器人腿部?jī)A角傳感器易受外界環(huán)境的影響,特別是外界溫度的影響,造成傾角傳感器測(cè)量信號(hào)中含有較多噪聲信號(hào),測(cè)量誤差大,難以利用直接測(cè)量的傾角信號(hào)估計(jì)足端位置。為了更好地得到精確的傾角信號(hào),本文采集了傾角傳感器輸出的溫度信號(hào),把溫度信號(hào)和經(jīng)過卡爾曼濾波方法濾波后的信號(hào)作為建立的基于蟻群聚類算法的RBF神經(jīng)網(wǎng)絡(luò)模型的樣本數(shù)據(jù),對(duì)傳感器傾角信號(hào)進(jìn)行溫度補(bǔ)償。經(jīng)過實(shí)驗(yàn)驗(yàn)證,能夠很好地去除傾角傳感器信號(hào)中的噪聲,對(duì)傾角信號(hào)進(jìn)行溫度補(bǔ)償,使得測(cè)量誤差能夠控制在0.75%以內(nèi),為估計(jì)足端位置提供了高精度的角度值。
參考文獻(xiàn):
[1] 趙杰,張赫,劉玉斌,等. 六足機(jī)器人HITCR-I的研制及步行實(shí)驗(yàn)[J]. 華南理工大學(xué)學(xué)報(bào)(自然科學(xué)版),2012,40(12):17-23.
[2] 朱月青,張志利,譚立龍,等. BP神經(jīng)網(wǎng)絡(luò)在傾角傳感器數(shù)據(jù)融合中的應(yīng)用[J]. 儀表技術(shù)與傳感器,2010(1):101-102.
[3] 邵曉敏. 全量程無線傾角傳感器設(shè)計(jì)[D]. 上海:上海大學(xué),2012.
[4] 曹建安,張樂平,吳昊,等. 采用傾角傳感器實(shí)現(xiàn)空間旋轉(zhuǎn)角度測(cè)量的解析方法研究[J]. 西安交通大學(xué)學(xué)報(bào),2013,47(10):109-114.
[5] 袁健,周忠海,金光虎,等. 基于卡爾曼濾波的自主式水下航行器大尺度編隊(duì)控制[J]. 智能系統(tǒng)學(xué)報(bào),2013(4):344-348.
[6] 王小冬. 卡爾曼濾波在衛(wèi)星導(dǎo)航中的應(yīng)用研究[D]. 大連:大連海事大學(xué),2008.
[7] 羅貞. 基于卡爾曼濾波器的系統(tǒng)狀態(tài)估計(jì)和故障檢測(cè)[D]. 武漢:華中科技大學(xué),2013.
[8] 乃永強(qiáng),李軍. 基于極限學(xué)習(xí)機(jī)的機(jī)械臂自適應(yīng)神經(jīng)控制[J]. 信息與控制,2015,44(3):257-262.
[9] 喬俊飛,韓紅桂. RBF神經(jīng)網(wǎng)絡(luò)的結(jié)構(gòu)動(dòng)態(tài)優(yōu)化設(shè)計(jì)[J]. 自動(dòng)化學(xué)報(bào),2010,36(6):865-872.
[10] 張朋,范福玲,楊益,等. 基于時(shí)間序列模型的超聲信號(hào)濾波算法研究[J]. 傳感技術(shù)學(xué)報(bào),2015(3):396-400.
[11] 鄭立斌,王紅梅,顧寄南,等. RBF神經(jīng)網(wǎng)絡(luò)在機(jī)器人視覺伺服控制中的應(yīng)用[J]. 機(jī)床與液壓,2015,43(15):41-43.
[12] 孫艷梅,都文和,馮昌浩,等. 基于蟻群聚類算法的RBF神經(jīng)網(wǎng)絡(luò)在壓力傳感器中的應(yīng)用[J]. 傳感技術(shù)學(xué)報(bào),2013,26(6):806-809.
[13] 柏繼云. 蟻群優(yōu)化算法及覓食行為模型研究[D]. 哈爾濱:哈爾濱工業(yè)大學(xué),2013.
[14] 楊劍峰. 蟻群算法及其應(yīng)用研究[D]. 杭州:浙江大學(xué),2007.
[15] 黃延紅. 基于蟻群算法的聚類算法研究[D]. 成都:電子科技大學(xué),2011.