基于擴(kuò)展卡爾曼濾波的農(nóng)機(jī)路徑跟蹤控制算法研究

摘要：智能農(nóng)機(jī)的路徑跟蹤精度受田地地形、農(nóng)機(jī)結(jié)構(gòu)等多因素的影響，會導(dǎo)致壓壞滴灌帶等嚴(yán)重后果。以棉花播種機(jī)為研究對象，提出一種由Stanley跟蹤算法和線性二次最優(yōu)控制（LQR）算法組成的切換式跟蹤算法，引導(dǎo)農(nóng)機(jī)快速入線和保持直線精度；另外為消除靜態(tài)橫向誤差，加入針對航向角誤差的擴(kuò)展卡爾曼濾波器（EKF）。仿真結(jié)果顯示，切換式跟蹤算法在進(jìn)入目標(biāo)直線后可有效消除靜態(tài)橫向誤差。實(shí)車試驗(yàn)結(jié)果表明，當(dāng)給定初始橫向誤差為0.5 m，速度為3.6 km/h時，入線時間為6.88 s，超調(diào)量為0.041 m；當(dāng)給定初始橫向誤差為0 m，速度為3.6 km/h時，直線跟蹤精度控制在±0.025 m范圍內(nèi)，滿足實(shí)際農(nóng)機(jī)高精度的直線作業(yè)要求，表明該算法具有良好的跟蹤精度和抗干擾能力，有利于提升農(nóng)業(yè)生產(chǎn)效率。

關(guān)鍵詞：智能農(nóng)機(jī)；定位技術(shù)；擴(kuò)展卡爾曼濾波；路徑跟蹤；自動駕駛

中圖分類號：S220

文獻(xiàn)標(biāo)識碼：A

文章編號：20955553 （2024） 070215 07

Research on path tracking control algorithm of agricultural machinery

based on extended Kalman filter

Qian Junnan， Feng Sang， Li Hang， Zhang Yong

（School of Mechanical and Electrical Engineering， Guangdong University of Technology， Guangzhou， 510006， China）

Abstract：

The path tracking accuracy of intelligent agricultural machinery is affected by many factors such as field terrain and machinery structure， which may lead to serious consequences such as crushing the drip irrigation belt. In this paper， taking the cotton planter as the research object， a switching tracking algorithm consisting of Stanley tracking algorithm and linear quadratic optimal control （LQR） algorithm is proposed to guide the agricultural machine to enter the line quickly and maintain the straight line accuracy. In addition， in order to eliminate the static lateral error， an extended Kalman filter （EKF） for heading angle error is added. The simulation results show that the switching tracking algorithm can eliminate the static lateral error after entering the straight target line. The real vehicle test results show that when the given initial lateral error is 0.5 m and the speed is 3.6 km/h， the entry time is 6.88 s， and the overshoot is 0.041 m. When the given initial lateral error is 0 m， and the speed is 3.6 km/h， the straight-line tracking accuracy is controlled within ±0.025 m， which meets the requirements of high-precision straight-line operation of actual agricultural machinery. It shows that the algorithm studied has good tracking accuracy and anti-interference ability and it is conducive to improving agricultural production efficiency.

Keywords：

intelligent agricultural machinery; positioning technology; extended Kalman filter; path tracking; automatic driving

0 引言

隨著“智慧農(nóng)業(yè)”的推行，農(nóng)機(jī)自動導(dǎo)航技術(shù)逐漸成為智能化精準(zhǔn)農(nóng)業(yè)中的重要組成部分［13］。路徑跟蹤控制器的作用是使得農(nóng)機(jī)能夠跟隨既定目標(biāo)路徑［4］，其性能優(yōu)劣將直接影響農(nóng)機(jī)能否正確完成給定的駕駛?cè)蝿?wù)。目前國內(nèi)外學(xué)者都對農(nóng)機(jī)自動駕駛路徑跟蹤方面展開廣泛的研究。

