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

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

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

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

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

1.1 激光定位算法原理

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

1.1.1 三角定位算法

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

1.1.2 三邊定位算法

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

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

1.2 位姿計算

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

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

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

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

萬向車臺位姿確定包括位置和姿態(tài)角2部分的確定，即車體前進(jìn)的方向與全局坐標(biāo)系中X軸的正向夾角就是車臺相對于全局坐標(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′），萬向車臺導(dǎo)引參考坐標(biāo)（全局坐標(biāo)）（xo，yo，αo），其相互之間存在的關(guān)系為

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

即

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

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

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

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

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

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

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

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

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

常規(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)的動態(tài)特性。

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

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

確定其變量的基本論域非常關(guān)鍵，關(guān)系到車體位置偏差e及方向偏差θ太大時，萬向車臺就失去了自動導(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 個量化等級，均?。?3，-2，-1，0，1，2，3｝，并設(shè)其子集為｛NB，NM，ZO，PS，PM，BP｝。假設(shè)，萬向車臺車體的縱軸線越過路徑中心線，在右邊時e為正，在左邊時e為負(fù)，α以順時針轉(zhuǎn)動方向?yàn)樨?fù)。

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

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

模糊控制器的設(shè)計核心是根據(jù)實(shí)際操作經(jīng)驗(yàn)與專業(yè)技術(shù)知識，建立標(biāo)準(zhǔn)的模糊規(guī)則表。考慮Δkp，Δki，Δkd這 3 個參數(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組合推理知識[7]，推出相應(yīng)的模糊關(guān)系，假設(shè) R1，R2，R3，…，Rk，因此總的控制規(guī)則所對應(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ī)則，推出對應(yīng)的修正參數(shù)代入

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

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

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

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

輸入信號選擇階躍信號，設(shè)定采樣時間為1 ms，同時加入常規(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è)置一個隨機(jī)干擾信號，其輸出響應(yīng)及誤差響應(yīng)的仿真結(jié)果如圖9所示。

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

圖9 設(shè)置干擾信號模糊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é)時間短，超調(diào)相對較小，控制性能較好。

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

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

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

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

5 結(jié)語

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

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

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

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

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

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

[6] 丁永強(qiáng)，丁志強(qiáng)．一種萬向車臺驅(qū)動輪及運(yùn)動學(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． ■