商世龍 劉 通 韓禮辰 李博宇

沈陽(yáng)航空航天大學(xué) 遼寧 沈陽(yáng) 110136

1 研究背景與意義

2011年發(fā)生在日本福島的核電站泄漏事故,造成16萬(wàn)日本民眾被迫搬離家園,12.5公里的地區(qū)被劃為禁區(qū),在這種危險(xiǎn)惡劣的環(huán)境下,只有依靠移動(dòng)機(jī)器人來(lái)進(jìn)行探測(cè)和偵察。此外,隨著移動(dòng)機(jī)器人應(yīng)用領(lǐng)域的不斷擴(kuò)大,面臨的環(huán)境和任務(wù)也越來(lái)越復(fù)雜,機(jī)器人要經(jīng)常工作于一些空間狹小、轉(zhuǎn)角較多的工作場(chǎng)合。如何在復(fù)雜的環(huán)境下靈活快捷的執(zhí)行各種任務(wù)是當(dāng)下研究的熱點(diǎn)。

2 兩輪機(jī)器人整體方案設(shè)計(jì)

2.1 兩輪機(jī)器人的基本平衡理論 兩輪機(jī)器人在自然狀態(tài)下是不穩(wěn)定結(jié)構(gòu),車輪支持力方向與車身方向存在角度θ偏差,為對(duì)兩輪擬器人受力分析,現(xiàn)將系統(tǒng)簡(jiǎn)化為可左右移動(dòng)平臺(tái)上倒立擺模型。簡(jiǎn)化模型如圖2.1所示,若對(duì)系統(tǒng)增加額外的受力,使得恢復(fù)力與位移方向相反才行,控制倒立擺底部的車輪,使得它做加速運(yùn)動(dòng),這樣兩輪機(jī)器人自身會(huì)受到額外的慣性力,改力與車輪的加速度方向相反,大小成正比,這樣倒立擺受到的恢復(fù)力為：

式中,由于θ很小,所以進(jìn)行了線性化。假設(shè)負(fù)反饋是車輪加速度α與偏角θ成正比,比例為k1,如果比例k1＞g,那么恢復(fù)力的方向便于位移方向相反了。

圖2.1 兩輪機(jī)器人簡(jiǎn)化模型

此外,為了使得倒立擺能夠盡快在垂直位置穩(wěn)定下來(lái),還需要增加阻尼力。因此需要另外增加控制阻尼力,增加的阻尼力與偏角的速度成正比,方向相反。因此式(2.1)可變?yōu)椋?/p>