在國外，Alipour等［5］提出一種基于滑?？刂埔?guī)則的輪式移動機(jī)器人系統(tǒng)的軌跡追蹤控制器。Khalaji［6］提出一種基于轉(zhuǎn)矩計(jì)算的PID控制器，用于拖拉機(jī)—掛車輪式機(jī)器人的軌跡跟蹤。Han等［7］提出一種能適應(yīng)不同地面附著特性的基于滑動估計(jì)的農(nóng)機(jī)路徑跟蹤算法。Murillo等［8］提出了一種具有雙轉(zhuǎn)向機(jī)構(gòu)的鉸接式拖拉機(jī)數(shù)學(xué)模型，能夠用與小型土地的靈活作業(yè)。Nehme等［9］提出了基于激光雷達(dá)的魯棒結(jié)構(gòu)跟蹤方法，適合葡萄園等復(fù)雜場景，降低導(dǎo)航成本。在國內(nèi)，Xu等［10］提出了基于4WS農(nóng)機(jī)高效MPC控制器，兼顧了效率與精度。Zhou等［11］提出全球定位系統(tǒng)（GPS）受擾環(huán)境中，拖拉機(jī)—掛車系統(tǒng)的一般路徑跟蹤框架。武濤等［12］設(shè)計(jì)了一種快速終端滑模跟蹤控制算法，有效抑制振動帶來的干擾。張智剛等［13］開發(fā)了一套基于星基增強(qiáng)精密單點(diǎn)定位的農(nóng)機(jī)自動導(dǎo)航系統(tǒng)。張培培等［14］采用滑?？刂?，對拖拉機(jī)和牽引式農(nóng)具進(jìn)行路徑跟蹤。

為提高農(nóng)業(yè)生產(chǎn)效率，避免自動駕駛農(nóng)機(jī)偏離目標(biāo)路徑壓壞滴灌帶而造成損失，本文以棉花播種機(jī)為試驗(yàn)平臺，提出一種切換式跟蹤控制算法，采用Stanley算法引導(dǎo)農(nóng)機(jī)快速入線，入線后采用LQR算法保持直線精度；采用基于航向誤差的EKF濾波器消除定位系統(tǒng)安裝帶來的靜態(tài)橫向誤差并進(jìn)行試驗(yàn)驗(yàn)證。

1 路徑跟蹤策略

1.1 LQR控制器

1.1.1 運(yùn)動學(xué)模型

農(nóng)機(jī)在農(nóng)田工作時處于低速狀態(tài)，受輪胎側(cè)偏影響小，可以采用車輛二自由度運(yùn)動學(xué)模型［15］來近似描述，如圖1所示。OXY為慣性坐標(biāo)系；X和Y為橫縱坐標(biāo)；X·為農(nóng)機(jī)在橫坐標(biāo)方向上的速度，m/s；Y·為農(nóng)機(jī)在縱坐標(biāo)方向的速度，m/s；A點(diǎn)是質(zhì)心；O′點(diǎn)為車輛的瞬時轉(zhuǎn)動中心；lf和lr分別為控制點(diǎn)與前后軸的距離，m；δf為前輪轉(zhuǎn)角，rad；β為質(zhì)心側(cè)偏角；φ為航向角，rad；v為行駛速度，m/s；ey為橫向誤差，m。

根據(jù)幾何運(yùn)動關(guān)系可以得到農(nóng)機(jī)運(yùn)動學(xué)模型

X·=vcos（φ+β）

Y·=vsin（φ+β）

φ·=vsin βlr

（1）

其中，β=tan-1lrLtanδf，則

Y·=vφ+tan-1lrLtanδf

（2）

φ·=vLtanδf

（3）

式中：

L——農(nóng)機(jī)的軸距，m。

1.1.2 軌跡跟蹤誤差模型

為計(jì)算簡便，設(shè)定Y=0為目標(biāo)直線。跟蹤過程主要存在車輛質(zhì)心到目標(biāo)路徑的橫向偏差ey和車輛方向與路徑方向的航向偏差eφ。描述ey和eφ有

ey=-Xsinφref+Ycosφref

（4）

eφ=φ-φref

（5）

式中：

φref——參考路徑的方位角，rad。

前輪轉(zhuǎn)角通過方向盤電機(jī)控制，采用PD控制有

δf=uf·PPD·Kpτs2+（1+KpDPD）s+PPD

（6）

式中：

uf——期望車輪轉(zhuǎn)角，rad；

PPD——比例系數(shù)；

s——拉普拉斯變換域中的頻率；

DPD——微分系數(shù)；

τ——時間常數(shù)；

