本發明屬于新一代信息，具體是一種實景三維模型增強的無人設備自主定位方法。

背景技術：

1、工業廠區環境復雜、設備眾多，巡檢工作對于工業廠區的安全生產和設備維護至關重要。傳統的巡檢方式主要依靠人工巡檢，但其存在人員風險高、效率低、成本高等問題，同時也難以滿足化工廠區巡檢的高效性和全面性要求。隨著無人設備及定位技術的發展，無人設備巡檢成為了代替人工巡檢的重要手段。而無人設備自主定位導航是無人設備實現自主化、高效巡檢的關鍵技術。對于室外環境，gnss已經十分成熟。但在復雜工業廠區環境下，由于多路徑效應、建筑物的遮擋等因素的影響，gnss無法提供準確的位置服務。為了保障工業廠區工作人員的安全及設備、生產的正常運行，實現廠區巡檢無人設備的高精度自主定位是及其重要的。

2、近些年，隨著實景三維的快速發展和技術成熟，實景三維模型得到了廣泛的應用。實景三維模型通過高精度攝影測量、激光掃描等技術，能夠真實還原工業廠區的復雜環境，生成細節豐富的三維數字模型，為無人設備提供真實環境中的空間參考，并在三維空間中進行自主定位。但實景三維模型數據量巨大，實時匹配和處理速度較慢；單傳感器的激光雷達點云匹配在環境退化、遮擋物多的情況加魯棒性不加，無法實現無人設備高精度定位。

技術實現思路

1、本發明針對背景技術中存在的問題，提出了一種實景三維模型增強的無人設備自主定位方法。

2、技術方案：

3、一種實景三維模型增強的無人設備自主定位方法，在無人設備運動過程中，通過激光雷達掃描獲得無人設備的三維激光點云數據；

4、定位方法包括以下步驟：

5、s1、基于實景三維模型，通過搜索最近鄰關鍵幀和最近鄰凸包來構建關鍵幀子地圖sk；構建所有的關鍵幀點云子地圖集合kk，使得當前幀的激光點云與實景三維模型的重疊增多，加快匹配速度；

6、s2、使用一種基于高階積分慣性姿態解算方法，以實現慣性里程計和激光點云畸變糾正；

7、s3、使用非線性幾何觀測器融合慣性里程計與實景三維模型匹配結果，實現無人設備定位。

8、優選的，s1中，關鍵幀子地圖sk的構建步驟為：

9、s1-1、通過最鄰近搜索從全局地圖歷史關鍵幀中挑選出與當前位姿最接近的關鍵幀；

10、s1-2、將關鍵幀與對應的點云數據拼接，獲得用于當前幀匹配的子地圖，作為關鍵幀子地圖sk。

11、優選的，每個關鍵幀都鏈接到其對應的點云掃描，以鍵值對的形式存在。

12、優選的，s2中，通過下式求解慣性傳感器imu在實景三維模型坐標系下i時刻的位姿：

13、

14、其中，分別是慣性傳感器imu在實景三維模型坐標系下i時刻的位置、速度、姿態的四元數；i＝1,…,m表示時刻tk-1和tk之間m個imu測量值的索引；δti表示imu觀測i與i-1之間的時間間隔；是線性加加速度的估計值，表示角加速度；表示旋轉矩陣；分別表示imu的加速度計測量和陀螺儀測量值；表示矩陣張量積運算。

15、具體的：

16、

17、優選的，所述激光點云畸變糾正通過下式進行：

18、

19、式中，分別是畸變糾正后的位置、姿態的四元數；使用將投影到地圖坐標系下；

20、完成點云中所有點的投影后，即得到了畸變糾正后的整幀三維激光點云。

21、優選的，s3具體包括：

22、s3-1、三維激光點云-實景三維模型匹配；通過下式進行：

23、

24、其中，表示配準位姿增量；dj表示掃描的三維激光點云和實景三維模型中點云中對應點對在迭代變換后的距離；和為與對應點簇的相應協方差矩陣；δtk表示待優化的變換矩陣，表示δtk的轉置，表示三維激光點云到實景三維模型的變換；

