基于數(shù)據(jù)融合算法的四輪機(jī)器人姿態(tài)估計(jì)

陳 佳，游曉明

(上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620)

基于數(shù)據(jù)融合算法的四輪機(jī)器人姿態(tài)估計(jì)

陳 佳，游曉明

(上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620)

針對四輪機(jī)器人傳感器實(shí)際獲取數(shù)據(jù)誤差過大的問題，設(shè)計(jì)了一種數(shù)據(jù)融合算法來進(jìn)行四輪機(jī)器人姿態(tài)估計(jì)并進(jìn)行實(shí)現(xiàn)。文中設(shè)計(jì)了一種聯(lián)合卡爾曼濾波和一階互補(bǔ)濾波的數(shù)據(jù)融合模型，并將其應(yīng)用于機(jī)器人角度傳感器上來處理實(shí)際得到的數(shù)據(jù)，使其更加貼合理想值從而方便后續(xù)使用。在3種不同環(huán)境下進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明，這種數(shù)據(jù)融合方法能較準(zhǔn)確地進(jìn)行角度校正、平滑數(shù)據(jù)，且計(jì)算量小、效率較高，適用于實(shí)時(shí)數(shù)據(jù)的處理，具有實(shí)際應(yīng)用前景。

機(jī)器人；數(shù)據(jù)融合；卡爾曼濾波；一階互補(bǔ)濾波；角度校正

定位技術(shù)能夠獲取移動機(jī)器人在任何時(shí)刻的相對位置，是實(shí)現(xiàn)導(dǎo)航和保證機(jī)器人自主導(dǎo)航的關(guān)鍵[1]。感知環(huán)境是移動機(jī)器人定位的重要一環(huán)，對得到的粗糙數(shù)據(jù)進(jìn)行處理可以提高傳感器數(shù)據(jù)的準(zhǔn)備性。文獻(xiàn)[2]指出以數(shù)據(jù)融合方式處理數(shù)據(jù)，符合機(jī)器人的高效化與精準(zhǔn)化發(fā)展趨勢，文獻(xiàn)[3]指出這種方式在室內(nèi)定位具有優(yōu)勢。 針對于數(shù)據(jù)融合的研究已取得較大成就，現(xiàn)有的傳感器數(shù)據(jù)融合算法主要包括：加權(quán)平均法、卡爾曼濾波[4]、貝葉斯推理[5]、粒子濾波[6]、模糊邏輯[7]、神經(jīng)網(wǎng)絡(luò)[8]等算法?？紤]到移動機(jī)器人動態(tài)收集數(shù)據(jù)的效率要求，卡爾曼濾波與一階互補(bǔ)濾波被選用建立數(shù)據(jù)融合模型?？柭鼮V波適用于動態(tài)環(huán)境中的傳感器信息實(shí)時(shí)融合，具有依賴于前一狀態(tài)的遞推特性，這種遞推特性加快了其運(yùn)算速度，可用于非線性噪聲情況下[9]。文獻(xiàn)[10]指出卡爾曼濾波具有自適性，可以適應(yīng)動態(tài)環(huán)境。一階互補(bǔ)濾波的結(jié)構(gòu)非常簡單，不需要繁雜的計(jì)算就能得出相對比較好的融合效果。

由此可見，針對機(jī)器人行動狀態(tài)的動態(tài)變化，數(shù)據(jù)融合模型的子項(xiàng)選用卡爾曼濾波和一階互補(bǔ)濾波具有實(shí)際應(yīng)用意義。

1 相關(guān)工作

1.1 卡爾曼濾波算法

卡爾曼濾波可用于解決在最優(yōu)估計(jì)、預(yù)測、噪聲濾除和最優(yōu)操作等方面的問題[11]。而卡爾曼濾波器是一種由數(shù)字計(jì)算機(jī)實(shí)現(xiàn)的遞推算法，每個(gè)遞推周期中包含被估計(jì)的量的時(shí)間更新周期和量測周期兩個(gè)過程。時(shí)間更新由上一步的量測更新結(jié)果和設(shè)計(jì)卡爾曼濾波器時(shí)的先驗(yàn)信息確定。因此，量測更新可看作是卡爾曼濾波的輸入，估計(jì)值可看作是輸出[12]。輸入輸出之間由時(shí)間更新和量測更新算法聯(lián)系。

