本發(fā)明涉及導(dǎo)航定位，特別是涉及一種不依賴gnss的在濃煙或大霧環(huán)境下導(dǎo)航和定位的方法。

背景技術(shù)：

1、導(dǎo)航定位技術(shù)在lbs應(yīng)用、自主機(jī)器人、無人駕駛、增強(qiáng)現(xiàn)實(shí)等領(lǐng)域獲得了越來越廣泛的應(yīng)用。在現(xiàn)實(shí)生活中，人們利用該技術(shù)來引導(dǎo)飛機(jī)、車輛、船舶或者個(gè)人乃至機(jī)器人安全、準(zhǔn)確地抵達(dá)目的地。隨著近年來機(jī)器人和人工智能技術(shù)的發(fā)展，越來越多的移動(dòng)機(jī)器人、無人機(jī)、智能小車也需要用到精確導(dǎo)航和定位技術(shù)。而為了實(shí)現(xiàn)精確的導(dǎo)航和定位，使用最廣泛也是最有效的方法就是采用gnss衛(wèi)星定位技術(shù)。目前比較常見的有美國的gps、歐洲的伽利略、俄羅斯的glonass和我國的北斗。移動(dòng)物體上安裝衛(wèi)星定位模塊，不間斷和太空中的衛(wèi)星發(fā)生聯(lián)系，即可準(zhǔn)確地獲取自身的精確位置信息和運(yùn)動(dòng)信息。目前，民用衛(wèi)星定位技術(shù)精度通常在10米左右，付費(fèi)或軍用的定位精度可以達(dá)到一米級(jí)。但是在某些應(yīng)用場(chǎng)景，人們需要更高的定位精度。為了進(jìn)一步提高定位精度，可以采用激光雷達(dá)、視覺相機(jī)、陀螺儀等多種傳感器輔助配合，這樣可以使定位精度達(dá)到厘米級(jí)。

2、現(xiàn)有定位技術(shù)嚴(yán)重依賴gnss衛(wèi)星定位，而在實(shí)際應(yīng)用中，很多場(chǎng)景下是沒有衛(wèi)星定位信號(hào)的，比如隧道中、工業(yè)管道內(nèi)、礦井下、大型建筑物內(nèi)部、橋梁底部等，這些場(chǎng)景下的定位精度就會(huì)大幅度降低以至于無法滿足基本的使用要求。為了解決這個(gè)問題，近年來人們開始采用激光雷達(dá)、視覺相機(jī)、陀螺儀等硬件傳感器輔助定位和導(dǎo)航，取得了很不錯(cuò)的效果。但是，在某些場(chǎng)景下，即使加上了上述的各種傳感器，依然無法取得令人滿意的定位和導(dǎo)航效果。最典型的就是濃煙或大霧環(huán)境下。在實(shí)際生活中，我們會(huì)經(jīng)常遇到這種情況。比如在建筑內(nèi)部，發(fā)生了火災(zāi)而產(chǎn)生大量濃煙，由于在建筑物內(nèi)部，因此gnss衛(wèi)星定位信號(hào)幾乎沒有，視覺相機(jī)完全被濃煙遮蔽，而煙霧顆粒大小接近激光的波長，會(huì)對(duì)激光造成大量反射和折射，導(dǎo)致激光雷達(dá)效果大打折扣。這時(shí)候精確的定位和導(dǎo)航就是一個(gè)難題。再比如，大霧天氣穿過一個(gè)很長的隧道，在充滿大量粉塵的環(huán)境下進(jìn)入礦井，都會(huì)遇到同樣的問題。

3、因此，本領(lǐng)域亟需一種能夠在沒有g(shù)nss衛(wèi)星定位信號(hào)的情況下，即使環(huán)境中充滿濃煙、大霧或者粉塵的情況下，依然可以準(zhǔn)確定位和導(dǎo)航的技術(shù)方案。

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

1、本發(fā)明的目的是提供一種能夠在濃煙、大霧或者粉塵的環(huán)境中進(jìn)行定位和導(dǎo)航的技術(shù)方案。

2、為實(shí)現(xiàn)上述目的，本發(fā)明提供了如下方案：

3、一種不依賴gnss的在濃煙或大霧環(huán)境下導(dǎo)航和定位的方法，包括：

