買專利賣專利找龍圖騰，真高效！ 查專利查商標(biāo)用IPTOP,全免費(fèi)！專利年費(fèi)監(jiān)控用IP管家,真方便！

龍圖騰網(wǎng)恭喜河海大學(xué)申請(qǐng)的專利基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法獲國(guó)家發(fā)明授權(quán)專利權(quán)，本發(fā)明授權(quán)專利權(quán)由國(guó)家知識(shí)產(chǎn)權(quán)局授予，授權(quán)公告號(hào)為：CN119687904B 。

龍圖騰網(wǎng)通過(guò)國(guó)家知識(shí)產(chǎn)權(quán)局官網(wǎng)在2025-05-09發(fā)布的發(fā)明授權(quán)授權(quán)公告中獲悉：該發(fā)明授權(quán)的專利申請(qǐng)?zhí)?專利號(hào)為：202510205752.X，技術(shù)領(lǐng)域涉及：G01C21/00；該發(fā)明授權(quán)基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法是由黃浩乾;張未;石瑞峰;王迪;王俊瑋設(shè)計(jì)研發(fā)完成，并于2025-02-25向國(guó)家知識(shí)產(chǎn)權(quán)局提交的專利申請(qǐng)。

本基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法在說(shuō)明書(shū)摘要公布了：本發(fā)明公開(kāi)了基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法。首先獲取AUV所配置傳感器的數(shù)據(jù)；處理得到的傳感器的數(shù)據(jù)，初始化系統(tǒng)參數(shù)，進(jìn)行局部地圖的跟蹤，在基于ORB?SLAM3關(guān)鍵幀選擇的基礎(chǔ)上，根據(jù)IMU的觀測(cè)信息判斷當(dāng)前是否為動(dòng)態(tài)環(huán)境，AUV是否劇烈運(yùn)動(dòng)，再繼續(xù)進(jìn)行關(guān)鍵幀的選擇；對(duì)選擇的關(guān)鍵幀的姿態(tài)進(jìn)行優(yōu)化，使用雙約束進(jìn)行姿態(tài)校正，恢復(fù)大視角快速?gòu)澢\(yùn)動(dòng)下的局部軌跡，生成新的地圖點(diǎn)；進(jìn)行閉環(huán)檢測(cè)與校正；生成全局軌跡與地圖。本發(fā)明解決了自主水下航行器可能遇到動(dòng)態(tài)環(huán)境或者本身運(yùn)動(dòng)劇烈導(dǎo)致位姿漂移的問(wèn)題，同時(shí)使得視覺(jué)慣性同步定位與地圖構(gòu)建系統(tǒng)的準(zhǔn)確性和魯棒性大幅提高。

