路成強，曾 潔，李千振

(大連交通大學電氣信息學院，遼寧 大連 116028)

基于卡爾曼濾波的兩輪自平衡遙控小車設計

路成強，曾 潔，李千振

(大連交通大學電氣信息學院，遼寧 大連 116028)

針對現有兩輪自平衡遙控小車系統(tǒng)穩(wěn)定性不佳、實現功能單一、車體擺動幅度過大等不足，提出了兩輪自平衡遙控小車改進方案。采用STM32F103單片機作為主控器，移植實時操作系統(tǒng)μC/OS-II，選擇直流電機、傳感器以及外圍電路，設計了兩輪自平衡遙控小車的硬件控制部分。利用MPU6050模塊實時獲取小車的當前運行姿態(tài)，經卡爾曼濾波處理位姿信息后發(fā)送給主控器，隨后利用線性PID控制算法調節(jié)電機轉速，結合雙閉環(huán)控制實現了小車的直立平衡行走。搖桿電位器結合無線通信模塊可遠程遙控小車，利用超聲波傳感器和GSM模塊實現避障和遠程監(jiān)測功能，通過TFT彩色液晶屏和藍牙手機客戶端實現人機交互功能。試驗結果表明，小車系統(tǒng)運行平穩(wěn)、功能多元、抗干擾能力強，具有很高的準確性和穩(wěn)定性，實現了自平衡遙控理論的擴展，在機器人領域具有廣闊的應用前景。

STM32F103; 卡爾曼濾波; 線性PID; 雙閉環(huán)控制; 無線通信; 遠程遙控

0 引言

隨著科技的發(fā)展和產業(yè)結構轉型戰(zhàn)略的驅動，兩輪小車系統(tǒng)的研究與應用迅猛發(fā)展。兩輪小車在移動機器人基礎上逐漸探索研究，其本質是自主移動機器人，主體結構類似于倒立擺系統(tǒng)[1]。移動機器人技術隨著計算機、微電子技術的進步而快速發(fā)展，我國關于兩輪小車的研究正處于飛速發(fā)展的初期階段，開展此項研究對其構建數學模型，依賴自平衡理論實現控制，將對于提高我國在該領域的科研水平具有重要的理論和現實意義。

本文在對現有兩輪小車技術總結分析的基礎上，從電子器件選擇、硬件結構設計和控制算法應用等方面進行改進，完成了本系統(tǒng)的總體方案。該小車在穩(wěn)定性和多功能性等方面均有良好的表現。

1 系統(tǒng)總體平衡原理與運動分析

1.1 兩輪自平衡小車平衡原理

兩輪自平衡小車是一個高度不穩(wěn)定系統(tǒng)。自平衡原理主要描述小車在運動情況下如何動態(tài)地調整姿態(tài)使其平衡。理想狀態(tài)下，小車重力方向和地面對小車支持力方向相反，角度相差180°，φ角度為0°，此時系統(tǒng)受力均衡，小車保持直立平衡狀態(tài)。但實際情況下，角度不會為0°，小車重力方向和地面對小車支持力方向總是不處于一條直線，合力也不為0。根據牛頓運動定律可知，小車會有前傾或后仰的趨勢，導致角度越來越大。當小車呈現后仰的傾向狀態(tài)，達到一定程度后，如果可以改變小車讓它有后退的傾向狀態(tài)，則使小車后退產生前傾的趨勢去及時糾正原來小車前進過程中后仰的傾向狀態(tài)；當小車呈現前傾的狀態(tài)，同理即可。兩種狀態(tài)在行進中循環(huán)往復，則小車在豎直方向上就會實時保持動態(tài)直立平衡狀態(tài)。

1.2 兩輪自平衡小車遙控原理

搖桿電位器結合兩個無線通信模塊NRF24L01來控制小車在行進過程中一直保持平衡穩(wěn)定運行狀態(tài)。例如向前推動控制桿，無線通信模塊發(fā)射方將模擬量的變化傳輸到接收方，再通過A/D轉換模塊轉化為數字信號給主控器。主控器依據程序改變速度給定值，速度的反饋值和給定值會產生偏差，導致車體前傾。MPU6050模塊實時檢測到車體角度變化信息后傳送到主控器。主控器通過電機驅動模塊驅動電機向前轉動產生相應的力矩來維持小車平衡狀態(tài)，從而實現無線遠程遙控功能。

