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

龍圖騰網恭喜東南大學申請的專利一種基于LiDAR/IMU/UWB緊耦合里程計的車輛定位方法獲國家發明授權專利權，本發明授權專利權由國家知識產權局授予，授權公告號為：CN119354208B 。

龍圖騰網通過國家知識產權局官網在2025-04-18發布的發明授權授權公告中獲悉：該發明授權的專利申請號/專利號為：202411463377.0，技術領域涉及：G01C21/28；該發明授權一種基于LiDAR/IMU/UWB緊耦合里程計的車輛定位方法是由徐啟敏;姚鈺歡;趙鑫;李旭設計研發完成，并于2024-10-18向國家知識產權局提交的專利申請。

本一種基于LiDAR/IMU/UWB緊耦合里程計的車輛定位方法在說明書摘要公布了：本發明公開了一種基于LiDARIMUUWB緊耦合里程計技術的車輛定位方法。該方法首先對原始點云數據進行預處理，包括點云畸變去除、地面和非地面點云分割，從而為LiDAR關鍵幀里程計的位姿估計與后續的點云建圖提供了基礎；進而，構建緊耦合模型，包括基于IMU預積分的模型和UWB距離觀測的模型，并進行局部因子圖優化，從而能在城市特征退化環境下為LiDAR關鍵幀里程計提供一個較為準確的位姿初值。最后，對LiDAR關鍵幀進行優化，根據局部優化后的位姿初值將靜態邊緣點云投影到世界坐標系下，并利用“邊緣點—直線”構成的約束來進一步優化當前位姿。本方法在SLAM特征退化環境下具有良好的定位性能，能夠實現更準確的車輛定位。

