基于有限状态机的农机导航多任务调度研究

白晓平，王 卓，胡静涛

(1.中国科学院 沈阳自动化研究所，沈阳 110016；2.中国科学院大学，北京 100049)



基于有限状态机的农机导航多任务调度研究

白晓平1,2，王 卓1,2，胡静涛1,2

(1.中国科学院 沈阳自动化研究所，沈阳 110016；2.中国科学院大学，北京 100049)

为了解决总线式农机导航控制系统中的事件响应及数据延迟问题，将农机导航控制功能划分成不同优先级的多个子任务模块，将有限状态机理论引入作为导航控制系统多任务调度的理论基础。基于有限状态机理论和实际导航控制过程建立导航事件与导航控制任务的关联关系，并对每个任务设有优先级，实现了对导航控制事件的快速分级响应。最后，采用基于本文提出的多任务调度方法的插秧机自动导航控制系统进行了实车路径跟踪实验。实验结果表明：本文所提调度方法可以很好地完成导航控制任务，直线路径平均跟踪误差为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