具有通信約束的分布式SOR多智能體軌跡估計算法

盧虎，蔣小強(qiáng)，閔歡

空軍工程大學(xué) 信息與導(dǎo)航學(xué)院，西安 710077

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

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

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

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

1 多智能體軌跡估計模型

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

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

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

1.1 通用量測模型

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

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

(1)

1.2 最大似然位姿估計

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

(2)

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

(3)

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

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

2.1 分級求解策略

2.1.1 旋轉(zhuǎn)矩陣的松弛初始化

將式(3)中的第2項(xiàng)提取出來，作為旋轉(zhuǎn)估計的目標(biāo)函數(shù):

(4)

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

(5)

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

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

(6)

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

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

(7)

2.1.2 完整位姿估計

(8)

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

(9)

(10)

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

(11)

進(jìn)一步可得

(12)

2.2 分布式算法描述

2.1節(jié)的分級位姿優(yōu)化方法中，將非線性的式(3)轉(zhuǎn)化為了兩個線性最小二乘問題式(7)和式(12)，通?？梢杂眉惺絻杉壡蠼夥椒ㄟM(jìn)行一次迭代的整體求解；但在多智能體場景中，式(7)和式(12)對應(yīng)的Hessian具體結(jié)構(gòu)也決定了其可以用完全分布式的方法來求解。

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

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

(13)

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

(14)

將式(14)中與xα相關(guān)的項(xiàng)提出來，得到

(15)

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

(16)

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

(17)

圖2 軌跡估計示意圖及其對應(yīng)的Hessian矩陣

2.4 標(biāo)記初始化方法

(18)

迭代完成后同樣對β進(jìn)行標(biāo)記；重復(fù)上述步驟，直至所有智能體均完成初始化。

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

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

3.1 實(shí)驗(yàn)數(shù)據(jù)設(shè)置

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

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

圖3 整體算法流程

圖4 仿真實(shí)驗(yàn)數(shù)據(jù)設(shè)置

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

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

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

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

(19)

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

(20)

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

3.2 實(shí)驗(yàn)結(jié)果

3.2.1 松弛因子影響分析

1) 仿真數(shù)據(jù)集實(shí)驗(yàn)

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

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

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

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

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

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

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

2) CSAIL數(shù)據(jù)集實(shí)驗(yàn)

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

圖9 CSAIL中不同γ下旋轉(zhuǎn)和位姿的收斂過程

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

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

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

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

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

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

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

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

圖12 估計軌跡的可視化圖

3.2.3 不同智能體下算法的擴(kuò)展性

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

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

3.2.4 標(biāo)記初始化對收斂速度的影響

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

圖14 有標(biāo)記初始化和無標(biāo)記初始化收斂過程對比

3.2.5 算法精度分析

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

(21)

(22)

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

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

表1 ATE和ARE誤差對比

3.2.6 信息交換量分析

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

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

圖15 信息傳輸量對比

4 結(jié) 論

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

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