按照上面的控制方法,可把倒立擺模型變?yōu)閱螖[模型,能夠穩(wěn)定在垂直位置上,因此,可得控制車輪加速度的控制算法：

式中,θ1為傾角,θ2為角速度,k1,k2為比例系數(shù)。只要保證k1＞g,k2＞g條件下,可以使得系統(tǒng)像單擺一樣維持在直立狀態(tài)。

2.2 兩輪機(jī)器人硬件總體方案設(shè)計(jì) 兩輪機(jī)器人自平衡控制系統(tǒng)硬件框圖如圖2.2所示。

圖2.2 兩輪機(jī)器人自平衡控制系統(tǒng)硬件框圖

2.3 兩輪機(jī)器人軟件總體方案設(shè)計(jì) 兩輪機(jī)器人直立控制器采用PD控制,與角度成比例的控制量是稱為比例控制；與角速度成比例的控制量稱為微分控制(角速度是角度的微分)其中微分參數(shù)相當(dāng)于阻尼力,可以有效抑制車模震蕩。

兩輪機(jī)器人速度控制器采用PI控制,為了保證系統(tǒng)穩(wěn)定,可以將該系統(tǒng)近似為一個(gè)積分環(huán)節(jié)。將原來(lái)的微分環(huán)節(jié)和這個(gè)積分環(huán)節(jié)合并,形成－一個(gè)比例控制環(huán)節(jié)。這樣可以保持系統(tǒng)控制傳遞函數(shù)不變,避免了微分計(jì)算。

3 兩輪機(jī)器人硬件電路設(shè)計(jì)

3.1 兩輪機(jī)器人核心控制模塊設(shè)計(jì) 核心控制模塊由STM32F103RCT6系統(tǒng)板和外圍電路組成。為實(shí)現(xiàn)兩輪機(jī)器人自平衡,需要采集和傳遞各種信號(hào),其設(shè)計(jì)方案為：將單片機(jī)與傳感器模塊連接,通過(guò)陀螺儀采集信號(hào),傳送給單片機(jī)獲得兩輪機(jī)器人的傾斜角度,加速度傳感器可以獲得小車的加速度,將單片機(jī)與電機(jī)內(nèi)部的編碼器連接可獲得平均速度；單片機(jī)與驅(qū)動(dòng)模塊以PWM信號(hào)傳遞,用來(lái)控制電機(jī)工作,使兩輪機(jī)器人保持直立狀態(tài)。

3.2 兩輪機(jī)器人姿態(tài)檢測(cè)模塊設(shè)計(jì) 姿態(tài)檢測(cè)模塊采用MPU6050傳感器和外圍電路構(gòu)成,其電路原理圖如圖3.1所示。

圖3.1 MPU6050電路原理圖

3.3 兩輪機(jī)器人驅(qū)動(dòng)模塊設(shè)計(jì) 本方案采用的驅(qū)動(dòng)芯片為TB6612芯片,TB6612芯片為雙驅(qū)動(dòng),即可以驅(qū)動(dòng)兩個(gè)電機(jī),其相對(duì)于傳統(tǒng)的L298N效率上提高很多且相比L298N的熱耗性和外圍二極管續(xù)流電路,它無(wú)需外加散熱片,外圍電路簡(jiǎn)單,只需外接電源濾波電容就可以直接驅(qū)動(dòng)電機(jī),利于減小系統(tǒng)尺寸。對(duì)于PWM信號(hào),它支持高達(dá)100KHz的頻率有較大優(yōu)勢(shì)。

3.4 兩輪機(jī)器人電機(jī)及編碼器設(shè)計(jì) 兩輪機(jī)器人的驅(qū)動(dòng)電機(jī)采用直流電機(jī),其電機(jī)內(nèi)部自帶編碼器,其上集成了上拉電阻,可直接輸出方波,編碼器可獲得兩輪機(jī)器人的平均速度,以PWM信號(hào)形式傳遞給單片機(jī)實(shí)時(shí)控制姿態(tài)。

4 兩輪機(jī)器人軟件設(shè)計(jì)

4.1 系統(tǒng)主程序 系統(tǒng)進(jìn)入工作狀態(tài)時(shí)主函數(shù)進(jìn)行單片機(jī)的初始化,初始化的工作包括兩部分,一部分是對(duì)于單片機(jī)各個(gè)應(yīng)用到的模塊進(jìn)行初始化。第二部分是應(yīng)用于程序初始化,即對(duì)于兩輪機(jī)器人控制程序中應(yīng)用得到的變量值進(jìn)行初始化,初始化工作結(jié)束后便等待姿態(tài)檢測(cè)子程序獲取姿態(tài)信息。系統(tǒng)在主程序初始化之后,每5ms中斷一次,就可以讀取MPU6050這一姿態(tài)控制模塊采集的姿態(tài)數(shù)據(jù),與此同時(shí)完成電機(jī)速度檢測(cè),在姿態(tài)數(shù)據(jù)讀取完成之后,為了解決陀螺儀漂移問(wèn)題,采取將加速度計(jì)與陀螺儀信號(hào)融合的方法。

4.2 兩輪機(jī)器人平衡控制子程序 MPU6050獲得兩輪機(jī)器人的加速度和傾角之后,使用DMP濾波及角度融合。以車身傾角為輸入量,通過(guò)控制兩個(gè)電機(jī)的正反轉(zhuǎn)保持車身平衡。調(diào)用PD算法對(duì)車體進(jìn)行控制。其平衡控制算法為：

上式中balance是車身平衡的電機(jī)控制變量,Bias為濾波后的傾角偏差值,Gyro為小車當(dāng)前的角速度。經(jīng)過(guò)PD算法處理后的PWM波可以用來(lái)控制電機(jī)的輸出。

4.3 兩輪機(jī)器人速度控制子程序 本系統(tǒng)以編碼器所測(cè)得脈沖數(shù)作為速度反饋環(huán)節(jié)。形成了速度的閉環(huán)控制。速度控制的函數(shù)參數(shù)為兩輪機(jī)器人速度的偏差,本文在電機(jī)控制上采用了PI算法,其速度算法公式為：

上式中Velocity為速度的電機(jī)控制量,Encoder為上一次目標(biāo)速度與實(shí)際速度的偏差,Encoder_Integral為速度偏差的累積。獲得速度的電機(jī)控制量之后,再將(4.1)式與(4.2)相加,獲得最終的PWM值,然后將其送到PWM寄存器。

4.4 轉(zhuǎn)向控制子程序 兩輪機(jī)器人轉(zhuǎn)向環(huán)使用P(比例)控制器或者P(比例)D(微分)控制器,一般的控制系統(tǒng)單純的P控制或者PI控制就可以了,轉(zhuǎn)向環(huán)就是這種“一般的控制系統(tǒng)”,對(duì)響應(yīng)要求不高,所以只使用P控制即可。

相比于直立環(huán)和速度環(huán),轉(zhuǎn)向環(huán)是最不重要的,如果缺少了直立環(huán)和速度環(huán),車身無(wú)法長(zhǎng)時(shí)間保付直立。轉(zhuǎn)向環(huán)的作用是使兩輪機(jī)器人行駛過(guò)程中,跟隨給定的z軸角速度,具體來(lái)說(shuō),若設(shè)定的z軸目標(biāo)角速度為零,那么應(yīng)該兩輪機(jī)器人應(yīng)該走直線。

5 總結(jié)與展望

本文主要研究了基于單片機(jī)的兩輪自平衡機(jī)器人控制系統(tǒng)。通過(guò)硬件與軟件來(lái)調(diào)整車身的姿態(tài),實(shí)現(xiàn)兩輪機(jī)器人動(dòng)態(tài)平衡和直線行走。

本次設(shè)計(jì)只是簡(jiǎn)單地建立了自平衡機(jī)器人的模型,在未來(lái)還有以下方面可以繼續(xù)研究和提高：

(1)使用速度更快的控制器,如DSP,ARM等,提高系統(tǒng)控制速度。

(2)采用高精度的陀螺儀及加速度計(jì),提高系統(tǒng)姿態(tài)檢測(cè)精度。