基于模糊PID對萬向車臺路徑引導控制

湯子龍 ，丁志強，丁永強，高世燕

（1.蘭州理工大學 電氣工程與信息工程學院，蘭州 730050；2.甘肅工大舞臺技術工程有限公司，蘭州 730050；3.甘肅正隆工程招標代理有限責任公司，天水 741000）

萬向車臺是一種能夠自動導引，高能電池組提供動力，以微控制器為控制核心，裝有非接觸導引裝置的無人駕駛舞臺機械設備，具有安全保護功能，能夠沿預設的路徑導引行走，在國內正處于研發測試階段，尚未得到實際的應用。導引定位功能是萬向車臺準確行走的關鍵[1-3]，因此定位技術成為研究的熱點。路徑導引的一個重要任務就是根據自身的感知系統確定自身的位置，從而按照用戶的需求做出正確的行為決策和路徑選擇。在此，采用激光導引技術作為萬向車臺的導引方式，該技術具有導引柔性和較高的信息維度特性，在工程中得到了廣泛的應用。

實現激光自動導引萬向車臺控制的基礎是路徑跟蹤偏差值的矯正[4-6]，也是研發萬向車臺的關鍵技術。萬向車臺導引系統的精度由定位精度決定，因PID控制具有可靠、簡單、魯棒性好等優點，在工業控制領域普遍應用，但模糊控制器、模糊PID等控制方法具有更強的適應性，應用時取得了更好的控制效果。因此，文中采取模糊PID控制器實現萬向車臺的路徑導引控制，其穩定性、魯棒性、易用性更好，是一種最優偏差控制導引路徑的方法。

1 激光導引系統定位原理與位姿計算

1.1 激光定位算法原理

要實現準確定位，萬向車臺在行走的過程中激光掃描傳感器同時掃描到反射板不少于3塊，根據反射板的位置信息，按照激光定位算法在預設的全局坐標系中確定萬向車臺的坐標與方位，然后反饋給車臺微控制器進行處理。常用的定位算法有三角定位算法和三邊定位算法。

1.1.1 三角定位算法

三角定位算法通過對萬向車臺縱軸與反光板間的夾角測量來完成定位。如圖1（a）所示，測得∠AOB和∠BOC，利用外接圓的知識作2個外接圓。由外接圓的性質得知，三點不在一條直線上只有1個外接圓，推出車臺一定位于2個圓相交的其中一個交點處，另一個交點處是第2個反光板。該算法對反光板的布設位置及其相互之間的角度準確度要求較高，且兩圓圓心距離滿足一定的條件，對測量信號的角度分辨率要求極高，因此難以獲取精確的外置。該算法有一定的局限性。

1.1.2 三邊定位算法

三邊定位算法通過測量萬向車臺與全局坐標中預制的反光板之間的距離來實現定位。如圖1（b）所示，測量車體到3個反光板A，B，C的距離分別為R1，R2，R3；以 R1，R2，R3為半徑，分別以 3 個反光板A，B，C的中心為圓心作圓，由圓的性質得出車體的位置在3個圓的交點處。三邊定位方法與三角定位方法相比，實現更加容易、精確，且對硬件環境條件要求低，在GPS定位中成功的應用，故文中采用三邊定位算法。

圖1 常用定位算法Fig.1 Common location algorithm

1.2 位姿計算

為盡量減小系統誤差，在舞臺面周圍的墻壁上安裝多塊反光板可以提高定位的精度。以舞臺面為參考，建立全局坐標系XOY，行走過程中萬向車臺上的激光掃描儀轉動1周，可以檢測到m個反光板1，2，…，i，…，m（m≥3），得到在全局坐標系 XOY 中與反光板匹配的信息坐標分別為（x1，y1），（x2，y2），…，（xi，yi），…，（xm，ym），對應生成 m 個圓，可知萬向車臺的位置一定在m個圓的交點處，如圖2所示。

圖2 反光板的動態匹配Fig.2 Dynamic matching of reflective plate

構建的m個圓方程為

任意選取方程（1）中的2個圓方程，相減得到1個線性方程（xo，yo）；將方程（1）兩兩相減可得個線性方程，組成1個超定方程組，則出現了方程的冗余；用最小二乘法解此方程組，得到萬向車臺在全局坐標系中的位置坐標為（xo，yo）。