1.3 兩輪自平衡小車運動分析

小車在保持動態(tài)平衡狀態(tài)下，完成靜止、前進、后退、轉彎等復雜動作。小車的兩個車輪是維持小車直立行走的動力來源，分別由兩個直流電機來驅動兩個車輪轉動。從控制角度分析，控制對象是兩輪小車，控制輸入量是兩個直流電機的轉動速度。兩輪小車運動控制系統(tǒng)可以分為三個子系統(tǒng)[2]。

(1)小車平衡控制系統(tǒng)。

小車實時姿態(tài)信息作為輸入量，保持小車處于動態(tài)直立平衡狀態(tài)是首要任務，而小車平衡狀態(tài)是通過系統(tǒng)負反饋實現控制兩個直流電機正反向轉動，來抵消傾斜的趨勢?？刂菩≤嚻胶鉅顟B(tài)穩(wěn)定條件有：

① 利用MPU6050傳感器精確測量小車傾角和傾角速度的大小。

② 通過控制電機轉速實現對車輪的運動控制，以控制車輪的加速度為主要最終目的。

(2)小車速度控制系統(tǒng)。

通過車體傾角和車輪速度共同控制兩個直流電機轉動速度，在保持自平衡的基礎上實現控制小車運動速度。車輪速度控制需要測量兩個電機的轉速，車體傾角的變化由MPU6050模塊直接測得Y軸的角速度和角加速度。小車角度和速度控制系統(tǒng)框圖如圖1所示。

圖1 角度和速度控制系統(tǒng)框圖

圖1中：K1和K2分別為角度控制中微分控制參數和比例控制參數；K3和K4分別為速度控制中比例控制參數和微分控制參數。

利用微分控制的目的是增加車模角度和速度的穩(wěn)定性，防止控制超調。

(3)小車方向控制系統(tǒng)。

主控器通過電機驅動模塊分別驅動兩個直流電機控制兩個車輪轉動，使得左右車輪產生轉動差速來實現小車方向的控制。實際過程中，由于小車負載較大，行進過程存在較大慣性，為防止小車方向控制中過沖現象發(fā)生，增加了微分控制環(huán)節(jié)來對電機差動控制量進行修正；依據小車差動控制方案，小車方向控制還需簡單的比例控制；為防止出現積分飽和現象而導致電機的響應速度降低，則將積分控制環(huán)節(jié)舍棄[3]。

小車運動控制系統(tǒng)框圖如圖2所示。

圖2 運動控制系統(tǒng)總框圖

綜上所述，小車平衡和方向控制系統(tǒng)是用PD控制算法控制電機的輸出電壓來控制電機的轉速達到控制目的；而小車速度控制系統(tǒng)也是用PD控制算法，但是通過調節(jié)小車的傾角大小實現控制目的。

2 系統(tǒng)硬件電路設計方案

2.1 硬件控制系統(tǒng)

兩輪自平衡遙控小車的硬件系統(tǒng)包括：主控器、電源模塊、顯示模塊、數據無線通信模塊、MPU6050模塊、A/D轉換模塊、全球移動通信系統(tǒng)(global system for mobile communication,GSM)模塊、電機驅動模塊等。STM32F103RCT6作為系統(tǒng)的主控器，采用超聲波傳感器和GSM模塊實現避障和遠程監(jiān)測功能，被測得的數據利用薄膜晶體管(thin film transistor,TFT)液晶屏實時顯示，通過藍牙模塊實現數據無線傳輸功能。利用GSM模塊實現數據短信發(fā)送功能，搖桿電位器結合無線通信模塊NRF24L01遠程遙控小車，最終實現兩輪自平衡遙控小車系統(tǒng)整體功能。小車硬件控制系統(tǒng)如圖3所示。

圖3 硬件控制系統(tǒng)框圖

2.2 主控器電路

