基于虚拟探照灯的离合制动履带底盘寻径跟踪.pdf

基于虚拟探照灯的离合制动履带底盘寻径跟踪陈子文1，熊扬凡1，胡宗锐1，李聪2，庞有伦3，杨明金1（1.西南大学工程技术学院，重庆400715；2.重庆工业职业技术学院机械工程与自动化学院，重庆401120；3.重庆市农业科学院农业机械化研究所，重庆401329）摘要：油动履带底盘由于具有成本低、技术成熟、动力充足、续航能力强、维修便捷等特点而被广泛应用于农业生产，但其操控精度低、振动严重等问题对其应用造成挑战。该研究以农用油动履带底盘为对象，根据共性传动结构提出离合制动式履带底盘，并以 STM32F303 作为主控器搭建基于实时动态差分全球导航卫星系统（real-timekinematicglobalnavigationsatellitesystem，RTK-GNSS）的自动导航系统，建立其运动学模型，提出一种改进模糊式预瞄控制算法虚拟探照灯寻径跟踪（virtualsearchlightpathfindingtracking，VSPT）算法。针对横向偏差和速度变化引起的跟踪振荡问题，提出基于横向偏差指数的视域角动态调整方法和基于行驶速度的虚拟目标点判断方法，并通过仿真和试验验证算法的有效性。仿真结果表明，在底盘前进速度 0.4m/s 时，横向偏差指数、视域增益 k1和目标增益 k2取值分别为 1/4，0.005radm 和 6s1时可获得较好的导航效果。现场试验结果表明，在水泥路面，不同初始位姿下，直线导航平均上线距离为 1.64m，平均横向偏差和航向偏差为 0.44cm 和 1.57；行驶速度提高会导致导航精度降低，适当修正 k1、k2可维持较好的导航效果，3 种行驶速度下获得的平均横向和航向偏差分别为 0.75cm 和 1.05，平均纠偏次数为 4.7 次；田间路况下，由于土路附着系数增加，转向相对平稳，纠偏次数降低至 2.7 次，相同参数时 2 种路况下的导航效果接近。研究表明，VSPT 算法针对离合制动式履带底盘具有良好的跟踪效果和路况适应性，该研究可为离合制动履带平台提供一种高效、稳定的导航控制方案。关键词：精准农业；车辆；导航；RTK-GNSS；履带底盘；控制算法doi：10.11975/j.issn.1002-6819.202303024中图分类号：S224.9文献标志码：A文章编号：1002-6819(2023)-12-0010-10陈子文，熊扬凡，胡宗锐，等.基于虚拟探照灯的离合制动履带底盘寻径跟踪J.农业工程学报，2023，39（12）：10-19.doi:10.11975/j.issn.1002-6819.202303024http:/www.tcsae.orgCHENZiwen,XIONGYangfan,HUZongrui,etal.PathfindingandtrackingofclutchbraketrackchassisbasedonvirtualsearchlightJ.TransactionsoftheChineseSocietyofAgriculturalEngineering(TransactionsoftheCSAE),2023,39(12):10-19.(inChinesewithEnglishabstract)doi:10.11975/j.issn.1002-6819.202303024http:/www.tcsae.org0引言农业机械自动导航是精准农业的关键技术，可以极大减轻操作人员的劳动强度，同时提高作业质量与作业效率1-3。底盘总成是移动式农用动力机械的核心部件之一，在通用式底盘上搭载其他功能性农具可实现多种农业生产作业4。相较于普通轮式拖拉机，自走式履带底盘体积小巧，拥有更低的接地比压、滑转率和滚动阻力系数，地形适应能力广。尤其在复杂多变的山地和黏湿土壤的田间环境下，履带式底盘表现出卓越的综合性能5。目前，农机装备自动导航研究主要集中在轮式拖拉机和电驱动力平台，涉及的导航控制方法包括 PID 控制6、模糊控制7-8、预瞄跟踪控制9、非线性模型预测控制10、基于位姿偏差的转向角线性控制11-12、基于运动特性的导航控制13以及反步滑动模式控制14等。然而，针对低成本、技术成熟，采用转向离合器进行转向的油动机械换挡履带底盘的导航系统研究较少。ZHANG 等15针对单缸柴油履带车导航提出一种基于虚拟阿克曼转向模型的状态反馈导航控制系统，并设计了基于脉宽调制原理的前向比例控制方法，在果园环境中的平均跟踪误差为 5.1cm，跟踪误差标准差为 8.4cm。丁幼春等16针对搭载油菜播种机的履带底盘设计了免疫 PID 导航控制器，当行驶速度为 0.5m/s 时，其平均导航偏差为 4.2cm，最大偏差为 11.9cm。刘志杰等17针对小型履带拖拉机，通过搭载组合导航系统，并利用神经网络提出了一种虚拟雷达模型的路径跟踪控制算法，当行驶速度为 0.36、0.75m/s 时，平均横向偏差分别为 3.1、5.1cm。吴才聪等18针对履带底盘提出模糊自适应纯追踪控制方法，水泥路面试验表明，当行驶速度为 0.8m/s时，模糊自适应纯追踪控制器最大跟踪偏差为 3.9cm，平均绝对偏差为 1.8cm。在农业作业场景中，应用于此类农用动力机械的路径跟踪算法必须满足较高的精度要求。例如，在播种、施肥和喷药等环节，精准的路径跟踪有助于减少重复覆盖和遗漏区域，从而提高作业效率19。同时，较高的行收稿日期：2023-03-05修订日期：2023-05-11基金项目：国家重点研发计划项目（2018YFD0700603-1）；重庆市技术创新与应用发展专项重点项目（cstc2021jscx-gksbX0013）作者简介：陈子文，博士，讲师，研究方向为农业机器人和机器视觉技术。Email：chenziwen_通信作者：杨明金，博士，教授，博士生导师，研究方向智能农业装备和农产品产地加工技术。Email：第39卷第12期农 业 工 程 学 报Vol.39No.12102023年6月TransactionsoftheChineseSocietyofAgriculturalEngineeringJune2023驶速度也可缩减作业周期，以适应农业生产的季节性要求20。但过高的速度会导致导航系统的精度和稳定性降低，因此要求跟踪算法在保证导航精度的要求下尽可能提高行驶速度。另一方面，在实际农业作业中，自动导航系统需在较短时间内上线并开始跟踪期望路径。若上线距离过长，不但会降低作业效率，甚至影响整个农业生产过程。然而，油动换挡履带底盘难以进行精准的速度控制，其速度控制只能通过调整油门和机械档位实现，速度滞后较严重，同时由于各侧履带无法单独速度控制，因此只能进行单一模式转向。值得关注的是，在离合制动式履带平台上，每一次的转向动作，尤其在直线跟踪过程中，都会对车辆造成一次刚性冲击。这主要是由于转向时，履带平台的单侧履带轮锁死，从而导致平台速度瞬间改变造成的。频繁转向会导致搭载在平台上的功能性农具的作业精度下降，甚至无法正常工作；并且过于频繁的纠偏可能会对农具或履带平台自身造成损坏。故此类履带平台在农业作业中并不适宜频繁转向纠偏。目前的各类路径跟踪算法并未充分考虑车辆在跟踪过程中的纠偏次数，尤其是针对离合制动式履带平台的算法，因此，在设计适用于此类平台的跟踪算法时，既需要严格限制纠偏次数，又要确保跟踪精度得到满足。这一原则有助于提高农业作业效率，减少搭载农具的精度损失和避免农具及履带平台的损坏。本文以 3WZF-400 汽油驱动履带底盘为研究对象，提出一种虚拟探照灯寻径路径跟踪方法，通过仿真和导航试验对系统和算法性能进行验证，该方法综合考虑离合制动式履带平台在跟踪过程中的纠偏次数限制和跟踪精度需求，旨在为此类平台提供一种高效、稳定的导航解决方案。1材料与方法1.1离合制动式履带底盘构成本文以 3WZF-400 汽油驱动履带底盘（苏州博田自动化技术有限公司生产）为研究对象搭建自动导航系统，如图 1 所示，主要由履带总成、传动系统和操纵系统组成。履带车总长 1.2m，宽 0.9m，橡胶履带宽 0.2m，动力由 5.4kW 单缸汽油机提供，该履带底盘体积小，对各种果园、田地道路条件有较强适应性。履带底盘动力传动与转向系统如图 2 所示。发动机将动力传递给手动换挡变速箱，离合器手柄可控制动力离合器的闭合，将动力传递给驱动桥，左右两个驱动轮前端均设有常闭离合器，通过转向手闸轧线控制离合器打开，切断动力传输，实现停机或转向。由于该类动力平台通过控制离合器闭合，实现车辆制动或转向，故将此类结构和操控方式称为离合制动式履带底盘。为实现履带底盘自动操控，对手动操控系统进行升级，采用气动系统代替手动操作，应用 3 组独立控制的双作用气缸拉动轧线控制 3 个离合器的闭合，实现启停、直线行驶、转向等动作21。1234568109121314151171.导航控制箱2.气泵3.转向手闸4.换挡手柄5.动力离合器手柄6.气动系统控制箱7.RTK-GNSS 移动站定位天线8.STM32F303 控制器9.RTK-GNSS 移动站接收机10.ZigBee 无线模块11.电台天线12.无线电台13.RTK-GNSS 基站接收机14.RTK-GNSS 基站天线15.上位机1.Navigationcontrolbox2.Airpump3.Steeringhandbrake4.Shifthandle5.Powerclutchhandle6.Pneumaticsystemcontrolbox7.RTK-GNSSmobilestation positioning antenna 8.STM32F303 controller 9.RTK-GNSS mobilestationreceiver10.ZigBeewirelessmodule11.Radioantenna12.Radiostation13.RTK-GNSS fixed station receiver 14.RTK-GNSS fixed station antenna15.Computer图 1离合制动式履带底盘系统组成Fig.1Compositionofclutch-braketrackchassis1234561.左转向离合器2.右转向离合器3.动力离合器4.变速箱5.发动机6.履带总成1.Left steering clutch 2.Right steering clutch 3.Power clutch 4.Gearbox5.Engine6.Trackassembly图 2履带底盘转向结构示意图Fig.2Steeringmechanismdiagramoftrackchassis1.2导航控制系统架构导航控制系统采用以 ARMCortexM4 为内核的STM30F303VCT6 单片机作为中控器，数据接收、数据解析、运算处理等任务均由该单片机完成。全球卫星导航系统（globalnavigationsatellitesystem，GNSS）采用上海华测导航技术股份有限公司的 P3-DU 北斗高精度定位测向接收机，搭配有载波相位差分技术（real-timekinematic，RTK），多系统双频，支持双天线定位测向解算，在装备有移动站接收机的基础上，通过搭建基站接收机和无线电台，实现 RTK-GNSS 高精度定位数据的接收。导航控制系统整体架构如图 3 所示，RTK-GNSS定位模块利用无线电台接收差分信号，经差分系统校准后，中控器采取通用异步串行通信（universalasynchronousreceiver/transmitter，UART）方式获取高精度定位数据。将解析数据与设定的期望路径进行比较，利用偏差跟踪算法得到预期驾驶动作，通过控制执行机构以达到路径第12期陈子文等：基于虚拟探照灯的离合制动履带底盘寻径跟踪11跟踪的目的。上位机可以监控导航控制系统的工作状态，其与中控器之间采用 UART 方式并通过 ZigBee 无线模块进行数据的相互传输。中控器将解析后的定位信息以及导航过程中所产生的各项导航参数反馈给上位机，在系统发生异常时可通过发送相关指令终止导航作业。在本研究中，选择 ZigBee 模块主要考虑到其成本低、体积小、使用方便等优势，有利于在短时间内实现系统原型的搭建与初步测试。RTK-GNSS定位模块中控器上位机输出控制电路执行机构UARTUARTZigbeeSTM32F303华测P3-DU图 3导航控制系统架构Fig.3Structureofnavigationcontrolsystem2路径跟踪控制方法2.1履带底盘运动学模型根据离合制动式履带底盘的工作原理，只能依靠单侧履带轮完全制动来实现转向，即转向时内侧履带的驱动轮角速度为零，外侧履带轮角速度不变，且履带底盘作业过程中驱动轮的速度无法随意调节，在导航平面坐标系 X-O-Y（n 系）中建立履带车运动学模型，如图 4 所示，G(x,y)为履带车几何中心坐标。考虑到农业作业中一般行驶速度较低，可做如下假设：1）履带底盘质心与几何中心重合；2）履带底盘在作业过程中非制动时履带的带速恒定；3）忽略双侧履带的滑移和滑转。GbOr Gvvrv1voXY注：G 为履带底盘几何中心点，G点为转动后履带底盘几何中心点，o 为旋转中心；为履带底盘航向角，02，rad；b 为双侧履带中轴线间距，m；vl为左侧履带的带速，ms1；vr为右侧履带的带速，ms1；v 为履带底盘几何中心速度，ms1；r 为转弯半径，m；为转向角，rad。Note:Gisthegeometriccenterpointofthetrackchassis,Gisthegeometriccenterpointofthetrackchassisafterturning,oisthecenterofrotation,istheheadingangleofthetrackchassis,02,rad;biscenteraxisspacingoftracksonbothside,m;vlisthespeedofthelefttrack,ms1;vristhespeedoftherighttrack,ms1;visthespeedofthegeometriccenterofthetrackchassis,ms1;risturningradius,m;meanssteeringangle,rad.图 4离合制动式履带底盘运动学模型Fig.4Kinematicmodelofclutch-braketrackchassis定义履带底盘的航向角 为 Y 轴正方向顺时针旋转至速度 v 的角度，转向角 的大小与转向对应的圆心角相等，其值与转向持续时间有关。根据运动学原理有：xy=sincos1/rv（1）其中：1r=?vlvrvl+vr?2b（2）以右转为例，在转向时有：v=rrlvl（3）式中 rl表示左侧履带几何中心相对于旋转中心 o 的距离，m。由于右转时右侧履带完全制动，即：vr=0（4）所以有：rrl=1/21b（5）将式（5）带入式（3）可得：v=vl2（6）由式（6）可看出，履带底盘的旋转中心位于履带的几何中心处，车辆进行转向时，旋转中心在左右两侧履带的几何中心之间切换，故两侧履带可独立控制转速和转向的差速模型不适用于此类履带底盘。2.2虚拟探照灯寻径跟踪算法原理路径跟踪的实质是通过控制车辆的运动减少车辆与期望路径之间的偏差，在农机导航作业中，作业路径通常由直线段构成。为提高作业质量和效率，要求农机自动导航系统上线快速稳定、直线跟踪精度高以及对颠簸复杂路面抗干扰性好22。目前大多数跟踪算法按照某种控制策略计算车辆的前轮期望转角，从而通过改变前轮转角控制车辆运动轨迹的曲率，如纯跟踪算法23、Stanley 算法24、MPC 算法25-26。对于差速转向的履带底盘，可以通过改变双侧履带的速度调整履带底盘运动轨迹的曲率，所以可利用转换轮式车辆的跟踪算法实现对目标路径的跟踪控制。而对于离合制动式履带底盘，双侧履带的带速不可随意调节，同时受到速度传感器缺失和路面环境等因素影响，利用转向时间间接获取期望转向角会产生较大误差。此外，为避免履带底盘在跟踪过程中频繁转向纠偏导致的作业精度下降、农具以及履带底盘自身损坏，应对纠偏次数严格限定。本文提出的虚拟探照灯寻径跟踪（virtualsearchlightpathfindingtracking，VSPT）算法假设履带底盘搭载一种光线射程为无限远的虚拟探照灯，并设定一个位于期望路径及其延长线上且不断运动的假想目标点 C，通过模拟探照灯搜索物体的方式建立履带底盘不断追逐目标点的路径跟踪方法。假定该虚拟探照灯安装于履带底盘几何中心处，其视域角为，且光束投射方向与履带底盘航向一致。图 5 所示，若目标点 C 位于虚拟探照灯视域内，则履带底盘直线前进，反之，目标点 C 位于虚拟探照灯视域范围外，履带底盘通过转向寻找该目标点并使其位于视域内。12农业工程学报（http:/www.tcsae.org）2023年GCCGGC右转Turn right左转Turn left vvva.右转a.Turn rightb.直行b.Go straightc.左转c.Turn left注：为虚拟探照灯的视域角，rad；C 为假想目标点；为 v 与 GC 之间的夹角,，rad。Note:isthevisualangleofvirtualsearchlight,rad;Cistheimaginarytargetpoint;istheanglebetweenvandGC,rad.图 5基于虚拟探照灯的履带底盘转向示意图Fig.5Trackchassissteeringdiagrambasedonvirtualsearchlight设履带底盘导航路径坐标点序列 A1，A2，Ai，An，如图 6 所示，以 AiAi+1为期望路径进行分析，视域角的大小影响导航作业的稳定性，当履带底盘逐渐靠近期望路径时，视域角 不变，由于视场变小，则视域内可见的期望路径变短，类比于真实场景中驾驶员视野变窄，此时算法对横向偏差极度敏感，将不断调整航向导致系统振荡。同时在横向偏差较小的情况下，若视域角 过大，也会导致导航误差增加，系统稳定性下降。故本文提出一种基于横向偏差指数的视域角动态变化方法，视域角反比于横向偏差 d 的 次方，指数 可用于调节视域角的变化速率，具体计算式为=k1d（7）式中 k1表示视域增益，radm；d 表示横向偏差，m；表示横向偏差指数。规定目标点始终于车辆前方，即位于直线 QAi+1及履带底盘几何中心G在期望路径 AiAi+1上的投影点 Q向 Ai+1点方向的延长线上。目标点 C 的位置控制履带底盘向期望路径靠近的迫切程度。低速行驶时履带底盘目标点靠近当前位置，履带底盘应尽快接近期望路径，高速行驶时则应避免很小的横向偏差引起跟踪振荡。因此将直线 QC 的长度 m 与履带底盘行驶速度之间设置为正比例关系，计算式为m=vk2（8）式中 k2表示目标增益，s1。若直接利用点 C 坐标与视域方程判断目标点 C是否位于视域范围内，则运算过程较为复杂繁琐。为简化计算过程，本文采用期望航向偏差角 作为中间参数进行求解，可表示为=arctank2dv（9）由式（9）可知，与横向偏差有关，d 愈大，愈趋近于 0.5，反之，趋近于 0。G Ai+1QC iAi GddvQ C DEXYO v注：Ai为期望路径起点；Ai+1为期望路径终点；d 为横向偏差，m；为期望航向偏差角，0 0.5，rad；Q 为 G 在期望路径 AiAi+1上的投影点；D、E 为视域边缘与 AiAi+1的交点；i为从 Y 轴正方向旋转至期望路径AiAi+1的角度，0i2，rad；为履带底盘航向偏差角，rad；C为 G 位于 G时所设定的假想目标点；为 G 位于 G时 v 与 GC之间的夹角，rad；为 G 位于 G时的航向偏差角，rad；Q为 G在期望路径AiAi+1上的投影点。Note:Aiisthestartingpointofthedesiredpath;Ai+1istheendpointofthedesiredpath;disthelateraldeviation,m;istheidealangleofcoursedeviation,0 0.5,rad;QistheprojectionpointofG onthedesiredpathAiAi+1;D,EaretheintersectionpointsofthevisualfieldedgesandAiAi+1;iistheanglerotatingclockwisefromthepositivedirectionofY-axistodesiredpathAiAi+1,0i2,rad;istheangleofcoursedeviationofthetrackchassis,rad;C is the imaginary target point set when G is at G point;is theintersectionanglebetweenvandGCwhenGisatG;istheangleofcoursedeviationwhenGisatG;QistheprojectionofGonthedesiredpathAiAi+1whenGisatG.图 6路径跟踪模型Fig.6Pathtrackingmodel如图 6 所示，若位于 G 点与 G点时履带底盘参数，i，d 等相等，则 绝对值相等。而 G 点位于有向线段 AiAi+1左侧，G点位于右侧，显然绝对值相等的 不能够准确描述上述情况。为赋予 更多物理含义，定义直线 DE 的长度为履带平台的视野范围，若视野范围大于 0，则航向偏差角 为正值，反之，航向偏差角 为负值。履带底盘航向与假想目标点之间的夹角 可直接指导履带底盘的驾驶动作，根据 和 的取值，可通过VSPT 算法进行求解。为保证履带底盘通过转向搜寻目标点时路径最短，即转角最小，绝对值应小于，且转弯方向总是朝着 绝对值逐渐减小的方向。若 为正，目标点 C 位于履带底盘右侧，需向右转向；若 为负，则目标点 C 位于履带底盘左侧，向左转向。2.3基于数据流图的软件设计及算法实现数据流图（dataflowdiagram，DFD）是描述系统信息流以及数据变换流程的图形27，具有描述系统逻辑结构的能力28。建立的 DFD 如图 7 所示，数据源包括上位机、RTK-GNSS 定位系统以及空闲中断服务函数。上位机提供用户预设的基于 WGS84 坐标系（e 系）下的期望路径坐标序列和原点坐标，系统将两者做坐标转换26，得到 n 系下的期望路径坐标。RTK-GNSS 定位系统通过标准串口实时输出不定长数据。为解决不定长数据稳定接收同时提高系统工作效率，采用直接存储器访问（directmemoryaccess，DMA）方式，结合串口空闲中断和循环队列策略进行数据接收，通过数据解析以及坐标转换得到履带底盘航向、速度以及 n 系下的定位坐标。第12期陈子文等：基于虚拟探照灯的离合制动履带底盘寻径跟踪13将数据传入 VSPT 算法后解算出行驶动作并传递给执行机构，从而完成履带底盘的自动驾驶。RTK-GNSS定位系统上位机空闲中断服务函数坐标转换原始数据数据首地址、数据长度数据提取原始数据原始数据数据解析执行机构最新帧数据原始数据坐标转换VSPT算法行驶动作期望路径坐标(e)原点坐标(e)期望路径坐标(n)定位坐标(n)定位坐标(e)原点坐标(e)原点坐标(e)航向、速度DMA数据传输处理注：e 为 WGS84 坐标系；n 为平面坐标系。Note:eistheWGS84coordinatesystem;nistheplanecoordinatesystem.图 7数据流图Fig.7Dataflowdiagram通过 DMA 传输，结合空闲中断与循环队列的数据接收方式的优势在于系统进行工作运算的同时能够进行数据的接收。每次进行数据提取时均能捕捉到最新序列的数据帧，提高了定位数据与实际位置的吻合度；省略了 CPU 数据接收流程，提高了系统运算的效率和稳定性。3仿真与现场试验3.1评价指标为精确评价路径跟踪算法的导航效果，将履带底盘跟踪期望路径的过程以上线点为界限分为上线过程以及直线跟踪过程，如图 8 所示。起始点作为履带底盘的初始位置，上线点指横向偏差小于 3.0cm 同时航向偏差小于 2.0且与起始点相距最近的采样点。上线过程采用上线距离作为评价指标；直线跟踪过程采用横向偏差、航向偏差均值、标准差和均方根以及纠偏次数作为评价指标。上线过程On-line process直线跟踪过程Straight-line tracking process期望路径 Desired path 实际轨迹 Real trajectory 起始点 Initial position 上线点 On-line position图 8上线过程与直线跟踪过程Fig.8On-lineprocessandstraight-linetrackingprocess上线距离为从起始点到上线点的实际轨迹在期望路径上的投影长度，表征系统在偏离期望路径时的响应速度。横向偏差均值 ldm为直线跟踪过程中履带平台几何中心位置采样点距期望路径横向偏差 d 绝对值的均值，可表示为dj=(Yi+1Yi)(xjXi)+(Xi+1Xi)(Yiyj)(Yi+1Yi)2+(Xi+1Xi)2（10）ldm=qj=1?djq?（11）式中下标 j 表示采样点序号；q 表示采样点总数；(xj,yj)表示履带底盘在平面坐标系下的几何中心坐标，m；Ai(Xi,Yi)，Ai+1(Xi+1,Yi+1)表示期望路径两端点在平面坐标系下的坐标，m。航向偏差均值 cdm为直线跟踪过程中履带平台航向角 与 i偏差绝对值的平均值，可表示为cdm=qj=1?jiq?（12）纠偏次数指上线后导航系统自动控制履带底盘左、右转向的次数，衡量算法对直线跟踪的振荡情况。履带底盘在直线跟踪中频繁左、右转向会导致车体剧烈振动，从而影响定位精度，延长导航时间，无法满足作业平稳性的需求；同时车体频繁振荡对机械、电机结构的耐久性均产生影响。试验中动态采集履带底盘转向控制信号并传输给上位机，采样频率 5Hz。如图 9，单次试验过程中采集 150 个控制信号，图中数据点代表控制器发出的右转、左转或直行控制信号。图中虚线为上线时刻所对应的控制数据序列的位置，纠偏次数为上线之后的转向次数。由于履带平台转向控制响应具有滞后性，经试验验证，单帧控制数据无法使履带平台转向，判定为控制噪声，3 帧及以上可作为判断转向的依据。因而定义一次纠偏为上位机接收连续 3 帧及以上的转向数据。试验中 150 个控制数据点中发生了 3 次纠偏。直行Go straight第1次纠偏The first correction第2次纠偏The second correction第3次纠偏The third correction501001500数据点 Data point上线分界线 Dividing line of on-line纠偏数据点 Deviation correction data point数据点序列Data sequence右转Turn right左转Turn left图 9纠偏次数统计Fig.9Statisticalofdeviationcorrection3.2仿真试验为验证虚拟探照灯寻径跟踪算法的导航效果并对指数、视域增益 k1、目标增益 k2进行标定，利用 MATLAB对其进行仿真试验。设定仿真过程中的路径起始点坐标为(0,0)，终点坐标为(10m,10m)，履带底盘的初始横向偏差为 0.5m，初始航向偏差角 为 25，采样时间为0.2s，假设双侧履带在非制动情况时速度恒定，为 0.4m/s，14农业工程学报（http:/www.tcsae.org）2023年履带轴线间距 b=0.9m。取初始 k1=0.005radm，k2=6.0s1进行仿真试验，试验结果如表 1。通过对比导航效果（试验号 15），选取较优值=1/4。仿真试验发现，控制参数 k1、k2的取值对导航效果有重要影响。因此在 取值确定后，需初步评估 k1、k2的合理值，为现场试验提供依据。图 10 为上线过程和直线行驶过程的仿真轨迹。根据图 10 和表 1 可知，视域增益 k1对上线距离影响较小，但对直线跟踪过程中 ldm的大小有重要影响，在合理范围内视域增益 k1增大则 ldm变小。目标增益 k2对上线距离有重要影响，随着 k2的增大，上线距离先减小后增大，这是由于过大的 k2会引起较大的超调，降低履带底盘的上线性能，且 k2越小轨迹越平滑，反之愈发激进。仿真结果表明：虚拟探照灯寻径的跟踪算法具有较高的跟踪精度，能够满足导航要求。通过比较导航效果，最终选取 的较优值为 1/4，k1的较优值为 0.005radm，k2为 6.0s1。表1路径跟踪仿真试验结果Table1Path-trackingsimulationtestresults试验号TestNo.速度Speed/(ms1)初始横向偏差Initiallateraldeviation/cm初始航向偏差Initialcoursedeviation/()k1/(radm)k2/(s1)上线距离On-linedistance/m横向偏差绝对值Absolutevalueoflateraldeviation/cm航向偏差绝对值Absolutevalueofcoursedeviation/()均值Mean标准差Standarddeviation均方根Rootmeansquad均值Mean标准差Standarddeviation均方根Rootmeansquad10.40.52510.00506.01.831.170.761.280.540.881.0320.40.5251/20.00506.00.960.350.200.390.360.790.9330.40.5251/30.00506.00.950.170.100.210.370.610.7440.40.5251/40.00506.00.950.070.090.110.410.490.6450.40.5251/50.00506.00.950.060.090.100.410.480.6460.40.5251/40.00502.01.160.150.090.170.380.210.4470.40.5251/40.005010.01.140.090.140.120.470.490.6480.40.5251/40.00026.00.930.140.170.090.370.450.6190.40.5251/40.00106.00.910.120.190.090.370.450.60100.40.5251/40.02506.00.920.510.280.560.450.911.02注：为横向偏差指数，k1为视域增益，k2为目标增益。下同Note:isthelateraldeviationindex,k1isthefield-of-viewgain,andk2isthetargetgain.Thesamebelow.0.60.660.40.200.80.460.260.86y/ma.上线过程a.On-line processx/m0.9050.9100.9200.9150.9250.930.920.91y/mb.直线跟踪过程b.Straight-line tracking processx/m期望路径Desired pathk1=0.005,k2=6.0k1=0.025,k2=6.0k1=0.005,k2=10.0k1=0.005,k2=2.0上线点On-line point图 10路径跟踪仿真轨迹Fig.10Pathtrackingsimulationtrajectory3.3现场试验3.3.1试验方法本试验旨在探究并优化 VSPT 算法对于直线跟踪的性能。试验在西南大学内水泥路和试验田土路开展，试验时间为 2022 年 12 月 8 日至 9 日。利用华测高精度RTK-GNSS 系统（数据更新频率为 5Hz）采集试验场地中两点坐标，并将其连线作为期望路径，设置其中一点作为当地水平坐标系下的坐标原点，导航系统运行时控制器将经纬度转换为 n 系下的坐标，并实时向上位机发送履带底盘的当前坐标、横向偏差、航向偏差等导航参数，以控制履带底盘导航运行。考虑到作业环境以及作业参数的复杂性，为全面验证本文提出算法的导航效果，首先进行算法参数验证，然后针对不同初始参数、运动参数和路况情况进行对比试验。算法参数验证试验以仿真结果为依据，通过实地试验验证导航效果。初始参数试验通过设置履带平台不同初始位姿，包括初始横向偏差和初始航向偏差，验证导航算法对不同初始状态的导航效果，试验中选取初始横向偏差分别为 0.5、1.0 和 1.5m，初始航向偏差角选取25。运动参数试验为不同运动速度下的导航试验，旨在验证算法对不同速度的适应性，并分析速度对导航效果的影响，选取低速（1 档）、中速（2 档）、高速（3 挡）3 种档位，对应的实测速度分别为 0.4、0.8 和1.2m/s。考虑到履带底盘的应用场景，选择水泥路面和具有杂草覆盖的土路进行路况对比试验。3.3.2算法参数可行性验证利用上文仿真试验所标定的算法参数进行现场实验，水泥路面采用低速 1 档，设置初始横向偏差 0.5m 以及初始航向偏差角 25，取控制参数 k1=0.005radm、k2=6.0s1、=1/4，该组试验重复 5 次，结果取均值，结果见表 2 中试验号 1。较仿真结果相比，现场试验各项评价指标均有所增加，上线距离增大 0.52m，横向和航向偏差绝对值均值分别增大 0.35cm 和 1.34。分析其主要原因为仿真运动学模型中未考虑履带底盘的滑移、滑转以及定位系统的噪声等误差，同时执行机构存在滞后现象。根据现场试验结果，仿真获取的控制参数可实现较好的导航效果，实际上线距离为 1.44m，横偏均值为 0.41cm，且航偏均值为 1.81。第12期陈子文等：基于虚拟探照灯的离合制动履带底盘寻径跟踪15表2现场试验结果Table2Fieldtestresult试验号TestNo.路况Roadconditions速度Speed/(ms1)初始横向偏差Initiallateraldeviation/cm初始航向偏差Initialcoursedeviation/()k1/(radm)k2/(s1)上线距离On-linedistance/m横向偏差绝对值Absolutevalueoflateraldeviation/cm航向偏差绝对值Absolutevalueofcoursedeviation/()N均值Mean标准差Standarddeviation均方根Rootmeansquad均值Mean标准差Standarddeviation均方根Rootmeansquad1水泥路0.40.525.00.0056.01.440.410.270.591.810.441.56520.40.525.00.0056.01.480.420.330.521.600.661.81430.41.025.00.0056.01.760.550.280.681.670.491.49240.41.025.00.0056.01.610.420.370.521.520.551.51450.41.525.00.0056.01.830.430.300.521.380.651.55460.41.525.00.0056.01.730.410.330.511.460.531.56270.80.5250.0056.01.411.051.001.452.011.182.322380.80.5250.0105.01.520.750.570.941.050.781.30490.80.5250.0154.01.801.120.641.301.250.681.411101.20.5250.0105.01.431.391.0