易強(qiáng) 宋子瑜

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

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

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

中圖分類號：TP242.2? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼：A? ? ? ? ? ? ? ? ? 文章編號：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ī)器人只需要一個動量輪即可進(jìn)行單邊平行，而以角為平衡點(diǎn)的機(jī)器人需要三個相互垂直的動量輪[1]。因此以角為平衡的機(jī)器人應(yīng)用空間更大，但是難度更高[2]。

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

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

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

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

測量方程可表示為：

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

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

（1）預(yù)估過程

（2）校正過程

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

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

2? 線性模型建立

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

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

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

式中err（k+1）是陀螺儀在k+1時刻的測量誤差。

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

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

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

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

令

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

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

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

令

則式（14）可以表示為：

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

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

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

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

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

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

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

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

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

由下式得出：

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

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

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

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

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

4? 結(jié)論

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