改進(jìn)的兩輪直立車多回路控制

（中原工學(xué)院 機(jī)電學(xué)院，鄭州 451191）

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

在此，以2個(gè)RS-380電機(jī)驅(qū)動(dòng)的兩輪直立車為控制對(duì)象，為提升高車速下直立車控制的穩(wěn)定性和響應(yīng)性，對(duì)傳統(tǒng)的姿態(tài)和轉(zhuǎn)向控制進(jìn)行改進(jìn)，添加了角速度內(nèi)環(huán)回路，并將速度控制與直立控制進(jìn)行串聯(lián)。經(jīng)實(shí)測(cè)對(duì)比，該雙串級(jí)閉環(huán)控制系統(tǒng)，相較于傳統(tǒng)算法，在姿態(tài)和速度穩(wěn)定性方面的控制得到改善。

1 直立車底層的搭建

直立車硬件系統(tǒng)以K60芯片為微控制器，外圍包含電源模塊、IMU模塊、輪速傳感器模塊、道路識(shí)別模塊和雙電機(jī)驅(qū)動(dòng)模塊。直立車硬件系統(tǒng)的組成如圖1所示。

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

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

1.1 道路識(shí)別模塊

電磁引導(dǎo)線的交變磁場頻率為20 kHz，直立車通過LC并聯(lián)諧振電路感應(yīng)該頻率，以引導(dǎo)車身的行進(jìn)方向。諧振電路頻率為

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

采集信號(hào)的位置如圖2所示，前瞻為30 cm的小車在圖中賽道的3個(gè)位置上進(jìn)行采集。

圖2 采集信號(hào)位置Fig.2 Position of signal acquisition

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

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

道路識(shí)別模塊的輸出模擬量經(jīng)MCU的AD采集可獲取該信號(hào)量，采用比值法[7]對(duì)其進(jìn)行處理可得到中線B，即

式中：E1，E2分別為MCU的AD采集的左、右電感感應(yīng)電動(dòng)勢(shì)放大整流后的值；C為增益系數(shù)。

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

直立車采用2個(gè)直流電機(jī)驅(qū)動(dòng)兩側(cè)車輪，在此采用4個(gè)N溝道的MOSFET管構(gòu)成橋式電路，實(shí)現(xiàn)單個(gè)電機(jī)的轉(zhuǎn)速控制，進(jìn)而控制直立車的循跡方向和車身姿態(tài)。其中，由于電機(jī)加減速產(chǎn)生的反電壓可以達(dá)到十幾V，為保護(hù)主控板，在其PWM輸出端采用一個(gè)三態(tài)雙向緩沖器芯片74LVC245作為與驅(qū)動(dòng)電路的隔離[8]，有效地解決了供電問題，并能提高驅(qū)動(dòng)負(fù)載的能力。

1.3 IMU模塊

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

四元數(shù)-互補(bǔ)濾波法，將歸一化后的加速度計(jì)重力向量與陀螺儀積分推算出的重力向量進(jìn)行叉積，再將該叉積進(jìn)行積分，通過互補(bǔ)濾波將該積分值補(bǔ)償?shù)浇撬俣纫孕拚勇輧x誤差，最后采用一階龍哥庫塔法求解并積分得出四元數(shù)，將四元數(shù)進(jìn)行反三角函數(shù)計(jì)算即可得到3個(gè)歐拉角。

卡爾曼姿態(tài)融合法，能夠?qū)︻A(yù)測(cè)模型和觀測(cè)模型進(jìn)行權(quán)衡，對(duì)加速度進(jìn)行幾何代換獲得歐拉角，陀螺儀可以獲得該軸角速度，將角速度和該歐拉角作為原始數(shù)據(jù)，通過協(xié)方差矩陣進(jìn)行迭代遞推，最終獲取該歐拉角的最優(yōu)值。

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

圖4 卡爾曼與四元數(shù)姿態(tài)解算俯仰角度的對(duì)比Fig.4 Comparisons between Kalman and Quaternion attitude algorithm for pitch angle

1.4 輪速傳感器模塊

采用mini編碼器測(cè)量輪速，編碼器齒輪每運(yùn)轉(zhuǎn)一周輸出的引腳輸出脈沖數(shù)等于編碼器線數(shù)。該信號(hào)通過MCU的FTM可獲取設(shè)定時(shí)間內(nèi)一定數(shù)量的脈沖，此外編碼器具有方向輸出功能，通過GPIO讀取高低電平可獲取電機(jī)齒輪旋轉(zhuǎn)方向。由于直立車編碼器齒輪與動(dòng)力輸出齒輪直接嚙合，左輪或右輪的輪速Vi為

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

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

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

