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

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

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

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

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

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

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

1 多智能體軌跡估計(jì)模型

考慮三維空間中一組智能體的集合Ω={α,β,γ,…}，定義時(shí)刻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為平移向量。

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

圖1 多智能體軌跡估計(jì)示意圖

1.1 通用量測(cè)模型

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

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

(1)

1.2 最大似然位姿估計(jì)

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

(2)

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

(3)

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

2 基于SOR的分布式軌跡估計(jì)

2.1 分級(jí)求解策略

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

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

(4)

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

(5)

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

簡(jiǎn)潔起見(jiàn)，改寫(xiě)式(5)為

(6)

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

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

(7)

2.1.2 完整位姿估計(jì)

(8)

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

(9)

(10)

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

(11)

進(jìn)一步可得

(12)

2.2 分布式算法描述

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

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

在式(7)和式(12)中，若待求解量r和p按所歸屬的不同的智能體分為多個(gè)子向量形式，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)提出來(lái)，得到

(15)

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

(16)

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

(17)

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

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

(18)

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

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

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

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

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

為了測(cè)試本文算法對(duì)智能體規(guī)模的可擴(kuò)展性和收斂性，共設(shè)置了7種不同的測(cè)試數(shù)據(jù)：分別是數(shù)量為2、4、9、16、25、36、49的多智能體軌跡仿真數(shù)據(jù)。如圖4所示，各種顏色的立方體代表不同的智能體，每個(gè)智能體在立方體表面上運(yùn)動(dòng)，其中立方體的頂點(diǎn)表示待優(yōu)化的智能體位姿，邊表示智能體自身內(nèi)部測(cè)量值(即頂點(diǎn)間的相對(duì)位姿)；當(dāng)智能體處于相鄰的頂點(diǎn)時(shí)，可以進(jìn)行局部的通信(模擬通信受限條件)和測(cè)量，產(chǎn)生智能體間測(cè)量值，如圖4中連接不同智能體的灰色邊。仿真中設(shè)置測(cè)量噪聲的標(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í)測(cè)數(shù)據(jù)集設(shè)置

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

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

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

(19)

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

(20)

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

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

3.2.1 松弛因子影響分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖11 每個(gè)智能體上算法的收斂性

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

圖12 估計(jì)軌跡的可視化圖

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

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

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

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

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

圖14 有標(biāo)記初始化和無(wú)標(biāo)記初始化收斂過(guò)程對(duì)比

3.2.5 算法精度分析

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

(21)

(22)

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

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

表1 ATE和ARE誤差對(duì)比

3.2.6 信息交換量分析

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

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

圖15 信息傳輸量對(duì)比

4 結(jié) 論

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

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