基于卡爾曼濾波算法的自平衡機器人檢測分析

易強 宋子瑜

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

摘? 要：對立方體機器人姿態的檢測也是立方體機器人能夠實現單點平衡的重要一環。姿態檢測反映了立方體機器人平衡時的姿態角，當立方體機器人姿態角發生改變時，自平衡控制系統接收到姿態角的改變而做出反應，調整動量輪轉速轉向來保證立方體機器人平衡。將傳感器安裝在立方體方塊上時，隨著立方體方塊姿態的變化可獲得穩定的翻滾角和俯仰角，基本實現立方體機器人的姿態角測量。

關鍵詞：姿態檢測? 機器人? 姿態傳感器? 姿態角? 卡爾曼濾波器

中圖分類號：TP242.2? ? ? ? ? ? ? ? ? ? ? ? 文獻標識碼：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

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

自平衡機器人開發的難點在于自身姿態的平衡保持，姿態平衡最重要的就是要確定立方體機器人當前的姿態角，只有正確的姿態角才能保證立方體機器人對動量輪的控制能夠達到立方體機器人的單點平衡。若借助固定的檢測系統（攝像機）來對立方體進行檢測，會由于立方體機器人的不斷運動，造成數據的不準確、丟失等問題[3-4]。

立方體的檢測系統應當屬于自身，這樣的檢測系統減少數據傳輸過程中的丟失問題，同時使立方體姿態角的實時性更好[5-6]。

1? 卡爾曼濾波器設計

卡爾曼濾波的信號模型可以分為狀態方程和測量方程兩個部分。卡爾曼濾波通過遞歸計算的方法用上一時刻的估計值和當前時刻的測量值來計算當前時刻的估計值。卡爾曼濾波器的離散系統信號模型的狀態方程可以表示為：

測量方程可表示為：

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

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

（1）預估過程

（2）校正過程

a）濾波增益計算

預估過程中通過k-1時刻的估計值Xk-1去預估k時刻的狀態值Xk-，同時由k-1時刻的協方差Pk-1和過程噪聲的協方差來預測此時k時刻的協方差Pk-。而校正過程則是先求出k時刻的卡爾曼增益Kk，然后通過公式（5）和（6）來校正k時刻的估計值Xk和協方差Pk。卡爾曼濾波算法的流程如圖1所示：

2? 線性模型建立

首先，建立測量系統模型，即陀螺儀姿態角測量的線性模型。本設計立方體機器人繞著自身所在的坐標系旋轉的角速度作為陀螺儀的輸入量，立方體機器人的姿態角作為有效輸出。不考慮誤差，陀螺儀輸出的角速度與立方體的姿態角存在如下關系：

式中θ（k+1）為立方體在k+1時刻姿態角，θ（k）為立方體在k時刻的姿態角，ω（k）為立方體在k時刻繞自身坐標系旋轉時的角速度，△t為系統的采樣周期，v（k）為系統的過程噪聲。

事實上在計算物體姿態角的過程中應當考慮陀螺儀測量的誤差對測量結果的影響，因此最后的關系式應為：

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

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

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

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

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

令

可以得到姿態角測量系統的狀態方程為：

對于姿態角而言，加速度計的角度輸出值可以表示為：

因為陀螺儀的輸出值不會受到加速度計的檢測狀態的影響，所以加速度計的誤差輸出值為0。所以由檢測系統的狀態變量X可知，姿態角的輸出可以表示為：

令

則式（14）可以表示為：

式中C為系統的觀測矢量。

卡爾曼濾波器在式（13）和（16）的基礎進行設計，用加速度計輸出的翻滾角和俯仰角修正陀螺儀的姿態角測量誤差，從而提高姿態角測量的準確性。

3? 數據融合

姿態角測量的數據融合主要分為以下幾個步驟：

計算測量的偏差值

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

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

（2）計算卡爾曼增益K

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

由下式得出：

（3）修正姿態角的系統預測值

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

濾波協方差P（k+1）算法更新

在完成了k時刻的計算后應該更新系統的濾波協方差，這樣可以保證算法的遞歸計算、不斷循環，其計算公式為：

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

4? 結論

本文研究了單點自平衡立方體的姿態檢測，設計了立方體的姿態檢測系統對姿態角進行檢測，為立方體單點平衡的研究打下基礎，選擇姿態角數據融合的方法，選用卡爾曼濾波器進行數據融合，通過卡爾曼濾波器利用加速度計的姿態角對陀螺儀的姿態角進行修正。

參考文獻

[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.