基于超寬帶測距的異構移動機器人軌跡跟蹤控制

衛(wèi)恒， 呂強,*， 王國勝， 林輝燦， 梁冰

(1. 陸軍裝甲兵學院 兵器與控制系， 北京 100072； 2. 江西理工大學 信息工程學院， 贛州 341000)

移動機器人自主軌跡跟蹤控制的關鍵即為機器人在環(huán)境中的精確定位。在室外寬闊環(huán)境中，基于GPS與慣性測量單元(Inertial Measurement Unit，IMU)融合的自主導航系統(tǒng)研究已經較為成熟[1-2]，尤其近年來隨著人工智能(Artificial Intelligence， AI)的快速發(fā)展，已經將深度學習用于車輛在復雜、動態(tài)環(huán)境中提高GPS定位精度[3]。在室內或GPS信號受到抑制的環(huán)境中進行導航，研究較多的是機器人同時定位與地圖構建(Simultaneous Localization and Mapping， SLAM)算法[4-5]，尤其基于激光、視覺的SLAM算法得到快速發(fā)展[6-8]。近年來也引入了基于深度學習的SLAM算法，雖有發(fā)展，但遠沒有達到實際應用的程度[9-10]。

上述室內定位算法都只是提供了機器人的局部坐標，當轉換到全局坐標時，由于IMU的累計誤差，以及運動對傳感器測量數據的影響等因素，會出現測量精度不夠等情況，特別是當基于多機器人系統(tǒng)時，算法的復雜度又大幅度提升，實時性也受到影響[4]。不同于視覺、超聲波和激光等傳感器，本文采用的超寬帶(Ultra Wideband，UWB)技術可以穿透墻壁和森林，其相較于傳統(tǒng)窄帶射頻信號，響應頻率可以達到20 Hz以上，時間分辨率高，抗多徑效應能力強，穿透能力強，有利于在室內完成對機器人的實時定位[11]。文獻[12]提出了基于UWB的微型四旋翼自主飛行算法，但未對UWB數據進行處理，飛機在懸停時抖動比較劇烈[12]。張一淳等提出了基于運動學模型的室內全向機器人軌跡跟蹤算法，但未考慮機器人的動力學模型，且沒有外部定位系統(tǒng)，未對位置閉環(huán)控制進行驗證[13]。

本文以微型飛行器crazyflie2.0[14]與全向機器人[15]為基礎，研究了基于UWB的機器人軌跡跟蹤控制算法，并完成了軌跡跟蹤、協同控制等實驗，在實際環(huán)境中驗證了UWB定位系統(tǒng)和控制算法的實時性和穩(wěn)定性。

1 超寬帶定位系統(tǒng)

以DecaWave公司最新推出的UWB測距芯片DW1000為基礎[16]，構建了4個錨點、多標簽的實時定位系統(tǒng)。

1.1 雙向測距算法

在錨點和標簽上都裝上接收機和發(fā)射機，通過雙向測距可以消除時鐘誤差和偏移誤差，得到更加精確的測距結果，其算法如圖1所示。

圖1 雙向測距算法Fig.1 Two-way ranging algorithm

(1)

則標簽與錨點之間的距離為

dRA=fc

(2)

式中：c=3×108m/s為電磁波在空氣中的傳播速度。

實驗中分別選取測距距離為1和15 m 2個點進行測量，結果如圖2所示。

圖2 雙向測距算法實測結果Fig.2 Measured results of two-way ranging algorithm

由實驗可得，測距精度在±5 cm，響應時間5 ms，滿足機器人控制對定位的實時性和準確性要求。但是在實驗過程中，發(fā)現會隨機出現奇異值。奇異值的出現對后續(xù)的機器人定位，軌跡跟蹤等會有極大的影響，甚至會出現機器人失控等反應，所以在1.2節(jié)設計了奇異值去除模塊。

1.2 改進的馬氏距離奇異值檢測算法

針對UWB測距隨機出現的奇異值，本系統(tǒng)對馬氏距離奇異值檢測算法進行了改進，設計了奇異值去除模塊。傳統(tǒng)馬氏距離奇異值檢測算法直接利用傳感器測得數據的均值和協方差矩陣計算馬氏距離，算出來的結果有可能是不穩(wěn)健的，容易受到奇異值遮蔽作用的影響，導致正常數據和奇異值算出來的馬氏距離差別不大，從而無法去除奇異值。而基于最小協方差行列式(Minimum Covariance Determinant，MCD)估計的馬氏距離奇異值檢測算法中，所利用的均值和協方差矩陣都是在MCD估計中計算出的穩(wěn)健估計量，奇異值和正常數據的馬氏距離有較大差值，從而達到檢測并去除奇異值的作用。

