武 濤 李彥明 徐長賡 劉漢文 陳小倩 劉成良

(1.上海交通大學(xué)機(jī)械與動力工程學(xué)院， 上海 200240； 2.上海上實(shí)現(xiàn)代農(nóng)業(yè)開發(fā)有限公司， 上海 202183)

0 引言

在水稻直播機(jī)實(shí)際的自主作業(yè)過程中，傳感器易受到環(huán)境狀況的干擾導(dǎo)致測量輸出不準(zhǔn)確或者某些狀態(tài)難以測得。此外，由于水田環(huán)境結(jié)構(gòu)復(fù)雜多變，也會給直播機(jī)帶來許多不確定性擾動[1-3]。因此，為提高水稻直播機(jī)在自動駕駛過程中的抗干擾能力以及得到較為準(zhǔn)確合理的狀態(tài)值，需采取一定的狀態(tài)估計(jì)方法。

許多學(xué)者在自動駕駛車輛狀態(tài)估計(jì)方法上進(jìn)行了大量的探索研究[4-11]。作為一種基于模型在隱馬爾可夫鏈上的遞歸貝葉斯估計(jì)，卡爾曼濾波(Kalman filter, KF)相關(guān)理論已非常成熟[12-13]，被證明是無約束系統(tǒng)最小方差意義下的最優(yōu)估計(jì)，目前是無約束動態(tài)系統(tǒng)狀態(tài)估計(jì)應(yīng)用最普遍的方法[14-17]?；诰€性模型的KF針對非線性模型無法實(shí)現(xiàn)精確估計(jì)，而采取將非線性模型線性化的擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF)和基于UT變換的無跡卡爾曼濾波(Unscented Kalman filter, UKF)能較精確地實(shí)現(xiàn)對無約束非線性模型狀態(tài)的估計(jì)[18-22]。文獻(xiàn)[18]提出一種基于EKF的多傳感融合通用框架，用于微型飛行器導(dǎo)航定位。該算法理論上能處理無限多類型傳感器及其延遲，以及傳感器相對或絕對測量，最終對飛行器的狀態(tài)信息進(jìn)行估計(jì)。文獻(xiàn)[20]分別采用EKF和UKF估計(jì)航天器的位置和速度，結(jié)果表明UKF的平均估計(jì)精度較高，因?yàn)槭褂孟闰?yàn)測量分布的無跡變換(UKF)比基于線性近似的EKF更接近原始分布。文獻(xiàn)[22]使用UKF融合GPS、IMU和車輪編碼器的測量值對車輛姿態(tài)進(jìn)行估計(jì)，使用Stanley控制器作為路徑跟蹤控制算法，提高了車輛自動駕駛的定位精度和魯棒性。雖然卡爾曼濾波被廣泛應(yīng)用，但是它不能處理狀態(tài)量的約束，有可能出現(xiàn)不符合物理實(shí)際的估計(jì)值，并且它要求系統(tǒng)模型必須準(zhǔn)確，否則可能出現(xiàn)發(fā)散的情況。

基于滾動優(yōu)化原理的滾動時域估計(jì)(Moving horizon estimation, MHE)通過將狀態(tài)估計(jì)問題轉(zhuǎn)化為固定時域的優(yōu)化問題，能較好地處理狀態(tài)量的約束和帶有不確定性的非線性系統(tǒng)，目前在跟蹤定位、故障診斷、參數(shù)估計(jì)等領(lǐng)域得到了廣泛的研究與應(yīng)用[23-30]。文獻(xiàn)[25]使用MHE狀態(tài)估計(jì)方法解決傳感器具有時滯、丟失值和不同頻率的問題，此外，還引入了線性模型MHE的高效計(jì)算方法，但僅適用于可以找到線性MHE問題解析解的情況。文獻(xiàn)[27] 針對移動機(jī)器人的位姿估計(jì)問題提出一種基于MHE高效通用的多傳感器融合方案，此方案基于多線程架構(gòu)，降低了計(jì)算成本。利用仿真和真實(shí)實(shí)驗(yàn)數(shù)據(jù)對該方案進(jìn)行測試，并與UKF進(jìn)行比較，結(jié)果表明MHE估計(jì)性能優(yōu)于UKF。文獻(xiàn)[30]基于縱滑側(cè)偏聯(lián)合工況下的八自由度整車模型，并考慮車輛系統(tǒng)現(xiàn)實(shí)存在的物理約束，使用MHE對車輛的速度等參數(shù)進(jìn)行了估計(jì)，估計(jì)結(jié)果較為符合實(shí)際情況。

