趙保睿，李昆鵬，王昊，蘇明

（天津職業(yè)技術(shù)師范大學(xué)自動(dòng)化與電氣工程學(xué)院，天津，300222）

關(guān)鍵字：STM32;自平衡自行車；PID

0 引言

自行車動(dòng)力平衡特性已經(jīng)被世界各國(guó)學(xué)者研究了上百年，并且由于計(jì)算機(jī)科學(xué)和控制科學(xué)的發(fā)展，自行車機(jī)器人也吸引了越來(lái)越多的學(xué)者參與加入。中國(guó)雖然起步相對(duì)較晚,但近年來(lái)通過(guò)利用試驗(yàn)結(jié)果來(lái)證明理論分析的準(zhǔn)確性和控制算法的可行性,已經(jīng)達(dá)到了良好的成效。例如張宏志、李健、祝洪宇實(shí)現(xiàn)了具有跟隨功能的自平衡自行車。其他兩輪平衡車的研究也有很多成果以供學(xué)習(xí)，袁鑫宏等提出了一種基于MPU6050六軸傳感器的平衡車姿態(tài)檢測(cè)及控制方案；黨新安等提出了一種單片機(jī)控制PID調(diào)節(jié)的自平衡方法；楊高對(duì)雙輪自平衡機(jī)器人控制系統(tǒng)進(jìn)行了研究；王振運(yùn)實(shí)現(xiàn)了一種基于RT-Thread和STM32的雙輪自平衡機(jī)器人；古訓(xùn),陳紅實(shí)現(xiàn)了基于Arduino單片機(jī)的自平衡車。

本設(shè)計(jì)在總結(jié)以上研究結(jié)果的基礎(chǔ)上，以STM32為主控芯片,以集成陀螺儀和加速度計(jì)的MPU6050為姿態(tài)傳感器，接收角度和角速度反饋,以PID算法來(lái)完成自行車直立平衡，并集成循跡避障、遠(yuǎn)程操控功能，使其更加實(shí)用。

1 總體設(shè)計(jì)

本設(shè)計(jì)的自平衡自行車物理結(jié)構(gòu)設(shè)計(jì)模型如圖1物理模型所示。該模型由車架主體、動(dòng)力能源、控制電路板，直流電機(jī)，舵機(jī)、避障，循跡等部分組成。車體使用UG NX模擬設(shè)計(jì)，由可控把手、置物車背、作為驅(qū)動(dòng)輪的后輪、作為從動(dòng)輪的前輪組成。動(dòng)力能量源是一個(gè)12V電池，用于為舵機(jī)和直流電機(jī)供電。舵機(jī)位于車輛前部，用于控制車輛轉(zhuǎn)向與平衡?？刂齐娐稰CB板集主控、Bluetooth、驅(qū)動(dòng)等模塊于一身。避障和跟蹤模塊安裝于車身。

圖1 物理模型

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

2.1 主控制器模塊

主控模塊使用的是STM32系列的C8T6單片機(jī)，該單片機(jī)是基于arm Cortex-M3內(nèi)核的32位控制器，最大工作頻率為72Mhz；其內(nèi)置20KSRAM和65KFLASH，集成了I2CSPIUSART等多種通訊接口；它包含多個(gè)定時(shí)器，可以輸出多個(gè)PWM信號(hào)。本設(shè)計(jì)中，采用SPI進(jìn)行OLED屏幕顯示模塊通訊，采用I2C協(xié)議讀取MPU6050數(shù)據(jù)，采用串口通信方式接受藍(lán)牙模塊數(shù)據(jù)，并輸出PWM波形以控制電機(jī)和舵機(jī)。主控制器模塊電路如圖2主控制器模塊所示。

圖2 主控制器模塊

2.2 姿態(tài)處理模塊

姿態(tài)處理模塊使用的是MPU6050。MPU6050是一個(gè)六軸運(yùn)動(dòng)處理組件，內(nèi)有三軸陀螺儀和三軸加速度計(jì)。本設(shè)計(jì)將MPU6050通過(guò)SCL和SDA接口與STM32相連，其測(cè)量數(shù)據(jù)經(jīng)內(nèi)置AD轉(zhuǎn)換器轉(zhuǎn)換后，通過(guò)I2C總線接口即可與主控制器通訊，實(shí)現(xiàn)姿態(tài)檢測(cè)。檢測(cè)結(jié)果經(jīng)過(guò)計(jì)算后通過(guò)單片機(jī)的PWM引腳產(chǎn)生脈寬調(diào)制信號(hào)使舵機(jī)轉(zhuǎn)動(dòng)來(lái)達(dá)到平衡。姿態(tài)處理模塊電路如圖3姿態(tài)處理模塊所示。

