兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)探討

1、Word參考資料，下載后可編輯兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)探討 摘要：介紹了一種采納數(shù)字信號(hào)操縱器dsPIC33EP16GS502和運(yùn)動(dòng)傳感器MPU-6050等設(shè)計(jì)的兩輪自平衡小車的操縱系統(tǒng)。該系統(tǒng)利用卡爾曼濾波算法對(duì)陀螺儀和加速度計(jì)的信號(hào)進(jìn)行融合，計(jì)算出小車傾角和角速度的最優(yōu)估量值，采納PID算法對(duì)驅(qū)動(dòng)電機(jī)的兩路PWM信號(hào)進(jìn)行操縱，從而實(shí)現(xiàn)兩輪小車的自平衡操縱。文中詳細(xì)介紹了兩輪小車自平衡操縱系統(tǒng)硬件和軟件的設(shè)計(jì)。 關(guān)鍵詞：卡爾曼濾波；數(shù)據(jù)融合；自平衡；兩輪；dsPIC33EP16GS502 兩輪自平衡車系統(tǒng)是一種高度不穩(wěn)定系統(tǒng)，具有傳統(tǒng)的一階倒立擺的特性，其平衡操縱系統(tǒng)利用安裝在車體上的陀螺

2、儀和加速度傳感器，來檢測(cè)車體姿態(tài)的變化，再通過伺服操縱系統(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所示。加速度傳感器MPU-6050用于車體傾角及小車加速度的測(cè)量，藍(lán)牙串口通信模塊用于實(shí)現(xiàn)號(hào)碼對(duì)小車的操縱，安裝在兩個(gè)車輪上的光電檢測(cè)電路各輸出AB兩路信號(hào)，處理后可以得到小車的速度及方向信息，TB

3、6612FNG用于驅(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

4、-6050、OLED顯示模塊及藍(lán)牙串口模塊接口的設(shè)計(jì)。操縱模塊的主要功能為：(1)讀取MPU-6050內(nèi)部存放器，獲得三軸加速度傳感器和三軸陀螺儀數(shù)據(jù)，再利用卡爾曼濾波法對(duì)加速度傳感器輸出的數(shù)據(jù)和陀螺儀輸出的數(shù)據(jù)進(jìn)行解算，計(jì)算出角速度、角速度誤差等；(2)與藍(lán)牙串口模塊通信，接收號(hào)碼APP發(fā)來的命令，發(fā)送當(dāng)前數(shù)據(jù)、狀態(tài)到號(hào)碼上顯示；(3)操縱OLED模塊顯示數(shù)據(jù)、狀態(tài)；(4)讀取光電傳感器對(duì)安裝在車輪上的光柵編碼盤的檢測(cè)信號(hào)，處理后得到小車的速度和運(yùn)動(dòng)方向；(5)輸出30kHz、15位分辨率(實(shí)際范圍為032000)的PWM信號(hào)給電機(jī)驅(qū)動(dòng)芯片，從而實(shí)現(xiàn)對(duì)兩個(gè)車輪的精確操縱。 2、2電機(jī)驅(qū)動(dòng)電路

5、 電機(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足輸出兩路30kHz的PWM信號(hào)，實(shí)現(xiàn)電機(jī)轉(zhuǎn)速操縱。 2、3測(cè)量及通信電路 車體傾角、加速度測(cè)量及通信電路如圖4所示，DSC通過2個(gè)I/O足與MPU-6050相連，以I2C接口的方式讀取MPU-6050的測(cè)量結(jié)果5；為實(shí)現(xiàn)號(hào)碼以藍(lán)牙方式對(duì)小車進(jìn)行操縱，設(shè)計(jì)中使用了藍(lán)牙串口模塊，該模塊與DSC的串行口相連，如圖4所示，圖中的D3用于指示藍(lán)牙連接狀

6、態(tài)，按下S1鍵可以斷開藍(lán)牙連接。 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所示。MPU6050的姿態(tài)解算采納DMP(DMP是MPU6050內(nèi)部的運(yùn)動(dòng)引擎，全稱DigitalMotionProcessor，直接輸出四元數(shù))方式，在平衡車實(shí)際調(diào)試中發(fā)覺卡爾曼濾波進(jìn)行數(shù)據(jù)融合和通過DMP輸出的四元數(shù)得到的角度最為穩(wěn)定。平衡小車的卡爾曼濾波器設(shè)計(jì)關(guān)鍵要素是選取狀態(tài)向量，平衡小車的車體傾斜角度和車體的傾

7、斜角速度是倒數(shù)關(guān)系，所以狀態(tài)向量的元素可以選擇為車體的角速度（Q_gyro）、車體的傾角（Q_angle）等來估量陀螺儀的零位偏（Q_bias)，卡爾曼濾波采樣時(shí)間為dt=0005，觀查噪聲協(xié)方差矩陣（R_angle）6,7?？柭鼮V波融合流程圖如圖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è)試

8、得出，以直立環(huán)PD操縱參數(shù)確定為例，其確定流程如圖7所示。其余各環(huán)的操縱參數(shù)確實(shí)定與直立環(huán)類似，不再贅述。 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é)論見表1、表2所示。從表1、表2可以看出直立環(huán)中應(yīng)?。簁p=500、kd=1�7，依據(jù)實(shí)踐經(jīng)驗(yàn)，對(duì)kp、kd都乘以06，然后取整即得：kp=300、kd=1。 4、2小車速度操縱 在平衡小車的速度操縱中，采納最常使用的PI(比例積分)操縱器，PI操縱器是一種線性操縱器，是將偏差的比例(

9、P)和積分(I)通過線性組合構(gòu)成操縱量，對(duì)被控對(duì)象進(jìn)行操縱。依據(jù)速度操縱調(diào)試，由此可以得出結(jié)論見表3所示。依據(jù)表3，設(shè)計(jì)中選取kp=80、kd=0.4。 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)無法檢測(cè)現(xiàn)象、陀螺儀漂移等問題。轉(zhuǎn)向環(huán)的作用是使小車行駛的過程中，跟隨給定的Z軸角速度，例如若設(shè)定的Z軸目標(biāo)角速度為零，那么小車應(yīng)該走一個(gè)直線，依據(jù)轉(zhuǎn)向操縱調(diào)試，由此可以得出結(jié)論見表4所示。依據(jù)表4中的數(shù)據(jù)，設(shè)計(jì)中轉(zhuǎn)向環(huán)操縱中，取kp=1。綜上所述，各操縱環(huán)的選擇適當(dāng)?shù)腜ID參數(shù)，然后再進(jìn)行綜合調(diào)試，可以使平衡小車能穩(wěn)定的做自平衡運(yùn)動(dòng)。 5結(jié)束語 文中介紹了一種硬件上采納數(shù)字信號(hào)操縱器dsPIC33EP16GS5