多回路PID控制下的兩輪平衡車系統設計

劉澤偉,馮靖靖,李婉鈺,趙 倩

(1.曲阜師范大學工學院，山東 日照 276826；2.曲阜師范大學計算機學院，山東 日照 276826)

0 引言

移動機器人因集控制、計算機、電子、機械等多學科知識于一體，已成為目前科學研究較活躍的領域之一。同時，隨著移動機器人的智能化發展[1]，其應用范圍越來越廣泛，面臨的環境和任務也越來越復雜。兩輪平衡車的概念正是在這一背景下被提出來的。兩輪平衡車自身的不穩定性以及非線性使它成為優良的驗證控制理論和控制方法的平臺。

針對兩輪自平衡機器人，目前采用的控制策略主要有傳統比例積分微分(proportional integral differential,PID)控制、模糊比例微分(fuzzy proportional-differential,FuzzyPD)控制等[2]。其中，傳統PID控制相較于其他控制算法[3]，不需要建立準確的數學模型和專家經驗，控制面廣泛且易于實現，但仍存在易受干擾、高速下穩定性和響應性差的問題。為解決該問題，本系統從控制器結構入手，設計了以角速度環為主回路的雙串級PID控制器。該控制器采用LPC54606為主控芯片，并以搭配姿態、圖像、速度等多個傳感器的兩輪平衡車作為被控對象。經實測，車速提升后的系統穩定性和響應速度良好。

1 硬件系統設計

硬件系統如圖1所示。系統硬件主要由LPC54606單片機、互補金屬氧化物(complementary metal oxide semiconductor,CMOS)攝像頭、六軸慣性測量單元(inertial measurement unit,IMU)、1 024線編碼器、主從一體式藍牙、電機驅動組成。攝像頭、陀螺儀和編碼器的數據作為單片機的輸入。電機驅動作為單片機的輸出。LPC54606單片機主要完成姿態、速度、方向三個模塊的數據處理以及控制器算法實現。藍牙僅在PID參數整定、試驗數據采集時開啟，在正常循跡時關閉。所有模塊由所設計的升壓、穩壓、降壓電源電路提供電力。

圖1 硬件系統框圖

2 姿態信息采集及處理

目前的兩輪平衡車姿態數據測量大多通過陀螺儀和加速度計等IMU模塊獲取。陀螺儀具有較好的動態性能，但用角速度積分得到角度的檢測方法會累加漂移誤差。加速度計靜態特性好，但運動過程中產生的運動加速度會干擾輸出信號。故需要利用濾波器算法融合兩者各自得到的角度信息，從而最大程度地抑制噪聲對測量角度的影響。

2.1 原始數據處理及單位轉換

利用單片機模擬集成電路總線(inter-integrated circuit,I2C)與IMU模塊通信，所讀取到的IMU多字節數據為加速度計和陀螺儀的原始數據，經過數據處理和單位轉換后的數據才可用于后續處理。

(1)

式中：G0和G分別為IMU單位轉換前后的角速度，rad/s；D為陀螺儀角速度零點偏差；K1為陀螺靈敏度。

(2)

式中：A0和A分別為IMU單位轉換前后的加速度；K2為三軸加速度計靈敏度，m/s2。

對單片機進行多次在線調試，確定上電后，D長時間內在一微小數值范圍內變化，對于本系統可設置為定值。配置IMU三軸陀螺儀寄存器的FS_SEL=3，則量程范圍為±34.9 rad/s，選定16位帶符號數作為測量數據輸出，最大值為±32 767，故K1=938.9 LSB/(rad/s)。同理可求，K2=16 384 LSB/g(g為重力加速度)。

2.2 卡爾曼濾波下的數據融合

兩輪平衡車這一自然不穩定系統需具有快速響應以及高適應性的特點，從而在復雜環境下保持自我動態平衡。故本系統選擇適應于動態環境下且具有較高融合精度的卡爾曼濾波算法。卡爾曼濾波是一種從含噪聲的測量值中估計系統狀態的、應用于離散線性濾波的迭代算法，具有較強抑制噪聲的能力，在航天技術、通信工程等領域有著廣泛應用[4-5]。