圖3 姿態(tài)處理模塊

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

電機(jī)驅(qū)動(dòng)模塊使用的是TB6612FNG。TB6612FNG是一種只需要一個(gè)外部濾波電容即可驅(qū)動(dòng)電機(jī)的直流電機(jī)驅(qū)動(dòng)器。模塊VM1連接電源電池，VCC連接內(nèi)部5V供電后，即可通過(guò)PWMA、AN1、AN2控制電機(jī)及其轉(zhuǎn)速。電機(jī)驅(qū)動(dòng)模塊電路如圖4電機(jī)驅(qū)動(dòng)模塊所示。

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

2.4 電源電路模塊

電源電路模塊由電源電池和穩(wěn)壓電路組成。電源電池使用的是12V航模電池，可向電壓驅(qū)動(dòng)模塊供電。穩(wěn)壓電路可產(chǎn)生5V和3.3V兩個(gè)電壓，為主控制器模塊（STM32單片機(jī)）、姿態(tài)處理模塊（MPU6050）等模塊供電。電源電路模塊電路如圖5電源電路所示。

圖5 電源電路

2.5 藍(lán)牙串口模塊

藍(lán)牙串口模塊使用國(guó)產(chǎn)bk3231藍(lán)牙模塊。該模塊可以通過(guò)tx和rx兩根信號(hào)線使藍(lán)牙與主控制器模塊通訊，輸送指令以完成對(duì)自行車的遠(yuǎn)程控制。藍(lán)牙串口模塊電路如圖6藍(lán)牙串口模塊所示。

圖6 藍(lán)牙串口模塊

2.6 屏幕顯示模塊

屏幕顯示模塊使用0.96英寸，分辨率為128*64的OLED顯示屏。它以四線SPI方式與主控芯片進(jìn)行通信，可以實(shí)時(shí)顯示電池電壓、自行車傾角、舵機(jī)角度等信息。

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

3.1 平衡程序設(shè)計(jì)

本設(shè)計(jì)將平衡程序分為2個(gè)部分，分別是角度測(cè)量與濾波部分和控制平衡部分。

3.1.1 角度測(cè)量與濾波

本設(shè)計(jì)采用了MPU6050對(duì)自行車進(jìn)行姿態(tài)檢測(cè)，使用加速度計(jì)所測(cè)傾角與陀螺儀所測(cè)傾角兩者進(jìn)行濾波融合的方法來(lái)計(jì)算角度。

首先利用加速度計(jì)測(cè)量。下面以X軸為例。

第一步，計(jì)算X軸方向上的加速度：

式中：β為自行車傾角，g 是重力加速度，a是自行車加速度，可由下式計(jì)算得出：

式中：d 為 MPU6050到自行車電機(jī)軸的距離，á為運(yùn)行時(shí)角加速度，a0為運(yùn)行時(shí)X軸方向線加速度，可由MPU6050測(cè)量得出。

第二步，利用a t an2 ( x , y )函數(shù)計(jì)算傾角：

式中，Accel_Z 為Z軸方向加速度，可近似為重力加速度g。

然后利用陀螺儀測(cè)量，同樣以X軸為例。

陀螺儀輸出的是角速度，需將輸出的原始數(shù)據(jù)轉(zhuǎn)換為單位是°/s的數(shù)據(jù)，可由下式得：

式中：k0與MPU6050初始化時(shí)的量程有關(guān)，本設(shè)計(jì)中為16.4。

從陀螺儀測(cè)量所得的角速度信息經(jīng)積分后即可得到角度：

式中：Gyro 是先前經(jīng)過(guò)計(jì)算轉(zhuǎn)化后的角速度，dt為采樣周期，本設(shè)計(jì)中為0.005s。

最后對(duì)加速度計(jì)所測(cè)傾角與陀螺儀所測(cè)傾角兩者進(jìn)行一階互補(bǔ)濾波融合：

