盧虎，蔣小強，閔歡

空軍工程大學 信息與導航學院，西安 710077

多智能體(無人機、機器人等)同步定位與建圖技術(MSLAM)是機器感知的核心技術，在MSLAM過程中，多智能體的軌跡估計是后端優(yōu)化部分的一個重要環(huán)節(jié)，每個智能體利用自身測量和其他智能體的關聯(lián)位姿，優(yōu)化自身的軌跡以減小累積誤差，可以極大提高復雜場景三維地圖構建速度和定位精度，是復雜人工智能的核心技術之一。

現(xiàn)有多智能體軌跡估計研究涉及通信受限[1-2]、異質(zhì)結構[3-4]、一致性分析[5]以及魯棒位置重識[6]等多個技術領域；在具體實現(xiàn)算法上，從卡爾曼濾波[7]、信息濾波[8]、粒子濾波[9-10]到當下基于最大似然的位姿圖優(yōu)化方法均有涉及。但是，現(xiàn)有研究[11-14]基本都是將多個多智能的測量結果集中到中心節(jié)點來進行位姿的估計和優(yōu)化，具有信息傳輸量大、易受干擾、對硬件性能要求過高等諸多技術缺陷。由于通信約束是實際應用中影響系統(tǒng)性能的關鍵因素，僅利用局部信息傳輸?shù)姆植际杰壽E估計算法一直是MSLAM領域的研究熱點，如Aragues等[15]提出分布式雅克比迭代法，對二維運動的位姿軌跡進行估計；Knuth和Barooah[16]基于黎曼優(yōu)化提出了一種分布式算法，實現(xiàn)了三維條件下異構機器人之間的協(xié)作定位；Schuster等[17]結合濾波和圖優(yōu)化的各自優(yōu)勢，提出了一種解耦的分布式多機器人軌跡優(yōu)化算法。但是，上述這3種算法均要求智能體共享自身的全部信息，大規(guī)模群體條件下，會對整個集群系統(tǒng)的通信帶寬造成極大負荷，且系統(tǒng)拓撲結構擴展性差。

雖然，Cunningham等[18-19]發(fā)現(xiàn)上述方法具有信息交換量大的不足，并基于因子圖理論[20]中的變量消元算法，提出了DDF-SAM和DDF-SAM 2.0算法，通過對因子圖中的位姿進行變量消元和邊緣化操作，實現(xiàn)了智能體之間共享只包含地圖點的因子圖以優(yōu)化自身軌跡，在一定程度上降低了信息交換量，也使得高斯消元法成為分布式軌跡估計的主流算法。但是，高斯消元法仍存在兩個重要缺陷：① 邊緣化操作會使代表測量值的Hessian矩陣變得稠密，使得信息交換量隨智能體之間的關聯(lián)位姿數(shù)呈二次方增長；② 需要好的線性化點，在大規(guī)模場景下很難保證智能體之間的一致線性化。

在此背景下，本文針對通信約束場景下多智能體最大似然軌跡估計問題，提出了一種基于超松弛迭代法(Successive Over-Relaxation, SOR)的分布式估計算法，旨在兼顧最小化信息交換量和保證軌跡估計精度，算法通過將軌跡估計轉化為二次優(yōu)化問題，并重新參數(shù)化為線性問題，最后采用分布式SOR算法分別求解，從而達到位姿優(yōu)化的目的；此外，由于對估計量的初始值采用了標記初始化方法[21]，減少了收斂所需的迭代次數(shù)，極大程度滿足了多智能體系統(tǒng)的實時性要求。

1 多智能體軌跡估計模型

考慮三維空間中一組智能體的集合Ω={α,β,γ,…}，定義時刻i智能體α的位姿為xαi，這里xαi∈SE(3)(SE(3)為三維空間中的特殊歐式群)；又定義xαi=(Rαi,tαi)，其中Rαi∈SO(3)為旋轉矩陣(SO(3)為特殊正交群)，tαi∈R3為平移向量。

顯然，智能體α的軌跡可以表示為時序上排列的位姿集合xα=[xα1,xα2,xα3,…]，如圖1所示。

圖1 多智能體軌跡估計示意圖

1.1 通用量測模型

假設每個智能體可直接獲得兩類相對位姿的測量信息：智能體自身的測量和智能體間的測量，其中，智能體自身的測量由里程計信息(時間上連續(xù)的位姿，如圖1中的xαi和xαi+1)和回環(huán)測量(時間上不連續(xù)的位姿，圖1中的xαi-1和xαi+1)組成；智能體間的測量為不同智能體某時刻的相對位姿(可以通過視線內(nèi)觀測、相遇點測量和回環(huán)檢測等方式得到) 圖1中連接xαi和xβj的紅線。

顯然，兩種不同類型的測量均為某一對智能體位姿之間的相對測量，記為xαi和xβj，測量模型可以表示為

