基于多傳感器融合的機(jī)器人編隊ADRC控制

毛文勇，仇 翔，張文安

(浙江工業(yè)大學(xué)信息工程學(xué)院，杭州 310023)

0 引言

近些年，隨著機(jī)器人和人工智能的快速發(fā)展，機(jī)器人領(lǐng)域的變化趨勢也從固定的機(jī)械裝置向能夠做更多復(fù)雜任務(wù)的自主移動機(jī)器人過渡。如未知環(huán)境探測、遠(yuǎn)程醫(yī)療、物流、服務(wù)業(yè)等領(lǐng)域［1-2］。移動機(jī)器人控制系統(tǒng)是一種非完整系統(tǒng)，其控制問題一直是個難題，而其中的編隊控制更是移動機(jī)器人控制中的關(guān)鍵問題之一。

在移動機(jī)器人的編隊問題的研究已經(jīng)有許多研究成果。例如，利用李亞普諾夫函數(shù)設(shè)計速度誤差控制規(guī)律，實(shí)現(xiàn)目標(biāo)位置的跟蹤控制。但是缺點(diǎn)在于動態(tài)控制器過于理想化，在實(shí)際中很難實(shí)現(xiàn)［3］。智能控制律被用于多智能體系統(tǒng)，并實(shí)現(xiàn)了同步跟蹤［4］。神經(jīng)網(wǎng)絡(luò)控制方法雖然具有學(xué)習(xí)能力，但學(xué)習(xí)速度慢，神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)選擇復(fù)雜［5-6］。此外，還有基于行為的方法［78］、虛擬結(jié)構(gòu)法［9］、領(lǐng)航跟隨法［10-11］以及其他智能控制算法［12］。盡管軌跡跟蹤問題已有大量研究結(jié)果，但多數(shù)結(jié)果只是依靠仿真來驗(yàn)證算法的有效性。在實(shí)際應(yīng)用中，就需要考慮更多因素，例如如何提高移動機(jī)器人定位精度從而提高軌跡跟蹤控制精度，其次，由于環(huán)境的未知，干擾的不確定等因素都會對控制方法的實(shí)際實(shí)施效果產(chǎn)生影響，需要提高控制方法的魯棒性。在移動機(jī)器人編隊完成特定的任務(wù)工作時，隊伍中的個體移動機(jī)器人需要知道其他移動機(jī)器人的位置信息，目前在這方面的研究也需要加強(qiáng)。

為此，本文采用融合激光傳感器和里程計數(shù)據(jù)的方法提高移動機(jī)器人定位精度，進(jìn)而采用自抗擾控制技術(shù)實(shí)現(xiàn)主從式移動機(jī)器人編隊跟蹤控制，提高了移動機(jī)器人編隊控制系統(tǒng)對運(yùn)動環(huán)境不確定性的魯棒性。最后，利用三臺輪式差速移動機(jī)器人設(shè)計了主從式移動機(jī)器人編隊控制實(shí)驗(yàn)系統(tǒng)，并在該實(shí)驗(yàn)平臺上驗(yàn)證了所提出算法的有效性和優(yōu)越性。

1 移動機(jī)器人軌跡跟蹤誤差模型

本文實(shí)驗(yàn)所有的移動機(jī)器人考慮的是兩輪差速輪式移動機(jī)器人。為了建立移動機(jī)器人的數(shù)學(xué)模型，簡化機(jī)器人的機(jī)械結(jié)構(gòu)，得到如圖1所示的差動機(jī)器人簡易模型。如圖1所示，差動機(jī)器人具有獨(dú)立驅(qū)動的左輪和右輪，然后再配備一個萬向輪保持平衡。

對圖1中的符號作如下說明:

V(t):移動機(jī)器人移動線速度

w(t):移動機(jī)器人移動角速度

VL(t):移動機(jī)器人左輪相對地面的移動線速度

圖1 兩輪差動驅(qū)動移動機(jī)器人模型示意圖

Vr(t):移動機(jī)器人右輪相對地面的移動線速度

θ(t):移動機(jī)器人X軸正方向的夾角，即航向角

