[發(fā)明專利]面向大規(guī)模SLAM的多傳感器融合與數(shù)據(jù)管理方法

有效

• 申請?zhí)枺?/td>: 201911021085.0
• 申請日: 2019-10-25
• 公開（公告）號: CN110726413B
• 公開（公告）日: 2021-03-23
• 發(fā)明（設(shè)計(jì)）人: 楊紹武;楊文婧;吳慧超;陳伯韜
• 申請（專利權(quán)）人: 中國人民解放軍國防科技大學(xué)
• 主分類號: G01C21/28
• 分類號: G01C21/28
• 代理公司: 國防科技大學(xué)專利服務(wù)中心 43202
• 代理人: 王文惠

1.面向大規(guī)模SLAM的多傳感器融合與數(shù)據(jù)管理方法，其特征在于包括以下步驟：

第一步：每臺機(jī)器人獨(dú)立運(yùn)行基于VINS-Mono改進(jìn)的SLAM框架；

改進(jìn)的具體步驟如下：

1.1測量處理前端，保留基于視覺的檢測跟蹤、IMU預(yù)積分，并新增里程計(jì)原始數(shù)據(jù)接收與緩存，增加三種傳感器信息的數(shù)據(jù)對齊處理；

1.1.1接收圖像，提取Harris角點(diǎn)，利用金字塔光流跟蹤相鄰幀，通過RANSAC去除異常點(diǎn)，進(jìn)一步計(jì)算得到相機(jī)歸一化平面下的特征點(diǎn)云，然后緩存特征點(diǎn)云數(shù)據(jù)；

1.1.2接收緩存IMU的數(shù)據(jù)，并進(jìn)行預(yù)積分；

1.1.3接收緩存里程計(jì)數(shù)據(jù)；

1.1.4完成特征點(diǎn)云、IMU、里程計(jì)的數(shù)據(jù)對齊，保證相鄰的特征點(diǎn)數(shù)據(jù)幀之間有完整的IMU數(shù)據(jù)，且每幀特征點(diǎn)數(shù)據(jù)幀能對應(yīng)一條時間戳臨近的里程計(jì)的數(shù)據(jù)，兩者時間戳之差的絕對值不超過預(yù)先設(shè)定的閾值detal_t；

1.2非線性圖優(yōu)化，保留原有的視覺信息、IMU預(yù)積分信息，并新增里程計(jì)信息，通過三種信息聯(lián)合優(yōu)化求解滑窗內(nèi)的狀態(tài)；

1.2.1首先構(gòu)建一個容量為m的滑動窗口，滑動窗口的每個單元保留原有的視覺信息、IMU預(yù)積分信息，并增加對一條步驟1.1.4所述的數(shù)據(jù)對齊的里程計(jì)信息；

1.2.2基于滑動窗口的第t-1、第t個單元中的里程計(jì)信息，計(jì)算機(jī)器人自滑窗內(nèi)第t-1至第t單元對應(yīng)的時間段的航偏角，記為odom_yaw，與滑窗中第t個單元的IMU預(yù)積分信息中的航偏角imu_yaw比較，若兩者之差的絕對值在預(yù)先設(shè)定的閾值detal_yaw的范圍內(nèi)，則認(rèn)為此時里程計(jì)沒有打滑，數(shù)據(jù)可信度較高；

1.2.3對滑動窗口中的數(shù)據(jù)進(jìn)行非線性優(yōu)化，保留原有的IMU約束和視覺約束，新增里程計(jì)信息的約束，主要步驟為：遍歷滑動窗口中的所有單元，計(jì)算其中相鄰兩個單元[t-1,t]內(nèi)基于里程計(jì)信息計(jì)算的里程，記為odom_dist，其計(jì)算過程為odom_dist＝[(x_t-x_t-1)²+(y_t-y_t-1)²]^1/2，式中，x_t和y_t分別為滑動窗口中t單元中里程計(jì)記錄的x軸方向位移、y方向位移，同理，x_t-1和y_t-1分別為滑動窗口中t-1單元中里程計(jì)記錄的x軸方向位移、y方向位移，其中1≤t；

1.3閉環(huán)檢測，改進(jìn)后的具體步驟如下：

1.3.1以機(jī)器人SLAM建圖起點(diǎn)為地圖原點(diǎn)，構(gòu)建邊長為d的柵格地圖；若將柵格的左下角點(diǎn)坐標(biāo)記為(x0,y0)，則該柵格包含的地圖范圍為{(x,y)|x0x＝x0+d,y0yy0+d}；以每個柵格為單元，將位于其柵格范圍內(nèi)的關(guān)鍵幀數(shù)據(jù)全部編碼，并添加該柵格中心點(diǎn)[(x0+d/2),(y0+d/2)]記為index，以一柵格一條的形式打包存入輕量級數(shù)據(jù)庫，只在內(nèi)存中維護(hù)閉環(huán)檢測所需的柵格范圍內(nèi)的地圖資源；

1.3.2實(shí)時檢測與機(jī)器人當(dāng)前位置距離最近的柵格，確定柵格中心點(diǎn)，并從數(shù)據(jù)庫中以柵格中心點(diǎn)index為檢索條件，取回全部地圖數(shù)據(jù)進(jìn)行解碼，實(shí)現(xiàn)數(shù)據(jù)的一級檢索；

1.3.3在該柵格的地圖數(shù)據(jù)中，通過對機(jī)器人當(dāng)前位置坐標(biāo)與歷史關(guān)鍵幀坐標(biāo)進(jìn)行比對，找到與機(jī)器人當(dāng)前位置最鄰近的所有歷史關(guān)鍵幀，并放入閉環(huán)檢測關(guān)鍵幀比對庫供其使用，實(shí)現(xiàn)數(shù)據(jù)的二級檢索；

第二步：每臺機(jī)器人關(guān)鍵幀數(shù)據(jù)管理模塊，將抽樣篩選過的關(guān)鍵幀或更新的位姿圖發(fā)送給服務(wù)器；

第三步：服務(wù)器首先為每個VINS-Mono客戶端建立一個帶編號的關(guān)鍵幀數(shù)據(jù)隊(duì)列，并將接收到的數(shù)據(jù)暫時統(tǒng)一存儲在局部位姿數(shù)據(jù)緩沖池中；同時，服務(wù)器的檢測線程開啟工作，實(shí)時處理緩沖池中的關(guān)鍵幀數(shù)據(jù)，保證數(shù)據(jù)不在內(nèi)存中冗余；

第四步：服務(wù)器端使用基于特征提取與特征匹配的方法，將每個客戶端關(guān)鍵幀與基準(zhǔn)客戶端關(guān)鍵幀存儲池逐一進(jìn)行特征匹配，若檢測到超過25個匹配點(diǎn)則進(jìn)行關(guān)鍵幀位姿換算，并利用獲得的坐標(biāo)轉(zhuǎn)換關(guān)系進(jìn)行局部位姿圖間的坐標(biāo)融合，形成全局位姿圖；

第五步：服務(wù)器初次計(jì)算得出的機(jī)器人間的位置轉(zhuǎn)換關(guān)系會被存儲記錄，以便系統(tǒng)對整個機(jī)器人群體的位置掌握和行為調(diào)控；

第六步：隨著客戶端探索環(huán)境過程的繼續(xù)，會不斷產(chǎn)生新的位姿圖或更新已有的位姿轉(zhuǎn)換關(guān)系，因此客戶端會不斷更新在服務(wù)器中的局部位姿數(shù)據(jù)緩沖列表，并利用三維仿真器對全局位姿圖進(jìn)行實(shí)時更新顯示。