單路徑導(dǎo)引的車式移動(dòng)機(jī)器人協(xié)同編隊(duì)控制

王常順 王 丹 彭周華

(1.大連海事大學(xué)船舶電氣工程學(xué)院,遼寧大連 116000;2.山東交通學(xué)院信息科學(xué)與電氣工程學(xué)院,山東濟(jì)南 250357)

1 引言

在瀝青路面就地?zé)嵩偕B(yǎng)護(hù)中,通常由2–4臺(tái)加熱機(jī)和銑刨機(jī)編組后沿施工車道列隊(duì)低速行駛以滿足施工的工藝需求[1].限于瀝青的粘度特性,加熱后的路面溫度高達(dá)160?C.為減少施工人員在惡劣高溫環(huán)境下的工作時(shí)間,降低人力成本,提高施工質(zhì)量和經(jīng)濟(jì)效益,研究就地?zé)嵩偕鷻C(jī)組協(xié)同控制系統(tǒng)來(lái)實(shí)現(xiàn)機(jī)組列隊(duì)自動(dòng)駕駛具有重要的應(yīng)用價(jià)值.編組中各工程車輛均采用前輪轉(zhuǎn)向,后輪驅(qū)動(dòng)的底盤結(jié)構(gòu),可視為車式移動(dòng)機(jī)器人[2].

軌跡跟蹤[2–8]是移動(dòng)機(jī)器人集群控制[9–19]的基礎(chǔ),得到了很多研究者的關(guān)注.例如,文獻(xiàn)[3]基于滑?？刂扑惴ㄔO(shè)計(jì)了直流電機(jī)驅(qū)動(dòng)的CLMRs路徑跟蹤控制器,實(shí)現(xiàn)了曲線軌跡跟蹤.文獻(xiàn)[4]設(shè)計(jì)的滑模軌跡跟蹤控制器實(shí)現(xiàn)了精準(zhǔn)的并行倒車.但滑?？刂浦械母哳l抖振會(huì)給液壓式轉(zhuǎn)向機(jī)構(gòu)帶來(lái)嚴(yán)重的磨損.文獻(xiàn)[5]基于神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)了模型預(yù)測(cè)控制器,實(shí)現(xiàn)了包含不確定性參數(shù)的移動(dòng)機(jī)器人的軌跡跟蹤控制.但受限于精準(zhǔn)的數(shù)學(xué)模型和復(fù)雜的計(jì)算量,工程實(shí)現(xiàn)上具備一定的難度.文獻(xiàn)[6–7]利用LESO將系統(tǒng)未建模動(dòng)態(tài)和模型不確定性當(dāng)作外部擾動(dòng)進(jìn)行補(bǔ)償,設(shè)計(jì)了移動(dòng)機(jī)器人軌跡跟蹤自抗擾控制器,便于工程實(shí)現(xiàn),但未展開(kāi)針對(duì)CLMRs編隊(duì)控制的研究.

