買專利賣專利找龍圖騰，真高效！ 查專利查商標用IPTOP,全免費！專利年費監控用IP管家,真方便！

龍圖騰網恭喜浙江工業大學申請的專利一種基于激光與視覺異步融合的SLAM算法獲國家發明授權專利權，本發明授權專利權由國家知識產權局授予，授權公告號為：CN114742874B 。

龍圖騰網通過國家知識產權局官網在2025-04-29發布的發明授權授權公告中獲悉：該發明授權的專利申請號/專利號為：202210313473.1，技術領域涉及：G06T7/579；該發明授權一種基于激光與視覺異步融合的SLAM算法是由邢科新;張興盛;董為;李偉騰;慈雯祺;蒲茜設計研發完成，并于2022-03-28向國家知識產權局提交的專利申請。

本一種基于激光與視覺異步融合的SLAM算法在說明書摘要公布了：本發明公開了一種基于激光與視覺異步融合的SLAM算法，首先將傳感器的數據按較快的傳感器采樣頻率劃分為m個采樣區間，每個采樣區間內會存在兩種情況：1只擁有相機的位姿估計；2同時擁有來自激光傳感器和視覺傳感器的位姿估計。結合Kalman濾波器及多傳感器異步融合的方法，得到了改進后的基于激光與視覺異步融合位姿估計算法。為了有效利用每個傳感器不同的特性，根據機器人的運行狀態，引入基于角度的加權系數，提高測量的置信度。該方法可以更好地利用視覺傳感器中的冗余數據，提高算法的跟蹤精度。同時，將視覺傳感器姿態估計作為激光傳感器姿態估計的先驗，減少迭代次數，提高算法的效率。

本發明授權一種基于激光與視覺異步融合的SLAM算法在權利要求書中公布了：1.一種基于激光與視覺異步融合的SLAM算法，其特征在于，包括以下步驟：步驟1對于由視覺傳感器反饋得到的位姿增量，根據相機小孔成像原理以及剛體變換公式，假設空間點P在世界坐標系下的齊次坐標為P＝XW,YW,ZW,1T；則其在相機的像素坐標系下的坐標為： 其中u,v為點p在像素坐標系下的坐標值；s為點p在相機坐標系下的深度值；K為相機的內參矩陣；R為旋轉矩陣；t為平移矩陣；步驟2通過ORB特征提取算法得到當前幀中圖像的特征點，將rBRIEF描述符得到的當前幀中特征點的描述符與上一時刻圖像中的特征點進行描述符之間的匹配，建立對應的匹配關系；將上一時刻的匹配點通過相機的內參矩陣以及初始位姿投影到當前幀圖像中；由于相機的位姿通常存在偏差，所以投影點與當前地圖上的匹配點之間通常會存在偏差；假設特征點P在當前幀圖像中的坐標為P＝u1,v1T，對應上一幀中的匹配點在當前幀中的匹配點坐標為P’＝u2,v2T；通常由于相機位姿的偏差導致這兩個點的坐標并不相同，因此定義誤差函數： 其中e是誤差值，u,v是當前幀中特征點的像素坐標，P是當前幀特征點對應的匹配點；將其改寫為李代數形式： 其中expδ為相機坐標系的李代數表達式，e為空間點P投影到當前圖像中與匹配點之間的投影誤差；通常一幀圖像中會存在若干個匹配點，根據所有的匹配點得到投影誤差函數： 其中ui表示當前幀中第i個特征點；Pi表示其對應的匹配點的世界坐標；si表示該點的深度；K表示相機的內參矩陣；expδ表示相機坐標系的李代數表達式；步驟3利用高斯牛頓法或者列文伯格-馬夸爾特方法求解公式4，得到位姿估計為： 其中z1k1為視覺傳感器在k1時刻的最優估計，R為旋轉矩陣，t為平移矩陣；v1k1為相機估計噪聲且假設服從高斯分布；步驟4假設k2時刻得到的激光傳感器數據為令Siδ2為世界坐標，MSiδ2為坐標點Siδ2的占用概率，為MSiδ2的導數；MSiδ2與的值可通過對世界坐標Siδ2進行雙線性插值求得；那么機器人從上一時刻到當前時刻的位姿增量可由下式給出： 其中 步驟5根據位姿增量得到機器人在激光傳感器位姿估計下的坐標為：z2k2＝z2k2-1+Δδ2+v2k28其中z2k2為激光傳感器在k2時刻的最優位姿估計；z2k2-1為其在上一時刻的最優位姿估計；v2k2為觀測噪聲且假設服從高斯分布，滿足如下關系： 其中viki，vjTkj為測量噪聲，E代表均值，δij，為克羅內克函數，Riki為k時刻來自視覺或者激光傳感器的協方差矩陣；步驟6采用了異步融合方式對來自兩個傳感器的數據進行融合；首先將傳感器的數據按較快的傳感器采樣頻率劃分為m個采樣區間，每個采樣區間內會存在兩種情況：1只擁有相機的位姿估計；2同時擁有來自激光傳感器和視覺傳感器的位姿估計；結合Kalman濾波器及多傳感器異步融合的方法，得到了改進后的基于激光與視覺異步融合位姿估計算法如下： 其中為根據上一時刻機器人位姿的最優估計得到的預測位姿；P0k|k為預測位姿的協方差矩陣；ziki為k時刻來自視覺或者激光傳感器的最優位姿估計；Rik為k時刻來自視覺或者激光傳感器的協方差矩陣，下標i代表傳感器數據的個數；當采樣區間內只有相機的數據時，i＝1，此時式10變為： 此時估計模型轉變為以較快的傳感器采樣頻率下對單個傳感器數據進行位姿估計的Kalman濾波器；當采樣區間內同時存在相機的數據以及激光傳感器的位姿估計結果時，此時式10變為式12、13的形式： 此時估計模型變為以較慢的傳感器采樣速率下對激光傳感器與視覺傳感器進行融合估計的Kalman濾波算法；算法以上一時刻的最優估計作為預測量，首先融合當前時刻傳感器一估計的位姿，然后再融合傳感器二估計的位姿結果，最終以兩次融合后的傳感器估計值作為最終位姿估計的結果；步驟7為了有效利用每個傳感器不同的特性，引入置信度概念；使用置信度w表示對兩個傳感器位姿估計的置信度，將式10改為： 其中wi代表第i個傳感器的位姿估計置信度；步驟8傳感器的特性通常與機器人運動過程相關，因此wi應與機器人的運動狀態相關；當機器人運動沿直線行駛時，算法增大對視覺的置信度；當機器人出現旋轉時，算法增大對激光的置信度；每次融合時算法會根據激光傳感器最新的位姿估計判斷機器人當前運動過程中是否發生旋轉；其由式6得到，這里記為Δα2；將角度變化量代入wi得到： 其中Δα2為上一時刻到當前時刻估計的機器人旋轉角度；根據當前機器人運行效果，每個采樣周期內機器人旋轉角度不超過0.25rad，因此式15修改為： 將式16代入式14得到：

如需購買、轉讓、實施、許可或投資類似專利技術，可聯系本專利的申請人或專利權人浙江工業大學，其通訊地址為：310014 浙江省杭州市拱墅區潮王路18號；或者聯系龍圖騰網官方客服，聯系龍圖騰網可撥打電話0551-65771310或微信搜索“龍圖騰網”。