選擇STM32F103RCT6作為主控器，這是一款由意法半導體公司推出的基于ARM Cortex-M3內核的新型32位嵌入式微處理器，最高工作頻率可達72 MHz，可以產生多路PWM控制電機。其具有超低功耗、極低成本、高性能、穩(wěn)定性極強等優(yōu)點[4]，提高了對工作模式選擇的快速回應及對控制系統(tǒng)進行模塊化設計，實時完成多項復雜數據的處理。

2.3 數據無線通信電路

文獻[15] 為準確估計內蒙河套灌區(qū)灌溉水入滲補給地下水量，采用試驗研究與數值模擬相結合的方法，分別根據灌水前后地下水位變化和土壤含水率變化計算了灌溉水入滲補給地下水系數，得到作物生育期(最大灌水量183.72 mm)灌溉補給地下水系數為15%，秋澆灌溉(灌水量491.00 mm)補給地下水系數為30%。河套灌區(qū)地下水位埋深相對較淺，通過灌水前后的土壤含水率變化情況和數值模擬結果顯示：灌水2～4 d補給地下水量達到最大，8～10 d后即完成對地下水的入滲補給，不同灌水量灌溉水入滲規(guī)律基本一致，入滲補給量和入滲時間與灌溉水量直接相關。

無線通信模塊可實現兩輪小車與控制平臺系統(tǒng)的實時數據傳輸，實現對兩輪小車的實時監(jiān)測和控制。第一種情況：無線通信模塊采用NRF24L01，它采用通用的四線SPI接口與主控器通信[5]，結合搖桿電位器傳輸的模擬信號通過A/D轉換模塊轉化為數字信號傳遞給主控器，然后主控器及時改變速度給定值來達到小車的遠程遙控功能。第二種情況：無線通信模塊采用HC-05藍牙模塊，通過設置AT指令使其配置為主模塊，與手機藍牙串口助手配對，進行數據無線傳輸到智能手機上，可將數據導出。

2.4 MPU6050模塊電路

姿態(tài)采集模塊采用MPU6050。該模塊是一款整合性的六軸運動處理器。它整合三軸陀螺儀和三軸加速度器數據，以實時獲取小車的當前運行姿態(tài)；然后通過卡爾曼濾波對測得的Y軸角速度和角加速度濾波和解耦，選定正確的狀態(tài)變量，從而得到準確穩(wěn)定的傾角和傾角速度；將位姿信息經外部設備電路傳給主控器，避免了陀螺儀與加速度器組合時的軸間差的問題。

2.5 A/D轉換電路

采用A/D轉換器芯片HX711，它內部集成穩(wěn)壓電源、片內時鐘振蕩器等其他同類型芯片所需要的外圍電路，具有集成度高、響應速度快及抗干擾性強等優(yōu)點，獨特之處是所有控制信號由管腳驅動，無需對芯片內部的寄存器編程操作。輸入開關設定通道A，模擬差分輸入直接與橋式傳感器的差分輸出相接，經過128倍增益放大后，差分輸出電壓傳輸給主控器。

2.6 GSM短信模塊電路

采用型號YD-SIM900A的GSM短信模塊，以短消息形式對指定手機進行數據傳輸。基于GSM模塊的短消息功能的串行口，可以用來控制指定的手機快速、可靠地實現個人用戶信息處理等功能。該短信系統(tǒng)由控制中心和顯示屏組成，控制中心實時將最新接收的信息內容發(fā)送到顯示屏，構成控制中心與顯示屏端的通信鏈路[6]。主控器通過與GSM短信息有關的AT指令給指定手機發(fā)送被測數據信息，實現遠程監(jiān)測小車系統(tǒng)當前運行狀態(tài)的功能。

2.7 超聲波避障電路

(1)

式中：V為小車行駛速度；L為小車行駛距離。

進而主控器判斷小車是否在安全距離范圍之內，再進一步有效調節(jié)控制車輪的轉向。

由于超聲波是一種聲波，其聲速與溫度有關，如溫度每升高1 ℃，傳播速度增加約0.6 m/s。綜合考慮溫度補償，利用溫度傳感器DS18B20進行測量，以避免溫度因素對測量的誤差影響，實現了速度的精準測量。若現場溫度為W時，超聲波傳播速度的計算公式[9]為：

