摘要： 【目的】融合定位設(shè)備大多依賴(lài)于BDS-RTK，而B(niǎo)DS-RTK 功耗大，在網(wǎng)絡(luò)信號(hào)不佳的區(qū)域無(wú)法獲取差分鏈路，只能使用標(biāo)準(zhǔn)單點(diǎn)定位（Standard point positioning，SPP），而SPP 定位系統(tǒng)存在定位誤差大、抗干擾能力弱的問(wèn)題。本研究致力于解決這些問(wèn)題。【方法】提出一種慣性導(dǎo)航系統(tǒng)（Inertial navigation system，INS） 和BDSSPP傳感器信息融合卡爾曼濾波方法，基于該方法開(kāi)發(fā)了低功耗融合定位系統(tǒng)。采用BDS-RTK 作為基準(zhǔn)，測(cè)試了BDS-SPP 的低功耗模塊靜態(tài)和動(dòng)態(tài)的誤差、航姿參考系統(tǒng)（Attitude and heading reference system，AHRS） 零偏和噪聲，同時(shí)進(jìn)行濾波器融合定位試驗(yàn)，檢測(cè)在單天線(xiàn)B D S 受到干擾時(shí)A H R S 的斷點(diǎn)續(xù)航情況?！窘Y(jié)果】BDS 定位的靜態(tài)誤差為0.472 6 m，BDS-SPP/INS 融合定位系統(tǒng)動(dòng)態(tài)平均標(biāo)準(zhǔn)差小于1.913 7 m，相較于融合前減少0.165 2 m。斷點(diǎn)續(xù)航試驗(yàn)結(jié)果表明，融合定位系統(tǒng)偏移距離平均標(biāo)準(zhǔn)差為3.636 5 m，相較于融合前減少了2.590 0 m。BDS-SPP/INS 融合定位系統(tǒng)比BDS-RTK 定位系統(tǒng)功率降低了33.3 W；融合后的輸出頻率較單獨(dú)采用BDS-SPP 情況提高了3 倍?！窘Y(jié)論】本文的融合定位裝置提高了BDS-SPP 的抗干擾能力，減少了定位誤差，可以在缺少RTK 鏈路的情況下為農(nóng)業(yè)機(jī)器人提供定位，可以為農(nóng)業(yè)機(jī)器人導(dǎo)航研究提供了技術(shù)基礎(chǔ)。

關(guān)鍵詞： 農(nóng)業(yè)機(jī)器人；低功耗；BDS；信息融合；Kalman 濾波；INS

中圖分類(lèi)號(hào)： TP273;V249.32 文獻(xiàn)標(biāo)志碼： A 文章編號(hào)： 1001-411X（2024）03-0437-09

隨著城鎮(zhèn)化進(jìn)程加快，進(jìn)城務(wù)工的勞動(dòng)力不斷增加，加之老齡化問(wèn)題日趨嚴(yán)重，“誰(shuí)來(lái)種地”和“如何種地”成為全世界面臨的共同問(wèn)題[1]，無(wú)人農(nóng)場(chǎng)和智慧農(nóng)業(yè)是解決這一問(wèn)題的重要途徑[2-5]。

我國(guó)南方地區(qū)農(nóng)田面積較小且分散，田塊之間的邊界不明顯，大型農(nóng)業(yè)設(shè)備難以在此類(lèi)農(nóng)田環(huán)境中開(kāi)展工作，使用小型無(wú)人駕駛農(nóng)機(jī)是解決此問(wèn)題的有效途徑，所以針對(duì)復(fù)雜的農(nóng)田環(huán)境開(kāi)發(fā)一套小型可靠的導(dǎo)航定位系統(tǒng)極為迫切，其中降低成本和減少功耗是需要解決的關(guān)鍵問(wèn)題[6-7]。