移動(dòng)機(jī)器人的編隊(duì)控制研究中,常采用領(lǐng)航–跟隨法[8–12,17–22]、虛擬結(jié)構(gòu)法[13–14]和基于行為法[15]等策略.文獻(xiàn)[8]基于交通流理論提出了一種汽車隊(duì)列縱向魯棒控制器,但其橫向控制精度無(wú)法達(dá)到施工要求.文獻(xiàn)[9]通過(guò)跟蹤期望速度和角速度,實(shí)現(xiàn)兩臺(tái)移動(dòng)機(jī)器人運(yùn)動(dòng)中保持相對(duì)固定的間距和角度.文獻(xiàn)[11]進(jìn)一步實(shí)現(xiàn)了4臺(tái)移動(dòng)機(jī)器人在操場(chǎng)形狀軌跡上的編隊(duì)控制,但曲線路徑的跟蹤性能和隊(duì)形保持的精度無(wú)法令人滿意.為更好保持隊(duì)形,文獻(xiàn)[16–18]將編隊(duì)控制問(wèn)題轉(zhuǎn)換為軌跡跟蹤問(wèn)題,根據(jù)各車式移動(dòng)機(jī)器人在編隊(duì)中的位置,單獨(dú)計(jì)算出軌跡參數(shù),在運(yùn)動(dòng)學(xué)層面上實(shí)現(xiàn)了CLMRs編隊(duì)控制,但沒(méi)有進(jìn)行運(yùn)動(dòng)學(xué)協(xié)同和動(dòng)力學(xué)控制相關(guān)的研究.文獻(xiàn)[19]利用RBF神經(jīng)網(wǎng)絡(luò)逼近并補(bǔ)償CLMRs系統(tǒng)模型的非線性部分和未知擾動(dòng),通過(guò)多路徑導(dǎo)引實(shí)現(xiàn)了固定隊(duì)形控制.文獻(xiàn)[15,20]中的分布式編隊(duì)控制器同樣僅適用于多條路徑導(dǎo)引的固定隊(duì)形控制.然而受施工道路未知曲率的影響,彎道上的機(jī)組集群隊(duì)形難以準(zhǔn)確描述隊(duì)形.另一方面因天氣等外部因素,施工中會(huì)不斷調(diào)整領(lǐng)航車速和車間距,上述方法需不斷計(jì)算并更新跟隨機(jī)器人的軌跡參數(shù),增加了系統(tǒng)的計(jì)算負(fù)擔(dān),甚至引起不穩(wěn)定,降低了系統(tǒng)的實(shí)用性.

本文針對(duì)施工背景下的CLMRs集群,首先在運(yùn)動(dòng)學(xué)層級(jí)設(shè)計(jì)了分布式協(xié)同編隊(duì)制導(dǎo)律,通過(guò)單條參數(shù)化路徑導(dǎo)引CLMRs列隊(duì)行駛,減輕路徑參數(shù)更新引起的計(jì)算和通信負(fù)擔(dān),規(guī)避了負(fù)速度;然后在動(dòng)力學(xué)層級(jí)基于LESO設(shè)計(jì)了帶有外部擾動(dòng)補(bǔ)償?shù)腃LMRs前向速度和姿態(tài)角控制器,降低速度超調(diào)和轉(zhuǎn)向操縱頻率,提高了橫向控制精度;最后通過(guò)級(jí)聯(lián)系統(tǒng)穩(wěn)定性理論分析了閉環(huán)系統(tǒng)的穩(wěn)定性,并通過(guò)數(shù)字仿真和加熱機(jī)群實(shí)驗(yàn)結(jié)果驗(yàn)證了控制器的有效性.

2 問(wèn)題描述

2.1 CLMRs數(shù)學(xué)模型

為實(shí)現(xiàn)編組中工程車輛沿施工路線以隊(duì)列形式自動(dòng)駕駛,本文研究的車式移動(dòng)機(jī)器人采用圖1所示的阿克曼結(jié)構(gòu),通過(guò)后輪提供前進(jìn)動(dòng)力,通過(guò)操縱前輪控制行進(jìn)方向.在一個(gè)由N臺(tái)車式移動(dòng)機(jī)器人和M個(gè)虛擬領(lǐng)導(dǎo)者組成的系統(tǒng)中,第i個(gè)車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型[2–3]可表示為

圖1 車式移動(dòng)機(jī)器人示意圖Fig.1 Structure of CLMRs

式中:pi=(xi yi)T表示CLMRs在固定坐標(biāo)系XEYE下的位置,φi表示車體的姿態(tài)角,θi表示前輪的方向角,L表示后輪到前輪的軸距,qi=(vi ωi)T是載體坐標(biāo)系下機(jī)器人的行進(jìn)速度和前輪側(cè)轉(zhuǎn)角速度.

與差速式移動(dòng)機(jī)器人不同,CLMRs的轉(zhuǎn)向角速度與前向速度有關(guān),也無(wú)法實(shí)現(xiàn)原地轉(zhuǎn)向.運(yùn)動(dòng)過(guò)程中所受的非完整約束可表示為

