本發(fā)明涉及定位導(dǎo)航，具體涉及一種基于雙目圖像分析與緊耦合slam融合的導(dǎo)航定位系統(tǒng)。

背景技術(shù)：

1、lbs由室外定位系統(tǒng)和室內(nèi)定位系統(tǒng)兩類組成。室外定位系統(tǒng)研究較為成熟，常用的包括gps和我國自主研發(fā)的北斗衛(wèi)星導(dǎo)航系統(tǒng)，這類系統(tǒng)在戶外精度高、信號穩(wěn)定性強(qiáng)，配合相應(yīng)設(shè)備通?？蛇_(dá)到亞米級。然而當(dāng)處于室內(nèi)、隧道等衛(wèi)星信號衰弱甚至屏蔽的地點，定位將產(chǎn)生劇烈漂移，因此室內(nèi)導(dǎo)航定位已經(jīng)成為當(dāng)前的一大研究熱點。

2、面對當(dāng)前公共建筑愈發(fā)復(fù)雜龐大，居民對陌生環(huán)境下室內(nèi)定位導(dǎo)航的需求日益高漲。在室內(nèi)導(dǎo)航定位系統(tǒng)的研究中，目前主流研究思想是搭建無線傳感器網(wǎng)絡(luò)的實現(xiàn)方案，常用的方法包括無線網(wǎng)絡(luò)、藍(lán)牙和超寬帶等，但是這些方法通常受制于設(shè)備網(wǎng)絡(luò)系統(tǒng)的搭建。未解決上述問題，我們將圖像分析應(yīng)用于室內(nèi)慣性導(dǎo)航系統(tǒng)中，采用多傳感融合的思想提高室內(nèi)導(dǎo)航定位的精度，以實現(xiàn)非特征環(huán)境下的精準(zhǔn)感知與導(dǎo)航定位技術(shù)關(guān)鍵技術(shù)和應(yīng)用。

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

1、本發(fā)明的目的是提供一種基于雙目圖像分析與緊耦合slam融合的導(dǎo)航定位系統(tǒng)，以解決現(xiàn)有技術(shù)中的上述不足之處。

2、為了實現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：優(yōu)選的，

3、一個實時同步定位、建圖和著色框架，一種基于雙目圖像分析與緊耦合slam融合的導(dǎo)航定位系統(tǒng)，用于弱gnss信號復(fù)雜環(huán)境下。該系統(tǒng)包括數(shù)據(jù)采集層、數(shù)據(jù)處理層、應(yīng)用層：

4、數(shù)據(jù)采集層使用基于激光-慣導(dǎo)-視覺-imu-gps融合slam方法，將激光點云特征與imu數(shù)據(jù)和相機(jī)提取并優(yōu)化后的特征信息進(jìn)行一同運算，提高整個系統(tǒng)在非結(jié)構(gòu)化環(huán)境中的定位精度，通過關(guān)鍵幀的匹配降低整個系統(tǒng)的計算量，使得精度與計算量之間取得平衡，保證了系統(tǒng)的準(zhǔn)確率和實時性。

5、數(shù)據(jù)處理層將下層產(chǎn)生的數(shù)據(jù)處理整合，通過雙目視覺圖像分析方法進(jìn)行灰度化后實景分析和消失點檢測，估算出的地面平面位置及相應(yīng)寬度深度，并通過最小光度誤差與特征匹配渲染地圖的紋理，構(gòu)建地圖幾何結(jié)構(gòu)，實現(xiàn)精準(zhǔn)定位與建圖。

6、應(yīng)用層將已提前訓(xùn)練完成的規(guī)劃路徑上傳至自主決策系統(tǒng)，為實現(xiàn)最終的自主定位導(dǎo)航系統(tǒng)。

7、數(shù)據(jù)采集層中包含以下步驟：

8、1、使用雙相機(jī)、imu和激光雷達(dá)提取特征信息

