本發(fā)明涉及定位追蹤，尤其涉及基于多維幾何特征的rgb-d慣性位姿估計(jì)方法、系統(tǒng)及設(shè)備。

背景技術(shù)：

1、中國(guó)專利(cn201910250449.6)提供一種基于rgb-d和imu信息融合的位姿估計(jì)方法,包括：s1在rgb-d相機(jī)數(shù)據(jù)和imu數(shù)據(jù)的時(shí)間同步之后,對(duì)rgb-d相機(jī)采集的灰度圖像和深度圖像以及i?mu采集的加速度、角速度信息進(jìn)行預(yù)處理,獲取世界坐標(biāo)系下相鄰幀匹配的特征點(diǎn)和i?mu狀態(tài)增量；s2依據(jù)位姿估計(jì)系統(tǒng)的系統(tǒng)外參,對(duì)系統(tǒng)中視覺慣導(dǎo)裝置進(jìn)行初始化；s3根據(jù)初始化后的視覺慣導(dǎo)裝置的信息和世界坐標(biāo)系下相鄰幀匹配的特征點(diǎn)、imu狀態(tài)增量構(gòu)建系統(tǒng)的最小二乘優(yōu)化函數(shù),使用優(yōu)化方法迭代求解出最小二乘優(yōu)化函數(shù)的最優(yōu)解,將最優(yōu)解作為位姿估計(jì)狀態(tài)量；進(jìn)一步地,進(jìn)行回環(huán)檢測(cè),獲取全局一致的位姿估計(jì)狀態(tài)量。由此,使得特征點(diǎn)深度估計(jì)更加準(zhǔn)確,提高系統(tǒng)的定位精度。

2、中國(guó)專利(cn202011358438.9)是一種基于點(diǎn)線特征的rgb-d視覺slam方法,包括如下步驟：s1、rgb-d相機(jī)內(nèi)參標(biāo)定；s2、線特征提取；s3、獲取相機(jī)初始變換位姿；s4、關(guān)鍵幀到當(dāng)前幀的直線投影；s5、形成視覺里程計(jì)模塊的誤差函數(shù)；s6、當(dāng)前幀相機(jī)初始變換位姿的迭代優(yōu)化和當(dāng)前幀相機(jī)的位姿求解；

3、s7、關(guān)鍵幀的判斷；s8、局部ba優(yōu)化；s9、回環(huán)檢測(cè)；s10、重定位；s11、地圖構(gòu)建；s12、結(jié)果輸出。本發(fā)明提供的slam方法,以rgb-d相機(jī)為圖像采集設(shè)備,利用場(chǎng)景中物體的點(diǎn)和邊緣線條作為定位特征實(shí)現(xiàn)slam,在低語(yǔ)義、低紋理等富有挑戰(zhàn)性的場(chǎng)景下具有較好的準(zhǔn)確性。

4、然而隨著人工智能和機(jī)器人技術(shù)的不斷發(fā)展，機(jī)器人位姿估計(jì)成為了一個(gè)研究熱點(diǎn)。在位姿估計(jì)系統(tǒng)中，視覺慣性位姿估計(jì)是一種關(guān)鍵技術(shù)，通過(guò)使用相機(jī)和慣性傳感器imu來(lái)估計(jì)機(jī)器人的運(yùn)動(dòng)軌跡。然而，地面自主移動(dòng)機(jī)器人通常需要長(zhǎng)期作業(yè)于購(gòu)物中心，商場(chǎng)等復(fù)雜的室內(nèi)結(jié)構(gòu)場(chǎng)景，其中包含大面積的弱紋理區(qū)域，運(yùn)動(dòng)的人或購(gòu)物車等未知的運(yùn)動(dòng)對(duì)象。傳統(tǒng)的基于點(diǎn)特征的視覺慣性位姿估計(jì)技術(shù)往往因點(diǎn)特征提取數(shù)量不足、不穩(wěn)定或跟蹤失敗導(dǎo)致位姿估計(jì)系統(tǒng)精度降低或崩潰，使其在實(shí)際應(yīng)用中的弱紋理、無(wú)紋理以及動(dòng)態(tài)場(chǎng)景區(qū)域限制很大。

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

1、(一)發(fā)明目的

2、為解決背景技術(shù)中存在的技術(shù)問(wèn)題，本發(fā)明提出基于多維幾何特征的rgb-d慣性位姿估計(jì)方法，解決傳感器在弱紋理、無(wú)紋理場(chǎng)景下定位的問(wèn)題，使得定位方法更加魯棒，此外還去除了動(dòng)態(tài)特征，解決了在動(dòng)態(tài)環(huán)境下累積漂移和定位失效的問(wèn)題。

