基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法_圖文

1、第卷第期年月傳感技術(shù)學(xué)報(bào)！，（口。，。）：，：；：；：￥基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法葉锃鋒馮恩信（西安交通大學(xué)電子與信息工程學(xué)院，西安）摘要：針對自平衡兩輪車姿態(tài)角的在線估計(jì)問題，采用四元數(shù)的姿態(tài)解算算法。利用非線性最小二乘法對自平衡兩輪車陀螺儀信號擬合，建立了隨機(jī)漂移誤差數(shù)學(xué)模型，應(yīng)用卡爾曼濾波融合陀螺儀和加速度計(jì)輸出的信號，補(bǔ)償了陀螺儀輸出角速度的隨機(jī)漂移誤差得到了自平衡兩輪車姿態(tài)的最優(yōu)估計(jì)。實(shí)驗(yàn)結(jié)果表明，這種姿態(tài)估計(jì)算法是有效的，有利于車體的自平衡控制。關(guān)鍵詞：傳感器；姿態(tài)估計(jì)；四元數(shù)；卡爾曼濾波；隨機(jī)漂移中圖分類號：文獻(xiàn)標(biāo)識碼：文章編號：（）自平衡兩輪車是一種具有自平衡能

2、力的新式交通工具。自平衡兩輪車通過對車體姿態(tài)的實(shí)時(shí)檢測，輸出控制量來驅(qū)動(dòng)電機(jī)帶動(dòng)車體前進(jìn)或后退以保持平衡行駛。因此一種可靠的車體姿態(tài)估計(jì)算法就顯得尤為重要，它直接影響著車體的自平衡性能。對于姿態(tài)估計(jì)算法的研究已有很多【。姿態(tài)估計(jì)算法必須要考慮姿態(tài)檢測傳感器本身的測量誤差，它會對姿態(tài)估計(jì)的準(zhǔn)確性產(chǎn)生很大的影響。中國科學(xué)院沈陽自動(dòng)化研究所的張思奇，趙憶文提出了應(yīng)用于移動(dòng)機(jī)器人的四元數(shù)姿態(tài)估計(jì)方法，但他們沒有考慮傳感器的測量誤差對姿態(tài)估計(jì)的影響【】。和等討論了姿態(tài)解算過程中誤差的來源。但并沒給出誤差數(shù)學(xué)模型和校正誤差的方法。和給收稿日期：一一修改日期：一出了陀螺儀的數(shù)學(xué)模型但并沒有給出一種簡單有效的

3、校正陀螺儀隨機(jī)偏移誤差的方法。本文采用四元數(shù)法作為自平衡兩輪車的姿態(tài)估計(jì)算法。同時(shí)，提出了卡爾曼濾波的方法來融合陀螺儀和加速度計(jì)輸出的信號，修正了陀螺儀輸出量中的隨機(jī)漂移分量實(shí)現(xiàn)了自平衡兩輪車姿態(tài)的最優(yōu)估計(jì)。四元數(shù)姿態(tài)解算算法自平衡兩輪車系統(tǒng)結(jié)構(gòu)自平衡兩輪車的系統(tǒng)框圖如圖所示。微處理器通過不斷檢測車體所處的姿態(tài)采用相應(yīng)的控制指令驅(qū)動(dòng)電機(jī)前進(jìn)或后退從而保證車體平衡。所以，準(zhǔn)確的姿態(tài)估計(jì)是車體自平衡實(shí)現(xiàn)的前提。第期葉锃鋒馮恩信：基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法匝亟圃臥圣壓至卜徽處理器卜回囤自平衡兩輪車系統(tǒng)框圖坐標(biāo)系變換關(guān)系以自平衡兩輪車為研究對象建立如網(wǎng)所示的坐標(biāo)系以表征兩輪車運(yùn)動(dòng)前后的