式中：angle1為融合后的角度，angle為加速度計(jì)所測(cè)量角度值，(G yro* dt )為陀螺儀所測(cè)量角度值，dt為采樣周期，本設(shè)計(jì)采用0.005s。k為濾波系數(shù)，本設(shè)計(jì)采用0.02。由此式可得出較為準(zhǔn)確的自行車傾角。

3.1.2 平衡控制

平衡控制部分采用PID控制，通過(guò)直立環(huán)和速度環(huán)最終作用于舵機(jī)和電機(jī)，使自行車保持平衡。

直立環(huán)使用PD控制，方程如下所示：

式中：Gyro 為上文陀螺儀所測(cè)得角速度，Balance_Kp和Balance_Kd 分別為比例系數(shù)和微分系數(shù)。Bias為當(dāng)前角度與機(jī)械中值的偏差，可由下式得出：

式中，angle1為計(jì)算得出的自行車傾角，x為設(shè)計(jì)的機(jī)械中值，于調(diào)試部分給出。

速度環(huán)采用PI控制，方程如下所示：

式中：Encoder為讀取的速度偏差經(jīng)一階低通濾波后所得值，這樣可以降低速度值變化速率，防止直立環(huán)被速度環(huán)干擾，Encoder _ Integral 為Encoder在采樣周期內(nèi)積分所得位移，Velocity _ Kp 和Velocity _ Ki 分別為比例系數(shù)和積分系數(shù)。

本設(shè)計(jì)中舵機(jī)采用PI控制：

式中：Encoder1可由(11)式得出，Encoder1 _Integral為Encoder1在采樣周期內(nèi)的積分，Kp和Ki 分別為比例系數(shù)與積分系數(shù)。

同理，電機(jī)控制采用增量式PI控制：

式中：Bias1是速度偏差，由讀取編碼器的現(xiàn)速度與期望的Velocity 相減得來(lái)，Last_bias 是對(duì)上一次Bias1進(jìn)行儲(chǔ)存所得的變量，Kp和Ki分別為比例系數(shù)和積分系數(shù)。

最后，根據(jù)計(jì)算所得出的Servo值和Motor值可由主控芯片輸出所需要PWM值控制舵機(jī)和電機(jī)使自行車保持平衡。

3.2 循跡程序設(shè)計(jì)

循跡模塊使用兩個(gè)TCRT5000光電傳感器并安裝于車身左右兩側(cè)。將左右兩個(gè)TCRT5000傳感器的輸出連接到主控的I0口后，通過(guò)讀取I0口的高低電平判斷自行車是否在黑線上運(yùn)行，根據(jù)程序即可使自行車達(dá)到循跡前行的效果。程序設(shè)計(jì)大致如下：當(dāng)左側(cè)TCRT5000傳感器檢測(cè)到黑線時(shí)，傳輸信號(hào)至主控芯片，主控芯片輸出PWM波控制舵機(jī)向左扭轉(zhuǎn)一定角度；當(dāng)右側(cè)TCRT5000傳感器檢測(cè)到黑線時(shí)，傳輸信號(hào)至主控芯片，主控芯片輸出PWM波控制向右扭轉(zhuǎn)一定角度。程序掃描周期為50ms，舵機(jī)的左右方向通過(guò)更改PWM波的脈寬即可控制。

3.3 避障程序設(shè)計(jì)

避障模塊使用E18-D80NK紅外光電感應(yīng)器。將E18-D80NK傳感器的輸出連接到主控芯片的I0端口后，通過(guò)讀取I0端口的高低電平，判斷自行車前方是否有障礙物，根據(jù)程序即可完成避障。程序設(shè)計(jì)大致如下：若前方存在障礙，E18-D80NK紅外光電感應(yīng)器輸出信號(hào)至主控芯片，主控芯片輸出PWM波控制舵機(jī)向左扭轉(zhuǎn)一定角度，以完成避障。程序掃描周期為50ms，舵機(jī)的左右方向通過(guò)更改PWM波的脈寬即可控制。

3.4 手機(jī)APP設(shè)計(jì)

本設(shè)計(jì)可通過(guò)手機(jī)APP與自行車進(jìn)行藍(lán)牙連接并通信。手機(jī)在Android操作系統(tǒng)下安裝后綴為apk的安裝包，打開(kāi)軟件后可直接使用藍(lán)牙連接至自行車，在APP主頁(yè)即可查看自行車的傾斜角度和電源電壓，并可以操縱自行車進(jìn)行轉(zhuǎn)向等操作。

