本發(fā)明涉及定位導航，尤其涉及一種基于因子圖優(yōu)化的rtk/ins組合定位方法。

背景技術(shù)：

1、現(xiàn)有的定位技術(shù)可利用多種傳感器的測量信息來估計幀間運動，包括視覺傳感器、激光雷達傳感器、gnss（global?navigation?satellite?system?,?全球?qū)Ш叫l(wèi)星系統(tǒng)）以及imu（inertial?measurement?unit?,慣性測量單元）等。

2、gnss的rtk(real?time?kinematic，實時動態(tài))載波相位差分技術(shù)充分利用流動站和參考站的觀察數(shù)據(jù)構(gòu)造雙差觀測值，可大幅削弱或消除部分誤差，且雙差觀測值包含的誤差較小，具備把雙差觀測值解出的實數(shù)解恢復為整數(shù)解的能力，定位精度可達厘米級，定位精度高。但是，gnss的接收機至少需要四顆衛(wèi)星才能完成定位，且gnss信號受環(huán)境干擾的特點易在城市、峽谷、隧道以及立交橋等存在遮擋的環(huán)境中被放大，易導致gnss接收機出現(xiàn)失鎖現(xiàn)象，會影響導航精度，嚴重時會出現(xiàn)無法導航的問題。另外，市面上還存在另一種ins(inertial?navigation?system，慣性導航系統(tǒng))，具備獨立推算位置、速度和姿態(tài)的能力，其信號不易受到外界環(huán)境干擾，具有短時高精度的特點，且數(shù)據(jù)更新率高，可提供豐富的姿態(tài)信息，其運用也十分廣泛。但是，實際使用時，由于ins的定位推算為一個積分的過程，其誤差會隨著時間的變化而累計，即使用時間越長其定位精度越差，同時，在使用時，需要先對其提供初始位置、速度和姿態(tài)，通常用在短時定位需求中。因此，行業(yè)會采用rtk/ins組合導航系統(tǒng)來實現(xiàn)rtk與ins的優(yōu)勢融合、劣勢互補，以提升導航系統(tǒng)的精度，為用戶提供更精確、連續(xù)且可靠的導航定位信息。

3、gnss/ins融合的關(guān)鍵技術(shù)在于單歷元使用卡爾曼濾波，通過系統(tǒng)執(zhí)行完好性監(jiān)測剔除粗差的做法規(guī)避質(zhì)量差的gnss更新帶來的位置誤差發(fā)散的風險。但只純粹依賴當前歷元imu輸出計算導航解增量，存在前后時刻軌跡信息利用不足的問題。而引入額外的傳感器進行輔助，則引出成本增加，以及適用性、使用范圍變窄等問題?，F(xiàn)有技術(shù)中基于滑動窗平滑方法的rtk/ins緊耦合算法研究往往假設(shè)衛(wèi)星可見性保持不變，或者只在雙差載波觀測充足且模糊度有固定解的區(qū)間做后處理融合，這不能滿足需要一定實時性的長距離高精度定位需求。

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

1、本發(fā)明針對現(xiàn)有技術(shù)中存在的技術(shù)問題，提供一種基于因子圖優(yōu)化的rtk/ins組合定位方法，對移出窗口的狀態(tài)采用舒爾補對其邊緣化，保留了移出狀態(tài)的節(jié)點的先驗信息，保證了浮點模糊度的精度的同時降低了計算成本，增加了rtk/ins組合定位結(jié)果的成功率與可靠性。

2、根據(jù)本發(fā)明的第一方面，提供了一種基于因子圖優(yōu)化的rtk/ins組合定位方法，包括：

3、步驟1，設(shè)置滑動窗口的長度，基于滑動窗口內(nèi)的imu數(shù)據(jù)和gnss數(shù)據(jù)構(gòu)建各節(jié)點時刻的模糊度參數(shù)的狀態(tài)向量；

4、步驟2，對移出所述滑動窗口的模糊度參數(shù)的狀態(tài)進行邊緣化處理后，結(jié)合先驗信息得到邊緣化的先驗信息；

5、步驟3，基于所述狀態(tài)向量構(gòu)建gnss因子和imu預積分因子，基于所述邊緣化的先驗信息、gnss因子和imu預積分因子構(gòu)建模糊度浮點解的目標函數(shù)，基于所述目標函數(shù)計算得到的模糊度進行定位。

6、在上述技術(shù)方案的基礎(chǔ)上，本發(fā)明還可以作出如下改進。

7、可選的，所述狀態(tài)向量為：

8、；

9、式中；

