本發(fā)明涉及無人機設(shè)備，具體為一種基于圖優(yōu)化定位的無人機定位方法。

背景技術(shù)：

1、在無人機定位技術(shù)的發(fā)展過程中，傳統(tǒng)的方法主要依賴全球定位系統(tǒng)(gps)來實現(xiàn)位置定位。然而，gps信號在室內(nèi)或遮擋環(huán)境下容易受到干擾，定位精度和可靠性難以保證。此外，僅使用gps數(shù)據(jù)無法提供高精度的姿態(tài)估計，這對于無人機的穩(wěn)定飛行和精確操作是非常關(guān)鍵的。為了提高定位精度和魯棒性，研究人員開始探索多傳感器融合技術(shù)，通過結(jié)合激光雷達、視覺傳感器和慣性測量單元(i?mu)等多種傳感器數(shù)據(jù)，提高無人機在復(fù)雜環(huán)境下的定位能力。

2、現(xiàn)有的多傳感器融合方法雖然能夠在一定程度上提高定位精度，但依然面臨一些挑戰(zhàn)。例如，多傳感器數(shù)據(jù)的融合處理復(fù)雜，對計算資源的需求高，實時性難以滿足。此外，如何有效處理各種傳感器的數(shù)據(jù)不一致性和噪聲問題，以及如何在動態(tài)變化的環(huán)境中快速適應(yīng)，也是現(xiàn)有技術(shù)中的難點。

技術(shù)實現(xiàn)思路

1、針對現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于圖優(yōu)化定位的無人機定位方法，以解決上述背景技術(shù)中提出的問題。

2、為實現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：一種基于圖優(yōu)化定位的無人機定位方法，包括以下步驟：

3、s1、構(gòu)建基于地圖定位的窗口優(yōu)化模型；

4、s2、基于地圖定位的滑動窗口，確定地圖匹配位姿和優(yōu)化變量的殘差；

5、s3、基于地圖定位的滑動窗口，確定激光里程計相對位姿和優(yōu)化變量的殘差；

6、s4、基于地圖定位的滑動窗口，確定i?mu預(yù)積分和優(yōu)化變量的殘差；

7、s5、基于地圖匹配位姿和優(yōu)化變量的殘差、激光里程計相對位姿和優(yōu)化變量的殘差以及i?mu預(yù)積分和優(yōu)化變量的殘差的各因子對應(yīng)的hess?i?an矩陣構(gòu)建完整的hess?ian矩陣模型；

8、s6、基于地圖定位的滑動窗口，進行邊緣化處理，并確定邊緣化形成的先驗因子對應(yīng)的殘差；

9、s7、不斷往滑窗里添加新的信息，并邊緣化舊的信息，進行循環(huán)，完成基于圖優(yōu)化定位的無人機定位。

10、進一步優(yōu)化本技術(shù)方案，所述步驟s1中，所述窗口優(yōu)化模型如下所示：

11、

12、其中，r是殘差，j是殘差關(guān)于狀態(tài)量的雅可比，σ是信息矩陣。

13、進一步優(yōu)化本技術(shù)方案，所述步驟s2中，地圖匹配位姿和優(yōu)化變量的殘差對應(yīng)的因子為地圖先驗因子，一個地圖先驗因子僅約束一個位姿，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

14、進一步優(yōu)化本技術(shù)方案，所述步驟s3中，激光里程計相對位姿和優(yōu)化變量的殘差對應(yīng)的因子為激光里程計因子，一個激光里程計因子約束兩個位姿，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

15、進一步優(yōu)化本技術(shù)方案，所述步驟s4中，imu預(yù)積分和優(yōu)化變量的殘差對應(yīng)的因子為imu因子，一個imu因子約束兩個位姿，同時約束兩個時刻imu的速度與bias，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

16、進一步優(yōu)化本技術(shù)方案，所述步驟s5中，完整的hessian矩陣模型用公式表示為：

17、

18、其中，

19、

20、完整的hess?i?an矩陣模型乘法寫成累加形式為：

21、

22、累加的過程，就是各因子對應(yīng)的hess?i?an矩陣的疊加過程。

23、進一步優(yōu)化本技術(shù)方案，所述步驟s6中，邊緣化處理包括以下流程：

24、移除舊的幀；

25、添加新的幀。

26、進一步優(yōu)化本技術(shù)方案，在移除舊的幀時，移除過程表示為：

27、

28、在實際使用中，對上式進行拆分：

29、

30、式中，第一行表示剩余變量對應(yīng)的各類因子的組合，而第二行作為單獨的因子，稱為邊緣化先驗因子；

31、邊緣化先驗因子只有在第一次邊緣化之前是不存在的，完成第一次邊緣化之后就一直存在，并且隨著后續(xù)新的邊緣化進行，其內(nèi)容不斷更新。

