王占山, 徐 朋

(1.蘇州工業(yè)職業(yè)技術(shù)學(xué)院,江蘇 蘇州 215104,E-mail:feifei0001feifei@163.com;2.燕山大學(xué) 機(jī)械工程學(xué)院,河北 秦皇島 066004)

在移動(dòng)機(jī)器人應(yīng)用領(lǐng)域中,路徑優(yōu)化問題占有相當(dāng)重要的比重。如何在具有障礙物的復(fù)雜環(huán)境下,迅速求解并規(guī)劃出一條從起點(diǎn)到終點(diǎn)距離最短、路徑最精確的優(yōu)化路徑是目前移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn)問題[1-3]。目前移動(dòng)機(jī)器人路徑優(yōu)化主要是基于PID控制的蟻群算法。傳統(tǒng)的PID控制器對其非線性、時(shí)變、耦合及參數(shù)和結(jié)構(gòu)不確定的復(fù)雜過程,其控制能力較差[4]。依據(jù)數(shù)學(xué)模式去調(diào)整PID控制器需要用復(fù)雜的數(shù)學(xué)計(jì)算,且PID控制器比例增益KP是固定的,因而導(dǎo)致PID控制器缺少適應(yīng)能力,而蟻群算法也有穩(wěn)定性不強(qiáng),容易收斂于局部極值的缺點(diǎn)[5-6]。

在移動(dòng)機(jī)器人控制及路徑優(yōu)化算法研究方面,N Khan等[7]研究了移動(dòng)機(jī)器人的動(dòng)態(tài)系統(tǒng)為非線性時(shí),依據(jù)線性控制方法提出幾個(gè)控制策略,例如利用PI及PD的參數(shù)調(diào)整方式求解,以及將自調(diào)式PID控制器應(yīng)用于移動(dòng)機(jī)器人,但依舊難以解決控制器適應(yīng)能力弱的缺點(diǎn)。趙高杰等[8]提出一種變步長的蟻群優(yōu)化算法,某種程度上解決了移動(dòng)機(jī)器人在柵格狀態(tài)下的路徑優(yōu)化問題,但是算法中的弗洛蒙信息素被限制,導(dǎo)致路徑在非柵格狀態(tài)下容易出現(xiàn)輕微波動(dòng),影響移動(dòng)機(jī)器人的優(yōu)化效果。Kim等[9]提出一種動(dòng)態(tài)環(huán)境下探索移動(dòng)機(jī)器人路徑優(yōu)化的新方法,但是該方法在局部路徑優(yōu)化及避障礙方面誤差較大,不太適合實(shí)際應(yīng)用環(huán)境。Jyoti等[10]發(fā)展出一種智能型基因算法,找尋最佳路徑,從而完成移動(dòng)機(jī)器人到達(dá)終點(diǎn)的目標(biāo)并完成避障任務(wù),但是仿真證明其誤差收斂性較差,收斂于零的速度較慢。熊君麗等[11]提出一種移動(dòng)機(jī)器人基于動(dòng)態(tài)窗口控制的改進(jìn)蟻群路徑優(yōu)化算法,有效解決了移動(dòng)機(jī)器人局部路徑和全局路徑動(dòng)態(tài)規(guī)劃問題,但是其所提出的蟻群算法搜索時(shí)間偏長的問題依舊沒有完美解決。陸州等[12]提出一種包括克隆、疫苗接種、選擇否定等具免疫思想的移動(dòng)機(jī)器人路徑搜索及優(yōu)化方法,有效解決了移動(dòng)機(jī)器人的全局規(guī)劃問題,但其建立的坐標(biāo)工作模型不適合人工智能時(shí)代移動(dòng)機(jī)器人的自適應(yīng)控制問題[13-14]。

基于以上問題,本文提出一種基于自適應(yīng)性控制的類免疫路徑優(yōu)化算法,在移動(dòng)機(jī)器人的路徑優(yōu)化、避障及誤差收斂性等方面進(jìn)行全面分析[15-17]。本文首先介紹了移動(dòng)機(jī)器人的三維結(jié)構(gòu)模型,然后提出了基于自適應(yīng)小腦模型控制器設(shè)計(jì)的類免疫算法流程,分析了避障策略和運(yùn)動(dòng)模型[18-20]。最后通過仿真及實(shí)驗(yàn)說明了本文所提方法的正確性。

