温室环境监测机器人路径跟踪算法与试验

韦鉴峰，吕恩利，王飞仁，林伟加，阮清松

(华南农业大学 工程学院，广州 510642)

0 引言

导航与定位是温室机器人路径跟踪的关键技术之一[1]。温室环境监测机器人作为智能农业发展重要组成部分，其路径跟踪控制是解决 “怎么走”的问题。温室环境监测机器人可以归类为非完整约束轮式移动机器人[2]，由于存在非完整约束,使运动控制难度较高[3]。对具有非完整约束的温室环境监测机器人的研究是解决其路迹跟踪控制的重要途径[4-5]。

针对非完整约束结构轮式移动机器人轨迹跟踪问题，很多学者进行了相关的研究。宋立博[6-8]等通过对轮式移动机器人的运动学模型进行分析，提出了不同工作要求下机器人的轨迹跟踪控制算法，并设计了控制器，通过仿真证明了算法的有效性；但这些算法都只是基于计算机仿真验证，没有通过试验验证算法的实用性和有效性。在难以准确建立运动学模型的情况下，李逃昌[9-12]等应用纯跟踪模型对路径轨迹进行跟踪，并在相关的试验平台上进行算法试验，其跟踪准确性相对较高，获得较好的研究成果。

为解决温室环境监测机器人路径跟踪控问题，本文通过对其运动学模型分析，采用基于航向角误差算法的路径跟踪控制方法，以实现对机器人转向和速度控制，并通过仿真和试验验证算法的有效性和实用性，为解决温室环境监测机器人路径跟踪控制问题提供了参考。

1 系统结构与运动学模型

1.1 机器人系统结构

温室环境监测机器人(简称机器人)由安全检测系统、传感器系统、电气系统、控制器系统、电源供电系统、远程和上位机系统及避障系统等组成，如图1所示。平台长为550mm，宽为400mm，高为770mm，质量为30kg。有4个轮子，分别为2个转向轮和2个驱动轮，属汽车轮式移动机器人；轴距为350mm，转向轮通过操舵机构实现转向，驱动轮通过连接差动齿轮实现差速；机器人采用激光定位方式，由激光定位传感器NAV350实现定位。

当机器人运行时，状态指示灯亮绿灯，NAV350扫描周围环境检测反光标贴，实时获取机器人全局坐标位姿，角度编码器和速度传感器分别实时获取转向轮转角与机器人移动速度；传感器采集的数据通过I/O模块输入工控机器系统，系统将获取的反馈数据，应用相应算法进行处理，输出下一时刻机器人的速度和转角，实现机器人按照预设轨迹从起始点运动到目标点。

1.2 机器人运动学模型分析

假设室内路面平坦、运动速度可变，机器人实心轮胎无侧偏、无纵侧滑移，机器人的瞬时转向中心是两条车轴线的交点。根据机器人的结构特点，建立如图2所示的运动学模型。

1.急停开关 2.状态指示灯 3.激光定位扫描仪NAV350 4.激光避障传感器 5.I/O模块 6.工控机 7.绝对值式角度编码器 8.转向轮9.24V铅蓄电池 10.驱动轮

图2 机器人运动模型图

图2中，(X,O,Y)为笛卡尔全局坐标；(XV,V,YV)为机器人局部坐标；1、2为转向轮；3、4为驱动轮；5为等效转向轮；6为等效驱动轮；l为轴距(m)；θ为机器人航向角(rad)；ICR为机器人瞬时旋转中心；v为机器人运动速度(m/s)；γ为转向轮转角(rad)，R2为机器人瞬时旋转半径(m)。

根据建立的机器人运动学模型，将其结构简化为轮5和轮6组成的二自由度轮式自行车模型，可以得到其运动学方程式为

(1)

式中ν—机器人行驶速度(m/s)；

ω—移动机器人角度变化率(rad/s)；

θ—航向角(rad)；

γ—机器人等效转向轮转角(rad)；

l—机器人轴距(m)。

其中，V(x,y)为机器人运动学模型图中V点坐标，即驱动轮中心点坐标。式(1)表示移动机器人的位姿p(x,y,θ)与移动速度v和等效转向轮转角γ之间的关系。由图2几何关系可以得到

l/R2=tanγ

(2)

式中R—机器人瞬时转弯半径(m)；

由式(2)可以推导出机器人后轮轴中心处的瞬时转弯半径为R2=l/tanγ。

2 坐标转换与算法原理

2.1 坐标转换

上述系统结构中，机器人的采用激光定位方式，所使用的激光定位传感器NAV350具有建图和全局定位功能，采用三点定位方式。机器人的路径跟踪问题可转化为机器人跟踪期望路径上的连续点的问题，如图3所示。

图3 机器人路径跟踪示意图

