本發(fā)明涉及自動化導(dǎo)航與定位技術(shù)，特別是涉及一種基于點云多層采樣的視覺里程計初始化方法。

背景技術(shù)：

1、在現(xiàn)代無人機(uav)和無人地面車輛(ugv)協(xié)同任務(wù)中，精確的相對定位一直是一個關(guān)鍵且具挑戰(zhàn)性的任務(wù)。隨著自動化技術(shù)的發(fā)展，無人系統(tǒng)在農(nóng)業(yè)、軍事、救援、物流等領(lǐng)域的應(yīng)用日益廣泛。這些系統(tǒng)在執(zhí)行任務(wù)時需要實時、精確地知道自己的位置和姿態(tài)，確保任務(wù)的順利完成和系統(tǒng)的安全運行。

2、視覺慣性里程計(visual-inertial?odometry,vio)系統(tǒng)因其低成本和輕量化的優(yōu)勢，成為了許多無人系統(tǒng)的首選定位技術(shù)。vio系統(tǒng)通過融合視覺傳感器和慣性測量單元(imu)的數(shù)據(jù)，實時計算設(shè)備的三維位置和姿態(tài)。視覺傳感器提供豐富的環(huán)境信息，而imu則提供高頻率的運動數(shù)據(jù)，二者結(jié)合可以克服單一傳感器的局限性，提高定位的精度和魯棒性。

3、然而，vio系統(tǒng)在實際應(yīng)用中面臨一些挑戰(zhàn)。首先，vio系統(tǒng)在初始化過程中需要準確的初始位姿信息，這是確保后續(xù)定位精度的基礎(chǔ)。傳統(tǒng)的vio系統(tǒng)通常依賴先驗地圖或手動提供的初始值來進行初始化，這在實際應(yīng)用中存在較大的局限性。先驗地圖的獲取和維護需要大量的人力和物力，而手動提供初始值則要求操作人員具備較高的專業(yè)技能，這在復(fù)雜環(huán)境和動態(tài)場景中難以實現(xiàn)。

4、其次，vio系統(tǒng)在未知環(huán)境中的應(yīng)用也面臨許多不確定性。環(huán)境的復(fù)雜性和動態(tài)變化會對視覺傳感器的性能產(chǎn)生影響，如光照變化、遮擋和動態(tài)物體等，都會導(dǎo)致視覺特征點的丟失或錯誤匹配，進而影響系統(tǒng)的定位精度。同時，imu數(shù)據(jù)中存在的噪聲和漂移也會累積，導(dǎo)致系統(tǒng)的長時間定位漂移。

5、此外，現(xiàn)有的vio系統(tǒng)在處理尺度估計問題時也存在一定的局限性。視覺傳感器提供的圖像數(shù)據(jù)是無尺度的，無法直接反映實際的物理尺寸，而imu數(shù)據(jù)雖然可以提供運動的加速度信息，但在初始尺度未知的情況下，無法準確估計出系統(tǒng)的實際運動尺度。這導(dǎo)致了vio系統(tǒng)在初始化過程中需要額外的步驟來確定實際的尺度信息，從而增加了系統(tǒng)的復(fù)雜性和計算負擔(dān)。

6、一種傳統(tǒng)的vio系統(tǒng)依賴于先驗地圖或手動提供的初始值來進行初始化。這些方法通常需要大量的人力和物力來獲取和維護先驗地圖，或者需要操作人員具備較高的專業(yè)技能，手動提供初始位姿。這些方法在復(fù)雜環(huán)境和動態(tài)場景中難以實現(xiàn)，且對系統(tǒng)的適用性和魯棒性產(chǎn)生了限制。需要先驗地圖或手動提供初始值，增加了系統(tǒng)的復(fù)雜性和操作難度。在未知環(huán)境和動態(tài)場景中難以實現(xiàn)，限制了系統(tǒng)的適用性和魯棒性。對操作人員的專業(yè)技能要求較高，增加了系統(tǒng)的使用門檻。

7、另一種相關(guān)的現(xiàn)有技術(shù)是透視n點(pnp)算法，這種算法用于從二維圖像點估計三維物體姿態(tài)。pnp算法在計算機視覺中廣泛應(yīng)用，尤其是在增強現(xiàn)實(ar)和機器人定位等領(lǐng)域?；舅枷胧峭ㄟ^已知的三維點與其在圖像中的二維投影點之間的對應(yīng)關(guān)系，求解相機的姿態(tài)。具體實現(xiàn)過程中，pnp算法需要一些已知的三維點坐標及其在圖像中的二維投影點，使用這些對應(yīng)關(guān)系，通過最小化投影誤差來估計相機的姿態(tài)。常見的pnp算法有epnp(efficient?pnp)、upnp(unified?pnp)和rpnp(robust?pnp)等，它們在計算效率和魯棒性上有所不同。在視覺慣性里程計(vio)系統(tǒng)中，pnp算法常用于初始化階段，通過圖像中提取的特征點和已知的三維點之間的對應(yīng)關(guān)系，估計相機的初始姿態(tài)。這為后續(xù)的視覺和慣性數(shù)據(jù)融合提供了一個準確的初始值。pnp算法只能估計相機的姿態(tài)，但無法直接解決尺度估計問題。在vio系統(tǒng)中，雖然imu可以提供運動的加速度信息，但在初始尺度未知的情況下，無法準確估計系統(tǒng)的實際運動尺度，需要額外的步驟來確定實際的尺度信息，增加了系統(tǒng)的復(fù)雜性和計算負擔(dān)。

