本發(fā)明公開一種基于運動約束的輻射場slam位姿優(yōu)化方法，屬于位姿計算。

背景技術(shù)：

1、同步定位與建圖slam是計算機視覺領(lǐng)域的重要研究課題，涉及在無預(yù)先地圖信息的條件下，實時探索環(huán)境并估計位置。slam按地圖詳細程度分為稠密和稀疏兩種類型。稠密slam構(gòu)建詳細的三維模型，而稀疏slam僅提取關(guān)鍵特征點。傳統(tǒng)稠密slam系統(tǒng)通過高定位精度和實時處理能力，適應(yīng)大規(guī)模場景應(yīng)用，如自動駕駛、家庭機器人和增強現(xiàn)實。這些進展顯著推動了智能系統(tǒng)的自主性和交互能力。在高保真三維重建的研究中，輻射場理論展現(xiàn)出了其卓越的性能，神經(jīng)輻射場nerf和3d高斯飛濺表示方法已顯示出顯著的成功。為了克服傳統(tǒng)slam技術(shù)所面臨的限制，這些方法正在被引入到slam領(lǐng)域中，用于建立更為精細的幾何場模型。與傳統(tǒng)的地圖表示方法相比，這些新方法具有多方面的優(yōu)勢。它們能夠?qū)崿F(xiàn)連續(xù)的表面建模，降低內(nèi)存需求，并更有效地處理噪聲和異常值。此外，它們在處理遮擋或稀疏觀測時表現(xiàn)出更好的孔洞填充能力和場景繪制能力。這些方法還能生成更加密集且緊湊的地圖，并且能夠以任意分辨率重建為3d網(wǎng)格，從而提供了一種高質(zhì)量且靈活的三維重建解決方案。

2、神經(jīng)輻射場方法采用全連接非卷積深度網(wǎng)絡(luò)來表征場景，該網(wǎng)絡(luò)以一個連續(xù)的五維坐標作為輸入——包括空間位置和觀察方向，并輸出對應(yīng)空間點的體積密度以及視角相關(guān)的輻射強度。通過沿相機射線對這五維坐標進行查詢來實現(xiàn)視圖合成，并利用體渲染技術(shù)將顏色和密度信息投影到最終圖像中。引入了基于空間體素的nerf，僅對與當前相機視角空間相關(guān)的nerf部分進行更新。3d高斯方法采用顯式的3d高斯球來表示場景。3d高斯球包含了中心位置、高斯協(xié)方差矩陣、不透明度和球諧函數(shù)參數(shù)，其中中心位置和高斯協(xié)方差矩陣表示顯式3d高斯球的影響范圍，不透明度和球諧參數(shù)用來控制顏色表示。由于nerf和3d高斯，在跟蹤方面的實際表現(xiàn)形式是統(tǒng)一的，因此僅在跟蹤算法上具備一致性，將nerf和3d高斯統(tǒng)稱為輻射場算法。

3、盡管基于輻射場的slam能夠生成完整且密集的地圖，但在姿態(tài)估計方面與傳統(tǒng)slam方法相比仍存在不足。然而對定位的優(yōu)化有了重要的進展，但是也存在不足之處，比如回環(huán)檢測一旦失敗會造成毀滅性的重建錯誤，基于稀疏點云實現(xiàn)的slam容易出現(xiàn)跟蹤失敗的情況，引入額外數(shù)據(jù)源的過程中則需要面對噪聲的干擾問題。

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

1、本發(fā)明的目的在于提供一種基于運動約束的輻射場slam位姿優(yōu)化方法，以解決現(xiàn)有技術(shù)中，基于輻射場的slam的姿態(tài)估計精度不足問題。

2、一種基于運動約束的輻射場slam位姿優(yōu)化方法，包括，s1：獲取慣性測量單元imu數(shù)據(jù)幀和圖像幀，圖像幀包括rgb圖像幀和深度圖像幀，其中rgb圖像數(shù)據(jù)幀和深度圖像數(shù)據(jù)幀幀率一致；

