基于dsPIC33EP16GS502的兩輪自平衡小車控制系統(tǒng)的設(shè)計

乙金林，陳雯雯，張亞煒，沙春芳（通訊作者）

（鹽城師范學(xué)院物理與電子工程學(xué)院，江蘇鹽城，224007）

0 引言

兩輪自平衡車系統(tǒng)是一種高度不穩(wěn)定系統(tǒng)，具有傳統(tǒng)的一階倒立擺的特性，其平衡控制系統(tǒng)利用安裝在車體上的陀螺儀和加速度傳感器，來檢測車體姿態(tài)的變化，再通過伺服控制系統(tǒng)驅(qū)動電機(jī)進(jìn)行精確地調(diào)整，以實現(xiàn)系統(tǒng)的動態(tài)穩(wěn)定[1,2]。本文利用數(shù)字信號控制器、加速度傳感器等設(shè)計了一種兩輪自平衡小車的控制系統(tǒng)。

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

兩輪自平衡小車的控制系統(tǒng)主要由數(shù)字信號控制器dsPIC33EP16GS502、加速度傳感器MPU-6050、藍(lán)牙串口模塊（型號：DX-BT04）、直流雙電機(jī)驅(qū)動TB6612FNG等組成。系統(tǒng)框圖如圖1所示。

圖1 兩輪自平衡小車控制系統(tǒng)框圖

加速度傳感器MPU-6050用于車體傾角及小車加速度的測量，藍(lán)牙串口通信模塊用于實現(xiàn)手機(jī)對小車的控制，安裝在兩個車輪上的光電檢測電路各輸出AB兩路信號，處理后可以得到小車的速度及方向信息，TB6612FNG用于驅(qū)動車輪動作，OLED模塊用于數(shù)據(jù)和狀態(tài)顯示，所有的通信、測量及控制都在數(shù)字信號控制器dsPIC33EP16GS502控制下實現(xiàn)，下面介紹主要電路的設(shè)計。

2 硬件電路設(shè)計

■2.1 控制電路

系統(tǒng)控制電路如圖2所示。由數(shù)字信號控制器（DSC）dsPIC33EP16GS502實現(xiàn)，這是一種16位數(shù)字信號控制器，適合電機(jī)控制類應(yīng)用，其工作時鐘高達(dá)70MHz，內(nèi)部有5個16位、輸入時鐘頻率為960MHz的高速PWM模塊（每個PWM模塊可有兩個輸出），可以輸出高分辨率、高速PWM信號[3]，本設(shè)計中用于平衡車車輪的精確控制。此外，該DSC具有I2C接口、SPI接口及UART接口，方便與MPU-6050、OLED顯示模塊及藍(lán)牙串口模塊接口的設(shè)計?？刂颇K的主要功能為：（1）讀取MPU-6050內(nèi)部寄存器，獲取三軸加速度傳感器和三軸陀螺儀數(shù)據(jù)，再利用卡爾曼濾波法對加速度傳感器輸出的數(shù)據(jù)和陀螺儀輸出的數(shù)據(jù)進(jìn)行解算，計算出角速度、角速度誤差等；（2）與藍(lán)牙串口模塊通信，接收手機(jī)APP發(fā)來的命令，發(fā)送當(dāng)前數(shù)據(jù)、狀態(tài)到手機(jī)上顯示；（3）控制OLED模塊顯示數(shù)據(jù)、狀態(tài)；（4）讀取光電傳感器對安裝在車輪上的光柵編碼盤的檢測信號，處理后得到小車的速度和運動方向；（5）輸出30kHz、15位分辨率（實際范圍為0~32000）的PWM信號給電機(jī)驅(qū)動芯片，從而實現(xiàn)對兩個車輪的精確控制。

圖2 主控模塊電路圖

■2.2 電機(jī)驅(qū)動電路

電機(jī)驅(qū)動電路如圖3所示。TB6612FNG是東芝公司推出的一種電機(jī)控制專用芯片，其集成度高、驅(qū)動能力強(qiáng)[4]，分別控制兩個直流電機(jī)。設(shè)計中使用了DSC的6個I/O腳用于對TB6612的控制，其中4個I/O腳用于電機(jī)正轉(zhuǎn)/反轉(zhuǎn)/停止等功能控制，另外2個I/O腳輸出兩路30k Hz的PWM信號，實現(xiàn)電機(jī)轉(zhuǎn)速控制。

圖3 電機(jī)驅(qū)動電路

■2.3 測量及通信電路

車體傾角、加速度測量及通信電路如圖4所示，DSC通過2個I/O腳與MPU-6050相連，以I2C接口的方式讀取MPU-6050的測量結(jié)果[5]；為實現(xiàn)手機(jī)以藍(lán)牙方式對小車進(jìn)行控制，設(shè)計中使用了藍(lán)牙串口模塊，該模塊與DSC的串行口相連，如圖4所示，圖中的D3用于指示藍(lán)牙連接狀態(tài)，按下S1鍵可以斷開藍(lán)牙連接。

圖4 測量及通信電路

3 軟件設(shè)計

系統(tǒng)程序采用模塊化編程方式，每個模塊實現(xiàn)一定的功能，各模塊在主程序調(diào)用下協(xié)同運作實現(xiàn)小車的平衡控制。系統(tǒng)程序由MPU-6050讀寫、卡爾曼濾波、車速及運動方向檢測、PID控制、藍(lán)牙串口通信及數(shù)據(jù)顯示等模塊組成。系統(tǒng)主程序流程圖如圖5所示。

