林壽光

兩輪自平衡小車具有便攜、環(huán)保、操作簡(jiǎn)單、成本低等優(yōu)點(diǎn)，常應(yīng)用在空間探索、短途運(yùn)輸、商場(chǎng)和機(jī)場(chǎng)治安巡邏等領(lǐng)域［1?2］.本文提出了一種基于LQR 控制的自平衡小車設(shè)計(jì)軟硬件方案，并利用Simulink 平臺(tái)進(jìn)行仿真分析，搭建硬件平臺(tái)進(jìn)行實(shí)際測(cè)試.

1 系統(tǒng)整體方案設(shè)計(jì)

本系統(tǒng)以STM32F103RCT6 處理器為主控芯片，采用MPU6050 陀螺儀模塊獲取角度等傳感數(shù)據(jù)，運(yùn)用卡爾曼濾波算法進(jìn)行數(shù)據(jù)處理.然后通過LQR 控制算法，計(jì)算出左右兩輪最終的PWM 占空比調(diào)整姿態(tài)，達(dá)到控制小車平衡的效果.使用OLED 顯示小車參數(shù)，通過手機(jī)APP 和上位機(jī)實(shí)現(xiàn)人機(jī)交互，控制小車的運(yùn)動(dòng)姿態(tài).同時(shí)為了增加功能的多樣性，使用N10 激光雷達(dá)模塊實(shí)現(xiàn)避障、跟隨功能；通過讀取電池電量，在小車電壓過低時(shí)停止小車運(yùn)動(dòng).為了使小車達(dá)到動(dòng)態(tài)平衡，小車通過定時(shí)采集編碼器和陀螺儀的數(shù)據(jù)，獲取小車的角度和左右兩輪的轉(zhuǎn)動(dòng)情況，動(dòng)態(tài)調(diào)整小車姿態(tài).系統(tǒng)整體設(shè)計(jì)方案如圖1 所示.

圖1 系統(tǒng)整體設(shè)計(jì)框圖

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

兩輪自平衡小車的硬件分為機(jī)械結(jié)構(gòu)和硬件控制電路兩部分.機(jī)械結(jié)構(gòu)部分設(shè)計(jì)參考目前市面上較為成熟的自平衡車機(jī)械結(jié)構(gòu).主要硬件電路包括：主控器模塊、姿態(tài)檢測(cè)模塊、電機(jī)及驅(qū)動(dòng)電路、避障模塊、藍(lán)牙通信模塊和顯示模塊.

2.1 主控器模塊

控制器采用ST 公司的STM32F103?RCT6芯片，它是基于ARM 公司設(shè)計(jì)的Cortex?M3 內(nèi)核，最高工作頻率為72 MHz，具有ADC、USB、DMA、IIC、SPI、USART 等外圍設(shè)備及通信接口.定時(shí)器可產(chǎn)生多路PWM 用以控制電機(jī)，ADC 可以讀取電池電壓.具有高性能、低功耗、接口豐富等特點(diǎn).本設(shè)計(jì)主控器的最小系統(tǒng)如圖2 所示.

圖2 STM32F103 最小系統(tǒng)圖

2.2 姿態(tài)檢測(cè)模塊

采用MPU6050 模塊作為小車的姿態(tài)檢測(cè).芯片整合了3 軸陀螺儀和3 軸加速度計(jì)，通過內(nèi)部的DMP 檢測(cè)，使用庫函數(shù)得到四元數(shù)，再根據(jù)公式即可解算出姿態(tài)角，最后通過IIC 通信送到主控芯片.

傳感器測(cè)量的數(shù)據(jù)存在諸多不確定性，比如有很多噪聲會(huì)造成陀螺儀的漂移、車體擺動(dòng)等，對(duì)小車加速度計(jì)產(chǎn)生干擾，為了得到更確切的車體傾角值，本設(shè)計(jì)采用卡爾曼濾波算法對(duì)陀螺儀與加速度計(jì)的輸出值進(jìn)行數(shù)據(jù)融合［3?4］.MPU6050 通過IIC 與主控芯片實(shí)現(xiàn)通信，模塊及接口電路圖如圖3 所示.

圖3 MPU6050 模塊及接口電路圖

2.3 電機(jī)及驅(qū)動(dòng)電路

2.3.1 電機(jī)選型

