基于蟻群算法的機(jī)器人系統(tǒng)LQR最優(yōu)控制研究

武凌宇， 王曉東， 吳建德

(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500)

0 引 言

目前,對(duì)自平衡機(jī)器人[1]穩(wěn)定性控制的算法有很多，如PID、自適應(yīng)、最優(yōu)控制、智能控制等[2～7]，尤其利用線性二次最優(yōu)控制器(linear quadratic regulator,LQR)較傳統(tǒng)的PID控制可獲得更為穩(wěn)定的平衡過(guò)程，且使系統(tǒng)達(dá)到誤差跟蹤的效果更佳。針對(duì)LQR最優(yōu)控制存在的參數(shù)不確定性的問(wèn)題，起初出現(xiàn)較多人為參數(shù)整定方法，但該方法具有很大的滯后性和隨機(jī)性，增加控制系統(tǒng)的誤差和操作人員的工作強(qiáng)度，同時(shí)無(wú)法在線監(jiān)測(cè)機(jī)器人的姿態(tài)和位置估計(jì)，不利于自平衡機(jī)器人的控制。近年來(lái)，隨著傳統(tǒng)控制技術(shù)與現(xiàn)代計(jì)算機(jī)技術(shù)的結(jié)合，涌現(xiàn)出了一系列全新的組合式控制算法，可基于多種算法，如蟻群算法、遺傳算法、神經(jīng)網(wǎng)路、人工魚群算法、A*算法、免疫算法和粒子群算法等智能技術(shù)，用于參數(shù)、路徑尋優(yōu)等。文獻(xiàn)[6]提出了一種基于遺傳算法采用不同目標(biāo)函數(shù)的概念對(duì)權(quán)矩陣進(jìn)行優(yōu)化；文獻(xiàn)[8]利用參數(shù)滿意度對(duì)權(quán)矩陣進(jìn)行優(yōu)化，并且考慮了控制性能和系統(tǒng)能量之間的關(guān)系參數(shù),但是在實(shí)際應(yīng)用中,針對(duì)在特殊激勵(lì)源下進(jìn)行函數(shù)權(quán)矩陣優(yōu)化，以及在初始條件下可行解的構(gòu)造和在特殊課題、復(fù)雜環(huán)境下設(shè)計(jì)相應(yīng)算子(忽略外部環(huán)境)等問(wèn)題中，進(jìn)行的優(yōu)化策略具有局限性，應(yīng)用在實(shí)際系統(tǒng)中更是存在較大難度[8]。針對(duì)這些不足，本文引進(jìn)蟻群優(yōu)化(ant colony optimization,ACO)算法進(jìn)行控制器參數(shù)優(yōu)化。

本文基于蟻群算法對(duì)LQR最優(yōu)控制器的權(quán)矩陣Q中參數(shù)q11,q22,q33,q44進(jìn)行尋優(yōu)，并代入自平衡機(jī)器人系統(tǒng)模型進(jìn)行驗(yàn)證。實(shí)驗(yàn)證明：該方法優(yōu)于傳統(tǒng)人工選定LQR參數(shù)的整定方法，改善了人工優(yōu)化的隨機(jī)性和耗時(shí)性。

1 基于蟻群算法的LQR參數(shù)優(yōu)化

1.1 LQR最優(yōu)控制

對(duì)自平衡機(jī)器人建立狀態(tài)空間方程[9]

(1)

當(dāng)系統(tǒng)被外界干擾后，通過(guò)調(diào)節(jié)系統(tǒng)作出最快響應(yīng)，得到一個(gè)新的平衡狀態(tài)u*，使輸出跟蹤輸入，并保證性能指標(biāo)J最小。其充要條件為

u*=-Kx=-R-1BTPx

(2)

式中P為黎卡提方程(P，Q，R三者的關(guān)系式)的正定對(duì)稱解；K為線性最優(yōu)反饋增益矩陣。

二次型指標(biāo)函數(shù)為

(3)

式中Q為4×4維正半定對(duì)稱陣；加權(quán)陣R為2×2維正定對(duì)稱陣。

由文獻(xiàn)[10,11]對(duì)機(jī)器人建模，提取權(quán)矩陣參數(shù)

(4)

(5)

根據(jù)工程實(shí)踐，選擇不同的Q陣參數(shù)解，將R選為單位陣，僅對(duì)Q陣中的4個(gè)參量進(jìn)行優(yōu)化，同時(shí)選定4個(gè)量的范圍。要確定Q陣，必須確定q11,q22,q33,q44的數(shù)值，根據(jù)經(jīng)驗(yàn)值確定參數(shù)取值為5位數(shù)，整數(shù)部分2位，小數(shù)部分3位。數(shù)組(q11,q22,q33,q44)可以與20位數(shù)字序列相對(duì)應(yīng)，如圖1。則LQR參數(shù)優(yōu)化的問(wèn)題近似虛擬為經(jīng)典旅行商問(wèn)題(travel salesman problem,TSP)。