1 自適應(yīng)移動(dòng)機(jī)器人結(jié)構(gòu)模型

自適應(yīng)移動(dòng)機(jī)器人的結(jié)構(gòu)如圖1所示,采用四肢移動(dòng)結(jié)構(gòu)反向安裝,框架起到支撐作用,內(nèi)部安裝有電源裝置、驅(qū)動(dòng)模塊、控制模塊以及空間姿態(tài)傳感器,頂部安裝有燕尾槽,可以用于固定搭載物體。在運(yùn)動(dòng)過程中,利用空間姿態(tài)傳感器,實(shí)時(shí)監(jiān)測自適應(yīng)移動(dòng)機(jī)器人與地面的傾角狀態(tài),傳輸?shù)娇刂颇K,根據(jù)各種工況下的運(yùn)動(dòng)模式進(jìn)行計(jì)算分析,主動(dòng)調(diào)整四肢移動(dòng)結(jié)構(gòu)高度,從而保證移動(dòng)過程中的平穩(wěn)性。

圖1 自適應(yīng)移動(dòng)機(jī)器人內(nèi)部結(jié)構(gòu)示意圖

自適應(yīng)移動(dòng)機(jī)器人移動(dòng)結(jié)構(gòu),主要由主動(dòng)輪式結(jié)構(gòu),連桿機(jī)械臂,主動(dòng)電缸組成。主動(dòng)輪式結(jié)構(gòu)為驅(qū)動(dòng)行進(jìn)機(jī)構(gòu),由步進(jìn)電機(jī),行星減速器,驅(qū)動(dòng)輪組成,結(jié)構(gòu)緊湊,動(dòng)力強(qiáng)勁。連桿機(jī)械臂分為下支臂和上支臂,二者之間通過轉(zhuǎn)軸連接,可以相互轉(zhuǎn)動(dòng),在連桿機(jī)械臂上安裝有主動(dòng)電缸。當(dāng)?shù)孛娓叨绕鸱鼤r(shí),主動(dòng)電缸根據(jù)檢測參數(shù)計(jì)算獲得的伸縮量,主動(dòng)驅(qū)動(dòng)電缸伸縮,從而為自適應(yīng)機(jī)器人提供平穩(wěn)支持,保證在運(yùn)輸貴重、危險(xiǎn)等對穩(wěn)定性要求較高的物體的平穩(wěn),具體結(jié)構(gòu)如圖2所示。

圖2 移動(dòng)結(jié)構(gòu)示意圖

2 類免疫算法(AIA)求解最佳路徑規(guī)劃問題

2.1 類免疫算法最佳路徑規(guī)劃

(1) 抗體群的產(chǎn)生:在類免疫算法求解全局路徑規(guī)劃問題中,最重要的就是抗體群的決定。一條無障礙物的可行路徑包括起點(diǎn),終點(diǎn)和中間點(diǎn),在類免疫算法中,可以將一條路徑上的所有節(jié)點(diǎn)集合視為一個(gè)抗體。算法一開始先以隨機(jī)均勻的方式產(chǎn)生這些抗體群,也就是產(chǎn)生數(shù)個(gè)可能的最佳解,經(jīng)由類免疫系統(tǒng)的演化,演化出最佳記憶細(xì)胞,也就是最佳路徑。

(2) 親合度的定義:在類免疫演算求解全局路徑規(guī)劃問題上,親合度的定義為路徑的距離,也就是路徑節(jié)點(diǎn)中,從起點(diǎn)到終點(diǎn)路徑的總長度。

綜合以上所述,應(yīng)用類免疫算法求解全局最佳路徑規(guī)劃可分成以下幾個(gè)步驟完成:

步驟1:建立Grid-based的環(huán)境模型,包括起點(diǎn)、終點(diǎn)和靜態(tài)障礙物。

步驟2:設(shè)定類免疫算法的執(zhí)行參數(shù),編碼,和迭代數(shù)目。

步驟3:類免疫抗體的初始化,也就是產(chǎn)生最佳路徑的候選解。

