本發(fā)明涉及數(shù)據(jù)處理、機(jī)器人定位，具體為基于偏差濾波的加權(quán)融合農(nóng)業(yè)機(jī)器人定位方法和系統(tǒng)，分類(lèi)號(hào)：g01c21/28。

背景技術(shù)：

1、隨著現(xiàn)代農(nóng)業(yè)的發(fā)展，農(nóng)業(yè)機(jī)器人在農(nóng)田管理中的應(yīng)用越來(lái)越廣泛。這些機(jī)器人在田間作業(yè)時(shí)，需要高精度和高穩(wěn)定的定位系統(tǒng)來(lái)確保其導(dǎo)航和操作的準(zhǔn)確性。目前，常用的定位技術(shù)包括實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)（rtk）和慣性測(cè)量單元（imu）。rtk通過(guò)接收衛(wèi)星信號(hào)，能夠提供高精度的地理位置信息，其定位誤差不會(huì)隨時(shí)間累積，但rtk系統(tǒng)嚴(yán)重依賴(lài)衛(wèi)星信號(hào)。當(dāng)農(nóng)業(yè)機(jī)器人在樹(shù)蔭下、建筑物旁或其他遮擋環(huán)境中作業(yè)時(shí)，rtk信號(hào)容易受到干擾或中斷，導(dǎo)致定位精度下降；而imu通過(guò)測(cè)量加速度和角速度，能夠提供機(jī)器人的姿態(tài)和運(yùn)動(dòng)信息，其抗干擾能力強(qiáng)，但由于傳感器固有的噪聲和漂移，長(zhǎng)時(shí)間使用會(huì)導(dǎo)致累積誤差，使定位信息逐漸失真。所以?xún)H依靠單獨(dú)的rtk或imu難以同時(shí)滿(mǎn)足高精度定位和抗干擾。

2、目前，也有一些研究和應(yīng)用嘗試將rtk和imu結(jié)合使用，以期彌補(bǔ)各自的不足。然而，現(xiàn)有的融合方法在實(shí)際應(yīng)用中仍存在諸多問(wèn)題。比如復(fù)雜度較高，計(jì)算負(fù)荷較大，難以在資源有限的農(nóng)業(yè)機(jī)器人上高效運(yùn)行。在動(dòng)態(tài)環(huán)境中的表現(xiàn)仍不理想，尤其在衛(wèi)星信號(hào)突然中斷或嚴(yán)重干擾時(shí)，不能及時(shí)調(diào)整定位策略，導(dǎo)致機(jī)器人無(wú)法穩(wěn)定工作。在實(shí)時(shí)性要求高的農(nóng)業(yè)作業(yè)中，現(xiàn)有融合方法的響應(yīng)速度和魯棒性不夠，難以應(yīng)對(duì)突發(fā)的環(huán)境變化和信號(hào)波動(dòng)。

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

1、本發(fā)明提供了一種基于偏差濾波的加權(quán)融合農(nóng)業(yè)機(jī)器人定位方法和系統(tǒng)。

2、本發(fā)明技術(shù)方案如下：

3、一種基于偏差濾波的加權(quán)融合農(nóng)業(yè)機(jī)器人定位方法，包括以下步驟：

4、s1、基于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元分別獲取不同時(shí)刻農(nóng)業(yè)機(jī)器人位置、運(yùn)動(dòng)姿態(tài)的實(shí)際測(cè)量值，對(duì)應(yīng)得到實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)；

5、s2、將獲取的實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)輸入動(dòng)力學(xué)模型，分別獲得當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)；

6、基于上一時(shí)刻狀態(tài)協(xié)方差矩陣、狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣和過(guò)程噪聲的協(xié)方差矩陣，對(duì)應(yīng)獲得當(dāng)前時(shí)刻的預(yù)測(cè)協(xié)方差矩陣；

7、s3、將實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)分別輸入動(dòng)力學(xué)模型，分別獲得觀(guān)測(cè)值；計(jì)算觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，使用滑動(dòng)窗口對(duì)歷史時(shí)刻進(jìn)行遍歷，得到若干個(gè)差值后求均值，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的偏差均值；

8、基于預(yù)測(cè)協(xié)方差矩陣、狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣和觀(guān)測(cè)噪聲協(xié)方差矩陣，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的卡爾曼增益；

9、基于觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值、偏差均值和卡爾曼增益，對(duì)狀態(tài)先驗(yàn)估計(jì)進(jìn)行修正，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)；

10、基于卡爾曼增益、狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣，對(duì)預(yù)測(cè)協(xié)方差矩陣進(jìn)行調(diào)整，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣；

