本發(fā)明屬于無人機(jī)器狗的定位導(dǎo)航領(lǐng)域，具體為一種基于奇異值抗差卡爾曼濾波的無人機(jī)器狗rtk定位方法。

背景技術(shù)：

1、無人機(jī)器狗在自動(dòng)化、救援等用場(chǎng)景中，通常需要進(jìn)行動(dòng)區(qū)域建圖和感知定位。slam技術(shù)、激光雷達(dá)、慣性導(dǎo)航、gps/rtk定位等是無人機(jī)器狗常用的定位感知方法。gnss-rtk是一種基于全球定位衛(wèi)星系統(tǒng)的載波相位差分實(shí)時(shí)動(dòng)態(tài)定位技術(shù)。它能夠?qū)崟r(shí)提供動(dòng)態(tài)實(shí)時(shí)厘米級(jí)精度定位。rtk技術(shù)涉及兩個(gè)gnss接收機(jī)，其在無人機(jī)器狗終端應(yīng)用的原理可以概括為以下幾個(gè)步驟：基準(zhǔn)站和機(jī)器狗移動(dòng)站都配備有g(shù)nss接收機(jī)，將基準(zhǔn)站設(shè)置在良好接收、通信環(huán)境的地方固定，且已知其精確三維坐標(biāo)信息，將無人機(jī)器狗移動(dòng)站放入室外應(yīng)用環(huán)境中，機(jī)器狗可以是運(yùn)動(dòng)的也可以是靜止?；鶞?zhǔn)站接收并處理所有可視gnss衛(wèi)星的信號(hào)，計(jì)算出偽距觀測(cè)值和載波相位觀測(cè)值。將觀測(cè)值和本身位置信息通過無線數(shù)據(jù)傳輸鏈路發(fā)送給機(jī)器狗移動(dòng)站。機(jī)器狗移動(dòng)站接收數(shù)據(jù)后，同步觀測(cè)并采集gnss衛(wèi)星的載波相位數(shù)據(jù)，然后通過系統(tǒng)內(nèi)的差分處理，計(jì)算出載波相位整周模糊度。根據(jù)機(jī)器狗移動(dòng)站和基準(zhǔn)站的相關(guān)性，得出機(jī)器狗精準(zhǔn)的東北坐標(biāo)(e、n)和天坐標(biāo)(u)。

2、然而，大部分機(jī)器狗的應(yīng)用場(chǎng)景中，由于機(jī)器狗的設(shè)備空間狹小元器件密集，移動(dòng)站gnss天線安置后會(huì)受到各類器件的電磁干擾。再者機(jī)器狗的戶外應(yīng)用場(chǎng)景通常通信條件較差，遮擋和多路徑干擾會(huì)使天線信號(hào)短暫丟失或波動(dòng)，使基于卡爾曼濾波的rtk算法濾波的回歸性性能顯著降低，出現(xiàn)大量粗差嚴(yán)重影響模糊度固定結(jié)算，并且回歸穩(wěn)定的結(jié)算狀態(tài)消耗大量的時(shí)間，使定位精度大大降低無法實(shí)現(xiàn)高精度定位下降嚴(yán)重最終使機(jī)器狗的區(qū)域建圖和感知定位結(jié)果作廢。綜上所述，如何使在惡劣傳播環(huán)境里，機(jī)器狗gnss接收機(jī)數(shù)據(jù)面臨干擾、丟失或波動(dòng)時(shí)仍能實(shí)現(xiàn)穩(wěn)定rtk結(jié)算，且結(jié)果不產(chǎn)生異常跳變，降低粗差使浮動(dòng)解回歸現(xiàn)實(shí)狀態(tài)，提高模糊度固定概率，提高無人機(jī)器狗rtk定位魯棒性的重要問題。

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

1、為了克服已有技術(shù)的不足，針對(duì)機(jī)器狗進(jìn)行區(qū)域建圖或感知定位工作中，機(jī)器狗gnss接收機(jī)信號(hào)受電磁干擾、信號(hào)遮擋、多路徑效應(yīng)徑等影響導(dǎo)致rtk偽距觀測(cè)值、載波相位觀測(cè)值產(chǎn)生異常跳變或丟失，出現(xiàn)大量粗差，整周模糊度跳變失真難以搜索或無法固定，最終定位解算失敗或精度降低的問題，本發(fā)明提供了一種基于奇異值抗差卡爾曼濾波的無人機(jī)器狗rtk定位方法，該方法將機(jī)器狗接收機(jī)數(shù)據(jù)解算出來的雙差載波和偽距觀測(cè)方程線性化，帶入創(chuàng)新的奇異值抗差爾曼濾波算法中，然后通過模糊度固定算法(mlambda)搜索并固定整周模糊度，最終求得高精度固定解，將結(jié)果輸出到建圖定位數(shù)據(jù)中，實(shí)現(xiàn)了高精度且更加穩(wěn)定抗干擾的魯棒無人機(jī)器狗rtk定位。

