改進的兩輪直立車多回路控制

（中原工學院 機電學院，鄭州 451191）

兩輪直立智能車在賽道中，需同時兼顧速度、平衡與循跡，姿態與循跡控制效果的優劣直接決定行駛平均速度的高低。為提升在較高速度下的運行性能，對傳統算法[1-3]進行了一定程度的改進，文獻[4]提出的最優滑膜輸出跟蹤控制方法對外界的干擾具有完全的魯棒性，但只做了仿真并未對算法進行實測驗證；文獻[5]提出模糊PD控制方法，提升了控制的實時性、穩定性，但模糊規則表需要大量試驗調試和專家經驗才能建立；文獻[6]提出基于μC/OS-Ⅲ的直立車平衡系統的方案，提升了算法的實時性和MCU內核的利用率，但其仍屬于傳統直立控制范疇。

在此，以2個RS-380電機驅動的兩輪直立車為控制對象，為提升高車速下直立車控制的穩定性和響應性，對傳統的姿態和轉向控制進行改進，添加了角速度內環回路，并將速度控制與直立控制進行串聯。經實測對比，該雙串級閉環控制系統，相較于傳統算法，在姿態和速度穩定性方面的控制得到改善。

1 直立車底層的搭建

直立車硬件系統以K60芯片為微控制器，外圍包含電源模塊、IMU模塊、輪速傳感器模塊、道路識別模塊和雙電機驅動模塊。直立車硬件系統的組成如圖1所示。

圖1 直立車硬件系統的組成Fig.1 Composition of hardware system of vertical vehicle

直立車控制系統必須在硬件基礎上感知狀態信息，才能對車輛運行進行有效控制。圖中，模塊主要通過電感、陀螺儀、加速度計、編碼器等傳感器來獲取周圍環境信息，以便車輛能自動沿著路線行駛。對傳感器信息進行處理，可以得到5個運行變量——中線B，俯仰角速度ωq，橫擺角速度ωr，俯仰角度θ和車身平均速度V；一個常量——機械零點角度α。這些量的輸入構成串級控制的重要一環。

1.1 道路識別模塊

電磁引導線的交變磁場頻率為20 kHz，直立車通過LC并聯諧振電路感應該頻率，以引導車身的行進方向。諧振電路頻率為

在此，采用10 mH的工字電感作為電路中的諧振電感，得到與之匹配的諧振電容值。

采集信號的位置如圖2所示，前瞻為30 cm的小車在圖中賽道的3個位置上進行采集。

圖2 采集信號位置Fig.2 Position of signal acquisition

所采集到的工字電感原始波形，即運算放大整流前的波形如圖3a所示。由圖可見，得到的采集信號振幅只有幾十mV，且伴隨較多噪點，MCU的AD端口不能直接采集。在此，采用雙通道單電源運算放大器芯片OPA2350對該信號進行處理，后經整流、濾波以供單片機采集。運算放大整流后的波形如圖3b所示。由圖可見，經運算放大整流后的電壓有顯著的提高，且處理后的波動幅度不到1%，幅值和穩定性均滿足采集要求。直立車前瞻上的2個電感使用1個道路識別模塊即可。

圖3 運算放大整流前、后的波形Fig.3 Waveforms before and after operation amplification and rectification

道路識別模塊的輸出模擬量經MCU的AD采集可獲取該信號量，采用比值法[7]對其進行處理可得到中線B，即

式中：E1，E2分別為MCU的AD采集的左、右電感感應電動勢放大整流后的值；C為增益系數。

1.2 電機驅動模塊

直立車采用2個直流電機驅動兩側車輪，在此采用4個N溝道的MOSFET管構成橋式電路，實現單個電機的轉速控制，進而控制直立車的循跡方向和車身姿態。其中，由于電機加減速產生的反電壓可以達到十幾V，為保護主控板，在其PWM輸出端采用一個三態雙向緩沖器芯片74LVC245作為與驅動電路的隔離[8]，有效地解決了供電問題，并能提高驅動負載的能力。

1.3 IMU模塊

IMU由加速度計和陀螺儀組成。加速度計可獲取沿空間坐標系X-Y-Z三軸的加速度，陀螺儀可獲取繞該坐標軸的角速度，故對陀螺儀原始數據進行遞推平均濾波[9]從而得到ωq和ωr。姿態融合的目的是獲取歐拉角（X-Y-Z）：俯仰角θ，翻滾角φ和橫擺角ψ。工程中常使用的方法有四元數-互補濾波法[10]、卡爾曼姿態融合法[11]。

四元數-互補濾波法，將歸一化后的加速度計重力向量與陀螺儀積分推算出的重力向量進行叉積，再將該叉積進行積分，通過互補濾波將該積分值補償到角速度以修正陀螺儀誤差，最后采用一階龍哥庫塔法求解并積分得出四元數，將四元數進行反三角函數計算即可得到3個歐拉角。