保持穩(wěn)定是自平衡小車的主要功能.為了保證自平衡小車能夠長(zhǎng)時(shí)間在不動(dòng)或低速的狀態(tài)下工作，需要一款在低速穩(wěn)定性較高，又能輸出較大轉(zhuǎn)矩的電機(jī)［5］.通過比較測(cè)試，本設(shè)計(jì)采用直流減速電機(jī).減速電機(jī)型號(hào)選用MG513 型，集成500 線GMR 編碼器，堵轉(zhuǎn)電流最大不超過3 A.

電機(jī)的速度使用分辨率為500 的GMR 編碼器輸出AB 相進(jìn)行測(cè)量，電機(jī)減速比為1∶30，使用編碼器的計(jì)數(shù)測(cè)量功能，設(shè)置為一個(gè)周期計(jì)數(shù)4 次，可實(shí)現(xiàn)輪子轉(zhuǎn)一圈輸出60 000 個(gè)計(jì)數(shù).測(cè)量精度是13 線霍爾編碼器的38 倍，測(cè)量低速時(shí)比霍爾編碼器效果更好.

2.3.2 電機(jī)驅(qū)動(dòng)模塊

本設(shè)計(jì)采用AT8236 模塊作為電機(jī)驅(qū)動(dòng)，峰值電流高達(dá)6 A，遠(yuǎn)高于TB6612 型.可通過對(duì)輸入信號(hào)進(jìn)行脈寬調(diào)制（PWM）控制電機(jī)轉(zhuǎn)速.有正轉(zhuǎn)、反轉(zhuǎn)、制動(dòng)和停止4 種控制模式.內(nèi)部包含過流保護(hù)、短路保護(hù)、欠壓鎖定和過溫保護(hù).電機(jī)驅(qū)動(dòng)及接口電路如圖4 所示.

圖4 電機(jī)驅(qū)動(dòng)及接口電路圖

2.4 避障模塊

本設(shè)計(jì)通過N10 系列激光雷達(dá)實(shí)現(xiàn)小車的避障、跟隨功能.采用TOF（time of flight）方案測(cè)距，通過測(cè)量調(diào)制激光的發(fā)射、返回時(shí)間差測(cè)量物體與傳感器的相對(duì)距離.測(cè)量精度3 cm，角度分辨率是0.8°，具有360°全方位掃描角度，支持建圖、導(dǎo)航等ROS 核心功能.掃描角度和抗干擾能力遠(yuǎn)遠(yuǎn)超過超聲波避障模塊，可以讓避障、跟隨功能效果更穩(wěn)定.雷達(dá)接口電路如圖5 所示，其中1 腳REC 為GPS 經(jīng)緯度時(shí)分秒，2 腳PPS 為GPS 秒信號(hào)，3 腳和4腳為串口通信引腳.

圖5 雷達(dá)接口電路

2.5 藍(lán)牙通信模塊

系統(tǒng)通信模塊使用BT05 藍(lán)牙串口模塊.該模塊主從模式一體，可以和帶有藍(lán)牙設(shè)備的PC 端、手機(jī)端進(jìn)行通信連接.具有成本低、體積小、功耗低、收發(fā)靈敏性高等優(yōu)點(diǎn).

2.6 顯示模塊

OLED 模塊配置IIC/SPI 接口，使用I/O 數(shù)量少，顯示視角廣、對(duì)比度高、功耗低、體積小，對(duì)比于LCD 顯示屏，更適合應(yīng)用于小車.因此，系統(tǒng)顯示模塊采用0.96 寸OLED 顯示屏，用以顯示小車的運(yùn)行電壓、傾角、電機(jī)速度和避障距離等.

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

3.1 程序總體設(shè)計(jì)

軟件設(shè)計(jì)采用模塊化設(shè)計(jì)，方便代碼的可移植性和修改，分為主程序和中斷處理程序.

主程序：主要完成系統(tǒng)的初始化，實(shí)現(xiàn)功能模塊的調(diào)度.初始化包括時(shí)鐘初始化、定時(shí)器初始化、中斷初始化及串口、編碼器、ADC、OLED、傳感器等外設(shè)模塊初始化.

