本發(fā)明涉及定位，特別是指一種位姿估計(jì)優(yōu)化方法、裝置、定位設(shè)備、程序產(chǎn)品及介質(zhì)。

背景技術(shù)：

1、同步定位與地圖構(gòu)建(simultaneous?localization?and?mapping，slam)定位技術(shù)是一個(gè)跨學(xué)科領(lǐng)域，包括計(jì)算機(jī)視覺(jué)、數(shù)值分析和優(yōu)化以及傳感器融合，該技術(shù)旨在根據(jù)視覺(jué)-慣性以及其他傳感器的觀測(cè)數(shù)據(jù)，結(jié)合計(jì)算機(jī)視覺(jué)和優(yōu)化等相關(guān)技術(shù)，讓智能體能夠在未知環(huán)境中進(jìn)行準(zhǔn)確定位并構(gòu)建環(huán)境地圖，從而為自動(dòng)駕駛、探索、救援等任務(wù)提供技術(shù)支撐。

2、然而，現(xiàn)有的定位技術(shù)存在位姿估計(jì)精度較差以及需依賴(lài)錨點(diǎn)先驗(yàn)位置信息(需手動(dòng)測(cè)量或在線標(biāo)定)的問(wèn)題。

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

1、本發(fā)明的目的是提供一種位姿估計(jì)優(yōu)化方法、裝置、定位設(shè)備、程序產(chǎn)品及介質(zhì)，解決了現(xiàn)有定位技術(shù)的位姿估計(jì)精度較差以及需依賴(lài)錨點(diǎn)先驗(yàn)位置信息的問(wèn)題。

2、為達(dá)到上述目的，本發(fā)明的實(shí)施例提供一種位姿估計(jì)優(yōu)化方法，包括：

3、獲取目標(biāo)物體對(duì)應(yīng)的觀測(cè)信息，所述觀測(cè)信息包括：慣性測(cè)量單元(inertialmeasurement?unit，imu)數(shù)據(jù)、視覺(jué)數(shù)據(jù)和超寬帶(ultra-wideband，uwb)數(shù)據(jù)；

4、基于滑動(dòng)窗口優(yōu)化方式，根據(jù)所述觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息；

5、根據(jù)所述至少一個(gè)約束信息，對(duì)所述位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至所述模型收斂。

6、可選地，所述基于滑動(dòng)窗口優(yōu)化方式，根據(jù)所述觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息，包括：

7、對(duì)所述imu數(shù)據(jù)、所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)進(jìn)行數(shù)據(jù)對(duì)齊，獲得目標(biāo)觀測(cè)信息；

8、根據(jù)所述目標(biāo)觀測(cè)信息，構(gòu)建所述至少一個(gè)約束信息。

9、可選地，所述對(duì)所述imu數(shù)據(jù)、所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)進(jìn)行數(shù)據(jù)對(duì)齊，獲得目標(biāo)觀測(cè)信息，包括以下至少一項(xiàng)：

10、將所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)轉(zhuǎn)換至imu坐標(biāo)系下；

11、根據(jù)所述imu數(shù)據(jù)，確定imu傳播位姿，并根據(jù)所述imu傳播位姿，對(duì)所述uwb數(shù)據(jù)與所述視覺(jué)數(shù)據(jù)之間的時(shí)間差進(jìn)行補(bǔ)償。

12、可選地，所述根據(jù)所述至少一個(gè)約束信息，對(duì)所述位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至所述模型收斂，包括：

13、基于非線性最小二乘優(yōu)化方法，根據(jù)所述至少一個(gè)約束信息，構(gòu)建所述模型的損失函數(shù)；

14、基于所述觀測(cè)信息和所述損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

15、可選地，所述約束信息包括uwb幀間約束信息，所述損失函數(shù)包括根據(jù)所述uwb幀間約束信息定義的目標(biāo)誤差項(xiàng)；

16、所述基于所述觀測(cè)信息和所述損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂，包括：

17、對(duì)所述損失函數(shù)中的所述目標(biāo)誤差項(xiàng)進(jìn)行簡(jiǎn)化，獲得目標(biāo)損失函數(shù)；

18、基于所述觀測(cè)信息和所述目標(biāo)損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

19、可選地，所述對(duì)所述損失函數(shù)中的所述目標(biāo)誤差項(xiàng)進(jìn)行簡(jiǎn)化，獲得目標(biāo)損失函數(shù)，包括：

20、基于馬氏距離，對(duì)所述目標(biāo)誤差項(xiàng)進(jìn)行標(biāo)準(zhǔn)化，獲得所述目標(biāo)誤差項(xiàng)對(duì)應(yīng)的目標(biāo)函數(shù)；

21、針對(duì)所述目標(biāo)函數(shù)中的協(xié)方差矩陣，基于幾何近似，確定所述uwb數(shù)據(jù)的協(xié)方差；

