基于有限狀態機的農機導航多任務調度研究

白曉平，王 卓，胡靜濤

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

?

基于有限狀態機的農機導航多任務調度研究

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

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

為了解決總線式農機導航控制系統中的事件響應及數據延遲問題，將農機導航控制功能劃分成不同優先級的多個子任務模塊，將有限狀態機理論引入作為導航控制系統多任務調度的理論基礎?；谟邢逘顟B機理論和實際導航控制過程建立導航事件與導航控制任務的關聯關系，并對每個任務設有優先級，實現了對導航控制事件的快速分級響應。最后，采用基于本文提出的多任務調度方法的插秧機自動導航控制系統進行了實車路徑跟蹤實驗。實驗結果表明：本文所提調度方法可以很好地完成導航控制任務，直線路徑平均跟蹤誤差為3.28cm，曲線路徑平均跟蹤誤差為5.04cm。

農機；導航系統；有限狀態機；多任務調度

0 引言

農機的自動導航技術是現代農業機械裝備的重要支持技術之一。農機的自動導航可以減輕駕駛人員的疲勞度，提高作業質量和精度[1-3]。因此，農機導航研究具有重要的理論研究意義和實用價值，已經成為精準農業的重要研究內容。

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

針對以上問題，本文將導航控制任務劃分成多個子任務，并將實時操作系統和有限狀態機理論引入實現了任務與導航控制事件的關聯[10-14]。本文將定時讀取傳感器位姿數據改為傳感器定時向導航控制器發送最新的位姿數據，在接收到位姿數據后導航控制器立即計算控制量、發布控制命令，有效地避免了傳感器位姿數據延遲問題。本文還將重要的外部事件與單獨的處理任務相關聯，并對每個任務設有優先級，從而解決了導航控制系統存在事件響應延遲的問題。

1 農機導航控制系統簡介

1.1 硬件結構

導航控制系統主要由導航控制器、田間計算機、轉向控制單元、位置傳感器和航向傳感器及轉向角傳感器6部分組成，如圖1所示。

圖1 硬件結構圖

其中，導航控制器、位置傳感器和航向傳感器是導航控制系統的核心部件，導航控制器根據航向傳感器和位置傳感器的數據實時計算控制量，從而實現路徑的準確跟蹤。為共享航向傳感器和位置傳感器的數據，在導航控制器與這兩個傳感器之間加入了智能節點硬件模塊。智能節點將航向傳感器與智能傳感器的數據解析出來并在導航控制器需要時發送到總線上。將以上所述硬件部分集成到一起開發了如圖2所示的導航控制箱。

圖2 導航控制箱

田間計算機為導航控制系統提供了人機交互平臺。田間計算機主要負責路徑的采集、設置及各種導航命令的發布工作；還負責導航控制數據的保存和導航運行情況的監測和顯示工作。

轉向控制單元是導航控制系統的執行器，主要負責使車輪轉到導航控制器指定輸出角度；轉向角傳感器則主要用于當前車輪轉角的檢測，與轉向控制單元共同構成了自動轉向系統。

1.2 軟件結構

導航控制系統的軟件主要由如圖3所示的5個功能模塊組成。

圖3 軟件結構圖

中斷服務任務模塊是導航控制系統與田間計算機和傳感器交互的接口任務，主要負責外部信息和命令的接收與解析工作。參數設置任務模塊主要負責導航控制參數和期望路徑的設置工作，是導航控制的前提基礎，只有正確設置了這些參數才能開始導航作業。導航作業控制任務模塊是導航控制的核心功能模塊，主要負責控制量的計算。信息交互任務模塊負責收集導航作業控制所需的傳感器數據及發送導航作業中產生的導航控制信息。故障處理任務模塊負責導航控制中錯誤處理與錯誤報警工作。

2 基于有限狀態機的多任務調度研究

農機導航控制系統共劃分成了參數設置任務、導航作業控制任務、中斷服務任務、信息交互任務和故障處理任務5個任務模塊。這5個任務如何調度直接影響到導航控制系統的性能。導航控制系統的調度問題歸結為在一定的觸發條件下，系統將使5個任務中某一個任務進入運行態，并且某一確定時刻只能有一個任務處于運行態的問題。系統調度設計的目標是使得這6個任務協調、有序的運行，共同完成導航控制任務。本文將有限狀態機理論引入到系統調度的設計中，建立了導航控制系統工況狀態機，并基于工況狀態機和狀態轉移圖進行了導航控制系統多任務的設計。

2.1 有限狀態機理論簡介

有限狀態機是計算機科學和數學理論的抽象，是離散輸入輸出系統的模型。有限狀態機理論認為：系統可以有多個狀態，但某一確定時刻系統只能處于多個狀態中的一個；系統從一個狀態轉移到另一個狀態不僅取決于觸發條件還取決于當前所處的狀態[13-14]。有限狀態機包括以下幾個要素：