CLMRs運(yùn)動(dòng)主要受縱向牽引力及轉(zhuǎn)向力控制,考慮到實(shí)際運(yùn)行中前輪方向角通常比較小,其動(dòng)力學(xué)模型[2,10]可表示為

式中:τvi為牽引力,τwi為施加在轉(zhuǎn)向輪上的力矩,Fui為前進(jìn)方向所受的摩擦力,Fwi為橫向受力,和為其他摩擦力與外部干擾之和,Ji,Jb,Jv,Jh為系統(tǒng)中不確定的慣性系數(shù),與CLMRs參數(shù)有關(guān),r為車輪半徑,cθ=(mi+Jb+4Jv)/L2+Jh/r2.

2.2 路徑生成方法

本研究中通過(guò)虛擬領(lǐng)導(dǎo)者引導(dǎo)CLMRs沿期望路徑運(yùn)動(dòng),所需參數(shù)化路徑[21–22]至少是二階連續(xù)的.在實(shí)際應(yīng)用中可以采用最小二乘法或樣條插值對(duì)路徑曲線進(jìn)行擬合,進(jìn)而生成該參數(shù)化路徑.對(duì)于期望路徑上的任意點(diǎn),用prk=(xk(δ)yk(δ))T表示其位置,用表示其姿態(tài)角,其中:δ是與時(shí)間獨(dú)立的變量,

為了實(shí)現(xiàn)虛擬領(lǐng)導(dǎo)者和CLMRs的協(xié)同運(yùn)動(dòng),將虛擬領(lǐng)航者路徑參數(shù)的導(dǎo)數(shù)設(shè)計(jì)為

綜上分析,可將CLMRs的軌跡跟蹤誤差[19]記為

聯(lián)立式(1),可得到軌跡跟蹤誤差動(dòng)態(tài)為

2.3 控制目標(biāo)

本文的控制目標(biāo)為:考慮由式(1)和式(3)表示的CLMRs集群,通過(guò)設(shè)計(jì)協(xié)同路徑跟隨控制律和使得每個(gè)車式機(jī)器人都能以設(shè)定間距pdij和設(shè)定速度vr跟隨給定的參數(shù)化路徑,且誤差收斂至零點(diǎn)附近的有界臨域內(nèi),也即存在任意小的正常數(shù),滿足:

3 控制器設(shè)計(jì)

本節(jié)將從運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)兩個(gè)層面設(shè)計(jì)車式移動(dòng)機(jī)器人的協(xié)同編隊(duì)控制器,圖2為所設(shè)計(jì)的編隊(duì)控制器組成結(jié)構(gòu).

圖2 編隊(duì)控制器組成結(jié)構(gòu)Fig.2 Structure of formation controller

3.1 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

假設(shè)1每個(gè)車式移動(dòng)機(jī)器人與虛擬領(lǐng)航者之間都能進(jìn)行有向通信,并獲取臨近機(jī)器人的運(yùn)動(dòng)狀態(tài)信息.

3.2 動(dòng)力學(xué)控制器設(shè)計(jì)

在第3.1節(jié)中,為集群中的車式移動(dòng)機(jī)器人設(shè)計(jì)了期望前向速度和姿態(tài)角.本節(jié)中將設(shè)計(jì)CLMRs的動(dòng)力學(xué)控制器,使得實(shí)際前進(jìn)速度和姿態(tài)角可以滿足運(yùn)動(dòng)學(xué)控制器的要求.但在實(shí)際工程應(yīng)用中,由于工況改變以及運(yùn)動(dòng)中會(huì)受到外部干擾等因素影響,模型中部分參數(shù)會(huì)產(chǎn)生攝動(dòng),造成CLMRs模型中參數(shù)難以精確辨識(shí).將式(3)中CLMRs的動(dòng)力學(xué)模型重寫為

借鑒自抗擾控制器中利用ESO估計(jì)被控對(duì)象外部擾動(dòng)的方法[6–7],文中基于LESO分別設(shè)計(jì)了前向速度和姿態(tài)角控制器.