中斷處理程序：模塊功能主要在中斷服務(wù)中完成，包括姿態(tài)傳感器數(shù)據(jù)采集處理、電機(jī)編碼器數(shù)據(jù)處理、電壓檢測(cè)、控制算法、雷達(dá)控制等.

兩輪自平衡小車是一個(gè)不穩(wěn)定的系統(tǒng)［6］，為了能夠保證系統(tǒng)的正常運(yùn)行，處理器需要不斷檢測(cè)車身的姿態(tài)信息和左右車輪轉(zhuǎn)速，同時(shí)還需要對(duì)串口信息進(jìn)行處理，判斷自平衡小車需要執(zhí)行的動(dòng)作.除此之外，微處理器還需要對(duì)電池電量、OLED 顯示、激光雷達(dá)測(cè)距等任務(wù)進(jìn)行處理.根據(jù)各個(gè)任務(wù)對(duì)實(shí)時(shí)性的要求，完成自平衡小車的主要模塊分析、分類，以及程序執(zhí)行間分配，如表1 所示.根據(jù)上述對(duì)自平衡車的任務(wù)分析，設(shè)計(jì)自平衡車軟件總體框架，如圖6 所示.

表1 軟件處理任務(wù)實(shí)時(shí)性分析

圖6 自平衡小車軟件總體設(shè)計(jì)

主要模塊設(shè)計(jì)說明如下：

姿態(tài)控制.姿態(tài)控制是軟件系統(tǒng)的核心部分.通過先解析控制端發(fā)送的運(yùn)動(dòng)控制命令，然后利用MPU6050 模塊測(cè)到的傾角值和角加速度值作為平衡系統(tǒng)的輸入進(jìn)行運(yùn)算，最后將控制系統(tǒng)的輸出經(jīng)電機(jī)驅(qū)動(dòng)模塊轉(zhuǎn)變?yōu)镻WM 波施加在電機(jī)上，用以控制平衡車正常運(yùn)行.本設(shè)計(jì)采用LQR 作為姿態(tài)平衡控制算法.

卡爾曼濾波算法.該算法使用系統(tǒng)狀態(tài)方程預(yù)測(cè)當(dāng)前的值，使用傳感器測(cè)出來的觀測(cè)值對(duì)預(yù)測(cè)值進(jìn)行修正.系統(tǒng)以MPU6050 測(cè)量到的信號(hào)作為系統(tǒng)觀測(cè)值，首先對(duì)傾角和角加速度做先驗(yàn)估計(jì)，利用系統(tǒng)噪聲對(duì)系統(tǒng)預(yù)測(cè)方差進(jìn)行預(yù)測(cè).利用預(yù)測(cè)方差值與角度測(cè)量噪聲，計(jì)算當(dāng)前系統(tǒng)的Kalman 增益.通過上一狀態(tài)的最優(yōu)角度、最優(yōu)角加速度、系統(tǒng)卡爾曼增益對(duì)預(yù)測(cè)值進(jìn)行更新，得到當(dāng)前狀態(tài)的最優(yōu)角度及角速度解.最后更新協(xié)方差方程.

編碼器測(cè)速.本設(shè)計(jì)的電機(jī)自帶編碼器測(cè)速模塊能輸出電機(jī)的實(shí)時(shí)速度.當(dāng)電機(jī)旋轉(zhuǎn)時(shí)，編碼器會(huì)輸出若干個(gè)脈沖方波.編碼器測(cè)速是通過定時(shí)器的輸入捕獲模式實(shí)現(xiàn)，當(dāng)開啟定時(shí)器的輸入捕獲模式后，對(duì)編碼器接口進(jìn)行高電平捕獲，捕獲一個(gè)高電平，定時(shí)器計(jì)數(shù)器加1，最后讀取定時(shí)器計(jì)數(shù)寄存器中的計(jì)算值，計(jì)算得到小車的實(shí)際車速.

藍(lán)牙功能.藍(lán)牙模塊用于APP 與小車之間的通信，能實(shí)現(xiàn)控制小車的運(yùn)動(dòng)轉(zhuǎn)向與查看數(shù)據(jù)、調(diào)參等功能.藍(lán)牙模塊與手機(jī)的通信通過無線藍(lán)牙實(shí)現(xiàn)，與主控芯片的通信使用串口3 連接.

