基于卡爾曼濾波算法的自平衡機(jī)器人檢測(cè)分析

易強(qiáng) 宋子瑜

DOI：10.16660/j.cnki.1674-098x.2101-5640-2708

摘? 要：對(duì)立方體機(jī)器人姿態(tài)的檢測(cè)也是立方體機(jī)器人能夠?qū)崿F(xiàn)單點(diǎn)平衡的重要一環(huán)。姿態(tài)檢測(cè)反映了立方體機(jī)器人平衡時(shí)的姿態(tài)角，當(dāng)立方體機(jī)器人姿態(tài)角發(fā)生改變時(shí)，自平衡控制系統(tǒng)接收到姿態(tài)角的改變而做出反應(yīng)，調(diào)整動(dòng)量輪轉(zhuǎn)速轉(zhuǎn)向來(lái)保證立方體機(jī)器人平衡。將傳感器安裝在立方體方塊上時(shí)，隨著立方體方塊姿態(tài)的變化可獲得穩(wěn)定的翻滾角和俯仰角，基本實(shí)現(xiàn)立方體機(jī)器人的姿態(tài)角測(cè)量。

關(guān)鍵詞：姿態(tài)檢測(cè)? 機(jī)器人? 姿態(tài)傳感器? 姿態(tài)角? 卡爾曼濾波器

中圖分類號(hào)：TP242.2? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A? ? ? ? ? ? ? ? ? 文章編號(hào)：1674-098X（2021）03（c）-0001-4

Detection and Analysis of Self Balancing Robot Based on Kalman Filter Algorithm

YI Qiang1? ?SONG Ziyu2

（1.Suzhou Ruisai Precision Tools Co.， Ltd， Suzhou， Jiangsu， 215104 China; 2. School of Automation， Beijing Institute of Technology， Beijing， 100102 China ）

Abstract： The attitude detection of the cube robot is also an important part of the single point balance of the cube robot. Attitude detection reflects the attitude angle of the cube robot when it is in balance. When the attitude angle of the cube robot changes， the self balancing control system responds to the change of the attitude angle and adjusts the rotation speed of momentum wheel to ensure the balance of the cube robot. When the sensor is installed on the cube， the stable roll angle and pitch angle can be obtained with the change of the cube attitude， and the attitude angle measurement of the cube robot is basically realized.

Key Words： Attitude detection; Robot; Attitude sensor; Attitude angle; Kalman filter

立方體自平衡機(jī)器人主要分為以棱邊平衡的機(jī)器人和以角平衡的機(jī)器人，以棱邊平衡的機(jī)器人只需要一個(gè)動(dòng)量輪即可進(jìn)行單邊平行，而以角為平衡點(diǎn)的機(jī)器人需要三個(gè)相互垂直的動(dòng)量輪[1]。因此以角為平衡的機(jī)器人應(yīng)用空間更大，但是難度更高[2]。

自平衡機(jī)器人開發(fā)的難點(diǎn)在于自身姿態(tài)的平衡保持，姿態(tài)平衡最重要的就是要確定立方體機(jī)器人當(dāng)前的姿態(tài)角，只有正確的姿態(tài)角才能保證立方體機(jī)器人對(duì)動(dòng)量輪的控制能夠達(dá)到立方體機(jī)器人的單點(diǎn)平衡。若借助固定的檢測(cè)系統(tǒng)（攝像機(jī)）來(lái)對(duì)立方體進(jìn)行檢測(cè)，會(huì)由于立方體機(jī)器人的不斷運(yùn)動(dòng)，造成數(shù)據(jù)的不準(zhǔn)確、丟失等問(wèn)題[3-4]。

立方體的檢測(cè)系統(tǒng)應(yīng)當(dāng)屬于自身，這樣的檢測(cè)系統(tǒng)減少數(shù)據(jù)傳輸過(guò)程中的丟失問(wèn)題，同時(shí)使立方體姿態(tài)角的實(shí)時(shí)性更好[5-6]。

1? 卡爾曼濾波器設(shè)計(jì)

卡爾曼濾波的信號(hào)模型可以分為狀態(tài)方程和測(cè)量方程兩個(gè)部分。卡爾曼濾波通過(guò)遞歸計(jì)算的方法用上一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的測(cè)量值來(lái)計(jì)算當(dāng)前時(shí)刻的估計(jì)值?？柭鼮V波器的離散系統(tǒng)信號(hào)模型的狀態(tài)方程可以表示為：

測(cè)量方程可表示為：