卡爾曼姿態融合法，能夠對預測模型和觀測模型進行權衡，對加速度進行幾何代換獲得歐拉角，陀螺儀可以獲得該軸角速度，將角速度和該歐拉角作為原始數據，通過協方差矩陣進行迭代遞推，最終獲取該歐拉角的最優值。

采用這2種算法，對直立車俯仰角施加不同干擾，可得到俯仰角的實測效果如圖4所示。四元數算法求解微分方程時，采用了積分運算，故在開始解算角度時四元數法收斂速度慢于卡爾曼，但隨后四元數法響應速度和收斂速度均優于卡爾曼法。另外，卡爾曼融合法采用了大量的矩陣迭代運算，需要多次調整預測和測量值的協方差，才能達到最優效果。故在此選用四元數-互補濾波法，可由該算法獲取θ，據此可測得直立車機械零點角度α。

圖4 卡爾曼與四元數姿態解算俯仰角度的對比Fig.4 Comparisons between Kalman and Quaternion attitude algorithm for pitch angle

1.4 輪速傳感器模塊

采用mini編碼器測量輪速，編碼器齒輪每運轉一周輸出的引腳輸出脈沖數等于編碼器線數。該信號通過MCU的FTM可獲取設定時間內一定數量的脈沖，此外編碼器具有方向輸出功能，通過GPIO讀取高低電平可獲取電機齒輪旋轉方向。由于直立車編碼器齒輪與動力輸出齒輪直接嚙合，左輪或右輪的輪速Vi為

因車身平均速度為左右輪速的平均值，故

將式（3）代入式（4），整理可得平均速度V為

式中：T 為采樣時間，ms；P 為編碼器線數；N1，N2分別為T時間內左、右輪采集到的脈沖數；R為輪轂中心到地面的距離，cm；B1，B2分別為左、右輪編碼器齒輪旋轉系數，兩數互為相反數，正對編碼器齒輪時，順時針旋轉 B1=1，逆時針 B1=-1；Z1，Z2分別為編碼器、輪齒的齒數；V1，V2分別為左、右輪的輪速，cm/s；V 為小車平均速度，cm/s。

2 控制算法

直立車由雙電機驅動，左右電機為控制對象，控制器需具有直立、行駛同時兼顧轉向的功能。傳統直立車控制算法，在高速行駛狀態時轉向和姿態的跟隨能力欠佳，在此在傳統算法基礎上將速度-角度進行了串聯，并在轉向和角度增加了角速度內環回路。PID控制器輸出后經過處理換算為PWM，輸入驅動板后可使直立車沿賽道行駛。

2.1 傳統直立車PID控制系統

傳統直立車控制系統如圖5所示。

為使其具有速度控制、自動循跡與姿態平衡能力，利用3個單獨的閉環分別進行控制，輸出電機控制信號為各環輸出的疊加。為使車身自動保持平衡與循跡，期望角度和期望中線由內部算法給定，其值分別為機械零點α和0，速度設定值則由外部給定，以此控制直立車在不同速度下行進（其輸出PWM 計算式如式（8）式（9）所示）。

圖5 直立車傳統控制系統框圖Fig.5 Block diagram of traditional control system of upright vehicle

該控制方案雖簡單但存在一定問題，如：僅單獨施加直立控制，其車身平衡效果雖能滿足要求，但此時如果在該基礎上額外施加速度控制，速度控制勢必會打破直立車的平衡姿態，使得速度與直立控制互相耦合，此時如果再施加轉向控制，就會導致高速時轉向控制實時性和姿態的平衡性欠佳，進而循跡失敗。

2.2 改進多回路直立車PID控制系統

改進后直立車串級系統結構如圖6所示，在給定不變的情況下，采用行駛和轉向2個串級閉環實現直立車控制。

直立車以某一車速進行轉向時，可視為直立車車身繞Z軸維持一定的橫擺角速度。在轉向閉環中，方向PID計算輸出值為直立車身橫擺角度需要保持的角速度，橫擺角速度PID的計算結果即為串級轉向閉環的輸出值。串級轉向閉環可使直立車沿賽道中線行駛，其調節質量直接影響直立車在賽道上行駛的穩定性以及轉向的實時性，相較于傳統的單級差速閉環，由于其增加了橫擺角速度內環，則內環回路控制對象的等效時間常數變小，系統的過渡時間進而縮短，對二次擾動有更好的抑制作用，控制更加及時。

行駛閉環控制車模的行駛姿態和運行速度。在行駛速度閉環中，速度PID計算輸出的值為直立車在零點角α需要前傾或者后仰的角度，該輸出值需與機械零點角α相加才能將速度控制轉化為角度控制，俯仰角度PID計算輸出值為直立車θ角度需要保持的角速度，俯仰角速度PID計算結果即為串級行駛閉環的輸出值。直立車系統是經典的倒立擺系統[12]，其運動原理如圖7所示。由文獻[13]可知直立車加速度控制公式為

將其代入運動學公式v=v0+aΔt，得

