基于超寬帶測(cè)距的異構(gòu)移動(dòng)機(jī)器人軌跡跟蹤控制

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

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

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

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

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

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

以DecaWave公司最新推出的UWB測(cè)距芯片DW1000為基礎(chǔ)[16]，構(gòu)建了4個(gè)錨點(diǎn)、多標(biāo)簽的實(shí)時(shí)定位系統(tǒng)。

1.1 雙向測(cè)距算法

在錨點(diǎn)和標(biāo)簽上都裝上接收機(jī)和發(fā)射機(jī)，通過(guò)雙向測(cè)距可以消除時(shí)鐘誤差和偏移誤差，得到更加精確的測(cè)距結(jié)果，其算法如圖1所示。

圖1 雙向測(cè)距算法Fig.1 Two-way ranging algorithm

(1)

則標(biāo)簽與錨點(diǎn)之間的距離為

dRA=fc

(2)

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

實(shí)驗(yàn)中分別選取測(cè)距距離為1和15 m 2個(gè)點(diǎn)進(jìn)行測(cè)量，結(jié)果如圖2所示。

圖2 雙向測(cè)距算法實(shí)測(cè)結(jié)果Fig.2 Measured results of two-way ranging algorithm

由實(shí)驗(yàn)可得，測(cè)距精度在±5 cm，響應(yīng)時(shí)間5 ms，滿(mǎn)足機(jī)器人控制對(duì)定位的實(shí)時(shí)性和準(zhǔn)確性要求。但是在實(shí)驗(yàn)過(guò)程中，發(fā)現(xiàn)會(huì)隨機(jī)出現(xiàn)奇異值。奇異值的出現(xiàn)對(duì)后續(xù)的機(jī)器人定位，軌跡跟蹤等會(huì)有極大的影響，甚至?xí)霈F(xiàn)機(jī)器人失控等反應(yīng)，所以在1.2節(jié)設(shè)計(jì)了奇異值去除模塊。

1.2 改進(jìn)的馬氏距離奇異值檢測(cè)算法

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

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

(3)

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

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

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

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

1.3 位置測(cè)算模塊

考慮到UWB模塊可以得到較為準(zhǔn)確的兩點(diǎn)之間距離，加之三邊測(cè)量法計(jì)算量小易于實(shí)現(xiàn)，所以系統(tǒng)運(yùn)用該算法進(jìn)行位置測(cè)算。

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

(4)

實(shí)驗(yàn)中，實(shí)測(cè)了2組xy平面坐標(biāo)，1組為x軸邊界附近點(diǎn)，1組為一般點(diǎn)，如圖5所示。

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

圖5 三邊測(cè)量法的實(shí)測(cè)結(jié)果Fig.5 Measured results of trilateration algorithm

2 異構(gòu)機(jī)器人軌跡跟蹤控制

針對(duì)全向機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特點(diǎn)，提出了一種基于滑模+PID控制的逆動(dòng)力學(xué)前饋控制算法；針對(duì)UWB定位算法中出現(xiàn)的坐標(biāo)跳動(dòng)、邊緣效應(yīng)以及微型四旋翼的運(yùn)動(dòng)學(xué)特點(diǎn)，設(shè)計(jì)了基于EKF的軌跡跟蹤控制算法。

2.1 全向機(jī)器人控制

全向機(jī)器人為實(shí)驗(yàn)室自行設(shè)計(jì)，大小20 cm×20 cm，高12 cm，續(xù)航2 h。

四輪全向機(jī)器人的坐標(biāo)系如圖6所示，其中Ow-XwYw是以錨點(diǎn)為基點(diǎn)的全局坐標(biāo)系，Or-XrYr為相對(duì)坐標(biāo)系；(xw,yw,θw)為全局坐標(biāo)系下機(jī)器人位姿，(xr,yr,θr)為相對(duì)坐標(biāo)系下機(jī)器人位姿；v1～v4為機(jī)器人輪子線(xiàn)速度，θ為方位角，L為輪子到質(zhì)心的距離,f1～f4為每個(gè)全向輪上的驅(qū)動(dòng)力。

圖6 四輪全向機(jī)器人坐標(biāo)系Fig.6 Coordinate system of four-wheeled omnidirectional robot

分析可得四輪全向機(jī)器人的運(yùn)動(dòng)學(xué)模型為

(5)

動(dòng)力學(xué)模型為

(6)

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

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

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

(7)

式中：eq=(ex,ey,eθ)為軌跡跟蹤誤差；kx,ky,kθ>0為運(yùn)動(dòng)學(xué)參數(shù)。

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

(8)

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

(9)

式中：

實(shí)際應(yīng)用中，因?yàn)榇嬖跀_動(dòng)，所以式(9)通常為

(10)

設(shè)控制律為

U=G+(rc1+rc2)

(11)

(12)

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

rc1=GU-rc2

(13)

由式(10)可得

(14)

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

H-1(q)d-GU+rc2

(15)

由式(12)可得

(16)

式(15)和式(16)聯(lián)立可得

(17)

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

(18)

式中：

B=(0,0,I)T

選定滑模面為

σ=TX

(19)

式中：

令

(20)

式中：Γ=ΓT>0為待設(shè)計(jì)控制系數(shù)。

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

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

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

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

(21)

式中：Δtk為相鄰時(shí)間差，下標(biāo)k表示時(shí)間序列；ak為tk時(shí)刻加速度。

離散模型基于UWB的測(cè)量方程為

(22)

式中：dk為UWB測(cè)得的標(biāo)簽到各個(gè)錨點(diǎn)的距離；Sa=(xa,ya,za)為錨點(diǎn)的位置；ηk為測(cè)量誤差。