(1)

1.2 最大似然位姿估計

設x=[xα,xβ,xγ,…]為所有智能體待估計軌跡，又可知所有測量相互獨立，則x的最大似然估計為

(2)

則x?{(Rαi,tαi),?α∈Ω,?i}的最大似然估計可以通過求解集中式目標函數(shù)得到:

(3)

為求解式(3)，通常方法是先通過中心服務器或者指定一個智能體收集所有智能體的位姿軌跡和測量值ε，然后利用流形上的迭代優(yōu)化[23]、快速近似[24]和凸松弛[25]等方法來進行全局的優(yōu)化求解，實際中由于計算能力和通信帶寬的約束，集中式方法在絕大多數(shù)應用場景下不具有可擴展性，而分布式的方法則能很好地解決這個問題，因此需要設計一種分布式的方法來求解式(3)。

2 基于SOR的分布式軌跡估計

2.1 分級求解策略

2.1.1 旋轉矩陣的松弛初始化

將式(3)中的第2項提取出來，作為旋轉估計的目標函數(shù):

(4)

這里Rαi∈SO(3)，根據(jù)文獻[26]可將式(4)改寫為無約束的形式：

(5)

將式(5)的結果反投影到特殊正交群SO(3)，便可以得到旋轉估計值。

簡潔起見，改寫式(5)為

(6)

式中:r為由所有未知旋轉量Rαi(?α∈Ω,?i)按列展開后整合成的單個向量，矩陣Ar和向量br均為已知量。

至此問題轉化為常規(guī)的線性最小二乘問題：

(7)

2.1.2 完整位姿估計

(8)

將式(8)的非線性指數(shù)映射進行一階展開:

(9)

(10)

將所有的待求解量θαi、tαi(?α∈Ω,?i)表示為單個向量p，則式(10)可以簡化為

(11)

進一步可得

(12)

2.2 分布式算法描述

2.1節(jié)的分級位姿優(yōu)化方法中，將非線性的式(3)轉化為了兩個線性最小二乘問題式(7)和式(12)，通常可以用集中式兩級求解方法進行一次迭代的整體求解；但在多智能體場景中，式(7)和式(12)對應的Hessian具體結構也決定了其可以用完全分布式的方法來求解。

本節(jié)將具體闡述一種新型的只需共享部分位姿的、基于超松弛迭代法(SOR)的分布式求解方法。

在式(7)和式(12)中，若待求解量r和p按所歸屬的不同的智能體分為多個子向量形式，r=[rα,rβ,rγ,…]，p=[pα,pβ,pγ,…]；則式(7)和式(12)均可表示為通用形式:

(13)

式中：x為待求解的未知量，根據(jù)x的塊結構，可以將矩陣H和向量g劃分為等式右的形式，因此又有：

(14)

將式(14)中與xα相關的項提出來，得到

(15)

式(15)表示的等式集合本質(zhì)上跟式(13)相等，但式(15)中每個等式均與特定智能體相關聯(lián)，清晰地表示了每個智能體待估計變量與其他變量的關系，易于分布式求解。

(16)

2.3 分布式SOR算法的信息交換策略

(17)

圖2 軌跡估計示意圖及其對應的Hessian矩陣

2.4 標記初始化方法

(18)

迭代完成后同樣對β進行標記；重復上述步驟，直至所有智能體均完成初始化。

綜上，本文算法的整體流程如圖3所示。

3 實驗分析

3.1 實驗數(shù)據(jù)設置

3.1.1 仿真數(shù)據(jù)集設置

為了測試本文算法對智能體規(guī)模的可擴展性和收斂性，共設置了7種不同的測試數(shù)據(jù)：分別是數(shù)量為2、4、9、16、25、36、49的多智能體軌跡仿真數(shù)據(jù)。如圖4所示，各種顏色的立方體代表不同的智能體，每個智能體在立方體表面上運動，其中立方體的頂點表示待優(yōu)化的智能體位姿，邊表示智能體自身內(nèi)部測量值(即頂點間的相對位姿)；當智能體處于相鄰的頂點時，可以進行局部的通信(模擬通信受限條件)和測量，產(chǎn)生智能體間測量值，如圖4中連接不同智能體的灰色邊。仿真中設置測量噪聲的標準差為：σR=3°,σt=0.2 m，仿真結果取10次Monte-Carlo仿真的平均值。

圖3 整體算法流程

圖4 仿真實驗數(shù)據(jù)設置

3.1.2 實測數(shù)據(jù)集設置

為了分析對比不同測試條件下最優(yōu)松弛因子選取的差異，選取分割CSAIL實測數(shù)據(jù)集進行實驗，如圖5所示，藍色和綠色細線代表兩個智能體的里程計軌跡，藍色和綠色粗線表示機智能體自身回環(huán)測量，深藍色粗線表示智能體之間的回環(huán)測量。