3.2.1 前向速度控制器

對(duì)式(14)所示的前向速度控制系統(tǒng),設(shè)計(jì)二階的LESO為

式中:kvi為系統(tǒng)控制增益,ξ2i為正常數(shù).

3.2.2 姿態(tài)角控制器

聯(lián)立式(1)和式(15),可得CLMRs的姿態(tài)角控制系統(tǒng)

由式(19)可知,姿態(tài)角控制系統(tǒng)不是單純的積分串聯(lián)系統(tǒng),下面將結(jié)合反步法思想完成控制器設(shè)計(jì).

首先,引入虛擬控制量uφi=vitanθi/L,為姿態(tài)角控制系統(tǒng)設(shè)計(jì)二階LESO如下:

式中:kφi為控制增益,ξ3i為正常數(shù).

注1實(shí)際系統(tǒng)中考慮到CLMRs執(zhí)行部件的輸出連續(xù)且功率有限,而因外在環(huán)境引起的系統(tǒng)擾動(dòng)能量也有限,因此,fvi,fwi和均連續(xù)有界且可導(dǎo),即假設(shè)2是合理的.

4 穩(wěn)定性分析

本節(jié)將逐步分析LESO子系統(tǒng)、動(dòng)力學(xué)子系統(tǒng)、運(yùn)動(dòng)學(xué)子系統(tǒng)的穩(wěn)定性,最后分析文中所設(shè)計(jì)的閉環(huán)系統(tǒng)穩(wěn)定性.

4.1 LESO子系統(tǒng)穩(wěn)定性

因?yàn)锳1i,A2i,A3i都是Hurwitz矩陣,所以存在正定矩陣P1i,P2i和P3i使不等式(31)–(33)成立

式中:ε1i,ε2i,ε3i都是正數(shù),I2和I3分別表示2×2和3×3單位矩陣.

定理1在滿足假設(shè)2的前提下,式(28)所示的LESO前向速度控制子系統(tǒng):是輸入到狀態(tài)穩(wěn)定(input-to-state stable,ISS)的.

證針對(duì)式(28)所示的子系統(tǒng),選取如下Lyapunov方程

對(duì)式(34)求導(dǎo),可得

式中ζ1i ∈(0,1).根據(jù)文獻(xiàn)[23]中的定理4.6可知,式(28)所示的LESO子系統(tǒng)是ISS的,且存在

式中:λmax(·)和λmin(·)分別表示矩陣的最大特征值和最小特征值,t0表示初始時(shí)間. 證畢.

因式(29)和式(30)所示LESO子系統(tǒng)的證明過(guò)程與定理1類似,在此不再一一贅述.

定理2在滿足假設(shè)2的前提下,式(29)所示的LESO轉(zhuǎn)向角控制子系統(tǒng)是ISS的,且估計(jì)誤差的邊界為

式中ζ2i ∈(0,1).

定理3在滿足假設(shè)2的前提下,式(30)所示的LESO姿態(tài)控制子系統(tǒng)是ISS的,且估計(jì)誤差的邊界為

式中ζ3i ∈(0,1).

4.2 動(dòng)力學(xué)子系統(tǒng)穩(wěn)定性

因運(yùn)動(dòng)學(xué)中的速度跟蹤誤差和姿態(tài)跟蹤誤差是有界的,可知?jiǎng)恿W(xué)跟蹤誤差也是有界的. 證畢.

4.3 運(yùn)動(dòng)學(xué)系統(tǒng)穩(wěn)定性分析

4.4 級(jí)聯(lián)系統(tǒng)穩(wěn)定性分析

定理1–5分別證明了閉環(huán)系統(tǒng)中各個(gè)子系統(tǒng)的穩(wěn)定性,接下來(lái)將分析由這些子系統(tǒng)級(jí)聯(lián)的CLMRs協(xié)同編隊(duì)閉環(huán)系統(tǒng)的穩(wěn)定性.