8、需要說明的是，在上述背景技術(shù)部分公開的信息僅用于對本技術(shù)的背景的理解，因此可以包括不構(gòu)成對本領(lǐng)域普通技術(shù)人員已知的現(xiàn)有技術(shù)的信息。

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

1、本發(fā)明的主要目的在于解決上述背景技術(shù)中存在的問題，提供一種基于點云多層采樣的視覺里程計初始化方法。

2、為實現(xiàn)上述目的，本發(fā)明采用以下技術(shù)方案：

3、在本發(fā)明的第一方面，一種基于點云多層采樣的視覺里程計初始化方法，適用于無人機(uav)和無人地面車輛(ugv)協(xié)同任務(wù)的無人系統(tǒng)，包括以下步驟：

4、步驟一：使用klt稀疏光流算法在連續(xù)的多幀圖像間進行特征點匹配，構(gòu)建滑動窗口，以獲取穩(wěn)定和連續(xù)的特征點數(shù)據(jù)；

5、步驟二：利用步驟一得到的特征點數(shù)據(jù)，通過2d-2d特征關(guān)聯(lián)、pnp算法和三角化方法，從圖像序列中構(gòu)建出無尺度的sfm點云，為位姿估計提供幾何基礎(chǔ)；

6、步驟三：結(jié)合imu的運動信息和步驟二得到的sfm點云，通過預(yù)積分方法計算相鄰幀之間的位移和旋轉(zhuǎn)信息，進而估計出sfm點云的實際尺度、系統(tǒng)的速度和重力方向；

7、步驟四：在步驟三的基礎(chǔ)上，采用多層級采樣方法進行視覺-慣性對齊，包括首層采樣和次層采樣，以實現(xiàn)對初始相機位姿的快速收斂，產(chǎn)生粗配準位姿估計；

8、步驟五：利用步驟四得到的粗配準結(jié)果，構(gòu)建點輔助的ba優(yōu)化結(jié)構(gòu)，通過最小化視覺重投影誤差、imu預(yù)積分誤差和點-面距離誤差，進行精細優(yōu)化，獲得精確的相對位姿估計。

9、在本發(fā)明的第二方面，一種計算機程序產(chǎn)品，包括計算機程序，其特征在于，所述計算機程序由處理器執(zhí)行時實現(xiàn)所述的基于點云多層采樣的視覺里程計初始化方法。

10、本發(fā)明具有如下有益效果：

11、本發(fā)明提出一種結(jié)合從運動中重建(sfm)和多層采樣的方法，提供一種高效、準確的初始化方案，解決了現(xiàn)有vio系統(tǒng)在初始化過程中缺乏準確初始值的問題，提高了系統(tǒng)在未知環(huán)境中的適用性和魯棒性。本發(fā)明提出的方法結(jié)合從運動中重建(structure?frommotion,sfm)和多層采樣，通過多次采樣和迭代優(yōu)化，逐步細化參數(shù)空間，從而實現(xiàn)高效、準確的初始化方案。sfm技術(shù)通過分析圖像序列中的移動物體，重建三維結(jié)構(gòu)，而多層采樣方法則通過多次采樣和迭代，快速收斂到最優(yōu)的初始位姿。兩者的結(jié)合不僅能夠提高初始化的精度和效率，還能在未知環(huán)境中實現(xiàn)魯棒的定位。

12、通過本發(fā)明提出的結(jié)合多層采樣和sfm方法的初始化方案，顯著提高了vio系統(tǒng)在未知環(huán)境中的初始化精度和效率，具有以下顯著優(yōu)點：

13、1、高精度的初始位姿估計：通過多層采樣方法，快速收斂到最優(yōu)的初始相機位姿，確保初始位姿的高精度和魯棒性。結(jié)合sfm點云和imu數(shù)據(jù)，解決了傳統(tǒng)方法中存在的尺度估計誤差和軌跡漂移問題。

14、2、高效的初始化過程：多層采樣方法顯著減少了采樣空間，提高了采樣效率，縮短了初始化時間。協(xié)同優(yōu)化框架結(jié)合視覺重投影誤差、imu預(yù)積分誤差和點-面距離誤差，通過最小化這些誤差函數(shù)，快速獲得精確的相對位姿估計。

15、3、魯棒性強：不依賴先驗地圖或手動提供的初始值，增強了系統(tǒng)在未知環(huán)境中的適用性。提出了一種新的協(xié)同優(yōu)化框架，有效解決了現(xiàn)有方法中存在的漂移問題。

16、通過本發(fā)明的實施，可以顯著提高vio系統(tǒng)在未知環(huán)境中的初始位姿估計精度和效率，具有廣泛的應(yīng)用前景和實用價值。

17、本發(fā)明實施例中的其他有益效果將在下文中進一步述及。