關(guān)于自動(dòng)駕駛技術(shù)對(duì)穩(wěn)定可靠定位系統(tǒng)的要求，國(guó)內(nèi)外研究者進(jìn)行了很多相關(guān)研究[8-11]，莊朝文等[12] 提出了一種利用多傳感器融合技術(shù)來(lái)降低成本的組合導(dǎo)航定位系統(tǒng)，利用定位和航向輸出，并以元件參數(shù)誤差補(bǔ)償校正，運(yùn)用一階馬爾可夫模型建立傳感器參數(shù)誤差模型，該系統(tǒng)融合了Kalman濾波的全球定位系統(tǒng)（Global positioning system，GPS） 和慣性導(dǎo)航系統(tǒng)（Inertial navigation system，INS） 信息，通過(guò)仿真試驗(yàn)驗(yàn)證了算法的可行性。張京等[13] 采用自適應(yīng)Kalman 濾波算法建立了一種綜合濾波模型，融合了INS 和全球?qū)Ш叫l(wèi)星系統(tǒng)（Global navigation satellite system，GNSS），通過(guò)提高GNSS 信號(hào)品質(zhì)以及航向角的變化幅度指引信息融合策略，仿真試驗(yàn)結(jié)果顯示在直線(xiàn)行駛時(shí)融合系統(tǒng)的航向平均誤差為?0.02°，標(biāo)準(zhǔn)差為0.50°；轉(zhuǎn)向行駛?cè)诤舷到y(tǒng)的航向平均誤差為0.62°，標(biāo)準(zhǔn)差為2.42°。王力等[14] 在此基礎(chǔ)上提出了一種自適應(yīng)序貫無(wú)跡卡爾曼濾波（Unscented Kalman filter，UKF）改進(jìn)算法，提高了北斗衛(wèi)星導(dǎo)航系統(tǒng)（Beidou navigationsatellite system， BDS） 與INS 融合后的精度。Shen 等[15] 采用徑向基函數(shù)多層感知機(jī)的容積卡爾曼濾波處理導(dǎo)航中GPS 信號(hào)丟失的問(wèn)題，當(dāng)信號(hào)中斷500 s 時(shí)，平均誤差為23.11 m。Brown 等[ 1 6 ]針對(duì)無(wú)人駕駛車(chē)失鎖問(wèn)題，提出組合 GPS、微機(jī)電系統(tǒng)和慣性導(dǎo)航組件，車(chē)輛在失鎖后20 s 內(nèi)，誤差超過(guò)10 m。魏永超等[ 1 7] 提出了基于DS-SVR 的BDS/INS 組合導(dǎo)航算法，利用Dempster-Shafer（DS） 證據(jù)理論算法融合，在缺失衛(wèi)星信號(hào)時(shí)通過(guò)支持向量回歸機(jī)（Support vector regression，SVR） 消除INS 誤差，試驗(yàn)結(jié)果表明DS-SVR 算法優(yōu)化了導(dǎo)航能力、提高了導(dǎo)航精度。張曉寒等[18] 在農(nóng)機(jī)受環(huán)境因素干擾以致BDS 失調(diào)、INS 不足以導(dǎo)航時(shí)，以BDS/INS 兩者之間的位置和速度差值為模型，利用蜂群算法優(yōu)化BP 神經(jīng)網(wǎng)絡(luò)輔助組合導(dǎo)航，結(jié)果表明誤差小于0.6 m/s，位置誤差小于30 m。

本文針對(duì)旱田農(nóng)機(jī)導(dǎo)航系統(tǒng)[19-23]、水田農(nóng)機(jī)導(dǎo)航系統(tǒng)[24-27] 在復(fù)雜農(nóng)田環(huán)境下的使用需求，研究一種低功耗的定位系統(tǒng)；針對(duì)低功耗的BDS 標(biāo)準(zhǔn)單點(diǎn)定位（BDS-standard point positioning，BDS-SPP）系統(tǒng)輸出頻率低的問(wèn)題，基于運(yùn)動(dòng)模型設(shè)計(jì)了融合INS 的Kalman 濾波器以提高輸出頻率，搭建基于低功耗微控制單元（Microcontroller unit，MCU） 的測(cè)試系統(tǒng)平臺(tái)，設(shè)計(jì)一種適應(yīng)多種復(fù)雜環(huán)境要求的、基于Kalman 濾波器的BDS-SPP/INS 融合定位系統(tǒng)。

1 設(shè)計(jì)運(yùn)動(dòng)學(xué)模型

本試驗(yàn)采用融合BDS-SPP/INS 導(dǎo)航系統(tǒng)以克服信號(hào)短暫丟失的問(wèn)題，張聞?dòng)畹萚28] 指出實(shí)時(shí)動(dòng)態(tài)（Real-time kinematic，RTK） 定位失鎖時(shí)采用此方法在16.65 m 內(nèi)的誤差小于20 cm。本文在此基礎(chǔ)上采用低成本、低精度的BDS-SPP 與航姿參考系統(tǒng)（Attitude and heading reference system，AHRS，INS 的一種傳感器） 進(jìn)行融合，進(jìn)行信息融合前先構(gòu)建一個(gè)機(jī)器人運(yùn)動(dòng)學(xué)模型（圖1）。

式中，v 為質(zhì)點(diǎn)在此時(shí)刻的瞬時(shí)速度，ω為質(zhì)點(diǎn)的瞬時(shí)角速度，θ為質(zhì)點(diǎn)的偏移角度，θ為質(zhì)點(diǎn)在偏移角度變化率，x為質(zhì)點(diǎn)在X 軸投影的速度，y為質(zhì)點(diǎn)在Y 軸投影的速度。

2 Kalman 濾波器

2.1 系統(tǒng)框架

本文設(shè)計(jì)的組合導(dǎo)航系統(tǒng)由B D S - S P P 和INS 組成，BDS-SPP 提供載體的位置和速度信息，INS 提供自身運(yùn)動(dòng)狀態(tài)的姿態(tài)信息，結(jié)構(gòu)如圖2 所示。采用三軸加速度計(jì)、三軸陀螺儀和三軸磁場(chǎng)計(jì)組成的AHRS 作為慣性導(dǎo)航系統(tǒng)，其中包含了嵌入式的姿態(tài)數(shù)據(jù)解算單元與航向信息。