卡爾曼濾波器算法實現過程如下[6]。

①二維狀態向量系統的構建。

研究兩輪平衡車的直立狀態，即為研究車體的俯仰狀態。由于俯仰角和俯仰角速度為導數關系，本系統將車體實際俯仰角θ作為一個狀態向量。系統俯仰角選取繞x軸，偏航角選取繞y軸，橫滾角選取繞z軸，計算式如式(3)所示。

(3)

式中：Ay和Az分別為加速度計y軸和z軸重力加速度[7]。

以加速度計估計得到的陀螺儀常值偏差b作為另一狀態向量。構建二維狀態向量系統，故卡爾曼濾波器中向量均為二維向量，便于在單片機中編程實現。

②用離散化方程描述連續系統。

狀態預測方程為：

X(k)=AX(k-1)+BU(k-1)+W(k-1)

(4)

觀測方程為：

Z(k)=H(k)X(k)+V(k)

(5)

式中：Z(k)為加速度計測得的角度；H為(k-1)時刻到k時刻的狀態變量對觀測量增益矩陣；V(k)為k時刻加速度計觀測噪聲。

③預測階段。

要估計k時刻的實際角度，必須根據(k-1)時刻的角度來預測，同時計算k時刻的系統噪聲協方差。

先驗估計計算式為：

X(kk-1)=AX(k-1k-1)AT+BU(k)

(6)

式中:X(kk-1)為利用(k-1)時刻的角度預測出的角度結果;X(k-1k-1)為(k-1)時刻的最優角度。

計算誤差協方差：

P(kk-1)=AP(k-1k-1)+Q

(7)

qa和qω數值代表卡爾曼濾波器對于所用IMU模塊數據的信任程度，值越小代表信任程度越高，則系統中陀螺儀值更接近于實際值。因此，qω小于qa，一般小三個數量級左右。

④校正階段。

卡爾曼增益計算式為：

(8)

式中：Kg(k)為卡爾曼增益；H為系數矩陣，由于陀螺儀積分得到的角度值與加速度計測得角度值相對應，故H=[1 0]；R為測量噪聲的協方差矩陣，R=[Rθ]，Rθ越大則觀測值中被卡爾曼濾波器視為噪聲的成分越大。

后驗估計計算式為：

X(kk)=X(kk-1)+Kg(k)Z(k)-HX(kk-1)

(9)

更新誤差協方差：

P(kk)=[I-Kg(k)H]P(kk-1)

(10)

式中：I為單位矩陣。

⑤迭代計算。

在已得到k時刻的角度最優估計值的前提下，當系統進入(k+1)時刻狀態，k時刻的后驗估計將作為(k+1)時刻的先驗估計。重復預測階段和校正階段，使卡爾曼濾波器循環下去，得到姿態的最優估計。

2.3 卡爾曼濾波算法參數調試

3 多回路PID控制及分析

本系統采用以角速度環為主回路的雙串級PID來改善傳統PID控制結構，即串級轉向環和串級直立行駛環。該串級控制理論衍生于四軸飛行器[8-9]。多回路PID控制結構如圖2所示。

圖2 多回路PID控制結構圖

圖2中：X1為速度環輸出；X2為角度環輸出；X3為轉向環輸出；N1為直立閉環控制器受到的干擾信號；N2為方向閉環控制器受到的干擾信號；OUT1為直立閉環控制器的輸出；OUT2為方向閉環控制器的輸出。

兩個角速度環的輸出不同。直立速度控制的角速度環輸出為兩電機的脈寬調制(pulse width modulation,PWM)波信號。串級轉向的角速度環輸出為兩電機差速轉彎時的PWM差值。兩者疊加在電機上，如式(11)所示。

PL,R=Pθ±PT

(11)

式中：PL,R為左右兩電機的PWM值；Pθ為直立速度環輸出的PWM值；PT為串級轉向環輸出的兩電機差速轉彎時的PWM差值。