32、進一步優(yōu)化本技術(shù)方案，在添加新的幀時，隨著定位過程的進行，不斷循環(huán)邊緣化處理的過程，從而維持窗口長度不變。

33、進一步優(yōu)化本技術(shù)方案，所述步驟s7中，循環(huán)過程包括以下內(nèi)容：

34、位姿初始化；

35、地圖匹配；

36、添加優(yōu)化因子；

37、執(zhí)行優(yōu)化；

38、輸出r和t；

39、位姿數(shù)是否等于窗口長度，若不等于，則返回到地圖匹配繼續(xù)循環(huán)，若等于，則進行邊緣化處理后，再返回到地圖匹配繼續(xù)循環(huán)。

40、與現(xiàn)有技術(shù)相比，本發(fā)明提供了一種基于圖優(yōu)化定位的無人機定位方法，具備以下有益效果：

41、1、該基于圖優(yōu)化定位的無人機定位方法，通過構(gòu)建一個滑動窗口模型，有效整合了地圖匹配位姿、激光里程計和i?mu預(yù)積分等多種傳感器數(shù)據(jù)，實現(xiàn)了高精度和高魯棒性的無人機定位，利用圖優(yōu)化技術(shù)，通過最小化殘差來優(yōu)化整個系統(tǒng)的狀態(tài)估計，這樣不僅可以處理傳感器數(shù)據(jù)之間的不一致性，還可以有效抑制噪聲影響。

42、2、該基于圖優(yōu)化定位的無人機定位方法，確保了與真實環(huán)境的高度一致性，通過激光里程計和i?mu因子增強了相對位姿的精確度，邊緣化處理確保了系統(tǒng)在不斷更新信息的同時，能夠去除不再重要的舊數(shù)據(jù)，減少計算負擔(dān)，hess?ian矩陣的構(gòu)建和優(yōu)化因子的逐步累加，大大提升了優(yōu)化過程的效率和穩(wěn)定性，使得無人機在無gps環(huán)境下也能實現(xiàn)高精度的自主定位和導(dǎo)航，極大地擴展了其應(yīng)用范圍和環(huán)境適應(yīng)能力。

技術(shù)特征：

1.一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，包括以下步驟：

2.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s1中，所述窗口優(yōu)化模型如下所示：

3.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s2中，地圖匹配位姿和優(yōu)化變量的殘差對應(yīng)的因子為地圖先驗因子，一個地圖先驗因子僅約束一個位姿，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

4.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s3中，激光里程計相對位姿和優(yōu)化變量的殘差對應(yīng)的因子為激光里程計因子，一個激光里程計因子約束兩個位姿，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

5.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s4中，imu預(yù)積分和優(yōu)化變量的殘差對應(yīng)的因子為imu因子，一個imu因子約束兩個位姿，同時約束兩個時刻imu的速度與bias，并對殘差關(guān)于優(yōu)化變量的雅可比以及對應(yīng)的hessian矩陣進行可視化處理。

6.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s5中，完整的hessian矩陣模型用公式表示為：

7.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s6中，邊緣化處理包括以下流程：

8.根據(jù)權(quán)利要求7所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，在移除舊的幀時，移除過程表示為：

9.根據(jù)權(quán)利要求7所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，在添加新的幀時，隨著定位過程的進行，不斷循環(huán)邊緣化處理的過程，從而維持窗口長度不變。

10.根據(jù)權(quán)利要求1所述的一種基于圖優(yōu)化定位的無人機定位方法，其特征在于，所述步驟s7中，循環(huán)過程包括以下內(nèi)容：

技術(shù)總結(jié)
本發(fā)明公開了一種基于圖優(yōu)化定位的無人機定位方法，涉及無人機設(shè)備技術(shù)領(lǐng)域，包括以下步驟：S1、構(gòu)建窗口優(yōu)化模型；S2、確定地圖匹配位姿和優(yōu)化變量的殘差；S3、確定激光里程計相對位姿和優(yōu)化變量的殘差；S4、確定IMU預(yù)積分和優(yōu)化變量的殘差；S5、構(gòu)建完整的Hessian矩陣模型；S6、進行邊緣化處理；S7、不斷往滑窗里添加新的信息，并邊緣化舊的信息，完成基于圖優(yōu)化定位的無人機定位。通過激光里程計和IMU因子增強了相對位姿的精確度，邊緣化處理確保了系統(tǒng)在不斷更新信息的同時，能夠去除不再重要的舊數(shù)據(jù)，減少計算負擔(dān)，Hessian矩陣的構(gòu)建和優(yōu)化因子的逐步累加，大大提升了優(yōu)化過程的效率和穩(wěn)定性。

技術(shù)研發(fā)人員：王俊,裴海,王嶸,蘇樹明,陳祥君
受保護的技術(shù)使用者：福建牛盾智防科技有限公司
技術(shù)研發(fā)日：
技術(shù)公布日：2024/10/21