农业机器人视觉导航的预测跟踪控制方法研究_周俊

图像;识别;视觉

106

第20卷第6期

2004年11月农业工程学报

TransactionsoftheCSAEVol.20　No.6Nov.　2004

农业机器人视觉导航的预测跟踪控制方法研究

周　俊1,刘成良1,姬长英2

(1.上海交通大学机电控制研究所,上海200030;　2.南京农业大学工学院,南京210031)

摘　要:农用拖拉机的视觉导航技术可以帮助人员远离某些高温、高湿以及有毒害的作业环境,提高作业的自动化智能化程度,还能实现精确定点作业以促进农业可持续发展等。该文首先分析了轮式拖拉机跟踪引导路径的行为特点,建立起相应的非线性随机数学模型。而后,基于卡尔曼滤波的思想融合了各传感器的观测值给出预测跟踪控制方法。避免了视觉系统为主的计算耗时导致状态反馈滞后而产生的不利影响,改善了导航控制的鲁棒性和精度。仿真和初步试验结果都表明了此方法的有效性。

关键词:农业机器人;视觉导航;卡尔曼滤波;机器视觉

中图分类号:TP242.62;S232　　　　文献标识码:A　　　　文章编号:1002-6819(2004)06-0106-05

0　引　言

无论是为了避免人员在某些高温、高湿或有毒的农业环境中直接作业,还是出于实现精确定点的处方作业以确保农业高效生产及可持续发展等原因考虑,研制智能化农业机械都非常必要。目前,研究自主导航的轮式移动机器人来代替传统的拖拉机以其基础性的应用前景被普遍重视。

Wilson[1]在回顾50年来世界范围内农业车辆自主导航的发展历史时指出,跟踪先前行走路径的相对导航和依据一系列已知位置点的绝对导航是两种最常用的方式,机器视觉和GPS则恰好与它们分别对应,应用潜力大。相对而言,机器视觉还具有感知信息丰富,适应农田非结构化环境能力强等特点,因此近年来频繁出现在各种相关的研究报告之中。

在欧洲大陆,为了推广有机农业(欧盟计划2005年达到10%～15%的规模),尽量避免或减少农药的使用,精确机械除草和喷药受到普遍关注,文献[2]详细地介绍了为顺应这种形势目前欧洲大陆各国农业车辆导航的研究情况,对机器视觉在其中发挥的作用进行了仔细分析。文献[3]认真考察了近15年来美国在此领域的研究状况,从机械式、机器视觉、GPS以及磁罗盘等4个导航信息获取传感器入手分别做了介绍,其中机器视觉的应用情况被给予了较大篇幅的描述。

Torii详细地介绍了近几年来日本各大学、研究机构及部分厂商的研究情况,简要地回顾了亚洲其它国家和地区的一些研究现状,同样肯定了视觉导航的优势。并在文中具体介绍了他们研制的自主跟踪农作物行的视觉导航原型车辆,给出了初步的试验结果。

中国近年来也在积极开展农用车辆视觉导航方面的研究,内容涵盖了跟踪路径识别、导航控制方法直至原型样机设计等各个主要方面

[6,7]

[4]

[5]

视觉导航中,首先需要处理分析视觉系统采样的场

景图像,识别出可靠的导航路径(在农田环境中,往往是曲率很小的农作物行或垄等特征景物),再确定出机器人相对于识别路径的位姿(横向距离及航向角),最后由控制模块根据位姿信息去实时调节前轮导向角,实现机器人自主导航。虽然目前图像处理的软硬件技术发展迅猛,但可靠的视觉识别算法的计算耗时依然不可忽视,且复杂的非结构化农田场景又增加了识别难度,以至于导航视觉系统无法及时地感知机器人位姿变化,从而直接影响到导航控制的性能。

本文中将根据卡尔曼滤波的基本原理,融合各传感器信息提出预测跟踪控制方法,以克服视觉系统及其它信号处理环节(如卡尔曼滤波本身)的计算耗时共同造成的状态反馈滞后。最后给出仿真结果和初步的试验数据。

1　线性状态反馈预测控制