圖1 20位數(shù)字序列分配

依據(jù)文獻(xiàn)[12],實(shí)驗(yàn)開(kāi)始每一個(gè)橫坐標(biāo)點(diǎn)處有10個(gè)數(shù)字可供選擇(0～9)，而從橫坐標(biāo)看，每只螞蟻每次僅前進(jìn)一個(gè)單位長(zhǎng)度，最終經(jīng)過(guò)多次迭代，可以確定出一條最優(yōu)路徑，得到具有20個(gè)數(shù)字的序列，根據(jù)圖1，確定數(shù)組(q11,q22,q33,q44)。

1.2 蟻群算法[13～15]

圖2為搜索路徑的描述過(guò)程。圖2(a)為初始時(shí)刻，模擬路徑的分配，路徑BC和CE為1個(gè)單位距離，路徑BD和DE為2個(gè)單位距離，假設(shè)A為蟻穴，F(xiàn)為食物源，螞蟻從蟻穴出發(fā)尋找食物。圖2(b)為在0.5個(gè)單位時(shí)間時(shí)，螞蟻在B點(diǎn)處隨機(jī)選擇路徑，即概率相等。由于螞蟻沿途留下的信息素會(huì)隨著時(shí)間的推移逐漸消散揮發(fā)，信息素濃度大小表示路途的遠(yuǎn)近，而信息素會(huì)吸引更多的螞蟻重復(fù)這條路[16]，經(jīng)過(guò)1個(gè)單位時(shí)間的運(yùn)行，將產(chǎn)生最短路徑，如圖2(c)所示。

圖2 模擬螞蟻覓食路徑

1.3 控制器流程分析

根據(jù)上述設(shè)計(jì)方案得設(shè)計(jì)流程如圖3。

圖3 執(zhí)行流程

具體實(shí)現(xiàn)步驟如下：

1)程序開(kāi)始選擇m只人工螞蟻置于原點(diǎn)(起始點(diǎn))，設(shè)置算法初始參數(shù)：信息啟發(fā)式因子α，能見(jiàn)度的重要性β，信息素?fù)]發(fā)系數(shù)ρ，信息素初始濃度C，信息素強(qiáng)度素Q*和最大迭代次數(shù)N。

2)設(shè)置一個(gè)長(zhǎng)度為20的數(shù)組antk，k=1,2,3,…,m，存放螞蟻k走過(guò)的位置信息。

3)當(dāng)開(kāi)始執(zhí)行迭代時(shí)，螞蟻出發(fā)，每經(jīng)過(guò)一個(gè)節(jié)點(diǎn)時(shí),計(jì)算狀態(tài)轉(zhuǎn)移概率

(6)

式中τij為螞蟻從節(jié)點(diǎn)i轉(zhuǎn)移到節(jié)點(diǎn)j的信息素，即軌跡強(qiáng)度[12];ηij為能見(jiàn)度信息，通常取ηij=2/dij；α為信息啟發(fā)因子，表示途經(jīng)過(guò)的螞蟻在行進(jìn)過(guò)程中不斷積累的信息素對(duì)于當(dāng)前的螞蟻群體的啟發(fā)作用，數(shù)值越大，螞蟻協(xié)作性越強(qiáng)，越傾向于選擇其他螞蟻?zhàn)哌^(guò)的路徑，即隨意搜索性降低，而該值過(guò)小則會(huì)導(dǎo)致其值過(guò)早陷入局部最優(yōu)；期望啟發(fā)因子 (能見(jiàn)度的重要性)，能見(jiàn)度概念的引入可以使算法的收斂速度加快，但是其值過(guò)大則會(huì)造成人工螞蟻每次只選擇距離當(dāng)前節(jié)點(diǎn)最接近的候選節(jié)點(diǎn)，即會(huì)造成算法進(jìn)入局部最優(yōu)的窘?jīng)r，所以在實(shí)際中應(yīng)用時(shí)，期望啟發(fā)因子這一項(xiàng)需要忽略，則轉(zhuǎn)移概率最終將完全取決于信息素。

4)用輪盤賭的方法為螞蟻選擇下一節(jié)點(diǎn)，并移動(dòng)到該點(diǎn)，更新位置數(shù)組。

5)經(jīng)過(guò)20個(gè)單位時(shí)間后，所有螞蟻都經(jīng)過(guò)了20個(gè)整數(shù)點(diǎn)，一次迭代實(shí)現(xiàn)，得出相應(yīng)數(shù)組(q11，q22,q33,q44)，并且求出相應(yīng)評(píng)價(jià)值，評(píng)價(jià)指標(biāo)函數(shù)