在低功耗融合系統(tǒng)架構(gòu)下，BDS-SPP/INS 定位系統(tǒng)的設(shè)計(jì)涉及定位精度和運(yùn)行時(shí)間之間的權(quán)衡，雖然BDS 可以長(zhǎng)時(shí)間提供穩(wěn)定的定位服務(wù)，但它容易受到干擾，導(dǎo)致定位漂移。相比之下，INS 不依賴(lài)于電磁信號(hào)，可以在短時(shí)間內(nèi)提供高精度定位，但隨著時(shí)間積累其測(cè)量誤差增加。為了解決這些問(wèn)題，將BDS 與INS 融合，可以有效提高定位精度，滿(mǎn)足長(zhǎng)時(shí)間不間斷導(dǎo)航的需求。本文采用Kalman濾波進(jìn)行多傳感器數(shù)據(jù)融合，當(dāng)BDS-SPP 因信號(hào)被遮擋或減弱而無(wú)法提供準(zhǔn)確的定位信息時(shí)，INS 可以暫時(shí)接替BDS-SPP 進(jìn)行導(dǎo)航，但I(xiàn)NS 會(huì)隨著時(shí)間推移產(chǎn)生速度和位置的累計(jì)誤差。因此，只能在較短的時(shí)間或距離內(nèi)使用INS 導(dǎo)航，當(dāng)BDS-SPP 信號(hào)恢復(fù)正常后，對(duì)INS 的誤差進(jìn)行修正，使定位恢復(fù)正常。

2.2 基礎(chǔ)Kalman 濾波器

采用Kalman 濾波融合BDS-SPP 與INS 是組合導(dǎo)航的有效方法[29-30]，通過(guò)建立物理系統(tǒng)模型，同時(shí)利用濾波算法，從帶有誤差的信號(hào)中估計(jì)出置信度更高的信號(hào)，將得到的誤差估計(jì)值對(duì)INS 進(jìn)行修正[31]。

Kalman 濾波算法是一種線(xiàn)性動(dòng)態(tài)系統(tǒng)的最優(yōu)估計(jì)算法。它基于2 個(gè)假設(shè)：信息過(guò)程是基于模型的，由白噪聲激發(fā)；每次測(cè)量信號(hào)都包含附加的白噪聲分量。對(duì)于滿(mǎn)足這些條件的系統(tǒng)，Kalman 濾波器是最優(yōu)的信息融合器。Kalman 濾波算法的流程包括5 個(gè)核心更新迭代公式。

向前推算狀態(tài)變量：

式中，x-k為k時(shí)刻的后驗(yàn)狀態(tài)估計(jì)結(jié)果， xk-1為k-1時(shí)刻的后驗(yàn)狀態(tài)估計(jì)結(jié)果，uk和uk-1分別為k和k-1時(shí)刻的狀態(tài)方程輸入，ax和ay分別為車(chē)輛坐標(biāo)系X和Y軸加速度，A為狀態(tài)轉(zhuǎn)移矩陣，B為將輸入轉(zhuǎn)換為狀態(tài)的矩陣（uk控制矩陣），t為時(shí)間步長(zhǎng)。

向前推算誤差協(xié)方差：

P-k = APk-1AT +Q， （9）

式中，P-k為k 時(shí)刻的先驗(yàn)估計(jì)協(xié)方差，Pk和Pk-1分別為k 和k?1 時(shí)刻的后驗(yàn)估計(jì)協(xié)方差，Q為過(guò)程激勵(lì)噪聲協(xié)方差。

計(jì)算Kalman 增益：

Kk = P-k HT（HP-kHT + R）-1， （10）

式中，Kk為濾波增益矩陣，H為狀態(tài)變量到測(cè)量的轉(zhuǎn)換矩陣，R為測(cè)量噪聲協(xié)方差。

由觀測(cè)變量zk更新估計(jì)：

Pk = （I- KkH） P-k ， （12）

式中，I 為單位矩陣。

基于上述遞推方程和狀態(tài)方程設(shè)計(jì)了Kalman濾波器。公式（4） 是用k-1時(shí)刻系統(tǒng)的后驗(yàn)狀態(tài)估計(jì)值xk-1預(yù)測(cè)k 時(shí)刻系統(tǒng)的狀態(tài)值，同時(shí)用k?1 時(shí)刻的誤差協(xié)方差Pk-1和過(guò)程噪聲Q預(yù)測(cè)k 時(shí)刻的先驗(yàn)誤差協(xié)方差P-k，即公式（9），結(jié)合公式（4）、（9） 組成時(shí)間更新方程；由公式（3） 計(jì)算Kalman 增益，公式（4） 進(jìn)行校正更新，為進(jìn)行下一步估計(jì)k+1時(shí)刻的系統(tǒng)狀態(tài)值的迭代更新操作，更新Pk值得到公式（12），該公式利用Kalman 增益參數(shù)進(jìn)行更新，實(shí)現(xiàn)對(duì)系統(tǒng)狀態(tài)值更準(zhǔn)確的預(yù)測(cè)和估計(jì)。公式（5） 中的x、y、θ和v 由BDS-SPP 定位系統(tǒng)獲得，公式（6） 中的ax、ay和θ 由姿態(tài)傳感器系統(tǒng)AHRS 獲得。通過(guò)前期試驗(yàn)調(diào)整Q和R，使估計(jì)誤差的方差最小以獲得最優(yōu)估計(jì)。

