白曉平，王 卓，胡靜濤

(1.中國科學(xué)院 沈陽自動(dòng)化研究所，沈陽 110016；2.中國科學(xué)院大學(xué)，北京 100049)

?

基于有限狀態(tài)機(jī)的農(nóng)機(jī)導(dǎo)航多任務(wù)調(diào)度研究

白曉平1,2，王 卓1,2，胡靜濤1,2

(1.中國科學(xué)院 沈陽自動(dòng)化研究所，沈陽 110016；2.中國科學(xué)院大學(xué)，北京 100049)

為了解決總線式農(nóng)機(jī)導(dǎo)航控制系統(tǒng)中的事件響應(yīng)及數(shù)據(jù)延遲問題，將農(nóng)機(jī)導(dǎo)航控制功能劃分成不同優(yōu)先級(jí)的多個(gè)子任務(wù)模塊，將有限狀態(tài)機(jī)理論引入作為導(dǎo)航控制系統(tǒng)多任務(wù)調(diào)度的理論基礎(chǔ)?；谟邢逘顟B(tài)機(jī)理論和實(shí)際導(dǎo)航控制過程建立導(dǎo)航事件與導(dǎo)航控制任務(wù)的關(guān)聯(lián)關(guān)系，并對(duì)每個(gè)任務(wù)設(shè)有優(yōu)先級(jí)，實(shí)現(xiàn)了對(duì)導(dǎo)航控制事件的快速分級(jí)響應(yīng)。最后，采用基于本文提出的多任務(wù)調(diào)度方法的插秧機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)進(jìn)行了實(shí)車路徑跟蹤實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：本文所提調(diào)度方法可以很好地完成導(dǎo)航控制任務(wù)，直線路徑平均跟蹤誤差為3.28cm，曲線路徑平均跟蹤誤差為5.04cm。

農(nóng)機(jī)；導(dǎo)航系統(tǒng)；有限狀態(tài)機(jī)；多任務(wù)調(diào)度

0 引言

農(nóng)機(jī)的自動(dòng)導(dǎo)航技術(shù)是現(xiàn)代農(nóng)業(yè)機(jī)械裝備的重要支持技術(shù)之一。農(nóng)機(jī)的自動(dòng)導(dǎo)航可以減輕駕駛?cè)藛T的疲勞度，提高作業(yè)質(zhì)量和精度[1-3]。因此，農(nóng)機(jī)導(dǎo)航研究具有重要的理論研究意義和實(shí)用價(jià)值，已經(jīng)成為精準(zhǔn)農(nóng)業(yè)的重要研究?jī)?nèi)容。

早期的農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)，主要采用個(gè)人計(jì)算機(jī)或工控機(jī)為導(dǎo)航控制器。個(gè)人計(jì)算機(jī)或工控機(jī)價(jià)格高，且需要交流電源供電，不利于形成產(chǎn)品[3-7]。近年來，出現(xiàn)了基于CAN總線的分布式自動(dòng)導(dǎo)航控制系統(tǒng)[8-9]。分布式自動(dòng)導(dǎo)航控制系統(tǒng)中，位姿傳感器、導(dǎo)航控制器、轉(zhuǎn)向控制器及田間計(jì)算機(jī)各自獨(dú)立，彼此間的信息交互由CAN總線網(wǎng)絡(luò)來承擔(dān)。分布式導(dǎo)航控制系統(tǒng)的優(yōu)點(diǎn)是系統(tǒng)中任一獨(dú)立設(shè)備可以通過總線訪問任何一臺(tái)設(shè)備，實(shí)現(xiàn)了數(shù)據(jù)的高度共享。但是，分布式導(dǎo)航控制系統(tǒng)中，由于導(dǎo)航控制器與位姿傳感器各自獨(dú)立，如果采用定時(shí)讀取傳感器數(shù)據(jù)進(jìn)行導(dǎo)航控制的方式會(huì)引發(fā)位姿數(shù)據(jù)延遲問題，對(duì)控制精度有很大的影響。除此之外，如果采用主程序加定時(shí)的方式進(jìn)行導(dǎo)航控制的設(shè)計(jì)所有事件并沒有等級(jí)之分，當(dāng)重要的事件到來時(shí)如果有其他事件處理代碼正在執(zhí)行，則該事件只能等待該事件處理完畢后才能得以處理，這種對(duì)重要事件的延時(shí)處理會(huì)造成導(dǎo)航出錯(cuò)。