進(jìn)行卡爾曼濾波首先需要構(gòu)造考慮噪聲的數(shù)學(xué)模型，當(dāng)噪聲是高斯白噪聲時(shí)，卡爾曼濾波可以給出最佳估計(jì)值[13]。然后再通過量測信息與前一步估計(jì)信息的加權(quán)平均得到現(xiàn)在的最優(yōu)估計(jì)。其中權(quán)值等參數(shù)根據(jù)過程的進(jìn)展而改變，其同樣是個(gè)動態(tài)的過程

X[K]=X[K-1]+K[K]×(Z-X[K-1])

(1)

式(1)是卡爾曼濾波算法的核心，體現(xiàn)卡爾曼濾波前后狀態(tài)的依賴性及加權(quán)平均的思想。算法通過結(jié)合當(dāng)前的測量值對上一時(shí)刻預(yù)測的最優(yōu)估計(jì)再次進(jìn)行校準(zhǔn)，從而得到更接近實(shí)際的更優(yōu)化數(shù)據(jù)。

經(jīng)過卡爾曼濾波，傳感器所獲取的受外界噪聲、震動干擾的粗糙數(shù)據(jù)得以處理，趨于平穩(wěn)，效果得以改善。

1.2 一階互補(bǔ)濾波算法

一階互補(bǔ)濾波是加權(quán)平均法的一種簡單形式，通過預(yù)先設(shè)置權(quán)值來對不同傳感器信息進(jìn)行加權(quán)平均

angle=K×angle_h+(1-K)×angle_i

(2)

式(2)是一階互補(bǔ)濾波公式，電子羅盤取值angle_h和陀螺儀積分所得角度angle_i，借由權(quán)值K進(jìn)行融合后的角度可得到融合角度angle。通過改變K的大小，可以對陀螺儀和電子羅盤在角度獲取中所占的比重進(jìn)行調(diào)節(jié)。

一階互補(bǔ)濾波運(yùn)算所需時(shí)間較短，適用于需要在行走過程中不斷進(jìn)行角度修正的移動機(jī)器人平臺。

2 姿態(tài)估計(jì)模型及參數(shù)設(shè)計(jì)

2.1 數(shù)據(jù)融合模型

針對于多傳感器的數(shù)據(jù)融合模型搭建有很多種分類方式，從多傳感器系統(tǒng)的信息流通形式和綜合處理層次角度進(jìn)行分類，可分為：集中式、分布式、混合式和多級式4種[14]。

文獻(xiàn)[15]指出，分布式數(shù)據(jù)融合模型有著計(jì)算簡單，插口可添加，各部分可單獨(dú)處理等優(yōu)點(diǎn)，適用于多角度傳感器的四輪移動機(jī)器人。

圖1 分布式數(shù)據(jù)融合系統(tǒng)框圖

圖1是角度校正的數(shù)據(jù)融合模型。各傳感器的數(shù)據(jù)先進(jìn)行單獨(dú)處理，最后再進(jìn)行數(shù)據(jù)融合。通過處理，各個(gè)傳感器獲取的角度信息都得到了一定程度上的校準(zhǔn)，可信度得以提高，之后再進(jìn)行不帶反饋的數(shù)據(jù)融合，在得到相對準(zhǔn)確信息的同時(shí)，提高了運(yùn)行效率。

2.2 參數(shù)設(shè)計(jì)

結(jié)合卡爾曼濾波與一階互補(bǔ)濾波的數(shù)據(jù)融合模型需要對以下參數(shù)進(jìn)行設(shè)定：

(1)卡爾曼濾波參數(shù)設(shè)計(jì)?？柭鼮V波中需要事先設(shè)定的參數(shù)是測量方差R和過程方差Q。在實(shí)際測量時(shí)，可以發(fā)現(xiàn)測量值Z的浮動范圍約為±5，故測量方差R設(shè)為5。Q是過程方差，一般與系統(tǒng)初始狀態(tài)是無關(guān)的，它的值無法計(jì)算得到，但經(jīng)過仿真對比，可發(fā)現(xiàn)Q的值較小時(shí)，最終的濾波效果很好，但需要更多時(shí)間來完成濾波。相對的，當(dāng)Q比較大時(shí)，卡爾曼濾波的速度能顯著提高，但會出現(xiàn)比較大的誤差。通過實(shí)驗(yàn)仿真,Q值被確定為0.004 5；