萬向車臺位姿確定包括位置和姿態角2部分的確定，即車體前進的方向與全局坐標系中X軸的正向夾角就是車臺相對于全局坐標系的方向角αo。系統中，全局坐標系與反光板的局部坐標系存在的關系如圖3所示。

圖3 坐標系之間的關系Fig.3 Relationship between coordinate systems

由圖可見，1塊反光板在全局坐標系中的坐標位置為（xi，yi，αi），在局部坐標系中的坐標為（xi′，yi′，αi′），萬向車臺導引參考坐標（全局坐標）（xo，yo，αo），其相互之間存在的關系為

由式（2）可得，第1塊反光板的方向角α1為

即

式中：α1，…，αi為根據第 1，…，i塊反光板得到萬向車臺的方向角；θ1，…，θi為檢測到第 1，…，i塊反光板在萬向車臺局部坐標系中的的方位角；（x1，y1），…，（xi，yi）為第 1，…，i塊反光板在全局坐標系中的坐標值；（xo，yo）為萬向車臺上安裝的激光掃描儀在全局坐標系中的坐標值。

由式（4）可得到的萬向車臺相對全局坐標系的方位角αo為

由式（1）和式（5）可得行走過程中萬向車臺確定的位姿（xo，yo，αo）。

2 模糊PID控制系統的設計

2.1 控制系統結構

模糊PID參數自整定控制系統在運行的過程中對一些不確定的因素，如參數、干擾、延遲等進行檢測分析，運用模糊推理的策略對PID的3個參數在線自整定，即具備常規PID控制系統的使用方便、魯棒性較強、原理簡單等優點，又具備適應性、靈活性、精確性等優點。

萬向車臺的控制器由常規PID控制器與模糊控制器2部分組成。模糊系統的輸入量為萬向車臺的位置偏差e與方向偏差θ；輸出量為PID的3個參數變化值。應用預先確定的模糊控制規則對PID的各個參數進行在線修改，結構如圖4所示。

圖4 控制系統Fig.4 Control system

2.2 常規PID控制器設計

常規PID控制器可用以下算法表示為

從系統的響應速度、穩態性、穩態誤差、超調量等條件考慮，比例系數kp，積分系數ki，微分系數kd具有的作用分別為kp能夠提高系統的調節精度，改變響應速度；ki能夠控制系統的穩態誤差；kd能夠控制系統的動態特性。

PID參數整定是對PID的 3個參數 kp，ki，kd與車臺的位置偏差e及方向偏差θ之間關系的模糊處理。在萬向車臺行走的過程中實時檢測e與θ的變化， 對 3個參數 Δkp，Δki，Δkd按照模糊控制規則進行在線修改，當e與θ不同時，滿足控制參數的不同要求，達到良好的控制性能。

2.3 模糊PID控制器設計

確定其變量的基本論域非常關鍵，關系到車體位置偏差e及方向偏差θ太大時，萬向車臺就失去了自動導引控制。在此，取

e 基本論域為［-15，15］；

θ 基本論域為［-15°，15°］；

kp基本論域為［-20，20］；

ki基本論域為［-5，5］；

kd基本論域為［-0.5，0.5］。

基本論域均為 7 個量化等級，均取｛-3，-2，-1，0，1，2，3｝，并設其子集為｛NB，NM，ZO，PS，PM，BP｝。假設，萬向車臺車體的縱軸線越過路徑中心線，在右邊時e為正，在左邊時e為負，α以順時針轉動方向為負。

模糊控制器的設計方法有理論法與試錯法2類。實際應用中結合這2種方法能夠得到更好的控制器。隸屬度函數采用靈敏度高的三角形函數，如圖5所示。

圖5 隸屬度函數Fig.5 Membership function

模糊控制器的設計核心是根據實際操作經驗與專業技術知識，建立標準的模糊規則表。考慮Δkp，Δki，Δkd這 3 個參數之間的相互影響，模糊推理規則運用Mamdani方法，為兩輸入三輸出的模糊控制系統，運用if-then控制規則——if（e is NB）and（θ is NM）then（Kp is PB）（Ki is NB）（Kd is PS），得出49條模糊語句，其規則見表1～3。

按照 Δkp，Δki，Δkd模糊控制規則表的信息，運用Mamdani組合推理知識[7]，推出相應的模糊關系，假設 R1，R2，R3，…，Rk，因此總的控制規則所對應的模糊關系R分別為

表1 Δkp的模糊控制規則Tab.1 Δkpfuzzy control rules

表2 Δki的模糊控制規則Tab.2 Δkifuzzy control rules