針對(duì)以上問題，本文將導(dǎo)航控制任務(wù)劃分成多個(gè)子任務(wù)，并將實(shí)時(shí)操作系統(tǒng)和有限狀態(tài)機(jī)理論引入實(shí)現(xiàn)了任務(wù)與導(dǎo)航控制事件的關(guān)聯(lián)[10-14]。本文將定時(shí)讀取傳感器位姿數(shù)據(jù)改為傳感器定時(shí)向?qū)Ш娇刂破靼l(fā)送最新的位姿數(shù)據(jù)，在接收到位姿數(shù)據(jù)后導(dǎo)航控制器立即計(jì)算控制量、發(fā)布控制命令，有效地避免了傳感器位姿數(shù)據(jù)延遲問題。本文還將重要的外部事件與單獨(dú)的處理任務(wù)相關(guān)聯(lián)，并對(duì)每個(gè)任務(wù)設(shè)有優(yōu)先級(jí)，從而解決了導(dǎo)航控制系統(tǒng)存在事件響應(yīng)延遲的問題。

1 農(nóng)機(jī)導(dǎo)航控制系統(tǒng)簡(jiǎn)介

1.1 硬件結(jié)構(gòu)

導(dǎo)航控制系統(tǒng)主要由導(dǎo)航控制器、田間計(jì)算機(jī)、轉(zhuǎn)向控制單元、位置傳感器和航向傳感器及轉(zhuǎn)向角傳感器6部分組成，如圖1所示。

圖1 硬件結(jié)構(gòu)圖

其中，導(dǎo)航控制器、位置傳感器和航向傳感器是導(dǎo)航控制系統(tǒng)的核心部件，導(dǎo)航控制器根據(jù)航向傳感器和位置傳感器的數(shù)據(jù)實(shí)時(shí)計(jì)算控制量，從而實(shí)現(xiàn)路徑的準(zhǔn)確跟蹤。為共享航向傳感器和位置傳感器的數(shù)據(jù)，在導(dǎo)航控制器與這兩個(gè)傳感器之間加入了智能節(jié)點(diǎn)硬件模塊。智能節(jié)點(diǎn)將航向傳感器與智能傳感器的數(shù)據(jù)解析出來并在導(dǎo)航控制器需要時(shí)發(fā)送到總線上。將以上所述硬件部分集成到一起開發(fā)了如圖2所示的導(dǎo)航控制箱。

圖2 導(dǎo)航控制箱

田間計(jì)算機(jī)為導(dǎo)航控制系統(tǒng)提供了人機(jī)交互平臺(tái)。田間計(jì)算機(jī)主要負(fù)責(zé)路徑的采集、設(shè)置及各種導(dǎo)航命令的發(fā)布工作；還負(fù)責(zé)導(dǎo)航控制數(shù)據(jù)的保存和導(dǎo)航運(yùn)行情況的監(jiān)測(cè)和顯示工作。

轉(zhuǎn)向控制單元是導(dǎo)航控制系統(tǒng)的執(zhí)行器，主要負(fù)責(zé)使車輪轉(zhuǎn)到導(dǎo)航控制器指定輸出角度；轉(zhuǎn)向角傳感器則主要用于當(dāng)前車輪轉(zhuǎn)角的檢測(cè)，與轉(zhuǎn)向控制單元共同構(gòu)成了自動(dòng)轉(zhuǎn)向系統(tǒng)。

1.2 軟件結(jié)構(gòu)

導(dǎo)航控制系統(tǒng)的軟件主要由如圖3所示的5個(gè)功能模塊組成。

圖3 軟件結(jié)構(gòu)圖

中斷服務(wù)任務(wù)模塊是導(dǎo)航控制系統(tǒng)與田間計(jì)算機(jī)和傳感器交互的接口任務(wù)，主要負(fù)責(zé)外部信息和命令的接收與解析工作。參數(shù)設(shè)置任務(wù)模塊主要負(fù)責(zé)導(dǎo)航控制參數(shù)和期望路徑的設(shè)置工作，是導(dǎo)航控制的前提基礎(chǔ)，只有正確設(shè)置了這些參數(shù)才能開始導(dǎo)航作業(yè)。導(dǎo)航作業(yè)控制任務(wù)模塊是導(dǎo)航控制的核心功能模塊，主要負(fù)責(zé)控制量的計(jì)算。信息交互任務(wù)模塊負(fù)責(zé)收集導(dǎo)航作業(yè)控制所需的傳感器數(shù)據(jù)及發(fā)送導(dǎo)航作業(yè)中產(chǎn)生的導(dǎo)航控制信息。故障處理任務(wù)模塊負(fù)責(zé)導(dǎo)航控制中錯(cuò)誤處理與錯(cuò)誤報(bào)警工作。

2 基于有限狀態(tài)機(jī)的多任務(wù)調(diào)度研究

