毛文勇，仇 翔，張文安

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

0 引言

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

在移動(dòng)機(jī)器人的編隊(duì)問題的研究已經(jīng)有許多研究成果。例如，利用李亞普諾夫函數(shù)設(shè)計(jì)速度誤差控制規(guī)律，實(shí)現(xiàn)目標(biāo)位置的跟蹤控制。但是缺點(diǎn)在于動(dòng)態(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)用中，就需要考慮更多因素，例如如何提高移動(dòng)機(jī)器人定位精度從而提高軌跡跟蹤控制精度，其次，由于環(huán)境的未知，干擾的不確定等因素都會(huì)對(duì)控制方法的實(shí)際實(shí)施效果產(chǎn)生影響，需要提高控制方法的魯棒性。在移動(dòng)機(jī)器人編隊(duì)完成特定的任務(wù)工作時(shí)，隊(duì)伍中的個(gè)體移動(dòng)機(jī)器人需要知道其他移動(dòng)機(jī)器人的位置信息，目前在這方面的研究也需要加強(qiáng)。

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

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

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

對(duì)圖1中的符號(hào)作如下說明:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

步驟一:由前一時(shí)刻的位姿 Xk|k通過式 xk+1|k=Φk+1|kxk|k預(yù)測(cè)新的狀態(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) 對(duì)步驟一的狀態(tài)值Xk+1|k進(jìn)行校正。

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

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

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

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

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

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

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

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

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

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

可得:

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

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

在實(shí)驗(yàn)中，為了能夠簡(jiǎn)化驅(qū)動(dòng)程序編寫，方便不同機(jī)器人之間的信息傳遞，機(jī)器人開發(fā)環(huán)境采用ROS操作系統(tǒng)［15-16］。此外，為了能夠有良好的人機(jī)交互，將ROS與QT結(jié)合使用，編寫上位機(jī)程序，直觀的實(shí)時(shí)體現(xiàn)機(jī)器人的動(dòng)態(tài)性能。首先為每臺(tái)機(jī)器人建立/Robot、/serial_node兩個(gè)節(jié)點(diǎn)。主機(jī)的/MasterRobot節(jié)點(diǎn)分別訂閱激光節(jié)點(diǎn)/hokuyo_node的/scan話題、/serial_ node節(jié)點(diǎn)的/M1Speed和/M2Speed兩個(gè)話題。/MasterRobot節(jié)點(diǎn)在線實(shí)時(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。將兩個(gè)偏差值運(yùn)用各自的自抗擾控制器獲取的控制量u。最終通過式 (8)算出左右輪的期望速度。每天從移動(dòng)機(jī)器人發(fā)布/motorL和/motorR兩個(gè)話題，上位機(jī)通過這兩個(gè)節(jié)點(diǎn)向電機(jī)發(fā)送實(shí)時(shí)控制速度。由此，移動(dòng)機(jī)器人每個(gè)周期通過傳感器采集位姿信息，然后與期望位置作差值比較得出理想的線速度值傳送給電機(jī)，如此反復(fù)更新速度數(shù)據(jù)來控制每個(gè)從機(jī)器人的最終位置。

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

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

以下將給出在自抗擾技術(shù)下實(shí)驗(yàn)得出的結(jié)果。在整個(gè)編隊(duì)試驗(yàn)過程中記錄編隊(duì)中所有移動(dòng)機(jī)器人的橫坐標(biāo)x，縱坐標(biāo)y以及實(shí)時(shí)運(yùn)動(dòng)速度v。自抗擾控制器中，各個(gè)參數(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í)時(shí)速度對(duì)比，圖5機(jī)器人x軸跟隨情況，圖6機(jī)器人y軸跟隨情況。

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

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

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

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