履带式移动机器人自主跟随算法研究

蒋建东，张 钧，李聪聪，章恩光

(浙江工业大学 特种装备制造与先进加工技术教育部重点实验室，浙江 杭州 310014)

履带式移动机器人自主跟随算法研究

蒋建东，张 钧，李聪聪，章恩光

(浙江工业大学 特种装备制造与先进加工技术教育部重点实验室，浙江 杭州 310014)

基于履带式移动机器人自主跟随控制平台，研究并设计了一种履带式农业机械自主跟随控制系统.结合神经模糊控制(ANFIS)思想，提出一种自适应前视距离自主跟随算法，并进行了履带式移动机器人的运动学求解并建立其纯追踪模型，在纯追踪算法中对前视距离参数进行动态调节.通过将引导车行驶路径划分成多段直线路径组合成的折线路径，使跟随车自主跟随的本质转化为对折线路径的快速跟踪，实现跟随车对引导车的自主跟随.仿真及试验结果表明：折线路径平均横向偏差由0.092 m减少为0.022 m，调节时间缩短约50%，该算法对折线路径的跟踪精度较高，响应时间短,满足自主跟随需求.

履带式；自主跟随；前视距离；模糊神经；路径跟踪

当前由于农业机械自主导航控制系统成本高昂[1]，其研究多局限于实验室环境，对于应付道路行驶与农田耕作的恶劣环境及突发情况的能力还有待提高，无法大规模应用.由此寻找一种既能提高农业机械自动化、智能化，同时又能兼顾其成本，可靠性与稳定性的新型农机智能化控制系统已成为各国研究的重点.为了兼顾上述特点，欧美日开展了农业机械自主跟随控制系统的研究[2]，该系统由引导车与跟随车组成，引导车由驾驶员驾驶，跟随车在其后自主跟随.由于该系统中包含了驾驶人员，有效提高了该系统的智能化程度，使得其应对道路行驶，恶劣耕作环境和突发情况的能力有了较大提高.

当前农用车辆自主跟随控制系统研究多集中于轮式车辆直线路径跟随前进，既有采用固定前视距离纯追踪算法进行农业机械导航控制[3]，但存在超调，也有采用模糊控制进行导航控制[4]，横向偏差与航向偏差为输入，前轮转角为期望输出，同时模糊规则由驾驶人员主观提出，跟踪误差大，无法快速修正.上述跟踪算法对于复杂多变路径的跟踪精度均不能满足农业精确作业要求，即使有些系统可以沿曲线路径跟随行驶，但由于算法过于复杂，成本过高，不适合大规模推广[5-6].针对提高复杂路径的跟随响应和跟踪精度，笔者通过研究两车协同作业的路径跟随方法，提出了一种基于ANFIS的自适应前视距离自主跟随算法.通过这种路径跟随方式，有效提高了模糊规则的客观性和准确性，较好地改善了履带式移动机器人的自主跟随效果.

1 履带式移动机器人平台及其硬件架构

履带式移动机器人平台自主跟随控制系统硬件框图如图1所示，主要包括PC端上位机系统，MCU主控单元，GPS/DR(Global positioning system/dead reckoning)导航控制单元，视频处理单元、无线收发单元，速度测量单元，电机驱动单元，其中STM32F103ZET6作为MCU主控芯片.履带式移动机器人自主跟随控制系统实物图如图2所示.

图1 履带式移动机器人硬件系统框图Fig.1 Block diagram of hardware system

图2 履带式移动机器人自主跟随系统Fig.2 Autonomous following control system

2 自主跟随软件流程及其算法

2.1 自主跟随软件流程

履带式移动机器人自主跟随控制系统采用两种模式，分别是遥控模式，自主跟随模式.遥控模式下操控人员通过PC端上位机软件直接发送指令控制履带式移动机器人行驶.自主跟随模式流程图如图3所示，设置路径更新周期T与传感器采样周期t，令T=nt,每隔一个采样周期读取引导车、跟随车坐标和速度信息，每隔一个路径更新周期更新一次跟踪路径，不断循环，实现跟随车自主跟随引导车.

图3 自主跟随模式软件流程图Fig.3 Software flow chart of autonomous following mode

2.2 两车协同作业的路径跟随方法