定理6考慮由多個(gè)車式移動(dòng)機(jī)器人構(gòu)成的分布式協(xié)同編隊(duì)系統(tǒng),其中,式(1)和式(3)分別表述了其運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程,式(11)表述了其運(yùn)動(dòng)學(xué)控制律,式(12)表述了其路徑更新律,式(16)和式(19)表述了基于LESO 的動(dòng)力學(xué)控制器.若假設(shè)1 和2 滿足,則CLMRs協(xié)同編隊(duì)閉環(huán)控制系統(tǒng)是ISS的.

證根據(jù)定理1–5,由式(17)(21)(24)和式(26)中的子系統(tǒng)組成的級(jí)聯(lián)系統(tǒng)是ISS的,且當(dāng)t →∞時(shí),有

式(50)表明,閉環(huán)系統(tǒng)中所有信號(hào)都是一致最終有界的. 證畢.

5 試驗(yàn)驗(yàn)證與結(jié)果分析

本節(jié)對(duì)4臺(tái)如圖1所示的車式移動(dòng)機(jī)器人集群進(jìn)行協(xié)同編隊(duì)試驗(yàn)測(cè)試,其通信拓?fù)浣Y(jié)構(gòu)如圖3所示,分別通過(guò)仿真試驗(yàn)和4臺(tái)工程車輛編隊(duì)測(cè)試來(lái)驗(yàn)證所設(shè)計(jì)控制器的有效性和控制性能.

圖3 通訊拓?fù)浣Y(jié)構(gòu)Fig.3 Communication topology

5.1 仿真測(cè)試

考慮由虛擬領(lǐng)航者和CR1–CR4組成的CLMRs集群,仿真中CLMRs選取的物理參數(shù)[19]如表1所示.給定虛擬領(lǐng)航者的期望速度vd=1.7 m/s,期望隊(duì)列間距pij=34 m.為驗(yàn)證CLMRs集群形成期望隊(duì)列并沿期望路徑運(yùn)動(dòng)的性能,初始狀態(tài)時(shí),將CLMRs隨機(jī)分布在期望路徑的兩側(cè),且因回轉(zhuǎn)半徑較大,仿真中設(shè)定CLMRs的初始方向與起始路徑點(diǎn)夾角不超過(guò)90?,初始值如表2所示.

表1 CLMRs參數(shù)Table 1 CLMRs parameters

表2 仿真中CLMRs的初始狀態(tài)Table 2 Initial states of CLMRs in simulation

為了進(jìn)一步驗(yàn)證本文控制器的效果,參照文獻(xiàn)[19]中的神經(jīng)網(wǎng)絡(luò)PID算法進(jìn)行了仿真對(duì)比試驗(yàn),得到的仿真結(jié)果如圖4–7所示.圖中分別展示了協(xié)同編隊(duì)的CLMRs運(yùn)動(dòng)軌跡、姿態(tài)角、前向速度及轉(zhuǎn)向輪的偏轉(zhuǎn)角度.

圖4 CLMRs路徑跟隨性能Fig.4 Path following performance

從圖中可以看出,CLMRs能夠按期望的等間距隊(duì)形跟蹤期望路徑,跟蹤誤差小,前進(jìn)速度穩(wěn)定,姿態(tài)角及車輪方向角變化平穩(wěn),無(wú)振蕩現(xiàn)象,說(shuō)明所設(shè)計(jì)控制器實(shí)現(xiàn)了CLMRs協(xié)同編隊(duì)控制,且對(duì)系統(tǒng)參數(shù)攝動(dòng)和外部擾動(dòng)具備很好的抑制作用.相對(duì)于文獻(xiàn)[19]中的控制方法,本文控制器可以有效地減小速度沖擊和姿態(tài)角振蕩的幅度,降低操縱轉(zhuǎn)向的幅度和頻率,具有明顯的工程應(yīng)用優(yōu)勢(shì).

圖5 姿態(tài)角Fig.5 Attitude angle

圖6 前向運(yùn)動(dòng)速度Fig.6 Forward velocity

