基于AGV 的復(fù)雜場景視覺SLAM 慣導(dǎo)系統(tǒng)設(shè)計

馮悅鳴，呂 勤，李 巖

（國網(wǎng)浙江省電力有限公司嘉興供電公司，浙江嘉興 314033）

文獻[1]基于圖模型提出一種視覺SLAM 系統(tǒng)誤差動態(tài)補償方法，利用改進生成樹遍歷得到包含完整場景的最小關(guān)鍵幀子集，采用基于雙窗口約束的平差策略限制計算量，通過四叉樹均衡化算法進行三維地圖點選取，可以得到更加精確的位姿估計與三維重建結(jié)果，提升位姿估計精度與重建模型的一致性。文獻[2]提出一種同時定位與地圖創(chuàng)建算法，利用RansacPnP 算法得到亞像素級的匹配點對，為初值進一步最小化重投影誤差進行局部優(yōu)化，基于匹配點對描述子距離提出一種不確定性模型，進行實地測試來判斷其實際精度和魯棒性，具有良好的綜合性能，滿足工業(yè)現(xiàn)場要求。

在傳統(tǒng)的視覺SLAM 系統(tǒng)中，當(dāng)載體運動速度很快、環(huán)境特征缺失、圖像清晰度較低時，攝像機采集的圖像可能存在相鄰幀的異常匹配特征或出現(xiàn)采集圖像模糊問題[3]，導(dǎo)致SLAM 系統(tǒng)的魯棒性和精度下降，甚至導(dǎo)致整個系統(tǒng)的失效?；贏GV(Automated Guided Vehicle，自動引導(dǎo)機器人）的視覺SLAM 慣性導(dǎo)航系統(tǒng)在一定程度上緩解了上述問題，提高了系統(tǒng)的魯棒性和精度，減小了光照等外界因素的影響。因此，在復(fù)雜的環(huán)境下，基于AGV 的SLAM 慣性導(dǎo)航系統(tǒng)將具有更重要的研究地位和廣闊的發(fā)展前景。

1 硬件結(jié)構(gòu)設(shè)計

硬件結(jié)構(gòu)包括發(fā)射器、信號處理器、接收器、傳感器、計算機與基準(zhǔn)尺、相機設(shè)備、AGV 系統(tǒng)、CPU 芯片等[4]。該系統(tǒng)具有多任務(wù)并行功能，且可完成測量任務(wù)，發(fā)射站負責(zé)發(fā)射任務(wù)，接收器的數(shù)量沒有限制；系統(tǒng)具有較強的抗干擾能力，前提是確保發(fā)射站和激光接收器的光敏設(shè)備對于光波的敏感性相同、光波相同[5-7]。硬件總體構(gòu)架如圖1 所示。

圖1 硬件總體構(gòu)架

完整的AGV 系統(tǒng)包括車載系統(tǒng)（即下位機）、外圍設(shè)施（如充電站）、控制系統(tǒng)（即上位機）。在AGV系統(tǒng)體系結(jié)構(gòu)中，上位機作為控制終端，功能是調(diào)度系統(tǒng)任務(wù)，是系統(tǒng)的中心構(gòu)造?？刂平K端具體工作職責(zé)是對所有車載系統(tǒng)傳送的數(shù)據(jù)進行接收，同時實時監(jiān)控系統(tǒng)的運行狀態(tài)。除此之外，還要負責(zé)搭建人機交互界面，使接收指令任務(wù)、解析任務(wù)能夠正常順利進行；還具備分配任務(wù)規(guī)劃路徑調(diào)度車輛的功能，亦可完成交通管理。外圍設(shè)備的功能是保證系統(tǒng)的安全、正常運作，隸屬輔助設(shè)備之一。外圍設(shè)備有導(dǎo)引系統(tǒng)以及激光反射板、通信中轉(zhuǎn)站、能量供給站等。下位機負責(zé)AGV 的管控，借助通訊技術(shù)與控制技術(shù)進行通訊，在對下達的指令進行接收任務(wù)后，能夠完成AGV 的自主定位、追蹤路徑的規(guī)劃、系統(tǒng)運動控制以及對障礙的檢測等[8-9]。

1.1 AGV系統(tǒng)

AGV 系統(tǒng)的控制借助物流調(diào)控上位系統(tǒng)、AGV地面控制系統(tǒng)、AGV 車載控制系統(tǒng)共同協(xié)作[10-13]。在AGV 系統(tǒng)中，導(dǎo)航定位技術(shù)是整個系統(tǒng)的核心，內(nèi)部構(gòu)成有終端控制系統(tǒng)、車載控制系統(tǒng)。終端控制系統(tǒng)的主要構(gòu)成包括對地圖的管理、對任務(wù)的調(diào)度、對路徑的規(guī)劃等；車載控制系統(tǒng)的主要構(gòu)成有導(dǎo)航系統(tǒng)、定位系統(tǒng)、驅(qū)動設(shè)備等，工作內(nèi)容為無線通信的接收以及完成對接收數(shù)據(jù)信息的交互任務(wù)。AGV 總體結(jié)構(gòu)如圖2 所示。

圖2 AGV總體結(jié)構(gòu)

AGV 的工作原理：沿著磁條進行自主導(dǎo)航任務(wù)，到達指定區(qū)域后停止磁導(dǎo)航任務(wù)，自動切換為二維碼導(dǎo)航，在執(zhí)行完二維碼的定位以及導(dǎo)航任務(wù)后，前往特定站點實現(xiàn)區(qū)域服務(wù)器與中央服務(wù)器的通信，接收指令信息并對路徑進行規(guī)劃。

1.2 接收器

其工作原理是將接收器固定在后交互式運行形式中，并將發(fā)射站固定在AGV 車體上，如工裝房中的墻壁、地面等合適位置。在AGV 車體進行運動的過程中，發(fā)射站將保持對空間環(huán)境的持續(xù)掃描狀態(tài)。一旦發(fā)出的激光信號被固定接收器接收后，便能夠?qū)崟r定位解算出發(fā)射站與全局導(dǎo)航坐標(biāo)系相對位置關(guān)系、姿態(tài)信息。接收器是導(dǎo)航領(lǐng)域的地標(biāo)位置點，三維坐標(biāo)需要保證高精確度，并且需要進行統(tǒng)一化保存在導(dǎo)航坐標(biāo)系之中。