2 控制算法

直立車由雙電機(jī)驅(qū)動(dòng)，左右電機(jī)為控制對(duì)象，控制器需具有直立、行駛同時(shí)兼顧轉(zhuǎn)向的功能。傳統(tǒng)直立車控制算法，在高速行駛狀態(tài)時(shí)轉(zhuǎn)向和姿態(tài)的跟隨能力欠佳，在此在傳統(tǒng)算法基礎(chǔ)上將速度-角度進(jìn)行了串聯(lián)，并在轉(zhuǎn)向和角度增加了角速度內(nèi)環(huán)回路。PID控制器輸出后經(jīng)過處理換算為PWM，輸入驅(qū)動(dòng)板后可使直立車沿賽道行駛。

2.1 傳統(tǒng)直立車PID控制系統(tǒng)

傳統(tǒng)直立車控制系統(tǒng)如圖5所示。

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

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

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

2.2 改進(jìn)多回路直立車PID控制系統(tǒng)

改進(jìn)后直立車串級(jí)系統(tǒng)結(jié)構(gòu)如圖6所示，在給定不變的情況下，采用行駛和轉(zhuǎn)向2個(gè)串級(jí)閉環(huán)實(shí)現(xiàn)直立車控制。

直立車以某一車速進(jìn)行轉(zhuǎn)向時(shí)，可視為直立車車身繞Z軸維持一定的橫擺角速度。在轉(zhuǎn)向閉環(huán)中，方向PID計(jì)算輸出值為直立車身橫擺角度需要保持的角速度，橫擺角速度PID的計(jì)算結(jié)果即為串級(jí)轉(zhuǎn)向閉環(huán)的輸出值。串級(jí)轉(zhuǎn)向閉環(huán)可使直立車沿賽道中線行駛，其調(diào)節(jié)質(zhì)量直接影響直立車在賽道上行駛的穩(wěn)定性以及轉(zhuǎn)向的實(shí)時(shí)性，相較于傳統(tǒng)的單級(jí)差速閉環(huán)，由于其增加了橫擺角速度內(nèi)環(huán)，則內(nèi)環(huán)回路控制對(duì)象的等效時(shí)間常數(shù)變小，系統(tǒng)的過渡時(shí)間進(jìn)而縮短，對(duì)二次擾動(dòng)有更好的抑制作用，控制更加及時(shí)。

行駛閉環(huán)控制車模的行駛姿態(tài)和運(yùn)行速度。在行駛速度閉環(huán)中，速度PID計(jì)算輸出的值為直立車在零點(diǎn)角α需要前傾或者后仰的角度，該輸出值需與機(jī)械零點(diǎn)角α相加才能將速度控制轉(zhuǎn)化為角度控制，俯仰角度PID計(jì)算輸出值為直立車θ角度需要保持的角速度，俯仰角速度PID計(jì)算結(jié)果即為串級(jí)行駛閉環(huán)的輸出值。直立車系統(tǒng)是經(jīng)典的倒立擺系統(tǒng)[12]，其運(yùn)動(dòng)原理如圖7所示。由文獻(xiàn)[13]可知直立車加速度控制公式為

將其代入運(yùn)動(dòng)學(xué)公式v=v0+aΔt，得

式中：v0為直立車Δt內(nèi)的初速度；v為直立車的Δt內(nèi)的末速度；θ為直立車當(dāng)前角度與零點(diǎn)角α的差值；θ′為角速度；k1，k2均為比例系數(shù)。

圖6 直立車串級(jí)控制系統(tǒng)框圖Fig.6 Block diagram of cascade control system of upright vehicle

圖7 直立車運(yùn)動(dòng)原理簡圖Fig.7 Schematic of upright vehicle movement

由圖可見，結(jié)合式（7）分析可知，假設(shè)直立車v0方向?yàn)橛遥?dāng)串級(jí)行駛閉環(huán)中速度PID輸出的角度為 β（β>α）時(shí)，θ>0，θ′>0，此時(shí)車身加速度 a2方向?yàn)橛遥瑒t直立車末速度v增加。同理，當(dāng)速度閉環(huán)輸出的角度為 γ（γ<α）時(shí)，θ<0，θ′<0，此時(shí)車身加速度 a1方向?yàn)樽螅瑒t直立車末速度v減少。速度控制和平衡控制，轉(zhuǎn)化為角度和角速度的控制，根據(jù)該原理可以有效地消除傳統(tǒng)速度PID與直立平衡PID并聯(lián)所產(chǎn)生的耦合。

