面向兩輪平衡機(jī)器人姿態(tài)檢測系統(tǒng)的研究與設(shè)計

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

近些年來，在人工智能大火的背景下，機(jī)器人作為人工智能的一個研究領(lǐng)域也頻頻出現(xiàn)在人們的視線。機(jī)器人在一些危險、高難度的工業(yè)環(huán)境以及狹小空間的工業(yè)領(lǐng)域很快就得到了廣泛應(yīng)用。而且人們?nèi)粘Ｉ钪械妮o助機(jī)器人的工作環(huán)境也逐漸涉及到不平坦路面、斜坡、樓梯等復(fù)雜環(huán)境。因此在不同環(huán)境下保證機(jī)器人能夠穩(wěn)定地行走成為機(jī)器人完成任務(wù)的首要條件，在此背景下出現(xiàn)了兩輪自平衡機(jī)器人。

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

1 系統(tǒng)總體設(shè)計

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

圖1 整體設(shè)計方案

2 自平衡機(jī)器人平衡模型的建立

2.1 自平衡機(jī)器人的運(yùn)動分析

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

圖2 輪子轉(zhuǎn)動保持小車平衡

為了使機(jī)器人一直處于直立平衡狀態(tài)，需要對模型運(yùn)動學(xué)模型進(jìn)行分析，從而建立可靠的數(shù)學(xué)模型。如圖3所示，對機(jī)器人的運(yùn)動姿態(tài)進(jìn)行運(yùn)動學(xué)分析時可以將機(jī)器人系統(tǒng)簡化成一級倒立擺，而處于靜止時的倒立擺是倒立擺模型分析的基礎(chǔ)，因此應(yīng)先對靜止?fàn)顟B(tài)的一級倒立擺進(jìn)行受力情況分析。

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

F=mgsinθ

(1)

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

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

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

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

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

Fg=macosθ

(2)

此情況下受到的合回復(fù)力為

F=mgsinθ-macosθ

(3)

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

F=mgθ-mk1θ

(4)

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

F=mgθ-mk1θ-mk2θ′

(5)

根據(jù)上面的分析可以得出車輪加速度與傾斜角度之間的關(guān)系為

a=mk1θ+mk2θ′

(6)

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

2.2 自平衡機(jī)器人的運(yùn)動微分方程

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

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

小車運(yùn)動微分方程表達(dá)式為

(7)

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

(8)

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

(9)

2.3 PID控制器設(shè)計

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

(10)

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

圖6 加入反饋后的控制系統(tǒng)結(jié)構(gòu)圖

(11)

此時，系統(tǒng)的兩個極點(diǎn)為

(12)

k1>g,k2>0時，兩個極點(diǎn)都位于s平面的左半平面根據(jù)穩(wěn)定判據(jù)系統(tǒng)穩(wěn)定。因此，k1>g,k2>0時，小車可以保持平衡。

根據(jù)對控制系統(tǒng)的分析，系統(tǒng)中PID控制器的輸出方程可寫為

OUT_Motor=Kp·Angle+Kd·Angle_dot

(13)

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

3 姿態(tài)檢測系統(tǒng)

3.1 姿態(tài)檢測的方式

自平衡機(jī)器人本身是一個不穩(wěn)定非線性系統(tǒng)，因此在自平衡機(jī)器人維持穩(wěn)定狀態(tài)的過程中需要實(shí)時檢測車身姿態(tài)。過程中需要對得出的數(shù)據(jù)進(jìn)行整合，然后通過控制系統(tǒng)及時調(diào)整車身姿態(tài)來保持平衡。因此在機(jī)器人姿態(tài)檢測時的精度和控制系統(tǒng)對車身姿態(tài)的調(diào)整速度對于系統(tǒng)的穩(wěn)定運(yùn)轉(zhuǎn)至關(guān)重要。

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

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

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

3.2 卡爾曼濾波數(shù)據(jù)融合

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

由卡爾曼濾波原理可知，由前一狀態(tài)的估計值和當(dāng)前狀態(tài)的觀測值產(chǎn)生對狀態(tài)變量的估計更新，從而得到當(dāng)前狀態(tài)的估計值，工作流程如圖8所示。首先計算先驗(yàn)狀態(tài)估計，再根據(jù)估計誤差和卡爾曼增益矩陣，更新后驗(yàn)狀態(tài)估計后，得出后驗(yàn)估計誤差和協(xié)方差矩陣后，再次循環(huán)。

圖8 工作流程圖

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

xk=Axk-1+Buk+wk

(14)

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

zk=Hkxk+vk

(15)

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

vk～N(0,Rk)

(16)

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

① 先驗(yàn)狀態(tài)估計：

(17)

② 先驗(yàn)估計誤差協(xié)方差：

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

(18)

③ 卡爾曼增益：

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

(19)

④ 后驗(yàn)狀態(tài)估計：

(20)

⑤ 后驗(yàn)誤差協(xié)方差：

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

(21)

3.3 陀螺儀與加速度計輸出值轉(zhuǎn)換

由于陀螺儀輸出的是電壓值而系統(tǒng)需要的是陀螺儀檢測到的角速度，因此需要對陀螺儀測得的電壓值進(jìn)行轉(zhuǎn)換。其輸出的電壓值與對應(yīng)加速度值的轉(zhuǎn)換關(guān)系為

(22)

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

同樣加速計參數(shù)的轉(zhuǎn)換公式為

(23)

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

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

(24)

3.4 調(diào)試結(jié)果

調(diào)試過程首先需要對濾波器的各個參數(shù)進(jìn)行設(shè)定。本系統(tǒng)中需要設(shè)定的值主要有三個：① Q_gyro為陀螺儀權(quán)值；② Q_angle為加速度計的權(quán)值；③ R_angle為卡爾曼增益。

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

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

通過觀察放大后的波形如圖10所示,可以看出傾角改變會導(dǎo)致濾波器產(chǎn)生一定的滯后，這種滯后情況的存在會影響控制系統(tǒng)對姿態(tài)調(diào)整過程。這不利于機(jī)器人的穩(wěn)定性，因此需要修正濾波的參數(shù)，然后觀測修整后輸出波形的效果。

圖10 放大后的波形

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

圖11 參數(shù)調(diào)整后輸出波形較

4 結(jié)束語

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