月面多機(jī)器人的分布式協(xié)同定位算法研究

魏明珠，謝曉梅，嚴(yán) 鵬，徐利梅

(電子科技大學(xué)航空航天學(xué)院，成都611731)

1 引言

隨著我國載人航天工程和月球探測(cè)工程的發(fā)展，月面探測(cè)的技術(shù)條件在不斷突破，但月面環(huán)境苛刻，不適合人類的長期作業(yè)[1]。在有人參與的深空探測(cè)前期，需要無人深空探測(cè)任務(wù)突破關(guān)鍵問題[2]。目前機(jī)器人技術(shù)發(fā)展迅猛，很多領(lǐng)域理論漸趨成熟，應(yīng)用研究也深入到工業(yè)、軍事等多個(gè)層面。因此，利用月面機(jī)器人進(jìn)行探測(cè)，可完成更廣泛區(qū)域的月球科學(xué)探測(cè)任務(wù)，并為載人登月提供科學(xué)與工程數(shù)據(jù)和先期基礎(chǔ)設(shè)施。攜帶各種傳感器和采樣裝置的月面移動(dòng)機(jī)器人是月面探測(cè)的重要工具[3-5]。單個(gè)機(jī)器人的負(fù)載能力、數(shù)據(jù)處理能力等有限，利用多個(gè)月面移動(dòng)機(jī)器人系統(tǒng)可以獲得更大的探測(cè)效率，通過分布式的信息處理獲得更高效和更廣泛的信息量[6]。

多機(jī)器人協(xié)同進(jìn)行月表探測(cè)的前提是每個(gè)機(jī)器人能夠快速準(zhǔn)確地獲得自身在環(huán)境中的位置信息，因此多機(jī)器人系統(tǒng)中的定位是首要問題[7-8]。多機(jī)器人系統(tǒng)作為一個(gè)整體，機(jī)器人之間的協(xié)同定位可以利用機(jī)器人彼此的信息，獲得各自的位置。多機(jī)器人協(xié)同定位的方法有兩類：一類是集中式協(xié)同定位，每個(gè)機(jī)器人的信息都傳輸?shù)街醒胩幚韱卧M(jìn)行處理，然后再發(fā)送給各個(gè)機(jī)器人[9]；另一類是分布式處理，即每個(gè)機(jī)器人利用與鄰居機(jī)器人交互的信息計(jì)算自己的位置[10]。因此分布式定位方法具有實(shí)際的價(jià)值，也是人們研究的熱點(diǎn)。Costa等[11]提出了基于分布式加權(quán)多維標(biāo)度的算法，基于凸包中不含錨點(diǎn)的假設(shè)，利用質(zhì)心計(jì)算方法，定位網(wǎng)絡(luò)中機(jī)器人的位置，但成本函數(shù)本身建立比較復(fù)雜，算法的運(yùn)算量較大。Cui等[12]采用不依賴錨點(diǎn)的分布式定位方法，本質(zhì)是通過多邊法的原理定位，定位后的節(jié)點(diǎn)作為虛擬錨點(diǎn)，該方法在網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)上有局限。王玲等[13]提出了基于擴(kuò)展卡爾曼濾波的相對(duì)位姿測(cè)量方法，該方法需要機(jī)器人交互除位置信息外還有方位信息，同時(shí)沒有考慮大規(guī)模多機(jī)器人網(wǎng)絡(luò)的定位情況。

本文重點(diǎn)考慮基于測(cè)距信息的多機(jī)器人系統(tǒng)的分布式定位問題，對(duì)多機(jī)器人協(xié)同定位問題建立優(yōu)化方程模型，并提出基于自適應(yīng)步長的分布式梯度下降的定位方法，通過仿真試驗(yàn)分析算法的性能。

2 多機(jī)器人系統(tǒng)問題建模

假設(shè)n個(gè)月面機(jī)器人構(gòu)成一個(gè)自主移動(dòng)的網(wǎng)絡(luò)， p(τ)={pi(τ)}，i=1，…，n， 表示 n 個(gè)機(jī)器人在τ時(shí)刻的位置，其中第i個(gè)機(jī)器人的位置表示為 pi(τ)=[xi(τ)，yi(τ)]T。 D={dij}，i，j∈ n，表示機(jī)器人之間的測(cè)距信息的集合。對(duì)于機(jī)器人i與j，dij表示機(jī)器人之間距離的測(cè)量值，而|pipj|表示兩個(gè)機(jī)器人之間的真實(shí)距離，因此滿足如下計(jì)算公式，表示兩個(gè)機(jī)器人之間距離真值與測(cè)量值之間的誤差。