4、姿態(tài)變化?？蓪⒋诉^程分解為以下的三次坐標(biāo)系的變換。圖自平衡兩輪車坐標(biāo)系變換關(guān)東首先，坐標(biāo)系。一。以角速度妒繞。軸轉(zhuǎn)動(dòng)，旋轉(zhuǎn)角度為妒；其次，新坐標(biāo)系。一凰以角速度旁繞舶轉(zhuǎn)動(dòng)旋轉(zhuǎn)角度為口；最后，上述兩次旋轉(zhuǎn)后的坐標(biāo)系。一。，。以角速度，繞。軸轉(zhuǎn)動(dòng)，得到最終的坐標(biāo)系。一也邑，旋轉(zhuǎn)角度為。其中，驢稱為偏航角，稱為俯仰角，稱為橫滾角。它們統(tǒng)一被稱為歐拉角。自平衡兩輪車姿態(tài)檢測系統(tǒng)綜合考慮自平衡兩輪車對性能的要求和成本選擇了微機(jī)械雙功能傳感器來組成自平衡兩輪車的姿態(tài)檢測系統(tǒng)。、，、。圖慣性傳感囂在單個(gè)產(chǎn)品封裝內(nèi)集成了陀螺儀和加速度計(jì)。它能通過總線以數(shù)字量的形式輸出加速度信號和角速度信號。芯片如圖所示當(dāng)它以

5、上圖的形式放置時(shí)能同時(shí)測量沿軸的加速度和繞軸旋轉(zhuǎn)的角速度。對于自平衡兩輪車我們不需獲知偏航角。所以本文采用了兩個(gè)芯片正交擺放的方式來獲知車體的俯仰角和橫滾角。四元數(shù)姿態(tài)解算算法四元數(shù)是由愛爾蘭數(shù)學(xué)家哈密頓首先提出來的數(shù)學(xué)概念。它由一個(gè)實(shí)數(shù)單位和個(gè)虛數(shù)單位，；組成。其形式為：坤：印印，。四元數(shù)可以表示旋轉(zhuǎn)，它的標(biāo)量部分表示旋轉(zhuǎn)角度的大小，矢量部分表示轉(zhuǎn)軸的方向】。應(yīng)用四元數(shù)時(shí)要進(jìn)行歸一化。即使它的模值為。四元數(shù)和歐拉角的轉(zhuǎn)換關(guān)系為：吼嘰嘀（詈）一（詈）一（詈）油（詈）血（導(dǎo)）（號）（號）一（詈）一（詈）一（詈）（導(dǎo)）翻（詈）一（詈）（詈）一（等）（詈）一（導(dǎo)）出（詈）一（詈）一（詈）幽（詈）每（

6、號）幽（號）一（詈）四元數(shù)微分方程為”：知（）四元數(shù)微分方程可以實(shí)現(xiàn)四元數(shù)的更新其中，甜是陀螺儀測得的角速度。四階龍格一庫塔法可以用來求解四元數(shù)微分方程具體過程如下“：設(shè)÷（）（）?。ǎ┑取埃ㄊ郑┈敹。ǎǘ。┣?#247;（）（）則（）（）吉（）（）上式中，為更新周期，（）為初始四元數(shù)，（）為更新后的四元數(shù)。是時(shí)間段開始時(shí)的斜率，如是時(shí)間段中點(diǎn)的斜率，也是中點(diǎn)的斜率，托是時(shí)間段終點(diǎn)的斜率。姿態(tài)矩陣可以用四元數(shù)描述為”】：（印叩；叩；）（，）（枷）（：一，）（??；叩；）（“）（：）（。，一）（印；叩；呻；）從姿態(tài)矩陣中可以得到姿態(tài)角歸俯仰角：日一（）傳感技術(shù)學(xué)報(bào)第卷橫滾角：（）得到了

7、姿態(tài)角之后就實(shí)現(xiàn)了自平衡兩輪車的姿態(tài)估計(jì)。四元數(shù)法實(shí)驗(yàn)結(jié)果圖為自平衡兩輪車四元數(shù)法姿態(tài)解算的流程圖。在初始情況下自平衡兩輪車靜止在水平面上，此時(shí)歐托角都為。根據(jù)四元數(shù)和歐拉角的關(guān)系確定初始四元數(shù)。用四階龍格一庫塔法更新四元數(shù)和姿態(tài)矩陣獲取車體實(shí)時(shí)的姿態(tài)信息。圖四元數(shù)法姿態(tài)解算流程圖圖（）和圖（）分別是自平衡兩輪車在靜止情況下，四元數(shù)法得到的俯仰角和橫滾角。俯仰角和橫滾角的理想輸出值應(yīng)該都是。不過從圖中可以看出俯仰角和橫滾角都一直在累積。俯仰角從累積到左右，橫滾角則從累積到將近。這是由陀螺儀的隨機(jī)漂移誤差引起時(shí)間（）橫滾角圖四元數(shù)法解算后的姿態(tài)角的。要實(shí)現(xiàn)可靠的姿態(tài)估計(jì)就必須要校正這一誤差。數(shù)據(jù)