2.3 算法實(shí)現(xiàn)

在傳統(tǒng)與改進(jìn)控制系統(tǒng)中，為了行駛閉環(huán)和轉(zhuǎn)向閉環(huán)的輸出絕對(duì)值不超過PWM最大占空比范圍，進(jìn)行了限幅處理。改進(jìn)串級(jí)算法的左、右電機(jī)最終輸出PWM占空比τL，τR分別為

式中：PID1為串級(jí)行駛閉環(huán)輸出經(jīng)限幅處理后的值；PID2為串級(jí)轉(zhuǎn)向閉環(huán)輸出經(jīng)限幅處理后的值；Pre為PWM占空比精度。對(duì)于傳統(tǒng)算法，式中PID1為速度閉環(huán)與俯仰角度閉環(huán)輸出值之和；PID2為差速閉環(huán)的輸出值。若τL<0或τR<0，則輸出反向電壓使驅(qū)動(dòng)電機(jī)減速或者反轉(zhuǎn)。

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

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

3 算法調(diào)試與驗(yàn)證

搭建的直立車硬件調(diào)試平臺(tái)和實(shí)驗(yàn)環(huán)境如圖9所示。PID參數(shù)整定采用經(jīng)驗(yàn)整定的方法，對(duì)傳統(tǒng)的和改進(jìn)的直立閉環(huán)算法在賽道上進(jìn)行測(cè)試，并將響應(yīng)曲線進(jìn)行對(duì)比，得出試驗(yàn)結(jié)果。

圖9 直立車硬件調(diào)試平臺(tái)與實(shí)驗(yàn)環(huán)境Fig.9 Hardware debugging platform and experimental environment of upright vehicle

3.1 直立車俯仰角度響應(yīng)對(duì)比

將傳統(tǒng)和串級(jí)算法的Vd設(shè)定為0，將PID2都取0，直立車初始“平趴”于地面，進(jìn)行姿態(tài)平衡測(cè)試，得到的角度響應(yīng)曲線如圖10（a）所示。由圖可見，行駛串級(jí)閉環(huán)角θ超調(diào)量幾乎為0，且調(diào)節(jié)時(shí)間0.61 s，快于傳統(tǒng)算法0.89 s，此外穩(wěn)態(tài)誤差和調(diào)節(jié)精度均遠(yuǎn)優(yōu)于傳統(tǒng)直立閉環(huán)。

3.2 直立車速度響應(yīng)對(duì)比

將傳統(tǒng)串級(jí)算法的Vd設(shè)定為200 cm/s，將PID2都取0，進(jìn)行速度測(cè)試，得到的速度響應(yīng)圖像如圖10（b）所示。由圖可見，串級(jí)行駛閉環(huán)的上升時(shí)間1.68 s，略快于傳統(tǒng)速度直立閉環(huán)1.81 s，且調(diào)節(jié)過程中串級(jí)算法與期望值平均誤差4%，小于傳統(tǒng)算法平均誤差6.5%；串級(jí)算法消除了傳統(tǒng)速度與直立閉環(huán)耦合所產(chǎn)生的周期性波動(dòng)，速度穩(wěn)定性得到提高。

3.3 直立車循跡中線響應(yīng)對(duì)比

將傳統(tǒng)和串級(jí)算法的Vd設(shè)定為200 cm/s，測(cè)試賽道為“L”彎，彎折處曲率半徑約為45 cm，進(jìn)行轉(zhuǎn)向跟蹤測(cè)試，得到的循跡響應(yīng)圖像如圖10（c）所示。由圖可見，傳統(tǒng)算法最大偏離中線14.8 cm，并圍繞中線產(chǎn)生-3.4 cm的過調(diào)；串級(jí)算法最大偏離中線6.6 cm，且?guī)缀鯚o過調(diào)，線跟隨能力遠(yuǎn)強(qiáng)于傳統(tǒng)算法。

圖10 傳統(tǒng)算法與改進(jìn)算法的對(duì)比Fig.10 Comparision between traditional algorithm and improved algorithm

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

4 結(jié)語

所提出的改進(jìn)算法經(jīng)過實(shí)際調(diào)試，驗(yàn)證了該算法與傳統(tǒng)方案相比具有較高的魯棒性。由于串級(jí)算法實(shí)現(xiàn)簡單且實(shí)用，對(duì)傳統(tǒng)方案進(jìn)行改進(jìn)不需要額外的硬件要求，在直立車控制上具有一定的工程推廣和參考價(jià)值。