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

龍圖騰網獲悉山東科技大學;青島卓信通智能科技有限公司申請的專利一種動態場景下基于全局信息的多源緊耦合SLAM方法獲國家發明授權專利權，本發明授權專利權由國家知識產權局授予，授權公告號為：CN120160614B 。

龍圖騰網通過國家知識產權局官網在2025-08-26發布的發明授權授權公告中獲悉：該發明授權的專利申請號/專利號為：202510609372.2，技術領域涉及：G01C21/00；該發明授權一種動態場景下基于全局信息的多源緊耦合SLAM方法是由王海霞;陳鵬宇;盧曉;梁慧斌;張華宇;張治國;孫巧巧;劉芮東;王倩設計研發完成，并于2025-05-13向國家知識產權局提交的專利申請。

本一種動態場景下基于全局信息的多源緊耦合SLAM方法在說明書摘要公布了：本發明公開了一種動態場景下基于全局信息的多源緊耦合SLAM方法，屬于移動機器人領域。本發明通過多源傳感器數據融合與算法優化實現高效定位與建圖。首先，結合雙目相機、單線激光雷達、IMU及里程計數據，利用點云濾波、動態目標檢測和運動畸變校正技術，消除無效點云以及數據畸變，并融合相機與激光雷達觀測數據；其次，通過無跡卡爾曼濾波UKF融合IMU與里程計數據，輸出高精度初始位姿估計；最后，改進Gmapping算法，采用子圖融合與廣度優先搜索策略優化柵格地圖更新模塊，增強SLAM模塊環境特征保留能力。在動態、復雜場景下該方法具有更好地魯棒性，構建地圖更加符合真實環境，同時能有效減少地圖中偽障礙物特征。