針對無人水稻直播機(jī)自動駕駛過程中難以獲得精確的車輛模型以及傳感器測量存在誤差等問題，本文設(shè)計(jì)一種基于滾動時域的車輛運(yùn)動狀態(tài)估計(jì)方法，在每個采樣區(qū)間內(nèi)通過求解優(yōu)化問題的極小值估計(jì)出車輛在當(dāng)前時刻的狀態(tài)值：橫向偏差和航向角偏差，以提高車輛的抗干擾能力和路徑跟蹤控制精度。

1 滾動時域估計(jì)原理

基于模型預(yù)測滾動優(yōu)化原理來討論MHE的數(shù)學(xué)原理，受擾動離散的非線性狀態(tài)空間表達(dá)式為

(1)

式中k——采樣時刻x(k)——狀態(tài)變量

u(k)——控制輸入w(k)——外部干擾

z(k)——測量向量v(k)——測量噪聲

f、h——非線性過程和測量模型

狀態(tài)值和測量值歷史序列為

(2)

(3)

式中Qi、Ri——正定的懲罰矩陣，反映對模型干擾和測量噪聲的程度，通常為方差的對角矩陣

通過最小化式(3)可以求出歷史所有狀態(tài)的估計(jì)值，只取最后一個估計(jì)值作為當(dāng)前時刻的狀態(tài)值。新的采樣時刻到來，將新的測量數(shù)據(jù)補(bǔ)充進(jìn)測量序列，在線重新求解式(3)。但隨著時間的增加，數(shù)據(jù)越來越多，優(yōu)化問題越來越復(fù)雜，直至不可解。并且隨著時間的推移，過久的數(shù)據(jù)對當(dāng)前狀態(tài)的影響逐漸降低，因此，引入固定時域窗口N，考慮固定窗口的滾動時域估計(jì)，只使用最近N個狀態(tài)值和測量值，有

(4)

將系統(tǒng)模型作為等式約束，加上狀態(tài)和控制的不等式約束，用優(yōu)化問題替代全信息MHE問題,有

(5)

(6)

其中

(7)

式中Fk、Hk——系統(tǒng)模型和測量模型雅可比矩陣

由此，通過MHE算法將狀態(tài)估計(jì)問題轉(zhuǎn)換成固定時域的優(yōu)化問題，處理了系統(tǒng)約束、模型干擾和非線性以及測量噪聲，能提高估計(jì)的合理性和準(zhǔn)確性。

2 基于MHE的直播機(jī)運(yùn)動狀態(tài)估計(jì)器

2.1 帶擾動的直播機(jī)運(yùn)動學(xué)模型

要控制車輛首先要對車輛的運(yùn)動建立數(shù)學(xué)模型，模型越精確對車輛的運(yùn)動描述就越精確。由于在高度非結(jié)構(gòu)化水田環(huán)境下，直播機(jī)動力學(xué)模型參數(shù)難以獲取，因此建立簡單易用的運(yùn)動學(xué)模型。不考慮車輛在垂直方向的運(yùn)動，假設(shè)車身系統(tǒng)為剛性，左右側(cè)車輪具有相同的轉(zhuǎn)向角度和速度，忽略車輛動力學(xué)特性，將直播機(jī)簡化為二輪車模型，如圖1所示。圖中，M(xm,ym)為車體控制點(diǎn)；A(xa,ya)、B(xb,yb)為期望路徑P上的兩點(diǎn);θp為航向角的期望值。用橫向偏差、航向角偏差、前輪轉(zhuǎn)角、行駛速度和前后輪軸距等參數(shù)建立直播機(jī)的運(yùn)動學(xué)模型，從而描述直播機(jī)的運(yùn)動狀態(tài)。

