基于AGV 的復雜場景視覺SLAM 慣導系統設計

馮悅鳴，呂 勤，李 巖

（國網浙江省電力有限公司嘉興供電公司，浙江嘉興 314033）

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

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

1 硬件結構設計

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

圖1 硬件總體構架

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

1.1 AGV系統

AGV 系統的控制借助物流調控上位系統、AGV地面控制系統、AGV 車載控制系統共同協作[10-13]。在AGV 系統中，導航定位技術是整個系統的核心，內部構成有終端控制系統、車載控制系統。終端控制系統的主要構成包括對地圖的管理、對任務的調度、對路徑的規劃等；車載控制系統的主要構成有導航系統、定位系統、驅動設備等，工作內容為無線通信的接收以及完成對接收數據信息的交互任務。AGV 總體結構如圖2 所示。

圖2 AGV總體結構

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

1.2 接收器

其工作原理是將接收器固定在后交互式運行形式中，并將發射站固定在AGV 車體上，如工裝房中的墻壁、地面等合適位置。在AGV 車體進行運動的過程中，發射站將保持對空間環境的持續掃描狀態。一旦發出的激光信號被固定接收器接收后，便能夠實時定位解算出發射站與全局導航坐標系相對位置關系、姿態信息。接收器是導航領域的地標位置點，三維坐標需要保證高精確度，并且需要進行統一化保存在導航坐標系之中。

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

1.3 傳感器

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

2 軟件功能設計

2.1 融合人工信標的多目SLAM算法流程

SLAM 系統是非線性優化框架，由前端的位姿跟蹤和后端的全局優化構成。系統的后端以全局閉環檢測為基礎執行針對位姿的優化任務。與傳統的純人工SLAM 算法相比較，基于AGV 的復雜場景視覺SLAM 算法具有更強的光照變化適應性，并且在一定程度上能夠減少人工信標的使用數量，降低了人力、物力、資源的投入量；除此之外，系統的魯棒性和實用性也得到了明顯的提高。SLAM 算法流程分為前端與后端，具體流程如圖3 所示。

圖3 SLAM算法總體流程圖

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

2.2 AGV驅動功能設計

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

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

2.3 SLAM慣導系統的姿態解算

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

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

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

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

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

3 實 驗

為驗證基于AGV 的復雜場景視覺SLAM 慣導系統設計合理性進行了實驗驗證分析。

3.1 實驗環境及條件設置

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

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

3.2 實驗結果與分析

數據波動曲線分為兩部分。前50 組數據顯示的是AGV 在恒速和靜止狀態下的定位數據，波動圖像如圖4 所示；后50 組數據是AGV 在勻速直線運動時的定位數據波動結果，對比如圖5 所示。

圖4 靜態環境下實驗對比結果

由圖4 和圖5 可得出實驗結論：

圖5 動態環境下實驗對比結果

1）靜態環境下

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

2）動態環境下

在運動環境下，沒有融合攝像頭傳統方式的定位結果數據波動比較大，融合了攝像頭的基于AGV的復雜環境SLAM 算法系統的定位數據結果波動比較小，定位準確度很高。

實驗顯示，無論在靜態環境中還是在運動環境下，嵌入攝像頭的基于AGV 的復雜環境SLAM 慣導系統的定位數據波動程度均小于未嵌入攝像頭的傳統AGV 運動系統，即前者的定位準確性明顯更高。

4 結束語

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

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