(7)

式中T為采樣時(shí)間;LP為模擬計(jì)算點(diǎn)。當(dāng)系統(tǒng)穩(wěn)定時(shí)間過(guò)長(zhǎng)或者超調(diào)過(guò)大等情況時(shí)，Qe會(huì)較大。

6)當(dāng)m只人工螞蟻完成一次路徑搜索后，對(duì)兩節(jié)點(diǎn)之間的信息素含量進(jìn)行更新，更新信息素如下

(8)

(9)

(10)

式中F為信息強(qiáng)度素，其影響算法收斂速度，是一個(gè)常量，表示螞蟻k在當(dāng)前路徑搜索中釋放的信息素總濃度；Lk為螞蟻k本次路徑搜索所走過(guò)的路徑總長(zhǎng)度。

7)重復(fù)執(zhí)行步驟(3)～步驟(6)，選取每次迭代的最優(yōu)路徑，直到達(dá)到最大迭代次數(shù)。

2 仿真實(shí)驗(yàn)分析

2.1 設(shè)置實(shí)驗(yàn)裝置

兩輪自平衡機(jī)器人以嵌入式運(yùn)動(dòng)控制器GUS為控制核心，操作系統(tǒng)為WinCE，滿足工業(yè)IEC61131—3標(biāo)準(zhǔn)，OtoStudio為軟件應(yīng)用平臺(tái)，運(yùn)行仿真平臺(tái)PC采用Intel(R)Core(TM)i7—3537U，采用64位操作系統(tǒng)，基于X64的處理器。利用陀螺儀作為傾角位置傳感器，ISM4803作為驅(qū)動(dòng)芯片模塊，為方便控制機(jī)器人運(yùn)行，并觀察和記錄運(yùn)行中的狀態(tài)，采用工業(yè)觸摸顯示屏。圖4為機(jī)器人控制系統(tǒng)結(jié)構(gòu)。

圖4 兩輪自平衡機(jī)器人控制系統(tǒng)結(jié)構(gòu)

2.2 實(shí)驗(yàn)結(jié)果與比較

規(guī)定初始參數(shù)：m=100,α=1，ρ=0.5,Q*=100，各路徑初始信息素含量值為20000，為了避免算法得到局部最優(yōu)結(jié)果取消了能見(jiàn)度系數(shù)的作用，故令β=0。用橫坐標(biāo)為0～20，縱坐標(biāo)為0～9的二維坐標(biāo)顯示結(jié)果，N次迭代后，最終蟻群算法運(yùn)行結(jié)果如圖5。20位數(shù)字序列為50754096680851208041，得參數(shù)結(jié)果為q11=50.754,q22=9.668,q33=8.512,q44=8.041。

圖5 蟻群算法最優(yōu)路徑結(jié)果

將結(jié)果帶入自平衡機(jī)器人的模型仿真中，得到機(jī)器人俯仰通道階躍響應(yīng)仿真曲線如圖6，輸出分別為機(jī)器人的位移(m)、速度(m/s)、俯仰角度(rad)以及機(jī)器人俯仰角速度(rad/s)。與傳統(tǒng)人工確定參數(shù)對(duì)比，如表1，可以看出：輸出為機(jī)器人位移量時(shí)，人工選擇較蟻群算法選擇的調(diào)節(jié)時(shí)間ts多0.63s；輸出為機(jī)器人速度量時(shí)，人工選擇較蟻群算法選擇的谷值時(shí)間tL和調(diào)節(jié)時(shí)間tS分別多0.11s，0.59s；輸出為機(jī)器人俯仰角度時(shí)，人工選擇比蟻群算法選擇的谷值時(shí)間tL,峰值時(shí)間tP和調(diào)節(jié)時(shí)間tS分別多0.03,0.22,0.31s，最大偏差值是蟻群算法選擇較人工選擇僅多0.006rad；俯仰角速度量，人工選擇較蟻群算法選擇的峰值時(shí)間tP和調(diào)節(jié)時(shí)間tS分別多0.06,0.67s，最大偏差值是蟻群算法選擇較人工選擇僅多0.022rad/s。

表1 俯仰通道人工選擇參數(shù)與蟻群算法選擇參數(shù)調(diào)節(jié)時(shí)間結(jié)果對(duì)比

圖6 俯仰通道階躍響應(yīng)仿真曲線