有一組數據集合D={xi|i=1,2,…}，若T為均值，S為協方差矩陣，設B=[xi-T](i=1,2,…)，則該組數據的馬氏距離計算公式為

(3)

基于MCD的馬氏距離奇異值檢測算法即采用Rousseeuw和Driessen提出的FAST-MCD算法[18]，首先，通過迭代得到穩(wěn)健的均值和協方差矩陣，然后再計算穩(wěn)健的馬氏距離，最后進行奇異值檢測。算法流程如圖3所示。實驗中，仍選取測距距離為1和15 m 2個點進行測量，結果如圖4所示。

圖3 基于MCD的馬氏距離奇異值檢測算法流程圖Fig.3 Flowchart of Mahalanobis distance singular value detection algorithm based on MCD

圖4 基于穩(wěn)健馬氏距離奇異值去除之后的測距結果Fig.4 Ranging results based on robust Mahalanobis distance after removal of singular value

由實驗可得，測距精度在±5 cm，響應時間為5 ms，奇異值被去除。實驗證明，基于MCD的馬氏距離奇異值檢測算法，能夠在不影響測距實時性的前提下，有效去除隨機出現的奇異值。

1.3 位置測算模塊

考慮到UWB模塊可以得到較為準確的兩點之間距離，加之三邊測量法計算量小易于實現，所以系統(tǒng)運用該算法進行位置測算。

已知錨點坐標為(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)， (x4,y4,z4)，未知點坐標為(x0,y0)，以及未知點到4個已知錨點的距離分別為d1,d2,d3,d4，由此可得

(4)

實驗中，實測了2組xy平面坐標，1組為x軸邊界附近點，1組為一般點，如圖5所示。

由實驗可得，坐標測量較準確，測量誤差±4 cm，但是在坐標軸邊界附近的點，誤差偏大為±8cm，分析其主要原因是三邊測量法在邊界處將UWB測距誤差的影響放大了。針對一般點誤差和邊緣效應，后續(xù)依據機器人運動學和動力學模型，設計了基于擴展卡爾曼濾波(EKF)的優(yōu)化算法，對得到的坐標進行優(yōu)化處理。

圖5 三邊測量法的實測結果Fig.5 Measured results of trilateration algorithm

2 異構機器人軌跡跟蹤控制

針對全向機器人的運動學和動力學特點，提出了一種基于滑模+PID控制的逆動力學前饋控制算法；針對UWB定位算法中出現的坐標跳動、邊緣效應以及微型四旋翼的運動學特點，設計了基于EKF的軌跡跟蹤控制算法。

2.1 全向機器人控制

全向機器人為實驗室自行設計，大小20 cm×20 cm，高12 cm，續(xù)航2 h。

四輪全向機器人的坐標系如圖6所示，其中Ow-XwYw是以錨點為基點的全局坐標系，Or-XrYr為相對坐標系；(xw,yw,θw)為全局坐標系下機器人位姿，(xr,yr,θr)為相對坐標系下機器人位姿；v1～v4為機器人輪子線速度，θ為方位角，L為輪子到質心的距離,f1～f4為每個全向輪上的驅動力。

圖6 四輪全向機器人坐標系Fig.6 Coordinate system of four-wheeled omnidirectional robot

分析可得四輪全向機器人的運動學模型為

(5)

動力學模型為

(6)

式中：m為機器人總質量；I為機器人繞中心旋轉的轉動慣量；R為輪子半徑；Ra為電機的電樞電阻；Kt為電機轉矩常數；U=(U1,U2,U3,U4)T為電機電壓；r=(r1,r2,r3,r4)為輪子轉速；q為全局坐標系下機器人坐標；n為電機齒輪減速比。

假設期望軌跡為qd(t)=(xd(t),yd(t),θd(t))T，期望速度為vd=(vxd,vyd,wd)T。

運動學控制設計如下：

(7)

式中：eq=(ex,ey,eθ)為軌跡跟蹤誤差；kx,ky,kθ>0為運動學參數。

設ωi為全向輪的角速度，則vi=Rωi，由式(5)可得

(8)

將式(8)代入式(6)可得

(9)

式中：

實際應用中，因為存在擾動，所以式(9)通常為

(10)