8、融合算法前面工作指出了陀螺儀輸量中存在隨機(jī)漂移分量，本節(jié)提出了卡爾曼濾波的數(shù)據(jù)融合算法來解決這一問題。卡爾曼濾波是一種最優(yōu)遞歸數(shù)據(jù)處理算法。年在他的論文中首次提出卡爾曼濾波。卡爾曼濾波可以根據(jù)陀螺儀和加速度計(jì)不同的數(shù)學(xué)模型通過建立方程關(guān)系來消除測量誤差。陀螺儀可用于測量角速度動(dòng)態(tài)跟蹤性能好。不過陀螺儀受溫度不穩(wěn)定力矩等因素的影響，會產(chǎn)生隨機(jī)漂移誤差。加速度計(jì)用于測量加速度，通過測得的加速度值與重力加速度的比值得到傾角值。加速度計(jì)靜態(tài)性能好但動(dòng)態(tài)跟蹤性能差。在進(jìn)行數(shù)據(jù)融合之前，必須明確陀螺儀輸出數(shù)據(jù)的數(shù)學(xué)模型。陀螺儀模型陀螺儀的測量模型可以表示為“：甜。（）（）占（）口日。（）（）式中，。（）是

9、陀螺儀的輸出量，（）是角速度真值，口。（）是測量噪聲，占（）是隨機(jī)漂移誤差。（）可以表示為：占（）（）（）參數(shù)，、丁和，需要由陀螺儀輸出實(shí)際測量值擬合得到。寫成微分方程的形式：；（）：；一占（）（）。離散化后得到差分模型：個(gè)（川）南占（）贏（：）（式中。是采樣時(shí)間。陀螺儀隨機(jī)漂移誤差是非線性的，可以用非線性最小二乘法進(jìn)行數(shù)據(jù)擬合“。圖（）和圖（）是將陀螺儀靜止放置于水平試驗(yàn)臺上，采集其連續(xù)輸出的信號得到的結(jié)果。軸和軸陀螺儀的理想輸出值應(yīng)該都為。，而實(shí)際輸出角速度均值分別為。和而正是這一漂移造成了姿態(tài)估計(jì)中的角度累積的結(jié)果。通過法擬合后的殘差量是零均值的高斯白噪聲過程因此擬合模型很理想的，充分地

10、擬合了漂移誤差真實(shí)值。擬合的最佳參數(shù)值列于表。第期葉侄鋒，馮恩信：基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法音女萋）“）船；二。耋羞¨¨！【）、軸魁蛘螄¨似÷們”驀”！）、！【耋”一。¨）?。┮唬S：樣軾¨妙片月型，。一二。裊陀螺儀漂移模型參數(shù)卡爾曼濾波卡爾曼濾波用于數(shù)據(jù)融合時(shí)，可將姿態(tài)角和陀螺儀偏移量描述成由狀態(tài)方程和量測方程組成的二維系統(tǒng)如圖所示。目卡爾曼濾波結(jié)構(gòu)圖。為系統(tǒng)矩陣，口。為控制輸入矩陣，峨為量測矩陣，丘為卡爾曼增益?？柭鼮V波的遞推過程：口一一：墨研（。巧。冠）。（）坼毛（缸一鞏：）；（瓦。）：式中，是誤差協(xié)方差矩陣，