相較較于人工試探法多次選定最優(yōu)參數(shù)結(jié)果，基于蟻群算法的LQR參數(shù)選定方案響應(yīng)速度更快，明顯縮短了調(diào)節(jié)時(shí)間，而系統(tǒng)輸出量的最大偏差值相對(duì)誤差較小，對(duì)整體系統(tǒng)的穩(wěn)定性影響可忽略。實(shí)驗(yàn)結(jié)果表明，蟻群算法選擇的權(quán)值使得機(jī)器人在俯仰通道上更快速的平穩(wěn)，達(dá)到更優(yōu)的自平衡效果。

3 結(jié) 論

本文以兩輪自平衡機(jī)器人為研究對(duì)象，通過(guò)對(duì)其控制器參數(shù)優(yōu)化，得到如下結(jié)論：

1)利用蟻群算法可以優(yōu)化LQR最優(yōu)控制器的權(quán)矩陣，通過(guò)仿真實(shí)驗(yàn)結(jié)果驗(yàn)證所設(shè)計(jì)算法的實(shí)用性和有效性。

2)與傳統(tǒng)人工選擇方法比較，結(jié)果表明：改進(jìn)后的算法能夠快速確定參數(shù)值，提高了系統(tǒng)動(dòng)態(tài)的穩(wěn)定性，縮短控制時(shí)間，減輕了人工優(yōu)化參數(shù)的工作量，改善了算法參數(shù)的隨機(jī)性和耗時(shí)性。

3)僅利用蟻群算法對(duì)權(quán)矩陣參數(shù)提出優(yōu)化策略，此外利用多算法融合策略可以對(duì)最大偏差值進(jìn)行優(yōu)化，實(shí)驗(yàn)效果有待在后續(xù)工作中進(jìn)一步完成。

[1] 阮曉鋼.兩輪自平衡機(jī)器人的研究與設(shè)計(jì)[M].北京:科學(xué)出版社,2012:1-12.

[2] 劉浩梅,張昌凡.基于LQR的環(huán)形單擊倒立擺穩(wěn)定控制及實(shí)現(xiàn)[J].中南大學(xué)學(xué)報(bào),2012,43(9):3496-3501.

[3] 符 川,屈鐵軍,孫世國(guó).動(dòng)調(diào)頻液柱阻尼器基于遺傳算法的LQR控制優(yōu)化設(shè)計(jì)[J].振動(dòng)與沖擊,2015,34(2):210-214.

[4] 馬永杰,云文霞.遺傳算法研究進(jìn)展[J].計(jì)算機(jī)應(yīng)用研究,2012,29(4):1201-1206.

[5] 胡壽松,王執(zhí)銓,胡維禮.最優(yōu)控制理論與系統(tǒng)[M].北京:科學(xué)出版社,2005.

[6] 郭一峰,徐趙東,涂 青,等.基于遺傳算法的LQR算法中權(quán)矩陣的優(yōu)化分析[J].振動(dòng)與沖擊,2010,29(11):217-220.

[7] 史恩秀,陳敏敏,李 俊，等.基于蟻群算法的移動(dòng)機(jī)器人全局路徑規(guī)劃方法研究[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2014,45(6):53-57.

[8] 張葛祥,金煒東,胡來(lái)招.多變量系統(tǒng)控制的參數(shù)滿意優(yōu)化設(shè)計(jì)[J].控制理論與應(yīng)用,2004,21(3):362-366.

[9] 屠運(yùn)武,徐俊艷,張培仁,等.自平衡控制系統(tǒng)的建模與仿真[J].系統(tǒng)仿真學(xué)報(bào),2004,16(4):839-341.

[10] 宋西蒙.倒立擺系統(tǒng)LQR—模糊控制算法研究[D].西安:西安電子科技大學(xué),2006：33-36.

[11] 吳春明,陳 治,姜 明.一群算法中系統(tǒng)初始化及系統(tǒng)參數(shù)的研究[J].電子學(xué)報(bào),2006,34(8):1530-1533.

[12] 樊曉平,羅 熊,易 晟,等.復(fù)雜環(huán)境下基于蟻群優(yōu)化算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].控制與決策,2004,19(2):166-170.

[13] 鮑慶勇,李舜酩,沈 峘,等.自主移動(dòng)機(jī)器人局部路徑規(guī)劃綜述[J].傳感器與微系統(tǒng),2009,28(9):1-4.

[14] Erin B,Abiyev R,Ibrahim D.Teaching robot navigation in the presence of obstacles using a computer simulation program[J].Procedia-Social and Behavioral Sciences,2010,2(2):565-571.

[15] 張波雷,許蘊(yùn)山,夏海寶.一種基于自適應(yīng)蟻群算法的數(shù)據(jù)關(guān)聯(lián)方法[J].傳感器與微系統(tǒng),2012,31(8):27-29.

[16] 朱慶保，楊志軍.基于變異和動(dòng)態(tài)信息素更新的蟻群優(yōu)化算法[J].軟件學(xué)報(bào)，2004，15(2)：185-192.