綜上所述，該線路的牽引供電系統(tǒng)在單邊供電模式下可以滿足2列AW3列車同時起動的負荷需求，在大雙邊供電模式下可以滿足4列AW0列車同時起動的負荷需求，其牽引供電能力滿足標(biāo)準(zhǔn)及設(shè)計的相關(guān)要求。通過對測試數(shù)據(jù)進行后期分析，可以發(fā)現(xiàn)既有設(shè)計單位的基本測試數(shù)據(jù)能滿足線路初期和負載特性需求。

1.3 傳感器

激光雷達工作原理類似于聲吶，但是相比聲吶系統(tǒng)激光雷達將激光作為外部發(fā)射以及接收的信號源，這保證了測距過程的速率有了明顯的提高。而且激光雷達可以使測距的誤差明顯縮小，提高系統(tǒng)的精確度，減少了測距過程所造成的結(jié)果不準(zhǔn)確現(xiàn)象的發(fā)生[14-16]。激光雷達將獲取到的環(huán)境數(shù)據(jù)信息依據(jù)分散點云的形式予以展現(xiàn)。點云可以較詳細地對信息進行描述，蘊含的信息量比較豐富。蘊含的信息包括比較準(zhǔn)確的距離以及角度等數(shù)據(jù)信息。通過這些，激光雷達作為環(huán)境傳感器能夠直接利用點云狀態(tài)信息的絕對不變性，將相鄰的每一刻的激光雷達獲取到的點云數(shù)據(jù)進行信息的匹配與描述，完成前后時刻機器人相對運動的運算主要包括旋轉(zhuǎn)和平移的運動，借以完成對自身的定位。

2 軟件功能設(shè)計

2.1 融合人工信標(biāo)的多目SLAM算法流程

SLAM 系統(tǒng)是非線性優(yōu)化框架，由前端的位姿跟蹤和后端的全局優(yōu)化構(gòu)成。系統(tǒng)的后端以全局閉環(huán)檢測為基礎(chǔ)執(zhí)行針對位姿的優(yōu)化任務(wù)。與傳統(tǒng)的純?nèi)斯LAM 算法相比較，基于AGV 的復(fù)雜場景視覺SLAM 算法具有更強的光照變化適應(yīng)性，并且在一定程度上能夠減少人工信標(biāo)的使用數(shù)量，降低了人力、物力、資源的投入量；除此之外，系統(tǒng)的魯棒性和實用性也得到了明顯的提高。SLAM 算法流程分為前端與后端，具體流程如圖3 所示。

圖3 SLAM算法總體流程圖