步驟 4:計(jì)算每個(gè)抗體的親合度。

步驟5:進(jìn)行株落選擇并更新抗體基因,使類免疫算法能演算化最佳記憶細(xì)胞。

步驟6:判斷結(jié)束條件是否成立,若成立則輸出類免疫算法得到的最佳解,否則,繼續(xù)下一次迭代,回到步驟4。

由類免疫算法得到的路徑是不連續(xù)的,從機(jī)器人的控制器角度而言,這個(gè)不連續(xù)的路徑必須被平滑化,以獲得連續(xù)平滑的軌跡。有了平滑化后的軌跡,即可由上述設(shè)計(jì)的類免疫控制器來追蹤平滑處理后的連續(xù)軌跡,實(shí)現(xiàn)路徑追蹤的目的。要解決這個(gè)路徑平滑化的問題,目前使用最多的是利用B-spline內(nèi)插法來完成所規(guī)劃的路徑平滑化目標(biāo)。本計(jì)劃將在軟件上實(shí)現(xiàn)B-spline路徑平滑技術(shù),并整合在類免疫路徑規(guī)劃器內(nèi),完成路徑平滑的目標(biāo),如圖3所示,其中P為控制點(diǎn)。以下僅針對B-spline曲線的基本特性和數(shù)學(xué)模型做簡要的說明。

圖3 平滑化前后的路徑比較

本論文提出了類免疫算法及其在移動(dòng)機(jī)器人的整合應(yīng)用,包括調(diào)整控制器參數(shù),設(shè)計(jì)最佳控制器和設(shè)計(jì)最佳全局路徑規(guī)劃器,并結(jié)合B-spline路徑平滑器,以驗(yàn)證這些方法的有效性和優(yōu)點(diǎn)。并使自主性移動(dòng)機(jī)器人順利的從起點(diǎn)到終點(diǎn),追蹤平滑的路徑,順利的避開障礙物,到達(dá)目的地,完成全局路徑規(guī)劃的任務(wù)。主要分成三個(gè)部份:

(一) 設(shè)計(jì)最佳控制器:以類免疫算法快速的調(diào)整控制器參數(shù),并設(shè)計(jì)最佳運(yùn)動(dòng)控制器。

(二) 全局路徑規(guī)劃:在靜態(tài)環(huán)境已知的情況下,針對機(jī)器人不同的任務(wù),規(guī)劃出最佳的路徑。

(三)路徑平滑化與變形:由全局路徑規(guī)劃器所得到的路徑通常都是由數(shù)個(gè)不連續(xù)的線段所組成,以運(yùn)動(dòng)控制的角度來看,追蹤這些不連續(xù)的點(diǎn),機(jī)器人必須在不連續(xù)點(diǎn)停住,再追蹤下一個(gè)點(diǎn),這會(huì)增加到達(dá)目標(biāo)的時(shí)間。所以路徑必須經(jīng)過平滑化,本論文采用B-spline路徑平滑器,使機(jī)器人能夠順利的追蹤平滑化的路徑,到達(dá)終點(diǎn)。

2.2 自適應(yīng)小腦模型控制器設(shè)計(jì)

建立一個(gè)多輸入多輸出的小腦模式控制器,其輸出為:

zCMAC=F(s)

F:RLRn是CMAC輸入變量s=[s1,…,sL]T∈S?RL的S集合與輸出變量zCMAC=[zCMAC1,…,zCMACn]T∈Z?Rn的Z集合的非線性函數(shù)。為了模仿人類小腦的運(yùn)作,系統(tǒng)輸入輸出透過具有關(guān)聯(lián)記憶空間A,在空間A內(nèi)的任何元素都由0或1組成。依輸入型態(tài)而定,在數(shù)學(xué)上可借助一對函數(shù)映像描述關(guān)系式來描述小腦模式控制器輸入與輸出之間的關(guān)系:

G:S→A;saG(s)=a(s)∈A

(1)

P:ARn;aaP(a)

(2)

選擇函數(shù)P產(chǎn)生輸出zCMAC∈Rn如下:

zCMAC=P(a)=(aTW)T

(3)

式中:a∈RM為關(guān)聯(lián)向量;W≡[w1,…,wn]∈RM×n是小腦模式控制器的權(quán)重矩陣。