Kp——電機(jī)轉(zhuǎn)角與車輪轉(zhuǎn)角的比例系數(shù)。

由式（1）～式（6）推導(dǎo)得軌跡跟蹤誤差模型

e·y=vφ+lrLδf

e·φ=vLδf

δ·f=δ·f

δ··f=-1+KpDPDτδ·f-PPDτδf+PPD·Kpτuf

（7）

將式（7）改寫成狀態(tài)空間方程，有

x·=Ax+Buf

（8）

其中，

A=

0vvlr/L0

00vL0

0001

00-KpPPDτ（1+DPDKp）τ

，

B=

000KpPPDτ

。

1.1.3 LQR控制器設(shè)計(jì)

LQR控制器［16］是一種多目標(biāo)最優(yōu)控制。LQR控制的目標(biāo)函數(shù)

J（x）=∑∞t=0xTQx+uTRu

（9）

式中：

J（x）——目標(biāo)函數(shù)；

x——狀態(tài)量；

u——控制量；

Q——狀態(tài)量加權(quán)矩陣；

R——控制量加權(quán)矩陣。

其中，Q=

100000

01000

0010

0001

，

R=［500］。

一般全狀態(tài)反饋控制器控制律有

u=-Kx

（10）

式中：

K——控制增益系數(shù)。

將式（10）代入目標(biāo)函數(shù)有

J（x）=∑∞t=0xT（Q+KTRK）x

（11）

假設(shè)存在常量矩陣P使系統(tǒng)趨于穩(wěn)定，可得

K=（R+BTPB）-1BTPA

（12）

P為Ricatti方程正定解，即

P=-ATPB（R+BTPB）-1BTPA+ATPA+Q

（13）

1.2 Stanley控制器

采用基于幾何學(xué)的Stanley控制器［17， 18］，將車輛前軸作為控制參考點(diǎn)，由圖2可得

δf=eφ+arctanKeyv

（14）

1.3 基于航向誤差的擴(kuò)展卡爾曼濾波器

對系統(tǒng)中RTK-GPS的定位、IMU的姿態(tài)信息以及農(nóng)機(jī)的車速傳感器信號數(shù)使用擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）［19， 20］可得估計(jì)位置和航向角。

在卡爾曼濾波中，系統(tǒng)的狀態(tài)改變和觀測服從

xk=Aexk-1+Beuk-1

ze（k+1）=Cexk

（15）

式中：

xk、xk-1——

當(dāng)前k時刻和上一時刻的模型狀態(tài)變量；

Ae——系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；

Be——控制輸入矩陣；

Ce——系統(tǒng)輸出矩陣。

在基于航向誤差的擴(kuò)展卡爾曼濾波器中，以X·、Y·、vb、φ、wb以及φb作為狀態(tài)變量；以v、w為控制輸入。航跡推算擴(kuò)展卡爾曼濾波的連續(xù)型表達(dá)式為

x·=

X·

Y·

v·b

φ·

w·b

φ·b

=

（v-vb）sin φ

（v-vb）cos φ

vb

w+wb

wb

φb

（16）

式中：

vb——

傳感器測量速度過程產(chǎn)生的零點(diǎn)漂移；

φb、φ·b——

組合導(dǎo)航測量航向角和航向角速度加速過程中傳感器產(chǎn)生的零點(diǎn)漂移；

w——IMU測量的航向角速度，rad/s；

wb、w·b——

IMU測量航向角速度和航向角加速度過程中產(chǎn)生的漂移。

根據(jù)前向歐拉法，可得離散化模型為

x（k+1）

=

X（k+1）

Y（k+1）

vb（k+1）

φ（k+1）

wb（k+1）

φb（x+1）

=

X（k）+［v（k）-vb（k）］sin［φ（k）］dt

Y（k）+［v（k）-vb（k）］cos［φ（k）］dt

vb（k）

φ（k）+［w（k）+wb（k）］dt

wb（k）

φb（k）

（17）

根據(jù)雅可比矩陣法，Ae和Be矩陣為

Ae=

00-sinφ（v-vb）cosφ00

00-cosφ-（v-vb）sinφ00

000000

0000-10

000000

000000

（18）

Be=

sinφ0

cosφ0

00

00

（19）

系統(tǒng)傳感器包含GPS-RTK與IMU，在狀態(tài)觀測中以質(zhì)心位置和航向角作為觀測值，故ze和Ce矩陣表示為