(2)一階互補(bǔ)濾波參數(shù)設(shè)計(jì)。一階互補(bǔ)濾波形式簡單，使用方便，需要預(yù)先設(shè)定的參數(shù)是權(quán)值K。通過在不同環(huán)境下的實(shí)驗(yàn)可以發(fā)現(xiàn)機(jī)器人受磁場的影響很大，在多次實(shí)驗(yàn)后可以確定陀螺儀積分得到的角度所占權(quán)值比較大，為0.995；電子羅盤所占權(quán)值很小，為0.005。根據(jù)上述公式，可將權(quán)值K設(shè)為0.005。

3 實(shí)驗(yàn)結(jié)果

3.1 卡爾曼濾波結(jié)果

圖2是卡爾曼濾波修正的前后對比圖， 當(dāng)機(jī)器人運(yùn)動時(shí)，由于外界環(huán)境的干擾，陀螺儀直接獲取的數(shù)據(jù)會出現(xiàn)較大的振蕩。如圖可見在使用卡爾曼濾波后，則在短時(shí)間內(nèi)得到明顯修正，貼合實(shí)際值，效率較高。

圖2 陀螺儀卡爾曼濾波修正對比圖

3.2 不同環(huán)境下實(shí)驗(yàn)結(jié)果

為驗(yàn)證模型的通用性，分別在實(shí)驗(yàn)室內(nèi)、走廊、室外3個(gè)場景里進(jìn)行移動機(jī)器人自轉(zhuǎn)實(shí)驗(yàn)。

3.2.1 實(shí)驗(yàn)室內(nèi)

如圖3所示，隨著自轉(zhuǎn)的進(jìn)行，機(jī)器人的朝向角度會產(chǎn)生一定的角度漂移，但誤差被控制在15°以內(nèi)，較好地消除了實(shí)驗(yàn)室里儀器、地下電纜的干擾，可以得到較為精確的角度信息。

圖3 實(shí)驗(yàn)室內(nèi)角度修正圖

3.2.2 走廊

走廊里存在的干擾比較少，從圖4中可以看出測量值的變化趨勢貼合實(shí)際值，基本呈一條直線。在長時(shí)間運(yùn)轉(zhuǎn)后出現(xiàn)的角度偏差在容許范圍內(nèi)，可以準(zhǔn)確體現(xiàn)機(jī)器人的朝向方位。

圖4 走廊角度修正圖

3.2.3 室外

在室外進(jìn)行實(shí)驗(yàn)的干擾最少，如圖5所示，測量值與實(shí)際值基本重合，可見經(jīng)由數(shù)據(jù)融合后獲得的角度值與實(shí)際角度值趨于一致，效果較好。

圖5 室外角度修正圖

4 結(jié)束語

為改善傳感器實(shí)際獲得的粗糙的數(shù)據(jù)，本文設(shè)計(jì)了一種結(jié)合卡爾曼濾波與一階互補(bǔ)濾波的數(shù)據(jù)融合模型，并通過自行編程對四輪機(jī)器人的實(shí)時(shí)姿態(tài)信息進(jìn)行軟件上的二次處理。

實(shí)驗(yàn)證明這種數(shù)據(jù)融合方法能一定程度上對數(shù)據(jù)進(jìn)行校正，使其貼近理想值。且計(jì)算量小、效率較高，

適用于實(shí)時(shí)數(shù)據(jù)的處理。為機(jī)器人的后續(xù)功能的實(shí)現(xiàn)提供了相對理想的前提條件。參數(shù)的設(shè)置是數(shù)據(jù)融合模型的一個(gè)值得探究的方向。本次的數(shù)據(jù)融合模型參數(shù)采用的是預(yù)設(shè)數(shù)值，在后續(xù)研究中將繼續(xù)以參數(shù)設(shè)置作為重點(diǎn)，嘗試用神經(jīng)網(wǎng)絡(luò)等算法訓(xùn)練參數(shù)或是設(shè)計(jì)自適應(yīng)參數(shù)調(diào)整模型來改良現(xiàn)在的數(shù)據(jù)融合模型。