V≈331.5+0.607W

(2)

得到超聲波傳播速度后，再精確計算超聲波模塊距離障礙物的距離。

3 系統(tǒng)軟件設計方案

3.1 線性PID控制算法

采用線性PID控制算法，保證系統(tǒng)處于閉環(huán)控制狀態(tài)，并準確達到設置的平衡狀態(tài)。直流電機的速度閉環(huán)控制和小車的傾角閉環(huán)控制，共同構成雙閉環(huán)控制系統(tǒng)。PID算法的輸入是傳感器獲取的角度和角速度值的偏差值，輸出是電機的PWM占空比值，從而控制電機轉速大小，進而調整角度值的偏差，使得角度值趨近系統(tǒng)平衡零點[10]。PID控制器是一種線性控制器，且整個系統(tǒng)中存在誤差計算，則采用增量式PID控制算法，其控制公式為：

(3)

式中：Kp為比例系數；TI為積分時間常數；TD為微分時間常數；u0為系統(tǒng)初始輸出值；e(k)為k時刻偏差值；u(k)為k時刻PID控制器的輸出，T為采樣周期。

3.2 卡爾曼濾波融合算法

卡爾曼濾波的基本思想是：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和現時刻的觀測值來更新對狀態(tài)變量的估計，從而得出現時刻的估計值[11]?？柭鼮V波原理如圖4所示。

圖4 卡爾曼濾波原理圖

選用MPU6050作為兩輪小車姿態(tài)采集傳感器，由于傳感器采用數字I2C接口讀取數據，讀取的數據比A/D轉換器采集的數據會更精準穩(wěn)定。MPU6050傳感器采集回來的數據具有較大的擾動，小車控制系統(tǒng)不應該直接采用捕獲的數據，需經卡爾曼濾波算法進行數據融合處理，最終準確計算小車的傾角θ。已知傾角速度和傾角存在導數關系，θ可作為狀態(tài)向量；加速度計估計陀螺儀的偏差δ，將δ作為另一狀態(tài)向量[12]，則有：

(4)

式中：ωgyro為陀螺儀測量值；ωg為陀螺儀的測量噪聲；θacce為加速度計測量值轉化后的角度；ωa為加速度計的測量噪聲；ωa和ωg為相互獨立的零均值高斯白噪聲。

4 結果分析

4.1 傾角測量與調試

當小車傾斜時，由受力分析可知，重力加速度g在Y軸方向有加速度分量，于是Y軸輸出電壓不為0，則電壓變化量Δu為：

Δu=kgsinθ≈kgθ

(5)

由電壓變化量Δu得出小車傾角θ的度數。但小車運動過程中產生的信號有很大干擾，還需要和陀螺儀測量的傾角速度進行積分得出角度后對比校正，最終得出穩(wěn)定準確的傾角值。多次實際調試，角度控制中比例控制參數等于155，微分控制參數等于0.7。

4.2 速度測量與調試

將計數器安裝在電機輸出軸的光電編碼盤上，在速度控制周期間隔100 ms內讀取出光碼盤脈沖信號的個數進行累加，從而計算出電機的轉動速度。為了減小安裝光碼盤帶來的誤差，小車速度采用兩個電機速度的平均值。速度控制中包括比例和微分控制參數，而比例和微分定義是指反饋速度而言，如果相對于車模的反饋位置(速度的積分量)，這兩個參數可分別對應稱作積分和比例控制參數。多次實際調試，速度控制中積分控制參數等于0.18，比例控制參數等于3.2。

5 結束語

本文詳細闡述了兩輪自平衡遙控小車控制系統(tǒng)的總體設計方案，分析了兩輪小車自平衡原理、遙控原理和三個運動子系統(tǒng)，逐步完成了硬件電路和軟件程序的設計。采用線性PID控制算法和卡爾曼濾波，實現了小車的遠程遙控、監(jiān)測和避障功能。兩輪遙控小車可作為電子設計作品、試驗研究模型、高檔玩具等，也可適用于狹小空間探索、復雜地形偵查、危險物品運輸等惡劣環(huán)境?；诳刂谱云胶饫碚撛O計的小車具有非常廣闊的應用前景。