ze（k+1）=Cex（k）

（20）

式中：

ze——傳感器輸出的測量向量；

Ce——觀測矩陣，Ce=

100000

010000

000101

。

1.4 路徑跟蹤控制器總體設(shè)計(jì)

分別以不同初始橫向誤差和不同速度對LQR控制器和Stanley控制器都進(jìn)行仿真試驗(yàn)，如圖3和圖4所示。

由圖3可以看出，在不同初始橫向位置，相比LQR控制器，Stanley控制器入線時間減少了7.7～11.1 s，而入線距離減少了7.7～10.3 m，說明Stanley入線效率高。根據(jù)圖4可知，隨著速度提高，LQR控制器入線時會發(fā)生0.03 m的超調(diào)，但是線上精度的均值和標(biāo)準(zhǔn)差均小于0.003 m，相比Stanley控制器的線上控制效果更為出色。

因此選擇將兩種控制器結(jié)合，利用Stanley控制器進(jìn)行入線；在農(nóng)機(jī)入線后，切換為LQR控制器完成直線路徑跟蹤。其中入線判斷標(biāo)準(zhǔn)為橫向誤差絕對值小于0.05 m，航向誤差絕對值小于0.03 rad（1.72°）。

為模擬傳感器誤差的工況，設(shè)置固定航向偏移角為0.7°，對速度為3.6 km/h時進(jìn)行仿真，如圖5所示。

從圖5可以看出，定位系統(tǒng)因存在安裝偏差（即航向誤差為0.7°），入線用時3 s，入線后會導(dǎo)致0.03 m的橫向誤差和0.7°的航向角誤差。引入EKF后，入線用時3.5 s，橫向誤差和航向角誤差均在0附近波動，有效消除了安裝誤差造成的靜態(tài)誤差。

最后根據(jù)仿真結(jié)果設(shè)計(jì)如圖6所示的路徑跟蹤控制器。

2 實(shí)車試驗(yàn)

2.1 農(nóng)機(jī)自動駕駛系統(tǒng)試驗(yàn)平臺及場景

為了避免GPS-RTK雙天線因遮擋而受干擾，將其與集成控制器一同水平安裝于農(nóng)機(jī)頂部。其中GPS-RTK模塊用于獲取農(nóng)機(jī)實(shí)時定位信息；IMU模塊中加速度計(jì)可以獲取農(nóng)機(jī)實(shí)時的速度與加速度信息，陀螺儀用于獲取農(nóng)機(jī)實(shí)時的姿態(tài)信息；控制器間通過CAN輸出期望電機(jī)角度至電機(jī)執(zhí)行機(jī)構(gòu)，實(shí)現(xiàn)自動轉(zhuǎn)向。本文采用的農(nóng)機(jī)參數(shù)如表1所示。

農(nóng)機(jī)自動駕駛系統(tǒng)實(shí)車試驗(yàn)平臺組成如圖7所示，該系統(tǒng)主要由約翰迪爾903型農(nóng)機(jī)、集成控制器、電動轉(zhuǎn)向系統(tǒng)、GPS-RTK雙天線、顯示終端等構(gòu)成。

為了檢驗(yàn)自動駕駛系統(tǒng)的可行性，在某棉花種植試驗(yàn)場地進(jìn)行試驗(yàn)。將農(nóng)機(jī)入線時的入線時間、入線距離以及超調(diào)量作為自動駕駛系統(tǒng)入線性能的評價指標(biāo)；把農(nóng)機(jī)的實(shí)際行駛路徑相對參考路徑的絕對平均橫向誤差以及其標(biāo)準(zhǔn)差，作為自動駕駛系統(tǒng)跟蹤直線精度的評價指標(biāo)。

2.2 不同行駛速度下的直線跟蹤試驗(yàn)

試驗(yàn)時農(nóng)機(jī)行駛的速度設(shè)為3.6 km/h，初始航向誤差為0°，設(shè)置農(nóng)機(jī)的初始橫向誤差為0.5 m、0.8 m和1 m。農(nóng)機(jī)在不同初始橫向誤差下的入線效果如圖8所示，表2為不同初始橫向誤差下的入線性能數(shù)據(jù)。