x(t):移動機(jī)器人X軸坐標(biāo)

y(t):移動機(jī)器人Y軸坐標(biāo)

M:移動機(jī)器人質(zhì)心點(diǎn)

G:移動機(jī)器人兩輪中點(diǎn)

L:移動機(jī)器人兩輪之間的距離

H:移動機(jī)器人G點(diǎn)與M點(diǎn)之間的距離

假設(shè)1:機(jī)器人的質(zhì)地均勻，質(zhì)心為幾何中心點(diǎn)，且機(jī)器人運(yùn)動過程中左右兩輪與地面純滾動無滑動。

基于該假設(shè)，移動機(jī)器人的運(yùn)動學(xué)模型可描述為:

當(dāng)移動機(jī)器人在做轉(zhuǎn)向運(yùn)動時，其瞬時運(yùn)動可以近似為直線運(yùn)動。假設(shè)采樣時間為△t，獲得機(jī)器人的里程計模型:

如此就可以根據(jù)根據(jù)里程計模型實(shí)時估算機(jī)器人的位置信息。本文考慮的移動機(jī)器人編隊問題指的是各個跟隨機(jī)器人以恒定的橫向距離以及縱向距離跟蹤主移動機(jī)器人的運(yùn)動。因此，需要描述每個從機(jī)機(jī)器人與主機(jī)器人的跟隨誤差模型。其中，MR代表主移動機(jī)器人，S1，S2分別代表兩臺從機(jī)器人。因?yàn)槊颗_從機(jī)器人與主機(jī)器人建立的誤差模型都是一樣的，下面僅由SR代表從機(jī)器人。

現(xiàn)根據(jù)MR和SR各自的動態(tài)里程計模型，對機(jī)器人編隊系統(tǒng)的位置偏差和角度偏差進(jìn)行分析。記ex和ey為主從機(jī)器人之間的位置偏差，eθ為住從機(jī)器人之間的航向角偏差。這樣建立主從機(jī)器人跟蹤誤差表示為:

其中:xm，ym表示主機(jī)器人的位置信息，xs，ys表示從機(jī)器人的位置信息，θm表示主機(jī)器人的航向角，θs表示從機(jī)器人的航向角。實(shí)驗(yàn)中主移動機(jī)器人實(shí)時把自身的位姿信息發(fā)送給從移動機(jī)器人。

本文編隊需要解決的問題是:當(dāng)主移動機(jī)器人以額定的線速度和角速度移動時，從移動機(jī)器人能夠快速的從起始位置跟蹤上主移動機(jī)器人的狀態(tài)。即以相同的線速度和角速度跟隨主移動機(jī)器人移動，并且能夠保持主從移動機(jī)器人之間的橫縱距離為定值dqx和dqy。

肋拱施工是渡槽工程的關(guān)鍵部位，混凝土強(qiáng)度又是重中之重，而混凝土中間產(chǎn)品是最直接的反應(yīng)。為此，首先為了掌握混凝土強(qiáng)度情況，在澆筑過程中制作多組混凝土抗壓試件，測出肋拱混凝土3 d、7 d、28 d強(qiáng)度，并繪制出曲線圖（如圖2）。

2 移動機(jī)器人主從跟蹤控制器設(shè)計

2.1 移動機(jī)器人定位算法

多傳感器融合是把在不同位置的多個不同傳感器所提供的局部環(huán)境的不完整信息加以融合。消除傳感器之間可能存在的冗余信息。多傳感器融合的理論就是更加精準(zhǔn)的反應(yīng)環(huán)境信息。目前在工業(yè)、軍事等眾多領(lǐng)域有了廣泛的應(yīng)用。

從上述可知移動機(jī)器人能夠通過電機(jī)編碼器依據(jù)公式(2)估算出移動機(jī)器人的當(dāng)前位姿。然而在實(shí)際應(yīng)用中，由于輪子打滑等因素會導(dǎo)致定位精度降低。從式 (2)可看出里程計估算會導(dǎo)致測量累積誤差，從而導(dǎo)致定位結(jié)果有較大的偏差，這將給后續(xù)的軌跡跟蹤控制帶來較大影響。為此，本文采用多傳感器融合［13-14］的方法來提高定位精度?；诶锍逃嫼图す鈧鞲衅魅诤系囊苿訖C(jī)器人定位方法由以下算法給出。