图1是轮式移动机器人的导航框图。测量装置由机器视觉、车辆纵向速度传感器以及导向轮角位移传感器等共同构成,负责实时、精确和可靠地感知系统状态值。

图1　导航系统框图

Fig.1　Frameofnavigationsystem

文献[8]中用磁罗盘测量航向角,基于三角测距原理用视觉传感器测量横向偏差,最后直接反馈这两个状态而获得前轮转角的期望控制量。文献[9]中虽然利用了模糊控制的方法,但实际上在制定模糊规则时,也同样是利用航向角和横向偏差的线性组合来得到每条模糊规则的输出值,类似于直接线性状态反馈法。日本东京大学的Torii等人[2]在农田试验时,采用了直接线性反馈航向、横向距离以及当前转向角等3个机器人状态分量值计算转向角的控制量。

,。

收稿日期:2002-09-09　修订日期:2004-05-28

基金项目:教育部科学技术重点研究项目(03091);上海市科委重点攻关项目(03dz19302)

作者简介:周　俊,博士,上海市　上海交通大学机电控制研究所,

图像;识别;视觉

　第6期周　俊等:农业机器人视觉导航的预测跟踪控制方法研究107

个反馈系数,但无论从人工驾驶实际经验还是由理论分析出发都可以明显看出当前导向轮转角值在横向跟踪控制中的重要地位。航向角和横向距离只能静态地表达机器人相对跟踪路径的位姿,而导向轮转角则反映了这种静态位姿的变化趋势。本研究中考虑到实时性以及现有试验样机传感器布置等因素,也将运用直接线性反馈航向、横向距离以及当前转向角等3个状态分量值来计算控制量。即:

d=K1el+K2eh+K3????

角(rad)(转向时内外侧轮的转向角并不相同,这里实际

上采用了前桥中点处等效的虚拟转向轮来分析),eh和el分别为轮式机器人相对于引导路径的航向角(rad)及横向距离(m)。

(1)

式中　el、eh、??——分别为预测的横向距离、航向角以及当前实测前轮转角;K1、K2、K3——分别为3个状态分量各自的反馈系数;??d——期望的导向轮转角控制量。

引言中已经指出,系统各计算耗时会造成状态反馈滞后。如果根据滞后严重的反馈状态计算控制量交由执行机构去实现,则执行机构动作时,轮式机器人的状态与图像采样时刻相比已发生了较多的变化,当纵向速度较高时变化尤其显著。这样,导航控制系统便无法及时地对被控系统的状态作出正确反应,势必会引起各项控制性能下降,甚至出现不稳定、振荡等后果。因此,本研究里提出预测线性状态反馈控制算法。运用前一个采样周期中卡尔曼滤波对当前周期机器人状态的预测结果作为反馈量,而不是象通常那样直接使用本周期采样时刻的状态的最优估计值。虽然最优估计值比预测值更为精确,但对于实时控制而言,系统不能允许其严重滞后。结果形成了如图1所示的控制系统框图,机器人诸状态测量结果不直接提供给控制器,而是先经过卡尔曼滤波来预测一个周期后的机器人状态,以作为下个周期控制量产生的依据。

有必要指出,实际系统中卡尔曼滤波并不仅仅具有此处所描述的功能,还可以辅助导航路径识别等,这里暂且只从预测控制的角度阐述,其它方面不予讨论。

[13]

图2　轮式移动机器人运动学模型

Fig.2　Kinematicsmodelofwheeledmobilerobot

由运动学关系可知,轮式移动机器人转向半径R为

(2)

tan??

则航向角eh的微分方程可以表示为

rr

eh=+wh=tan??+wh(3)

RB

式中　wh——由道路不平、自身振动以及引导路径曲率的些微变化等因素产生的高斯白噪声随机扰动。

前轮轴中心点速度vf为

r

vf=(4)

cos??

则横向距离的微分方程可以表示为

el=vrtan??coseh+vrsineh+wl(5)

R=

其中wl同样是由路面不平、自身振动以及引导路径曲

率微小变化等因素产生的高斯白噪声随机扰动。

转向机构可以认为是惯性环节