根據(jù)圖8和表2可以看出，農(nóng)田試驗(yàn)的結(jié)果與仿真試驗(yàn)結(jié)果趨勢相似，但因?yàn)檗r(nóng)田道路對農(nóng)機(jī)自動駕駛系統(tǒng)的干擾性很大，以及農(nóng)機(jī)轉(zhuǎn)向系統(tǒng)中液壓偏移的影響導(dǎo)致的響應(yīng)滯后等各種環(huán)境因素，使試驗(yàn)入線性能效果與仿真入線結(jié)果存在差異。

當(dāng)初始橫向誤差絕對值為0.5 m時，在入線過程出現(xiàn)約0.041 m的超調(diào)，在7 s內(nèi)自動駕駛系統(tǒng)即可控制農(nóng)機(jī)入線，且入線距離為11.24 m。當(dāng)初始航向誤差絕對值增加至1 m入線時，入線時間增加了約3 s，入線距離增加約5 m，超調(diào)量均保持在0.04 m左右。由此可見，隨著初始航向誤差的增加，入線時間以及入線距離逐漸增加。當(dāng)橫向誤差越大時，航向誤差隨之增加，控制器會輸出較大的期望前輪轉(zhuǎn)角量，使農(nóng)機(jī)的航向角增大；當(dāng)接近參考路徑時，逐漸減小前輪轉(zhuǎn)角，使農(nóng)機(jī)的航向誤差逐漸降低，最后農(nóng)機(jī)逐步跟蹤至直線上。入線過程中有出現(xiàn)超調(diào)，但仍可快速收斂至0附近。

2.3 不同行駛速度下的直線跟蹤試驗(yàn)

試驗(yàn)時速度分別為3.6 km/h、5 km/h和8 km/h，初始橫向誤差為0 m。農(nóng)機(jī)在不同行駛速度下的直線跟蹤效果如圖9所示，包括橫向誤差及航向誤差。表3為不同行駛速度下的直線跟蹤性能數(shù)據(jù)統(tǒng)計(jì)。

根據(jù)圖9和表3可以看出，實(shí)際農(nóng)機(jī)作業(yè)環(huán)境中存在著許多的干擾因素，對農(nóng)機(jī)自動駕駛系統(tǒng)的影響很大。當(dāng)初始橫向誤差絕對值為0 m、農(nóng)機(jī)以3.6 km/h速度行駛時，橫向誤差的絕對值均值為0.012 m，標(biāo)準(zhǔn)差為0.015 m，實(shí)際可以控制農(nóng)機(jī)保持在橫向誤差約為±0.025 m的范圍附近波動；當(dāng)以8 km/h速度行駛時，橫向誤差的絕對值均值增加了0.011 m，橫向誤差最大值增加了0.033m，農(nóng)機(jī)保持在距離參考路徑±0.06 m的范圍內(nèi)波動，并且航向誤差的波動范圍也有所增加。隨著速度升高時，橫向誤差的絕對值均值和標(biāo)準(zhǔn)差隨之增加，這是因?yàn)檗r(nóng)機(jī)在顛簸路段以較高速行駛時，自身的振動也更加大，容易出現(xiàn)滑移。此外，當(dāng)農(nóng)機(jī)走過顛簸農(nóng)田道路時，因行駛速度較大時，使轉(zhuǎn)向執(zhí)行機(jī)構(gòu)對輸出的前輪轉(zhuǎn)角響應(yīng)滯后，導(dǎo)致地形因素對農(nóng)機(jī)自動駕駛系統(tǒng)的干擾有放大作用。

3 結(jié)論

1）" 本文采用Stanley算法和四階LQR算法組成的切換式控制算法實(shí)現(xiàn)智能農(nóng)機(jī)的跟蹤控制，可以保證智能農(nóng)機(jī)能夠在跟蹤指定直線時能夠迅速入線和直線跟蹤保持高精度；同時加入基于EKF的航向?yàn)V波器，能夠減少定位傳感器帶來的誤差。