图3中，(X,O,Y)为全局坐标；(xc,V，yc)为机器人局部坐标系；机器人当前的坐标点为V(XV,YV,θ)；A点为期望路径上的一点，坐标为A(XA,YA)，即为机器人跟踪的目标点，这两点全局坐标已知。现为方便路径算法的计算，要将已知的A点的坐标A(XA,YA)转化为机器人局部坐标系(xc,V,yc)下的坐标A1(xA,yA)。本研究采用的是几何方法推算全局坐标A(XA,YA)在局部坐标系(xc,V,yc)下的坐标，其几何关系原理如图4所示。

图4 机器人目标点坐标转换几何原理图

transformation of robot target point

图4中，A点是图3中的期望路径上的点A的简化形式；坐标系(x′,V,y′)为全局坐标系(X,O,Y)的平移坐标系。由图4可知：要求A点在局部坐标系的坐标即为求线段VB和VF的长度。由几何关系有

(3)

(4)

公式(4)中，(xA,yA)即为目标点A在机器人局部坐标系下的坐标，公式中所有的参数都是已知，由此可知，A的局部坐标也可知。

2.2 航向角误差算法

在机器人路径跟踪中，其跟踪稳定性、鲁棒性、准确性和精度等是评价机器人路径跟踪好坏的重要指标。研究移动机器人的速度和方向控制与优化问题，是解决和优化机器人轨迹跟踪能力重要途径，本文基于机器人运动学模型研究其方向和速度控制问题。为方便计算，记机器人运动速度为v，转向角为γ，用β表示航向角误差值，如图5所示。

由图5可知，机器人当前位置与到目标点的方向误差值可以表示为

(5)

式中β—航向角误差值(rad)。

其中，(xA,yA)为图4中目标点A在机器人局部坐标系中的相对坐标。在本研究中，机器人转向角的大小为直接控制量。当航向角误差角β较小时，采用如下线性控制，即

γH=kH·β

(6)

式中γH—机器人转角(rad)；

kH—常数系数。

图5 机器人航向角误差示意图

式(6)为航向角误差值与转向角关系式，kH可根据需求取值。由式(6)可知：当limβ=0时，limγH=0，即当机器人航向角误差值收敛于0时，机器人的转向角γH也收敛于0；机器人转向采用舵机驱动，其转向具有极限值γmax，当γH>γmax时取γH=γmax。通过式(6)可算得机器人转向角大小γH，实现了机器人的转向控制。在机器人运动过程中，式(6)仅能控制其运动方向，无法控制其运动速度；然而机器人的运动速度也是影响其能否快速收敛于期望轨迹的关键因素，且机器人在不同位置其运动速度也不同。因此，采用合适的速度控制策略控制机器人的速度至关重要。在工程应用中，机器人能否快速到目标点，并在器人到达目标点时能否镇定，是评价机器人响应性能的重要标准。本文采用线性控制方式对机器人的运动速度进行控制，其控制策略如下，即

(7)

式中vH—机器人运动速度(m/s)；

kv—正的常系数。

其中，kv可根据实际情况取值。由式(7)可知：随着机器人收敛于目标点A，其相对坐标(xA,yA)也越小，运动速度也越小。当limxA=yA=0时，limvH=0，采用该速度控制策略可使机器人到达目标点后镇定，且不存在较大的运动加速度，不会对机器人结构造成冲击，表现出较好的机械性能。机器人的运动采用电机驱动，其运动速度具有极限值，当vH>vmax时，取vH=vmax。式(6)和式(7)为机器人在航向角较小时采用的一种控制机器人的转向角和速度的方式和策略。在机器人航向角较大时，从式(6)可知：当kH不变时，γH较大，会使机器人获得一个较大转向阶跃信号，对机器人转向轮转向舵机产生较大机械冲击，不利于机器人机构的保护。因此，航向角较大时，可采用纯跟踪方法对机器人的转向进行控制。

2.3 纯跟踪算法原理

航向角误差算法是当航向角误差值较小时采用的一种控制方法。当航向角误差较大时，可采用纯跟踪算法。纯跟踪(pure pursuit)算法是一种几何计算法，原理是计算机器人到达期望点所需要走的圆弧长。该方法简单、容易实现，是模拟汽车驾驶员的视觉、采用前视距离的方式，已经广泛应用在路径跟踪领域上。该算法的原理如图6所示。

图6 纯跟踪算法模型

图6中，(X,V,Y)构成了机器人局部坐标系，A(xA,yA)为机器人目标点。根据几何关系，得出该算法的表达式为

(8)

由式(8)整理得

R=L2/2y

(9)

(10)

式中L—圆弧弦长(m)；

R—圆弧的半径(m)。

式(9)和式(10)中，L为连接机器人驱动轮中点V和A圆弧段的弦长。