假設(shè)機(jī)器人之間量測(cè)的距離信息組成集合ε，則系統(tǒng)目標(biāo)是通過最小化均方差的擬合優(yōu)度準(zhǔn)則(式(1))，獲得多機(jī)器人系統(tǒng)的最佳位置的估計(jì)，或者機(jī)器人通過移動(dòng)形成最佳的預(yù)定的網(wǎng)絡(luò)拓?fù)洹?/p>

因此，多機(jī)器人系統(tǒng)定位問題表示為優(yōu)化方程，如式(2)所示。

其中，pi和pj是待估計(jì)的機(jī)器人位置，dij是機(jī)器人之間的距離量測(cè)值。估計(jì)量與測(cè)量值之間的差越小，表明機(jī)器人估計(jì)的位置信息越準(zhǔn)確。

3 基于梯度的分布式定位方法

3.1 梯度下降定位法

在優(yōu)化理論中，采用梯度下降法可以獲得目標(biāo)函數(shù)的局部最優(yōu)值。對(duì)于多機(jī)器人系統(tǒng)，如果將每個(gè)機(jī)器人看作一個(gè)節(jié)點(diǎn)，機(jī)器人之間的聯(lián)系看作線段，則多機(jī)器人系統(tǒng)可以假設(shè)為一個(gè)圖，若這個(gè)圖是廣義的全局剛性圖，則上述優(yōu)化問題具有唯一的最優(yōu)值，即局部最優(yōu)解就是該問題的全局最優(yōu)解，因此每個(gè)機(jī)器人獲得唯一的最優(yōu)位置[14]。

其中 ατ是步長；(p)；Δgij(p) = 2[OT，…， (pi-pj)T，OT…OT，(pj-pi)T，…，OT]，ij(p) 含有 n 個(gè)向量，每個(gè)向量的維數(shù)是2×1，因此ij(p)是一個(gè)包含2n個(gè)元素的行向量。

3.2 自適應(yīng)步長的計(jì)算

在梯度下降法中，步長決定算法收斂的速度和穩(wěn)定性，固定步長通常無法有效平衡收斂的快速性和穩(wěn)定性，為此尋找合適的步長成為該方法的主要問題。這里采用了一種自適應(yīng)的步長計(jì)算方法，Barzilai-Borwein步長(簡(jiǎn)稱BB步長)，可以在每次迭代過程中找到最大或最小的步長，從而提高算法效率。BB步長的計(jì)算公式為式(4)。

分布式梯度算法采用BB步長，分為2個(gè)階段進(jìn)行，第1階段是分布式步長計(jì)算，第2階段是每個(gè)機(jī)器人利用鄰居的測(cè)量信息分布式迭代更新定位[15]。

3.3 BB步長的分布式計(jì)算

假設(shè)每個(gè)機(jī)器人都可以獲得自身與近鄰的當(dāng)前位置信息和上一個(gè)時(shí)刻的位置估計(jì)。上述步長計(jì)算公式(4)中的分子和分母分別用標(biāo)量θ和γ表示，則第 i個(gè)機(jī)器人在 τ(0)時(shí)刻的初值θi(τ(0)) 和γi(τ(0)) 如式(5)所示。

任意時(shí)刻τ(t+1)的θi(τ(t+1))和 γi(τ(t+1))迭代如式(6)、式(7)所示。

其中Wij滿足Metropolis準(zhǔn)則；Ni表示機(jī)器人i的鄰居集合，即 Ni={j：(i，j) ∈ε}； Ni表述集合Ni的基數(shù)，即集合中元素個(gè)數(shù)。

對(duì)于多機(jī)器人系統(tǒng)構(gòu)成的通信網(wǎng)絡(luò)，如果該網(wǎng)絡(luò)表示的圖是連接圖，即機(jī)器人之間可以彼此交互，則每個(gè)機(jī)器人在有限的時(shí)間內(nèi)可以計(jì)算迭代步長，如式(8)所示。

證明：式(8)中的分子寫成如下形式：

θ(τ(t+1))=W·θ(τ(t))，θ(τ(t))=Wt·θ(τ(0))，其中，θ(τ)=[θ1(τ)，…，θn(τ)]。 由于矩陣 W滿足如下條件： Wkz-1ζ≤λ2(W)k·‖z‖， λ2(W)＜ 1，λ2(W) 是矩陣W的第2個(gè)特征值，ζ是向量z中所有元素的均值，則，‖θ(τ(0))‖。 由于 ‖·‖≥ ‖·‖∞( ‖·‖與‖·‖∞分別表示元素·的二范數(shù)與無窮范數(shù))，式(9)和式(10)成立。