農(nóng)機(jī)導(dǎo)航控制系統(tǒng)共劃分成了參數(shù)設(shè)置任務(wù)、導(dǎo)航作業(yè)控制任務(wù)、中斷服務(wù)任務(wù)、信息交互任務(wù)和故障處理任務(wù)5個(gè)任務(wù)模塊。這5個(gè)任務(wù)如何調(diào)度直接影響到導(dǎo)航控制系統(tǒng)的性能。導(dǎo)航控制系統(tǒng)的調(diào)度問題歸結(jié)為在一定的觸發(fā)條件下，系統(tǒng)將使5個(gè)任務(wù)中某一個(gè)任務(wù)進(jìn)入運(yùn)行態(tài)，并且某一確定時(shí)刻只能有一個(gè)任務(wù)處于運(yùn)行態(tài)的問題。系統(tǒng)調(diào)度設(shè)計(jì)的目標(biāo)是使得這6個(gè)任務(wù)協(xié)調(diào)、有序的運(yùn)行，共同完成導(dǎo)航控制任務(wù)。本文將有限狀態(tài)機(jī)理論引入到系統(tǒng)調(diào)度的設(shè)計(jì)中，建立了導(dǎo)航控制系統(tǒng)工況狀態(tài)機(jī)，并基于工況狀態(tài)機(jī)和狀態(tài)轉(zhuǎn)移圖進(jìn)行了導(dǎo)航控制系統(tǒng)多任務(wù)的設(shè)計(jì)。

2.1 有限狀態(tài)機(jī)理論簡(jiǎn)介

有限狀態(tài)機(jī)是計(jì)算機(jī)科學(xué)和數(shù)學(xué)理論的抽象，是離散輸入輸出系統(tǒng)的模型。有限狀態(tài)機(jī)理論認(rèn)為：系統(tǒng)可以有多個(gè)狀態(tài)，但某一確定時(shí)刻系統(tǒng)只能處于多個(gè)狀態(tài)中的一個(gè)；系統(tǒng)從一個(gè)狀態(tài)轉(zhuǎn)移到另一個(gè)狀態(tài)不僅取決于觸發(fā)條件還取決于當(dāng)前所處的狀態(tài)[13-14]。有限狀態(tài)機(jī)包括以下幾個(gè)要素：

1)狀態(tài)。行為模型的基本組成部分，反映了系統(tǒng)中某個(gè)對(duì)象所處的階段和活動(dòng)情況。

2)轉(zhuǎn)移。對(duì)象從一個(gè)狀態(tài)轉(zhuǎn)移到另一個(gè)狀態(tài)的過程。

3)事件。引起對(duì)象狀態(tài)轉(zhuǎn)化的事件及條件。

4)動(dòng)作。在狀態(tài)轉(zhuǎn)移時(shí)對(duì)象所采取的行動(dòng)。

2.2 有限狀態(tài)機(jī)狀態(tài)確定

導(dǎo)航控制系統(tǒng)在導(dǎo)航控制過程中會(huì)處于很多不同的運(yùn)行狀態(tài)，根據(jù)實(shí)際導(dǎo)航控制過程可以得到如表1所示的6種運(yùn)行狀態(tài)。

其中，空閑狀態(tài)是導(dǎo)航控制系統(tǒng)上電后無任何操作時(shí)所處的狀態(tài)，導(dǎo)航控制系統(tǒng)處于此種狀態(tài)時(shí)會(huì)執(zhí)行一些無實(shí)際意義的代碼，將這些代碼的集合稱為空閑任務(wù)。其余的5個(gè)狀態(tài)是以5個(gè)導(dǎo)航控制任務(wù)模塊為基礎(chǔ)的，每個(gè)導(dǎo)航任務(wù)模塊是導(dǎo)航系統(tǒng)處于該狀態(tài)下時(shí)導(dǎo)航控制器執(zhí)行的功能代碼的總和。這5種狀態(tài)會(huì)在事件的觸發(fā)下發(fā)生轉(zhuǎn)移，每次狀態(tài)轉(zhuǎn)移表示一次任務(wù)切換。任務(wù)模塊與有限狀態(tài)機(jī)狀態(tài)的對(duì)應(yīng)關(guān)系如表2所示。

表1 導(dǎo)航控制系統(tǒng)狀態(tài)

2.3 有限狀態(tài)機(jī)觸發(fā)事件的確定

事件是驅(qū)動(dòng)狀態(tài)機(jī)狀態(tài)轉(zhuǎn)化的原動(dòng)力。表2給出了導(dǎo)航控制過程中的事件，這些事件是由田間計(jì)算機(jī)輸入的信息直接引起或者間接引起的。事件可以有相關(guān)變量，從而使得事件不僅能傳遞命令，同時(shí)可以攜帶自己的參數(shù)。每個(gè)事件具有生命周期，每個(gè)事件都會(huì)經(jīng)過接收、解析和處理3個(gè)過程，在經(jīng)過這3個(gè)過程后事件將不再會(huì)對(duì)狀態(tài)機(jī)有效。

表2 導(dǎo)航控制系統(tǒng)的觸發(fā)事件