9、2、對于傳入的lidar掃描，由于幀內(nèi)連續(xù)移動而導(dǎo)致的運動失真由imu反向傳播補(bǔ)償。

10、3、將全局地圖中的一定數(shù)量的點(即跟蹤點)投影到當(dāng)前圖像。訂閱當(dāng)前激光幀運動畸變校正后的點云信息。

11、在數(shù)據(jù)處理層中，包含以下步驟：

12、(1)、首先將下層產(chǎn)生的數(shù)據(jù)整理，進(jìn)行特征提取與imu預(yù)積分。

13、常用的imu必須必須包含用于測量角速度的三軸陀螺儀和用于測量加速度的加速度儀，再通過運算求得運動目標(biāo)的速度、空間位置等信息。

14、三軸軸陀螺儀是測定角速度的裝置，其由兩相同質(zhì)量的塊構(gòu)成，兩質(zhì)量塊速度相反，其基本工作原理是科里奧利力，角速度公式為：

15、ωm(t)＝ω′(t)+bg(t)ng(t)???(1)

16、式中,ω(t)為角速度真值；ω′(t)為測量值；bg(t)為零點漂移；ng(t)為測量噪聲。

17、三軸加速度計時慣性傳感器，其實測定目標(biāo)相對于載體坐標(biāo)系的重力加速度和絕對加速度之和的作用，其測量值基本公式如下：

18、am(t)＝r′(t)(am(t)-gw)+ba(t)+na(t)???(2)

19、式中，r′(t)表示世界坐標(biāo)系轉(zhuǎn)換到體坐標(biāo)系的旋轉(zhuǎn)；am(t)為相對世界坐標(biāo)系下的加速度矢量；gw為相對世界坐標(biāo)系下的重力矢量；na(t)是測量噪聲；ba(t)表示加速度計的漂移誤差。

20、并將完整狀態(tài)向量x∈r29定義為：

21、x＝[grit,gpit,gvt,bgt,bat,ggt,irct,ipct,itc,φt]t???(3)

22、其中g(shù)g∈r3是在全局幀(即第一個lidar幀)中表示的重力矢量，itc是imu和相機(jī)之間的時間偏移，同時假設(shè)lidar已經(jīng)與imu同步，φ是相機(jī)內(nèi)參矩陣。

23、p＝[gpx,gpy,gpz,cr,cg,cb]t＝[gpt,ct]t???(4)

24、(2)、其雙目視覺圖像分析包括以下步驟：

25、1)、先將每個相機(jī)的固有參數(shù)和兩個相機(jī)之間的非固有變換進(jìn)行離線校準(zhǔn)。

26、2)、采用兩個圖像采集設(shè)備獲取的圖片后對每一張圖片提取角點，對相應(yīng)角點進(jìn)行匹配和光流法跟蹤。

27、3)、對結(jié)果進(jìn)行緊耦合優(yōu)化，在緊耦合優(yōu)化滑動窗口時，需要進(jìn)行視覺殘差計算，其是指在當(dāng)前幀上某一特征點的投影誤差。對于特征點i，在第b幀進(jìn)行觀察時視覺殘差為：

28、

29、式中,和分別指代當(dāng)?shù)赽幀進(jìn)行觀察時系統(tǒng)估算出的特征點坐標(biāo)和圖片采集設(shè)備在成像平面上實際坐標(biāo)；rc值準(zhǔn)確度受兩方面影響,一方面是成像本身制約因素，另一方面是圖像觀察結(jié)果,最主要影響因素是圖片采集設(shè)備在進(jìn)行離線校準(zhǔn)標(biāo)定結(jié)果是否準(zhǔn)確無誤。本發(fā)明在實際實驗時采用自標(biāo)定相機(jī),內(nèi)參非常精準(zhǔn),視覺殘差可低至1像素左右。