3、s2：按照圖像幀的時間戳，對數(shù)據(jù)幀進行分割，分離出在兩幀圖像幀間的所有imu數(shù)據(jù)幀；

4、s3：使用輻射場損失的避障系統(tǒng)vo預(yù)測位姿；

5、s4：對數(shù)據(jù)幀進行預(yù)積分，求得在世界坐標系下當前幀相對于前一幀的運動數(shù)據(jù)，包括速度、位置和姿態(tài)的運動增量；

6、s5：記錄當前運動增量相對零偏的雅可比矩陣，每次零偏更新時，用雅可比矩陣更新運動增量；

7、s6：記錄當前運動增量的噪聲協(xié)方差矩陣；

8、s7：當圖像幀超過200幀且關(guān)鍵幀數(shù)量超過10幀時，基于輻射場損失預(yù)測的位姿對imu的全局重力和速度進行更正；

9、s8：計算運動估計值和測量值的殘差，位姿初始值設(shè)為上一幀的位姿；

10、s9：計算總損失，完成位姿優(yōu)化。

11、s3包括，計算slam的圖像幀與真實圖像幀的顏色深度聯(lián)合損失：

12、；

13、式中，為范數(shù)，為slam的圖像幀中采樣幀的顏色，為slam的圖像幀中采樣幀的深度，為真實圖像幀中采樣幀的顏色，為真實圖像幀中采樣幀的深度；

14、使用上一幀的估計位姿作為神經(jīng)輻射場nerf的輸入初始位姿，使用多層感知機mlp作為解碼器，使用特征網(wǎng)格來表示采樣幀的顏色和占用值：

15、；；

16、式中，表示顏色解碼器，表示姿態(tài)，表示顏色特征網(wǎng)格，表示占用率解碼器，表示占用率特征網(wǎng)格。

17、s3包括，計算沿采樣幀的采樣光線所在射線的占用積分：

18、；

19、式中，表示沿射線的第個位置的占用率，表示沿射線上的占用值，表示沿射線上的占用值；

20、對采樣幀的顏色和深度進行綜合處理：

21、；；

22、式中，是沿射線上的顏色，是沿射線上的深度。

23、s3包括，使用顯式的3d高斯球用作slam的場景表示：

24、}；；

25、式中，是第幀的三維高斯場景表示，是第幀的位置，表示3d高斯球的中心點，每個幀對應(yīng)一個3d高斯球，是第個3d高斯球的三維協(xié)方差矩陣，描述3d高斯球的形狀和方向，是第個3d高斯球的不透明度，是第個3d高斯球的顏色，是采樣幀總個數(shù)，為三維尺度向量，表示拉伸形狀，為旋轉(zhuǎn)矩陣，表示方向，使用四元數(shù)存儲旋轉(zhuǎn)矩陣；

26、對投影生成二維協(xié)方差：

27、；

28、式中，是投影函數(shù)的仿射近似的雅可比矩陣，表示視圖變換；

29、投影完成后形成2d高斯投影，以2d高斯投影中心點作為對應(yīng)的3d高斯球的中心點，沿射線方向?qū)?d高斯投影進行深度排序，第個2d高斯投影的不透明度為：

30、；

31、式中，表示以自然常數(shù)為底的指數(shù)運算，表示行列式運算；

32、使用對采樣幀的顏色和深度進行綜合處理的方法對深度排序后生成的圖像進行渲染，對給定的3d高斯球和相機姿態(tài)，和為：

33、；

34、；

35、式中，表示與不同的2d高斯球，表示第個2d高斯投影的深度。

36、s4包括，建立初始的運動增量，其中初始和為三維的零矩陣，為單位矩陣，接收第幀imu幀的加速度、角速度、時間輸入，結(jié)合上一幀的加速度噪聲零偏和角速度噪聲零偏計算：

37、；；

