面向兩輪平衡機器人姿態檢測系統的研究與設計

(昆明理工大學 信息工程與自動化學院，云南 昆明 650500)

近些年來，在人工智能大火的背景下，機器人作為人工智能的一個研究領域也頻頻出現在人們的視線。機器人在一些危險、高難度的工業環境以及狹小空間的工業領域很快就得到了廣泛應用。而且人們日常生活中的輔助機器人的工作環境也逐漸涉及到不平坦路面、斜坡、樓梯等復雜環境。因此在不同環境下保證機器人能夠穩定地行走成為機器人完成任務的首要條件，在此背景下出現了兩輪自平衡機器人。

根據兩輪自平衡機器人姿態檢測系統中的加速度計和陀螺儀的互補特性，對采集到的數據進行濾波和數據融合研究，并根據兩者的互補特性進行了濾波算法設計。由于陀螺儀所測得角速度值只能短時間內處于穩定狀態，這是由其本身特性所導致的；而加速度傳感器測得的數據又存在很大的白噪聲，在進行數據處理時通常會使用加權平均法和神經網絡等方法。但是這些方法都存在一些缺陷，或是檢測精度達不到要求，亦或是其模型和涉及的參數優化太過復雜不能適應本系統。因此在本系統中采用卡爾曼濾波算法，不僅要對陀螺儀的輸出信號進行放大和濾波還要根據其互補特性設計卡爾曼濾波器。采用卡爾曼濾波器對兩者的數據進行數據融合可以得出穩定和準確的數據，采用PID算法對車身的傾斜角度和驅動電機進行閉環控制。

1 系統總體設計

系統采用MC9S12X128作為姿態檢測的主控制器，利用陀螺儀和加速度計對自平衡機器人的傾斜角度和角速度進行測量，包括陀螺儀的輸出信號處理減少直接使用其輸出力矩對檢測精度的影響，以及利用卡爾曼濾波器對陀螺儀和加速度傳感器的數據進行融合以得到更加準確穩定的角度和角速度值。然后通過PID控制算法對直流電機進行閉環控制。設計方案圖如圖1所示。

圖1 整體設計方案

2 自平衡機器人平衡模型的建立

2.1 自平衡機器人的運動分析

兩輪自平衡機器人通過車輪與地面接觸，車輪和地面發生相對位移而上半部分由于慣性保持原來位置不動，從而造成身體傾斜。圖2為通過輪子轉動保持平衡的示意圖。若保證機器人能夠快速恢復到平衡狀態，必須準確穩定地實時檢測車身的平衡狀態，將機器人身體傾斜產生的力矩通過控制車輪轉動抵消，從而保持機器人平衡。

圖2 輪子轉動保持小車平衡

為了使機器人一直處于直立平衡狀態，需要對模型運動學模型進行分析，從而建立可靠的數學模型。如圖3所示，對機器人的運動姿態進行運動學分析時可以將機器人系統簡化成一級倒立擺，而處于靜止時的倒立擺是倒立擺模型分析的基礎，因此應先對靜止狀態的一級倒立擺進行受力情況分析。

由一級倒立擺受力分析可以得出回復力的大小為

F=mgsinθ

(1)

在倒立擺的模型中由于其回復力的方向和偏離平衡狀態的方向是一致的，因此不能像單擺那樣回復到平衡的狀態[1]。在系統中增加一種可使得回復力的方向與倒立擺的偏離方向相反的力以滿足倒立擺可保持平衡的需要。

圖3 一級倒立擺模型受力分析圖

為得到一個與傾斜方向相反的合回復力，需要控制倒立擺下面的車輪做加速度運動。倒立擺在加速情況下的受力情況，如圖4所示。

圖4 非慣性系中的倒立擺受力分析

假定這種情況下的加速度為α。以地面作為參考系，根據牛頓力學可知倒立擺受到的慣性力為

Fg=macosθ

(2)

此情況下受到的合回復力為

F=mgsinθ-macosθ

(3)

在平衡機器人控制系統中，考慮到可控偏移角度θ值比較小的情況下可以對其進行線性化處理。當車身發生偏移時假設偏移角對機器人產生的加速度α與此時機器人姿態偏移角θ成正比，且二者之間的系數為k1，則式(3)可變換為

F=mgθ-mk1θ

(4)