本發(fā)明授權(quán)基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法在權(quán)利要求書(shū)中公布了：1.一種基于改進(jìn)關(guān)鍵幀決策及姿態(tài)優(yōu)化的水下定位與建圖方法，其特征在于，包括以下步驟：S1.獲取AUV所配置傳感器的數(shù)據(jù)；S2.處理步驟S1得到的傳感器的數(shù)據(jù)，初始化系統(tǒng)參數(shù)，進(jìn)行局部地圖的跟蹤，在基于ORB-SLAM3關(guān)鍵幀選擇的基礎(chǔ)上，根據(jù)IMU的觀測(cè)信息判斷當(dāng)前是否為動(dòng)態(tài)環(huán)境，AUV是否劇烈運(yùn)動(dòng)，再繼續(xù)進(jìn)行關(guān)鍵幀的選擇，具體包括：S21.對(duì)步驟S1中獲得的圖像幀進(jìn)行特征提取和匹配：使用SIFT算法進(jìn)行特征點(diǎn)提取，利用暴力匹配算法將幀與幀之間的特征進(jìn)行匹配，建立特征對(duì)應(yīng)關(guān)系；S22.對(duì)步驟S1中獲得的IMU數(shù)據(jù)進(jìn)行積分；S23.通過(guò)步驟S21匹配的特征點(diǎn)和步驟S22積分后的IMU數(shù)據(jù)，計(jì)算使用最小化重投影誤差來(lái)優(yōu)化相機(jī)位姿，實(shí)現(xiàn)局部地圖跟蹤；其中，重投影誤差ei計(jì)算如式4所示：ei＝‖xi-πRtXi+tt‖24其中，xi是第i個(gè)特征點(diǎn)在圖像上的觀測(cè)位置；π是相機(jī)的投影函數(shù)，將三維點(diǎn)映射到二維圖像平面；Rt是t時(shí)刻的相機(jī)旋轉(zhuǎn)矩陣；tt是t時(shí)刻的相機(jī)平移向量；Xi是第i個(gè)特征點(diǎn)在世界坐標(biāo)系中的位置；‖·‖2表示歐幾里得范數(shù)，計(jì)算實(shí)際觀測(cè)點(diǎn)和重投影點(diǎn)之間的距離；S24.基于ORB-SLAM3算法選擇關(guān)鍵幀：設(shè)定一個(gè)閾值E1，判斷當(dāng)前幀中特征點(diǎn)的重投影誤差ei是否超過(guò)閾值E1，如果超過(guò)，則說(shuō)明當(dāng)前幀提供了新的視覺(jué)信息，將其作為主關(guān)鍵幀，用于提供視覺(jué)信息；S25.基于IMU觀測(cè)信息進(jìn)行關(guān)鍵幀決策：步驟S1中獲得的IMU觀測(cè)數(shù)據(jù)中加速度的均方根角速度的均方根其中t指時(shí)間，b表示第b個(gè)采樣時(shí)刻，N表示取樣的數(shù)據(jù)點(diǎn)的總數(shù)，如果RMS1T1或者RMS2T2，其中T1為設(shè)定的加速度均方根的閾值，T2為設(shè)定的角速度均方根的閾值，則按照新的閾值E2選擇新的關(guān)鍵幀，將其作為副關(guān)鍵幀，用于提供慣性信息，E2如式5所示：E2＝E1-m·RMS1+RMS25其中，m是設(shè)定的調(diào)節(jié)系數(shù)；S3.對(duì)步驟S2最后選擇的關(guān)鍵幀的姿態(tài)進(jìn)行優(yōu)化，使用雙約束進(jìn)行姿態(tài)校正，恢復(fù)大視角快速?gòu)澢\(yùn)動(dòng)下的局部軌跡，生成新的地圖點(diǎn)，具體包括：S31.建立步驟S24中的主關(guān)鍵幀與步驟S25中的副關(guān)鍵幀之間的雙測(cè)量約束：首先主關(guān)鍵幀通過(guò)誤差ri建立視覺(jué)約束，ri計(jì)算如式6所示：ri＝‖xi-πRkXi+tk‖26其中，Rk和tk分別是主關(guān)鍵幀k的旋轉(zhuǎn)矩陣和平移向量；然后副關(guān)鍵幀通過(guò)IMU預(yù)積分描述兩個(gè)關(guān)鍵幀之間的運(yùn)動(dòng)來(lái)建立慣性約束，IMU預(yù)積分如式7所示： 其中，Ck,k-1是相機(jī)姿態(tài)變化的協(xié)方差矩陣，Rk-1和tk-1分別是副關(guān)鍵幀k-1的旋轉(zhuǎn)矩陣和平移向量，是Rk-1的轉(zhuǎn)置矩陣，pk和pk-1分別是主關(guān)鍵幀k和副關(guān)鍵幀k-1的位置向量；S32.設(shè)立優(yōu)化目標(biāo)，通過(guò)最小化目標(biāo)函數(shù)來(lái)優(yōu)化相機(jī)位姿，將目標(biāo)函數(shù)設(shè)置為其中，第一項(xiàng)指的是視覺(jué)誤差，第二項(xiàng)指的是IMU預(yù)積分誤差；S33.通過(guò)滑模濾波算法來(lái)處理快速大曲率運(yùn)動(dòng)情況，優(yōu)化并恢復(fù)曲線運(yùn)動(dòng)下的軌跡，首先，將相機(jī)和IMU系統(tǒng)建模為狀態(tài)空間模型，如式8所示： 其中，xk、xk+1是系統(tǒng)狀態(tài)向量，uk是IMU測(cè)量，yk是觀測(cè)數(shù)據(jù)，ωk和vk分別是過(guò)程噪聲和觀測(cè)噪聲，F(xiàn)k是狀態(tài)轉(zhuǎn)移矩陣，Bk是控制輸入矩陣，Hk是觀測(cè)矩陣；接著設(shè)計(jì)滑模面σk用于引導(dǎo)估計(jì)狀態(tài)xk收斂到期望值，σk如式9所示：σk＝Ckxk-dk9其中，Ck是滑模面設(shè)計(jì)矩陣，dk是期望值；然后選擇滑?？刂坡蓏k，使得估計(jì)的狀態(tài)xk在滑模面上保持穩(wěn)定，zk如式10所示：zk＝-Kksgnσk10其中，Kk是控制增益矩陣，sgn·是符號(hào)函數(shù)；最后設(shè)計(jì)滑模濾波器，將滑模控制律應(yīng)用于狀態(tài)更新，如式11所示： 滑模濾波算法通過(guò)優(yōu)化相機(jī)的位姿和IMU的狀態(tài)來(lái)處理快速大曲率運(yùn)動(dòng)中的動(dòng)態(tài)變化，恢復(fù)局部曲線運(yùn)動(dòng)；S34.根據(jù)優(yōu)化后的相機(jī)位姿，生成新的地圖點(diǎn)；S4.進(jìn)行閉環(huán)檢測(cè)與校正；S5.生成全局軌跡與地圖。

如需購(gòu)買、轉(zhuǎn)讓、實(shí)施、許可或投資類似專利技術(shù)，可聯(lián)系本專利的申請(qǐng)人或?qū)＠麢?quán)人河海大學(xué)，其通訊地址為：210024 江蘇省南京市鼓樓區(qū)西康路1號(hào)；或者聯(lián)系龍圖騰網(wǎng)官方客服，聯(lián)系龍圖騰網(wǎng)可撥打電話0551-65771310或微信搜索“龍圖騰網(wǎng)”。