38、；

39、式中，為第幀到第幀的姿態(tài)增量，為李代數(shù)到李群的指數(shù)映射，為第幀到第幀速度中包含重力分量的增量，為第幀到第幀的姿態(tài)增量，為第幀到第幀位置中包含重力分量的增量，為第幀到第幀速度中包含重力分量的增量。

40、s5包括：

41、；；

42、；

43、；

44、；

45、式中，加速度噪聲零偏和角速度噪聲零偏在幀間是恒定的，是幀間加速度噪聲零偏，是幀間角速度噪聲零偏，表示求偏導(dǎo)，是李代數(shù)中的反對稱化操作，為李代數(shù)空間中的右雅可比矩陣。

46、s6包括：

47、；

48、；

49、；

50、式中，和是中間矩陣，為第幀到第幀的旋轉(zhuǎn)增量，為單位矩陣，為幀的，為當前零偏的協(xié)方差矩陣，為上一幀零偏的協(xié)方差矩陣，為加速度和角速度的零均值高斯噪聲協(xié)方差矩陣。

51、s7包括s7.1：待優(yōu)化重力和速度為，其中表示第幀的速度，表示重力，優(yōu)化目標是相鄰幀間的速度和位移的預(yù)測值與imu測量值的殘差；

52、分別將速度和位移殘差公式轉(zhuǎn)換成矩陣形式：

53、；

54、；

55、式中，為第幀的姿態(tài)，為第幀圖像幀到第幀圖像幀的速度中包含重力分量的增量，為第幀圖像幀到第幀圖像幀的位置中包含重力分量的增量，為第幀的位置；

56、合并得第一聯(lián)立式：

57、；

58、根據(jù)第一聯(lián)立式使用最小二乘法求解待定系數(shù)矩陣；

59、s7.2：修正重力方向，將重力矢量的模長固定作為重力的大小，將一對正交與重力方向的單位向量作為待優(yōu)化量，重力加速度為：

60、；

61、式中，為重力的模長，為重力的方向，、為待求系數(shù)，、為一對正交的單位向量，、都與重力方向正交，為、的聯(lián)合矩陣，為、的聯(lián)合矩陣；

62、每輪迭代更新后，將更新后的重力方向歸一化以更新；

63、將重力加速度的計算式帶入第一聯(lián)立式得到第二聯(lián)立式：

64、；

65、根據(jù)第二聯(lián)立式使用最小二乘法求解待求系數(shù)矩陣。

66、s8包括：

67、；

68、；

69、；

70、式中，為姿態(tài)估計值和測量值的殘差，為速度估計值和測量值的殘差，為位置估計值和測量值的殘差，為李群到李代數(shù)的對數(shù)映射，為第幀圖像幀的姿態(tài)，為第幀圖像幀的速度，為第幀圖像幀的位置，是幀間加速度噪聲零偏增量，是幀間角速度噪聲零偏增量。

71、計算總損失：

72、；

73、式中，為的對角線矩陣；

74、；

75、式中，為跟蹤的總損失，為權(quán)重系數(shù)；

76、采用梯度下降法優(yōu)化位姿、速度和零偏，當總損失最小時即到達最優(yōu)位姿；

77、由于每輪梯度下降后使用當前運動增量相對噪聲的雅可比矩陣對姿態(tài)、速度和位移的預(yù)積分項做更新：

78、；

79、；

80、；

81、式中，、、是積分項更新后的姿態(tài)、速度和位移。

82、相對比現(xiàn)有技術(shù)，本發(fā)明具有以下有益效果：提供了優(yōu)化函數(shù)來優(yōu)化imu零偏的干擾，進一步提高額外數(shù)據(jù)源的魯棒性問題，相比此前基于慣性對輻射場slam的方法初始化了運動參數(shù)和重力參數(shù)，使得輻射場slam對于定位有更加的適應(yīng)性，為輻射場slam的實際應(yīng)用提供了解決方法。