本發明授權一種動態場景下基于全局信息的多源緊耦合SLAM方法在權利要求書中公布了：1.一種動態場景下基于全局信息的多源緊耦合SLAM方法，其特征在于，包括以下步驟： 步驟1：傳感器數據融合處理； 結合雙目相機、激光雷達、IMU及里程計數據，利用點云濾波、動態目標檢測和運動畸變校正技術，消除無效點云以及數據畸變，并融合雙目相機與激光雷達的觀測數據； 步驟2：位姿估計； 通過時空配準將IMU數據統一到里程計坐標系下，然后使用無跡卡爾曼濾波融合里程計與IMU，輸出初始位姿估計數據； 步驟3：SLAM建圖； 改進Gmapping算法，采用子圖融合與廣度優先搜索策略優化柵格地圖更新模塊； 步驟1中，具體包括以下步驟： 步驟1.1：對雙目相機輸出的3D點云進行無效點云濾除，包括地面、墻面及動態障礙物在內的無效點云，并通過動態投影生成2D點云； 步驟1.2：對激光雷達的2D點云進行運動畸變校正；基于里程計和IMU數據計算點云中每個點的平移和旋轉偏移量，生成校正后的激光雷達點云； 步驟1.3：將雙目相機的2D點云與激光雷達的2D點云進行時空配準，融合后轉換為激光數據格式，得到融合激光觀測數據； 步驟3中，具體包括以下步驟： 步驟3.1：將融合激光觀測數據與初始位姿估計數據輸入改進的Gmapping算法中，通過粒子濾波器確定最終位姿； 步驟3.2：采用子圖融合與廣度優先搜索策略更新柵格地圖，生成環境特征準確的全局地圖； 步驟3.2中，具體包括以下步驟： 步驟3.2.1：為每一幀融合激光數據生成子柵格地圖，并根據占據概率規則更新子圖； 步驟3.2.2：將各子圖與全局地圖進行融合，對相同柵格采用加權概率疊加規則； 步驟3.2.3：采用廣度優先搜索對全局地圖中的封閉自由區域填充為未知狀態，消除偽障礙物特征； 步驟1.1中，無效點云濾除包括以下步驟： 步驟1.1.1：通過體素濾波對雙目相機輸出的3D點云進行降采樣，并通過直通濾波濾除激光雷達掃描平面以上的點云以及地面點云，得到點云集合P； 步驟1.1.2：隨機選擇點云集合P＝{pi,i＝1,2…,k}中三個點p1、p2、p3，計算三點所在平面的方程：Ax+By+Cz+D＝0； 步驟1.1.3：依次計算點pixi,yi,zi到平面的距離di： 將di小于設定閾值dmax的點視為平面內點，如果內點數量滿足預設的最小內點數，則使用內點繼續擬合；如果內點數量不足，則取消原始內點標注，重新進行隨機采樣擬合，直至迭代結束，將內點從點云集合P中刪除，實現無效點云濾除； 步驟1.1.4：通過統計濾波濾除剩余點云集合中的離散點，從雙目相機輸出的3D點云中提取出相對獨立的障礙物3D點云信息； 步驟1.1.5：基于yolo目標檢測算法識別動態障礙物，結合雙目視差測距計算目標空間距離，刪除目標識別框內的點云； 步驟1.1.6：將處理完后的點云實時投影至相機坐標系的水平坐標面，即獲得障礙物2D點云； 步驟1.1.5中，具體包括以下步驟： 步驟1.1.5.1：對特定移動目標進行圖像數據集采集與模型訓練； 對特定的移動目標預先進行圖像數據集采集，然后在配置好的yolo環境下訓練模型，以便在雙目相機上通過yolo目標檢測算法進行特定移動目標的識別； 步驟1.1.5.2：根據雙目視差測距原理計算動態目標的空間距離Z； 根據yolo目標檢測算法得到目標在圖像坐標系下的目標識別框的中心像素坐標xp,yp和像素尺寸size_xp、size_yp；根據yolo目標檢測算法識別的移動目標在雙目相機上的成像視差，計算空間距離Z：具體如下： 移動目標中心點P在雙目相機的成像點分別為PL、PR，兩個成像點PL和PR之間的距離為： 其中，b為雙目相機的基線，f為焦距，L是雙目相機光心到成像面的距離的兩倍，xL、xR分別表示點P在左、右攝像頭成像平面上的成像位置相對于成像平面中心的水平偏移量； 根據相似三角形理論，計算出P的空間距離Z： 視差xL-xR通過雙目圖像匹配獲取； 步驟1.1.5.3：將目標識別框的像素坐標轉換為相機坐標系下的三維坐標，并刪除框內點云； 首先根據像素分辨率size_p將目標識別框的中心像素坐標和像素尺寸轉換成圖像坐標系下的實際坐標xtru,ytru和實際尺寸size_xtru、size_ytru 然后分別計算出目標識別框的四個點頂點pi在圖像坐標系下的真實坐標： 假設目標識別框在圖像坐標系下的坐標為pxtru-i,ytru-i，在相機坐標系下的坐標為PXc-i,Yc-i,Zc-i； 根據相似關系，計算目標識別框的四個頂點pi在相機坐標系下的坐標Xc-i,Yc-i,Zc-i： 其中，f為焦距，Zc-i為雙目視差測距提供的目標空間距離Z值，將點云中點坐標x、y在相機坐標系下目標識別框范圍內，Z值小于Zc-i的點刪除，去除特定移動目標點云； 步驟1.2中，通過里程計和IMU數據對激光雷達點云進行畸變校正，包括以下步驟： 步驟1.2.1：基于激光雷達采集周期內里程計與IMU的位姿變化，計算每個激光點的平移偏移量； 假設激光雷達發射第一個激光點的時刻為ts，雷達坐標系為fs，發射最后一個激光點的時刻為te，則里程計和IMU的數據采集范圍應為ts-1至te+1，確保能夠完整計算出激光雷達一個采集周期內的平移和旋轉；假設ts-1時刻的里程計位移和旋轉矩陣為Tstart，te+1時刻的位移和旋轉矩陣為Tend，則從ts-1至te+1輪速計坐標變換矩陣T計算公式為： 其中，R和trs即分別為在ts至te的一個周期內發生的旋轉和位移對應的矩陣； 假設一個采集周期內某一個激光點對應時刻為tp，這時的雷達坐標系為fp，激光點在fp坐標系下的坐標為p，trs-x和trs-y分別為一個采集周期內X方向和Y方向的總偏移量，則p相對于第一個激光點在X和Y方向的偏移量posX、posY如下： 步驟1.2.2：根據IMU角速度積分，計算繞Z軸的旋轉偏移量； 假設發射第一個采集點時激光雷達的角度為θs，采集周期內第k幀IMU數據相對于第一幀IMU數據對應時刻轉動的角度值為θk，ωτ是時刻t角速度，則θk的計算公式為： 設θfront、θback分別為tp前一時刻和后一時刻的IMU數據相對第一幀IMU數據的旋轉角度，則p相對于第一個數據點繞Z軸的旋轉偏移量rotZ為： 步驟1.2.3：通過坐標變換矩陣Tcur對每個激光點進行校正； 假設第一個采集點的變換矩陣為Ts，校正后的點為pcorr，根據每個點相對于第一個點的平移和旋轉偏移量，確定當前點p的坐標系fp到第一個采集點的坐標系fs之間的坐標變換矩陣Tcur，計算出點p校正后的坐標pcorr為： pcorr＝Ts -1×Tcur×p； 激光雷達每完成一次數據采集，分別對點云中的每一個點執行步驟1.2.1、步驟1.2.2操作，確定每一個點相對于第一個點的平移和旋轉偏移量，然后根據步驟1.2.3所述公式進行坐標變換后，即得到運動畸變校正后的激光雷達點云； 步驟1.3中，包括以下步驟： 步驟1.3.1：通過Autoware標定工具獲取雙目相機與激光雷達的外參矩陣R,T，將雙目相機2D點云轉換至激光雷達坐標系；具體如下： 在已知相機內參條件下，通過Autoware中的標定工具，聯合標定雙目相機與激光雷達，獲取二者坐標系外參矩陣，設R為旋轉矩陣，T為平移矩陣，pc為經過步驟1.1處理后的雙目相機2D點云坐標，pcl為雙目相機2D點云在激光雷達坐標系下的坐標，根據下式進行雙目相機2D點云與激光雷達2D點云的空間配準，得到雙目相機2D點云在激光雷達坐標系下的坐標； 步驟1.3.2：將時間戳對齊的雙目相機2D點云與激光雷達2D點云進行并集融合，并通過開源工具轉換為激光數據格式；具體如下： 假設進行畸變校正后的激光雷達2D點云集為A，通過空間配準后的激光雷達坐標系下的雙目相機2D點云集合為B，將時間戳對齊的A和B通過并集操作進行融合，即得到融合點云數據；然后將融合點云數據轉換為激光數據格式。

如需購買、轉讓、實施、許可或投資類似專利技術，可聯系本專利的申請人或專利權人山東科技大學;青島卓信通智能科技有限公司，其通訊地址為：266590 山東省青島市黃島區前灣港路579號；或者聯系龍圖騰網官方客服，聯系龍圖騰網可撥打電話0551-65771310或微信搜索“龍圖騰網”。