4、讀取全球?qū)Ш叫l(wèi)星系統(tǒng)、慣性測(cè)量單元、激光雷達(dá)、可見光/紅外攝像頭的原始數(shù)據(jù)，根據(jù)實(shí)時(shí)的計(jì)算需求，選取此時(shí)此刻有用的若干項(xiàng)所述原始數(shù)據(jù)進(jìn)行濾波整合，得到一個(gè)最終目標(biāo)數(shù)值；

5、將激光慣性里程計(jì)、慣性測(cè)量單元和全球?qū)Ш叫l(wèi)星系統(tǒng)采集的數(shù)據(jù)進(jìn)行融合；

6、對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)、激光雷達(dá)、慣性測(cè)量單元以及微型計(jì)算機(jī)做硬件層面的時(shí)間同步；

7、通過濾波算法將運(yùn)動(dòng)的煙霧、粉塵、霧氣顆粒所產(chǎn)生的激光反射過濾掉，從而產(chǎn)生清晰的激光成像，再配合陀螺儀，就能夠感知周圍的環(huán)境元素。

8、可選的，所述對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)、激光雷達(dá)、慣性測(cè)量單元以及微型計(jì)算機(jī)做硬件層面的時(shí)間同步包括：

9、進(jìn)行全球?qū)Ш叫l(wèi)星系統(tǒng)和微型計(jì)算機(jī)的時(shí)鐘同步操作：當(dāng)有條件接收gnss信號(hào)時(shí)，將gnss接收器作為整個(gè)系統(tǒng)的時(shí)鐘源；

10、衛(wèi)星定位芯片在gnss定位成功時(shí)輸出秒脈沖信號(hào)，其上升沿與時(shí)鐘整秒起始時(shí)刻對(duì)齊，將秒脈沖信號(hào)接入計(jì)算機(jī)模組的gpio接口，令計(jì)算機(jī)上的linux系統(tǒng)的時(shí)鐘與此秒脈沖信號(hào)對(duì)齊，開啟linux內(nèi)核的pps-gpio模塊，調(diào)用內(nèi)核函數(shù)來記錄指定gpio接口上電平發(fā)生變化的時(shí)刻；

11、進(jìn)行激光雷達(dá)和慣性測(cè)量單元的時(shí)鐘同步；

12、將同步后的時(shí)鐘作為一個(gè)整體，該整體通過網(wǎng)線與計(jì)算機(jī)相連，其時(shí)鐘通過精確時(shí)鐘同步協(xié)議向計(jì)算機(jī)對(duì)齊，借助于二者網(wǎng)口所支持的硬件時(shí)鐘，ptp協(xié)議可以實(shí)現(xiàn)亞毫秒量級(jí)的同步。

13、可選的，所述濾波算法具體為：

14、選擇固定于imu的參考系作為機(jī)器人的隨體坐標(biāo)，記為b系；將固定于地球、初始時(shí)刻原點(diǎn)與b系重合且w方向指向地球質(zhì)心的參考系稱為局域世界系，記為w系；將gnss系統(tǒng)中定義的原點(diǎn)位于地球質(zhì)心、相對(duì)于地球靜止的ecef參照系記為e系，將原點(diǎn)位于地球質(zhì)心、相對(duì)恒星靜止的eci參照系記為e系；將原點(diǎn)與w系重合，三軸分別指向正東、正北和地球質(zhì)心相反方向的坐標(biāo)系稱為n系；

15、為保持計(jì)算量可控，采用固定大小的滑動(dòng)窗口的方式，僅對(duì)最近一系列時(shí)刻的系統(tǒng)狀態(tài)進(jìn)行優(yōu)化；將系統(tǒng)狀態(tài)量描述為：

16、

17、δt＝[δtg,δtr,δte,δtc]

18、

19、其中，滑動(dòng)窗口的大小為n，xi描述某處于滑動(dòng)窗口內(nèi)的ti時(shí)刻系統(tǒng)的狀態(tài)，包括b系相對(duì)狀態(tài)即w系的位移轉(zhuǎn)動(dòng)速度imu加速度和角速度的零點(diǎn)偏差值ba?andbg，gnss接收器的時(shí)鐘相對(duì)各個(gè)衛(wèi)星系統(tǒng)的偏差值δt＝[δtg,δtr,δte,δtc]以及漂移量下角標(biāo)g,r,e,c分別代表了4大導(dǎo)航衛(wèi)星系統(tǒng)gps、glonss、galileo和北斗；xl,c,r分別代表激光雷達(dá)、攝像頭、gnss接收天線與imu的相對(duì)位姿；