25、s3-2、基于非線性幾何觀測器的融合狀態估計，通過下式進行：

26、

27、其中，分別表示無人設備融合狀態估計的位置、角速度、姿態的四元數、速度、加速度；γl∈{1,…,5}為常數增益系數，為兩次gicp姿態的間隔時間，表示四元數實部，表示四元數虛部，表示相對姿態，pe表示位置誤差；imu傳播得到預測和實景三維模型匹配得到觀測之間的姿態與位置誤差為和表示由當前姿態估計得到的旋轉矩陣。

28、優選的，s3中，三維激光點云與實景三維模型的匹配算法采用基于局部點云均值方差匹配的ndt方法、基于特征的匹配方法或基于深度學習的匹配方法。

29、優選的，s3中，使用擴展卡爾曼濾波ekf、因子圖或粒子濾波pf融合慣性里程計與實景三維模型匹配結果。

30、本發明的有益效果

31、本發明提出一種基于關鍵幀的子地圖構建方法，首先，通過最鄰近搜索從全局地圖歷史關鍵幀中挑選出與當前位姿最接近的關鍵幀，然后將這些關鍵幀對應的點云數據拼接在一起，生成用于當前幀匹配的子地圖，使得當前幀的激光點云與實景三維模型的重疊增多，減小計算開銷，加快匹配速度。

32、本發明提出了一種基于實景三維模型匹配的激光-慣性融合定位方法。首先，在傳統姿態解算模型的基礎上，增加關于線性加加速度與角加速度的三階時間積分，從而為三維激光點云的運動畸變去除提供準確的姿態，實現高頻的慣性里程計和精確的點云畸變糾正；然后使用非線性幾何觀測器融合慣性里程計與實景三維模型匹配的姿態估計，通過增益系數來修正無人設備當前的位姿、角速度偏差和加速度偏差，以減少系統誤差的積累，確保位置估計和速度估計的準確性。

技術特征：

1.一種實景三維模型增強的無人設備自主定位方法，在無人設備運動過程中，通過激光雷達掃描獲得無人設備的三維激光點云數據；

2.根據權利要求1所述的方法，其特征在于s1中，關鍵幀子地圖sk的構建步驟為：

3.根據權利要求2所述的方法，其特征在于每個關鍵幀都鏈接到其對應的點云掃描，以鍵值對的形式存在。

4.根據權利要求1所述的方法，其特征在于s2中，通過下式求解慣性傳感器imu在實景三維模型坐標系下i時刻的位姿：

5.根據權利要求4所述的方法，其特征在于：

6.根據權利要求4所述的方法，其特征在于所述激光點云畸變糾正通過下式進行：

7.根據權利要求1所述的方法，其特征在于s3具體包括：

8.根據權利要求1所述的方法，其特征在于s3中，三維激光點云與實景三維模型的匹配算法采用基于局部點云均值方差匹配的ndt方法、基于特征的匹配方法或基于深度學習的匹配方法。

9.根據權利要求1所述的方法，其特征在于s3中，使用擴展卡爾曼濾波ekf、因子圖或粒子濾波pf融合慣性里程計與實景三維模型匹配結果。

技術總結
本發明公開了一種實景三維模型增強的無人設備自主定位方法，針對復雜工業廠區，首先，通過搜索最近鄰關鍵幀和最近鄰凸包來構建關鍵幀子地圖，然后構建所有的關鍵幀點云子地圖集合，使得當前幀的激光點云與實景三維模型的重疊增多，加快匹配速度；然后，使用一種基于高階積分慣性姿態解算方法，以實現高頻的慣性里程計和精確的激光點云畸變糾正；最后，使用非線性幾何觀測器融合慣性里程計與實景三維模型匹配結果，即可實現高精度的無人設備定位。

技術研發人員：周寶定,楊鈞富,張德津,黃喆磊,尚洪猛,張星,王慶,徐洪富,江俊池
受保護的技術使用者：深圳大學
技術研發日：
技術公布日：2025/4/24