基于LQR 控制的兩輪自平衡小車設計

林壽光

兩輪自平衡小車具有便攜、環保、操作簡單、成本低等優點，常應用在空間探索、短途運輸、商場和機場治安巡邏等領域［1?2］.本文提出了一種基于LQR 控制的自平衡小車設計軟硬件方案，并利用Simulink 平臺進行仿真分析，搭建硬件平臺進行實際測試.

1 系統整體方案設計

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

圖1 系統整體設計框圖

2 硬件電路設計

兩輪自平衡小車的硬件分為機械結構和硬件控制電路兩部分.機械結構部分設計參考目前市面上較為成熟的自平衡車機械結構.主要硬件電路包括：主控器模塊、姿態檢測模塊、電機及驅動電路、避障模塊、藍牙通信模塊和顯示模塊.

2.1 主控器模塊

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

圖2 STM32F103 最小系統圖

2.2 姿態檢測模塊

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

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

圖3 MPU6050 模塊及接口電路圖

2.3 電機及驅動電路

2.3.1 電機選型

保持穩定是自平衡小車的主要功能.為了保證自平衡小車能夠長時間在不動或低速的狀態下工作，需要一款在低速穩定性較高，又能輸出較大轉矩的電機［5］.通過比較測試，本設計采用直流減速電機.減速電機型號選用MG513 型，集成500 線GMR 編碼器，堵轉電流最大不超過3 A.

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

2.3.2 電機驅動模塊

本設計采用AT8236 模塊作為電機驅動，峰值電流高達6 A，遠高于TB6612 型.可通過對輸入信號進行脈寬調制（PWM）控制電機轉速.有正轉、反轉、制動和停止4 種控制模式.內部包含過流保護、短路保護、欠壓鎖定和過溫保護.電機驅動及接口電路如圖4 所示.

圖4 電機驅動及接口電路圖

2.4 避障模塊

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

圖5 雷達接口電路

2.5 藍牙通信模塊

系統通信模塊使用BT05 藍牙串口模塊.該模塊主從模式一體，可以和帶有藍牙設備的PC 端、手機端進行通信連接.具有成本低、體積小、功耗低、收發靈敏性高等優點.

2.6 顯示模塊

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

3 軟件設計

3.1 程序總體設計

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

主程序：主要完成系統的初始化，實現功能模塊的調度.初始化包括時鐘初始化、定時器初始化、中斷初始化及串口、編碼器、ADC、OLED、傳感器等外設模塊初始化.

中斷處理程序：模塊功能主要在中斷服務中完成，包括姿態傳感器數據采集處理、電機編碼器數據處理、電壓檢測、控制算法、雷達控制等.

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

表1 軟件處理任務實時性分析

圖6 自平衡小車軟件總體設計

主要模塊設計說明如下：

姿態控制.姿態控制是軟件系統的核心部分.通過先解析控制端發送的運動控制命令，然后利用MPU6050 模塊測到的傾角值和角加速度值作為平衡系統的輸入進行運算，最后將控制系統的輸出經電機驅動模塊轉變為PWM 波施加在電機上，用以控制平衡車正常運行.本設計采用LQR 作為姿態平衡控制算法.

卡爾曼濾波算法.該算法使用系統狀態方程預測當前的值，使用傳感器測出來的觀測值對預測值進行修正.系統以MPU6050 測量到的信號作為系統觀測值，首先對傾角和角加速度做先驗估計，利用系統噪聲對系統預測方差進行預測.利用預測方差值與角度測量噪聲，計算當前系統的Kalman 增益.通過上一狀態的最優角度、最優角加速度、系統卡爾曼增益對預測值進行更新，得到當前狀態的最優角度及角速度解.最后更新協方差方程.

編碼器測速.本設計的電機自帶編碼器測速模塊能輸出電機的實時速度.當電機旋轉時，編碼器會輸出若干個脈沖方波.編碼器測速是通過定時器的輸入捕獲模式實現，當開啟定時器的輸入捕獲模式后，對編碼器接口進行高電平捕獲，捕獲一個高電平，定時器計數器加1，最后讀取定時器計數寄存器中的計算值，計算得到小車的實際車速.

藍牙功能.藍牙模塊用于APP 與小車之間的通信，能實現控制小車的運動轉向與查看數據、調參等功能.藍牙模塊與手機的通信通過無線藍牙實現，與主控芯片的通信使用串口3 連接.

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

雷達功能.雷達在上電時會自動傳回數據包，不需要進行額外設置，只需要在串口中斷中進行接收即可.一幀數據有16 個點的位置信息，包括角度和距離.

3.2 控制算法設計

本設計采用LQR 作為姿態平衡控制算法，需要先推導出小車的數學模型，再代入系統狀態方程解出反饋矩陣.

3.2.1 自平衡車數學模型

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

表2 兩輪自平衡小車具體參數

圖7 兩輪平衡小車實物

圖8 小車正向運動模型

圖9 小車轉向運動模型

小車的正向運動表示為：

小車的轉向運動表示為：

由式（1）、式（2）、式（3）可得系統的狀態方程：

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

矩陣中的元素為：

3.2.2 LQR 控制算法

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

LQR（Linear Quadratic Regulator），即線性二次調節器.是根據系統的狀態空間模型設計優化的動態控制器.一旦系統的狀態偏離平衡狀態，LQR 可以在不消耗過多能量的情況下通過狀態調整快速回歸平衡狀態［9］.

設系統（要求系統完全能控）的狀態空間表達式為：

結合上述自平衡小車的數學模型及公式，式中：

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

使二次型目標J函數達到極小值

式中：Q為n×n維半正定加權矩陣，R為n×n維正定加權矩陣.在工程實際運用中，Q和R是對稱矩陣且常取對角陣.

為了使二次型目標函數J最小，需要計算出反饋矩陣K［10］：

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

4 系統仿真與實際控制實驗

4.1 系統仿真

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

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

①根據實際選用小車的參數，主要程序如下所示：

②代入所選小車的實際參數，運行程序結果如下：

③然后利用Matlab/Simulink 對其進行仿真，仿真框圖如圖10 所示.

圖10 Simulink 仿真框圖

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

圖11 Simulink 仿真輸出波形圖

4.2 實際控制實驗

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

其中LQR控制對應的C語言代碼如下所示：

LQR 控制器的效果是使所有狀態變量都為0，為了實現App 控制的便捷，引入“Tar?get_x_speed”“Target_angle_x”“Target_gyro_z”三個量，分別表示目標前進速度、校正后的平衡中值、目標轉向速度.以App 控制小車前進為例，此時，目標是使得x_speed=Target_x_speed≠0，而LQR 控制器的效果是使所有狀態變量都為0，那么，可以把“x_speed?Target_x_speed”作為新的狀態變量，使它變為0 即等價于使得x_speed=Target_x_speed.

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

圖12 小車硬件平臺

圖13 APP波形監控

圖14 上位機顯示界面

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

5 結語

根據兩輪自平衡小車的數學模型，搭建了硬件平臺，利用LQR 控制算法對小車進行姿態控制.通過Simulink 對系統仿真，并對硬件平臺進行了實際驗證測試，結果表明LQR控制算法下的自平衡小車能夠在外部干擾的情況下，短時間內進行姿態糾正，重新恢復到平衡狀態，具有穩定性高、抗干擾能力強、調節速度快等特點.同時具有避障、跟隨、手機APP 遙控等功能，具有應用推廣價值.