基于模糊PID對(duì)萬向車臺(tái)路徑引導(dǎo)控制

湯子龍 ，丁志強(qiáng)，丁永強(qiáng)，高世燕

（1.蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州 730050；2.甘肅工大舞臺(tái)技術(shù)工程有限公司，蘭州 730050；3.甘肅正隆工程招標(biāo)代理有限責(zé)任公司，天水 741000）

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

實(shí)現(xiàn)激光自動(dòng)導(dǎo)引萬向車臺(tái)控制的基礎(chǔ)是路徑跟蹤偏差值的矯正[4-6]，也是研發(fā)萬向車臺(tái)的關(guān)鍵技術(shù)。萬向車臺(tái)導(dǎo)引系統(tǒng)的精度由定位精度決定，因PID控制具有可靠、簡(jiǎn)單、魯棒性好等優(yōu)點(diǎn)，在工業(yè)控制領(lǐng)域普遍應(yīng)用，但模糊控制器、模糊PID等控制方法具有更強(qiáng)的適應(yīng)性，應(yīng)用時(shí)取得了更好的控制效果。因此，文中采取模糊PID控制器實(shí)現(xiàn)萬向車臺(tái)的路徑導(dǎo)引控制，其穩(wěn)定性、魯棒性、易用性更好，是一種最優(yōu)偏差控制導(dǎo)引路徑的方法。

1 激光導(dǎo)引系統(tǒng)定位原理與位姿計(jì)算

1.1 激光定位算法原理

要實(shí)現(xiàn)準(zhǔn)確定位，萬向車臺(tái)在行走的過程中激光掃描傳感器同時(shí)掃描到反射板不少于3塊，根據(jù)反射板的位置信息，按照激光定位算法在預(yù)設(shè)的全局坐標(biāo)系中確定萬向車臺(tái)的坐標(biāo)與方位，然后反饋給車臺(tái)微控制器進(jìn)行處理。常用的定位算法有三角定位算法和三邊定位算法。

1.1.1 三角定位算法

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

1.1.2 三邊定位算法

三邊定位算法通過測(cè)量萬向車臺(tái)與全局坐標(biāo)中預(yù)制的反光板之間的距離來實(shí)現(xiàn)定位。如圖1（b）所示，測(cè)量車體到3個(gè)反光板A，B，C的距離分別為R1，R2，R3；以 R1，R2，R3為半徑，分別以 3 個(gè)反光板A，B，C的中心為圓心作圓，由圓的性質(zhì)得出車體的位置在3個(gè)圓的交點(diǎn)處。三邊定位方法與三角定位方法相比，實(shí)現(xiàn)更加容易、精確，且對(duì)硬件環(huán)境條件要求低，在GPS定位中成功的應(yīng)用，故文中采用三邊定位算法。

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

1.2 位姿計(jì)算

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

圖2 反光板的動(dòng)態(tài)匹配Fig.2 Dynamic matching of reflective plate

構(gòu)建的m個(gè)圓方程為

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

萬向車臺(tái)位姿確定包括位置和姿態(tài)角2部分的確定，即車體前進(jìn)的方向與全局坐標(biāo)系中X軸的正向夾角就是車臺(tái)相對(duì)于全局坐標(biāo)系的方向角αo。系統(tǒng)中，全局坐標(biāo)系與反光板的局部坐標(biāo)系存在的關(guān)系如圖3所示。

圖3 坐標(biāo)系之間的關(guān)系Fig.3 Relationship between coordinate systems

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

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

即

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

由式（4）可得到的萬向車臺(tái)相對(duì)全局坐標(biāo)系的方位角αo為

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

2 模糊PID控制系統(tǒng)的設(shè)計(jì)

2.1 控制系統(tǒng)結(jié)構(gòu)

模糊PID參數(shù)自整定控制系統(tǒng)在運(yùn)行的過程中對(duì)一些不確定的因素，如參數(shù)、干擾、延遲等進(jìn)行檢測(cè)分析，運(yùn)用模糊推理的策略對(duì)PID的3個(gè)參數(shù)在線自整定，即具備常規(guī)PID控制系統(tǒng)的使用方便、魯棒性較強(qiáng)、原理簡(jiǎn)單等優(yōu)點(diǎn)，又具備適應(yīng)性、靈活性、精確性等優(yōu)點(diǎn)。

萬向車臺(tái)的控制器由常規(guī)PID控制器與模糊控制器2部分組成。模糊系統(tǒng)的輸入量為萬向車臺(tái)的位置偏差e與方向偏差θ；輸出量為PID的3個(gè)參數(shù)變化值。應(yīng)用預(yù)先確定的模糊控制規(guī)則對(duì)PID的各個(gè)參數(shù)進(jìn)行在線修改，結(jié)構(gòu)如圖4所示。