首先，通過在線讀取安裝在左右驅(qū)動電機(jī)上的碼盤測量并計算出機(jī)器人左右輪的線速度。進(jìn)而依據(jù)式 (2)和采樣周期Δt推算出當(dāng)前移動機(jī)器人的位 置和航向作為估計值。為此，定義狀態(tài)變量［x y VxVy］，系統(tǒng)的狀態(tài)方程為:Xk+1=ΦXk+Wk。其中x和y表示移動機(jī)器人在全局坐標(biāo)系中的坐標(biāo)值;Vx和Vy分別是x軸和y軸方向的移動速度;Φ為tk時刻到tk+1時刻的狀態(tài)轉(zhuǎn)移矩陣，表達(dá)式見式(3);Wk是協(xié)方差為Q的系統(tǒng)噪聲矩陣。

其次，依據(jù)激光傳感器測得移動機(jī)器人位置數(shù)據(jù)作為測量值，為此，定義系統(tǒng)的測量方程為:Zk=Xk+Vk。其中，Xk是激光測得的移動機(jī)器人位置數(shù)據(jù);Vk是協(xié)方差為R的系統(tǒng)測量噪聲矩陣。

最后，通過下面步驟一到步驟五反復(fù)迭代更新移動機(jī)器人位置信息。

步驟一:由前一時刻的位姿 Xk|k通過式 xk+1|k=Φk+1|kxk|k預(yù)測新的狀態(tài)值Xk+1|k。

步驟二:由上一次的誤差協(xié)方差Pk|k通過式pk+1|k=Φ+Q得到新的誤差Pk+1|k。

步驟四:通過式xk+1|k+1=xk+1|k+K(Zk－xk+1|k) 對步驟一的狀態(tài)值Xk+1|k進(jìn)行校正。

步驟五:為了下次預(yù)測的迭代操作，需要由式pk+1|k+1=(I－K)pk+1|k得到更新后的Pk+1|k+1值。

2.2 主從式軌跡跟蹤的ADRC控制器設(shè)計

自抗擾控制技術(shù)作為一種新型的控制算法，發(fā)揚(yáng)PID技術(shù)的精髓并且吸取現(xiàn)代控制理論。它不依賴被控對象精確的模型，能夠?qū)ο到y(tǒng)中存在的不確定干擾有很強(qiáng)的抑制作用。ADRC控制器主要由三部分組成:跟蹤微分器(TD)、擴(kuò)張狀態(tài)觀測器 (ESO)、非線性誤差反饋控制律。本文機(jī)器人編隊問題的關(guān)鍵在于各個從機(jī)器人與主機(jī)器人之間的距離控制。每個從機(jī)器人的輸出是每個從機(jī)器人的實(shí)際期望距離dx和dy。為了得到更為精準(zhǔn)的控制結(jié)果。本采用自抗擾技術(shù)對系統(tǒng)加以控制。由于主從移動機(jī)器的跟隨誤差模型是一階系統(tǒng)，因此自抗擾控制器設(shè)計主要包括兩個部分，忽略了跟蹤微分器。第一部分是擴(kuò)張狀態(tài)觀測器，主要是得到主從移動機(jī)器人的橫向距離跟蹤值dx以及縱向距離跟蹤值dy，還有就是總擾動的實(shí)時估計值fx和fy。

以橫向間距為例，縱向間距的跟蹤模型可類似得到。擴(kuò)張狀態(tài)觀測器表示為:

第二部分為控制規(guī)律的設(shè)計，具體的形式如下:

利用自抗擾技術(shù)為編隊中每個跟從移動機(jī)器人的跟隨控制系統(tǒng)設(shè)計兩個ADRC跟蹤控制器，其具體的控制框圖如圖2所示。

圖2 ADRC的系統(tǒng)結(jié)構(gòu)圖