3、(二)技術(shù)方案

4、為解決上述問(wèn)題，本發(fā)明第一方面提供了基于多維幾何特征的rgb-d慣性位姿估計(jì)方法，包括：

5、數(shù)據(jù)預(yù)處理，對(duì)imu數(shù)據(jù)做積分處理，并對(duì)rgb圖像原始數(shù)據(jù)提取點(diǎn)、線特征，利用消失點(diǎn)對(duì)線特征進(jìn)行分類，以判斷所提取的線是否具有結(jié)構(gòu)規(guī)律；

6、多模態(tài)路標(biāo)分類，使用基于交叉重投影的空間一致性驗(yàn)證和深度范圍限制將路標(biāo)重建，將恢復(fù)出的3d路標(biāo)劃分為準(zhǔn)確路標(biāo)和其他路標(biāo)；

7、動(dòng)態(tài)特征去除，結(jié)合人和非人動(dòng)態(tài)對(duì)象上提取的點(diǎn)、線特征的幾何約束和運(yùn)動(dòng)約束去除動(dòng)態(tài)點(diǎn)、線特征；

8、因子圖優(yōu)化，結(jié)合點(diǎn)特征的重投影殘差，線特征的重投影殘差和消失點(diǎn)的測(cè)量殘差，作為新的因子加入到因子圖中，利用滑動(dòng)窗口進(jìn)行因子圖優(yōu)化，以獲取低漂移的當(dāng)前位姿。

9、作為本發(fā)明的一個(gè)技術(shù)方案，在所述數(shù)據(jù)預(yù)處理中，線特征的提取包括步驟：

10、s101，定義當(dāng)前線段長(zhǎng)度和圖像最長(zhǎng)邊的比值為響應(yīng)值；

11、s102，計(jì)算當(dāng)前幀所提線段的響應(yīng)值；

12、s103，按照響應(yīng)值的高低對(duì)線段進(jìn)行排序；

13、s104，提取指定數(shù)目的線段，以完成高質(zhì)量線特征的提取；

14、作為本發(fā)明的一個(gè)技術(shù)方案，在所述數(shù)據(jù)預(yù)處理中，點(diǎn)特征的提取包括步驟：

15、s201，對(duì)圖像劃分網(wǎng)格，并在單一網(wǎng)格中提取指定數(shù)目的高響應(yīng)值fast角點(diǎn)；

16、s202，根據(jù)在特征周圍設(shè)置掩模和非極大值抑制的思想選取均勻分布的fast角點(diǎn)特征；

17、s203，采用雙向lk光流的點(diǎn)特征跟蹤方法去除錯(cuò)誤匹配，以完成高質(zhì)量點(diǎn)特征的提取。

18、作為本發(fā)明的一個(gè)技術(shù)方案，在所述數(shù)據(jù)預(yù)處理中，還包括利用消失點(diǎn)特征約束直線方向，以補(bǔ)充線特征的結(jié)構(gòu)規(guī)律，同時(shí)利用depth圖像獲取特征的深度信息，以恢復(fù)特征的3d空間位置。

19、作為本發(fā)明的一個(gè)技術(shù)方案，在所述數(shù)據(jù)預(yù)處理中，采用1-l?i?ne?ransac檢測(cè)圖像消失點(diǎn)來(lái)對(duì)線特征進(jìn)行分類。

20、作為本發(fā)明的一個(gè)技術(shù)方案，在所述數(shù)據(jù)預(yù)處理中，根據(jù)圖像數(shù)據(jù)，對(duì)多維幾何特征表幀圖像信息進(jìn)行融合，包括點(diǎn)特征，線特征和消失點(diǎn)特征，利用elsed線段檢測(cè)器檢測(cè)直線特征。

21、作為本發(fā)明的一個(gè)技術(shù)方案，在所述多模態(tài)路標(biāo)分類中，在ba優(yōu)化過(guò)程中固定所述準(zhǔn)確路標(biāo)的3d空間位置，僅優(yōu)化所述其他路標(biāo)的3d空間位置，以提高后端優(yōu)化速度。

22、作為本發(fā)明的一個(gè)技術(shù)方案，在所述動(dòng)態(tài)特征去除中，還包括采用運(yùn)動(dòng)一致性檢測(cè)，將運(yùn)動(dòng)約束擴(kuò)展至線特征，結(jié)合3d路標(biāo)重建恢復(fù)出的3d空間位置和滑動(dòng)窗口中的優(yōu)化位姿來(lái)識(shí)別動(dòng)態(tài)線特征。