圖4 控制系統(tǒng)Fig.4 Control system

2.2 常規(guī)PID控制器設(shè)計(jì)

常規(guī)PID控制器可用以下算法表示為

從系統(tǒng)的響應(yīng)速度、穩(wěn)態(tài)性、穩(wěn)態(tài)誤差、超調(diào)量等條件考慮，比例系數(shù)kp，積分系數(shù)ki，微分系數(shù)kd具有的作用分別為kp能夠提高系統(tǒng)的調(diào)節(jié)精度，改變響應(yīng)速度；ki能夠控制系統(tǒng)的穩(wěn)態(tài)誤差；kd能夠控制系統(tǒng)的動(dòng)態(tài)特性。

PID參數(shù)整定是對(duì)PID的 3個(gè)參數(shù) kp，ki，kd與車臺(tái)的位置偏差e及方向偏差θ之間關(guān)系的模糊處理。在萬向車臺(tái)行走的過程中實(shí)時(shí)檢測(cè)e與θ的變化， 對(duì) 3個(gè)參數(shù) Δkp，Δki，Δkd按照模糊控制規(guī)則進(jìn)行在線修改，當(dāng)e與θ不同時(shí)，滿足控制參數(shù)的不同要求，達(dá)到良好的控制性能。

2.3 模糊PID控制器設(shè)計(jì)

確定其變量的基本論域非常關(guān)鍵，關(guān)系到車體位置偏差e及方向偏差θ太大時(shí)，萬向車臺(tái)就失去了自動(dòng)導(dǎo)引控制。在此，取

e 基本論域?yàn)椋?15，15］；

θ 基本論域?yàn)椋?15°，15°］；

kp基本論域?yàn)椋?20，20］；

ki基本論域?yàn)椋?5，5］；

kd基本論域?yàn)椋?0.5，0.5］。

基本論域均為 7 個(gè)量化等級(jí)，均?。?3，-2，-1，0，1，2，3｝，并設(shè)其子集為｛NB，NM，ZO，PS，PM，BP｝。假設(shè)，萬向車臺(tái)車體的縱軸線越過路徑中心線，在右邊時(shí)e為正，在左邊時(shí)e為負(fù)，α以順時(shí)針轉(zhuǎn)動(dòng)方向?yàn)樨?fù)。

模糊控制器的設(shè)計(jì)方法有理論法與試錯(cuò)法2類。實(shí)際應(yīng)用中結(jié)合這2種方法能夠得到更好的控制器。隸屬度函數(shù)采用靈敏度高的三角形函數(shù)，如圖5所示。

圖5 隸屬度函數(shù)Fig.5 Membership function

模糊控制器的設(shè)計(jì)核心是根據(jù)實(shí)際操作經(jīng)驗(yàn)與專業(yè)技術(shù)知識(shí)，建立標(biāo)準(zhǔn)的模糊規(guī)則表?？紤]Δkp，Δki，Δkd這 3 個(gè)參數(shù)之間的相互影響，模糊推理規(guī)則運(yùn)用Mamdani方法，為兩輸入三輸出的模糊控制系統(tǒng)，運(yùn)用if-then控制規(guī)則——if（e is NB）and（θ is NM）then（Kp is PB）（Ki is NB）（Kd is PS），得出49條模糊語句，其規(guī)則見表1～3。

按照 Δkp，Δki，Δkd模糊控制規(guī)則表的信息，運(yùn)用Mamdani組合推理知識(shí)[7]，推出相應(yīng)的模糊關(guān)系，假設(shè) R1，R2，R3，…，Rk，因此總的控制規(guī)則所對(duì)應(yīng)的模糊關(guān)系R分別為

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

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

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

利用模糊推理合成規(guī)則，推出對(duì)應(yīng)的修正參數(shù)代入

式中：分別為 PID 控制器參數(shù) kp，ki，kd的初始參數(shù)。按照常規(guī)算法得到，在線運(yùn)行不斷檢測(cè)系統(tǒng)的輸出值，實(shí)時(shí)計(jì)算其位置偏差與角度偏差，模糊化后得到e與θ；由模糊控制算法采用重心法去模糊化的修正參數(shù)與的和來確定 kp，ki，kd的調(diào)整量，實(shí)現(xiàn)控制器的參數(shù)調(diào)整。

3 引導(dǎo)路徑控制算法

萬向車臺(tái)驅(qū)動(dòng)機(jī)構(gòu)采用直流永磁電機(jī)，通過系統(tǒng)辨識(shí)將控制對(duì)象的傳遞函數(shù)簡(jiǎn)化為二階系統(tǒng)，其傳遞函數(shù)為

通過MatLab的Simulink模塊在Windows 7系統(tǒng)環(huán)境下，根據(jù)定義的模糊推理控制規(guī)則，編寫模糊PID控制算法，并建立模糊PID控制器模型。