慣性傳感器初始化時(shí)間為15 s，啟動(dòng)設(shè)備時(shí)同步開(kāi)啟BDS 和AHRS 模塊初始化，在所有模塊完成初始化后開(kāi)始融合定位工作。該板卡會(huì)根據(jù)定位質(zhì)量提供標(biāo)志位，在$GPRMC 通信協(xié)議中有一個(gè)定位狀態(tài)標(biāo)志位，“A”表示正常定位狀態(tài)，而“F”則表示受干擾狀態(tài)，狀態(tài)由定位板卡自動(dòng)判斷。

當(dāng)標(biāo)志位為“A”時(shí)，使用公式（14） 中的R 矩陣，正常進(jìn)行融合定位獲得較為精準(zhǔn)的航向信息；當(dāng)標(biāo)志位為“F”時(shí)，Kalman 濾波器根據(jù)標(biāo)志位將R 矩陣切換為測(cè)量噪聲協(xié)方差矩陣R′，如公式（15） 所示，減小衛(wèi)星定位信號(hào)的權(quán)重，相對(duì)增加AHRS 傳感器信號(hào)的權(quán)重；初始航向使用切換前的航向信息，能較好地提高慣性定位精度，實(shí)現(xiàn)2 個(gè)信號(hào)源之間的無(wú)縫銜接。該切換發(fā)生在標(biāo)志位改變的采樣周期（0.1 s）。

3 低功耗融合系統(tǒng)架構(gòu)

3.1 BDS-SPP/INS 融合定位系統(tǒng)設(shè)計(jì)

系統(tǒng)主要包含BDS-SPP 衛(wèi)星標(biāo)準(zhǔn)單點(diǎn)定位模塊和AHRS 慣性傳感器模塊。慣性傳感器模塊采用W T G A H R S 2 - A H R S 提供姿態(tài)信息，WTGAHRS2 是一款高精度慣性導(dǎo)航儀，其慣性傳感器為高精度陀螺儀和加速度計(jì)，姿態(tài)精度可以達(dá)到0.01°。一個(gè)基于STM32F407ZGT6 搭建的低功耗MCU 測(cè)試系統(tǒng)采集BDS-SPP、BDS-RTK 以及融合后的定位數(shù)據(jù)；試驗(yàn)對(duì)照是和芯星通公司生產(chǎn)的高精度地基增強(qiáng)RTK-UM482-BOX2 定位設(shè)備同步測(cè)試的數(shù)據(jù)。此外還有一個(gè)數(shù)據(jù)記錄模塊和電源模塊。設(shè)計(jì)的6 個(gè)模塊組合整體結(jié)構(gòu)如圖3 所示，實(shí)物圖如圖4 所示。本文設(shè)計(jì)的融合定位系統(tǒng)的功率為2.7 W，其中STM32F407ZGT6 模塊為2.0 W，WTGAHRS2-AHRS 模塊為0.4 W，BDS-SPP 模塊為0.3 W，較RTK-UM482-BOX2 定位系統(tǒng)的功率（36.0 W） 降低了33.3 W。

3.2 系統(tǒng)工作流程

系統(tǒng)首先進(jìn)行初始化，包括HAL 坐標(biāo)初始化、時(shí)鐘初始化、延時(shí)初始化、LED 燈初始化、解碼初始化、串口初始化和GPIO 初始化。系統(tǒng)初始化后開(kāi)始對(duì)各個(gè)定位系統(tǒng)進(jìn)行檢測(cè)，判斷系統(tǒng)是否接收到數(shù)據(jù)，若數(shù)據(jù)接收正常，則執(zhí)行下一步解碼，然后將解碼得到的數(shù)據(jù)存儲(chǔ)于SD 卡；如接收數(shù)據(jù)出現(xiàn)異常，在系統(tǒng)判定沒(méi)有接收到數(shù)據(jù)時(shí)系統(tǒng)會(huì)跳過(guò)解碼流程， 執(zhí)行對(duì)下個(gè)導(dǎo)航定位系統(tǒng)的判定，當(dāng)RTK、BDS-SPP/INS 模塊都檢測(cè)完畢，系統(tǒng)將會(huì)檢測(cè)BDS-SPP 系統(tǒng)和INS 系統(tǒng)是否全部接收到信號(hào)，當(dāng)所有模塊均能正常接收到信號(hào)時(shí)，將通過(guò)Kalman 濾波對(duì)BDS-SPP/INS 進(jìn)行數(shù)據(jù)融合；系統(tǒng)將融合后的數(shù)據(jù)存儲(chǔ)于SD 卡，并回到初始化后的檢測(cè)，判定3 個(gè)導(dǎo)航系統(tǒng)是否接收到信號(hào)，循環(huán)執(zhí)行，直至系統(tǒng)關(guān)閉，程序結(jié)束。