圖1 水稻直播機(jī)運(yùn)動學(xué)模型Fig.1 Kinematic model of rice seeding machine

由于期望路徑AB固定，所以航向角偏差的導(dǎo)數(shù)與航向角速度相等，即

(8)

式中v——速度，m/s

R——轉(zhuǎn)彎半徑，m

θ——航向角，(°)

θe——航向角偏差，(°)

由圖1中幾何關(guān)系可得

(9)

式中L——前后輪軸距，m

δ——前輪轉(zhuǎn)角，控制輸入，(°)

因此

(10)

橫向偏差的導(dǎo)數(shù)即為速度v在該方向上的分量，即

(11)

式中de——橫向偏差，m

以橫向偏差de和航向角偏差θe為狀態(tài)變量，同時考慮水田中地況復(fù)雜多變具有許多不確定性干擾，為避免未知干擾導(dǎo)致建模不準(zhǔn)確，引入復(fù)合干擾項(xiàng)：橫向位置干擾|w1|≤P和航向角干擾|w2|≤K，P和K為干擾的最大值，由式(10)、(11)可得帶干擾的直播機(jī)運(yùn)動學(xué)模型為

(12)

在實(shí)際應(yīng)用中使用的是車輛離散模型，因此需對連續(xù)模型進(jìn)行離散化。令采樣周期為T，對式(12)進(jìn)行前向歐拉離散化得

(13)

式(13)離散模型的狀態(tài)量是基于橫向偏差和航向角偏差，可由車輛裝載的RTK-GPS傳感器測出的位姿信息與期望位姿信息計(jì)算，由此得測量模型

(14)

式中v1、v2——測量噪聲

2.2 MHE估計(jì)器方案

式(13)、(14)為直播機(jī)離散的非線性狀態(tài)空間表達(dá)式，可代入式(1)，通過MHE的求解公式(5)進(jìn)行直播機(jī)運(yùn)動狀態(tài)：橫向偏差和航向角偏差的估計(jì)?？傻肕HE算法流程：

(3)根據(jù)式(6)、(7)計(jì)算時刻k處狀態(tài)的后驗(yàn)估計(jì)協(xié)方差矩陣Pk。

(4)在k+1時刻，得到新狀態(tài)的測量值，更新窗口N內(nèi)的數(shù)據(jù)，重復(fù)步驟(2)、(3)。

優(yōu)化問題(式(5))的計(jì)算成本高度依賴于窗口N的大小，如果計(jì)算時間超過了傳感器的最大采樣周期，估計(jì)出的狀態(tài)則不是當(dāng)前采樣時刻的，需采取一定的方法提高M(jìn)HE的計(jì)算效率。如圖2所示，提出一種基于多線程的系統(tǒng)架構(gòu)，為M個主要傳感器的數(shù)據(jù)接收、分析與處理開辟單獨(dú)的線程，用來構(gòu)建測量矩陣，最后傳遞給MHE求解線程進(jìn)行優(yōu)化問題的求解。多個線程并行工作，可減少M(fèi)HE總體計(jì)算時間，提高M(jìn)HE采樣頻率。

圖2 多線程MHE求解架構(gòu)Fig.2 Multi-threading architecture of MHE

圖3 多頻率傳感器解決方案Fig.3 Solution of multi-frequency sensors

如圖3所示(圖中τ為所有測量傳感器采樣周期最大值)，該多線程架構(gòu)也能解決多個傳感器具有不同的測量頻率。圖中不同多邊形代表每個傳感器產(chǎn)生的有效值，雖然不同傳感器的數(shù)據(jù)可能會在不同時刻收到，但每個傳感器線程都會在有新數(shù)據(jù)到達(dá)時更新數(shù)據(jù)，因此傳給MHE求解線程的數(shù)據(jù)都是最新窗口內(nèi)的。針對傳感器出現(xiàn)的異常值使用馬氏距離閾值法進(jìn)行檢驗(yàn)剔除，計(jì)算式為

(15)

ζj——馬氏距離閾值

3 仿真驗(yàn)證