2.4 農(nóng)機(jī)導(dǎo)航控制系統(tǒng)工況狀態(tài)機(jī)

導(dǎo)航控制系統(tǒng)可以看成一個(gè)有限狀態(tài)機(jī)，本文稱其為工況狀態(tài)機(jī)。導(dǎo)航控制系統(tǒng)中多個(gè)子任務(wù)的調(diào)度問題則可以看成是在事件驅(qū)動(dòng)下狀態(tài)轉(zhuǎn)移問題。根據(jù)實(shí)際導(dǎo)航控制過程可以建立如圖4所示的狀態(tài)轉(zhuǎn)移圖。

2.5 農(nóng)機(jī)導(dǎo)航控制系統(tǒng)多任務(wù)設(shè)計(jì)

導(dǎo)航控制系統(tǒng)多任務(wù)的設(shè)計(jì)原則是滿足軟件總體結(jié)構(gòu)各功能模塊的功能并滿足圖4所示的狀態(tài)轉(zhuǎn)移關(guān)系。根據(jù)以上原則共創(chuàng)建了空閑任務(wù)、參數(shù)設(shè)置任務(wù)、中斷處理任務(wù)、導(dǎo)航作業(yè)任務(wù)、信息交互任務(wù)和故障處理任務(wù)等6個(gè)任務(wù)。

圖4 狀態(tài)轉(zhuǎn)移圖

空閑任務(wù)是實(shí)時(shí)操作系統(tǒng)自帶的任務(wù)，啟動(dòng)任務(wù)讓出CPU控制權(quán)后，空閑任務(wù)得到CPU控制權(quán)執(zhí)行空閑任務(wù)(導(dǎo)航控制系統(tǒng)處于q0狀態(tài))。

發(fā)生外部中斷(d1事件發(fā)生)后，中斷處理函數(shù)會(huì)簡(jiǎn)單地將中斷接收到的原始信息放入中斷接收緩沖序列，接著會(huì)將中斷處理任務(wù)的優(yōu)先級(jí)提到最高，從而實(shí)現(xiàn)其余任務(wù)到中斷處理任務(wù)的切換，即導(dǎo)航控制系統(tǒng)的狀態(tài)轉(zhuǎn)移到中斷處理狀態(tài)(其余狀態(tài)轉(zhuǎn)移到q2狀態(tài))。中斷處理任務(wù)會(huì)查詢中斷緩沖序列中是否還有未處理信息，如果有則提出信息進(jìn)行處理，如果中斷緩沖序列已經(jīng)為空，則表明信息已經(jīng)處理完畢。中斷處理任務(wù)會(huì)根據(jù)信息類型來判斷是何種事件發(fā)生。如果是參數(shù)設(shè)置命令(d2事件發(fā)生)，則會(huì)發(fā)送參數(shù)設(shè)置信號(hào)量從而實(shí)現(xiàn)從該任務(wù)到參數(shù)設(shè)置任務(wù)的切換(q2轉(zhuǎn)移到q1狀態(tài))；如果接收到的是位姿信息接收命令(d3事件發(fā)生)，該任務(wù)執(zhí)行完畢導(dǎo)航系統(tǒng)將轉(zhuǎn)向信息交互狀態(tài)的轉(zhuǎn)移(q2轉(zhuǎn)移到q4)；如果是開啟導(dǎo)航命令(d4事件發(fā)生)，則將導(dǎo)航作業(yè)控制任務(wù)和信息交互任務(wù)解除掛起，從而實(shí)現(xiàn)從該任務(wù)向?qū)Ш阶鳂I(yè)控制任務(wù)的轉(zhuǎn)移(q2轉(zhuǎn)移到q3)；如果是關(guān)閉導(dǎo)航命令(d9事件發(fā)生)，則將導(dǎo)航作業(yè)控制任務(wù)和信息交互任務(wù)掛起，停止導(dǎo)航作業(yè)，導(dǎo)航系統(tǒng)轉(zhuǎn)入q0態(tài)(q2轉(zhuǎn)移到q0)。中斷信息處理完畢后，該任務(wù)會(huì)將自身的優(yōu)先級(jí)恢復(fù)，并切換到其他任務(wù)執(zhí)行。

參數(shù)設(shè)置任務(wù)是導(dǎo)航控制的前提任務(wù)，只有完成了導(dǎo)航控制的參數(shù)設(shè)置才可以進(jìn)行導(dǎo)航控制，否則會(huì)發(fā)生導(dǎo)航作業(yè)錯(cuò)誤。該任務(wù)主要負(fù)責(zé)執(zhí)行導(dǎo)航作業(yè)前控制算法參數(shù)和期望路徑參數(shù)的設(shè)置。該任務(wù)收到中斷處理任務(wù)的通知后，會(huì)根據(jù)指令完成導(dǎo)航控制參數(shù)的設(shè)定和期望路徑的設(shè)置工作。