2）" 在仿真環(huán)境中，分別設(shè)置3組試驗(yàn)，在不同初始橫向誤差中，Stanley控制器與LQR控制器相比，入線時間減少76%，而入線距離減少57%；在不同速度下直線跟蹤，LQR的橫向位置誤差仍保持在0.003 m，而Stanley的橫向誤差隨著速度增大從0.04 m增加到0.08 m，說明LQR控制器受速度變化影響較小，魯棒性較好；結(jié)合兩者優(yōu)點(diǎn)，設(shè)計(jì)Stanley和LQR組成的切換式控制器。仿真試驗(yàn)結(jié)果表明，當(dāng)假設(shè)存在安裝誤差，無EKF的切換式控制器無法消除靜態(tài)橫向誤差和航向角誤差；而采用EKF則可有效消除這些靜態(tài)誤差，且橫向標(biāo)準(zhǔn)差僅為0.01 m。

3）" 以約翰迪爾903型農(nóng)機(jī)為試驗(yàn)平臺，并搭載集成控制器、GPS-RTK雙天線等設(shè)備，在棉花種植場地進(jìn)行自動駕駛作業(yè)試驗(yàn)。試驗(yàn)結(jié)果表明，當(dāng)初始橫向誤差為0.5 m、速度為3.6 km/h時，入線時間為6.88 s，入線距離為11.24 m，超調(diào)量為0.041 m，直線跟蹤精度控制在±0.025 m范圍內(nèi)，滿足實(shí)際農(nóng)機(jī)高精度的直線作業(yè)要求。

參 考 文 獻(xiàn)

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

Zhang Man， Ji Yuhan， Li Shichao， et al. Research progress of agricultural machinery navigation technology ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2020， 51（4）： 1-18.

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

Wang Hui， Wang Guimin， Luo Xiwen， et al. Path tracking control method of agricultural machine navigation based on aiming pursuit model ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2019， 35（4）： 11-19.

［3］徐廣飛， 陳美舟， 金誠謙， 等. 拖拉機(jī)自動駕駛關(guān)鍵技術(shù)綜述［J］. 中國農(nóng)機(jī)化學(xué)報， 2022， 43（6）： 126-134.

Xu Guangfei， Chen Meizhou， Jin Chengqian， et al. A review of key technology of tractor automatic driving ［J］. Journal of Chinese Agricultural Mechanization， 2022， 43（6）： 126-134.

［4］嚴(yán)國軍， 賁能軍， 顧建華， 等. 基于MPC的無人駕駛拖拉機(jī)軌跡跟蹤控制［J］. 重慶交通大學(xué)學(xué)報（自然科學(xué)版）， 2019， 38（9）： 1-6.

Yan Guojun， Ben Nengjun， Gu Jianhua， et al. Trajectory tracking control of intelligent tractor based on MPC algorithm ［J］. Journal of Chongqing Jiaotong University of Technology （Natural Science）， 2019， 38（9）： 1-6.

［5］Alipour K， Robat A B，Tarvirdizadeh B. Dynamics modeling and sliding mode control of tractor-trailer wheeled mobile robots subject to wheels slip ［J］. Mechanism and Machine Theory， 2019， 138： 16-37.

［6］Khalaji A K. Pid-based target tracking control of a tractor-trailer mobile robot ［J］. Proceedings of the Institution of Mechanical Engineers， Part C. Journal of Mechanical Engineering Science， 2019， 233（13）： 4776-4787.

［7］Han X， Kim H J， Jeon C W， et al. Application of a 3D tractor-driving simulator for slip estimation-based path-tracking control of auto-guided tillage operation ［J］. Biosystems Engineering， 2019， 178： 70-85.

［8］Murillo M， Sanchez G， Deniz N， et al. Improving path-tracking performance of an articulated tractor-trailer system using a non-linear kinematic model ［J］. Computers and Electronics in Agriculture， 2022， 196： 106826.

［9］Nehme H， Aubry C， Solatges T， et al. Lidar-based structure tracking for agricultural robots： Application to autonomous navigation in vineyards ［J］. Journal of Intelligent amp; Robotic Systems， 2021， 103（4）： 61.

［10］Xu J， Lai J， Guo R， et al. Efficiency-oriented MPC algorithm for path tracking in autonomous agricultural machinery ［J］. Agronomy， 2022， 12（7）： 1662.

［11］Zhou S， Zhao H， Chen W， et al. Robust path following of the tractor-trailers system in GPS-denied environments ［J］. IEEE Robotics and Automation Letters， 2019， 5（2）： 500-507.