4 設(shè)計(jì)試驗(yàn)

4.1 試驗(yàn)方案

試驗(yàn)以和芯星通公司生產(chǎn)的BDS-RTK[ 動(dòng)態(tài)定位精度（15+D×10?6） mm，D 為基站到移動(dòng)站的距離，m] 定位設(shè)備的同步測(cè)試數(shù)據(jù)為參考，采用對(duì)比檢測(cè)方法，選取廣州市天河區(qū)和增城區(qū)2 個(gè)區(qū)域?qū)Φ凸牡腂DS-SPP/INS 融合定位系統(tǒng)的區(qū)域定位性能進(jìn)行測(cè)試和分析。在2 個(gè)區(qū)域，分別進(jìn)行靜態(tài)和動(dòng)態(tài)定位精度測(cè)試，靜態(tài)測(cè)試點(diǎn)不少于3 個(gè)，選擇視野開(kāi)闊地帶進(jìn)行測(cè)試，測(cè)試時(shí)間不少于30min，每個(gè)測(cè)試點(diǎn)測(cè)試時(shí)打開(kāi)接收機(jī)供電，記錄各個(gè)接收機(jī)從開(kāi)始收斂到收斂足夠多的數(shù)據(jù)；動(dòng)態(tài)測(cè)試不少于10 組，每組測(cè)試范圍不小于1 km，測(cè)試平臺(tái)選用雅迪公司生產(chǎn)的電瓶車(chē)，天線(xiàn)穩(wěn)固安裝在電瓶車(chē)車(chē)頭。

I NS 慣性傳感器采用維特智能公司生產(chǎn)的WTGAHRS2-AHRS 提供姿態(tài)信息，為保證精度、減少誤差，將AHRS 面向前方安裝，接收機(jī)必須避開(kāi)金屬物品以及對(duì)天線(xiàn)起電磁干擾的遮擋物，以保證融合定位接收機(jī)數(shù)據(jù)的準(zhǔn)確性。

為驗(yàn)證本研究設(shè)計(jì)系統(tǒng)的抗干擾性，針對(duì)復(fù)雜自然條件下BDS-SPP/INS 融合定位系統(tǒng)可能存在的信號(hào)不好或者信號(hào)丟失情況，進(jìn)行3 組抗干擾試驗(yàn)，試驗(yàn)中采用金屬絕緣材料，將BDS-SPP 的天線(xiàn)接收器覆蓋4～10 s 不等的時(shí)間，隔絕信號(hào)接收，模擬自然環(huán)境下信號(hào)丟失情況，并觀察在丟失信號(hào)后融合定位系統(tǒng)的定位續(xù)航情況。

4.2 數(shù)據(jù)結(jié)果與分析

按BDS-SPP/INS 融合定位系統(tǒng)的定位終端性能測(cè)試指標(biāo)測(cè)算方法，編寫(xiě)Matlab 程序進(jìn)行統(tǒng)計(jì)計(jì)算，繪制圓概率誤差圖和距離誤差曲線(xiàn)圖。

采用高斯?克呂格投影（等角橫切橢圓柱投影） 作為坐標(biāo)轉(zhuǎn)換方法將空間坐標(biāo)轉(zhuǎn)換為高斯坐標(biāo)后，再以GPS 靜態(tài)定位真值作為坐標(biāo)原點(diǎn)，從而將高斯坐標(biāo)系轉(zhuǎn)換為當(dāng)?shù)刈鴺?biāo)系[32]。

4.2.1 靜態(tài)數(shù)據(jù)結(jié)果與分析

設(shè)計(jì)信息融合濾波器需要獲得先驗(yàn)誤差協(xié)方差，所以對(duì)比低功耗BDS-SPP 定位系統(tǒng)與高精度BDS-RTK 的靜態(tài)誤差值（2 種衛(wèi)星定位系統(tǒng)處于靜止?fàn)顟B(tài)，以高精度BDS-RTK 定位作為基準(zhǔn)），在廣州天河區(qū)和增城區(qū)進(jìn)行多組靜態(tài)定位性能測(cè)試，分別選取2 組試驗(yàn)結(jié)果進(jìn)行性能對(duì)比，測(cè)試時(shí)間為2021 年7 月，4 組試驗(yàn)時(shí)長(zhǎng)分別為2 100、1 800、900、2 280 s，總時(shí)間為7 080 s，具體數(shù)據(jù)如表1 所示。