導(dǎo)航作業(yè)控制任務(wù)是導(dǎo)航控制的核心子任務(wù)，主要負(fù)責(zé)控制量的計(jì)算。開啟導(dǎo)航作業(yè)后，導(dǎo)航控制任務(wù)會(huì)定時(shí)進(jìn)行控制量的計(jì)算和發(fā)送，從而實(shí)現(xiàn)聯(lián)合收割機(jī)對(duì)期望路徑的準(zhǔn)確跟蹤。導(dǎo)航作業(yè)控制任務(wù)還負(fù)責(zé)傳感器數(shù)據(jù)和誤差是否發(fā)散兩項(xiàng)監(jiān)測(cè)工作，在傳感器數(shù)據(jù)不正?；蛘邔?dǎo)航誤差發(fā)散的情況下發(fā)送故障信號(hào)。導(dǎo)航控制任務(wù)首先發(fā)送停止接收傳感器數(shù)據(jù)命令，表明在導(dǎo)航控制任務(wù)執(zhí)行期間不再接收傳感器數(shù)據(jù)，這樣可以防止數(shù)據(jù)偷換錯(cuò)誤發(fā)生；接著該任務(wù)會(huì)判斷此時(shí)接收到的新的傳感器數(shù)據(jù)的個(gè)數(shù)是否在正常的容限內(nèi)，如果超出容限則發(fā)送傳感器故障信號(hào)(d8事件發(fā)生)，故障處理任務(wù)由于得到該信號(hào)而切換到轉(zhuǎn)入運(yùn)行態(tài)(q3轉(zhuǎn)移到q5狀態(tài))。傳感器數(shù)據(jù)正常，則表明導(dǎo)航控制任務(wù)可以繼續(xù)執(zhí)行，該任務(wù)會(huì)計(jì)算控制量并將控制量發(fā)送給轉(zhuǎn)向控制單元；完成本次導(dǎo)航控制量計(jì)算后，該任務(wù)會(huì)調(diào)用延時(shí)函數(shù)將此任務(wù)掛起一個(gè)延時(shí)周期(d6事件發(fā)生)，此時(shí)系統(tǒng)會(huì)轉(zhuǎn)移到q4狀態(tài)(q3轉(zhuǎn)移到q4狀態(tài))。

信息交互任務(wù)負(fù)責(zé)發(fā)送導(dǎo)航作業(yè)控制中產(chǎn)生的航向偏差、橫向偏差等各種導(dǎo)航控制中產(chǎn)生的信息給田間計(jì)算機(jī)，并負(fù)責(zé)接收傳感器的數(shù)據(jù)。信息交互任務(wù)在導(dǎo)航作業(yè)任務(wù)的定時(shí)間隔中被啟用(d6事件發(fā)生)，該任務(wù)首先將本次導(dǎo)航作業(yè)控制中產(chǎn)生的誤差信息和航向信息發(fā)送給田間計(jì)算機(jī)，然后發(fā)送傳感器數(shù)據(jù)接收信號(hào)到總線上，傳感器會(huì)在收到停止接收傳感器數(shù)據(jù)信號(hào)之前發(fā)送傳感器數(shù)據(jù)到總線上。在接收傳感器的數(shù)據(jù)過程中，該任務(wù)會(huì)對(duì)接收數(shù)據(jù)個(gè)數(shù)進(jìn)行計(jì)數(shù)，從而判斷傳感器是否正常；導(dǎo)航作業(yè)控制任務(wù)定時(shí)(d7事件發(fā)生)到來后會(huì)結(jié)束該任務(wù)的運(yùn)行，轉(zhuǎn)而執(zhí)行導(dǎo)航作業(yè)控制任務(wù)(q4轉(zhuǎn)移到q3狀態(tài))。

故障處理任務(wù)主要負(fù)責(zé)導(dǎo)航過程中的故障處理工作。此軟件考慮了兩種導(dǎo)航作業(yè)故障情況：一是傳感器長(zhǎng)時(shí)間沒有發(fā)送數(shù)據(jù)給導(dǎo)航控制器；二是導(dǎo)航作業(yè)誤差發(fā)散。這兩種情況下，都會(huì)造成導(dǎo)航控制作業(yè)錯(cuò)誤。該任務(wù)通過判斷錯(cuò)誤類型號(hào)來決定是哪種錯(cuò)誤，根據(jù)錯(cuò)誤類型發(fā)送對(duì)應(yīng)錯(cuò)誤信息幀給田間計(jì)算機(jī)報(bào)警，并通過將導(dǎo)航作業(yè)控制任務(wù)掛起來停止導(dǎo)航作業(yè)；停止導(dǎo)航作業(yè)后，該任務(wù)會(huì)發(fā)送停止電機(jī)轉(zhuǎn)動(dòng)命令給轉(zhuǎn)向控制單元停止電機(jī)旋轉(zhuǎn)從而聯(lián)合收割機(jī)將停止運(yùn)行；故障處理完畢(d8事件發(fā)生)，系統(tǒng)轉(zhuǎn)入空閑態(tài)(q5轉(zhuǎn)移到q0狀態(tài))。