2、本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是：

3、一種基于奇異值抗差卡爾曼濾波的無人機(jī)器狗rtk定位方法，所述方法包括以下步驟：

4、步驟一、機(jī)器狗gnss接收機(jī)獲取衛(wèi)星、基準(zhǔn)站數(shù)據(jù)，基準(zhǔn)站gnss接收機(jī)將基準(zhǔn)站坐標(biāo)位置、載波相位和偽距測(cè)量值、星歷輔助數(shù)據(jù)信息通過無線傳輸鏈路發(fā)送給機(jī)器狗接收機(jī)；同時(shí)機(jī)器狗移動(dòng)站根據(jù)基準(zhǔn)站的衛(wèi)星數(shù)據(jù)，同步測(cè)量對(duì)應(yīng)衛(wèi)星數(shù)據(jù)，得到機(jī)器狗gnss接收機(jī)的載波、偽距測(cè)量值；將機(jī)器狗gnss接收機(jī)、基準(zhǔn)站gnss接收的載波、偽距測(cè)量值其進(jìn)行組合形成接收機(jī)觀測(cè)值向量，因?yàn)樾l(wèi)星發(fā)送多種頻率的載波信號(hào)、測(cè)距碼信號(hào)，不同的頻率信息對(duì)應(yīng)不同的載波信號(hào)和測(cè)距碼信號(hào)，可以形成多個(gè)不同頻率的接收機(jī)觀測(cè)值向量，以為l1頻率，對(duì)應(yīng)觀測(cè)值向量為：

5、

6、其中，表示機(jī)器狗gnss接收機(jī)編號(hào)，表示基準(zhǔn)站gnss接收機(jī)編號(hào)，表示共視衛(wèi)星編號(hào)，表示基準(zhǔn)站gnss接收機(jī)r對(duì)共視衛(wèi)星j偽距觀測(cè)值，表示基準(zhǔn)站gnss接收機(jī)r對(duì)共視衛(wèi)星j載波相位觀測(cè)值，同理；

7、步驟二、構(gòu)建雙差觀測(cè)方程，將(1)進(jìn)行調(diào)整，并對(duì)每個(gè)載波相位和偽距測(cè)量值都建立方程，r收到的j的載波測(cè)量值、偽距測(cè)量值有如下觀測(cè)關(guān)系方程：

8、

9、其中，c≈3.0×108m/s表示光速；表示接收機(jī)時(shí)鐘鐘差；表示衛(wèi)星時(shí)鐘鐘差；表示電離層延遲；表示對(duì)流程延遲；表示整周模糊度整數(shù)值；表示偽距觀測(cè)噪聲；表示載波相位觀測(cè)噪聲；表示衛(wèi)星與機(jī)器狗接收機(jī)星地實(shí)際距離；

10、同理對(duì)于u發(fā)送來的載波測(cè)量值、偽距測(cè)量值也建立如(2)所示方程，雙頻l1、l2多衛(wèi)星系統(tǒng)情況下，從共視衛(wèi)星中選擇一顆高度角大于15°且高度角穩(wěn)定最高、信噪比良好的衛(wèi)星i作為參考衛(wèi)星，兩接收機(jī)觀測(cè)衛(wèi)星i也形成和(2)一樣的觀測(cè)方程，兩接收機(jī)對(duì)于i和j分別形成不同的單差觀測(cè)方程，其中對(duì)于衛(wèi)星i的單差方程表示如下：

11、

12、分別將一系列j形成的單差方程和i的單差方程進(jìn)行星間差分稱為雙差，得到兩接收機(jī)的載波相位、偽距雙差觀測(cè)方程，表示如下：

13、

14、其中雙差載波相位觀測(cè)值；雙差偽距觀測(cè)值，雙差模糊度數(shù)值，雙差偽距觀測(cè)噪聲；雙差載波相位觀測(cè)噪聲，表示兩衛(wèi)星與兩接收機(jī)的雙差星地實(shí)際距離；

15、步驟三、奇異值抗差卡爾曼濾波，

16、步驟四，整數(shù)模糊度固定及固定解計(jì)算，將浮點(diǎn)模糊度向量nk固定為整周模糊度向量，并求解整數(shù)非模糊度狀態(tài)向量a，即最終機(jī)器狗接收機(jī)r位置固定解。