電池電壓檢測(cè).平衡小車使用12 V 的航模電池.電池電壓通過ADC 進(jìn)行測(cè)量，由于STM32 芯片最大只能轉(zhuǎn)換3.3 V 的模擬電壓，所以需將電壓通過電阻分壓（本設(shè)計(jì)分壓值采用1/11），再送到芯片進(jìn)行測(cè)量.最終測(cè)到的電壓需乘以11.

雷達(dá)功能.雷達(dá)在上電時(shí)會(huì)自動(dòng)傳回?cái)?shù)據(jù)包，不需要進(jìn)行額外設(shè)置，只需要在串口中斷中進(jìn)行接收即可.一幀數(shù)據(jù)有16 個(gè)點(diǎn)的位置信息，包括角度和距離.

3.2 控制算法設(shè)計(jì)

本設(shè)計(jì)采用LQR 作為姿態(tài)平衡控制算法，需要先推導(dǎo)出小車的數(shù)學(xué)模型，再代入系統(tǒng)狀態(tài)方程解出反饋矩陣.

3.2.1 自平衡車數(shù)學(xué)模型

兩輪自平衡小車的結(jié)構(gòu)主要由車體和雙輪兩部分組成，本設(shè)計(jì)中使用的小車實(shí)物及受力分析如圖7、圖8 和圖9 所示，通過分析兩輪自平衡車的車輪、車身等各部分模型，建立它們的聯(lián)系，得出整體的數(shù)學(xué)模型［7?8］.模型分析涉及的參數(shù)見表2.

表2 兩輪自平衡小車具體參數(shù)

圖7 兩輪平衡小車實(shí)物

圖8 小車正向運(yùn)動(dòng)模型

圖9 小車轉(zhuǎn)向運(yùn)動(dòng)模型

小車的正向運(yùn)動(dòng)表示為：

小車的轉(zhuǎn)向運(yùn)動(dòng)表示為：

由式（1）、式（2）、式（3）可得系統(tǒng)的狀態(tài)方程：

其中：vLO表示左輪無摩擦?xí)r線速度的大小，單位：rad/s；vRO表示右輪無摩擦?xí)r線速度的大小，單位：rad/s.

矩陣中的元素為：

3.2.2 LQR 控制算法

姿勢(shì)平衡控制是自平衡小車系統(tǒng)的核心，良好的控制算法能夠讓系統(tǒng)的性能更加穩(wěn)定和安全.常見的控制算法有PID 算法、LQR 算法等，本方采用LQR 算法.

LQR（Linear Quadratic Regulator），即線性二次調(diào)節(jié)器.是根據(jù)系統(tǒng)的狀態(tài)空間模型設(shè)計(jì)優(yōu)化的動(dòng)態(tài)控制器.一旦系統(tǒng)的狀態(tài)偏離平衡狀態(tài)，LQR 可以在不消耗過多能量的情況下通過狀態(tài)調(diào)整快速回歸平衡狀態(tài)［9］.

設(shè)系統(tǒng)（要求系統(tǒng)完全能控）的狀態(tài)空間表達(dá)式為：

結(jié)合上述自平衡小車的數(shù)學(xué)模型及公式，式中：

LQR 設(shè)計(jì)就是為了找到一個(gè)最優(yōu)控制量u(t)，用來保持兩輪自平衡小車的平衡，

使二次型目標(biāo)J函數(shù)達(dá)到極小值

式中：Q為n×n維半正定加權(quán)矩陣，R為n×n維正定加權(quán)矩陣.在工程實(shí)際運(yùn)用中，Q和R是對(duì)稱矩陣且常取對(duì)角陣.

為了使二次型目標(biāo)函數(shù)J最小，需要計(jì)算出反饋矩陣K［10］：

其中：P是Riccati方程ATP+PA+Q?PBR?1BTP=0的解.

4 系統(tǒng)仿真與實(shí)際控制實(shí)驗(yàn)

4.1 系統(tǒng)仿真

LQR 的Q陣和R陣系數(shù)的選取直接影響系統(tǒng)性能［11］，而Q和R的選取一般無規(guī)律可循，通常采用仿真試錯(cuò)法進(jìn)行設(shè)計(jì).LQR 的Q陣和R陣確定后，反饋矩陣K一般不用手動(dòng)計(jì)算，可以直接通過Matlab 中的lqr（）函數(shù)解出.