22、根據(jù)所述uwb數(shù)據(jù)的協(xié)方差，確定簡(jiǎn)化后的目標(biāo)誤差項(xiàng)；

23、根據(jù)所述簡(jiǎn)化后的目標(biāo)誤差項(xiàng)，獲得所述目標(biāo)損失函數(shù)。

24、為達(dá)到上述目的，本發(fā)明的實(shí)施例提供一種位姿估計(jì)優(yōu)化裝置，包括：

25、數(shù)據(jù)獲取模塊，用于獲取目標(biāo)物體對(duì)應(yīng)的觀測(cè)信息，所述觀測(cè)信息包括：慣性測(cè)量單元imu數(shù)據(jù)、視覺(jué)數(shù)據(jù)和超寬帶uwb數(shù)據(jù)；

26、約束構(gòu)建模塊，用于基于滑動(dòng)窗口優(yōu)化方式，根據(jù)所述觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息；

27、模型優(yōu)化模塊，用于根據(jù)所述至少一個(gè)約束信息，對(duì)所述位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至所述模型收斂。

28、可選地，所述約束構(gòu)建模塊包括：

29、數(shù)據(jù)對(duì)齊子模塊，用于對(duì)所述imu數(shù)據(jù)、所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)進(jìn)行數(shù)據(jù)對(duì)齊，獲得目標(biāo)觀測(cè)信息；

30、約束構(gòu)建子模塊，用于根據(jù)所述目標(biāo)觀測(cè)信息，構(gòu)建所述至少一個(gè)約束信息。

31、可選地，所述數(shù)據(jù)對(duì)齊子模塊包括：

32、數(shù)據(jù)轉(zhuǎn)換單元，用于將所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)轉(zhuǎn)換至imu坐標(biāo)系下；

33、時(shí)間補(bǔ)償單元，用于根據(jù)所述imu數(shù)據(jù)，確定imu傳播位姿，并根據(jù)所述imu傳播位姿，對(duì)所述uwb數(shù)據(jù)與所述視覺(jué)數(shù)據(jù)之間的時(shí)間差進(jìn)行補(bǔ)償。

34、可選地，所述模型優(yōu)化模塊包括：

35、函數(shù)構(gòu)建子模塊，用于基于非線性最小二乘優(yōu)化方法，根據(jù)所述至少一個(gè)約束信息，構(gòu)建所述模型的損失函數(shù)；

36、模型優(yōu)化子模塊，用于基于所述觀測(cè)信息和所述損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

37、可選地，所述約束信息包括uwb幀間約束信息，所述損失函數(shù)包括根據(jù)所述uwb幀間約束信息定義的目標(biāo)誤差項(xiàng)；所述模型優(yōu)化子模塊包括：

38、簡(jiǎn)化處理單元，用于對(duì)所述損失函數(shù)中的所述目標(biāo)誤差項(xiàng)進(jìn)行簡(jiǎn)化，獲得目標(biāo)損失函數(shù)；

39、模型優(yōu)化單元，用于基于所述觀測(cè)信息和所述目標(biāo)損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

40、可選地，所述簡(jiǎn)化處理單元包括：

41、標(biāo)準(zhǔn)化子單元，用于基于馬氏距離，對(duì)所述目標(biāo)誤差項(xiàng)進(jìn)行標(biāo)準(zhǔn)化，獲得所述目標(biāo)誤差項(xiàng)對(duì)應(yīng)的目標(biāo)函數(shù)；

42、第一處理子單元，用于針對(duì)所述目標(biāo)函數(shù)中的協(xié)方差矩陣，基于幾何近似，確定所述uwb數(shù)據(jù)的協(xié)方差；

43、第二處理子單元，用于根據(jù)所述uwb數(shù)據(jù)的協(xié)方差，確定簡(jiǎn)化后的目標(biāo)誤差項(xiàng)；

44、第三處理子單元，用于根據(jù)所述簡(jiǎn)化后的目標(biāo)誤差項(xiàng)，獲得所述目標(biāo)損失函數(shù)。

45、為達(dá)到上述目的，本發(fā)明的實(shí)施例提供一種定位設(shè)備，包括處理器和收發(fā)機(jī)，其中，所述處理器用于：

46、獲取目標(biāo)物體對(duì)應(yīng)的觀測(cè)信息，所述觀測(cè)信息包括：慣性測(cè)量單元imu數(shù)據(jù)、視覺(jué)數(shù)據(jù)和超寬帶uwb數(shù)據(jù)；

47、基于滑動(dòng)窗口優(yōu)化方式，根據(jù)所述觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息；

48、根據(jù)所述至少一個(gè)約束信息，對(duì)所述位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至所述模型收斂。

49、可選地，所述基于滑動(dòng)窗口優(yōu)化方式，根據(jù)所述觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息時(shí)，具體用于：