圖5 系統(tǒng)主程序流程圖

MPU6050的姿態(tài)解算采用DMP（DMP是MPU6050內(nèi)部的運動引擎，全稱Digital Motion Processor，直接輸出四元數(shù)）方式，在平衡車實際調(diào)試中發(fā)現(xiàn)卡爾曼濾波進(jìn)行數(shù)據(jù)融合和通過DMP輸出的四元數(shù)得到的角度最為穩(wěn)定。平衡小車的卡爾曼濾波器設(shè)計關(guān)鍵要素是選取狀態(tài)向量，平衡小車的車體傾斜角度和車體的傾斜角速度是倒數(shù)關(guān)系，所以狀態(tài)向量的元素可以選擇為車體的角速度(Q_gyro)、車體的傾角(Q_angle)等來估計陀螺儀的零位偏(Q_bias），卡爾曼濾波采樣時間為dt=0.005，觀查噪聲協(xié)方差矩陣(R_angle)[6,7]。卡爾曼濾波融合流程圖如圖6所示。

圖6 卡爾曼濾波融合流程圖

程序設(shè)計中每5ms測量一次傾角及加速度的值，這樣的測量頻率可以取得較好的卡爾曼濾波和互補(bǔ)濾波的效果。系統(tǒng)啟動后調(diào)用各個模塊采集數(shù)據(jù)，速度環(huán)采用PI（比例-積分）控制、轉(zhuǎn)向環(huán)P（比例）控制、直立（平衡）環(huán)采用PD（比例-微分）控制。數(shù)據(jù)處理后最終輸出兩路PWM控制左右兩個電機(jī)的運動，從而實現(xiàn)小車的控制。

在實際調(diào)試中，上述各環(huán)的參數(shù)需要實際測試得出，以直立環(huán)PD控制參數(shù)確定為例，其確定流程如圖7所示。其余各環(huán)的控制參數(shù)的確定與直立環(huán)類似，不再贅述。

圖7 直立環(huán)PD參數(shù)的確定

4 系統(tǒng)調(diào)試

平衡小車的調(diào)試主要分為三個環(huán)節(jié)：車體直立調(diào)試、小車速度控制、小車轉(zhuǎn)向控制，下面分別介紹。

■4.1 車體直立調(diào)試

平衡小車直立環(huán)使用PD（比例微分）控制器，直立控制調(diào)試，由此可以得出結(jié)論見表1、表2所示。

表1 直立環(huán)kp參數(shù)測試（kd=0）

表2 直立環(huán)kd參數(shù)測試（kp=500）

從表1、表2可以看出直立環(huán)中應(yīng)取：kp=500、kd=1.7，根據(jù)實踐經(jīng)驗，對kp、kd都乘以0.6，然后取整即得：kp=300、kd=1。

■4.2 小車速度控制

在平衡小車的速度控制中，采用最常使用的PI（比例積分）控制器，PI控制器是一種線性控制器，是將偏差的比例（P）和積分（I）通過線性組合構(gòu)成控制量，對被控對象進(jìn)行控制。根據(jù)速度控制調(diào)試，由此可以得出結(jié)論見表3所示。根據(jù)表3，設(shè)計中選取kp=80、kd=0.4。

表3 速度環(huán)kp和ki大小測試

■4.3 小車轉(zhuǎn)控制

平衡小車轉(zhuǎn)向控制比較簡單，設(shè)計中僅使用了比例（P）控制。具體而言，就是使用 Z 軸陀螺儀的數(shù)據(jù)作為轉(zhuǎn)向速度偏差進(jìn)行比例控制，目的是保持轉(zhuǎn)向速度為設(shè)定值。這種方法算法簡單、避免了編碼器對車輪滑動無法檢測現(xiàn)象、陀螺儀漂移等問題。轉(zhuǎn)向環(huán)的作用是使小車行駛的過程中，跟隨給定的Z軸角速度，例如若設(shè)定的Z軸目標(biāo)角速度為零，那么小車應(yīng)該走一個直線，根據(jù)轉(zhuǎn)向控制調(diào)試，由此可以得出結(jié)論見表4所示。根據(jù)表4中的數(shù)據(jù)，設(shè)計中轉(zhuǎn)向環(huán)控制中，取kp=1。

表4 轉(zhuǎn)向環(huán)kp大小測試

綜上所述，各控制環(huán)的選擇適當(dāng)?shù)腜ID參數(shù)，然后再進(jìn)行綜合調(diào)試，可以使平衡小車能穩(wěn)定的做自平衡運動。

5 結(jié)束語

文中介紹了一種硬件上采用數(shù)字信號控制器dsPIC33E P16GS502、加速度傳感器MPU-6050等，軟件上主要采用卡爾曼數(shù)據(jù)融合算法、PID控制算法的兩輪自平衡小車控制系統(tǒng)的設(shè)計。實際測試表明該小車在一般的水泥地面上自平衡控制范圍約為±15°，施加一定的干擾，小車仍能保持平衡；可以完成通過手機(jī)以藍(lán)牙方式發(fā)出的前進(jìn)、倒退、轉(zhuǎn)彎等指令，控制過程穩(wěn)定、可靠。