在SLAM 系統(tǒng)跟蹤過程中，以載體的運動模型為參考，與勻速模型類似，進行當(dāng)前運動狀態(tài)的位姿判別。在載體進行平穩(wěn)運動時，模型能夠達到較好的運行效果。然而，一旦載體運動速率超出適宜值，很容易出現(xiàn)跟蹤失敗的結(jié)果。視覺慣導(dǎo)系統(tǒng)的工作原理是以幀速率為參照標(biāo)準(zhǔn)進行傳感器的姿態(tài)、差值以及速率的追蹤，能夠獲取到可靠性比較高的位姿判別結(jié)果。

2.2 AGV驅(qū)動功能設(shè)計

AGV 的運輸能力以及傳動性能重點依靠驅(qū)動模塊，所以驅(qū)動模塊的設(shè)計在AGV 的設(shè)計中很重要。首先根據(jù)參考驅(qū)動模塊的結(jié)構(gòu)特點設(shè)計了差速驅(qū)動模式及形式構(gòu)成；第二步將分析后得到的驅(qū)動模塊主要性能參數(shù)作為基礎(chǔ)對驅(qū)動模塊的主要構(gòu)成部件進行選取。

驅(qū)動模塊的主要構(gòu)成為電動同軸安裝、電機交錯安裝。電機同軸安裝的安裝原理為:將驅(qū)動電機同減速器的輸出設(shè)備進行連接。此種布局構(gòu)成簡易，需要的設(shè)計需求較低，然而對于驅(qū)動模塊的橫向尺寸的標(biāo)準(zhǔn)卻比較高，比較適合應(yīng)用在整體尺寸需求較低的AGV 中。電機交錯安裝是將驅(qū)動電機和減速器相連接后，借助傳動機構(gòu)同驅(qū)動軸相連。這種布局方式較同軸安裝方式更能夠?qū)崿F(xiàn)兩級減速的效果。該種方式對減速器具有較低的需求標(biāo)準(zhǔn)，并且驅(qū)動模塊的橫向尺寸不大，整體構(gòu)架顯得更加緊湊，驅(qū)動輪同軸安裝的方式，更加適用于橫向尺寸要求較高的AGV中。

2.3 SLAM慣導(dǎo)系統(tǒng)的姿態(tài)解算

姿態(tài)解算的作用是負責(zé)姿態(tài)矩陣的實時更新。姿態(tài)解算所得的結(jié)果可以作為速度、位置的解算所需依據(jù)，所以姿態(tài)解算的結(jié)果準(zhǔn)確度在整個慣導(dǎo)系統(tǒng)解算中很重要。普遍的慣導(dǎo)系統(tǒng)的姿態(tài)解算的方法包括歐拉角法、四元數(shù)法、旋轉(zhuǎn)矢量法等。歐拉角法的解算原理是借助歐拉角微分方程執(zhí)行相關(guān)計算過程，進而能夠直接得到運算結(jié)果。但是，由于歐拉角微分方程包含了三角函數(shù)分式，因此在計算時容易出現(xiàn)零分現(xiàn)象。姿態(tài)解算中經(jīng)常運用四元數(shù)法，即將從b系到n系旋轉(zhuǎn)的角速度用Wnb表示。

由此可知，能夠借助一個四元數(shù)Q對b系相對于n系的旋轉(zhuǎn)進行表示，那么這個四元數(shù)的微分方程可以表示為：

式中，Q=q0+q1iq2j+q3k，上式用矩陣可表示為：

四元數(shù)的實部由q表示，3 個虛部由i、j、k表示，與空間三維坐標(biāo)系的3 個方向分別對應(yīng)。四元數(shù)是對剛體定點轉(zhuǎn)動運動描述的一種形式。Wnb代表b系到n系旋轉(zhuǎn)的角速度，Q代表著四元數(shù)的微分。

在針對四元微分方程的求解運算中，若獲取到每一個時刻的Wnb值，則可以計算出q0、q1、q2、q3的值，之后利用相關(guān)關(guān)系即可完成載體姿態(tài)的運算過程，分析出載體姿態(tài)的結(jié)果。

3 實 驗

為驗證基于AGV 的復(fù)雜場景視覺SLAM 慣導(dǎo)系統(tǒng)設(shè)計合理性進行了實驗驗證分析。

3.1 實驗環(huán)境及條件設(shè)置