30、4)、引入em算法進(jìn)行正交約束，使用高斯球體進(jìn)行基于em的方向檢測估計，設(shè)三個主消失方向分別為vdi，i＝1，2，3，通過em對目標(biāo)檢索獲取的線段li聚類，被檢索線段對應(yīng)的法線為nl，設(shè)z是圖像潛在變量的簇，那么相應(yīng)的高斯分布為：

31、

32、式中,為高斯方差，先驗概率p(zj＝i)＝pi。

33、5)、把滑動窗口中視覺殘差和慣導(dǎo)單元中預(yù)積分?jǐn)?shù)據(jù)約束放在同一框架之下進(jìn)行優(yōu)化處理。

34、在本系統(tǒng)設(shè)計中，采用緊耦合數(shù)據(jù)融合，滑動窗口內(nèi)的狀態(tài)向量為

35、

36、式中,為視覺與慣導(dǎo)的平移量；為視覺與慣導(dǎo)的旋轉(zhuǎn)量；λn為逆深度信息；xk為慣導(dǎo)的狀態(tài)變量,包括速度、三維位置坐標(biāo)、深度等。

37、最終就構(gòu)成了一個非線性最小二成問題，見下式：

38、

39、式中,至是視覺殘差；c是觀測點集；是慣導(dǎo)單元預(yù)積分?jǐn)?shù)據(jù)約束。

40、對于實時方向的估計更新，本發(fā)明將從視頻幀獲得的方向與來自imu的角度測量相結(jié)合。

41、x＝[γωλ]???(9)

42、

43、pt-＝apt-1at+q???(11)

44、式中,x為狀態(tài)向量；γ為來自視頻的橫滾角；ωγ為來自陀螺儀的橫滾角pt-是t時刻先驗估計協(xié)方差；q是過程激勵噪聲協(xié)方差,有更新方程為：

45、

46、

47、

48、式中，errort+1為誤差觀測；h為測量矩陣。

49、在對偏航腳和俯仰角的估計中，可以采用從加速計提取的重力分量表示，再用上述方法融合。

50、

51、

52、式中,βg、αg分別為偏航角和俯仰角；xg、yg、zg分別為重力在每個方向上的分量。

53、6)、采用邊緣化先驗法解決，對于無關(guān)狀態(tài)標(biāo)量進(jìn)行消元整合。

54、高斯牛頓法求解非線性最小二乘問題公

55、式為：

56、

57、式中,δxa、δxb為狀態(tài)變量,通過高斯消元消去ct,求得

58、(b-cta-1c)δxb＝bb-cta-1ba???(18)

59、從而舍棄狀態(tài)δxa，僅保留系統(tǒng)中的δxb以提高計算效率。

60、(3)、其構(gòu)建地圖幾何結(jié)構(gòu)包括以下步驟：

61、1)、對于傳入的lidar掃描，由于幀內(nèi)連續(xù)移動而導(dǎo)致的運動失真由imu反向傳播補(bǔ)償。

62、2)、利用誤差狀態(tài)迭代卡爾曼濾波器(esikf)最小化點對平面殘差來估計系統(tǒng)的狀態(tài)。

63、3)、通過雙目視覺圖像分析系統(tǒng)，進(jìn)行灰度化后實景分析和消失點檢測，估算出的地面平面位置及相應(yīng)寬度深度。

64、4)、在收斂狀態(tài)下，該掃描的點被附加到全局地圖上，并將相應(yīng)的體素標(biāo)記為激活或停用。

65、(4)、其渲染地圖的紋理包括以下步驟：

66、1)、利用幀到幀光流來跟蹤地圖點并通過最小化perspective-n-point?pnp)來優(yōu)化系統(tǒng)狀態(tài)跟蹤地圖點的投影誤差。

67、假設(shè)跟蹤了m個地圖點ρ＝{p1,...pm}在最后一幀圖像ik-1上，投影為{ρ1k-1，…ρmk-1}，它們在當(dāng)前圖像ik中的位置表示為{ρ1k，...ρmk}。