其中，O为该圆弧的圆心；L和y为可测量的已知量，且有，x=xA，y=yA。因此，R也已知，即机器人要从当前点V运动到目标点A，其运动半径为R。

本文所研究的对象为后轮驱动和前轮转向的类汽车机器人，与传统的差速转向移动机器人不同之处在于它是通过控制转向轮的方向来改变机器人的方向，具有非完整特性，控制难度较高。因此，对这种汽车的机器人的转向控制问题，很多研究人员都做了大量的研究工作，试图运用运动建模和数学几何手段解决，但精确度受到了很大的限制。

机器人航向角误差较大时，本节采用A.J.Kelly[13]提出的将四轮结构简化成二轮车自行车模型的方法。假设车轮与地面接触无侧向滑移，运动的时候没有出现打滑失速现象。机器人简化二轮车模型如图7所示。

图7 简化二轮车运动学模型

图7中，l为前轮和后轮的间距，即轴距(m)；R为机器人旋转半径(m)；γ为机器人等效转角(rad)。由几何关系可得

tanγ=l/R

(11)

结合图6和式(11)可知：机器人要以圆弧的方式从点V到达目标点A，则其圆弧半径为R与图(7)中的机器人旋转半径是一致的。由式(9)和式(11)可得

(12)

式中γc—等效转向角(rad)。

由式(12)可知：只要选择合适的目标点，得到目标A(XA,YA)的全局坐标值和机器人在全局坐标中的坐标值V(XV,YV)，再通过式(4)求出(xA,yA)，即可求出机器人等效转向的γc值，实现移动机器人的转向。与航向角误差值法求解机器人转向角大小方法一致，式(1)、(2)仅能求解机器人转向角的大小γc，并不能控制其运动速度。为保证机器人的运动的平顺性，在采用纯跟踪算法控制机器人运动阶段，采用与式(7)相似的线性控制方式，其速度表达式为

(13)

式中vC—机器人运动速度(m/s)；

kC—正的常系数；

kx—正的常系数；

β—航向角误差值(ad)。

由式(13)可知：始终有vC≤vmax，当航向角误差值β较大时，式子的分母也会比较大，vC会比较小。在本文中，当β<π/6时采用航向角误差控制算法；当π/6≤β≤π/4时，即当机器人获得一个较大的航向角误差值时，机器人当前点与目标点的横向误差较大。因此，机器人应先慢速转向，减小与目标点的横向误差，再以较快的速度靠近目标点；当路径为较曲折的路线时，采用该速度控制策略会使机器人更为精确地跟踪期望轨迹。

2.4 前视距离的控制

前视距离是机器人路径跟踪中的一个重要因素，前视距离的选择会影响机器人跟踪路径的准确性。前视距离取值较大，其跟踪圆弧半径较大，跟踪不会出现路线振荡，但会产生机器人走“捷径”的效果。如果路径周边所预留的空间较小，机器人会撞向路径周围的障碍物；如果前视距离取值较小，会出现机器人运动路径产生振荡，机器人运行不稳定。跟踪效果如图8所示。

图8 不同前视距离跟踪效果

由图8可以看出：不同的前视距离得到的效果不一样，因此选择合适的前视距离才能提高机器人的跟踪效果。通过软件的仿真和进行多次试验比较的方法获取不同前视距离的跟踪效果，然后筛选其中合适的固定值作为前视距离。这种方法在路径比较简单的情况下是可行的，但也有局限性。假设路径比较复杂，路径不具备导数的连续性，固定的前视距离往往不会取得较好的跟踪效果。针对此问题，本文提出了一种适合机器人前视距离的选择的方法，具体如图9所示。

图9 前视距离选择策略

图9中，C1(xc,yc)表示机器人的当前点。以C1为圆心、R1为半径，恰好与期望路径相切，切点为C2(x2,y2)；再以C2为圆心、R2为半径画圆，与期望路径交于点r1(xr1,yr1)、r2(xr2,yr2)，即为机器人的前视距离点；再根据机器人的前进方向进行选择其中一点，完成机器人前视点的选择。假设期望路径L是由无数个点(xi,yi)组成，即

(14)

其中，n为正整数。

由以上的描述可得

(15)

(16)

式中R1—C1半径(m)；

R2—C2半径(m)。

同时，有R1=0.01×t(t=1,2…)。当且仅当C1与期望路径只有1个交点时，停止取值。式(15)求得点C2，式(16)求得点r1、r2。假设某时刻机器人的前视点为r1，则r1的坐标r1(xr1,yr1)取点策略为

(17)

式中θ—常数。

式(16)和式(17)中，R2=0.01×m(m=1,2，…)，且R2max=a。其中，a为正的常数，取a=0.5m。在计算机的计算过程中，当R2<0.5时，R2的值不断地叠加，当且仅当R2=R2max=a或者arctan(y2-yr1/x2-xr1)=θ时停止取值。本文中，θ=π/4时，r1(xr1,yr1)即为机器人当前时刻的前视点。