表3 Δkd的模糊控制規則Tab.3 Δkdfuzzy control rules

利用模糊推理合成規則，推出對應的修正參數代入

式中：分別為 PID 控制器參數 kp，ki，kd的初始參數。按照常規算法得到，在線運行不斷檢測系統的輸出值，實時計算其位置偏差與角度偏差，模糊化后得到e與θ；由模糊控制算法采用重心法去模糊化的修正參數與的和來確定 kp，ki，kd的調整量，實現控制器的參數調整。

3 引導路徑控制算法

萬向車臺驅動機構采用直流永磁電機，通過系統辨識將控制對象的傳遞函數簡化為二階系統，其傳遞函數為

通過MatLab的Simulink模塊在Windows 7系統環境下，根據定義的模糊推理控制規則，編寫模糊PID控制算法，并建立模糊PID控制器模型。

輸入信號選擇階躍信號，設定采樣時間為1 ms，同時加入常規閉環控制器與常規PID控制器進行比較。其中的摸糊PID控制器仿真模型如圖6所示，則導引路徑跟蹤控制器MatLab/Simulink的仿真模型如圖7所示。

圖6 控制器模型Fig.6 Controller model

圖7 導引路徑跟蹤模型Fig.7 Guidance path tracking model

模糊PID控制器的控制增益可調，而常規PID控制器增益固定，其模糊PID控制系統輸出響應仿真結果如圖8所示。在t=5 s時設置一個隨機干擾信號，其輸出響應及誤差響應的仿真結果如圖9所示。

圖8 模糊PID控制系統輸出結果Fig.8 Output results of fuzzy PID control system

圖9 設置干擾信號模糊PID控制系統的輸出結果Fig.9 Set the output results of fuzzy PID control system for interference signals

由仿真輸出結果得知，模糊PID控制器系統階躍輸出響應結果及誤差輸出響應結果與常規PID、常規閉環控制系統輸出響應結果相比，具有控制更加穩定，抗干擾能力強，調節時間短，超調相對較小，控制性能較好。

4 路徑導引性能測試

路徑導引性能直接決定著萬向車臺行走的準確性，為了測試萬向車臺導引糾偏能否達到舞臺應用的標準，進行模擬在劇場環境中路徑的特點，預設了一段試驗路徑，以預設路徑的中心點為定位誤差記錄零點，進行測試。將預設的路徑與試驗導引行走后的實際路徑取其中5點進行比較，測量出偏差量，假設在預設路徑內的偏差為負，則在預設路徑外的偏差為正。通過多次測試求平均偏差，對萬向車臺路徑導引誤差分析，實現路徑導引性能測試。測試所得數據見表4，萬向車臺預設路徑與試驗第1次導引行走路徑如圖10所示。

表4 測試數據Tab.4 Test data

圖10 預設與導引行走路徑Fig.10 Presupposition and guided walking path

5 結語

由萬向車臺預設的路徑與實際導引行走路徑比較可得，萬向車臺在導引行走的過程中，通常在拐彎出現抖動偏離預設路徑，在直道上基本能按照預設的路徑正常運行，當偏離預設路徑時，系統會進行快速的糾偏。通過詳細的試驗數據計算，該萬向車臺系統導引定位性能滿足舞臺應用的要求標準。

[1] 王文深，張淼淼，盧怡．基于模糊PID算法的MPD仿真控制研究[J]．石油機械，2017，45（6）：36-39．

[2] 陳澤宇，趙廣耀，翟麗，等．基于模糊PID算法的雙側電傳動履帶車輛轉向控制策略研究[J]．中國機械工程學報，2013，24（3）：410-425．

[3] 鄭偉勇，李艷瑋．基于前饋-反饋的移動機器人軌跡跟蹤控制[J]．計算機工程與控制，2017，38（2）：539-543．

[4] 任柯燕，顧幸生，黃敬平，等．基于模糊PID對AGV的糾偏控制[J]．控制工程，2006，13（S1）：54-57．

[5] 倪振．激光導引四輪差動全方位移動AGV關鍵技術研究研究[D]．重慶：重慶大學，2013．

[6] 丁永強，丁志強．一種萬向車臺驅動輪及運動學的研究[J]．機械研究與應用，2017，30（3）：57-60．

[7] Salehpour S，Pourasad Y，Taheri S H．Vehicle path tracking by integrated chassis control[J]．J Cent South Univ，2015，22（4）：1378-1388． ■