圖5 CSAIL實測數(shù)據(jù)集

在SOR算法中，通過求解式(7)和式(12)，分別使式(6)和式(11)式達到最小值，可知第k次迭代整體的旋轉估計誤差為

(19)

第k次迭代整體的位姿向量誤差可定義為

(20)

隨著迭代次數(shù)的增加，er(k)、ep(k)將逐步收斂，迭代次數(shù)越多，求解的精度就越高。實驗中未做特殊說明，判斷兩種誤差收斂的閾值通常設置為ξr=ξp=10-2。

3.2 實驗結果

3.2.1 松弛因子影響分析

1) 仿真數(shù)據(jù)集實驗

為了分析松弛因子的選擇對實驗結果的影響和確定對應數(shù)據(jù)集的最優(yōu)松弛因子，選取智能體規(guī)模為49的仿真數(shù)據(jù)場景，在(0,2)范圍內(nèi)對γ選取了9種不同的值進行實驗，圖6表示在不同的γ值下，整體的姿態(tài)估計誤差和位姿向量估計誤差隨迭代次數(shù)的變化情況，可知當γ∈(0,2)時，分布式SOR算法總能收斂，而且，γ選取在1附近能達到較快的收斂速度，收斂誤差也較小。為了獲得最佳松弛因子，進一步選取γ∈[0.8,1.2]，步長取0.05，得到如圖7和圖8結果。

圖6 仿真數(shù)據(jù)集中不同γ下旋轉和位姿的收斂過程

圖7 仿真數(shù)據(jù)集中不同γ下旋轉和位姿的收斂過程

在松弛因子γ選取步長較小的情況下，從位姿估計曲線圖7(b)可以看出，松弛因子1可使位姿估計結果達到最優(yōu)。

圖8為姿態(tài)估計和位姿估計兩個過程的迭代次數(shù)隨松弛因子γ的變化曲線，易知γ=1.00將使旋轉和位姿估計兩步均達到最快的收斂速度。

圖8 仿真數(shù)據(jù)集中不同γ下收斂所需的迭代次數(shù)

綜上可得，在仿真合成數(shù)據(jù)集的實驗中，最優(yōu)松弛因子γ約為1.00，因此接下來幾個小節(jié)仿真數(shù)據(jù)集下的實驗，將固定γ=1.00。

2) CSAIL數(shù)據(jù)集實驗

由于松弛因子的選取與系數(shù)矩陣有關，因此最優(yōu)松弛因子需要根據(jù)不同實驗條件(對應不同的測試數(shù)據(jù))的實際情況來進行調(diào)整。在CSAIL數(shù)據(jù)集上，選取松弛因子γ∈[0.80,1.20]，步長為0.05進行實驗，得到圖9和圖10所示結果。

圖9 CSAIL中不同γ下旋轉和位姿的收斂過程

圖10 CSAIL不同γ下收斂所需的迭代次數(shù)

綜合分析姿態(tài)估計和位姿估計兩個過程的估計誤差和迭代次數(shù)，可以得出CSAIL數(shù)據(jù)集上最優(yōu)松弛因子在1.05附近取得。

綜上，在不同的測試條件下，可以對松弛因子γ進行調(diào)整，從而使分布式SOR多智能體軌跡估計算法的收斂速度和估計精度達到最優(yōu)效果。

3.2.2 局部收斂誤差分析和迭代停止條件

本節(jié)仍以49個智能體的軌跡仿真數(shù)據(jù)為測試樣本，對每個智能體上運行的分布式SOR算法的收斂性進行分析，進而給出分布式求解過程的收斂性與整體收斂性的關系。

如圖11所示，不同的顏色代表不同的智能體上算法的收斂過程，eri(k)和epi(k)分別為第i個智能體在第k次迭代時的姿態(tài)和位姿求解誤差。由于本文算法建立在位姿圖的基礎上，其利用各個智能體下短間隔可靠的局部里程計測量和回環(huán)測量構建一個涵蓋全部智能體的全局優(yōu)化問題，因此算法并不保證每個智能體的局部位姿達到最優(yōu)，而是使全局位姿最優(yōu)，如圖11(a)姿態(tài)解算過程最上方的藍色曲線，其誤差值從-1增長至收斂值，而其他智能體姿態(tài)則減小至收斂值，但從整體來看，如圖6(a)中γ=1時的曲線，姿態(tài)誤差則呈下降趨勢。從圖11(b)的位姿誤差變化趨勢可得同樣結論。圖11(c)和圖11(d)為迭代過程中姿態(tài)和位姿估計值的變化量，經(jīng)過幾次迭代之后，變化量會很快減小到10-2量級，如3.1節(jié)提到，實驗中設定的收斂閾值為ξr=ξp=10-2，當估計值的變化小于閾值時，就認為算法收斂，進而停止迭代。