1)狀態。行為模型的基本組成部分，反映了系統中某個對象所處的階段和活動情況。

2)轉移。對象從一個狀態轉移到另一個狀態的過程。

3)事件。引起對象狀態轉化的事件及條件。

4)動作。在狀態轉移時對象所采取的行動。

2.2 有限狀態機狀態確定

導航控制系統在導航控制過程中會處于很多不同的運行狀態，根據實際導航控制過程可以得到如表1所示的6種運行狀態。

其中，空閑狀態是導航控制系統上電后無任何操作時所處的狀態，導航控制系統處于此種狀態時會執行一些無實際意義的代碼，將這些代碼的集合稱為空閑任務。其余的5個狀態是以5個導航控制任務模塊為基礎的，每個導航任務模塊是導航系統處于該狀態下時導航控制器執行的功能代碼的總和。這5種狀態會在事件的觸發下發生轉移，每次狀態轉移表示一次任務切換。任務模塊與有限狀態機狀態的對應關系如表2所示。

表1 導航控制系統狀態

2.3 有限狀態機觸發事件的確定

事件是驅動狀態機狀態轉化的原動力。表2給出了導航控制過程中的事件，這些事件是由田間計算機輸入的信息直接引起或者間接引起的。事件可以有相關變量，從而使得事件不僅能傳遞命令，同時可以攜帶自己的參數。每個事件具有生命周期，每個事件都會經過接收、解析和處理3個過程，在經過這3個過程后事件將不再會對狀態機有效。

表2 導航控制系統的觸發事件

2.4 農機導航控制系統工況狀態機

導航控制系統可以看成一個有限狀態機，本文稱其為工況狀態機。導航控制系統中多個子任務的調度問題則可以看成是在事件驅動下狀態轉移問題。根據實際導航控制過程可以建立如圖4所示的狀態轉移圖。

2.5 農機導航控制系統多任務設計

導航控制系統多任務的設計原則是滿足軟件總體結構各功能模塊的功能并滿足圖4所示的狀態轉移關系。根據以上原則共創建了空閑任務、參數設置任務、中斷處理任務、導航作業任務、信息交互任務和故障處理任務等6個任務。

圖4 狀態轉移圖

空閑任務是實時操作系統自帶的任務，啟動任務讓出CPU控制權后，空閑任務得到CPU控制權執行空閑任務(導航控制系統處于q0狀態)。

發生外部中斷(d1事件發生)后，中斷處理函數會簡單地將中斷接收到的原始信息放入中斷接收緩沖序列，接著會將中斷處理任務的優先級提到最高，從而實現其余任務到中斷處理任務的切換，即導航控制系統的狀態轉移到中斷處理狀態(其余狀態轉移到q2狀態)。中斷處理任務會查詢中斷緩沖序列中是否還有未處理信息，如果有則提出信息進行處理，如果中斷緩沖序列已經為空，則表明信息已經處理完畢。中斷處理任務會根據信息類型來判斷是何種事件發生。如果是參數設置命令(d2事件發生)，則會發送參數設置信號量從而實現從該任務到參數設置任務的切換(q2轉移到q1狀態)；如果接收到的是位姿信息接收命令(d3事件發生)，該任務執行完畢導航系統將轉向信息交互狀態的轉移(q2轉移到q4)；如果是開啟導航命令(d4事件發生)，則將導航作業控制任務和信息交互任務解除掛起，從而實現從該任務向導航作業控制任務的轉移(q2轉移到q3)；如果是關閉導航命令(d9事件發生)，則將導航作業控制任務和信息交互任務掛起，停止導航作業，導航系統轉入q0態(q2轉移到q0)。中斷信息處理完畢后，該任務會將自身的優先級恢復，并切換到其他任務執行。

參數設置任務是導航控制的前提任務，只有完成了導航控制的參數設置才可以進行導航控制，否則會發生導航作業錯誤。該任務主要負責執行導航作業前控制算法參數和期望路徑參數的設置。該任務收到中斷處理任務的通知后，會根據指令完成導航控制參數的設定和期望路徑的設置工作。