20、沿用最大后驗(yàn)概率估計(jì)方法，將根據(jù)既有觀測(cè)數(shù)據(jù)估計(jì)系統(tǒng)狀態(tài)量x的最優(yōu)值；

21、

22、其中先驗(yàn)項(xiàng){rp，hp}囊括了當(dāng)前時(shí)間窗口之前系統(tǒng)的狀態(tài)，代表各種不同傳感器的觀測(cè)數(shù)據(jù)(上標(biāo)^標(biāo)記觀測(cè)量)，r(·)表示傳感器的觀測(cè)值與估計(jì)值之間的殘差函數(shù)，||·||∑表示mahalanobis范數(shù)。

23、可選的，在融合全球?qū)Ш叫l(wèi)星系統(tǒng)采集的數(shù)據(jù)時(shí)采用緊耦合的方式，通過直接的偽距測(cè)量數(shù)據(jù)構(gòu)造殘差因子，將其與激光雷達(dá)和慣性測(cè)量單元測(cè)量所得的殘差因子放在同一框架下進(jìn)行優(yōu)化。

24、可選的，還包括構(gòu)建慣性殘差因子，

25、慣性測(cè)量單元的測(cè)量頻率遠(yuǎn)高于激光雷達(dá)和相機(jī)的幀率，采用預(yù)積分的方法，定義從ti時(shí)刻到ti+1時(shí)刻的時(shí)間間隔內(nèi)的積分量

26、

27、這里轉(zhuǎn)動(dòng)根據(jù)語境分別用四元數(shù)形式q或與之相應(yīng)的轉(zhuǎn)動(dòng)矩陣r表示；從ti時(shí)刻到ti+1時(shí)刻系統(tǒng)位姿的變化用預(yù)積分量可以表示成

28、

29、由此定義慣性殘差因子為

30、

31、可選的，還包括構(gòu)建激光雷達(dá)面特征殘差因子，激光雷達(dá)將一個(gè)時(shí)間段內(nèi)所獲得的反射光點(diǎn)聚合成點(diǎn)云，作為一個(gè)數(shù)據(jù)單元返回，激光雷達(dá)自身運(yùn)動(dòng)會(huì)導(dǎo)致點(diǎn)云的畸變，為此需要對(duì)激光點(diǎn)云進(jìn)行畸變補(bǔ)償，記第i幀點(diǎn)云fi開始時(shí)刻激光雷達(dá)的位姿為而在采樣第j個(gè)光點(diǎn)時(shí)雷達(dá)的位姿為獲得的光點(diǎn)的坐標(biāo)為那么將投影到fi開始時(shí)刻雷達(dá)坐標(biāo)系中，能夠得到

32、

33、fi中所有光點(diǎn)都是在同一時(shí)刻采集，其中雷達(dá)的位姿通過imu積分并結(jié)合外參近似估計(jì)；

34、在畸變補(bǔ)償?shù)幕A(chǔ)上，沿用loam4中提取面特征的方法來構(gòu)造激光雷達(dá)部分的殘差因子，為平衡系統(tǒng)穩(wěn)定性和計(jì)算量，將位姿的變化或時(shí)間間隔超過一定閾值的幀加入滑動(dòng)窗口，這一集合記為{fi}i∈[0,n]，而fi-1與fi之間的非關(guān)鍵幀聚合為臨時(shí)地圖mi，對(duì)fn中的一個(gè)光點(diǎn)借助通過imu積分獲得的位姿估計(jì)和對(duì)fi幀所對(duì)應(yīng)的位姿估計(jì)以及外參可以將其坐標(biāo)投影到fi對(duì)應(yīng)的參照系中

35、

36、在臨時(shí)地圖mi中尋找與相近的若干光點(diǎn)，判斷它們是否構(gòu)成一個(gè)平面，如果是，記其平面參數(shù)為[n，d]，即對(duì)此平面上的點(diǎn)p，等式

37、ntp+d＝0。

38、成立，而其中n的含義為平面的法向量并且||n||＝1，由此可以構(gòu)造殘差因子