[1] 張燕,夏慶鋒,謝鵬飛.基于多傳感器的姿態(tài)測量系統(tǒng)設(shè)計(jì)[J].機(jī)器人技術(shù)與應(yīng)用,2013(3): 30-33.

[2] 惠黎明.基于多傳感器探測精度的正確融合航跡判斷[J].電子科技,2014,27(6):117-120.

[3] 夏俊,俞暉,羅漢文.基于多傳感器數(shù)據(jù)融合的室內(nèi)定位算法[J].上海師范大學(xué)學(xué)報(bào):自然科學(xué)版,2015,44(1):65-72.

[4] 王錦章,孫菲,劉先康,等.基于卡爾曼濾波的航跡信息預(yù)處理技術(shù)[J].電子科技,2013,26(8): 22-25.

[5] 張燕,朱明敏,宋蘇鳴.基于混合方式的貝葉斯網(wǎng)絡(luò)結(jié)構(gòu)學(xué)習(xí)[J].電子科技,2014,27(10):115-118.

[6] 李家森,席志紅,趙彥青.北斗/INS組合導(dǎo)航中人工魚群粒子濾波的應(yīng)用[J].電子科技,2013,26(6):157-161.

[7] Deng Wenqian,Li Xuemei,Gao Xifeng,et al.A modified fuzzy C-means algorithm for brain MR image segmentation and bias field correction[J].計(jì)算機(jī)科學(xué)技術(shù)學(xué)報(bào):英文版,2016(3):501-511.

[8] 趙小川.傳感器信息融合—Matlab程序?qū)崿F(xiàn)[M].北京:機(jī)械工業(yè)出版社,2014.

[9] Duan Jianmin,Shi Hui,Liu Dan,et al.Square root cubature Kalman filter-Kalman filter algorithm for intelligent vehicle position estimate[J].Procedia Engineering,2016(137): 267-276.

[10] 劉延飛,郭鎖利,侯榮昌.卡爾曼濾波在GPS定位誤差處理中的應(yīng)用[J].電子科技,2011,24(9):140-148.

[11] Zolghadr J, Cai Y. Locating a two-wheeled robot using extended kalman filter[J]. Tehnicki Vjesnik-Technical Gazette, 2015, 22(6):1481-1488.

[12] 陳鋒.卡爾曼濾波與卡爾曼預(yù)測方法的改進(jìn)及其在結(jié)構(gòu)損傷識別中的應(yīng)用[D].廣州:廈門大學(xué),2014.

[13] 王襄.基于卡爾曼濾波融合的移動機(jī)器人定位算法[J].電子測試,2016(3):39-40.

[14] 卞鴻巍,李安,覃方君,等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M].北京:國防工業(yè)出版社,2010.

[15] 魏鵬,路贊贊.無線傳感器網(wǎng)絡(luò)分布式多傳感器目標(biāo)檢測[J].電子科技,2014,27(3):143-146.

Design and Implementation of Four-wheels-robot Attitude Estimation Based on Data Fusion Algorithm

CHEN Jia,YOU Xiaoming

(School of Electronic and Electrical Engineering,Shanghai University of Engineering Science,Shanghai 201620,China)

To solve the problem about the error of sensors for four wheeled robot, a kind of data fusion algorithm is designed to estimate the posture of the robot. First of all, the Calman filter and the first-order complementary filter are used together as a type of data fusion model. Then the model is used to deal with the actual data, making it more ideal. And later function can be realized through it. Secondly, the data fusion model is proposed. Finally, the experiment is carried out under three kinds of conditions. The experimental results show that this data fusion method can correct the angle more accurately, smooth the data. It just needs small amount of calculation with higher efficiency. It is suitable for real-time data processing with a great prospect of practical application.

robot; data fusion; Calman filter; first-order complementary filter; angle correction

2017- 01- 03

國家自然科學(xué)基金(61673258)

陳佳(1993-)，女，碩士研究生。研究方向：嵌入式控制系統(tǒng)與智能算法等。游曉明(1963-)，女，博士，教授。研究方向：嵌入式系統(tǒng)等。

10.16180/j.cnki.issn1007-7820.2017.12.013

TP274.2

A

1007-7820(2017)12-048-03