10、其中，n表示滑動窗口的長度，和分別為第n個歷元的狀態(tài)節(jié)點在系下的三維位置和速度；為第n個歷元的載體姿態(tài)；為第n個歷元的加表零偏；為第n個歷元的陀螺零偏；t為矩陣轉(zhuǎn)置為第m個站星雙差模糊度。

11、可選的，所述步驟2中采用舒爾補定理對移出所述滑動窗口的模糊度參數(shù)的狀態(tài)進行邊緣化。

12、可選的，所述步驟2中得到的邊緣化的先驗信息為；

13、其中，為邊緣化殘差，為雅克比矩陣，為狀態(tài)向量。

14、可選的，所述gnss因子包括：設(shè)定頻率下的共視衛(wèi)星的雙差偽距因子和雙差載波因子；

15、所述雙差偽距因子和所述雙差載波因子；所述雙差偽距因子和雙差載波因子為：

16、；

17、式中，為待優(yōu)化的狀態(tài)向量，f為頻率，和為共視衛(wèi)星，和分別表示頻率上衛(wèi)星與參考衛(wèi)星的雙差偽距和載波觀測值；表示利用接收機近似坐標計算的雙差衛(wèi)地距；為雙差模糊度。

18、可選的，所述imu預積分因子為：

19、；

20、式中，；

21、；

22、其中，為imu相關(guān)變量的先驗信息；為待優(yōu)化的狀態(tài)向量；和分別為重力與哥氏力項分別對位置和速度的預積分；為重力向量在w系中的投用；為地球自轉(zhuǎn)向量在w系中的投影；、和分別為位置、速度和姿態(tài)的預計分項；為加表零偏；為陀螺零偏；、和分別為w系下的位置、速度和姿態(tài)；為b系向w系的轉(zhuǎn)換四元數(shù)；為b系向w系的轉(zhuǎn)換矩陣；k為歷元；為相鄰歷元的時間間隔；為時間間隔；為k-1歷元到k歷元世界坐標系轉(zhuǎn)換的四元數(shù)；為b系到w系的轉(zhuǎn)換四元數(shù)。

23、可選的，所述目標函數(shù)為：

24、；

25、式中，為待優(yōu)化的狀態(tài)向量，為邊緣化的先驗信息，為先驗信息，為雅克比矩陣；為imu預積分，k為歷元，為imu相關(guān)變量的先驗信息；和分別為頻率上的雙差偽距因子和雙差載波因子。

26、根據(jù)本發(fā)明的第二方面，提供一種基于因子圖優(yōu)化的rtk/ins組合定位系統(tǒng)，包括：狀態(tài)向量構(gòu)建模塊、邊緣化因子構(gòu)建單元和定位單元；

27、所述狀態(tài)向量構(gòu)建模塊，用于設(shè)置滑動窗口的長度，基于滑動窗口內(nèi)的imu數(shù)據(jù)和gnss數(shù)據(jù)構(gòu)建各節(jié)點時刻的模糊度參數(shù)的狀態(tài)向量；

28、所述邊緣化因子構(gòu)建單元，用于對移出所述滑動窗口的模糊度參數(shù)的狀態(tài)進行邊緣化處理后，結(jié)合先驗信息得到邊緣化的先驗信息；

29、所述定位單元，用于基于所述狀態(tài)向量構(gòu)建gnss因子和imu預積分因子，基于所述邊緣化的先驗信息、gnss因子和imu預積分因子構(gòu)建模糊度浮點解的目標函數(shù)，基于所述目標函數(shù)計算得到的模糊度進行定位。

30、根據(jù)本發(fā)明的第三方面，提供了一種電子設(shè)備，包括存儲器、處理器，所述處理器用于執(zhí)行存儲器中存儲的計算機管理類程序時實現(xiàn)基于因子圖優(yōu)化的rtk/ins組合定位方法的步驟。

31、根據(jù)本發(fā)明的第四方面，提供了一種計算機可讀存儲介質(zhì)，其上存儲有計算機管理類程序，所述計算機管理類程序被處理器執(zhí)行時實現(xiàn)基于因子圖優(yōu)化的rtk/ins組合定位方法的步驟。

32、本發(fā)明提供的一種基于因子圖優(yōu)化的rtk/ins組合定位方法、系統(tǒng)、電子設(shè)備及存儲介質(zhì)，對移出窗口的狀態(tài)采用舒爾補對其邊緣化，保留了移出狀態(tài)的節(jié)點的先驗信息，保證了浮點模糊度的精度的同時降低了計算成本，增加了ar成功率與可靠性；邊緣化的過程中使用fgo進行全局優(yōu)化，在每次迭代過程中都會重新進行線性化，一定程度上減小了初始近似位置不準確帶來的非線性問題。