圖6、7 為廣州市增城區(qū)第3 組試驗(yàn)的靜態(tài)誤差圖和圓概率誤差圖，根據(jù)圖6 數(shù)據(jù)計(jì)算得出在空曠地帶BDS-SPP 的靜態(tài)誤差為0.240 4 m，對(duì)比圖6A、6B，在同等條件下RTK 的誤差精度以及波動(dòng)小于BDS-SPP。將4 組試驗(yàn)的數(shù)據(jù)進(jìn)行計(jì)算，得到標(biāo)準(zhǔn)差、圓概率誤差、平均差、緯度真值和經(jīng)度（表1）。

4.2.2 動(dòng)態(tài)數(shù)據(jù)結(jié)果與分析

為了對(duì)比低功耗BDS-SPP/INS 的融合定位系統(tǒng)與高精度BDSRTK的動(dòng)態(tài)誤差值，在廣州天河區(qū)進(jìn)行了6 組動(dòng)態(tài)定位性能測(cè)試，測(cè)試時(shí)間為2021 年7 月，總時(shí)長(zhǎng)為2 633 s，且每次測(cè)試時(shí)間不等。將BDS-SPP 定位點(diǎn)與參照系BDS-RTK 定位點(diǎn)之間的直線(xiàn)距離稱(chēng)為BDS-SPP 定位偏移距離，將BDS-SPP/INS 融合定位點(diǎn)與參照系BDS-RTK 定位點(diǎn)之間的直線(xiàn)距離稱(chēng)為BDS-SPP/INS 定位偏移距離，為分析誤差大小求取其標(biāo)準(zhǔn)差，數(shù)據(jù)如表2 所示，由表2 可知，動(dòng)態(tài)試驗(yàn)中BDS-SPP/INS 定位偏移距離的最小標(biāo)準(zhǔn)差為1.141 5 m，最大標(biāo)準(zhǔn)差為3.051 6 m。BDS-SPP/INS 定位偏移距離的平均標(biāo)準(zhǔn)差小于1.913 7 m，比BDS-SPP 定位偏移距離小0.165 2 m。

廣州市動(dòng)態(tài)試驗(yàn)的定位性能測(cè)試結(jié)果如圖8所示， 分析圖8A 可以看到在同樣條件下融合后得到的定位點(diǎn)變化更加貼合BDS-RTK 定位點(diǎn)，融合后的輸出頻率為BDS-SPP 輸出頻率的3 倍，由圖8B 可知在此次試驗(yàn)中融合后的數(shù)據(jù)去除了高噪聲影響，融合后的數(shù)據(jù)更加平緩、波動(dòng)更小，更符合實(shí)際運(yùn)動(dòng)規(guī)律。

4.2.3 抗干擾試驗(yàn)結(jié)果與分析

針對(duì)融合定位系統(tǒng)工作中信號(hào)丟失問(wèn)題，設(shè)計(jì)了抗干擾試驗(yàn)，以高精度BDS-RTK 為測(cè)試真值，觀察當(dāng)BDS 信號(hào)丟失后的BDS-SPP/INS 定位偏移距離以及BDS-SPP 定位偏移距離。在廣州華南農(nóng)業(yè)大學(xué)校園內(nèi)固定道路進(jìn)行10 組測(cè)試，測(cè)試時(shí)間為2021 年12 月，總時(shí)間為150 min，隨機(jī)取3 組數(shù)據(jù)進(jìn)行分析，其中第1 組試驗(yàn)結(jié)果如圖9 所示，具體偏移距離統(tǒng)計(jì)數(shù)據(jù)如表3所示，由表3 可知，當(dāng)試驗(yàn)中BDS 丟失信號(hào)時(shí)，BDS-SPP 定位偏移距離的平均標(biāo)準(zhǔn)差為6.226 5 m，BDS-SPP/INS 定位偏移距離平均標(biāo)準(zhǔn)差為3.636 5 m，比BDS-SPP 定位系統(tǒng)減少2.590 0 m。

5 討論與結(jié)論

INS 慣性導(dǎo)航裝置中存在慣性傳感器誤差累積導(dǎo)致的慣性漂移問(wèn)題，可以通過(guò)校準(zhǔn)初始航向角和零偏的方法消除。未來(lái)可以進(jìn)一步研究慣性傳感器角速率測(cè)量零偏實(shí)時(shí)校準(zhǔn)方法和自回歸模型的航向校準(zhǔn)方法，增強(qiáng)其自適應(yīng)能力，提高慣性導(dǎo)航的精確度和可靠性，以更好地應(yīng)對(duì)各種復(fù)雜環(huán)境和操作場(chǎng)景下的導(dǎo)航需求。

本文提出了一種針對(duì)低功耗定位系統(tǒng)需求的技術(shù)方案。該方案采用低功耗的B D S - S P P 和INS 系統(tǒng)進(jìn)行融合，通過(guò)Kalman 濾波器進(jìn)行優(yōu)化，與高精度地基增強(qiáng)BDS-RTK 定位系統(tǒng)的對(duì)比試驗(yàn)結(jié)果表明，其功耗降低了33.3 W，在滿(mǎn)足定位準(zhǔn)確性要求的同時(shí)達(dá)到了功耗更低。此外，本文還針對(duì)復(fù)雜農(nóng)田環(huán)境要求，搭建了基于低功耗MCU 測(cè)試系統(tǒng)平臺(tái)，設(shè)計(jì)出低功耗的BDS-SPP/INS 融合定位系統(tǒng)。