3 多任務(wù)調(diào)度方案的性能分析

3.1 數(shù)據(jù)延遲問題分析

數(shù)據(jù)延遲問題是指導(dǎo)航控制器獲得的傳感器位姿數(shù)據(jù)并不是當(dāng)前的位姿數(shù)據(jù)，而是該時(shí)刻以前的傳感器數(shù)據(jù)。數(shù)據(jù)延遲會(huì)降低導(dǎo)航控制精度，在導(dǎo)航控制中應(yīng)該盡量減小數(shù)據(jù)延遲時(shí)間。數(shù)據(jù)延遲問題對(duì)導(dǎo)航控制精度的影響可以用式(1)的車輛運(yùn)動(dòng)學(xué)模型來進(jìn)行分析。則有

(1)

其中，dt是時(shí)間間隔；dx、dy分別為車輛坐標(biāo)沿設(shè)定軌跡的切向和法向在dt時(shí)間內(nèi)的變化量。

分布式導(dǎo)航控制系統(tǒng)使用GPS作為位置檢測(cè)工具，一般采樣間隔選為0.2s。如果采用定時(shí)方式進(jìn)行導(dǎo)航控制，定時(shí)時(shí)間到導(dǎo)航控制器發(fā)出讀取GPS數(shù)據(jù)命令，GPS節(jié)點(diǎn)會(huì)將最近一次解析到的完整的GPS位置數(shù)據(jù)發(fā)送給導(dǎo)航控制器。但是，最后解析到的數(shù)據(jù)并一定是這一時(shí)刻的GPS位置數(shù)據(jù)，也就是說導(dǎo)航控制器讀到的GPS數(shù)據(jù)具有一定的時(shí)間延遲，這個(gè)延遲最大可以達(dá)到0.2s。假設(shè)農(nóng)機(jī)行駛的速度v=1.2m/s,航向角為θ=30°，由式(1)可以得到法向位姿延遲偏差和切向位姿延遲偏差，有

其中，dy為車輛導(dǎo)航的橫向偏差的變化量，是導(dǎo)航控制決策的重要依據(jù)。dy=0.119 9表明：用于導(dǎo)航控制的橫向偏差數(shù)據(jù)比當(dāng)前實(shí)際的橫向偏差值小0.119 9。顯然，這會(huì)給控制決策帶來一定的偏差，從而影響導(dǎo)航控制精度。

本文采用將GPS位姿數(shù)據(jù)接收事件與控制任務(wù)綁定的方法來解決數(shù)據(jù)延遲問題。GPS解析到一幀GPS數(shù)據(jù)后，GPS節(jié)點(diǎn)就將此數(shù)據(jù)發(fā)送到總線上,導(dǎo)航控制器接收到一幀完整的GPS數(shù)據(jù)后立即啟動(dòng)控制任務(wù)進(jìn)行一次控制量的計(jì)算，保證了用于導(dǎo)航控制的GPS數(shù)據(jù)就是當(dāng)前車輛的位置信息，避免了數(shù)據(jù)延遲問題。而導(dǎo)航控制器輸出控制量的頻率基本上等于GPS傳感器的采樣頻率，可以通過調(diào)節(jié)GPS傳感器的采樣頻率來調(diào)節(jié)控制量的輸出頻率。這表明在傳感器采樣頻率一定的條件下，可以最大限度地提高控制頻率；而在一定的范圍內(nèi)，控制頻率越高，控制精度越好。

3.2 導(dǎo)航事件響應(yīng)速度分析