11、s4、基于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)的差值，引入平滑因子對(duì)上一時(shí)刻的權(quán)重因子進(jìn)行自適應(yīng)調(diào)整，分別得到當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的權(quán)重因子，對(duì)其進(jìn)行歸一化處理，得到融合權(quán)重；

12、基于融合權(quán)重對(duì)實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合狀態(tài)估計(jì)；對(duì)其后驗(yàn)協(xié)方差矩陣進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合協(xié)方差，基于融合狀態(tài)估計(jì)和融合協(xié)方差對(duì)農(nóng)業(yè)機(jī)器人進(jìn)行定位。

13、具體地，基于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)的差值，引入平滑因子對(duì)上一時(shí)刻的權(quán)重因子進(jìn)行自適應(yīng)調(diào)整，分別得到當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的權(quán)重因子，步驟為：

14、如果當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元二者之間狀態(tài)后驗(yàn)估計(jì)的差值大于設(shè)置的調(diào)整閾值，則將上一時(shí)刻平滑因子減去特定的調(diào)整變量；

15、否則，將上一時(shí)刻平滑因子加上特定的調(diào)整變量，得到當(dāng)前時(shí)刻平滑因子；

16、基于當(dāng)前時(shí)刻平滑因子和當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元二者之間狀態(tài)后驗(yàn)估計(jì)的差值對(duì)上一時(shí)刻的權(quán)重因子進(jìn)行自適應(yīng)調(diào)整，分別得到當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的權(quán)重因子、，公式表示為：

17、，

18、其中，是平滑因子的調(diào)整閾值，是當(dāng)前時(shí)刻平滑因子，是上一時(shí)刻平滑因子， β是調(diào)整變量；

19、，

20、其中，是慣性測(cè)量單元在上一時(shí)刻即 k-1時(shí)刻的權(quán)重因子，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)在上一時(shí)刻即 k-1時(shí)刻的權(quán)重因子，是當(dāng)前時(shí)刻平滑因子，是慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的狀態(tài)后驗(yàn)估計(jì)。

21、具體地，所述s3中將實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)分別輸入動(dòng)力學(xué)模型，分別獲得觀(guān)測(cè)值；計(jì)算觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，使用滑動(dòng)窗口對(duì)歷史時(shí)刻進(jìn)行遍歷，得到若干個(gè)差值后求均值，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的偏差均值，

22、對(duì)于慣性測(cè)量單元，公式表示為：

23、，

24、其中，是慣性測(cè)量單元的觀(guān)測(cè)值，是觀(guān)測(cè)方程，是慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)；

25、，

26、其中，是誤差值即觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，是實(shí)際測(cè)量值，是觀(guān)測(cè)值；

27、，

28、其中，是慣性測(cè)量單元的偏差均值， n是滑動(dòng)窗口的大小， i是滑動(dòng)窗口中各個(gè)數(shù)據(jù)點(diǎn)的索引， k是當(dāng)前時(shí)刻，是觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值；

29、對(duì)于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)，公式表示為：

30、，

31、其中，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的觀(guān)測(cè)值，是觀(guān)測(cè)方程，是慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)；

32、，

33、其中，是誤差值即觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，是實(shí)際測(cè)量值，是觀(guān)測(cè)值；

34、，

35、其中，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的偏差均值， n是滑動(dòng)窗口的大小， i是滑動(dòng)窗口中各個(gè)數(shù)據(jù)點(diǎn)的索引， k是當(dāng)前時(shí)刻，是觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值。

36、進(jìn)一步地，所述基于觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值、偏差均值和卡爾曼增益，對(duì)狀態(tài)先驗(yàn)估計(jì)進(jìn)行修正，得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)，具體表示如下：

37、對(duì)于慣性測(cè)量單元：

38、，

39、其中，是慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)，是慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)，是卡爾曼增益，是實(shí)際測(cè)量值，是觀(guān)測(cè)值，是偏差均值；

40、對(duì)于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)：

41、，

42、其中，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的狀態(tài)后驗(yàn)估計(jì)，是實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)，是卡爾曼增益，是實(shí)際測(cè)量值，是觀(guān)測(cè)值，是偏差均值。

43、具體地，所述s4中對(duì)得到的當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的權(quán)重因子進(jìn)行歸一化處理，得到融合權(quán)重，公式表示為：

44、，

45、其中，是慣性測(cè)量單元的融合權(quán)重，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的融合權(quán)重，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的權(quán)重因子，是慣性測(cè)量單元的權(quán)重因子；

46、基于融合權(quán)重對(duì)實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合狀態(tài)估計(jì)，具體為：

47、，

48、其中，是慣性測(cè)量單元的融合權(quán)重，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的融合權(quán)重，是慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的狀態(tài)后驗(yàn)估計(jì)；

49、對(duì)實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合協(xié)方差，具體為：

50、，