設控制律為

U=G+(rc1+rc2)

(11)

(12)

式中：KP、KI、KD分別為PID控制中3個分量上的系數；ev=v-vd。由式(11)可得

rc1=GU-rc2

(13)

由式(10)可得

(14)

由式(13)和式(14)可得

H-1(q)d-GU+rc2

(15)

由式(12)可得

(16)

式(15)和式(16)聯立可得

(17)

將式(17)按狀態(tài)方程寫為

(18)

式中：

B=(0,0,I)T

選定滑模面為

σ=TX

(19)

式中：

令

(20)

式中：Γ=ΓT>0為待設計控制系數。

2.2 微型四旋翼軌跡跟蹤控制

2.2.1 基于UWB的crazyflie2.0軌跡跟蹤控制

為適應機器人越來越小型化的趨勢，微型四旋翼選擇開源飛機crazyflie2.0，該飛機大小只有9 cm×9 cm，質量27 g，續(xù)航7 min。研究中針對飛機尺寸設計了適應飛機載重和尺寸的UWB模塊(大小3 cm×2 cm，質量6 g)，編寫了位置控制環(huán)和坐標系變換函數，基于UWB的運動學模型和測量模型分析如下。

若UWB在飛機運行中某一節(jié)點測得位置為S=(Sx,Sy,Sz)T，估計的速度為v=(vx,vy,vz)T，假設加速度a=(ax,ay,az)T在時間[tk-1,tk]保持不變，則可得到該節(jié)點的離散模型為

(21)

式中：Δtk為相鄰時間差，下標k表示時間序列；ak為tk時刻加速度。

離散模型基于UWB的測量方程為

(22)

式中：dk為UWB測得的標簽到各個錨點的距離；Sa=(xa,ya,za)為錨點的位置；ηk為測量誤差。

綜上所述，基于UWB的crazyflie2.0的運動學和測量模型可以寫為

(23)

圖7 基于UWB的crazyflie2.0自主控制流程圖Fig.7 Flowchart of crazyflie2.0 autonomous control based on UWB

基于UWB的crazyflie2.0自主控制流程圖如圖7所示。其中，{m1,m2,…,mn}為任務列表，ρx、ρy、ρz和ρθ為依據任務生成的期望軌跡，di為UWB測距結果，vx、vy、vz和vθ為當前四旋翼飛行速度，x、y、z和θ為四旋翼在全局坐標系下的位姿信息，φ(t)、?(t)、ψ(t)和f(t)為經過位置控制環(huán)解析出的與期望軌跡的差值，Δφ(t)、Δ?(t)、Δψ(t)和Δf(t)為經過姿態(tài)控制環(huán)解析的與期望姿態(tài)的差值，Δωφ、Δωθ、Δωψ和f為經過速率控制環(huán)解析的期望控制量。

2.2.2 基于EKF優(yōu)化的crazyflie2.0軌跡跟蹤控制

針對坐標跳動偏大問題，基于crazyflie2.0的運動學模型，采用EKF算法設計了優(yōu)化模塊。算法原理如下：

輸入：t-1時刻狀態(tài)(x,y)t-1,St-1；t時刻控制信號ut；t時刻觀測值zt。

輸出：t時刻狀態(tài)(x，y)t，St。

3 return(x,y)t,St

其中：函數predict()和update()是卡爾曼濾波的標準程序，以2.2.1節(jié)分析的crazyflie2.0運動學和測量模型為例，則此時predict()為

(24)

(25)

式中：“^”表示估計值，“-”表示先驗值。

update()為

(26)

(27)

(28)

(29)

式中：Sa為錨點位置。

3 實 驗

3.1 算法仿真實驗

3.1.1 全向機器人軌跡跟蹤算法

在MATLAB中對全向機器人控制算法進行了仿真驗證。仿真中設定L=0.12 m，m=2 kg，I=1.013 kg·m2，Kt=0.014，R=0.03 m，Ra=1.9 Ω。設期望軌跡函數為

(30)

實驗結果表明，全向機器人軌跡跟蹤狀態(tài)良好，響應速度快，穩(wěn)態(tài)誤差為0，驗證了軌跡跟蹤控制算法的有效性，可以將該算法用于實際全向機器人的軌跡跟蹤控制。

圖8 軌跡跟蹤仿真實驗結果Fig.8 Trajectory tracking simulation experimental results

3.1.2 基于EKF優(yōu)化的crazyflie2.0軌跡跟蹤算法