假設(shè):移動機(jī)器人在純滾動無打滑條件下運(yùn)動的，移動機(jī)器人線速度為v，角速度為ω，左輪速度為vl，右輪速度為vr。

移動機(jī)器人直線運(yùn)動時:有vl=vr=v。

移動機(jī)器人非直線運(yùn)動時，視其小范圍內(nèi)運(yùn)動用半徑為R的圓代替，則有:

可得:

這樣就可以實(shí)現(xiàn)移動機(jī)器人左右輪線速度與移動機(jī)器人移動線速度與角速度之間的轉(zhuǎn)換。

3 實(shí)驗(yàn)與結(jié)論

在實(shí)驗(yàn)中，為了能夠簡化驅(qū)動程序編寫，方便不同機(jī)器人之間的信息傳遞，機(jī)器人開發(fā)環(huán)境采用ROS操作系統(tǒng)［15-16］。此外，為了能夠有良好的人機(jī)交互，將ROS與QT結(jié)合使用，編寫上位機(jī)程序，直觀的實(shí)時體現(xiàn)機(jī)器人的動態(tài)性能。首先為每臺機(jī)器人建立/Robot、/serial_node兩個節(jié)點(diǎn)。主機(jī)的/MasterRobot節(jié)點(diǎn)分別訂閱激光節(jié)點(diǎn)/hokuyo_node的/scan話題、/serial_ node節(jié)點(diǎn)的/M1Speed和/M2Speed兩個話題。/MasterRobot節(jié)點(diǎn)在線實(shí)時得到/scan的激光數(shù)據(jù)、/M1Speed的左輪線速度、/M2Speed右輪線速度并用上一節(jié)提出的卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，得到精準(zhǔn)定位信息，然后通過/odom話題把主機(jī)的位置信息發(fā)送給從機(jī)。從機(jī)通過在線接收來自主機(jī)的/odom話題包來獲取期望位置。然后根據(jù)式 (3)誤差模型分別得出橫坐標(biāo)和縱坐標(biāo)的偏差值dx，dy。將兩個偏差值運(yùn)用各自的自抗擾控制器獲取的控制量u。最終通過式 (8)算出左右輪的期望速度。每天從移動機(jī)器人發(fā)布/motorL和/motorR兩個話題，上位機(jī)通過這兩個節(jié)點(diǎn)向電機(jī)發(fā)送實(shí)時控制速度。由此，移動機(jī)器人每個周期通過傳感器采集位姿信息，然后與期望位置作差值比較得出理想的線速度值傳送給電機(jī)，如此反復(fù)更新速度數(shù)據(jù)來控制每個從機(jī)器人的最終位置。

本實(shí)驗(yàn)用三臺移動機(jī)器人進(jìn)行編隊實(shí)驗(yàn)。一臺主移動機(jī)器人:以導(dǎo)航機(jī)器人的身份存在，主要任務(wù)是帶領(lǐng)其他從移動機(jī)器人運(yùn)動。兩臺從移動機(jī)器人:以跟隨者的身份存在，主要任務(wù)是通過上面設(shè)計的自抗擾控制器進(jìn)行跟隨編隊實(shí)驗(yàn)，驗(yàn)證所提算法的魯棒性。實(shí)驗(yàn)時，場地為已知邊界的室外環(huán)境。主移動機(jī)器人所處的位置設(shè)定為坐標(biāo)軸的原點(diǎn) (0，0)，將一臺從機(jī)器人放置在主移動機(jī)器人左側(cè) (－0.3，0)出，定這臺從移動機(jī)器人為S1，再將另外一條移動機(jī)器人放置在 (－0.6，0)出，定為S2。實(shí)驗(yàn)過程中運(yùn)動方式為:主移動先做直線運(yùn)動，在經(jīng)過一定時間后，改變運(yùn)行狀態(tài)，做弧線運(yùn)動。一臺從移動機(jī)器人以dx=0.3，dy=0.3與主移動機(jī)器人保持同步運(yùn)動，另一個臺從移動機(jī)器熱以dx=0.3，dy=－0.3與主移動機(jī)器人保持同步運(yùn)動。兩臺移動機(jī)器人從初始位置快速跟蹤到期望偏差位置，在這過程中每臺從移動機(jī)器人 (跟隨機(jī)器人)通過ROS節(jié)點(diǎn)實(shí)時得到主機(jī)的位置信息，然后進(jìn)行跟隨控制。上位機(jī)實(shí)時記錄主從移動機(jī)器人的軌跡信息。圖3顯示為機(jī)器人編隊中每臺機(jī)器人在自抗擾技術(shù)控制下的跟隨軌跡。圖中，綠色線表示一號從移動機(jī)器人，藍(lán)色線表示二號主移動機(jī)器人，黃色表示三號從移動機(jī)器人，紅色線表示期望軌跡。為了方便觀測，圖中的曲線是經(jīng)過平移顯示的。