51、其中，是慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的后驗(yàn)協(xié)方差矩陣。

52、進(jìn)一步地，所述s2中基于獲取的實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)，將獲取的狀態(tài)數(shù)據(jù)輸入動(dòng)力學(xué)模型，獲得當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)，具體為：

53、將獲取的實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)輸入動(dòng)力學(xué)模型，獲得上一時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)估計(jì)數(shù)據(jù)，基于上一時(shí)刻的狀態(tài)估計(jì)數(shù)據(jù)輸入動(dòng)力學(xué)模型，獲得當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)，公式表示為：

54、對(duì)于慣性測(cè)量單元：

55、，

56、其中，為上一時(shí)刻即k-1時(shí)刻慣性測(cè)量單元的狀態(tài)估計(jì)數(shù)據(jù)，為上一時(shí)刻即k-1時(shí)刻的控制輸入；為根據(jù)k-1時(shí)刻獲得當(dāng)前時(shí)刻的狀態(tài)估計(jì)數(shù)據(jù)即慣性測(cè)量狀態(tài)先驗(yàn)估；

57、對(duì)于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)：

58、，

59、其中，是根據(jù)k-1時(shí)刻獲得當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)，是k-1時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的狀態(tài)估計(jì)數(shù)據(jù)，是k-1時(shí)刻的控制輸入。

60、進(jìn)一步地，所述s2中基于上一時(shí)刻狀態(tài)協(xié)方差矩陣、狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣和過(guò)程噪聲的協(xié)方差矩陣，對(duì)應(yīng)獲得當(dāng)前時(shí)刻的預(yù)測(cè)協(xié)方差矩陣，公式表示為：

61、對(duì)于慣性測(cè)量單元：

62、，

63、其中，是當(dāng)前時(shí)刻慣性測(cè)量單元的預(yù)測(cè)協(xié)方差矩陣，是上一時(shí)刻即k-1時(shí)刻慣性測(cè)量單元的協(xié)方差矩陣，是k-1時(shí)刻慣性測(cè)量單元狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣，是k-1時(shí)刻過(guò)程噪聲的協(xié)方差矩陣；

64、對(duì)于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)：

65、，

66、其中，是當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的預(yù)測(cè)協(xié)方差矩陣，是上一時(shí)刻即k-1時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的協(xié)方差矩陣，是k-1時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣，是k-1時(shí)刻過(guò)程噪聲的協(xié)方差矩陣。

67、具體地，所述s3中基于卡爾曼增益、狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣，對(duì)預(yù)測(cè)協(xié)方差矩陣進(jìn)行調(diào)整，得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣，具體表示如下：

68、對(duì)于慣性測(cè)量單元：

69、，

70、其中，是慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣， i是單位矩陣，是卡爾曼增益，是狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣，是慣性測(cè)量單元的預(yù)測(cè)協(xié)方差矩陣；

71、對(duì)于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)：

72、，

73、其中，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的后驗(yàn)協(xié)方差矩陣， i是單位矩陣，是卡爾曼增益，是狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣，是實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)的預(yù)測(cè)協(xié)方差矩陣。

74、進(jìn)一步地，關(guān)于所述實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的動(dòng)力學(xué)模型表示如下：

75、，

76、，

77、其中，是 k+1時(shí)刻的狀態(tài)數(shù)據(jù)，是 k時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)或慣性測(cè)量單元的狀態(tài)向量，即狀態(tài)數(shù)據(jù)；是 k時(shí)刻的控制輸入，是關(guān)于狀態(tài)數(shù)據(jù)和控制輸入的狀態(tài)方程，是 k時(shí)刻的過(guò)程噪聲；是 k+1時(shí)刻的測(cè)量值，是 k時(shí)刻關(guān)于狀態(tài)數(shù)據(jù)的觀(guān)測(cè)方程，是 k時(shí)刻的測(cè)量噪聲。

78、本發(fā)明還提供了一種基于偏差濾波的加權(quán)融合農(nóng)業(yè)機(jī)器人定位系統(tǒng)，包括：

79、數(shù)據(jù)獲取模塊：用于基于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元分別獲取不同時(shí)刻農(nóng)業(yè)機(jī)器人位置、運(yùn)動(dòng)姿態(tài)的實(shí)際測(cè)量值，對(duì)應(yīng)得到實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)；

80、狀態(tài)預(yù)測(cè)模塊：將獲取的實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)數(shù)據(jù)和慣性測(cè)量狀態(tài)數(shù)據(jù)輸入動(dòng)力學(xué)模型，分別獲得當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)；基于上一時(shí)刻狀態(tài)協(xié)方差矩陣、狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣和過(guò)程噪聲的協(xié)方差矩陣，對(duì)應(yīng)獲得當(dāng)前時(shí)刻的預(yù)測(cè)協(xié)方差矩陣；