圖9 軌跡跟蹤誤差隨時間變化Fig.9 Variation of trajectory tracking deviation with time

利用ROS在Gazebo中對crazyflie2.0改進后的軌跡跟蹤算法進行了仿真，驗證了EKF優(yōu)化模塊的有效性。實驗中，實地采集了一組UWB數據作為飛機的飛行軌跡的實測值，仿真結果如圖10所示。

圖10 基于EKF crazyflie2.0運行軌跡Fig.10 crazyflie2.0 running trajectory based on EKF

實驗結果表明，通過EKF優(yōu)化模塊對數據進行處理后，飛行軌跡與期望軌跡基本重合，有效抑制了坐標跳動對飛行控制的影響，可以將該算法用于實際crazyflie2.0的軌跡跟蹤控制。

3.2 異構多機器人平臺算法驗證

基于UWB在ROS框架下構建了多機器人研究平臺，系統(tǒng)共包含4個全向機器人和一個crazyflie2.0微型四旋翼，實驗平臺如圖11所示?；谡紫到y(tǒng)，分別完成了單全向機器人軌跡跟蹤實驗、crazyflie2.0的定點懸停實驗和多機器人協同控制實驗，驗證了UWB定位和機器人控制算法的實時性、穩(wěn)定性和準確性。

圖11 多機器人研究平臺Fig.11 Multi-robot research platform

3.2.1 多機器人網絡構建

設計中每個全向機器人搭載微處理器樹莓派并安裝Ubuntu系統(tǒng)及ROS操作平臺，中央處理機直接控制crazyflie2.0并對所有機器人進行監(jiān)控。中央處理機分配任務后，每個全向機器人依據自己得到的任務信息，規(guī)劃路徑，完成軌跡跟蹤并向云端發(fā)送消息，通告所有機器人其目前狀態(tài)。crazyflie2.0由中央處理機完成路徑規(guī)劃并通過數傳發(fā)送給飛機實現控制。

系統(tǒng)通過WiFi進行通信，中央處理機通過SSH工具遠程登錄各個機器人，從而實現離板控制。在中央處理機和每個機器人上運行roscore，即每個機器人包括中央處理機都作為一個獨立的master，這樣設計有2個優(yōu)點：第一，假如某一個機器人出現問題或是中央處理機出現故障，不會造成整個多機器人系統(tǒng)癱瘓或運行混亂；第二，方便設計加入與退出機制，在運行過程中添加或減少機器人個數，例如某一機器人完成所分配任務后，可以選擇退出多機器人系統(tǒng)，這樣有助于減少中央處理機的負擔。在這里構建的多機器人系統(tǒng)，通過github中master_discovery和master_sync2個開源項目，對機器人加上時間標簽，并進行時間同步。多機器人系統(tǒng)網絡示意圖如圖12所示。

圖12 多機器人系統(tǒng)網絡示意圖Fig.12 Schematic diagram of multi-robot system network

3.2.2 UWB測距標定

由于UWB在不同環(huán)境使用時會受到經緯度、海拔高度、空氣質量、障礙物等影響，因此在實驗前需要進行標定。

在室內[0.4,4] m實測了已知真實距離的50組數據，然后進行曲線擬合，考慮到需要滿足精度要求前提下，又不影響計算速度，因此采取3次多項式擬合，得到擬合函數為

式中:C為相關系數，C2越趨近于1擬合曲線越精確。

3.2.3 crazyflie2.0定點懸停實驗

為了驗證crayflie2.0軌跡跟蹤控制算法和快速運動中UWB定位的實時性、準確性，設計了crazyflie2.0定點懸停實驗。實驗中，首先讀取當前位置(x,y,z)，設定目標點為(x,y,z+0.5)。當飛機懸停后，用較大力氣推動飛機施加干擾，驗證位置控制環(huán)的魯棒性和抗干擾性。實驗中UWB實測結果如圖13所示。

圖13 crazyflie2.0定點懸停UWB實測結果Fig.13 Measured UWB results of crazyflie2.0 fixed point hovering