3.1 串級直立行駛閉環控制

①角速度環。

角速度環的存在減少了直立車的震蕩。通過比較現有角速度與理想目標角速度，使直立車可以快速達到目標速度，輸出PWM控制電機，使車體保持角速度為0。在確保角速度調整的快速響應下，PID控制中微分D的影響最小，可以忽略。積分I起主要作用，可以有效地消除穩態溫差，但過大的積分量會破壞系統平衡，使參數難調試且易超調。故本系統只取比例P來進行控制，在一定程度上減少了擾動和干擾，有利于系統的穩定。位置式角速度環比例積分(proportional integral,PI)控制的關鍵代碼如下。

Gyro_Error=Angle_Output-fgyrox;//與角度環串聯

Gyro_Output=Gyro_Kp * Gyro_Error;//P控制

Gyro_Output=limit(Gyro_Output,1000,-1000);//輸出限幅

代碼中，角速度環的輸入為期望角速度和俯仰軸角速度，對應于代碼中的Angle_Output 和fgyrox。在離散化采樣處理中，Gyro_Error為當前時刻的角速度偏差。輸出限幅則是為了濾除高頻干擾，防止輸出值過大。

②角度環。

角速度環只能使車勉強立住，而使車在機械零位保持角速度為0是直立的關鍵，故串入角度環使直立車固定在設定角度。通過比較現有角度和目標角度，輸出此時理想的角速度。在確保角度不會過大產生震蕩的情況下，可以只利用比例P來控制，出現輕微震蕩再加入微分D，最終系統選定比例微分(proportional differentral,PD)控制器來控制。位置式角度環PD控制的關鍵代碼如下。

Angle_Error=(Set_Point +Speed_Output)-angle;//與速度環串聯

Angle_Sum_Error +=Angle_Error;//累加誤差

Angle_Output=Angle_Kp * Angle_Error +Angle_Kd * fgyrox;//PD控制

代碼中，角度環的輸入為期望角度值和俯仰軸角速度。前者為機械零位Set_Point和速度環輸出的偏差量Speed_Output的和。后者為fgyrox。angle為實際角度。在離散化采樣處理中，Angle_Error為當前時刻角度偏差。

③速度環。

前兩個環只能讓車穩穩地立在原地，但不能維持車速，一旦加速或減速便會打破直立平衡。故本設計在串級直立控制的基礎上串入速度控制。速度環的存在使車以一定速度前進，將現有速度與設定速度比較，輸出直立車行駛應保持的角度。控制器只需加入比例P便可實現優良的速度控制，而積分I可以讓速度更穩定。故本系統選擇引入PI控制器。增量式速度環PI控制的關鍵代碼如下。

Speed_Error=(Set_Speed-Real_Speed)-(angle-Set_Point);//與角度建立聯系

Speed_OutNew+=Speed_Kp * (Last_Speed-Real_Speed)+Speed_Ki * Speed_Error;//PI控制

Speed_OutNew=SpeedOutOld * 0.6 +SpeedOutNew * 0.4;//一階低通濾波

代碼中，速度環的輸入為期望速度和實際速度，即代碼中的Set_Speed和Real_Speed。Speed_Error為當前時刻的速度偏差。一階低通濾波可防止速度發生突變[10]。

3.2 串級方向閉環控制

轉向內環是為了消除兩電機機芯的偏差，使平衡車上電放置后沿直線行駛，系統選定P控制。轉向外環則是建立與圖像傳感器之間的控制聯系，其存在是使平衡車能夠按所計算的實際中線循跡。但若只靠單純的比例控制，偏離中線方向多就轉得多，很容易造成抖動和過沖。故本文在比例P控制的基礎上引入圖像權重法。位置式轉向外環P控制的核心代碼如下。