當兩者之間的比例系數大于重力加速度時，就能使合回復與傾斜方向相反。倒立擺在這種情況下可以回到平衡狀態，但這個過程需要的時間比較長。因此需要在系統中增加阻尼力，阻尼力的大小應該與傾斜角度的大小成正比方向且與傾斜方向相反。因此可將式(4)變換為

F=mgθ-mk1θ-mk2θ′

(5)

根據上面的分析可以得出車輪加速度與傾斜角度之間的關系為

a=mk1θ+mk2θ′

(6)

式中，θ為傾角；θ′為傾角速度；k1、k2為比例系數。由式(6)可知，k1>g可以保證回復到平衡位置，而阻尼系數k2的大小則決定了倒立擺從傾斜狀態回復到穩定狀態的時間長短。

2.2 自平衡機器人的運動微分方程

在機器人高度l和質量m已知前提下，可以將自平衡機器人模型簡化為放置于車輪上的倒立擺。假定車身在外界的干擾下產生了角加速度為x(t)的傾斜，對平衡機器人進行受力分析，得到車體傾角與車輪加速度為a(t)以及外力產生的加速度x(t)之間的運動方程。受力分析如圖5所示。

圖5 外力干擾條件下的受力分析

小車運動微分方程表達式為

(7)

當θ很小的時候有：cos(θ)≈1、sin(θ)≈θ，運動微分方程可簡化為

(8)

自平衡車靜止時a(t)=0,其運動微分方程為

(9)

2.3 PID控制器設計

由靜止時運動方程(9)可以得出傳遞函數：

(10)

按照式(6)，角度θ和角速度θ′的反饋能夠得到小車需要加速度值，因此引入θ及θ′之間的PD反饋環節在控制系統中，可得到加入反饋后的傳遞函數(式(11))和控制系統結構圖，如圖6所示。

圖6 加入反饋后的控制系統結構圖

(11)

此時，系統的兩個極點為

(12)

k1>g,k2>0時，兩個極點都位于s平面的左半平面根據穩定判據系統穩定。因此，k1>g,k2>0時，小車可以保持平衡。

根據對控制系統的分析，系統中PID控制器的輸出方程可寫為

OUT_Motor=Kp·Angle+Kd·Angle_dot

(13)

式中，OUT_Motor為PID控制輸出量；Angle為反饋傾角值；Angle_dot為反饋角速度值；Kp和Kd分別為比例系數和微分系數。

3 姿態檢測系統

3.1 姿態檢測的方式

自平衡機器人本身是一個不穩定非線性系統，因此在自平衡機器人維持穩定狀態的過程中需要實時檢測車身姿態。過程中需要對得出的數據進行整合，然后通過控制系統及時調整車身姿態來保持平衡。因此在機器人姿態檢測時的精度和控制系統對車身姿態的調整速度對于系統的穩定運轉至關重要。

根據平衡機器人自身特點和元器件特性采用MEMS加速度計和陀螺儀構成姿態檢測系統[3]。雖然通過單獨使用其中一種元件或者同時使用兩種元件都可以測量出控制系統所需要的參數，但是由于器件本身的特性使得測出的數據中存在大量的噪聲等缺陷。因此并不能將其輸出的數據直接作為控制系統輸入使用[4]。

利用示波器測得的陀螺儀與加速度計輸出波形如圖7所示。

圖7 陀螺儀與加速度計輸出波形

3.2 卡爾曼濾波數據融合

在相對簡單的系統中對姿態的檢測完全可以通過單一的傳感器完成，但是精度受到器件本身特性的限制程度較大。單純從改善硬件環境來換取精度上的可靠很難完成，并且隨著系統使用時間的增加，單一器件輸出數據的可靠性會越差。所以單一器件的姿態檢測系統并不能完成自平衡機器人這種長時間姿態檢測[5]。針對自平衡機器人對精度和反應速度有較高要求的系統，將使用兩個傳感器采集機器人姿態的數據并將其各自測得的數據先進行處理然后融合以獲取更高精度的角度[6]。

由卡爾曼濾波原理可知，由前一狀態的估計值和當前狀態的觀測值產生對狀態變量的估計更新，從而得到當前狀態的估計值，工作流程如圖8所示。首先計算先驗狀態估計，再根據估計誤差和卡爾曼增益矩陣，更新后驗狀態估計后，得出后驗估計誤差和協方差矩陣后，再次循環。