式中：v0為直立車Δt內的初速度；v為直立車的Δt內的末速度；θ為直立車當前角度與零點角α的差值；θ′為角速度；k1，k2均為比例系數。

圖6 直立車串級控制系統框圖Fig.6 Block diagram of cascade control system of upright vehicle

圖7 直立車運動原理簡圖Fig.7 Schematic of upright vehicle movement

由圖可見，結合式（7）分析可知，假設直立車v0方向為右，當串級行駛閉環中速度PID輸出的角度為 β（β>α）時，θ>0，θ′>0，此時車身加速度 a2方向為右，則直立車末速度v增加。同理，當速度閉環輸出的角度為 γ（γ<α）時，θ<0，θ′<0，此時車身加速度 a1方向為左，則直立車末速度v減少。速度控制和平衡控制，轉化為角度和角速度的控制，根據該原理可以有效地消除傳統速度PID與直立平衡PID并聯所產生的耦合。

2.3 算法實現

在傳統與改進控制系統中，為了行駛閉環和轉向閉環的輸出絕對值不超過PWM最大占空比范圍，進行了限幅處理。改進串級算法的左、右電機最終輸出PWM占空比τL，τR分別為

式中：PID1為串級行駛閉環輸出經限幅處理后的值；PID2為串級轉向閉環輸出經限幅處理后的值；Pre為PWM占空比精度。對于傳統算法，式中PID1為速度閉環與俯仰角度閉環輸出值之和；PID2為差速閉環的輸出值。若τL<0或τR<0，則輸出反向電壓使驅動電機減速或者反轉。

直立車的算法流程如圖8所示。初始化程序完成之后，給定一定的行駛速度，然后啟動周期為2 ms的PIT定時中斷。PIT中斷主要為for循環中的算法提供程序執行的時序標志位，據此將傳感器信息采集和處理以及角速度PID的周期設定為2 ms，角度和方向PID以及電量監測和信息傳輸設定為10 ms，速度PID設定為20 ms，這種程序結構有助于提高算法的執行效率以及實時性。

圖8 直立車算法流程Fig.8 Flow chart of upright vehicle algorithm

3 算法調試與驗證

搭建的直立車硬件調試平臺和實驗環境如圖9所示。PID參數整定采用經驗整定的方法，對傳統的和改進的直立閉環算法在賽道上進行測試，并將響應曲線進行對比，得出試驗結果。

圖9 直立車硬件調試平臺與實驗環境Fig.9 Hardware debugging platform and experimental environment of upright vehicle

3.1 直立車俯仰角度響應對比

將傳統和串級算法的Vd設定為0，將PID2都取0，直立車初始“平趴”于地面，進行姿態平衡測試，得到的角度響應曲線如圖10（a）所示。由圖可見，行駛串級閉環角θ超調量幾乎為0，且調節時間0.61 s，快于傳統算法0.89 s，此外穩態誤差和調節精度均遠優于傳統直立閉環。

3.2 直立車速度響應對比

將傳統串級算法的Vd設定為200 cm/s，將PID2都取0，進行速度測試，得到的速度響應圖像如圖10（b）所示。由圖可見，串級行駛閉環的上升時間1.68 s，略快于傳統速度直立閉環1.81 s，且調節過程中串級算法與期望值平均誤差4%，小于傳統算法平均誤差6.5%；串級算法消除了傳統速度與直立閉環耦合所產生的周期性波動，速度穩定性得到提高。

3.3 直立車循跡中線響應對比

將傳統和串級算法的Vd設定為200 cm/s，測試賽道為“L”彎，彎折處曲率半徑約為45 cm，進行轉向跟蹤測試，得到的循跡響應圖像如圖10（c）所示。由圖可見，傳統算法最大偏離中線14.8 cm，并圍繞中線產生-3.4 cm的過調；串級算法最大偏離中線6.6 cm，且幾乎無過調，線跟隨能力遠強于傳統算法。

圖10 傳統算法與改進算法的對比Fig.10 Comparision between traditional algorithm and improved algorithm

在此以兩輪直立車為平臺，從以下3個方面對傳統算法優化進行了改進：①在IMU信息處理方面，采用四元數-互補濾波法，并與卡爾曼姿態融合法進行比對，前者更易整定且角度收斂速度快，提高了直立車姿態穩定性；②在轉向控制方面，以比值法作為輸入，并以橫擺角速度環作為內環，具有超前控制和中線跟蹤的能力；③在姿態控制方面，行駛閉環直接以直立車平均速度作為輸入，以俯仰角作為速度控制量，消除了直立控制與速度控制的耦合；由于增添了俯仰角速度內環，有效地減少了內環回路的干擾，控制更加及時。

4 結語

所提出的改進算法經過實際調試，驗證了該算法與傳統方案相比具有較高的魯棒性。由于串級算法實現簡單且實用，對傳統方案進行改進不需要額外的硬件要求，在直立車控制上具有一定的工程推廣和參考價值。