3 仿真与试验

3.1 材料与设备

仿真在MatLab2015a环境下采用编程的方式进行，硬件为笔记本电脑型号X260。试验材料为上述机器人、反光标贴、上位机(型号为X260笔记本电脑)，软件平台为beckhoff/Twincat3。上位机与机器人之间采用WIFI通讯。机器人由直流伺服电机驱动，直流伺服驱动电机为24V电压驱动，型号为57B2C1230-SCO，额定功率120W，额定转速3 000r/min。经过测试，由电机驱动的机器人速度从0m/s达到0.32m/s，用时0.9s。机器人转向由转向舵机驱动，舵机型号为LF20MG，旋转角度范围为0°～90°，扭矩2Nm，其转角从-0.45rad到0.4rad用时1.27s。速度和角度的响应速度满足试验和应用要求。试验场地为11.4m×5.6m的长方形场地，其环境与温室环境相似。图10、图11为反光标贴与试验场地环境图。

机器人采用激光定位方式，因此需采用图中反光率比较强的反光材料。反光标贴直径为90mm，放置时尽可能与试验场地垂直。

图11 试验场地环境图

3.2 机器人路径跟踪仿真

仿真是基于上诉两种算法的基础上进行，为了证明算法稳定可靠和准确性，采用圆弧和直线路径进行仿真。

平面上的直线可以用ax+by+c=0来表示一条直线，系数为(a,b,c)。选取机器人仿真直线为(a,b,c)=(1,-1,0)，机器人的起始点为(x0,y0,θ0)=(1,4,-π/2)，仿真结果如图12所示。

图12 机器人直线路径仿真图

由图12可以看出：机器人在点(4,4)时开始收敛期望路径，跟踪稳定，跟踪误差趋向于零。

选取起始点为(x0,y0,θ0)=(0.5,-0.5,-2π/3)，期望路径为x2+y2=1，仿真结果如图13所示。

由图13可以看出：机器人大约在(0,1)位置第1次收敛于期望路径，之后一直沿着期望路径进行运动，具有较好的跟踪稳定性。

3.3 机器人路径跟踪试验

试验同样采用跟踪直线和跟踪圆弧轨迹的试验方案，验证机器人在工程应用中的路径跟踪效果。跟踪的直线为(a,b,c)=(1,1,-2.5)，机器人的起始点为(x0,y0,θ0)=(4.6,-1.4,1.9rad)，试验结果如图14所示。

图13 机器人圆弧路径仿真图

图14 机器人直线路径试验图

由图14可以看出：在试验过程中，机器人在点(1.5,-2)附近开始收敛，当机器人到达点(1,-1)时，机器人完全收敛，具有较快的收敛性；中间大约有2cm的振荡，最后趋向稳定；当跟踪稳定后，其横向误差约±8mm，符合温室环境路径限制的要求。

圆弧路径跟踪，选取起始点为(x0,y0,θ0)=(0.22,0.24,3.4rad)，期望路径为x2+y2=2.25，试验结果如图15所示。

由图15可以看出：机器人在收敛于期望圆弧轨迹过程中，并不能直接较好地收敛。跟踪过程中出现一次振荡，这是由于机器人的转向轮转角范围为-0.49～0.49rad，轴距为0.35m，根据公式(11)得机器人最小转弯半径为0.66m，该半径大于机器人第1次可以直接收敛于期望圆弧路径转弯半径，因此会出现1次振荡。第1次收敛于期望轨迹约在点(-0.5,-1.5)，此后没有出现机器人跟踪路径振荡的情况发生；当跟踪稳定后，其横向误差约±11mm，具有良好的精度和跟踪效果。

图15 机器人圆弧路径试验图

4 结论与讨论

1)基于机器人运动学模型，提出基于航向角误差算法的路径跟踪控制方法。计算机仿真和试验表明：所提出的算法能使机器人实现路径跟踪，且具有良好的跟踪稳定性。

2)分析了机器人在路径跟踪过程中不同前视距离点的选择对机器人路径跟踪效果的影响，并提出了一种与航向角误差大小相关的、前视距离可变的前视点的选择方法，且在仿真和试验中验证了该方法的有效性。

3)试验表明：该算法在与温室环境相近的试验场地中具有较好的跟踪效果，能够实现工程应用，为温室环境监测机器人的路径跟踪控制提供了参考。

在机器人运动过程中，当跟踪稳定后、转向轮会有左右抖动的情况，这应该与控制转向角输出的数据波动关系，也同地面的平整度有关。因此，下一步优化的方向可以采用滤波的方式减少输入和输出数据的波动，解决转向轮转向抖动问题。同时，可考虑采用俯仰脚校正算法提高其对地面不平整的定位精度，提高平台的综合性能，降低成本。