［12］武濤， 李彥明， 林洪振， 等. 基于干擾觀測器的直播機(jī)路徑跟蹤快速終端滑?？刂疲跩］. 農(nóng)業(yè)機(jī)械學(xué)報， 2021， 52（12）： 24-31.

Wu Tao， Li Yanming， Lin Hongzhen， et al. Fast terminal sliding mode control for autonomous rice seeding machine based on disturbance observer ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（12）： 24-31.

［13］張智剛， 王明昌， 毛振強(qiáng)， 等. 基于星基增強(qiáng)精密單點(diǎn)定位的農(nóng)機(jī)自動導(dǎo)航系統(tǒng)開發(fā)與測試［J］. 華南農(nóng)業(yè)大學(xué)學(xué)報， 2021， 42（6）： 109-116.

Zhang Zhigang， Wang Mingchang， Mao Zhenqiang， et al. Development and test of auto-navigation system for agricultural machinery based on satellite based precision single-point positioning ［J］. Journal of South China Agricultural University， 2021， 42（6）： 109-116.

［14］張培培， 楊自棟， 趙相君. 拖拉機(jī)—牽引式農(nóng)機(jī)具的運(yùn)動建模與跟蹤控制［J］. 中國農(nóng)機(jī)化學(xué)報， 2021， 42（4）： 128-133.

Zhang Peipei， Yang Zidong， Zhao Xiangjun. Kinematic model and tracking control for a trailed implement ［J］. Journal of Chinese Agricultural Mechanization， 2021， 42（4）： 128-133.

［15］狄桓宇， 張亞輝， 王博， 等. 自動駕駛橫向控制模型及方法研究綜述［J］. 重慶理工大學(xué)學(xué)報（自然科學(xué)）， 2021， 35（7）： 71-81.

Di Huanyu， Zhang Yahui， Wang Bo， et al. Review on the method and model of intelligent vehicle lateral control ［J］. Journal of Chongqing University of Technology （Natural Science）， 2021， 35（7）： 71-81.

［16］左帥， 劉成昊， 張笑， 等. 基于LQR的工程車輛自動駕駛路徑跟蹤技術(shù)研究［J］. 工程機(jī)械， 2022， 53（3）： 54-58， 10.

Zuo Shuai， Liu Chenghao， Zhang Xiao， et al. Research on automatic driving path tracking technology of engineering vehicles based on LQR ［J］. Construction Machinery and Equipment， 2022， 53 （03）： 54-58， 10.

［17］王鑫， 凌銘， 饒啟鵬， 等. 基于改進(jìn)Stanley算法的無人車路徑跟蹤融合算法研究［J］. 汽車技術(shù)， 2022（7）： 25-31.

Wang Xin， Ling Ming， Rao Qipeng， et al. Research on fusion algorithm of unmanned vehicle path tracking based on improved Stanley algorithm ［J］. Automotive Technology， 2022（7）： 25-31.

［18］熊璐， 楊興， 卓桂榮， 等. 無人駕駛車輛的運(yùn)動控制發(fā)展現(xiàn)狀綜述［J］. 機(jī)械工程學(xué)報， 2020， 56（10）： 127-143.

Xiong Lu， Yang Xing， Zhuo Guirong， et al. Review on motion control of autonomous vehicles ［J］. Journal of Mechanical Engineering， 2020， 56（10）： 127-143.

［19］楊菊花， 李文元， 陳光武， 等. 引入滑模觀測器的GPS/INS組合導(dǎo)航濾波方法［J］. 儀器儀表學(xué)報， 2019， 40（9）：78-86.

Yang Juhua， Li Wenyuan， Chen Guangwu， et al. GPS/INS integrated navigation filtering method based on sliding mode observer ［J］ Chinese Journal of Science Instrument， 2019， 40（9）： 78-86.

［20］孟秀云， 王語嫣. 一種SINS/GPS緊組合導(dǎo)航系統(tǒng)的改進(jìn)自適應(yīng)擴(kuò)展卡爾曼濾波算法［J］. 北京理工大學(xué)學(xué)報， 2018， 38（6）： 625-630， 636.

Meng Xiuyun， Wang Yuyan. An improved adaptive extended Kalman filter algorithm of SINS/GPS tightly-coupled integrated navigation system ［J］. Transactions of Beijing Institute of Technology， 2018， 38（6）： 625-630， 636.