11、是過程噪聲協(xié)方差矩陣且是測量噪聲協(xié)方差矩陣。在本文所設(shè)計(jì)的卡爾曼濾波算法中，缸和都為姿態(tài)角和陀螺儀隨機(jī)漂移誤差組成的二維向量，缸為加速度計(jì)測量角度值。這里為了計(jì)算方便，在建立加速度計(jì)誤差模型時(shí)將加速度計(jì)的輸出量看成是由加速度輸出真值和白噪聲組成。未考慮加速度計(jì)輸出偏差和過載情況。結(jié)合陀螺儀隨機(jī)漂移誤差模型和加速度計(jì)誤差模型確定參數(shù)如下：（）（”，盱”“赤（，），屯赤卜礬：】，曰：，：卜：。其中是加速度計(jì)測量得到的角度值，和分別是加速度計(jì)和陀螺儀的協(xié)方差，不同的取值表示不同的信任程度。是加速度計(jì)的測量噪聲協(xié)方差。本文通過實(shí)驗(yàn)修正來取得最佳的口和值。圖是陀螺儀輸出角速度量在卡爾曼濾波前后的效果對比

12、圖?？梢园l(fā)現(xiàn)。濾波前，陀螺儀輸出角速度帶有高斯白噪聲并且有。的隨機(jī)漂移誤差經(jīng)過卡爾曼濾波后，角速度量為零均值的高斯白噪聲過程。實(shí)現(xiàn)了陀螺儀隨機(jī)泓侈¨：蹤和補(bǔ)償。孚霎”一岳¨）二¨陀掣輸】引互一毒”蛞¨】）、）時(shí)間（）卡爾曼濾波后的珀垃出圖卡爾曼濾波前后對比圖自平衡兩輪車實(shí)驗(yàn)結(jié)果分析圖為自平衡兩輪車的靜止姿態(tài)估計(jì)。觀察圖可知，俯仰角和橫滾角都在。附近，誤差控制在傳感技術(shù)學(xué)報(bào)第卷。之內(nèi)，對比圖中的幾十度的誤差累積，可以發(fā)現(xiàn)卡爾曼濾波有效地修正了陀螺儀的隨機(jī)漂移誤差，解決了角度誤差累積的問題準(zhǔn)確地獲取了車體姿態(tài)。（露硒翟盍搬時(shí)間，（俯仰角！幾幾（柚加速度計(jì)鍘得

13、的角度酬八九時(shí)間徹四元敵法得到的角度圖動(dòng)態(tài)姿態(tài)估計(jì)葉鋰鋒（一）。男，西安交通大學(xué)電于與信息工程學(xué)院微波與光通信研究行碩士研究生主要研究方向?yàn)榍度胧较到y(tǒng)開發(fā)，傳感器信息融合和無線臣信等；結(jié)論本文提出了四元數(shù)法的姿態(tài)解算算法。并針對陀螺儀的隨機(jī)漂移誤差，提出了卡爾曼濾波的數(shù)據(jù)融合算法的解決方案。本文所設(shè)計(jì)的四元數(shù)法計(jì)算量小，精度高?？柭鼮V波有效地修正了陀螺儀輸出量中的隨機(jī)漂移分量充分滿足了自平衡兩輪車實(shí)時(shí)姿態(tài)估計(jì)的要求。通過實(shí)際測試，本文所設(shè)計(jì)的姿態(tài)估計(jì)算法能夠很好地跟蹤車體的真實(shí)姿態(tài)為自平衡兩輪車的研究提供了一種新的有效的姿態(tài)估計(jì)方法。參考文獻(xiàn)秦勇，臧希舶王曉宇纂于慣性傳感器的機(jī)器人姿態(tài)檢測系統(tǒng)的研究】傳感技術(shù)學(xué)報(bào)（）：吳黎明，張力鍇，李怡凡基于和單個(gè)三軸加速度傳感器的汽車運(yùn)動(dòng)姿態(tài)測皿】傳感技術(shù)學(xué)撤，（）：】張思奇趙憶文便攜式移動(dòng)機(jī)器人姿態(tài)解算方法做計(jì)算機(jī)信息（）：】一】。帥口】，】，：，冊帥卸，（）：張?zhí)旃?，千秀萍，王麗霞，等譯捷聯(lián)慣性導(dǎo)航技術(shù)（第二版）北京