如果采用定時(shí)的方式進(jìn)行導(dǎo)航控制，對(duì)外部事件的響應(yīng)一般有兩種方式：一是外部事件到來后直接在中斷函數(shù)中進(jìn)行處理。這種方式可保證最先到來的事件得到最快的響應(yīng)，但如果先來的事件處理時(shí)間比較長(zhǎng)則會(huì)導(dǎo)致后面的事件遲遲得不到響應(yīng)，從而造成中斷擁塞甚至系統(tǒng)崩潰，所以一般不建議使用這種事件處理方式。二是在接收到中斷處理事件后將響應(yīng)標(biāo)志位置位，然后在主循環(huán)中加入對(duì)事件的處理代碼。主循環(huán)在運(yùn)行過程中不斷的查詢?cè)摌?biāo)志位，當(dāng)查詢到該標(biāo)志位置位時(shí)即執(zhí)行時(shí)間處理代碼，一般采用這種方式對(duì)中斷事件進(jìn)行響應(yīng)。這種方式的缺點(diǎn)是當(dāng)中斷事件到來后，系統(tǒng)并不是立即進(jìn)行響應(yīng)而是繼續(xù)執(zhí)行主循環(huán)程序，只有當(dāng)查詢到標(biāo)志位時(shí)才進(jìn)行響應(yīng)。如果從中斷發(fā)生到執(zhí)行到查詢?cè)摌?biāo)志位程序之間有執(zhí)行時(shí)間比較長(zhǎng)的代碼在運(yùn)行，則中斷事件的響應(yīng)會(huì)有很大的延遲。為了彌補(bǔ)以上兩種方式的缺陷，本文采用事件與導(dǎo)航控制任務(wù)綁定，并對(duì)每個(gè)任務(wù)設(shè)有優(yōu)先級(jí)的方式來對(duì)外部事件進(jìn)行相應(yīng)。當(dāng)導(dǎo)航控制器收到導(dǎo)航控制命令后不需查詢標(biāo)志直接執(zhí)行與該事件綁定的任務(wù)，當(dāng)有更重要的任務(wù)要執(zhí)行時(shí)則會(huì)通過優(yōu)先級(jí)進(jìn)行CPU控制權(quán)的競(jìng)爭(zhēng)。

例如：當(dāng)田間計(jì)算機(jī)發(fā)出導(dǎo)航停止命令時(shí)，采用這種方式中斷事件到來后中斷程序中設(shè)置標(biāo)志位。但是如果在該中斷發(fā)生前導(dǎo)航控制器在執(zhí)行導(dǎo)航控制程序，則設(shè)置完標(biāo)志位后將繼續(xù)執(zhí)行導(dǎo)航控制程序，當(dāng)執(zhí)行完畢后會(huì)查詢導(dǎo)航停止標(biāo)志位停止導(dǎo)航控制。這說明已經(jīng)接收到導(dǎo)航控制命令，但由于查詢方式不會(huì)立即對(duì)該事件進(jìn)行響應(yīng)，控制器會(huì)再進(jìn)行一次導(dǎo)航控制，顯然這是我們不希望的。為了解決該問題，本文將導(dǎo)航停止事件與導(dǎo)航停止控制任務(wù)綁定。當(dāng)程序在執(zhí)行導(dǎo)航控制任務(wù)過程中，如果收到停止導(dǎo)航命令，中斷程序發(fā)出停止中斷信號(hào)量，退出中斷后由于停止導(dǎo)航任務(wù)優(yōu)先級(jí)高并且執(zhí)行條件得到滿足會(huì)搶占CPU控制權(quán)而得以執(zhí)行，從而立即停止導(dǎo)航。

4 實(shí)車路徑跟蹤實(shí)驗(yàn)

將導(dǎo)航控制系統(tǒng)安裝在圖6所示的插秧機(jī)上，構(gòu)成了插秧機(jī)實(shí)驗(yàn)平臺(tái)。采用本文提出的多任務(wù)設(shè)計(jì)方案，在較平整的路面上進(jìn)行了直線路徑與曲線路徑的跟蹤實(shí)驗(yàn)。

圖6 插秧機(jī)實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)結(jié)果表明：1.0m/s速度下，平均偏差為3.28cm。直線路徑跟蹤誤差曲線如圖7所示。

圖7 直線路徑跟蹤曲線

在1.0m/s速度下，平均跟蹤誤差為5.04cm。曲線路徑跟蹤誤差曲線分別如圖8所示。

圖8 曲線路徑跟蹤曲線

5 結(jié)論

1)提出了一種基于有限狀態(tài)機(jī)的導(dǎo)航控制系統(tǒng)多任務(wù)調(diào)度方法。通過分析，該調(diào)度方法采用將導(dǎo)航控制事件與導(dǎo)航控制子任務(wù)綁定，并對(duì)導(dǎo)航控制子任務(wù)設(shè)置優(yōu)先級(jí)，解決了導(dǎo)航控制事件響應(yīng)延遲和數(shù)據(jù)延遲問題。

2)實(shí)車實(shí)驗(yàn)結(jié)果表明：基于有限狀態(tài)機(jī)設(shè)計(jì)的多任務(wù)調(diào)度方案使得多個(gè)任務(wù)有序、快速地運(yùn)行，多個(gè)任務(wù)相互協(xié)作共同完成了導(dǎo)航控制目標(biāo)。

3)軟件的故障處理任務(wù)實(shí)現(xiàn)了聯(lián)合收割機(jī)在故障情況下的錯(cuò)誤報(bào)警和故障處理，減少了不必要的作業(yè)損失，避免出現(xiàn)危險(xiǎn)狀況。