隨著 t→∞，θi(τ(t))和γi(τ(t)) 分別收斂到 θ(τ(0))和 γ(τ(0))，因此各個(gè)機(jī)器人獨(dú)立計(jì)算的迭代步長可以收斂到算法統(tǒng)一的步長值，如式(12)所示。

3.4 機(jī)器人位置更新過程

第i個(gè)機(jī)器人的位置更新過程如式(14)所示。

將式(13)代入到式(14)，則每個(gè)機(jī)器人的位置更新的計(jì)算方法，如式(15)所示。

注意，式(15)表明每個(gè)機(jī)器人進(jìn)行位置更新的時(shí)候，分布式步長αi(τ)已經(jīng)收斂到一致的步長值ατ[16]。算法流程如下：

1)初始化。設(shè)定一致步長計(jì)算階段最大的迭代次數(shù)tmax，定位容忍誤差οtolerance。 假設(shè)位置更新迭代次數(shù)τ=0，對(duì)機(jī)器人位置進(jìn)行初始估計(jì)，采用均勻分布或者高斯分布的位置假設(shè)。

2)一致步長的計(jì)算。假設(shè)t=0，根據(jù)式(5)計(jì)算 θi(τ(t)) 和 γi(τ(t))；t=t+1，根據(jù)式(6)和式(7)更新θi(τ(t))和γi(τ(t)) ；直到t≤tmax停止迭代。

3)機(jī)器人位置更新。根據(jù)式(8)計(jì)算αi(τ(t))；τ=τ+1，根據(jù)式(15)計(jì)算新的位置；當(dāng)‖pi(τ+1)-pi(τ)‖≤οtolerance成立，停止位置更新。

4)輸出每個(gè)機(jī)器人的定位終值。p(τ+1)=[p1(τ +1)，…pn(τ +1)]。

4 仿真試驗(yàn)

多個(gè)機(jī)器人在未知環(huán)境中執(zhí)行探測(cè)任務(wù)時(shí)，首先需要到達(dá)指定位置，即構(gòu)成指定的多機(jī)器人網(wǎng)絡(luò)。這里目標(biāo)網(wǎng)絡(luò)為平面上10個(gè)機(jī)器人構(gòu)成的多機(jī)器人系統(tǒng)，如圖1所示。圖中節(jié)點(diǎn)表示機(jī)器人，虛線表示機(jī)器人之間的信息測(cè)量與交互關(guān)系；星號(hào)表示待定位的機(jī)器人位置；3個(gè)邊界點(diǎn)表示錨點(diǎn)機(jī)器人，其位置已知，用于確定整個(gè)網(wǎng)絡(luò)的全局坐標(biāo)，由圓圈標(biāo)識(shí)。根據(jù)圖論可知對(duì)于二維的網(wǎng)絡(luò)，如果至少3個(gè)非共線節(jié)點(diǎn)的絕對(duì)位置已知，那么整個(gè)網(wǎng)絡(luò)有唯一拓?fù)浣Y(jié)構(gòu)，即多機(jī)器人網(wǎng)絡(luò)中每個(gè)機(jī)器人位置有唯一的解。因此這里選用網(wǎng)絡(luò)邊界不共線的3個(gè)點(diǎn)作為錨點(diǎn)機(jī)器人。由于該算法是一種迭代方法，需要給每個(gè)機(jī)器人一個(gè)任意初始位置估計(jì)，如圖2表示，用圓圈標(biāo)識(shí)。為驗(yàn)證方法的收斂性，這里采用2種初值估計(jì)方法，第一假設(shè)初值在定位區(qū)域內(nèi)符合均勻分布，如圖2所示；第二假設(shè)初值在真實(shí)位置附近符合高斯分布。經(jīng)過該算法的運(yùn)算，任意一種分布的位置初值最終都會(huì)收斂到真實(shí)位置，即如圖3中三角表示的位置。

假設(shè)機(jī)器人初始位置估計(jì)滿足在給定區(qū)域[1 m×1 m]內(nèi)的均勻分布，分布式步長迭代次數(shù)tmax=20，每個(gè)機(jī)器人獨(dú)立采用分布式梯度方法，其位置估計(jì)誤差的收斂過程如圖4所示。每個(gè)機(jī)器人位置估計(jì)誤差收斂到給定的容忍誤差范圍之內(nèi)，即οtolerance=10-12，試驗(yàn)中位置更新迭代次數(shù)τ小于80。