圖7 轉(zhuǎn)向車輪角度Fig.7 Steering wheel angle

5.2 現(xiàn)場(chǎng)實(shí)車測(cè)試

為進(jìn)一步測(cè)試該控制器的性能,將所設(shè)計(jì)的控制器應(yīng)用到與山東省路橋集團(tuán)公司合作研發(fā)的就地?zé)嵩偕訜釞C(jī)組智能駕駛系統(tǒng)中.控制目標(biāo)是讓4臺(tái)加熱機(jī)編隊(duì)沿施工路線自動(dòng)行駛,行駛速度穩(wěn)定,橫向偏差不大于50 mm.該套機(jī)組底盤采用后輪驅(qū)動(dòng)和前輪轉(zhuǎn)向的阿克曼結(jié)構(gòu),前進(jìn)液壓馬達(dá)和轉(zhuǎn)向液壓缸通過(guò)比例閥控制,從而實(shí)現(xiàn)前進(jìn)速度和前輪角度的控制.

圖8 就地?zé)嵩偕訜釞C(jī)群Fig.8 Hot-in-place preheater vehicles

該系統(tǒng)中單車設(shè)計(jì)重量mi ≈18000 kg,軸距Li=13.75 m.施工前首先通過(guò)最小二乘法和樣條插值方法對(duì)施工車道進(jìn)行擬合處理,得到一組以施工起始點(diǎn)為原點(diǎn)的連續(xù)光滑參數(shù)路徑.機(jī)組通過(guò)RTK–DGPS和INS提供組合定位信息,然后進(jìn)行高斯投影運(yùn)算,可得到精度±10 mm的定位信息.因?qū)C(jī)組加熱裝置預(yù)熱時(shí)不能原地停留,通常人工駕駛機(jī)組進(jìn)入施工路段后再開(kāi)始自動(dòng)駕駛,此時(shí)已形成隊(duì)列且橫向偏差小于200 mm,但縱向偏差較大.隨著施工的進(jìn)行,路面曲率和坡度不斷變化,需要所設(shè)計(jì)的控制器能夠通過(guò)操縱方向盤和比例閥,控制加熱機(jī)組以設(shè)定間距的隊(duì)列沿施工路徑無(wú)偏差自動(dòng)駕駛.實(shí)驗(yàn)測(cè)試在濟(jì)南遙墻機(jī)場(chǎng)附近的青銀高速西向東路面養(yǎng)護(hù)施工中展開(kāi),經(jīng)采集后得到的實(shí)驗(yàn)路段參數(shù)路徑為:y=0.00096x3+0.006x2?0.26775x ?0.015841.

圖9中展示了施工中加熱機(jī)組的行駛軌跡,圖10展示了各車的橫向路徑跟蹤偏差及縱向距離偏差.從圖中可以看出,該系統(tǒng)實(shí)現(xiàn)了4臺(tái)加熱機(jī)組的協(xié)同編隊(duì)控制,行駛速度穩(wěn)定,控制精度符合預(yù)期目標(biāo).

圖9 機(jī)組定位信息Fig.9 Position information of vehicles

圖10 機(jī)組跟蹤誤差Fig.10 Tracking errors of vehicles

6 結(jié)論

本文針對(duì)含有模型不確定性和未知外部擾動(dòng)的車式移動(dòng)機(jī)器人集群編隊(duì)控制問(wèn)題進(jìn)行研究,首先設(shè)計(jì)了分布式協(xié)同編隊(duì)制導(dǎo)律和基于LESO的前進(jìn)速度及姿態(tài)控制律,實(shí)現(xiàn)了單路徑引導(dǎo)下的CLMRs協(xié)同編隊(duì)控制,然后利用級(jí)聯(lián)系統(tǒng)穩(wěn)定性理論分析了閉環(huán)系統(tǒng)穩(wěn)定性,最后通過(guò)數(shù)字仿真和加熱機(jī)群實(shí)驗(yàn)結(jié)果驗(yàn)證了控制器的有效性.