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

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

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

0 引言

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

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

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

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

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

2 硬件電路設(shè)計(jì)

■2.1 控制電路

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

圖2 主控模塊電路圖

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

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

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

■2.3 測(cè)量及通信電路

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

圖4 測(cè)量及通信電路

3 軟件設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

■4.2 小車速度控制

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

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

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

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

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

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

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

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