4)故障處理任務(wù)只對(duì)少數(shù)幾種故障情況進(jìn)行了考慮，實(shí)際導(dǎo)航作業(yè)中可能會(huì)遇到更多更復(fù)雜的故障，所以還需要在實(shí)際作業(yè)中不斷加強(qiáng)故障處理功能。

[1] 胡靜濤，高雷，白曉平，等.農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)研究進(jìn)展[J].農(nóng)業(yè)工程學(xué)報(bào)，2015，31(10)：1-10.

[2] 熊中剛,葉振環(huán),賀娟，等.基于免疫模糊PID的小型農(nóng)業(yè)機(jī)械路徑智能跟蹤控制[J].機(jī)器人,2015(2):212-223.

[3] 張智剛，羅錫文，周志艷，等．久保田插秧機(jī)的GPS導(dǎo)航控制系統(tǒng)設(shè)計(jì)[J]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，2006，37(7)：95-97．

[4] 安秋，姬長(zhǎng)英，周俊，等．基于CAN總線的農(nóng)業(yè)移動(dòng)機(jī)器人分布式控制網(wǎng)絡(luò)[J]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，2008，39(6)：123-126．

[5] Qin Zhang, John F Reid. Automated guidance control for agricultural tractor using redundant sensors[Z]. Pittsburg:PA,1999.

[6] Matsuo Y, Hamada Y, Yukumoto O, et al.Auto-steering ricetransplanter for straight traveling[C]//Kyoto,Japan.International Conference on Automation Technology for Off-Road Equipment,2004: 292-297.

[7] Darr M J, Stombaugh T S, Shearer S A. Controller areanetwork based distributed control for autonomous vehicles[J].American Society of Agricultural Engineers, 2005, 48(2):479-490.

[8] 胡煉，羅錫文，張志剛，等．基于CAN總線的分布式插秧機(jī)導(dǎo)航控制系統(tǒng)設(shè)計(jì)[J]．農(nóng)業(yè)工程學(xué)報(bào)，2009，25(12)：88-92．

[9] 陳康,周志峰,王永泉，等.基于G NSS的農(nóng)機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)設(shè)計(jì)[J].化工自動(dòng)化及儀表,2014(11):1294-1297.

[10] 楊明極，劉繼深，梁晶．基于μC/OS-Ⅱ的嵌入式車載微功率MP3的軟件設(shè)計(jì)[J]．哈爾濱理工大學(xué)學(xué)報(bào)，2009,14(Z1):82-85.

[11] 童曉斌，戴瑜興．基于實(shí)時(shí)操作系統(tǒng)μC/OS-Ⅱ的智能控制器嵌入式軟件設(shè)計(jì)[J]．電氣應(yīng)用，2007，26(8)：77-80．

[12] 王俊席，楊林，祝軻卿，等．基于實(shí)時(shí)操作系統(tǒng)的發(fā)動(dòng)機(jī)控制軟件開發(fā)平臺(tái)設(shè)計(jì)[J]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，2006，37(9)：36-38．

[13] 孫維堂，劉永賢，張禹，等．有限狀態(tài)機(jī)再開放式數(shù)控系統(tǒng)中的應(yīng)用[J]．東北大學(xué)學(xué)報(bào)，2007，28(8)：1174-1177．

[14] 賈建強(qiáng)，陳衛(wèi)東，席裕庚．基于有限狀態(tài)機(jī)的足球機(jī)器人行為涉及與綜合[J]．高技術(shù)通信，2004，14(4)：61-65．

Multi-task Scheduling Research of Farm Machinery Navigation System Based on the FSM

Bai Xiaoping1,2，Wang Zhuo1,2，Hu Jingtao1,2

(1.Key Laboratory of Industrial Informatics, Shenyang Institute of Automation Chinese Academy of Sciences, Shenyang 110016, China； 2.Graduate University of Chinese Academy of Sciences, Beijing 100049, China)

In order to solve the problems of incident response delay and data delay in bus-based navigation control system, this paper divides the navigation control task into several subtasks of varying priority, and introduces the finite state machine (FSM) as the theoretical basis of multi-task scheduling. This paper establishes relationships between the navigation events and navigation tasks with different priorities based on the FSM and actual navigation process, and realizes the fast grading response to the navigation events. At last, this paper does the path tracking experiments based on the transplanter experimental platform. The experimental results show that this multi-task scheduling algorithm completes the navigation control task, the average straight line tracking error is 3.28cm and the average curve line tracking error is 5.04cm.

farm machinery； navigation system； finite state machine； multi-task scheduling

2016-04-04

遼寧省科技攻關(guān)計(jì)劃項(xiàng)目(Y5L7160701)；國家高技術(shù)研究發(fā)展計(jì)劃(863計(jì)劃)項(xiàng)目(2013AA040403)

白曉平(1986-),男，遼寧朝陽人，博士研究生，(E-mail) baixiaopin@sia.cn。

S127

A

1003-188X(2017)05-0026-06