月面多機器人的分布式協同定位算法研究

魏明珠，謝曉梅，嚴 鵬，徐利梅

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

1 引言

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

多機器人協同進行月表探測的前提是每個機器人能夠快速準確地獲得自身在環境中的位置信息，因此多機器人系統中的定位是首要問題[7-8]。多機器人系統作為一個整體，機器人之間的協同定位可以利用機器人彼此的信息，獲得各自的位置。多機器人協同定位的方法有兩類：一類是集中式協同定位，每個機器人的信息都傳輸到中央處理單元進行處理，然后再發送給各個機器人[9]；另一類是分布式處理，即每個機器人利用與鄰居機器人交互的信息計算自己的位置[10]。因此分布式定位方法具有實際的價值，也是人們研究的熱點。Costa等[11]提出了基于分布式加權多維標度的算法，基于凸包中不含錨點的假設，利用質心計算方法，定位網絡中機器人的位置，但成本函數本身建立比較復雜，算法的運算量較大。Cui等[12]采用不依賴錨點的分布式定位方法，本質是通過多邊法的原理定位，定位后的節點作為虛擬錨點，該方法在網絡拓撲結構上有局限。王玲等[13]提出了基于擴展卡爾曼濾波的相對位姿測量方法，該方法需要機器人交互除位置信息外還有方位信息，同時沒有考慮大規模多機器人網絡的定位情況。

本文重點考慮基于測距信息的多機器人系統的分布式定位問題，對多機器人協同定位問題建立優化方程模型，并提出基于自適應步長的分布式梯度下降的定位方法，通過仿真試驗分析算法的性能。

2 多機器人系統問題建模

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

假設機器人之間量測的距離信息組成集合ε，則系統目標是通過最小化均方差的擬合優度準則(式(1))，獲得多機器人系統的最佳位置的估計，或者機器人通過移動形成最佳的預定的網絡拓撲。

因此，多機器人系統定位問題表示為優化方程，如式(2)所示。

其中，pi和pj是待估計的機器人位置，dij是機器人之間的距離量測值。估計量與測量值之間的差越小，表明機器人估計的位置信息越準確。

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

3.1 梯度下降定位法

在優化理論中，采用梯度下降法可以獲得目標函數的局部最優值。對于多機器人系統，如果將每個機器人看作一個節點，機器人之間的聯系看作線段，則多機器人系統可以假設為一個圖，若這個圖是廣義的全局剛性圖，則上述優化問題具有唯一的最優值，即局部最優解就是該問題的全局最優解，因此每個機器人獲得唯一的最優位置[14]。

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

3.2 自適應步長的計算

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

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

3.3 BB步長的分布式計算

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

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

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

對于多機器人系統構成的通信網絡，如果該網絡表示的圖是連接圖，即機器人之間可以彼此交互，則每個機器人在有限的時間內可以計算迭代步長，如式(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個特征值，ζ是向量z中所有元素的均值，則，‖θ(τ(0))‖。 由于 ‖·‖≥ ‖·‖∞( ‖·‖與‖·‖∞分別表示元素·的二范數與無窮范數)，式(9)和式(10)成立。

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

3.4 機器人位置更新過程

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

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

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

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

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

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

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

4 仿真試驗

多個機器人在未知環境中執行探測任務時，首先需要到達指定位置，即構成指定的多機器人網絡。這里目標網絡為平面上10個機器人構成的多機器人系統，如圖1所示。圖中節點表示機器人，虛線表示機器人之間的信息測量與交互關系；星號表示待定位的機器人位置；3個邊界點表示錨點機器人，其位置已知，用于確定整個網絡的全局坐標，由圓圈標識。根據圖論可知對于二維的網絡，如果至少3個非共線節點的絕對位置已知，那么整個網絡有唯一拓撲結構，即多機器人網絡中每個機器人位置有唯一的解。因此這里選用網絡邊界不共線的3個點作為錨點機器人。由于該算法是一種迭代方法，需要給每個機器人一個任意初始位置估計，如圖2表示，用圓圈標識。為驗證方法的收斂性，這里采用2種初值估計方法，第一假設初值在定位區域內符合均勻分布，如圖2所示；第二假設初值在真實位置附近符合高斯分布。經過該算法的運算，任意一種分布的位置初值最終都會收斂到真實位置，即如圖3中三角表示的位置。

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

圖1 多機器人的目標網形狀Fig.1 Target shape of multi-robot network

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

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

圖4 每個機器人的定位誤差隨時間的收斂性能Fig.4 Convergence performance of robots in the network

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

表1 不同初始估計條件的性能比價Table 1 Algorithm performance under different initial position guesses

圖5展示了上述對比試驗中，機器人從初始估計的位置(圖中星號)收斂到目標網絡定位節點(圖中三角號)的軌跡。圖5(a)假設初始位置為均勻分布，適合對未知環境機器人初始部署。從收斂軌跡可見，機器人移動的步長較大，而收斂有震蕩情況發生。圖5(b)假設初始位置估計為高斯分布，收斂軌跡步長比較小而且速度較快，適合當機器人定位誤差較大時重定位的情況。這也表明，初始位置估計誤差較大時，算法需要較長時間，但最終會收斂到全局的最優值。

在上述仿真試驗中，重點考慮了靜態的多機器人系統，即當多個機器人隨機分布在給定區域，利用有限的測距信息，可以快速確定每個機器人在整個網絡中的定位。對移動的多機器人網絡而言，如果可以保持彼此的通信暢通，利用該方法，機器人可以運動到指定目標位置，形成期望的網絡拓撲結構。這樣圖5中的收斂軌跡就演變成移動機器人的運動軌跡。

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

5 結論

該分布式定位算法可以分為一致步長迭代和機器人位置更新2個階段。仿真驗證表明當多機器人網絡拓撲為剛度圖時，該算法具有全局收斂性，即對機器人初始位置估計值不敏感。由于只需少數幾次迭代即可獲得一致步長，算法在滿足分布式運算的同時還可滿足計算效率和定位精度的要求。另外，對多個移動機器人形成指定隊列的問題，該算法從初值估計到精確定位的收斂軌跡可以作為移動機器人的運動軌跡，因此該方法可以擴展應用到多移動機器人網絡。