為達(dá)到實(shí)時(shí)控制及穩(wěn)定控制系統(tǒng)的目的,結(jié)合模糊集合以及小腦模型,提出模糊小腦模型控制器,并借助參數(shù)化的方式實(shí)時(shí)修正其輸出。

考慮雙輸入變量的問題,具有N個(gè)模糊規(guī)則的模糊系統(tǒng),將被設(shè)計(jì)成如下的型式:

(4)

(5)

其中:

(6)

此方程式可以被簡化為:

zAFCMAC=(hTAW)T

(7)

(8)

(9)

在方程式(7),矩陣A(是由CMAC決定)及向量h(由模糊規(guī)則決定,因應(yīng)數(shù)入而改變),而權(quán)重向量w是可調(diào)變的。

3 避障策略及運(yùn)動(dòng)學(xué)模型

3.1 避障策略

由上述類免疫算法判斷出移動(dòng)機(jī)器人的移動(dòng)軌跡,需要為機(jī)器人在行走途中能自主判斷突發(fā)的障礙物并進(jìn)行避障動(dòng)作制定避障策略,待避障結(jié)束后再回到軌跡追蹤模式繼續(xù)朝目的地前進(jìn)。本章將聚焦討論于避障策略,并利用Kinect傳感器判斷障礙物,判斷方式為圖4表示。

圖4 避障計(jì)算方式

圖4中A為移動(dòng)機(jī)器人到障礙物間距離,B為中線到障礙物邊界的距離加上移動(dòng)機(jī)器人寬度,C為所需移動(dòng)的距離,已知A與B時(shí)就能計(jì)算出C,就能準(zhǔn)確地計(jì)算出需要轉(zhuǎn)動(dòng)多少角度,以及移動(dòng)多少距離,方能避開障礙物。當(dāng)傳感器偵測到接近障礙物時(shí),開始進(jìn)行位移控制并藉此達(dá)成避障功能。

3.2 運(yùn)動(dòng)學(xué)模型

移動(dòng)機(jī)器人的幾何架構(gòu)如圖5所示。

圖5 移動(dòng)機(jī)器人幾何架構(gòu)圖

圖5中r為輪子的半徑,V為輪子的移動(dòng)速度,L為機(jī)器人中心點(diǎn)至每個(gè)輪子的距離,δ為移動(dòng)機(jī)器人的輪子1與卡氏坐標(biāo)x的夾角,機(jī)器人因運(yùn)動(dòng)產(chǎn)生偏轉(zhuǎn)角以θ表示。

參考圖5移動(dòng)機(jī)器人幾何架構(gòu)圖,可推導(dǎo)出機(jī)器人之運(yùn)動(dòng)學(xué)模型如下:

(10)

將上式寫成輪的速度向量矩陣可得下式:

(11)

定義系統(tǒng)的控制輸入訊號(hào)u如下:

(12)

其中:

(13)

將(13)整理成非線性系統(tǒng)的形式:

(14)

其中:

(15)

4 圓形軌跡仿真及實(shí)驗(yàn)驗(yàn)證

4.1 仿真分析

設(shè)定移動(dòng)機(jī)器人、目標(biāo)軌跡及AFCMAC的初始參數(shù):

設(shè)移動(dòng)機(jī)器人的初始參數(shù),L為輪子至中心點(diǎn)距離,r為輪子半徑,δ為車身出發(fā)的角度:

L=0.23m,r=0.05m,δ=π/4 rad

目標(biāo)軌跡初始參數(shù):

初始坐標(biāo):[x0y0θ0]=[0 m 0 m 0 rad]

目標(biāo)函數(shù):

[xryrθr]=[cos(t) m sin(t)m 0 rad],0≤t≤6移動(dòng)機(jī)器人起點(diǎn),隨著時(shí)間t變化,以此設(shè)計(jì)圓形軌跡追蹤。

AFCMAC的初始參數(shù):

表1 圓形軌跡初始參數(shù)

4.2 仿真結(jié)果

