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

毛文勇，仇 翔，張文安

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

0 引言

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

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

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

1 移動機器人軌跡跟蹤誤差模型

本文實驗所有的移動機器人考慮的是兩輪差速輪式移動機器人。為了建立移動機器人的數學模型，簡化機器人的機械結構，得到如圖1所示的差動機器人簡易模型。如圖1所示，差動機器人具有獨立驅動的左輪和右輪，然后再配備一個萬向輪保持平衡。

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

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

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

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

圖1 兩輪差動驅動移動機器人模型示意圖

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

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

x(t):移動機器人X軸坐標

y(t):移動機器人Y軸坐標

M:移動機器人質心點

G:移動機器人兩輪中點

L:移動機器人兩輪之間的距離

H:移動機器人G點與M點之間的距離

假設1:機器人的質地均勻，質心為幾何中心點，且機器人運動過程中左右兩輪與地面純滾動無滑動。

基于該假設，移動機器人的運動學模型可描述為:

當移動機器人在做轉向運動時，其瞬時運動可以近似為直線運動。假設采樣時間為△t，獲得機器人的里程計模型:

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

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

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

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

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

2 移動機器人主從跟蹤控制器設計

2.1 移動機器人定位算法

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

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

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

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

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

步驟一:由前一時刻的位姿 Xk|k通過式 xk+1|k=Φk+1|kxk|k預測新的狀態(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進行校正。

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

2.2 主從式軌跡跟蹤的ADRC控制器設計

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

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

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

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

圖2 ADRC的系統(tǒng)結構圖

假設:移動機器人在純滾動無打滑條件下運動的，移動機器人線速度為v，角速度為ω，左輪速度為vl，右輪速度為vr。

移動機器人直線運動時:有vl=vr=v。

移動機器人非直線運動時，視其小范圍內運動用半徑為R的圓代替，則有:

可得:

這樣就可以實現移動機器人左右輪線速度與移動機器人移動線速度與角速度之間的轉換。

3 實驗與結論

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

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

圖3 主從機器人軌跡圖

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

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

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

將記錄的數據進行分析，得到圖4機器人實時速度對比，圖5機器人x軸跟隨情況，圖6機器人y軸跟隨情況。

圖4 機器人實時速度對比

圖5 機器人x軸跟隨情況

圖6 機器人y軸跟隨情況

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