17、進(jìn)一步，所述步驟三的過程如下：

18、步驟3.1：觀測(cè)方程線性化，奇異值抗差卡爾曼濾波的系統(tǒng)狀態(tài)預(yù)測(cè)方程和系統(tǒng)測(cè)量方程分別如下：

19、

20、其中，表示歷元時(shí)刻t(k)，第t(k-1)歷元時(shí)刻待估參數(shù)的狀態(tài)向量，第t(k)歷元時(shí)刻先驗(yàn)待估參數(shù)的狀態(tài)向量，第t(k)歷元時(shí)刻后驗(yàn)估計(jì)參數(shù)的狀態(tài)向量，ak測(cè)量狀態(tài)轉(zhuǎn)移矩陣，ωk表示時(shí)刻t(k)的過程噪聲，且滿足均值為零協(xié)方差矩陣為qk的正態(tài)分布，ψk表示t(k)時(shí)刻的測(cè)量噪聲，且滿足均值為零協(xié)方差矩陣為rk的正態(tài)分布；yk表示時(shí)刻t(k)雙差觀測(cè)值向量，由雙頻情況下所有衛(wèi)星的雙差載波相位觀測(cè)值、雙差偽距觀測(cè)值如(4)進(jìn)行整體排列合成，hk表示t(k)時(shí)刻測(cè)量狀態(tài)轉(zhuǎn)換矩陣；

21、yk對(duì)應(yīng)的雙差觀測(cè)方程轉(zhuǎn)化為：

22、

23、根據(jù)yk可得到狀態(tài)向量xk，xk由所求r位置向量er＝(xr,yr,zr)t速度信息向量vr＝(vx,vy,vz)，雙頻l1、l2條件下所有(3)的單差模糊度數(shù)值組成nk向量；

24、狀態(tài)向量xk和yk分別對(duì)應(yīng)ak和hk，狀態(tài)向量xk與yk之間是非線性關(guān)系所以將hk表示為雅可比矩陣形式：

25、

26、在k歷元和上一歷元k-1間隔δt之間，機(jī)器狗gnss移接收機(jī)r運(yùn)動(dòng)必然有位置的轉(zhuǎn)移變換，ak表示為如下形式：

27、

28、其中,表示j個(gè)數(shù)，m＝(6+(n+1)*2)*(6+(n+1)*2)，當(dāng)r處于靜態(tài)時(shí)δt＝0，非靜態(tài)時(shí)δt不變；

29、qk稱為過程噪聲協(xié)方差矩陣，機(jī)器狗gnss接收機(jī)r處于動(dòng)態(tài)模式，那么相應(yīng)的qk由如下形式表示：

30、

31、其中,er表示地心固心坐向地理坐標(biāo)enu的轉(zhuǎn)移矩陣，分別表示水平緯度e方向、經(jīng)度n方向、高度u方向加速度的方差，diag()表示對(duì)角矩陣且對(duì)角線元素分別對(duì)應(yīng)括號(hào)內(nèi)部對(duì)應(yīng)位置元素，當(dāng)機(jī)器狗gnss接收機(jī)r處于靜態(tài)模式時(shí)qv取零矩陣；

32、步驟3.2：測(cè)量誤差協(xié)方差矩陣奇異值抗差調(diào)整，測(cè)量噪聲協(xié)方差矩陣rk的形式如下：

33、

34、其中，rφ,1、rφ,2、rp,1、rp,1由如下計(jì)算得到：

35、

36、其中，表示非參考衛(wèi)星j＝1的載波相位測(cè)量值的標(biāo)準(zhǔn)差，其下角標(biāo)數(shù)字1表示載波頻率為l1；表示非參考衛(wèi)星j＝2的偽距測(cè)量值的標(biāo)準(zhǔn)差，下角標(biāo)數(shù)字1表示載波頻率為l1；

37、對(duì)于rk通過奇異值分解來調(diào)整矩陣，如下：

38、

39、其中σk表示如下：

40、

41、其中，而σk是一個(gè)對(duì)角矩陣，uk和vk是正交矩陣(即它們的列向量是正交單位向量)，特別的σk對(duì)角線上的元素稱為奇異值，這種分解將矩陣rk表示為三個(gè)簡(jiǎn)單矩陣的乘積，便于分析和處理；

42、對(duì)奇異值矩陣σk對(duì)角線上每個(gè)元素取平方根操作；

43、

44、簡(jiǎn)化為：

45、

46、的計(jì)算重組結(jié)果如下：

47、