圖3 主從機(jī)器人軌跡圖

以下將給出在自抗擾技術(shù)下實(shí)驗(yàn)得出的結(jié)果。在整個編隊試驗(yàn)過程中記錄編隊中所有移動機(jī)器人的橫坐標(biāo)x，縱坐標(biāo)y以及實(shí)時運(yùn)動速度v。自抗擾控制器中，各個參數(shù)在試驗(yàn)過程中具體數(shù)值為:

ADRC_X(橫向距離):Kpx=1．3，b0x=2，β1x=100，β2x=1 000

ADRC_Y(縱向距離):Kpy=2，b0y=4，β1y=200，β2y=1 000

將記錄的數(shù)據(jù)進(jìn)行分析，得到圖4機(jī)器人實(shí)時速度對比，圖5機(jī)器人x軸跟隨情況，圖6機(jī)器人y軸跟隨情況。

圖4 機(jī)器人實(shí)時速度對比

圖5 機(jī)器人x軸跟隨情況

圖6 機(jī)器人y軸跟隨情況

本文研究了移動機(jī)器人的編隊問題，運(yùn)用先進(jìn)的自抗擾技術(shù)，設(shè)計了特定的自抗擾控制器。并且通過搭建移動機(jī)器人編隊實(shí)驗(yàn)平臺驗(yàn)證了所提算法的魯棒性。在實(shí)驗(yàn)時，兩臺從移動機(jī)器人的起始速度為0，當(dāng)主移動機(jī)器人以0.1 m/s的速度勻速直線運(yùn)動時，兩臺從移動機(jī)器人快速實(shí)現(xiàn)跟蹤。而且從圖4兩臺從移動機(jī)器人的速度響應(yīng)曲線可以看出，兩臺移動機(jī)器人不僅能夠快速的響應(yīng)到主機(jī)的運(yùn)動速度并且穩(wěn)定在0.1 m/s上下波動。還能在主移動機(jī)器人運(yùn)動狀態(tài)發(fā)生改變后，每臺從移動機(jī)器人還是能夠快速穩(wěn)定到期望的速度位置。從移動機(jī)器人的坐標(biāo)信息來分析。上述我們假設(shè)過主機(jī)的位置為 (0，0)。由于一號從機(jī)器人的初始位置為 (－0.6，0)，但是要保持與主移動機(jī)器人dx=0.3，dy=0.3的間距的同步運(yùn)動。因此首先需要快速跟蹤到主移動機(jī)器人的當(dāng)前位置，這也解釋了圖4中一號從移動機(jī)器人在起始階段速度大幅度提升，而后續(xù)逐漸平穩(wěn)。從圖5也可以看出一號移動機(jī)器人由－0.3逐漸平穩(wěn)的跟隨主機(jī)移動，即使中間段有一部分主機(jī)改變運(yùn)動狀態(tài)也能較好的重新達(dá)到穩(wěn)定狀態(tài)。相應(yīng)的，從圖6也可以得出二號從移動機(jī)器人相識的結(jié)論。通過上述的分析結(jié)果表明:本文所設(shè)計的自抗擾控制算法能夠較好的抑制外部干擾的影響并且對不可比避免的內(nèi)部干擾予以補(bǔ)償。算法具有較高的魯棒性和較好的控制精度，很好的實(shí)現(xiàn)文章開始提出的移動機(jī)器人編隊控制。