81、狀態(tài)更新模塊：用于將實(shí)時(shí)動(dòng)態(tài)差分定位狀態(tài)先驗(yàn)估計(jì)和慣性測(cè)量狀態(tài)先驗(yàn)估計(jì)分別輸入動(dòng)力學(xué)模型，分別獲得觀(guān)測(cè)值；計(jì)算觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，使用滑動(dòng)窗口對(duì)歷史時(shí)刻進(jìn)行遍歷，得到若干個(gè)差值后求均值，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的偏差均值；基于預(yù)測(cè)協(xié)方差矩陣、狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣和觀(guān)測(cè)噪聲協(xié)方差矩陣，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的卡爾曼增益；基于觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值、偏差均值和卡爾曼增益，對(duì)狀態(tài)先驗(yàn)估計(jì)進(jìn)行修正，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)；基于卡爾曼增益、狀態(tài)觀(guān)測(cè)矩陣的雅可比矩陣，對(duì)預(yù)測(cè)協(xié)方差矩陣進(jìn)行調(diào)整，分別得到實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的后驗(yàn)協(xié)方差矩陣；

82、自適應(yīng)加權(quán)融合模塊：用于基于實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)的差值，引入平滑因子對(duì)上一時(shí)刻的權(quán)重因子進(jìn)行自適應(yīng)調(diào)整，分別得到當(dāng)前時(shí)刻實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的權(quán)重因子，對(duì)其進(jìn)行歸一化處理，得到融合權(quán)重；基于融合權(quán)重對(duì)實(shí)時(shí)動(dòng)態(tài)差分定位系統(tǒng)、慣性測(cè)量單元的狀態(tài)后驗(yàn)估計(jì)進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合狀態(tài)估計(jì)；對(duì)其后驗(yàn)協(xié)方差矩陣進(jìn)行自適應(yīng)加權(quán)融合處理，得到融合協(xié)方差，基于融合狀態(tài)估計(jì)和融合協(xié)方差對(duì)農(nóng)業(yè)機(jī)器人進(jìn)行定位。

83、本發(fā)明的有益效果在于：

84、1、本發(fā)明基于觀(guān)測(cè)值和實(shí)際測(cè)量值之間的差值，引入偏差均值，基于引入的偏差均值對(duì)imu和rtk的數(shù)據(jù)分別進(jìn)行處理，使其變得更加平滑并盡可能抑制異常數(shù)據(jù)波動(dòng)，降低擾動(dòng)的影響，然后對(duì)處理后的imu和rtk數(shù)據(jù)進(jìn)行自適應(yīng)加權(quán)融合處理，可以在原有數(shù)據(jù)基礎(chǔ)上提升定位數(shù)據(jù)的穩(wěn)定性和精確性，從而保障農(nóng)業(yè)機(jī)器人能夠持續(xù)、穩(wěn)定地進(jìn)行作業(yè)。

85、2、本發(fā)明提供的基于偏差濾波的加權(quán)融合農(nóng)業(yè)機(jī)器人定位系統(tǒng)具備自適應(yīng)調(diào)整功能，在rtk的衛(wèi)星信號(hào)干擾或中斷時(shí)，可以通過(guò)對(duì)融合權(quán)重的調(diào)整使其比重減小，切換至imu主導(dǎo)的定位方案，保證機(jī)器人的正常運(yùn)行，這種動(dòng)態(tài)調(diào)整機(jī)制使得機(jī)器人在各種復(fù)雜和惡劣環(huán)境下仍能保持高精度定位，減少因信號(hào)波動(dòng)帶來(lái)的作業(yè)中斷風(fēng)險(xiǎn)；即使在信號(hào)受阻的情況下，機(jī)器人依然能夠依靠imu數(shù)據(jù)進(jìn)行高效、準(zhǔn)確的定位，確保作業(yè)連續(xù)性和可靠性。

86、3、本發(fā)明將rtk與imu進(jìn)行結(jié)合，可以獲得關(guān)于農(nóng)業(yè)機(jī)器人的可靠性更好、精度更高、數(shù)據(jù)更新率更快的運(yùn)動(dòng)狀態(tài)數(shù)據(jù)，通過(guò)對(duì)運(yùn)動(dòng)狀態(tài)數(shù)據(jù)的處理，不僅可以實(shí)現(xiàn)農(nóng)業(yè)機(jī)器人的高精度定位，還可以降低傳感器及系統(tǒng)的整體成本，適用于大規(guī)模農(nóng)業(yè)機(jī)器人部署；通過(guò)融合多源傳感器數(shù)據(jù)，本發(fā)明顯著提升了系統(tǒng)的魯棒性和抗干擾能力。