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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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