48、觀測(cè)殘差向量表示為：

49、

50、對(duì)于觀測(cè)殘差向量zk中的第元素zi，將其進(jìn)行線性調(diào)整至合適的范圍，首先計(jì)算出zk中各元素的均值μ：

51、

52、將觀測(cè)殘差向量zk中的每個(gè)元素zi最終線性調(diào)整為得到線性調(diào)整的觀測(cè)殘差向量其中每個(gè)元素表示如下：

53、

54、則對(duì)應(yīng)的自適應(yīng)權(quán)重因子γi的計(jì)算方法如下：

55、

56、其中，k1為經(jīng)驗(yàn)常數(shù)，k2為經(jīng)驗(yàn)常數(shù)；

57、根據(jù)計(jì)算得到的自適應(yīng)降權(quán)因子γi組成的向量γ調(diào)整重組將向量γ復(fù)制并稱為γ'其中對(duì)應(yīng)元素重新命名為γj且同i表示元素序號(hào)，對(duì)于某一歷元k的其第i行和第j列的每個(gè)元素自適應(yīng)處理，得到最終的奇異值抗差重組的測(cè)量誤差協(xié)方差矩陣如下：

58、

59、步驟3.3：奇異值抗差增益計(jì)算及卡爾曼預(yù)測(cè)更新，根據(jù)奇異值抗差調(diào)整的和先驗(yàn)的誤差協(xié)方差矩陣計(jì)算奇異值自適應(yīng)增益結(jié)果如下：

60、

61、計(jì)算k歷元時(shí)刻的先驗(yàn)估計(jì)狀態(tài)向量將上一歷元的濾波后驗(yàn)更新的狀態(tài)向量進(jìn)行如下變換結(jié)果如下：

62、

63、同理計(jì)算先驗(yàn)的誤差協(xié)方差矩陣需要上一時(shí)刻的濾波后驗(yàn)更新的誤差協(xié)方差矩陣結(jié)果如下：

64、

65、后驗(yàn)狀態(tài)向量更新結(jié)果為：

66、

67、后驗(yàn)誤差協(xié)方差矩陣更新結(jié)果為：

68、

69、將k歷元時(shí)刻的后驗(yàn)狀態(tài)向量稱為后驗(yàn)誤差協(xié)方差矩陣儲(chǔ)存。

70、再進(jìn)一步，所述步驟四的過程如下：

71、首先，將經(jīng)過奇異值抗差卡爾曼濾波的進(jìn)行特定的線性化調(diào)整轉(zhuǎn)化為和將調(diào)整后的狀態(tài)向量分解為非模糊度浮點(diǎn)狀態(tài)向量和浮點(diǎn)模糊度解向量進(jìn)一步得到：

72、

73、其中，cov表示計(jì)算對(duì)應(yīng)向量的協(xié)方差矩陣，表示非模糊度浮點(diǎn)狀態(tài)向量的協(xié)方差矩陣，qb表示浮點(diǎn)模糊度協(xié)方差矩陣，表示狀態(tài)向量中非模糊度參數(shù)和模糊度參數(shù)的協(xié)方差矩陣；

74、其次，求解浮點(diǎn)模糊的整數(shù)解，對(duì)qb進(jìn)行l(wèi)d分解得到l表示對(duì)角線元素為1的下三角矩陣且非對(duì)角線元素為非整數(shù)，λ表示對(duì)角矩陣，對(duì)l的非對(duì)角線元素取整數(shù)得到矩陣l1，利用其對(duì)進(jìn)行變換得到為：

75、

76、其中，-t表示矩陣的逆轉(zhuǎn)置或轉(zhuǎn)置的逆；

77、得到后重復(fù)以上ld分解步驟，直至第n次被分解的單位下三角矩陣ln取整且只能取為單位陣，得到最終結(jié)果qz為：

78、

79、取其中稱為浮點(diǎn)雙差相位偏差，將和新得到的下三角矩陣g還有λ帶入mlambda搜索算法中，可得到整數(shù)雙差相位偏差

80、最終，得到整數(shù)模糊度解向量根據(jù)整數(shù)模糊度解向量調(diào)整得到整數(shù)非模糊度參數(shù)狀態(tài)向量過程如下所示：

81、

82、其中，表示整數(shù)非模糊度參數(shù)向量的狀態(tài)協(xié)方差矩陣，表示整數(shù)非模糊度狀態(tài)解也即最終機(jī)器狗位置固定解。

83、本發(fā)明的有益效果主要表現(xiàn)在：提高模糊度固定概率，提高無人機(jī)器狗rtk定位魯棒性。