式中Xk表示k時(shí)刻的系統(tǒng)狀態(tài)量，Yk表示k時(shí)刻的觀測(cè)變量。wk表示系統(tǒng)的測(cè)量噪聲，vk表示系統(tǒng)的觀測(cè)噪聲，具有獨(dú)立正態(tài)分布，其符合期望值為0。uk-1是k-1時(shí)刻的可控向量，Ak、Bk、Ck是k時(shí)刻的增益矩陣并由系統(tǒng)決定。

在這個(gè)條件下卡爾曼濾波器通過(guò)測(cè)量方程（2），從第一個(gè)時(shí)刻觀察到第k個(gè)時(shí)刻，這k個(gè)觀察值分別為Y（1），Y（2），...，Y（n），第j時(shí)刻的狀態(tài)X（j）由這k個(gè)觀察值進(jìn)行估計(jì)，并結(jié)合線性代數(shù)與統(tǒng)計(jì)知識(shí)，得到卡爾曼濾波算法的5個(gè)核心公式。事實(shí)上，卡爾曼濾波算法分為兩個(gè)部分，第一個(gè)部分為預(yù)估，當(dāng)前時(shí)刻的估計(jì)值由上個(gè)時(shí)刻的估計(jì)值得出。第二部分為校正，用當(dāng)前時(shí)刻的觀察值校正預(yù)測(cè)值，以此提高估計(jì)值的精確性。

（1）預(yù)估過(guò)程

（2）校正過(guò)程

a）濾波增益計(jì)算

預(yù)估過(guò)程中通過(guò)k-1時(shí)刻的估計(jì)值Xk-1去預(yù)估k時(shí)刻的狀態(tài)值Xk-，同時(shí)由k-1時(shí)刻的協(xié)方差Pk-1和過(guò)程噪聲的協(xié)方差來(lái)預(yù)測(cè)此時(shí)k時(shí)刻的協(xié)方差Pk-。而校正過(guò)程則是先求出k時(shí)刻的卡爾曼增益Kk，然后通過(guò)公式（5）和（6）來(lái)校正k時(shí)刻的估計(jì)值Xk和協(xié)方差Pk。卡爾曼濾波算法的流程如圖1所示：

2? 線性模型建立

首先，建立測(cè)量系統(tǒng)模型，即陀螺儀姿態(tài)角測(cè)量的線性模型。本設(shè)計(jì)立方體機(jī)器人繞著自身所在的坐標(biāo)系旋轉(zhuǎn)的角速度作為陀螺儀的輸入量，立方體機(jī)器人的姿態(tài)角作為有效輸出。不考慮誤差，陀螺儀輸出的角速度與立方體的姿態(tài)角存在如下關(guān)系：

式中θ（k+1）為立方體在k+1時(shí)刻姿態(tài)角，θ（k）為立方體在k時(shí)刻的姿態(tài)角，ω（k）為立方體在k時(shí)刻繞自身坐標(biāo)系旋轉(zhuǎn)時(shí)的角速度，△t為系統(tǒng)的采樣周期，v（k）為系統(tǒng)的過(guò)程噪聲。

事實(shí)上在計(jì)算物體姿態(tài)角的過(guò)程中應(yīng)當(dāng)考慮陀螺儀測(cè)量的誤差對(duì)測(cè)量結(jié)果的影響，因此最后的關(guān)系式應(yīng)為：

式中err（k+1）是陀螺儀在k+1時(shí)刻的測(cè)量誤差。

實(shí)際上陀螺儀的測(cè)量誤差并不是一個(gè)常值，這是一種具有時(shí)變性質(zhì)的誤差。設(shè)定陀螺儀的測(cè)量誤差與陀螺儀所測(cè)的角速度以及上一時(shí)刻的誤差線性相關(guān)，即：

式中δ是陀螺儀測(cè)量誤差err（k）與輸入的ω（k）之間的一種線性關(guān)系。

接下來(lái)結(jié)合（9）和（10）可以得到陀螺儀測(cè)量的最終模型為：

接下來(lái)把立方體的姿態(tài)角θ（k）和陀螺儀的測(cè)量誤差err（k）作為系統(tǒng)狀態(tài)，把加速度ω（k）作為系統(tǒng)的輸入量，那么陀螺儀的線性測(cè)量模型可以變成表示如下的狀態(tài)方程：

令

可以得到姿態(tài)角測(cè)量系統(tǒng)的狀態(tài)方程為：

對(duì)于姿態(tài)角而言，加速度計(jì)的角度輸出值可以表示為：