圖11 每個智能體上算法的收斂性

圖12為在49個智能體數(shù)據(jù)場景下，算法迭代10次和1 000次時整體的位姿(位姿中的平移部分)軌跡，可以看出在迭代10次時，相較于初始的軌跡，估計位姿的精度已經(jīng)有了很大的提高，且相對于迭代1 000次的結果，誤差并沒有特別明顯的改觀。因此，可以認為本文算法在迭代次數(shù)較少的情況下也能取得較高的估計精度。

圖12 估計軌跡的可視化圖

3.2.3 不同智能體下算法的擴展性

為了分析智能體數(shù)量對算法性能的影響，分別在不同的數(shù)據(jù)場景下進行實驗，圖13為7種仿真數(shù)據(jù)(2、4、9、16、25、36和49個智能體)下運行的結果，可以看出整體的姿態(tài)估計和位姿估計過程均能很快收斂，且由圖13(b)可知，對于位姿解算來說，智能體數(shù)量規(guī)模的增大會使整體的收斂誤差增大，但每個智能體的平均誤差相當，因此算法對智能體規(guī)模的適應性和可擴展性較強。

圖13 不同規(guī)模智能體下算法的收斂性

3.2.4 標記初始化對收斂速度的影響

2.4節(jié)中提出了一種標記初始化方法，相對于分別將r(0)、p(0)初始化為零向量的方法，標記初始化方法則能加速算法的收斂速度。如圖14所示，為49個智能體實驗數(shù)據(jù)場景下的初始化方法的對比，可以看出標記初始化方法減少了姿態(tài)估計和位姿估計的迭代次數(shù)，其中在位姿估計過程上體現(xiàn)尤為明顯。由于優(yōu)化過程中信息交換量與迭代次數(shù)呈線性關系(每次迭代均要進行通信)，標記初始化方法進一步減少了數(shù)據(jù)傳輸量。因此，在本節(jié)的其他實驗分析中，均默認采用了標記初始化方法。

圖14 有標記初始化和無標記初始化收斂過程對比

3.2.5 算法精度分析

為了評估本文算法的軌跡估計精度，分別對位置誤差和旋轉誤差進行分析。類似文獻[28]提出的方法，定義絕對位置誤差(Absolute Translation Error,ATE)和絕對旋轉誤差(Absolute Rotation Error, ARE)兩種策略：

(21)

(22)

表1為本文算法、集中式兩級求解算法和初始情況下的ATE和ARE值?？梢钥闯觯斉袛嚅撝郸蝦=ξp=10-2時，本文所提分布式算法可以達到集中式算法的精度水平，且當智能體規(guī)模為49時，本文算法的位置誤差仍然小于0.15 m，旋轉誤差小于0.03°。

當ξr=ξp=10-1時，相對于10-2條件下的估計結果，精度雖有所下降，但在實驗中各規(guī)模大小的智能體場景下，位置估計精度仍能達到分米級，且旋轉估計誤差小于0.1°。

表1 ATE和ARE誤差對比

3.2.6 信息交換量分析

表2 不同閾值下算法的迭代次數(shù)

圖15為本文算法與DDF-SAM算法的信息傳輸量對比，分析可知，即使判斷閾值設為10-2(即迭代次數(shù)較大條件)，本文算法的信息傳輸量在小集群條件下僅為DDF-SAM的0.01%，在49個智能體的大規(guī)模集群實驗條件下，信息傳輸量也只達到DDF-SAM的0.06%，而且可以看出，DDF-SAM的信息交換量隨智能體數(shù)目增長大致呈二次曲線，而本文算法基本為線性增長。

圖15 信息傳輸量對比

4 結 論

1) 提出了一種基于超松弛迭代法的分布式兩級多智能體軌跡估計算法，利用多智能體之間相對位姿的測量，達到優(yōu)化自身位姿軌跡的效果。實驗表明，所提算法有以下優(yōu)勢：① 分布式SOR算法的信息交換少，且只需進行局部信息傳輸，能適應通信受限的條件；② 收斂所需的迭代次數(shù)較少(即所需通信次數(shù))，且能達到跟集中式算法同樣的精度；③ 算法對智能體規(guī)模的適應性強，能應對大規(guī)模場景下多智能體的軌跡估計。

2) 本文所提分布式估計算法的信息交換量與智能體之間的關聯(lián)位姿數(shù)呈線性關系，且不需要線性化點，因而能很好地擴展到大規(guī)模多智能體場景中。由于迭代過程只要求智能體之間共享與相對測量相關的部分軌跡數(shù)據(jù)，即智能體不會存儲其他智能體的完整軌跡，因此也滿足隱秘性要求的應用場景。