圖6-圖8分別表示圓形追蹤軌跡仿真、圓形軌跡追蹤誤差仿真、移動(dòng)機(jī)器人各輪轉(zhuǎn)速仿真。其中,圖6分別表示了移動(dòng)機(jī)器人基于AFCMAC控制的類免疫算法和基于PID控制的蟻群算法路徑仿真,通過與期望軌跡仿真對比可以看到基于AFCMAC的類免疫算法與期望軌跡有著更好的吻合度,基于PID控制的蟻群算法相比期望軌跡吻合度相對較差,有一定誤差。所以充分證明本文所提算法的正確性。

圖6 圓形追蹤軌跡仿真

由仿真圖7可知,移動(dòng)機(jī)器人的位置誤差和姿態(tài)誤差都快速趨近于零,說明基于AFCMAC控制的類免疫算法在移動(dòng)機(jī)器人運(yùn)動(dòng)誤差精度方面是符合要求的。由仿真圖8可知移動(dòng)機(jī)器人各輪轉(zhuǎn)速曲線均處于平滑狀態(tài),從運(yùn)動(dòng)控制角度進(jìn)一步說明本文基于AFCMAC控制的類免疫算法是合理和正確的。

圖7 圓形軌跡追蹤誤差仿真

圖8 移動(dòng)機(jī)器人各輪轉(zhuǎn)速仿真

4.3 實(shí)驗(yàn)驗(yàn)證

為從實(shí)驗(yàn)角度來驗(yàn)證本文所用基于AFCMAC控制的類免疫算法的正確性,現(xiàn)以某公司生產(chǎn)的移動(dòng)機(jī)器人樣機(jī)圍繞圓形軌跡進(jìn)行實(shí)驗(yàn)驗(yàn)證,實(shí)驗(yàn)場景如圖9所示。

圖9 實(shí)驗(yàn)場景圖

首先設(shè)定反饋時(shí)間及目標(biāo)軌跡的初始參數(shù):

反饋時(shí)間初始參數(shù):

tf=0.1 s

式中:tf為嵌入式系統(tǒng)獲取移動(dòng)機(jī)器人電機(jī)反饋的時(shí)間。

目標(biāo)軌跡初始參數(shù):

初始坐標(biāo):

[x0y0θ0]=[0 m 0 m 0 rad]

目標(biāo)函數(shù):

[xryrθr]=[r1·cosωrtmr2·sinωrtm0 rad]

其中:

角速度ωr=1/(rad·s-1),短軸長r1=100 m,長軸長r2=150 m。

實(shí)驗(yàn)數(shù)據(jù)仿真圖分別如圖10和圖11所示。

圖10 圓形軌跡追蹤軌跡實(shí)驗(yàn)圖

圖11 追蹤誤差實(shí)驗(yàn)圖

由圖10可以看出基于AFCMAC控制的類免疫算法的移動(dòng)機(jī)器人進(jìn)行圓形軌跡追蹤,能夠精確到達(dá)目標(biāo)參考點(diǎn),并由圖11看出三個(gè)參數(shù)最終可以收斂到誤差趨近于零的結(jié)果,進(jìn)一步從實(shí)驗(yàn)角度證明了本文所用方法的正確性。實(shí)驗(yàn)結(jié)果如下:

5 結(jié)論

(1) 本文提出一種移動(dòng)機(jī)器人基于自適應(yīng)性小腦控制為核心的類免疫路徑優(yōu)化算法,通過不停迭代演化出最佳記憶細(xì)胞,然后通過自適應(yīng)性小腦控制器實(shí)現(xiàn)移動(dòng)機(jī)器人的最優(yōu)路徑規(guī)劃。

(2) 通過避障策略及移動(dòng)機(jī)器人的幾何架構(gòu)圖推導(dǎo)出運(yùn)動(dòng)學(xué)模型,基于運(yùn)動(dòng)學(xué)模型進(jìn)行移動(dòng)機(jī)器人的路徑仿真和誤差仿真。

(3) 最后,通過仿真分析對比及實(shí)驗(yàn)驗(yàn)證,證明本文所提基于自適應(yīng)性小腦控制的類免疫路徑優(yōu)化算法相比于基于PID控制的蟻群算法在移動(dòng)機(jī)器人路徑規(guī)劃方面有著更高的優(yōu)化精度和誤差迭代收斂性。