使用Matlab仿真驗(yàn)證MHE狀態(tài)估計(jì)算法的有效性。根據(jù)圖4控制系統(tǒng)結(jié)構(gòu)圖建立相應(yīng)的仿真模型?？刂扑惴ú捎梦墨I(xiàn)[3]提出的快速終端滑?？刂破?，控制車輛從起點(diǎn)移動到終點(diǎn)，通過向直播機(jī)運(yùn)動學(xué)模型和狀態(tài)值加高斯白噪聲來模擬系統(tǒng)干擾和測量誤差。使用MHE和EKF對帶噪聲的狀態(tài)進(jìn)行估計(jì)并與系統(tǒng)狀態(tài)真值比較，觀察MHE和EKF的估計(jì)效果。直播機(jī)系統(tǒng)控制仿真參數(shù)同文獻(xiàn)[3]，MHE仿真參數(shù)選取如表1所示。

圖4 控制系統(tǒng)結(jié)構(gòu)圖Fig.4 Block diagram of control system

考慮MHE計(jì)算時間不能超過傳感器采樣周期0.05 s，經(jīng)過仿真測試選取時域窗口值N等于5，單次計(jì)算所需時間約為0.01 s，滿足在線實(shí)時估計(jì)要求。使用高效的優(yōu)化控制軟件CasADi表述優(yōu)化問題，然后使用內(nèi)點(diǎn)法求解該優(yōu)化問題。圖5、6是整個行駛過程中車輛橫縱坐標(biāo)和航向角變化，藍(lán)色曲線是仿真得到的系統(tǒng)真值，紅色曲線是帶噪聲的測量值，綠色曲線在圖5、6中分別是使用MHE和EKF估計(jì)出的狀態(tài)值?？梢钥闯?，測量值具有很大的不確定性，在真值附近波動，使用MHE和EKF能有效地從測量值中估計(jì)出較為準(zhǔn)確的狀態(tài)值。

表1 MHE仿真參數(shù)Tab.1 Simulation parameters of MHE

圖5 MHE狀態(tài)估計(jì)結(jié)果Fig.5 States estimation results with MHE

圖6 EKF狀態(tài)估計(jì)結(jié)果Fig.6 States estimation results with EKF

圖7為MHE和EKF狀態(tài)估計(jì)絕對誤差散點(diǎn)圖，平均絕對誤差如表2所示?？梢钥闯?，MHE和EKF狀態(tài)估計(jì)誤差總體上比測量值誤差小很多，說明二者具備從不確定信息中提取精確信息的能力。此外，MHE狀態(tài)估計(jì)誤差也比EKF狀態(tài)估計(jì)誤差小，說明基于優(yōu)化方法求解的滾動時域狀態(tài)估計(jì)優(yōu)于傳統(tǒng)的基于估計(jì)誤差協(xié)方差的狀態(tài)估計(jì)，證明了本文所提MHE狀態(tài)估計(jì)算法的有效性。

圖7 MHE與EKF狀態(tài)估計(jì)絕對誤差Fig.7 Estimation error of MHE and EKF

表2 狀態(tài)估計(jì)平均絕對誤差Tab.2 Mean absolute error of states estimation

4 試驗(yàn)驗(yàn)證

4.1 試驗(yàn)平臺

試驗(yàn)采用中國洋馬農(nóng)機(jī)有限公司生產(chǎn)的YR10D型水稻直播機(jī)，對該型號直播機(jī)進(jìn)行自動駕駛改造，如圖8所示。自動駕駛系統(tǒng)主控制器采用裝載Linux系統(tǒng)的工業(yè)計(jì)算機(jī)，具有4核1.6 GHz高性能處理器、1 GB DDR3運(yùn)行內(nèi)存、4路帶隔離的3線 RS-232串口，1路CAN總線以及1路100 M以太網(wǎng)等通信接口。上位機(jī)主控制器通過CAN/RS-232的通信方式與車身各子系統(tǒng)進(jìn)行數(shù)據(jù)交互與協(xié)作，完成直播機(jī)自動駕駛和自主作業(yè)。下位機(jī)控制器為自行設(shè)計(jì)的STM32嵌入式控制器，具備CAN/RS-232通信方式。采用北京天?？乒旧a(chǎn)的DWQT-CAN-360-G型轉(zhuǎn)角傳感器，精度為0.1°，通信方式為CAN總線。RTK-GPS使用4G網(wǎng)絡(luò)差分基站，動態(tài)定位精度在±0.02 m，數(shù)據(jù)刷新率為20 Hz，通信方式為RS-232。方向盤使用直流電機(jī)驅(qū)動，采用位置-速度雙閉環(huán)控制，通信方式為RS-232。