導航作業控制任務是導航控制的核心子任務，主要負責控制量的計算。開啟導航作業后，導航控制任務會定時進行控制量的計算和發送，從而實現聯合收割機對期望路徑的準確跟蹤。導航作業控制任務還負責傳感器數據和誤差是否發散兩項監測工作，在傳感器數據不正?；蛘邔Ш秸`差發散的情況下發送故障信號。導航控制任務首先發送停止接收傳感器數據命令，表明在導航控制任務執行期間不再接收傳感器數據，這樣可以防止數據偷換錯誤發生；接著該任務會判斷此時接收到的新的傳感器數據的個數是否在正常的容限內，如果超出容限則發送傳感器故障信號(d8事件發生)，故障處理任務由于得到該信號而切換到轉入運行態(q3轉移到q5狀態)。傳感器數據正常，則表明導航控制任務可以繼續執行，該任務會計算控制量并將控制量發送給轉向控制單元；完成本次導航控制量計算后，該任務會調用延時函數將此任務掛起一個延時周期(d6事件發生)，此時系統會轉移到q4狀態(q3轉移到q4狀態)。

信息交互任務負責發送導航作業控制中產生的航向偏差、橫向偏差等各種導航控制中產生的信息給田間計算機，并負責接收傳感器的數據。信息交互任務在導航作業任務的定時間隔中被啟用(d6事件發生)，該任務首先將本次導航作業控制中產生的誤差信息和航向信息發送給田間計算機，然后發送傳感器數據接收信號到總線上，傳感器會在收到停止接收傳感器數據信號之前發送傳感器數據到總線上。在接收傳感器的數據過程中，該任務會對接收數據個數進行計數，從而判斷傳感器是否正常；導航作業控制任務定時(d7事件發生)到來后會結束該任務的運行，轉而執行導航作業控制任務(q4轉移到q3狀態)。

故障處理任務主要負責導航過程中的故障處理工作。此軟件考慮了兩種導航作業故障情況：一是傳感器長時間沒有發送數據給導航控制器；二是導航作業誤差發散。這兩種情況下，都會造成導航控制作業錯誤。該任務通過判斷錯誤類型號來決定是哪種錯誤，根據錯誤類型發送對應錯誤信息幀給田間計算機報警，并通過將導航作業控制任務掛起來停止導航作業；停止導航作業后，該任務會發送停止電機轉動命令給轉向控制單元停止電機旋轉從而聯合收割機將停止運行；故障處理完畢(d8事件發生)，系統轉入空閑態(q5轉移到q0狀態)。

3 多任務調度方案的性能分析

3.1 數據延遲問題分析

數據延遲問題是指導航控制器獲得的傳感器位姿數據并不是當前的位姿數據，而是該時刻以前的傳感器數據。數據延遲會降低導航控制精度，在導航控制中應該盡量減小數據延遲時間。數據延遲問題對導航控制精度的影響可以用式(1)的車輛運動學模型來進行分析。則有

(1)

其中，dt是時間間隔；dx、dy分別為車輛坐標沿設定軌跡的切向和法向在dt時間內的變化量。

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

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

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

3.2 導航事件響應速度分析

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

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

4 實車路徑跟蹤實驗

將導航控制系統安裝在圖6所示的插秧機上，構成了插秧機實驗平臺。采用本文提出的多任務設計方案，在較平整的路面上進行了直線路徑與曲線路徑的跟蹤實驗。

圖6 插秧機實驗平臺

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

圖7 直線路徑跟蹤曲線

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

圖8 曲線路徑跟蹤曲線

5 結論

1)提出了一種基于有限狀態機的導航控制系統多任務調度方法。通過分析，該調度方法采用將導航控制事件與導航控制子任務綁定，并對導航控制子任務設置優先級，解決了導航控制事件響應延遲和數據延遲問題。

2)實車實驗結果表明：基于有限狀態機設計的多任務調度方案使得多個任務有序、快速地運行，多個任務相互協作共同完成了導航控制目標。

3)軟件的故障處理任務實現了聯合收割機在故障情況下的錯誤報警和故障處理，減少了不必要的作業損失，避免出現危險狀況。

4)故障處理任務只對少數幾種故障情況進行了考慮，實際導航作業中可能會遇到更多更復雜的故障，所以還需要在實際作業中不斷加強故障處理功能。

[1] 胡靜濤，高雷，白曉平，等.農業機械自動導航技術研究進展[J].農業工程學報，2015，31(10)：1-10.

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

[3] 張智剛，羅錫文，周志艷，等．久保田插秧機的GPS導航控制系統設計[J]．農業機械學報，2006，37(7)：95-97．

[4] 安秋，姬長英，周俊，等．基于CAN總線的農業移動機器人分布式控制網絡[J]．農業機械學報，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]．農業工程學報，2009，25(12)：88-92．

[9] 陳康,周志峰,王永泉，等.基于G NSS的農機自動導航控制系統設計[J].化工自動化及儀表,2014(11):1294-1297.

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

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

[12] 王俊席，楊林，祝軻卿，等．基于實時操作系統的發動機控制軟件開發平臺設計[J]．農業機械學報，2006，37(9)：36-38．

[13] 孫維堂，劉永賢，張禹，等．有限狀態機再開放式數控系統中的應用[J]．東北大學學報，2007，28(8)：1174-1177．

[14] 賈建強，陳衛東，席裕庚．基于有限狀態機的足球機器人行為涉及與綜合[J]．高技術通信，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

遼寧省科技攻關計劃項目(Y5L7160701)；國家高技術研究發展計劃(863計劃)項目(2013AA040403)

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

S127

A

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