通過自平衡小車的數(shù)學(xué)模型及LQR 控制算法，由式（4）～式（9）代入程序.這里加權(quán)矩陣R不能太小，否則會(huì)導(dǎo)致控制量的急劇增大以至于超過系統(tǒng)的執(zhí)行能力；矩陣R也不能太大，否則控制作用太小會(huì)影響控制性能.結(jié)合實(shí)際，矩陣R中對(duì)角線上的元素選為1 較合適，可以根據(jù)實(shí)際情況再進(jìn)行合理的修改.具體步驟如下：

①根據(jù)實(shí)際選用小車的參數(shù)，主要程序如下所示：

②代入所選小車的實(shí)際參數(shù)，運(yùn)行程序結(jié)果如下：

③然后利用Matlab/Simulink 對(duì)其進(jìn)行仿真，仿真框圖如圖10 所示.

圖10 Simulink 仿真框圖

仿真結(jié)果如圖11 所示，由scope 可以看到，當(dāng)給小車一個(gè)10 度的沖擊，車體回到平衡位置所需時(shí)間大約是2 s，移動(dòng)了大約0.13 m，由此可見模型的建立正確且有效.

圖11 Simulink 仿真輸出波形圖

4.2 實(shí)際控制實(shí)驗(yàn)

使用Keil5 編程，實(shí)驗(yàn)小車使用圖7 實(shí)物底盤，以及上述分析的STM32F103 系列主控芯片和模塊搭建而成.

其中LQR控制對(duì)應(yīng)的C語言代碼如下所示：

LQR 控制器的效果是使所有狀態(tài)變量都為0，為了實(shí)現(xiàn)App 控制的便捷，引入“Tar?get_x_speed”“Target_angle_x”“Target_gyro_z”三個(gè)量，分別表示目標(biāo)前進(jìn)速度、校正后的平衡中值、目標(biāo)轉(zhuǎn)向速度.以App 控制小車前進(jìn)為例，此時(shí)，目標(biāo)是使得x_speed=Target_x_speed≠0，而LQR 控制器的效果是使所有狀態(tài)變量都為0，那么，可以把“x_speed?Target_x_speed”作為新的狀態(tài)變量，使它變?yōu)? 即等價(jià)于使得x_speed=Target_x_speed.

小車硬件平臺(tái)如圖12 所示，APP 波形界面如圖13 所示，分別顯示X、Y、Z三個(gè)軸的角度值.上位機(jī)顯示界面如圖14 所示，三個(gè)參數(shù)從上而下分別表示平衡小車傾角（單位：°）、雷達(dá)測(cè)距（單位：cm）和供電電池電壓（單位：V）.

圖12 小車硬件平臺(tái)

圖13 APP波形監(jiān)控

圖14 上位機(jī)顯示界面

通過系統(tǒng)仿真及實(shí)物硬件平臺(tái)測(cè)試結(jié)果表明，本設(shè)計(jì)能夠很好地實(shí)現(xiàn)小車的平衡、前進(jìn)、后退及轉(zhuǎn)向等功能，可以在OLED、手機(jī)APP 及上位機(jī)三種渠道顯示參數(shù)，同時(shí)還可以利用激光雷達(dá)模塊實(shí)現(xiàn)避障、追隨和電壓檢測(cè)等功能.硬件平臺(tái)在人為觸碰、負(fù)重改變等不同外部干擾情況下，均能快速恢復(fù)到平衡狀態(tài).

5 結(jié)語

根據(jù)兩輪自平衡小車的數(shù)學(xué)模型，搭建了硬件平臺(tái)，利用LQR 控制算法對(duì)小車進(jìn)行姿態(tài)控制.通過Simulink 對(duì)系統(tǒng)仿真，并對(duì)硬件平臺(tái)進(jìn)行了實(shí)際驗(yàn)證測(cè)試，結(jié)果表明LQR控制算法下的自平衡小車能夠在外部干擾的情況下，短時(shí)間內(nèi)進(jìn)行姿態(tài)糾正，重新恢復(fù)到平衡狀態(tài)，具有穩(wěn)定性高、抗干擾能力強(qiáng)、調(diào)節(jié)速度快等特點(diǎn).同時(shí)具有避障、跟隨、手機(jī)APP 遙控等功能，具有應(yīng)用推廣價(jià)值.