進(jìn)行了靜態(tài)與動(dòng)態(tài)試驗(yàn)，BDS-SPP 靜態(tài)漂移試驗(yàn)結(jié)果中，平均誤差為0.472 6 m，圓概率誤差為0.469 9 m；動(dòng)態(tài)試驗(yàn)結(jié)果中，BDS-SPP/INS 定位偏移距離的最小標(biāo)準(zhǔn)差為1.141 5 m，最大標(biāo)準(zhǔn)差為3.051 6 m。BDS-SPP/INS定位偏移距離的的平均標(biāo)準(zhǔn)差為1.913 7 m，比低功耗BDS-SPP 定位系統(tǒng)的小0.165 2 m。

對(duì)BDS-SPP/INS 融合定位系統(tǒng)在干擾條件下的試驗(yàn)結(jié)果得出，BDS-SPP/INS 定位偏移距離的平均標(biāo)準(zhǔn)差為3.636 5 m，比BDS-SPP 定位系統(tǒng)減少2.590 0 m。表明融合定位系統(tǒng)能夠在復(fù)雜條件下工作，可以在缺少RTK 鏈路的情況下提供基礎(chǔ)保障。由于引入了INS 的高頻數(shù)據(jù)，融合后的輸出頻率較單獨(dú)采用BDS-SPP 提高了3 倍，從而能夠更及時(shí)地對(duì)狀態(tài)進(jìn)行響應(yīng)，進(jìn)一步提高了BDS-SPP 的定位質(zhì)量。這為小型農(nóng)田機(jī)器人（如田間監(jiān)測(cè)） 的定位問(wèn)題提供了技術(shù)基礎(chǔ)。

參考文獻(xiàn)：

[1]HASSALL J. Future trends in precision agriculture[R/OL]. [2023-05-01]. https：//www.nuffieldscholar.org/sites/default/files/reports/2009_AU_James-Hassall_Future-Trends-In-Precision-Agriculture-A-Look-Into-The-Future-Of-Agricultural-Equipment.pdf.

[2]羅錫文， 王在滿(mǎn). 水稻生產(chǎn)全程機(jī)械化技術(shù)研究進(jìn)展[J]. 現(xiàn)代農(nóng)業(yè)裝備， 2014（1）： 23-29.

[3]羅錫文， 廖娟， 汪沛， 等. 提高農(nóng)業(yè)機(jī)械化水平促進(jìn)農(nóng)業(yè)現(xiàn)代化發(fā)展[J]. 中國(guó)農(nóng)村科技， 2021（1）： 6-11.

[4]陳慶根， 廖西元. 優(yōu)質(zhì)水稻生產(chǎn)投入與產(chǎn)出的經(jīng)濟(jì)效益評(píng)價(jià)[J]. 中國(guó)農(nóng)村經(jīng)濟(jì)， 2001（3）： 18-24.

[5]張智剛， 王進(jìn)， 朱金光， 等. 我國(guó)農(nóng)業(yè)機(jī)械自動(dòng)駕駛系統(tǒng)研究進(jìn)展[J]. 農(nóng)業(yè)工程技術(shù)， 2018， 38（18）： 23-27.

[6]韓樹(shù)豐， 何勇， 方慧. 農(nóng)機(jī)自動(dòng)導(dǎo)航及無(wú)人駕駛車(chē)輛的發(fā)展綜述（英文）[J]. 浙江大學(xué)學(xué)報(bào)（農(nóng)業(yè)與生命科學(xué)版）， 2018， 44（4）： 381-391.