輸入信號(hào)選擇階躍信號(hào)，設(shè)定采樣時(shí)間為1 ms，同時(shí)加入常規(guī)閉環(huán)控制器與常規(guī)PID控制器進(jìn)行比較。其中的摸糊PID控制器仿真模型如圖6所示，則導(dǎo)引路徑跟蹤控制器MatLab/Simulink的仿真模型如圖7所示。

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

圖7 導(dǎo)引路徑跟蹤模型Fig.7 Guidance path tracking model

模糊PID控制器的控制增益可調(diào)，而常規(guī)PID控制器增益固定，其模糊PID控制系統(tǒng)輸出響應(yīng)仿真結(jié)果如圖8所示。在t=5 s時(shí)設(shè)置一個(gè)隨機(jī)干擾信號(hào)，其輸出響應(yīng)及誤差響應(yīng)的仿真結(jié)果如圖9所示。

圖8 模糊PID控制系統(tǒng)輸出結(jié)果Fig.8 Output results of fuzzy PID control system

圖9 設(shè)置干擾信號(hào)模糊PID控制系統(tǒng)的輸出結(jié)果Fig.9 Set the output results of fuzzy PID control system for interference signals

由仿真輸出結(jié)果得知，模糊PID控制器系統(tǒng)階躍輸出響應(yīng)結(jié)果及誤差輸出響應(yīng)結(jié)果與常規(guī)PID、常規(guī)閉環(huán)控制系統(tǒng)輸出響應(yīng)結(jié)果相比，具有控制更加穩(wěn)定，抗干擾能力強(qiáng)，調(diào)節(jié)時(shí)間短，超調(diào)相對(duì)較小，控制性能較好。

4 路徑導(dǎo)引性能測(cè)試

路徑導(dǎo)引性能直接決定著萬向車臺(tái)行走的準(zhǔn)確性，為了測(cè)試萬向車臺(tái)導(dǎo)引糾偏能否達(dá)到舞臺(tái)應(yīng)用的標(biāo)準(zhǔn)，進(jìn)行模擬在劇場(chǎng)環(huán)境中路徑的特點(diǎn)，預(yù)設(shè)了一段試驗(yàn)路徑，以預(yù)設(shè)路徑的中心點(diǎn)為定位誤差記錄零點(diǎn)，進(jìn)行測(cè)試。將預(yù)設(shè)的路徑與試驗(yàn)導(dǎo)引行走后的實(shí)際路徑取其中5點(diǎn)進(jìn)行比較，測(cè)量出偏差量，假設(shè)在預(yù)設(shè)路徑內(nèi)的偏差為負(fù)，則在預(yù)設(shè)路徑外的偏差為正。通過多次測(cè)試求平均偏差，對(duì)萬向車臺(tái)路徑導(dǎo)引誤差分析，實(shí)現(xiàn)路徑導(dǎo)引性能測(cè)試。測(cè)試所得數(shù)據(jù)見表4，萬向車臺(tái)預(yù)設(shè)路徑與試驗(yàn)第1次導(dǎo)引行走路徑如圖10所示。

表4 測(cè)試數(shù)據(jù)Tab.4 Test data

圖10 預(yù)設(shè)與導(dǎo)引行走路徑Fig.10 Presupposition and guided walking path

5 結(jié)語

由萬向車臺(tái)預(yù)設(shè)的路徑與實(shí)際導(dǎo)引行走路徑比較可得，萬向車臺(tái)在導(dǎo)引行走的過程中，通常在拐彎出現(xiàn)抖動(dòng)偏離預(yù)設(shè)路徑，在直道上基本能按照預(yù)設(shè)的路徑正常運(yùn)行，當(dāng)偏離預(yù)設(shè)路徑時(shí)，系統(tǒng)會(huì)進(jìn)行快速的糾偏。通過詳細(xì)的試驗(yàn)數(shù)據(jù)計(jì)算，該萬向車臺(tái)系統(tǒng)導(dǎo)引定位性能滿足舞臺(tái)應(yīng)用的要求標(biāo)準(zhǔn)。

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

[2] 陳澤宇，趙廣耀，翟麗，等．基于模糊PID算法的雙側(cè)電傳動(dòng)履帶車輛轉(zhuǎn)向控制策略研究[J]．中國(guó)機(jī)械工程學(xué)報(bào)，2013，24（3）：410-425．

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

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

[5] 倪振．激光導(dǎo)引四輪差動(dòng)全方位移動(dòng)AGV關(guān)鍵技術(shù)研究研究[D]．重慶：重慶大學(xué)，2013．

[6] 丁永強(qiáng)，丁志強(qiáng)．一種萬向車臺(tái)驅(qū)動(dòng)輪及運(yùn)動(dòng)學(xué)的研究[J]．機(jī)械研究與應(yīng)用，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． ■