圖8 無人駕駛水稻直播機(jī)Fig.8 Unmanned rice seeding machine

該無人水稻直播機(jī)具備自動駕駛作業(yè)、精量播種、種箱容量檢測及堵塞報警、自動倒車和全地塊封邊作業(yè)等功能。同時在阿里云服務(wù)器上搭建云監(jiān)控管理系統(tǒng)，能實(shí)現(xiàn)遠(yuǎn)程監(jiān)控、路徑規(guī)劃及多機(jī)協(xié)同作業(yè)管理。貫徹了“智慧在云，智能在端，管控在屏”的未來無人農(nóng)場設(shè)計(jì)理念。

4.2 水稻直播機(jī)水田無人作業(yè)試驗(yàn)

圖10 橫向偏差與航向角偏差估計(jì)結(jié)果Fig.10 Estimation results of lateral deviation and heading angle deviation

為驗(yàn)證本文所提MHE估計(jì)算法對無人水稻直播機(jī)運(yùn)動狀態(tài)的估計(jì)效果，在上海市崇明區(qū)上實(shí)現(xiàn)代農(nóng)業(yè)開發(fā)有限公司建立的無人農(nóng)場里使用圖8所示直播機(jī)進(jìn)行實(shí)際無人播種作業(yè)。圖9為直播機(jī)一次試驗(yàn)時部分作業(yè)軌跡，可以看出，雖然直播機(jī)行駛軌跡總體較為平穩(wěn)，但由于水田中具有許多不確定性干擾以及傳感器測量帶有噪聲，直播機(jī)行駛軌跡在某些區(qū)域也會出現(xiàn)一定抖動。為了獲取更加準(zhǔn)確的狀態(tài)值從而抑制這些抖動，獲取8段直線行駛路徑的直播機(jī)運(yùn)動狀態(tài)de和θe，使用MHE算法進(jìn)行估計(jì)。

圖9 直播機(jī)作業(yè)軌跡Fig.9 Driving paths of rice seeding machine

選取路徑4分析時域窗口N的大小對MHE估計(jì)效果的影響。圖10為N等于1、3、5、10時，使用MHE對橫向偏差與航向角偏差的估計(jì)結(jié)果?？梢钥闯?，由于水田中具有許多不確定性干擾以及傳感器測量帶有噪聲，測量值在短時間內(nèi)存在較大跳變，控制器算出的控制量也會不穩(wěn)定，從而導(dǎo)致車輛發(fā)生抖振。可以看出，MHE估計(jì)后的狀態(tài)變化相比測量值更加平穩(wěn)，具有一定抑制干擾的能力。根據(jù)MHE的原理可知，N等于1時MHE算法是一種EKF算法，此時估計(jì)值與測量值基本吻合，只在波峰波谷處對測量值的不穩(wěn)定具有一定的抑制作用。N越大對系統(tǒng)干擾和測量誤差的抑制能力越強(qiáng)，但過大不能反映正常的狀態(tài)變化，因?yàn)殡S著時間的推移，越早的數(shù)據(jù)對當(dāng)前時刻的參考意義越小。如N等于10時，MHE估計(jì)值偏離測量值過多，不符合實(shí)際情況。因此，為保證MHE對系統(tǒng)干擾和測量誤差的抑制具有良好的效果，同時也能較好地反映狀態(tài)的真實(shí)變化，一般N取3～5，此時整體估計(jì)效果也優(yōu)于EKF算法(N=1)。