圖1 多機(jī)器人的目標(biāo)網(wǎng)形狀Fig.1 Target shape of multi-robot network

圖2 多機(jī)器人的初始估計(jì)位置Fig.2 Initial position guess of multi-robot network

圖3 分布式梯度方法的定位結(jié)果Fig.3 Localization result of distributed gradient based method

圖4 每個(gè)機(jī)器人的定位誤差隨時(shí)間的收斂性能Fig.4 Convergence performance of robots in the network

實(shí)際中機(jī)器人的初值估計(jì)可能存在多種情況。這里分別考慮機(jī)器人初始位置估計(jì)在給定區(qū)域[1 m×1 m]內(nèi)的均勻分布和以目標(biāo)位置為均值、標(biāo)準(zhǔn)差為0.4 m的高斯分布2種情況，每種條件進(jìn)行100次蒙特卡洛仿真，算法性能比較結(jié)果如表1所示。設(shè)置不同的步長計(jì)算迭代次數(shù)t，t=10、20、40、80。 算法性能從誤差收斂的迭代次數(shù)、定位誤差和計(jì)算時(shí)間來評(píng)價(jià)。結(jié)果表明，算法分布式計(jì)算步長需要的迭代次數(shù)對(duì)定位誤差和算法誤差收斂迭代次數(shù)的影響很小。但算法的計(jì)算時(shí)間與分布式步長計(jì)算的內(nèi)部迭代次數(shù)相關(guān)，隨著與內(nèi)部迭代次數(shù)的增加近似成比例變化。因此，在分布式計(jì)算步長過程中，t較小就可以滿足步長一致的要求。

表1 不同初始估計(jì)條件的性能比價(jià)Table 1 Algorithm performance under different initial position guesses

圖5展示了上述對(duì)比試驗(yàn)中，機(jī)器人從初始估計(jì)的位置(圖中星號(hào))收斂到目標(biāo)網(wǎng)絡(luò)定位節(jié)點(diǎn)(圖中三角號(hào))的軌跡。圖5(a)假設(shè)初始位置為均勻分布，適合對(duì)未知環(huán)境機(jī)器人初始部署。從收斂軌跡可見，機(jī)器人移動(dòng)的步長較大，而收斂有震蕩情況發(fā)生。圖5(b)假設(shè)初始位置估計(jì)為高斯分布，收斂軌跡步長比較小而且速度較快，適合當(dāng)機(jī)器人定位誤差較大時(shí)重定位的情況。這也表明，初始位置估計(jì)誤差較大時(shí)，算法需要較長時(shí)間，但最終會(huì)收斂到全局的最優(yōu)值。

在上述仿真試驗(yàn)中，重點(diǎn)考慮了靜態(tài)的多機(jī)器人系統(tǒng)，即當(dāng)多個(gè)機(jī)器人隨機(jī)分布在給定區(qū)域，利用有限的測(cè)距信息，可以快速確定每個(gè)機(jī)器人在整個(gè)網(wǎng)絡(luò)中的定位。對(duì)移動(dòng)的多機(jī)器人網(wǎng)絡(luò)而言，如果可以保持彼此的通信暢通，利用該方法，機(jī)器人可以運(yùn)動(dòng)到指定目標(biāo)位置，形成期望的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)。這樣圖5中的收斂軌跡就演變成移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡。

圖5 不同初始位置估計(jì)條件下機(jī)器人位置收斂軌跡Fig.5 Convergent trajectories of robots in the network under two initial position guesses

5 結(jié)論

該分布式定位算法可以分為一致步長迭代和機(jī)器人位置更新2個(gè)階段。仿真驗(yàn)證表明當(dāng)多機(jī)器人網(wǎng)絡(luò)拓?fù)錇閯偠葓D時(shí)，該算法具有全局收斂性，即對(duì)機(jī)器人初始位置估計(jì)值不敏感。由于只需少數(shù)幾次迭代即可獲得一致步長，算法在滿足分布式運(yùn)算的同時(shí)還可滿足計(jì)算效率和定位精度的要求。另外，對(duì)多個(gè)移動(dòng)機(jī)器人形成指定隊(duì)列的問題，該算法從初值估計(jì)到精確定位的收斂軌跡可以作為移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡，因此該方法可以擴(kuò)展應(yīng)用到多移動(dòng)機(jī)器人網(wǎng)絡(luò)。