路径规划是移动机器人自主跟随控制系统的重要组成部分[7].自主跟随控制系统通过引导车与跟随车信息互联的方式获取周围环境信息进行局部路径规划[8-9].通过确定起始点为A，终止点为B来确定一条导航直线AB进行直线路径跟踪，取履带式移动机器人行驶时的一段转弯路径进行分析，如图4所示，黑色虚线矩形代表引导车，黑色实线矩形代表跟随车，A，B，C，D，E所在点代表引导车重心位置.自主跟随模式下，令间距等于规定行使速度与路径更新周期的乘积，使得引导车行驶完一个路径更新周期时跟随车恰好能到达引导车之前一段路径的起始点附近，有利于跟随车快速准确地跟随下一段路径.引导车以一定速度行驶，每隔一个路径更新周期生成一条直线跟踪路径，跟随车实时跟踪引导车生成的最新跟踪路径，实现自主跟随.以这种方式进行的自主跟随系统，将转弯路径中的曲线路径分为几段直线路径进行路径跟踪，即AB，BC，CD，DE段，使得引导车行驶路径成为多段直线路径组合成的折线路径，跟随车自主跟随的本质便转换为对折线路径的快速跟踪.该方法不但减少了计算量，还便易工程实现.

图4 转弯路径规划Fig.4 Turning path planning

2.3 履带式移动机器人运动学求解与自主跟随算法

纯追踪算法作为一种路径跟踪算法已有许多年的历史[10]，该算法是一种几何算法，用来计算车辆到达目标位置的轨迹弧长，其核心在于确定一个合适的前视距离.该算法模拟车内驾驶员的视觉，具有简单、直观且兼具仿生学的特点，被广泛应用在路径跟踪领域上.

如图5所示建立导航坐标系(X—O—Y)和车体坐标系(vX—p—vY)，设履带式移动机器人转弯曲率为γ，γ为有符号数，机器人逆时针旋转时为正，顺时针旋转时为负，图5以顺时针旋转为例，故γ<0；R为移动机器人此刻的瞬时转弯半径，为有符号数；d为移动机器人相对于跟踪路径l的横向偏差，为有符号数，同时规定机器人在沿期望路径前进方向左侧时d<0,反之，d>0，图5所示d<0；φ为移动机器人相对于跟踪路径l的航向偏差，航向偏差等于当前航向角度与跟踪直线相对于X轴正向角度之差，规定车体坐标系vY轴偏向期望路径左侧时为正，右侧时为负；Ld为前视距离；ψ为移动机器人沿着转向弧线到达目标点T(vXt,vYt)时的航向变化角度；(vXt,vYt)为T点在车体坐标系下的坐标；α为跟踪路径l与大地坐标系X轴正向的角度差；θ为机器人前进方向与大地坐标系X轴正向的角度差.

图5 运动学、纯追踪模型几何示意图Fig.5 Geometry sketch map of kinematics and pure pursuit models

由履带式移动机器人运动学模型可得

(1)

式中：L为履带式移动机器人双侧履带中心间距；iL和iR分别为履带式移动机器人左右两侧履带的滑转率，可表示为

(2)

vl,vr分别为履带式移动机器人左右两侧履带的实际速度，在不考虑履带滑转率的情况下，履带的理论速度等于实际速度，即

vL=vl,vR=vr

(3)

由于实际行驶过程中，履带滑转率由多方因素决定，很难测定，假设履带式移动机器人在行驶过程中双侧履带没有产生滑动，结合式(1～3)可得履带式移动机器人运动学方程为

(4)

式中P为履带式移动机器人当前的位姿矩阵.对式(4)积分后得

(5)其中:(xp0,yp0,θ0)为履带式移动机器人在t=0时的位置和方向;(xp,yp,θ)为机器人当前位置和方向.

履带式移动机器人在P点的线速度和角速度可分别表示为

(6)

假设履带式移动机器人的转弯半径为R，由式vP=RωP可得机器人当前转弯半径为

(7)

由图5可得履带式移动机器人纯追踪方程为

(8)

已知R=1/γ，结合式(8)得

(9)

由图5可得

(10)

(11)

由式(11)可得

(12)

2.4 基于ANFIS的前视距离仿真

2.4.1 前视距离控制策略

由于前视距离是纯追踪算法中的一个关键参数[11]，如何确定一个合适的前视距离是本节研究的重点.前视距离的大小与农业机械的横向偏差、航向偏差和车速有直接关系.路径跟踪的主要目的便是使跟踪车辆的横向偏差与航向偏差都趋于零.当横向偏差与航向偏差过大时，农机需要较小的前视距离快速减小偏差，提高系统响应速度；当横向偏差与航向偏差较小时，需要增大前视距离防止系统响应超调，提高系统稳定性.当车速过大时，需要适当增大前视距离，防止系统响应超调，当车速较小时，适当减小前视距离有利于提高路径跟踪精度.由于农机行驶速度较低且基本无变化，可认为是恒速，故笔者不考虑速度对前视距离的影响.