for(int i=VALUE; i

{Turn_Error+=((m_line[i]-MIDDLE)* weight[i])/(sum_w*1.0);//圖像權重法

m_l +=m_line[i];}

Turn_Output=Turn_Kp * Turn_Error;//P控制

代碼中，轉向外環的輸入為根據攝像頭圖像所計算的實際中線和設定的理想中線，對應到圖像權重法中的m_line[i]和MIDDLE，將數字圖像每行的列用一維數組表示，同時根據圖像信息分布的不同劃分權重占比[11]。for循環的[VALUE,CUT_H]為圖像有效行區間。經大量試驗測試，當系統機械零位、攝像頭高度和角度確定后，VALUE取10效果較好，sum_w為有效行權重和。在離散化采樣處理中，Turn_Error為當前時刻的中線偏差。

3.3 控制周期設定

由于傳感器的數據采集和各控制算法對實時性要求很高，故系統需要精準地把控控制周期。PID控制周期遵循最內環控制周期最短、最外環控制周期最長的原則。在工程應用中，外環周期約為內環周期的3～5倍。在單片機中并不需要利用多個周期中斷定時器(periodic interrupt timer,PIT)定時中斷，而是將控制周期都設置成2 ms的倍數，通過在一個2 ms中斷服務程序中設置計數值,根據計數值的不同來調用不同控制周期的控制函數,從而實現相應的控制算法。定時器中斷控制流程如圖3所示。

圖3 定時器中斷控制流程圖

經多次試驗，本系統在串級直立行駛控制中：內環采樣周期設定為2 ms；角度環設定為6 ms；速度環設定為24 ms。串級轉向控制中，內環周期要與直立行駛控制內環周期相同，轉向外環則設定為6 ms。

3.4 手機藍牙PID調參軟件

為簡便經驗調參步驟，避免單片機端參數需重復更改并下載的問題，本系統采用云端手機編程環境(APP Inventor)開發設計手機APP，進行輔助調參。APP Inventor是基于云端的手機編程環境，用戶能通過該工具自行研發適合手機使用的任意應用程序。單片機端的HC-06主從一體式藍牙，與手機藍牙配對和相互通信。程序主要分為兩部分：一是對單片機通過藍牙接收的數據進行判斷，如在上位機程序中設定發送字符(不同字符代表不同動作指令)，從而控制兩輪平衡車的各個環的參數調整和系統初始化；二是將手機端調試更改的參數反饋至單片機。

系統最終整定的各環PID配置參數如表1所示。表1中：參數的符號正負與PID的雙極性有關。

表1 各環PID配置參數

4 創新點

本文設計的多回路PID控制下的兩輪平衡車系統的創新點如下所示。

①本系統更改了傳統PID的控制結構，設計了以角速度環為主回路的雙串級PID控制器，將速度和角度控制轉換為角度和角速度控制；消除了原先速度PID與直立平衡PID并聯所產生的耦合問題，提升了平衡車在高速行駛時的轉向穩定性和姿態跟隨能力。

②卡爾曼濾波器算法進行數據融合時，本系統采用基于Visual C++的軟件示波器進行卡爾曼濾波器的參數調試。上位機界面的交互性為參數整定過程提供了便利。

③本系統采用APP Inventor開發設計的手機端APP來簡便PID調參步驟。該APP開發編程效率高且易于操作，不需要過于復雜的算法和可視化界面編寫；主從一體式藍牙與手機交互通信，避免PID參數在單片機端更改和下載的麻煩。

5 結論

本系統基于四旋翼飛行器的控制理論衍生的串級理論，設計了以角速度環為主回路的雙串級PID控制，改善了傳統PID控制器的結構，解決了兩輪平衡車系統易受干擾、高速下穩定性和響應性差的問題。同時，對于數據融合的卡爾曼濾波器參數、PID參數的整定步驟，本文進行了交互性方面的簡化，改善了相關參數調節的費力耗時問題。測試和試驗結果表明，所設計的多回路控制下的兩輪平衡車系統能夠沿任意給定路徑快速、穩定地行駛全程。相關算法可以推廣至兩輪平衡車相關領域，具有較為廣闊的應用前景和豐富的研究價值。