綜上所述，基于UWB的crazyflie2.0的運(yùn)動(dòng)學(xué)和測(cè)量模型可以寫(xiě)為

(23)

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

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

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

針對(duì)坐標(biāo)跳動(dòng)偏大問(wèn)題，基于crazyflie2.0的運(yùn)動(dòng)學(xué)模型，采用EKF算法設(shè)計(jì)了優(yōu)化模塊。算法原理如下：

輸入：t-1時(shí)刻狀態(tài)(x,y)t-1,St-1；t時(shí)刻控制信號(hào)ut；t時(shí)刻觀測(cè)值z(mì)t。

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

3 return(x,y)t,St

其中：函數(shù)predict()和update()是卡爾曼濾波的標(biāo)準(zhǔn)程序，以2.2.1節(jié)分析的crazyflie2.0運(yùn)動(dòng)學(xué)和測(cè)量模型為例，則此時(shí)predict()為

(24)

(25)

式中：“^”表示估計(jì)值，“-”表示先驗(yàn)值。

update()為

(26)

(27)

(28)

(29)

式中：Sa為錨點(diǎn)位置。

3 實(shí) 驗(yàn)

3.1 算法仿真實(shí)驗(yàn)

3.1.1 全向機(jī)器人軌跡跟蹤算法

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

(30)

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

圖8 軌跡跟蹤仿真實(shí)驗(yàn)結(jié)果Fig.8 Trajectory tracking simulation experimental results

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

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

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

圖10 基于EKF crazyflie2.0運(yùn)行軌跡Fig.10 crazyflie2.0 running trajectory based on EKF

實(shí)驗(yàn)結(jié)果表明，通過(guò)EKF優(yōu)化模塊對(duì)數(shù)據(jù)進(jìn)行處理后，飛行軌跡與期望軌跡基本重合，有效抑制了坐標(biāo)跳動(dòng)對(duì)飛行控制的影響，可以將該算法用于實(shí)際crazyflie2.0的軌跡跟蹤控制。

3.2 異構(gòu)多機(jī)器人平臺(tái)算法驗(yàn)證

基于UWB在ROS框架下構(gòu)建了多機(jī)器人研究平臺(tái)，系統(tǒng)共包含4個(gè)全向機(jī)器人和一個(gè)crazyflie2.0微型四旋翼，實(shí)驗(yàn)平臺(tái)如圖11所示。基于整套系統(tǒng)，分別完成了單全向機(jī)器人軌跡跟蹤實(shí)驗(yàn)、crazyflie2.0的定點(diǎn)懸停實(shí)驗(yàn)和多機(jī)器人協(xié)同控制實(shí)驗(yàn)，驗(yàn)證了UWB定位和機(jī)器人控制算法的實(shí)時(shí)性、穩(wěn)定性和準(zhǔn)確性。

圖11 多機(jī)器人研究平臺(tái)Fig.11 Multi-robot research platform

3.2.1 多機(jī)器人網(wǎng)絡(luò)構(gòu)建

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

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

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

3.2.2 UWB測(cè)距標(biāo)定

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

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

式中:C為相關(guān)系數(shù)，C2越趨近于1擬合曲線(xiàn)越精確。

3.2.3 crazyflie2.0定點(diǎn)懸停實(shí)驗(yàn)

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

圖13 crazyflie2.0定點(diǎn)懸停UWB實(shí)測(cè)結(jié)果Fig.13 Measured UWB results of crazyflie2.0 fixed point hovering

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

3.2.4 單全向機(jī)器人軌跡跟蹤實(shí)驗(yàn)

為了驗(yàn)證全向機(jī)器人控制算法和UWB定位精度，設(shè)計(jì)了單機(jī)器人軌跡跟蹤實(shí)驗(yàn)。設(shè)定機(jī)器人期望軌跡函數(shù)為

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

(31)

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

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

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

3.2.5 多機(jī)器人協(xié)同控制實(shí)驗(yàn)

為了驗(yàn)證機(jī)器人控制算法、多機(jī)器人系統(tǒng)和通信網(wǎng)絡(luò)魯棒性、異構(gòu)機(jī)器人協(xié)作及多機(jī)器人條件下UWB的定位精度，設(shè)計(jì)了簡(jiǎn)單的異構(gòu)多機(jī)器人協(xié)同控制實(shí)驗(yàn)。

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

圖15 crazyflie2.0協(xié)同任務(wù)實(shí)驗(yàn)軌跡Fig.15 Experimental trajectory of crazyflie2.0 collaborative task

圖16 全向機(jī)器人協(xié)同任務(wù)實(shí)驗(yàn)軌跡Fig.16 Experimental trajectory of omnidirectional robot collaborative task

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

4 結(jié) 論

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

1) 搭建了異構(gòu)多機(jī)器人研究平臺(tái)，基本解決了傳統(tǒng)多機(jī)器人控制算法到實(shí)踐之間脫節(jié)的問(wèn)題，相較于基于運(yùn)動(dòng)捕獲系統(tǒng)的研究平臺(tái)，性?xún)r(jià)比顯著提高，應(yīng)用范圍更加廣泛，且該平臺(tái)傳輸數(shù)據(jù)非常簡(jiǎn)單，對(duì)機(jī)器人數(shù)據(jù)處理能力要求低，有利于真正實(shí)現(xiàn)移動(dòng)機(jī)器人的自主控制。

2) 實(shí)驗(yàn)結(jié)果表明UWB定位與軌跡跟蹤控制算法的實(shí)時(shí)性和準(zhǔn)確性能夠滿(mǎn)足機(jī)器人控制要求。另外，多機(jī)器人協(xié)同控制實(shí)驗(yàn)為后續(xù)多機(jī)器人編隊(duì)、軌跡跟蹤控制算法研究奠定基礎(chǔ)。