[7]張漫， 季宇寒， 李世超， 等. 農(nóng)業(yè)機(jī)械導(dǎo)航技術(shù)研究進(jìn)展[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2020， 51（4）： 1-18.

[8]姚立健， SANTOSH K P， 楊自棟， 等. 基于超寬帶無(wú)線(xiàn)定位的農(nóng)業(yè)設(shè)施內(nèi)移動(dòng)平臺(tái)路徑跟蹤研究[J]. 農(nóng)業(yè)工程學(xué)報(bào)， 2019， 35（2）： 17-24.

[9]陳黎卿， 許鳴， 柏仁貴， 等. 高地隙植保機(jī)輔助駕駛系統(tǒng)設(shè)計(jì)與試驗(yàn)[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2019， 50（9）： 25-32.

[10]LIU Y， MA X Y， SHU L， et al. From Industry 4.0 to Agriculture4.0： Current status， enabling technologies， andresearch challenges[J]. IEEE Transactions on IndustrialInformatics， 2021， 17（6）： 4322-4334.

[11]BECHAR A， VIGNEAULT C. Agricultural robots forfield operations. Part 2： Operations and systems[J].Biosystems Engineering， 2017， 153： 110-128.

[12]莊朝文， 富立， 范躍祖. 多傳感器融合技術(shù)在低成本車(chē)輛組合導(dǎo)航定位系統(tǒng)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，2002， 10（5）： 1-5.

[13]張京， 陳度， 王書(shū)茂， 等. 農(nóng)機(jī)INS/GNSS 組合導(dǎo)航系統(tǒng)航向信息融合方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2015， 46（S1）： 1-7.

[14]王力， 楊忠輝. 自適應(yīng)序貫UKF 的INS/BDS 緊組合導(dǎo)航算法[J]. 計(jì)算機(jī)仿真， 2019， 36（12）： 228-232.

[15]SHEN C， ZHANG Y， TANG J， et al. Dual-optimizationfor a MEMS-INS /GPS system during GPS outages basedon the cubature Kalman filter and neural networks[J]Mechanical Systems and Signal Processing， 2019， 133（106222）： 1-14.

[16]BROWN A K， LU Y. Performance test results of an integratedGPS/MEMS inertial navigation package[C]//Proceedings of the 17th International Technical Meetingof the Satellite Division of The Institute of Navigation（ION GNSS 2004）. Long Beach， California： ION GNSS，2004： 825-832.

[17]魏永超， 羅城仔， 鄧嵐， 等. 基于DS-SVR 算法優(yōu)化的BDS/INS 組合導(dǎo)航[J]. 航空計(jì)算技術(shù)， 2020， 50（1）： 57-60.

[18]張曉寒， 趙景波， 董振振. 農(nóng)機(jī)BDS/INS 組合導(dǎo)航算法研究[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào)， 2021， 42（2）： 171-177.

[19]張智剛， 羅錫文， 趙祚喜， 等. 基于Kalman 濾波和純追蹤模型的農(nóng)業(yè)機(jī)械導(dǎo)航控制[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2009，40（S1）： 6-12.

[20]丁幼春， 何志博， 夏中州， 等. 小型履帶式油菜播種機(jī)導(dǎo)航免疫 PID 控制器設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)， 2019， 35（7）：12-20.

[21]李逃昌， 胡靜濤， 高雷， 等. 基于模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機(jī)械路徑跟蹤方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2013，44（1）： 205-210.

[22]張聞?dòng)睿?丁幼春， 王雪玲， 等. 基于SVR 逆向模型的拖拉機(jī)導(dǎo)航純追蹤控制方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2016，47（1）： 29-36.

[23]BEVLY D M， GERDES J C， PARKINSON B W. A newyaw dynamic model for improved high speed control of afarm tractor[J]. Journal of Dynamic Systems， Measurementand Control， 2002， 124（4）： 659-667.

[24]何杰， 朱金光， 張智剛， 等. 水稻插秧機(jī)自動(dòng)作業(yè)系統(tǒng)設(shè)計(jì)與試驗(yàn)[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2019， 50（3）： 17-24.

[25]段賢強(qiáng)， 陶建峰， 覃程錦， 等. 變速條件下農(nóng)業(yè)機(jī)械路徑跟蹤穩(wěn)定控制方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2019， 50（9）： 18-24.

[26]劉兆朋， 張智剛， 羅錫文， 等. 雷沃ZP9500 高地隙噴霧機(jī)的GNSS 自動(dòng)導(dǎo)航作業(yè)系統(tǒng)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2018， 34（1）： 15-21.

[27]王輝， 王桂民， 羅錫文， 等. 基于預(yù)瞄追蹤模型的農(nóng)機(jī)導(dǎo)航路徑跟蹤控制方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)， 2019， 35（4）：11-19.

[28]張聞?dòng)睿?王進(jìn)， 張智剛， 等. 基于自校準(zhǔn)變結(jié)構(gòu)Kalman的農(nóng)機(jī)導(dǎo)航BDS 失鎖續(xù)航方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2020， 51（3）： 18-27.

[29]靳標(biāo)， 李建行， 朱德寬， 等. 基于自適應(yīng)有限沖激響應(yīng)?卡爾曼濾波算法的GPS/INS 導(dǎo)航[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2019， 35（3）： 75-81.

[30]洪劍青， 趙德安， 孫月平， 等. 基于卡爾曼濾波離散滑?？刂频拿鬏喆本€(xiàn)跟蹤[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2019， 50（9）：33-39.

[31]白秉旭， 田光兆. 基于模糊算法的自主導(dǎo)航拖拉機(jī)轉(zhuǎn)向系統(tǒng)設(shè)計(jì)[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào)， 2014， 35（6）： 135-137.

[32]劉健， 盧加華， 楊新云， 等. 中緯度地區(qū)蘭勃特投影和高斯?克呂格投影變形分析[J]. 地礦測(cè)繪， 2019， 35（3）： 12-14.

【責(zé)任編輯 莊 延】

基金項(xiàng)目：國(guó)家重點(diǎn)研發(fā)計(jì)劃（2022YFD2001601）；國(guó)家糖料產(chǎn)業(yè)技術(shù)體系（CARS-170405）