目前，一般通过仿真与实验确定一个合理的前视距离，但该方法由于前视距离固定，无法根据农机当前位置误差实时调整前视距离，并非是最优解.除此之外，也有采用模糊算法，通过归纳专家的操作经验来制定模糊规则表，从而在跟踪过程中对前视距离进行实时调节.由于农业机械驾驶人员的主观性导致利用直觉操作经验或直观感知来制定模糊规则的准确性大大降低，极大地提高了后期调试的工作量.因此将神经网络引入模糊系统，采用MATLAB中的ANFIS[12]，根据大量数据，通过自适应建模方法建立起模糊推理系统.由于用神经网络建立FIS是对数据进行处理的结果，更具客观性，同时采用输出量是数值函数的Sugeno型模糊模型，便于软件编程实现.

2.4.2 ANFIS训练样本采集

首先确定横向偏差的模糊论域为[-0.3,0.3](单位为m)，航向偏差的模糊论域为[-π/2,π/2](单位为弧度)，其中横向偏差以0.07 m为一档，航向偏差以15度为一档进行等比例划分仿真.通过对不同分档下不同大小的固定前视距离Matlab/Simulink仿真，确定行驶速度为0.3 m/s时各分档下的最佳前视距离.将这一系列仿真所得数据采集得到ANFIS训练样本，由于样本数据较多，只列出部分数据，如表1所示.

2.4.3 生成初始FIS

首先建立T-S型模糊模型，选择“And method”为“prod”, “Defuzzification”为“wtaver”,将上表所得训练样本装入ANFIS，选择网格分割法，网格分割法是将已装入ANFIS的数据用“网格”加以分割，然后按照设定的参数，依据模糊C-均值聚类方法建立起模糊系统.横向偏差与航向偏差均用7个模糊子集来覆盖输入变量，模糊语言值分别为横向偏差：{左大，左中，左小，零，右小，右中，右大}={LB,LM,LS,Z,RS,RM,RB}，航向偏差：{负大，负中，负小，零，正小，正中，正大}={FB,FM,FS,Z,ZS,ZM,ZB}.选择高斯型函数作为两个输入变量的隶属度函数，在输出量隶属函数类型中选则“linear”，表示选定输出量为输入量的线性函数.图6所示为ANFIS模型结构图，其中x1为横向偏差d,x2为航向偏差φ，y为前视距离Ld.

表1 自适应神经模糊系统训练样本

图6 ANFIS模型结构图Fig.6 ANFIS model structure diagram

2.4.4 生成隶属函数图及模糊规则

选用“hybrid”方法对样本数据进行训练，设置“Error Tolerance”为0，训练次数为500次，训练结束后得到误差为0.004 3，训练后得到横向偏差隶属度函数图、航向偏差隶属度函数图如图7所示.

由高斯型隶属函数公式与各隶属函数参数可得

覆盖输入量的各模糊子集隶属度函数表达式.整理输出函数参数可得出49个输出函数表达式.由此便可得出ANFIS根据训练样本建立的关于前视距离的2阶Sugeno型FIS的模糊规则,由于控制规则较多，所以只列出了部分规则：

R1：ifdis LB andφis FB thenLd=-0.567 6d-0.152 4φ-0.055 34

R2：ifdis LB andφis FM thenLd=-0.446 7d-0.154 7φ+0.078 13

R3：ifdis LB andφis FS thenLd=-0.472 7d-0.148 8φ+0.152 1

⋮

R47：ifdis RB andφis ZS thenLd=1.005d+0.188 1φ-0.095 37

R48：ifdis RB andφis ZM thenLd=0.993 4d+0.196 7φ-0.198 7

R49：ifdis RB andφis ZB thenLd=1.003d+0.173 5φ-0.274 1

图7 两种偏差隶属度函数图Fig.7 Two kinds of membership function diagram

3 自主跟随算法对比试验

设定履带式移动机器人初始位置在(0,0)(单位为米)处，，初始航向角为45°，行驶速度为0.3 m/s，固定前视距离取车速为2～3 s走过的距离时与人的驾驶行为接近[13]，故取0.6 m，图8黑色轨迹(农田作业边界和作业路径为典型折线段)为引导车行驶时传递给跟随车的期望跟踪路径，即

图8 折线路径试验场景Fig.8 Experiments of poly line path

两种路径跟踪算法对比如图9所示，其中实线代表动态前视距离算法，虚线代表固定前视距离算法.可知在y=x段时，两种算法效果相当，当x=

0.5进入折线路径时，动态Ld算法能快速跟踪期望路径，并将误差控制在较小范围内，其横向偏差最大为0.065 m，平均为0.024 m，需要时间2.17 s，固定Ld算法横向偏差快速增大且长期维持较大误差直至进入第三条直线路径，其最大横向偏差为0.153 m，平均横向偏差为0.103 m.进入y=x-1直线段后，动态Ld算法依旧能快速跟踪期望路径，横向偏差最大为0.065 m，平均为0.022 m，需要时间2.13 s，而固定Ld算法由于在y=0.5段积累的误差导致其无法及时跟踪期望路径，其横向偏差仍旧维持在较大水平，最大横向偏差为0.152 m，平均横向偏差为0.092 m.综上所述可知动态Ld算法在跟踪折线路径时的调节时间，最大横向偏差，平均横向偏差均明显小于固定Ld算法，满足自主跟随模式下跟踪路径的连续变化.