本發明授權一種基于LiDAR/IMU/UWB緊耦合里程計的車輛定位方法在權利要求書中公布了：1.一種基于LiDARIMUUWB緊耦合里程計的車輛定位方法，其特征在于：首先，對原始點云進行預處理，采用基于前景點分割和強度范圍過濾的動態點云濾除方法；然后，對UWB的觀測值構建觀測模型與IMU和LiDAR構建的運動模型采用因子圖的方式優化；最后，通過構建“邊緣點-直線”的約束來實現LiDAR關鍵幀里程計位姿優化，從而提升特征退化環境下的定位精度；具體步驟包括：步驟一：數據預處理結合pi前后的IMU位姿增量信息，以線性插值的方式來求取TLk,i，其表達式為： 其中,ΔTBk,front表示時間上在當前點云pi之前且距離最近的IMU的增量位姿，ΔTBk,back表示時間上在當前點云pi之后且距離最近的IMU的增量位姿，tfront和tback分別表示其對應時刻，tk,i表示第k幀中當前點云pi所在時刻；引入隨機采樣一致性的方法來分割地面和非地面點云，以便提升點云數據的處理效率，首先從當前幀的點云中任取四個點用于估計地面模型，建立地面模型Ax+By+Cz+S＝0，將三個點云的三維坐標分別代入到平面方程中來確定系數A，B，C，D，則其他點云點到地面的距離表示為： 其中，xi,yi,zi為點云pi的三維坐標；設定距離閾值為d0，若當前點云到平面的距離di小于等于該閾值，則認為pi為“局內點”，遍歷完成后，若當前“局內點”數量大于原有的“地面點”數量，則更新當前“局內點”為“地面點”；重復上述過程，不斷迭代，當最終的“地面點”滿足數量要求或達到最大迭代次數時，停止迭代，標記此時的“地面點”為真正的地面點，其他點則被標記為非地面點；步驟二：緊耦合模型構建1建立IMU預積分模型IMU能輸出三軸加速度和三軸角速度在實際應用中，IMU的測量值會受到高斯白噪聲n和零偏b的影響，其測量模型表達為： 其中，和分別表示IMU在車體坐標系下的角速度和加速度，bg和ba分別表示角速度和加速度的隨機游走，ng和na分別表示角速度和加速度的測量噪聲，g表示重力加速度，表示世界坐標系到車體坐標系的姿態四元數，wB表示角速度真值，aWt表示加速度真值；為了獲得每一時刻的位姿信息，首先需要根據IMU的測量信息建立運動學微分方程，其表達式為： 其中，表示世界坐標系到車體坐標系的姿態四元數的時間導數，表示哈密頓積四元數乘法，表示世界坐標系下速度的時間導數，aW表示世界坐標系下的加速度，表示世界坐標系下的位置的時間導數，vW表示世界坐標系下的速度；結合上述微分方程，得到IMU的運動學更新方程為： 其中，表示j時刻世界坐標系到車體坐標系的姿態四元數，表示j時刻世界坐標系下的速度，表示i時刻世界坐標系下的速度，表示t時刻車體坐標系下的加速度，表示j時刻世界坐標系下的位置，表示i時刻世界坐標系下的位置，表示i時刻世界坐標系下的速度；為保證因子圖模型的實時性，利用點云關鍵幀的位姿作為因子圖的狀態節點，只有到達了關鍵幀所在時刻才對因子圖進行優化；未到達關鍵幀所在時刻時，則對IMU數據進行連續積分；引入IMU預積分模型，其只與IMU輸出的加速度和角速度相關而不依賴于節點狀態，從而避免了重新積分，其表達式為： 其中，表示k時刻車體坐標系Bi下的位置，表示k時刻車體坐標系Bi下的速度，表第k時刻車體坐標系Bi下的姿態四元數，a表示車體坐標系下的加速度，w表示車體坐標系下的角速度，δt表示時間步長；w和a采用中值積分的方法獲得，表達式為： 其中，表示k時刻車體坐標系下的角速度，表示k時刻車體坐標系下的加速度，bg表示角速度隨機游走，ba表示加速度隨機游走；2建立緊耦合里程計模型建立IMU和UWB的殘差函數來約束因子圖中的節點狀態，IMU預積分模型的殘差函數為： 其中，rp表示位置誤差，rq表示姿態誤差，rv表示速度誤差，表示加速度計偏置誤差，表示陀螺儀偏置誤差，表示世界坐標系到車體坐標系Bi的四元數；基于緊耦合的UWB觀測模型以距離信息作為觀測量，其殘差函數為： 其中，表示k時刻由第i個UWB基站提供的距離觀測，hUWBXk,Xi表示k時刻轉換到UWB坐標系下的當前狀態與第i個基站的位置Xi之間的距離函數；各傳感器的觀測噪聲符合零均值的高斯分布，在建立了關于IMU預積分和UWB觀測的殘差函數后，將其轉化為最小二乘模型以進行局部因子圖優化，最小二乘模型表達式為： 其中，X*表示待優化的位姿，fi表示公式19和20中的殘差函數，∑i表示各傳感器數據的協方差；步驟三：關鍵幀優化當移動載體的平移或旋轉超過閾值thTrans和thRot時，將此時的點云幀視作關鍵幀并進行優化處理；若平移或旋轉不超過閾值，則不對點云幀進行優化處理；將k時刻投影到世界系下的邊緣點云標記為進而與局部點云地圖Me做匹配來進一步優化位姿；Me由空間和時間上距離比較近的關鍵幀中的邊緣點云所構成，在空間上，以對應的關鍵幀位置為基準，通過距離閾值thradius來選取世界坐標系下的其他關鍵幀點云，并一起構成Me；在時間上，以的時間戳為基準，將小于時間閾值thtime的關鍵幀也加入到Me中；隨后，對Me中的邊緣點云進行體素下采樣以消除重復點云；在構建好局部地圖Me后，通過KDtree在Me中尋找與的當前點云距離最近的5個點并求取這5個點的均值和協方差，其表達式分別為： 結合主成分分析法，若Ae的最大特征值是次大特征值的3倍以上，則認為這5個點構成了線特征，此時對應的特征向量α1為線特征的方向向量；從線特征中提取兩個具有代表性的“虛擬”點云，分別為： 利用中的當前點云和“虛擬”點云來建立距離殘差方程，其表達式為： 構建k時刻關鍵幀中所有邊緣點云關于其對應的直線特征的殘差方程，并建立最小二乘模型為： 其中，n表示當前關鍵幀中邊緣點云與線特征構成的約束總數；采用高斯牛頓法求解得到優化后的位姿，則幀間LiDAR里程計的約束因子表達為： 其中，表示上一關鍵幀時刻對應的優化后的位姿；此時，關于激光里程計的殘差函數為： 其中，hLiDARXk-1,Xk表示在進行LiDAR里程計優化之前，因子圖中相鄰狀態節點的位姿變化量；在建立關于LiDAR里程計的殘差函數后，將其加入到因子圖優化模型中以實現基于LiDARIMUUWB緊耦合的里程計位姿估計。

如需購買、轉讓、實施、許可或投資類似專利技術，可聯系本專利的申請人或專利權人東南大學，其通訊地址為：211102 江蘇省南京市江寧區東南大學路2號；或者聯系龍圖騰網官方客服，聯系龍圖騰網可撥打電話0551-65771310或微信搜索“龍圖騰網”。