[1] 袁帥.兩輪自平衡機器人系統(tǒng)的設計[D].北京:北京交通大學,2012.

[2] 綦聲波,張玲.“飛思卡爾”杯智能車設計與實踐[M].北京:北京航空航天大學出版社,2015.

[3] 楊建民,趙玫,楊洪勇,等.電磁導航智能尋跡小車方向控制算法的分析與應用[J].科學技術與工程,2016,16(33):71-78.

[4] 劉波文,孫巖.嵌入式實時操作系統(tǒng)uCOS-II經典實例—基于STM32處理器[M].2版.北京:北京航空航天大學出版社,2014.

[5] 王元哲,扈宏杰.磁導航智能車系統(tǒng)的設計與實現[J].自動化儀表,2011,32(11):5-8.

[6] 蔡紅娟,翟晟,蔡苗.基于STM32的GSM智能家居控制系統(tǒng)設計[J].自動化技術與應用,2013,32(8):37-40.

[7] 朱志強,張瀟宇,王林童,等.基于Arduino+LabVIEW的高精度超聲波測距系統(tǒng)設計[J].單片機與嵌入式系統(tǒng)應用,2016,16(9):49-51.

[8] 欒祿祥.室內移動機器人機器視覺定位系統(tǒng)的設計[J].自動化儀表,2017,38(2):49-52.

[9] 胡延蘇,高昂.超聲波測距誤差分析及校正研究[J].計算機測量與控制,2015,23(8):2820-2823.

[10]舒英利,寇磊,苑全德,等.雙超聲波數據融合的兩輪機器人平衡行進策略研究[J].自動化儀表,2017,38(2):36-40.

[11]張團善,何穎.卡爾曼濾波在兩輪自平衡代步車姿態(tài)檢測中的應用[J].單片機與嵌入式系統(tǒng)應用,2014,14(5):33-35.

[12]師樹恒,曹陽,師素娟.一種直立智能循跡小車的設計[J].自動化儀表,2014,35(2):66-68.

DesignofTwo-WheeledSelf-BalancingRemoteControlCarBasedonKalmanFilter

LU Chengqiang,ZENG Jie,LI Qianzhen

(School of Electrical and Information,Dalian Jiaotong University,Dalian 116028,China)

For the inadequacies of the existing two-wheeled self-balancing remote control car system, such as the poor stability, the single realization function and the large swing range of car body, an improved scheme of self-balancing remote control car is proposed.Firstly, using STM32F103 microcontroller as the main controller, transplanting the real-time operation system μC/OS-II, choosing DC motors, sensors and external circuits, and hardware control part of the two-wheeled self balancing car is designed.Secondly, MPU6050 module is used to obtain the current running posture of the car in real time, and kalman filter is given to deal with the posture information to transmit to the main controller.Then the linear PID controller is employed to adjust the motor speed.And combined with double close-loop controller, the balance of the car walking upright is achieved.Thirdly, the rocker potentiometer combined with wireless communication module can remote control car.Integrating ultrasonic sensors and GSM module, some functions can be realized, for example, obstacle avoiding and remote monitoring.Human-computer interaction function is realized through the TFT color LCD screen and bluetooth mobile phone client.Finally, the experiment results demonstrate that the car runs extremely smoothly and realizes the diversitied function, and the anti-interference ability is strong.It has the features of high accuracy and high stability, and the expansion of self-balancing remote control theory is realized.This possesses wide application prospects in robotics field.

STM32F103; Kalman filter; Linear PID; Double close-loop controller; Wireless communication; Remote control

TH-39; TP249

： A

10.16086/j.cnki.issn1000-0380.201709011

修改稿收到日期:2016-12-09

路成強(1993—)，男，在讀碩士研究生，主要從事嵌入式系統(tǒng)開發(fā)與應用、智能機器人及控制技術方向的研究。 E-mail:2305999283@qq.com。 曾潔(通信作者)，男，碩士，教授，主要從事嵌入式系統(tǒng)開發(fā)與應用、智能交通與車聯網技術方向的研究。 E-mail:zyz@djtu.edu.cn。