兩輪機器人自平衡控制系統設計

商世龍 劉 通 韓禮辰 李博宇

沈陽航空航天大學 遼寧 沈陽 110136

1 研究背景與意義

2011年發生在日本福島的核電站泄漏事故,造成16萬日本民眾被迫搬離家園,12.5公里的地區被劃為禁區,在這種危險惡劣的環境下,只有依靠移動機器人來進行探測和偵察。此外,隨著移動機器人應用領域的不斷擴大,面臨的環境和任務也越來越復雜,機器人要經常工作于一些空間狹小、轉角較多的工作場合。如何在復雜的環境下靈活快捷的執行各種任務是當下研究的熱點。

2 兩輪機器人整體方案設計

2.1 兩輪機器人的基本平衡理論 兩輪機器人在自然狀態下是不穩定結構,車輪支持力方向與車身方向存在角度θ偏差,為對兩輪擬器人受力分析,現將系統簡化為可左右移動平臺上倒立擺模型。簡化模型如圖2.1所示,若對系統增加額外的受力,使得恢復力與位移方向相反才行,控制倒立擺底部的車輪,使得它做加速運動,這樣兩輪機器人自身會受到額外的慣性力,改力與車輪的加速度方向相反,大小成正比,這樣倒立擺受到的恢復力為：

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

圖2.1 兩輪機器人簡化模型

此外,為了使得倒立擺能夠盡快在垂直位置穩定下來,還需要增加阻尼力。因此需要另外增加控制阻尼力,增加的阻尼力與偏角的速度成正比,方向相反。因此式(2.1)可變為：

按照上面的控制方法,可把倒立擺模型變為單擺模型,能夠穩定在垂直位置上,因此,可得控制車輪加速度的控制算法：

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

2.2 兩輪機器人硬件總體方案設計 兩輪機器人自平衡控制系統硬件框圖如圖2.2所示。

圖2.2 兩輪機器人自平衡控制系統硬件框圖

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

兩輪機器人速度控制器采用PI控制,為了保證系統穩定,可以將該系統近似為一個積分環節。將原來的微分環節和這個積分環節合并,形成－一個比例控制環節。這樣可以保持系統控制傳遞函數不變,避免了微分計算。

3 兩輪機器人硬件電路設計

3.1 兩輪機器人核心控制模塊設計 核心控制模塊由STM32F103RCT6系統板和外圍電路組成。為實現兩輪機器人自平衡,需要采集和傳遞各種信號,其設計方案為：將單片機與傳感器模塊連接,通過陀螺儀采集信號,傳送給單片機獲得兩輪機器人的傾斜角度,加速度傳感器可以獲得小車的加速度,將單片機與電機內部的編碼器連接可獲得平均速度；單片機與驅動模塊以PWM信號傳遞,用來控制電機工作,使兩輪機器人保持直立狀態。

3.2 兩輪機器人姿態檢測模塊設計 姿態檢測模塊采用MPU6050傳感器和外圍電路構成,其電路原理圖如圖3.1所示。

圖3.1 MPU6050電路原理圖

3.3 兩輪機器人驅動模塊設計 本方案采用的驅動芯片為TB6612芯片,TB6612芯片為雙驅動,即可以驅動兩個電機,其相對于傳統的L298N效率上提高很多且相比L298N的熱耗性和外圍二極管續流電路,它無需外加散熱片,外圍電路簡單,只需外接電源濾波電容就可以直接驅動電機,利于減小系統尺寸。對于PWM信號,它支持高達100KHz的頻率有較大優勢。

3.4 兩輪機器人電機及編碼器設計 兩輪機器人的驅動電機采用直流電機,其電機內部自帶編碼器,其上集成了上拉電阻,可直接輸出方波,編碼器可獲得兩輪機器人的平均速度,以PWM信號形式傳遞給單片機實時控制姿態。

4 兩輪機器人軟件設計

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

4.2 兩輪機器人平衡控制子程序 MPU6050獲得兩輪機器人的加速度和傾角之后,使用DMP濾波及角度融合。以車身傾角為輸入量,通過控制兩個電機的正反轉保持車身平衡。調用PD算法對車體進行控制。其平衡控制算法為：

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

4.3 兩輪機器人速度控制子程序 本系統以編碼器所測得脈沖數作為速度反饋環節。形成了速度的閉環控制。速度控制的函數參數為兩輪機器人速度的偏差,本文在電機控制上采用了PI算法,其速度算法公式為：

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

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

相比于直立環和速度環,轉向環是最不重要的,如果缺少了直立環和速度環,車身無法長時間保付直立。轉向環的作用是使兩輪機器人行駛過程中,跟隨給定的z軸角速度,具體來說,若設定的z軸目標角速度為零,那么應該兩輪機器人應該走直線。

5 總結與展望

本文主要研究了基于單片機的兩輪自平衡機器人控制系統。通過硬件與軟件來調整車身的姿態,實現兩輪機器人動態平衡和直線行走。

本次設計只是簡單地建立了自平衡機器人的模型,在未來還有以下方面可以繼續研究和提高：

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

(2)采用高精度的陀螺儀及加速度計,提高系統姿態檢測精度。