由實驗可得：①實驗中，位置跟蹤狀態(tài)良好，在懸停位置時x與y方向UWB定位精度在±8 cm，z方向的定位精度在±6 cm，位置刷新率為30 Hz。在人為施加干擾后，飛機能立即做出機動，在4 s內再次回到目標位置，并進入穩(wěn)定狀態(tài)。②2016年bitcraze研究組[12]發(fā)布在YouTube的實驗，使用6個錨點做了基于UWB的crazyflie2.0定點懸停實驗，但在實驗中，未對UWB數據進行處理，僅做了基本應用。本實驗只用了4個錨點，同時對測得的數據進行奇異值去除處理，并設計了EKF優(yōu)化模塊，飛行控制更加穩(wěn)定、響應更加迅速，實驗結果已經發(fā)布在YouKu上，但在實際實驗中，起飛階段過調達到30 cm，且人為干擾后，“對抗”機動響應不夠強烈，后續(xù)研究可針對這兩方面對控制算法進行改進。

3.2.4 單全向機器人軌跡跟蹤實驗

為了驗證全向機器人控制算法和UWB定位精度，設計了單機器人軌跡跟蹤實驗。設定機器人期望軌跡函數為

y=1.25+1.25sin(2πx/2.5)

(31)

需要注意的是，目前，由于全向機器人未搭載其他任何傳感器，只依靠UWB定位系統(tǒng)進行控制，所以當機器人初次開機時，需要進行初始姿態(tài)判斷。設計中，機器人開機后先向機體坐標系的x軸方向移動20 cm，然后再退回原位，從而判斷出機器人的初始姿態(tài)。實驗結果如圖14所示。

由實驗可得，機器人軌跡跟蹤狀態(tài)良好，誤差在4 cm，驗證了算法的實時性和魯棒性，同時可見，在彎道處UWB測量點較多，是由于目前控制算法為了得到平滑的運行軌跡，在遇到大角度彎道時會降速行駛，后續(xù)可進行大角度彎道快速通過的控制算法研究。

圖14 單機器人sin x軌跡跟蹤Fig.14 Single robot sin x trajectory tracking

3.2.5 多機器人協同控制實驗

為了驗證機器人控制算法、多機器人系統(tǒng)和通信網絡魯棒性、異構機器人協作及多機器人條件下UWB的定位精度，設計了簡單的異構多機器人協同控制實驗。

實驗內容：①全向機器人1號、2號、3號、4號分別移動到點(1.0，0.8) m，(1.0，1.8) m，(3，1.8) m，(3，0.8) m，crazyflie2.0置于3號全向機器人上，設定互不碰撞優(yōu)先級為1號>2號>3號>4號，當任意兩車x或y坐標相差小于10 cm時優(yōu)先級低的停車等待；②各全向機器人到達目標點后，3號機器人等待crazyflie2.0起飛，1號、2號、4號機器人開始跟蹤橢圓軌跡；③crazyflie2.0起飛，3號機器人跟蹤橢圓軌跡；④crazyflie2.0到達懸停點后，跟蹤三角形軌跡一周；⑤4號車停車等待，crazyflie2.0自主跟蹤4號車，先移動到4號車正上方后再垂直降落。實驗結果如圖15和圖16所示。

圖15 crazyflie2.0協同任務實驗軌跡Fig.15 Experimental trajectory of crazyflie2.0 collaborative task

圖16 全向機器人協同任務實驗軌跡Fig.16 Experimental trajectory of omnidirectional robot collaborative task

由實驗可得，多機器人網絡通信實時流暢，在實際多機器人條件下，異構移動機器人的軌跡跟蹤誤差在6 cm，能夠在室內實現有效、穩(wěn)定、實時的軌跡跟蹤，驗證了控制算法的實時性和穩(wěn)定性。但目前只是進行了簡單的異構機器人協同實驗，后續(xù)可基于該平臺繼續(xù)研究：①微型四旋翼降落到移動中的全向機器人上；②空地多機器人編隊控制算法研究。

4 結 論

本文基于UWB以全向機器人和crazyflie2.0四旋翼為基礎，提出了基于滑模+PID控制的逆動力學前饋控制算法和基于EKF優(yōu)化的軌跡跟蹤控制算法，并進行了仿真驗證。

1) 搭建了異構多機器人研究平臺，基本解決了傳統(tǒng)多機器人控制算法到實踐之間脫節(jié)的問題，相較于基于運動捕獲系統(tǒng)的研究平臺，性價比顯著提高，應用范圍更加廣泛，且該平臺傳輸數據非常簡單，對機器人數據處理能力要求低，有利于真正實現移動機器人的自主控制。

2) 實驗結果表明UWB定位與軌跡跟蹤控制算法的實時性和準確性能夠滿足機器人控制要求。另外，多機器人協同控制實驗為后續(xù)多機器人編隊、軌跡跟蹤控制算法研究奠定基礎。