图9 两种算法对比图Fig.9 Comparison of two algorithms

4 结 论

以履带式移动机器人为平台，针对履带式移动机器人两车协同跟随作业，进行了自主跟随控制平台及控制系统设计，以及两车协同作业的路径跟随模型及算法研究，提出了一种动态最优前视距离的农业履带移动机器人自主跟随算法，其核心在于通过跟随过程中不同横向偏差与航向偏差信息采集，通过自适应神经模糊算法确定出跟随模型的最佳前视距离，从而提高路径跟随精度，最后进行了典型折线路径跟随仿真及试验.结果表明：采用动态最优前视距离自主跟随算法的调节时间缩短约50%，折线路径平均横向偏差由0.092 m减少为0.022 m.因此该算法具有良好的实时调节性能，满足农机装备在农田作业复杂路径下的自主跟随需求.

[1] 张立彬,计时鸣,胥芳,等.农业机器人的主要应用领域和关键技术[J].浙江工业大学学报,2002,30(1):36-41.

[2] TORII T. Research in autonomous agriculture vehicles in Japan[J]. Computers and electronics in agriculture,2000,25(1/2):133-133.

[3] 张智刚,罗淆文,赵祚喜,等.基于Kalman滤波和纯追踪模型的农业机械导航控制[J].农业机械学报,2009,40(增):6-12.

[4] 陈军,蒋浩然,刘沛,等.果园移动机器人曲线路径导航控制[J].农业机械学报,2012,43(4):179-187.

[5] 陈军,朱忠祥,鸟巢谅,等.拖拉机沿曲线路径的跟踪控制[J].农业工程学报，2006,22(11)：108-111.

[6] 朱忠祥,松正河,谢斌,等.拖拉机队列自动控制系统[J].农业机械学报,2008,40(8):149-154.

[7] 王燕,张立彬,杨庆华.黄瓜采摘机器人运动规划与控制系统研究[D].杭州：浙江工业大学,2010.

[8] 王宇,董辉,徐建明,等.一种新的智能小车控制策略[J].浙江工业大学学报,2012,40(2):217-221.

[9] 陈晓眯,孟志青,徐杰.基于混合禁忌搜索算法的动态车辆路径研究[J].浙江工业大学学报,2009,37(5):580-585.

[10] 陈军,鸟巢谅,朱忠祥,等.拖拉机在牧草地上自动引导行走的控制[J].农业机械学报,2005,36(7):104-107.

[11] LI C H, ZHU X J, CAO G Y, et al. Identification of the Hammerstein model of a PEMFC stack based on least squares support vector machines[J]. Journal of power sources,2008,175(1):303-316.

[12] 刘怀国,孙建华,张冰,等.ANFIS及其在控制系统中的应用[J].华东船舶工业学院学报(自然科学版),2001,15(5):27-31.

[13] POMERLEAU D A. Neural network perception for mobile robot guidance[M]. Massachusetts: Kluwer Academic Publishers Norwell,1993.

(责任编辑：刘 岩)

Study on autonomous following algorithm of crawler-type mobile robot

JIANG Jiandong, ZHANG Jun, LI Congcong, ZHANG Enguang

(Key Laboratory of Special Purpose Equipment and Advanced Manufacturing Technology, Ministry of Education, Zhejiang University of Technology, Hangzhou 310014, China)

An autonomous tracking control system for agricultural machinery is studied and designed based on the autonomous tracking control platform of crawler-type mobile robot. An algorithm for adaptive look-ahead distance is proposed combined with neural fuzzy control (ANFIS). The kinematic model and the pure pursuit model for crawler-type mobile robot ate established and the parameters of look-ahead distance are adjusted dynamically. By dividing the driving path of the guided vehicle into a poly line path combined with multiple linear paths, the essence of the following vehicle is transformed into the fast track of the poly line path, and the guided vehicle is autonomously tracked by the following vehicle. The experimental results indicated that the average lateral deviation of the line path is reduced from 0.092 m to 0.022 m, and the adjust time is reduced by about 50%. This algorithm can meet the demands of autonomous tracking.

crawler-type; autonomous following; look-ahead distance; fuzzy neural; path following

2016-11-02

国家自然科学基金资助项目(51375456)；浙江省重大专项项目(2013C01048)

蒋建东(1974—)，男，四川广安人，教授，博士，主要从事机电系统设计与分析方面的研究,E-mail:jiangjd@zjut.edu.cn.

S232；TP242

A

1006-4303(2017)04-0355-06