68、perspective-n-point投影誤差：以第s個點ps＝[gpst,cts]∈ρ為例，投影誤差計算如下:

69、

70、

71、其中在每次esikf迭代中的當(dāng)前狀態(tài)估計值,計算方法如下：

72、

73、

74、

75、其中和分別是gps和的真實值。然后，我們得到真零殘差的一階泰勒展開

76、

77、

78、

79、

80、

81、

82、

83、

84、

85、

86、k＝(htr-1h+p-1)-1htr-1?(33)

87、

88、2)、通過最小化跟蹤點之間的幀到地圖光度誤差來進(jìn)一步細(xì)化系統(tǒng)的狀態(tài)估計。

89、以第s個跟蹤點ps∈ρ為例，光度誤差計算如下：

90、

91、考慮ys和cs的測量噪聲：

92、

93、

94、

95、

96、

97、

98、

99、

100、

101、

102、表示h，r，和p相似：

103、

104、

105、

106、

107、這個幀到地圖vio?esikf更新被迭代直到收斂。然后將收斂狀態(tài)估計用于渲染地圖的紋理；更新當(dāng)前跟蹤點集p以供下一幀使用；在lio或vio更新的下一幀中作為imu傳播的起點。

108、

109、3)、使用收斂狀態(tài)估計和原始輸入圖像，我們執(zhí)行紋理渲染以更新全局地圖中點的顏色。

110、首先，在所有激活的體素中的檢索所有點。假設(shè)總共有n個點，記為ζ＝{p1,...pn}。如果點ps落在當(dāng)前圖像幀中，通過對當(dāng)前圖像幀上相鄰像素的rgb顏色值進(jìn)行線性插值，得到其觀測顏色γs和協(xié)方差通過貝葉斯更新，圖像上新觀測到的點的顏色與記錄在地圖中的現(xiàn)有顏色值cs進(jìn)行融合，得到更新后的顏色值和cs的協(xié)方差：

111、

112、

113、

114、4)、在紋理渲染完成后，我們對所跟蹤的點集p進(jìn)行更新：若點集p的點計算出的pnp重投影誤差或者光度誤差較大，則從跟蹤點的集合中將其刪除；若將點投影到當(dāng)前圖像幀上，附近沒有其他跟蹤點(比如設(shè)置半徑為50像素)，則將其添加到點集p中。

115、在應(yīng)用層中，包含以下步驟：

116、應(yīng)用層將已提前訓(xùn)練完成的模型上傳自主定位導(dǎo)航系統(tǒng)。

117、其自主定位導(dǎo)航系統(tǒng)識別接收到的視頻信息，進(jìn)行圖像處理。圖像處理器基于nvidia?jetson?tx2平臺進(jìn)行搭建，deep?learning中的gan算法對圖像進(jìn)行增強(qiáng)，選用roberts算子模板進(jìn)行邊緣檢測。通過dqn網(wǎng)絡(luò)對路徑進(jìn)行規(guī)劃，最后通過自主定位導(dǎo)航系統(tǒng)將路徑規(guī)劃推薦給用戶。

118、在上述技術(shù)方案中，本發(fā)明提供的技術(shù)效果和優(yōu)點：

119、1、本發(fā)明通過將圖像分析應(yīng)用于室內(nèi)慣性導(dǎo)航系統(tǒng)中，提高對于陌生環(huán)境的適應(yīng)性，提高室內(nèi)導(dǎo)航定位的精度。

120、2、本發(fā)明通過提出了一個實時同步定位、建圖和著色框架，一種基于雙目圖像分析與緊耦合slam融合的導(dǎo)航定位系統(tǒng)，解決了基于lidar、慣性和視覺測量的緊耦合融合的實時同步定位、3d建圖和地圖渲染問題，提高系統(tǒng)的魯棒性和準(zhǔn)確性，避免了弱gnss信號下定位不準(zhǔn)，誤差較大的問題，解決了單一傳感器數(shù)據(jù)捕獲和處理精度不高的問題。