50、對(duì)所述imu數(shù)據(jù)、所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)進(jìn)行數(shù)據(jù)對(duì)齊，獲得目標(biāo)觀測(cè)信息；

51、根據(jù)所述目標(biāo)觀測(cè)信息，構(gòu)建所述至少一個(gè)約束信息。

52、可選地，所述處理器在對(duì)所述imu數(shù)據(jù)、所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)進(jìn)行數(shù)據(jù)對(duì)齊，獲得目標(biāo)觀測(cè)信息時(shí)，具體用于執(zhí)行以下至少一項(xiàng)：

53、將所述視覺(jué)數(shù)據(jù)和所述uwb數(shù)據(jù)轉(zhuǎn)換至imu坐標(biāo)系下；

54、根據(jù)所述imu數(shù)據(jù)，確定imu傳播位姿，并根據(jù)所述imu傳播位姿，對(duì)所述uwb數(shù)據(jù)與所述視覺(jué)數(shù)據(jù)之間的時(shí)間差進(jìn)行補(bǔ)償。

55、可選地，所述處理器在根據(jù)所述至少一個(gè)約束信息，對(duì)所述位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至所述模型收斂時(shí)，具體用于：

56、基于非線性最小二乘優(yōu)化方法，根據(jù)所述至少一個(gè)約束信息，構(gòu)建所述模型的損失函數(shù)；

57、基于所述觀測(cè)信息和所述損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

58、可選地，所述約束信息包括uwb幀間約束信息，所述損失函數(shù)包括根據(jù)所述uwb幀間約束信息定義的目標(biāo)誤差項(xiàng)；

59、所述處理器在基于所述觀測(cè)信息和所述損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂時(shí)，具體用于：

60、對(duì)所述損失函數(shù)中的所述目標(biāo)誤差項(xiàng)進(jìn)行簡(jiǎn)化，獲得目標(biāo)損失函數(shù)；

61、基于所述觀測(cè)信息和所述目標(biāo)損失函數(shù)，對(duì)所述模型進(jìn)行優(yōu)化，直至所述模型收斂。

62、可選地，所述處理器在對(duì)所述損失函數(shù)中的所述目標(biāo)誤差項(xiàng)進(jìn)行簡(jiǎn)化，獲得目標(biāo)損失函數(shù)時(shí)，具體用于：

63、基于馬氏距離，對(duì)所述目標(biāo)誤差項(xiàng)進(jìn)行標(biāo)準(zhǔn)化，獲得所述目標(biāo)誤差項(xiàng)對(duì)應(yīng)的目標(biāo)函數(shù)；

64、針對(duì)所述目標(biāo)函數(shù)中的協(xié)方差矩陣，基于幾何近似，確定所述uwb數(shù)據(jù)的協(xié)方差；

65、根據(jù)所述uwb數(shù)據(jù)的協(xié)方差，確定簡(jiǎn)化后的目標(biāo)誤差項(xiàng)；

66、根據(jù)所述簡(jiǎn)化后的目標(biāo)誤差項(xiàng)，獲得所述目標(biāo)損失函數(shù)。

67、為達(dá)到上述目的，本發(fā)明的實(shí)施例提供一種定位設(shè)備，包括收發(fā)機(jī)、處理器、存儲(chǔ)器及存儲(chǔ)在所述存儲(chǔ)器上并可在所述處理器上運(yùn)行的程序或指令；所述處理器執(zhí)行程序或指令時(shí)實(shí)現(xiàn)如上的位姿估計(jì)優(yōu)化方法。

68、為達(dá)到上述目的，提供一種計(jì)算機(jī)程序產(chǎn)品，包括計(jì)算機(jī)指令，該計(jì)算機(jī)指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)如上的位姿估計(jì)優(yōu)化方法的步驟。

69、為達(dá)到上述目的，本發(fā)明的實(shí)施例提供一種可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有程序或指令，所述程序或指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)如上的位姿估計(jì)優(yōu)化方法中的步驟。

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

71、本發(fā)明實(shí)施例的方法，在獲取目標(biāo)物體對(duì)應(yīng)的觀測(cè)信息后，再基于滑動(dòng)窗口優(yōu)化方式，根據(jù)觀測(cè)信息，構(gòu)建位姿估計(jì)任務(wù)對(duì)應(yīng)的至少一個(gè)約束信息，其中，觀測(cè)信息包括：imu數(shù)據(jù)、視覺(jué)數(shù)據(jù)和uwb數(shù)據(jù)，之后，根據(jù)至少一個(gè)約束信息，對(duì)位姿估計(jì)任務(wù)對(duì)應(yīng)的模型進(jìn)行優(yōu)化，直至模型收斂。這樣，基于滑動(dòng)窗口優(yōu)化方式，通過(guò)加入uwb數(shù)據(jù)作為約束信息，可以提高模型對(duì)目標(biāo)物體位姿的估計(jì)精度，且不需要依賴(lài)先驗(yàn)信息。