39、

40、可選的，還包括構(gòu)建gnss偽距和時(shí)鐘殘差因子；gnss接收器通過偽隨機(jī)噪聲信號(hào)的偏移量來計(jì)算電磁波從衛(wèi)星到達(dá)接受接收器的飛行時(shí)間，乘以光速c，來推算接收器與衛(wèi)星之間的距離，但這樣計(jì)算所得只能稱為“偽距”，它與真實(shí)的距離的關(guān)系近似描述為

41、

42、其中ζs是選擇向量，在δt＝[δtg,δtr,δte,δtc]中選擇與當(dāng)前衛(wèi)星相對(duì)應(yīng)的時(shí)間偏移量，δts代表衛(wèi)星時(shí)鐘誤差，ts,r和is,r分別代表大氣對(duì)流層和電離層對(duì)電磁波傳播的延遲，∈s,r是測(cè)量噪聲，當(dāng)衛(wèi)星信號(hào)受到地面障礙物的反射而發(fā)生延遲時(shí)，借助激光點(diǎn)云構(gòu)建的地圖，剔除傳播路徑上受到障礙物影響的衛(wèi)星信號(hào)，從而排除多徑效應(yīng)的影響；

43、在tk時(shí)刻，gnss接收器在w系中的坐標(biāo)可由結(jié)合外參獲得

44、

45、假設(shè)w系與enu參照系即n系的原點(diǎn)重合，它們之間的變換可以由旋轉(zhuǎn)矩陣表示，可以在系統(tǒng)初始化時(shí)進(jìn)行標(biāo)定，從n系到ecef參照系即e系的轉(zhuǎn)換參數(shù)可由起始點(diǎn)(即w系和n系原點(diǎn))處的緯度φ、精度λ、高度h計(jì)算得到

46、

47、其中a是地球長軸，e是離心率，最后是由e系到e系的轉(zhuǎn)換，設(shè)定在接收器接受信號(hào)的時(shí)刻e系和e系重合，即由于地球在電磁波傳播過程中的轉(zhuǎn)動(dòng)，衛(wèi)星發(fā)射信號(hào)時(shí)的ecef參照系e′系接與e系不重合，它們之間的關(guān)系可由轉(zhuǎn)動(dòng)矩陣rz(-wetf)描述，其中z標(biāo)記地球自轉(zhuǎn)軸，we是自轉(zhuǎn)角速度，將衛(wèi)星發(fā)射信號(hào)時(shí)在e系中的坐標(biāo)表示為

48、

49、其中由其廣播的偽隨機(jī)碼和星歷信息獲得，構(gòu)造偽距測(cè)量的殘差因子為

50、此外還需要考慮gnss接收器時(shí)鐘漂移的影響，考慮從tk-1時(shí)刻到tk時(shí)刻時(shí)鐘偏移量的演化

51、

52、構(gòu)造時(shí)鐘殘差因子

53、

54、本發(fā)明與現(xiàn)有技術(shù)相比，具有以下有益效果：

55、本發(fā)明提供了一種不依賴gnss而實(shí)現(xiàn)在濃煙或大霧環(huán)境下精確導(dǎo)航和定位的方法，包括：讀取全球?qū)Ш叫l(wèi)星系統(tǒng)、慣性測(cè)量單元、激光雷達(dá)、可見光/紅外攝像頭的原始數(shù)據(jù)，根據(jù)實(shí)時(shí)的計(jì)算需求，選取此時(shí)此刻有用的若干項(xiàng)所述原始數(shù)據(jù)進(jìn)行濾波整合，得到一個(gè)最終目標(biāo)數(shù)值；將激光慣性里程計(jì)、慣性測(cè)量單元和全球?qū)Ш叫l(wèi)星系統(tǒng)采集的數(shù)據(jù)進(jìn)行融合；對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)、激光雷達(dá)、慣性測(cè)量單元以及微型計(jì)算機(jī)做硬件層面的時(shí)間同步。本發(fā)明有效地解決了在gnss信號(hào)丟失的情況下，濃煙或者大霧環(huán)境中的定位和導(dǎo)航問題；解決了gnss信號(hào)時(shí)有時(shí)無的情況下的定位和導(dǎo)航問題，解決了激光在大霧和濃煙環(huán)境中的成像問題。