23、本發(fā)明第二方面提出了基于多維幾何特征的rgb-d慣性位姿估計(jì)系統(tǒng)，包括：

24、數(shù)據(jù)預(yù)處理模塊，用于對(duì)i?mu數(shù)據(jù)做積分處理，并對(duì)rgb圖像原始數(shù)據(jù)提取點(diǎn)、線特征，利用消失點(diǎn)對(duì)線特征進(jìn)行分類，以判斷所提取的線是否具有結(jié)構(gòu)規(guī)律；

25、多模態(tài)路標(biāo)分類模塊，用于使用基于交叉重投影的空間一致性驗(yàn)證和深度范圍限制將路標(biāo)重建，將恢復(fù)出的3d路標(biāo)劃分為準(zhǔn)確路標(biāo)和其他路標(biāo)；

26、動(dòng)態(tài)特征去除模塊，用于結(jié)合人和非人動(dòng)態(tài)對(duì)象上提取的點(diǎn)、線特征的幾何約束和運(yùn)動(dòng)約束去除動(dòng)態(tài)點(diǎn)、線特征；

27、位姿估計(jì)模塊，用于結(jié)合點(diǎn)特征的重投影殘差，線特征的重投影殘差和消失點(diǎn)的測(cè)量殘差，作為新的因子加入到因子圖中，利用滑動(dòng)窗口進(jìn)行因子圖優(yōu)化，以獲取低漂移的當(dāng)前位姿。

28、本發(fā)明第三方面提出了一種設(shè)備，所述設(shè)備包括：

29、一個(gè)或多個(gè)處理器；

30、存儲(chǔ)器，其上存儲(chǔ)有一個(gè)或多個(gè)程序；

31、當(dāng)所述一個(gè)或多個(gè)處理器執(zhí)行所述一個(gè)或多個(gè)程序時(shí)，實(shí)現(xiàn)如上述方法。

32、本發(fā)明的上述技術(shù)方案具有如下有益的技術(shù)效果：

33、本發(fā)明通過(guò)對(duì)圖像劃分網(wǎng)格，并在單一網(wǎng)格中提取指定數(shù)目的高響應(yīng)值fast角點(diǎn)，利用在特征周圍設(shè)置掩模和非極大值抑制的思想選取均勻分布的高質(zhì)量角點(diǎn)特征，提高點(diǎn)特征的質(zhì)量；為了獲得高質(zhì)量的線特征，定義當(dāng)前線段長(zhǎng)度和圖像最長(zhǎng)邊的比值為響應(yīng)值，通過(guò)計(jì)算當(dāng)前幀所提線段的響應(yīng)值，然后按照響應(yīng)值的高低對(duì)線段進(jìn)行排序，最后提取指定數(shù)目的線段來(lái)完成高質(zhì)量線特征的提取，有效減少冗余線段；過(guò)基于交叉重投影的空間一致驗(yàn)證和深度范圍限制將重建后的3d路標(biāo)分為準(zhǔn)確路標(biāo)和其他路標(biāo)，并在捆集優(yōu)化中固定住準(zhǔn)確路標(biāo)，顯著提高了后端優(yōu)化的速度；根據(jù)特征的幾何約束和運(yùn)動(dòng)約束來(lái)去除動(dòng)態(tài)點(diǎn)線特征，避免動(dòng)態(tài)特征對(duì)位姿估計(jì)造成影響，使得方法在動(dòng)態(tài)場(chǎng)景更加魯棒；通過(guò)結(jié)合點(diǎn)特征的重投影殘差，線特征的重投影殘差和消失點(diǎn)的測(cè)量殘差，利用滑動(dòng)窗口進(jìn)行因子圖優(yōu)化，獲得良好的位姿估計(jì)結(jié)果。

34、本發(fā)明能夠充分利用各傳感器的特性實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，提升定位精度以及魯棒性，即使在弱紋理的退化環(huán)境或運(yùn)動(dòng)場(chǎng)景都能實(shí)現(xiàn)高精度定位。另外本方法能夠部署于移動(dòng)機(jī)器人平臺(tái)或自動(dòng)駕駛車輛中，并具備實(shí)時(shí)運(yùn)行能力，具體以jetson?xav?i?er?nx為實(shí)驗(yàn)例，本方法能夠?qū)崿F(xiàn)10hz的處理速度。