因?yàn)橥勇輧x的輸出值不會(huì)受到加速度計(jì)的檢測(cè)狀態(tài)的影響，所以加速度計(jì)的誤差輸出值為0。所以由檢測(cè)系統(tǒng)的狀態(tài)變量X可知，姿態(tài)角的輸出可以表示為：

令

則式（14）可以表示為：

式中C為系統(tǒng)的觀測(cè)矢量。

卡爾曼濾波器在式（13）和（16）的基礎(chǔ)進(jìn)行設(shè)計(jì)，用加速度計(jì)輸出的翻滾角和俯仰角修正陀螺儀的姿態(tài)角測(cè)量誤差，從而提高姿態(tài)角測(cè)量的準(zhǔn)確性。

3? 數(shù)據(jù)融合

姿態(tài)角測(cè)量的數(shù)據(jù)融合主要分為以下幾個(gè)步驟：

計(jì)算測(cè)量的偏差值

計(jì)算陀螺儀所需的橫滾角φG、俯仰角θG的預(yù)估值與加速度計(jì)測(cè)量的橫滾角φA、俯仰角θA之間的偏差值。計(jì)算表達(dá)式如下：

式中errφ、errθ分別為橫滾角φ和俯仰角θ在k時(shí)刻的偏差，也就是校正值。

（2）計(jì)算卡爾曼增益K

其中，P（k-1）為k-1時(shí)刻的協(xié)方差，C（k）為系統(tǒng)輸出方程的觀察矢量，R（k）為測(cè)量噪聲協(xié)方差。

由下式得出：

（3）修正姿態(tài)角的系統(tǒng)預(yù)測(cè)值

通過(guò)式（16）可得出卡爾曼增益以，由式（17）（18）來(lái)修正系統(tǒng)姿態(tài)角的最終輸出值：

濾波協(xié)方差P（k+1）算法更新

在完成了k時(shí)刻的計(jì)算后應(yīng)該更新系統(tǒng)的濾波協(xié)方差，這樣可以保證算法的遞歸計(jì)算、不斷循環(huán)，其計(jì)算公式為：

定義一系列變量，陀螺儀的噪聲協(xié)方差Qangle為0.001，陀螺儀的漂移噪聲協(xié)方差Qgyro為0.003，加速度計(jì)的測(cè)量噪聲協(xié)方差Rangle為0.5，濾波器采樣周期△t為0.005。完成定義后根據(jù)五個(gè)公式在keil 5中建立響應(yīng)的模型，將加速度和陀螺儀所測(cè)量的姿態(tài)角輸入卡爾曼濾波器中進(jìn)行數(shù)據(jù)融合。計(jì)算出最優(yōu)角度、最優(yōu)飄零，更新角度測(cè)量模型模型。

4? 結(jié)論

本文研究了單點(diǎn)自平衡立方體的姿態(tài)檢測(cè)，設(shè)計(jì)了立方體的姿態(tài)檢測(cè)系統(tǒng)對(duì)姿態(tài)角進(jìn)行檢測(cè)，為立方體單點(diǎn)平衡的研究打下基礎(chǔ)，選擇姿態(tài)角數(shù)據(jù)融合的方法，選用卡爾曼濾波器進(jìn)行數(shù)據(jù)融合，通過(guò)卡爾曼濾波器利用加速度計(jì)的姿態(tài)角對(duì)陀螺儀的姿態(tài)角進(jìn)行修正。

參考文獻(xiàn)

[1] Marle C M. Symmetries of hamiltonian systems on symplectic and poisson manifolds[J]. Lecture Notes in Applied and Computational Mechanics， 2018， 50（13）： 2033-2049.

[2] Yin S Y， Ou Y P， Dai X， et al. An Adaboost based face detection system using parallel configurable architecture with optimized computation[J]. IEEE Systems Journal， 2017， 11（1）： 260-271.

[3] Zhang Z F， Sarlette A， Ling Z H. Integral control on Lie groups[J]. Systems and Control Letters， 2019， 80： 9-15.

[4] Hu C L， Gong L Y， Wang T J， et al. Effective human age estimation using a two-stage approach based on Lie Algebrized Gaussians feature[J]. Multimedia Tools and Applications， 2019， 74（11）： 4139-4159.

[5] Altuzarra O， Diez M， Corral J， et al. Kinematic analysis of a flexible tensegrity robot[J]. Mechanisms and Machine Science， 2017， 46： 457-464.

[6] YUANXF，XIANGYZ，WANGY，et al.Neural networks based PID control of bidirectional inductive power transfer system[J].Neural Processing Letters，2020，43（3）：837-847.