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

近些年來(lái)，在人工智能大火的背景下，機(jī)器人作為人工智能的一個(gè)研究領(lǐng)域也頻頻出現(xiàn)在人們的視線。機(jī)器人在一些危險(xiǎn)、高難度的工業(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)檢測(cè)系統(tǒng)中的加速度計(jì)和陀螺儀的互補(bǔ)特性，對(duì)采集到的數(shù)據(jù)進(jìn)行濾波和數(shù)據(jù)融合研究，并根據(jù)兩者的互補(bǔ)特性進(jìn)行了濾波算法設(shè)計(jì)。由于陀螺儀所測(cè)得角速度值只能短時(shí)間內(nèi)處于穩(wěn)定狀態(tài)，這是由其本身特性所導(dǎo)致的；而加速度傳感器測(cè)得的數(shù)據(jù)又存在很大的白噪聲，在進(jìn)行數(shù)據(jù)處理時(shí)通常會(huì)使用加權(quán)平均法和神經(jīng)網(wǎng)絡(luò)等方法。但是這些方法都存在一些缺陷，或是檢測(cè)精度達(dá)不到要求，亦或是其模型和涉及的參數(shù)優(yōu)化太過(guò)復(fù)雜不能適應(yīng)本系統(tǒng)。因此在本系統(tǒng)中采用卡爾曼濾波算法，不僅要對(duì)陀螺儀的輸出信號(hào)進(jìn)行放大和濾波還要根據(jù)其互補(bǔ)特性設(shè)計(jì)卡爾曼濾波器。采用卡爾曼濾波器對(duì)兩者的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合可以得出穩(wěn)定和準(zhǔn)確的數(shù)據(jù)，采用PID算法對(duì)車身的傾斜角度和驅(qū)動(dòng)電機(jī)進(jìn)行閉環(huán)控制。

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

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

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

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

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

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

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

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

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

F=mgsinθ

(1)

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

圖3 一級(jí)倒立擺模型受力分析圖

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

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

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

Fg=macosθ

(2)

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

F=mgsinθ-macosθ

(3)

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

F=mgθ-mk1θ

(4)

當(dāng)兩者之間的比例系數(shù)大于重力加速度時(shí)，就能使合回復(fù)與傾斜方向相反。倒立擺在這種情況下可以回到平衡狀態(tài)，但這個(gè)過(guò)程需要的時(shí)間比較長(zhǎng)。因此需要在系統(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)的時(shí)間長(zhǎng)短。

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

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

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

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

(7)

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

(8)

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

(9)

2.3 PID控制器設(shè)計(jì)

由靜止時(shí)運(yùn)動(dòng)方程(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)

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

(12)

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

根據(jù)對(duì)控制系統(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)檢測(cè)系統(tǒng)

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

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

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

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

圖7 陀螺儀與加速度計(jì)輸出波形

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

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

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

圖8 工作流程圖

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

xk=Axk-1+Buk+wk

(14)

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

zk=Hkxk+vk

(15)

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

vk～N(0,Rk)

(16)

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

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

(17)

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

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

(18)

③ 卡爾曼增益：

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

(19)

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

(20)

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

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

(21)

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

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

(22)

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

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

(23)

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

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

(24)

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

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

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

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

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

圖10 放大后的波形

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

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

4 結(jié)束語(yǔ)

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