實驗環(huán)境分為動態(tài)環(huán)境和靜態(tài)環(huán)境。在動態(tài)環(huán)境中，AGV 系統(tǒng)是沿著鋪設(shè)標(biāo)志的路徑進行勻速直線運動，那么當(dāng)AGV 小車停止運動時，通過輸出系統(tǒng)的定位信息來獲得靜態(tài)定位效果。由于試驗中地面打標(biāo)精度有限，且初始定位姿態(tài)的設(shè)定也包含一定的誤差范圍，因此很難獲得定位數(shù)據(jù)的絕對誤差。因此，測試實驗中的變化量僅限于攝像機是否嵌入。將文中方法與文獻[1]和文獻[2]方法進行對比，其中，文中對攝像機引入前后定位數(shù)據(jù)的波動進行了研究和分析，得出了AGV 定位的比較效果。

在AGV 的運行過程中，同時記錄攝像機引入前后的定位數(shù)據(jù)信息，然后將獲取的信息導(dǎo)入Matlab，利用斜線進行動態(tài)定位的數(shù)據(jù)擬合。將擬合結(jié)果作為動態(tài)測量值的期望值，將靜態(tài)定位數(shù)據(jù)的平均值作為靜態(tài)測量值的期望值，然后進行測量，并將期望值繪制在圖形中。需要注意的是，圖像中的期望曲線是在攝像機融合后，即嵌入攝像機的定位數(shù)據(jù)擬合后形成的。在沒有嵌入式攝像機的環(huán)境中，期望曲線與嵌入后的曲線基本吻合，因此在圖像中沒有顯示。

3.2 實驗結(jié)果與分析

數(shù)據(jù)波動曲線分為兩部分。前50 組數(shù)據(jù)顯示的是AGV 在恒速和靜止?fàn)顟B(tài)下的定位數(shù)據(jù)，波動圖像如圖4 所示；后50 組數(shù)據(jù)是AGV 在勻速直線運動時的定位數(shù)據(jù)波動結(jié)果，對比如圖5 所示。

圖4 靜態(tài)環(huán)境下實驗對比結(jié)果

由圖4 和圖5 可得出實驗結(jié)論：

圖5 動態(tài)環(huán)境下實驗對比結(jié)果

1）靜態(tài)環(huán)境下

為確保觀測更加明顯、清晰，在繪制圖像時將融合攝像頭后的X軸坐標(biāo)變化曲線沿Y軸向上平移了0.02 m 的距離，把期望曲線沿Y軸向上平移0.04 m 的距離。

2）動態(tài)環(huán)境下

在運動環(huán)境下，沒有融合攝像頭傳統(tǒng)方式的定位結(jié)果數(shù)據(jù)波動比較大，融合了攝像頭的基于AGV的復(fù)雜環(huán)境SLAM 算法系統(tǒng)的定位數(shù)據(jù)結(jié)果波動比較小，定位準(zhǔn)確度很高。

實驗顯示，無論在靜態(tài)環(huán)境中還是在運動環(huán)境下，嵌入攝像頭的基于AGV 的復(fù)雜環(huán)境SLAM 慣導(dǎo)系統(tǒng)的定位數(shù)據(jù)波動程度均小于未嵌入攝像頭的傳統(tǒng)AGV 運動系統(tǒng)，即前者的定位準(zhǔn)確性明顯更高。

4 結(jié)束語

當(dāng)運動載體的運動速率超過一定范圍時，易使AGV 系統(tǒng)的魯棒性和精度顯著降低，而基于AGV 的復(fù)雜環(huán)境的SLAM 慣導(dǎo)系統(tǒng)可以在不受外界干擾的情況下，借助于高頻的角速度和加速度特征，彌補視覺傳感器的不足，此外，在獲得穩(wěn)定的觀測數(shù)據(jù)的同時，還能有效地校正數(shù)據(jù)的波動，因此，基于AGV 的復(fù)雜環(huán)境的慣導(dǎo)系統(tǒng)的設(shè)計具有很大的市場需求和發(fā)展前景。

除此之外，在進一步探討研究基于AGV 的復(fù)雜慣導(dǎo)系統(tǒng)時，切記要將光照、動態(tài)障礙物遮擋、噪聲影響等外界因素納入考慮之中，針對以上問題，可以嘗試把慣導(dǎo)引入SLAM 系統(tǒng)中，通過這種方法將原系統(tǒng)前端出現(xiàn)的諸多問題加以改善與解決，盡量避免它們對運行效果產(chǎn)生影響，從而進一步提高該系統(tǒng)的魯棒性以及準(zhǔn)確度。