圖11為N=3時，使用MHE算法對直播機(jī) 8段路徑橫向偏差與航向角偏差的估計(jì)結(jié)果?？梢钥闯?，8段路徑中橫向偏差基本保持在-0.1～0.1 m，航向角偏差基本保持在-2°～2°，但橫向偏差測量值出現(xiàn)全程不平穩(wěn)現(xiàn)象。MHE估計(jì)算法能有效改善這種現(xiàn)象，使得狀態(tài)值變化平穩(wěn)同時也能反映出實(shí)際變化趨勢。說明MHE能有效消除系統(tǒng)干擾和測量誤差對橫向偏差的影響。相對來說航向角偏差變化較平穩(wěn)，沒有較多噪聲，MHE估計(jì)算法對減小其變化幅值起到一定作用。對試驗(yàn)數(shù)據(jù)分析發(fā)現(xiàn)，每段路徑相對于理想軌跡都會有一定的橫向位置和航向角穩(wěn)態(tài)偏差。如表3，各段路徑穩(wěn)態(tài)偏差基本一致，其中橫向穩(wěn)態(tài)偏差均值為-0.068 8 m，航向角穩(wěn)態(tài)偏差均值為-1.467 0°。使用MHE估計(jì)后各穩(wěn)態(tài)偏差大幅下降，橫向穩(wěn)態(tài)偏差與航向角穩(wěn)態(tài)偏差均值分別為-0.019 4 m和-0.366 5°，說明MHE估計(jì)算法能消除干擾給狀態(tài)帶來的穩(wěn)態(tài)偏差。

圖11 所有路徑橫向偏差與航向角偏差估計(jì)結(jié)果Fig.11 Estimation results of lateral deviation and heading angle deviation of all driving path

各段路徑估計(jì)結(jié)果顯示，本文提出的基于滾動時域的水稻直播機(jī)運(yùn)動狀態(tài)估計(jì)算法能從不確定的狀態(tài)信息中得到較為準(zhǔn)確的狀態(tài)值，能較好地消除系統(tǒng)干擾和測量誤差 ，對下一環(huán)節(jié)的精確控制具有實(shí)際意義。

表3 各段路徑橫向與航向角穩(wěn)態(tài)偏差Tab.3 Steady-state error of lateral and heading angle

5 結(jié)論

(1)基于帶系統(tǒng)擾動的非線性水稻直播機(jī)運(yùn)動學(xué)模型和帶噪聲的測量模型，提出一種基于滾動時域的無人水稻直播機(jī)運(yùn)動狀態(tài)估計(jì)算法。同時，為提高M(jìn)HE狀態(tài)估計(jì)算法的計(jì)算效率和解決傳感器采樣頻率不同的問題，提出一種基于多線程的MHE求解架構(gòu)。使用Matlab/CasADi進(jìn)行系統(tǒng)仿真，結(jié)果表明，本文提出的MHE狀態(tài)估計(jì)算法能有效補(bǔ)償系統(tǒng)干擾和消除測量噪聲，得到接近理論真值的狀態(tài)，而且MHE的估計(jì)效果優(yōu)于EKF。

(2)在上海市崇明區(qū)無人農(nóng)場進(jìn)行了水稻直播機(jī)無人作業(yè)試驗(yàn)，使用MHE算法對某段直線路徑中測得的橫向偏差和航向角偏差值進(jìn)行估計(jì)。試驗(yàn)結(jié)果顯示，時域窗口N取3～5時，MHE對補(bǔ)償系統(tǒng)干擾和消除測量誤差具有良好的效果，同時也能較好地反映狀態(tài)值的真實(shí)變化。

(3)使用MHE狀態(tài)估計(jì)算法對水稻直播機(jī)8段路徑的橫向偏差與航向角偏差進(jìn)行估計(jì)。結(jié)果顯示，MHE算法對消除橫向位置、航向角的穩(wěn)態(tài)誤差和抑制測量值的不平穩(wěn)性具有良好的效果，同時也能反映出實(shí)際狀態(tài)變化趨勢，證明了MHE狀態(tài)估計(jì)算法在補(bǔ)償系統(tǒng)干擾和消除測量誤差方面的優(yōu)異性。