d??(??+w??)+??=??(6)

式中　??——惯性时间常数;??d——期望的前轮转角控制量;w??——转向机构综合刚度、道路情况以及自身振动等因素产生的高斯白噪声随机扰动。

2　卡尔曼滤波

卡尔曼滤波运用预测和修正机制递推地给出系统状态的最优估计,在视觉导航的研究中经常被用来动态地改善视觉系统的精确性和可靠性。2.1　轮式移动机器人运动学模型

正确使用卡尔曼滤波需要导航系统准确的数学模型。农田作业轮式移动机器人系统自身的动力学特性比较复杂,并且与之作用的土壤等农田环境因素存在着较大的不确定性,其动力学模型很难被精确地建立。考虑纵向速度较小时轮胎侧滑现象不明显,本文从运动学的角度对之建模,作为卡尔曼滤波的基础。

农田作业过程中轮式移动机器人的纵向速度基本恒定,本文中不对它进行干涉。作为导航引导路径的农作物行等景物的曲率一般非常小,局部范围内近似为直线,因此采用图2所示的运动学模型来分析其自主跟踪

情况。其中,vr为后轮轴中心点速度(m/s),vf前轮轴([10-12]

当注意到车辆振动、路面颠簸以及发动机工作波动等因素给车速造成干扰时,有

vr=wv(7)

综合上述各式可建立轮式移动机器人非线性随机状态方程eh

-=

??

0??d+00

w??

rwh

tan??+(8)Belwl

vrtan??coseh+vrsineh

vw0

方程(8)有如下一般形式

X(t)=f(X,t)+BU(t)+W(t)(9)

式中　X——系统状态变量;U——系统控制量;W——高斯白噪声干扰。

图像;识别;视觉

108农业工程学报2004年　

2.2　观测方程

系统设计中已保证了方程(8)中的各状态分量均可由各自对应的测量装置直接观测。其中,机器视觉识别引导路径,同时确定出机器人相对于它的位姿。由于农田复杂的非结构化场景,在满足需要的精度和可靠性前提下,视觉系统观测间隔相对较长,实时性差。前轮转角由角位移传感器直接测得,车速传感器是基于测量驱动轮转速的原理设计的,它们二者实时性好,观测间隔可以比机器视觉小得多。

实际上在局部直线路径模型的前提下,方程(8)中的各状态分量之间具有冗余性,如可以根据横向距离、航向角以及纵向速度的测量结果来求解前轮转角值。但是,一方面各系统状态分量均具有不同干扰因素引起的系统噪声;另一方面视觉系统可能会因为作物等引导景物缺失、杂草等干扰景物大量出现以及光照条件异常变化等因素而失效(虽然在视觉算法中已尽量加以考虑,但此类情况现在还难以避免)。因此,冗余的观测非常必要。

即轮式机器人系统的离散时间观测方程为

Z(k)=X(k)+Vd(k)(10)式中　Z——系统离散时间观测序列;Vd——观测过

程中的噪声。

2.3　卡尔曼滤波器的设计

式(9)是一非线性方程组,因此需要运用扩展卡尔曼滤波,即围绕估值附近将系统方程线性化,再按线性方程进行次优估计,则:

时间传播方程

X(t/ti-1)=f(X(t/ti-1),t)+B??d(t)P(t/ti-1)=F(X(t/ti-1),t)P(t/ti-1)+

P(t/ti-1)F(X(t/ti-1),t)+Q

X(ti-1/ti-1)??X(ti-1)P(ti-1/ti-1)??P(ti-1)^F(X(t/ti-1),t)??

T

+

^

^

+

T

^

^^

辆速度两者测量实时性强的优势来提高预测的精度,递推的周期也可以小于控制周期,即运用同样的原理实现状态的一个周期内插值。

试验中,运用VC++多线程的办法,利用不同的线程去分别处理视觉运算和卡尔曼滤波递推运算,其中递推线程可以在一个控制周期内独立执行5次。控制周期在试验时选定为100ms,也即卡尔曼滤波递推每20ms运算一次。

3　仿真研究

借助MATLAB软件进行了仿真研究。试验原型样机中,把通用的小四轮拖拉机改装成农田作业轮式移动机器人。仿真中轮式机器人的结构参数与原型样机保持一致,前后轮轴距B为1.34m,前轮最大转角为30°。

纵向速度取1m/s和2m/s两种水平,以能反映农田作业的实际状况。线性反馈系数使用试验中整定出的一组系数:分别为0.76、1.22和0.94,可见航向角的权重较大。系统噪声和传感器观测噪声都与试验研究时保持一致。但试验研究中,系统噪声可以根据传感器的实测值在线自适应调整[13],这在仿真时不会出现。根据农田图像的离线处理经验,设定视觉系统等计算总共会引起0.1s的状态反馈滞后。

设定初始横向距离为1m,初始航向和前轮转角都为0,模拟轮式移动机器人消除横向距离偏差沿目标路径行走的过程。仿真结果如图3、4所示。

从仿真结果可以明显看出,随着纵向速度的提高,预测控制的效果越发明显。由于速度的增加导致了滞后时间内状态变化量扩大,常规控制结果甚至出现了振荡的迹象(图3b)。

(11)(12)

X=^X(t/t

i-1

)

ti-1≤t≤ti

其中,当ti-1≤t≤ti时,??d(t)为一定值。

测量修正方程

--1

K(ti)=P(t-i)[P(ti)+Rd]

--X(t+i)=X(ti)+K(ti)[Z(ti)-X(ti)]-P(t+i)=[I-K(ti)]P(ti)^

^

^

(13)(14)(15)

图3　非预测控制的仿真曲线

Fig.3　Simulationofnon-predictivecontrol

微分方程式(11)给出了前一周期对当前周期的状态预测结果X(ti/ti-1),用4阶龙格-库塔方法求其数值解,它将是预测线性状态反馈控制的基础。

ti-1到ti时刻的间隔是卡尔曼滤波的递推周期,此周期长度可以等于控制周期,也可以小于控制周期。当等于控制周期时,如果某ti时刻机器视觉因为意外原因测量失败或误差太大而被丢弃,这时就用其余传感器的

观测结果连同横向距离、航向角的预测值代入式(14),^

4　试验研究

农用轮式移动机器人试验原型样机如图5所示,基

于此原型装置,用校园中的一段绿篱模拟农作物行,开展自主跟踪试验。

图像;识别;视觉

　第6期周　俊等:农业机器人视觉导航的预测跟踪控制方法研究109

速。图6b中发动机工况与前者基本一样,而车速为约0.27m/s。

两次典型的试验结果如图6所示,其中轮式机器人都可以顺利实现消除横向偏差,跟踪拟定的路径。这表明了本研究中的一系列理论分析和相关软硬件设计的有效性。由于试验条件的限制,目前试验速度都比较低,视觉系统滞后效应表现不显著,进一步的对比试验没有开展。

5　结　论

农业的发展需要某些农用车辆具备自主导航能力,视觉导航以其独特的优势比较适宜被应用。但农田复杂的非结构化自然环境加剧了视觉系统的识别难度,明显增加了导航系统的计算量。这直接会导致状态反馈滞后严重,给导航控制系统带来了很大的负面影响。

本文基于卡尔曼滤波理论,融合各传感器的测量信号给出了预测跟踪控制方法,以抵消状态反馈滞后严重造成的不良影响,导航系统的控制精度与鲁棒性得到了改善。仿真和初步的试验结果都表明,该算法具有比较理想的结果。

[参　考　文　献]

[1]　WilsonJN.Guidanceofagriculturalvehicles-ahistorical

perspective[http://wendang.chazidian.computersandElectronicsinAgriculture,2000,25(1-2):3-9.[2]　KeicherR,SeufertH.Automaticguidancefor

agriculturalvehiclesintheeurope[http://wendang.chazidian.computersandElectronicsinAgriculture,2000,25:169-194.

[3]　ReidJF,ZhangQ,NoguchiN,etal.Agricultural

automaticguidanceresearchinNorthAmerica[http://wendang.chazidian.computersandElectronicsinAgriculture,2000,25:155-167.

[4]　ToriiT.Researchinautonomousagriculturevehiclesin

Japan[http://wendang.chazidian.computersandElectronicsinAagriculture,2000,25(1-2):133-153.

[5]　ToriiT,KitadeS,TeshimaT.Croprowtrackingbyan

autonomousvehicleusingmachinevision[J].JSAM,2000,62(2):41-48.

[6]　周　俊,姬长英.基于知识的视觉导航农业机器人行走路

径识别[J].农业工程学报,2003,19(6):101-105.[7]　王荣本,等.基于机器视觉的玉米施肥智能机器系统设计

概述[J].农业工程学报,2001,17(2):151-153.[8]　NoguchiN,

IshiiK,

TeraoH.

Developmentofan

agriculturemobilerobotusingageomagneticdirectionsensorandimagesensor[J].JAgricEngRes,1997,68:1-15.

[9]　TodaM,KitaniO,OkamotoT,etal.Navigationmethod

foramobilerobotviasonar-basedcroprowmappingand

fuzzylogiccontrol[J].JAgricEngRes,1999,72:299-309.

图6　视觉导航跟踪试验

Fig.6　Experimentofvisualnavigation

[10]YiMa,JanaKosecka,ShankarSSastry.Visionguided

navigationforanonholonomicmobilerobot[J].IEEETransonRobotics&Automation,1999,15(3):521-536.onavia

图4　预测控制的仿真曲线Fig.4　Simulationofpredictivecontr

ol

图5　农用轮式移动机器人原型

Fig.5　Prototypeofagriculturalwheeledmobilerobot

试验中,使用的计算机主频为800MHz,Windows98操作系统,用Windows多媒体定时器设置各循环周期。车辆最后边的轮廓线和其纵向中轴线的交点在地面上的投影为车辆轨迹记录点,用漏细沙的办法记下该点在路面上的痕迹。借助纤维卷尺测量各距离的大小,所有的测量值只精确到厘米。沿跟踪路径方向间隔20cm测量一个点,如果某测量点处没有细沙痕迹,则以相邻点的平均值代替。试验时,离合器与车速档位仍然由人工操作。图6a中速度约为0.94m/s(

车速传

图像;识别;视觉

110农业工程学报2004年　

vehiclefollowingproblem[J].IEEETransofVehicular

Technology,1995,40:654-662.dynamicvision[J].IEEETransAutoma&Contr,1996,41:393-413.

[12]NasserKehtarnavaz,NormanCoriswold,JuckSLee.

Visualcontrolofanautonomousvehicle(BART)—the[13]周　俊.农用轮式移动机器人视觉导航系统的研究[D].南京:南京农业大学博士学位论文,2003.

Predictivetrackingandcontrolmethodforvision-guided

navigationofagriculturalrobot

ZhouJun,LiuChengliang,JiChangying112

(1.MechatronicsInstitute,ShanghaiJiaotongUniversity,Shanghai200030,China;

2.CollegeofEngineering,NanjingAgriculturalUniversity,Nanjing210031,China)

Abstract:Toavoidtheunfavorableworkingconditionsandguaranteethesustainabilityofagriculture,itiswidelythoughtthatthewheeledmobilerobotguidedbymachinevision,substitutingforconventionaltractorsinsomefarmingactivities,willplayanimportantroleinthefuture.Thetimelag,however,producedmainlybyrobotvisionsystemandtheothersignalprocessingwillexertsomenegativeeffectsontheautonomousnavigationofthewheeledmobilerobot.BasedonKalmanfiltering,themeasurementsofallsensorswerefusedandapredictivecontrolmethodwasdevelopedskillfullytoovercomeit.Thenthekinematicalbehaviorofthewheeledmobilerobotwasanalyzedindetail,andthecorrespondingnonlinearstochasticmodelequationandtheobservationequationweresetuprespectively.Boththesimulationandtheinitialexperimentshowthatthismethodiseffectiveforthevisualnavigationofthewheeledmobilerobotinthefield.

Keywords:agriculturalrobot;vision-guidednavigation;Kalmanfiltering;machinevision