圖8 工作流程圖

卡爾曼濾波器解決離散時間控制系統的一般方法。假設卡爾曼濾波在k時刻真實狀態是從前一時刻k-1的狀態推算出來的，即

xk=Axk-1+Buk+wk

(14)

式中，xk為k時刻的狀態；A為k-1時刻狀態變換模型；B為作用在控制器向量uk上的輸入控制模型；wk為過程中的噪聲，假設其均值為零，協方差矩陣Qk符合多元正態分布，即wk～N(0,Qk)，且k時刻xk與zk滿足：

zk=Hkxk+vk

(15)

式中，Hk為將真實控制映射為觀測空間的觀測模型；vk為觀測噪聲，且vk=0，協方差矩陣Rk滿足正態分布：

vk～N(0,Rk)

(16)

卡爾曼濾波器的預估與更新[7]迭代過程如下，每個公式都假設初始狀態以及每一時刻的噪聲都是互相獨立的。

① 先驗狀態估計：

(17)

② 先驗估計誤差協方差：

Pk/k-1=AkPk-1AT+Q

(18)

③ 卡爾曼增益：

Kk=Pk|k-1HT(HPk|k-1HT+R)

(19)

④ 后驗狀態估計：

(20)

⑤ 后驗誤差協方差：

Pk=(I-KkH)Pk|k-1

(21)

3.3 陀螺儀與加速度計輸出值轉換

由于陀螺儀輸出的是電壓值而系統需要的是陀螺儀檢測到的角速度，因此需要對陀螺儀測得的電壓值進行轉換。其輸出的電壓值與對應加速度值的轉換關系為

(22)

式中，G為陀螺儀檢測的角速度；Vout為陀螺儀輸出電壓；Voffest為陀螺儀靜止時的輸出電壓；Vsen為陀螺儀靈敏度；K為放大電路放大系數。

同樣加速計參數的轉換公式為

(23)

式中，A為加速度計檢測的加速度；Vout為加速度計輸出電壓；Voffest為加速度計在平衡位置的輸出電壓；Vsen為加速度計靈敏度。

得出上述結果之后可以通過反三角函數得出與重力方向之間的夾角，但是此時得出夾角的單位為弧度，可以通過式(24)進行換算得出角度。

(24)

3.4 調試結果

調試過程首先需要對濾波器的各個參數進行設定。本系統中需要設定的值主要有三個：① Q_gyro為陀螺儀權值；② Q_angle為加速度計的權值；③ R_angle為卡爾曼增益。

前面已經完成了對示波器參數的設置，開始向上位機傳輸數據。此時平衡機器人處于平衡位置，通過施加外力使其與平衡位置產生一定夾角，通過上位軟件的波形顯示可以看到輸出波形如圖9所示，標號“1”表示加速度計測量的傾角曲線；標號“2”表示經卡爾曼濾波后產生的波形。從圖9中可以看出將兩者的數據融合濾波后得到的波形變得十分平滑，并在波形變化過程中無毛刺出現，并能夠精確顯示出平衡車的姿態信息。

圖9 卡爾曼濾波器輸出波形

通過觀察放大后的波形如圖10所示,可以看出傾角改變會導致濾波器產生一定的滯后，這種滯后情況的存在會影響控制系統對姿態調整過程。這不利于機器人的穩定性，因此需要修正濾波的參數，然后觀測修整后輸出波形的效果。

圖10 放大后的波形

經過參數調整后，輸出波形的滯后情況與未調整之前相比有了明顯的改善，波形表面的毛刺大幅減少，波形曲線也更為光滑，如圖11所示。

圖11 參數調整后輸出波形較

4 結束語

針對兩輪自平衡機器人運動過程中可能出現的狀態進行建模分析，并根據其運動方程設計兩輪機器人的控制過程。在姿態檢測過程中分析了兩輪平衡機器人姿態檢測過程中直接使用陀螺儀和加速計輸出的數據對檢測產生的影響。針對兩者產生信號的特性采用卡爾曼互補濾波能夠有效消除輸出信號的毛刺，減少輸出姿態角的動態誤差，提高了檢測精度，能夠滿足兩輪機器人的姿態控制需要。對數據的處理方式同樣適用于醫療健康設備中對人體姿態檢測等相似的系統。