4 設(shè)備調(diào)試

4.1 直立環(huán)PID參數(shù)調(diào)試

直立環(huán)采用PD（比例微分）控制器。調(diào)試過(guò)程分為三個(gè)步驟：獲取自行車的機(jī)械中值，獲取Kp值的極性和大小，獲取Kd值的極性和大小。

首先，得到機(jī)械中值。將自行車置于地上，緩慢晃動(dòng)自行車，記錄使自行車能近乎平衡的角度，一般在0°左右，可知機(jī)械中值為0。

然后調(diào)試kp值。根據(jù)自行車的PWM設(shè)置估計(jì)kp值在0～720區(qū)間中，設(shè)置kp=-200時(shí),自行車往哪邊倒下，舵機(jī)會(huì)往相反方向轉(zhuǎn)動(dòng)使自行車倒下，這說(shuō)明kp值極性應(yīng)為正。

Kp值太大易導(dǎo)致自行車產(chǎn)生低頻抖動(dòng)。所以先設(shè)定kp=100，此時(shí)自行車有了平衡的跡象，但是響應(yīng)不夠快；

設(shè)置kp=200，自行車響應(yīng)加快，但自行車仍無(wú)法保持平衡;

設(shè)定kp=500時(shí)自行車的響應(yīng)速度足夠快,但產(chǎn)生了低頻抖動(dòng)。此現(xiàn)象表明kp值足夠大了，此時(shí)通過(guò)添加微分控制可減小低頻抖動(dòng)。

最后調(diào)試Kd值。首先通過(guò)MPU6050所集成陀螺儀輸出的原始數(shù)據(jù),估算出kd在0到2之間。然后設(shè)置kd=-0.5，這時(shí)拿起自行車并搖動(dòng)，舵機(jī)將反向轉(zhuǎn)動(dòng)，說(shuō)明kd極性為正。

Kd值太大易導(dǎo)致自行車產(chǎn)生高頻抖動(dòng)。設(shè)置kd=0.5，kp太大產(chǎn)生的低頻抖動(dòng)基本消失。設(shè)置kd=1，自行車平衡性能已經(jīng)達(dá)到預(yù)期效果。設(shè)置kd=1.5，自行車會(huì)產(chǎn)生高頻抖動(dòng)現(xiàn)象。

在確定KP=500和Kd=1.5是P和D參數(shù)的最大值后，將系數(shù)都乘以0.6近似得到KP=300和Kd=1，即為最終確定的參數(shù)。這是因?yàn)橹矮@得的參數(shù)是KP和KD的最大值。根據(jù)實(shí)際經(jīng)驗(yàn)，最大值乘以0.6后的參數(shù)性能最好。調(diào)整后，自行車運(yùn)行平穩(wěn)且無(wú)抖動(dòng)，但直立時(shí)間短，說(shuō)明需要對(duì)速度環(huán)進(jìn)行調(diào)試。

4.2 速度環(huán)PID參數(shù)調(diào)試

速度環(huán)使用PI(比例積分)控制器。調(diào)試過(guò)程分為兩個(gè)步驟：獲取Kp值的極性和大小，獲取Ki值的極性和大小。

通過(guò)STM32定時(shí)器的編碼器接口方式將編碼器四倍放大，通過(guò)M測(cè)速法獲取自行車的速度信息，發(fā)現(xiàn)編碼器最大值在180左右。根據(jù)PWM輸出控制估算kp最大為80。先設(shè)定kp=-40，可觀察到自行車倒下時(shí)電機(jī)轉(zhuǎn)速變小，說(shuō)明kp極性為正。

速度環(huán)中積分控制與比例控制極性相同。并且存在一種相應(yīng)的比例可以描述PID變量在不同的系統(tǒng)間的關(guān)系。根據(jù)速度環(huán)kp的極性和大致范圍，最終確定kp=70，kd=0.5是速度控制環(huán)中kp、ki參數(shù)的最終值。

4.3 調(diào)試結(jié)果

經(jīng)過(guò)長(zhǎng)時(shí)間的實(shí)驗(yàn)和調(diào)試，自行車在開(kāi)始運(yùn)行約2s后即可自行平衡，在平整地面上已無(wú)法用肉眼直接觀察到可見(jiàn)的自行車傾角；給予一定干擾后自行車也可快速恢復(fù)平衡。