一种基于ROS的无人驾驶农机路径跟踪系统的制作方法

本发明涉及一种农用机械控制方法及装置，，尤其是涉及一种用于无人驾驶农机的路径跟踪控制方法及控制装置，属于智能农机技术领域。

背景技术：

农业机械自动导航是智能化农机的重要内容，也是降低人力成本、提高土地利用率的有效手段。因为我国农业产业模式和结构的转型使得农田出现集中经营的趋势，大面积的农田数量日益增多。传统的农田作业方式不仅效率低，且生产成本较高，无法适应农业现代化的发展。大面积成片农田的增加，为大型农业机械的应用提供了有利条件。

北斗卫星系统由我国自主开发，已经广泛地用于农业机械的自动导航，体现出了巨大的经济和社会价值。为此，基于北斗卫星开发了一种智能农机自动导航系统，通过基站的差分数据实现定位，同时向行车控制器发出指令，控制农机按照设定的路线行驶。

技术实现要素：

本发明的目的是为了克服现有控制系统结构复杂、不易进行容错控制和诊断的弊端，而设计的一种用于无人驾驶农机路径跟踪集成控制的方法和控制装置。

为了达到上述目的，本发明提供的基于ros的无人驾驶农机路径跟踪系统，包括感知监测层、决策控制层、底层执行层，

所述感知监测层，其采集车辆的位置及姿态信息以及车辆自身运动参数；

所述决策控制层，其接收车辆感知信息同时依据设定期望路径进行车辆控制参数的运算；

所述底层执行层，其接收控制输出结果并直接作用于车辆，使车辆沿期望路径行驶。

进一步地，所述感知监测层，包括gnss/ins组合导航子系统、前轮转角监测子系统、轮速监测子系统；

所述决策控制层，包括由工控机构成的硬件子系统、由ros控制系统及系统所有节点程序构成的软件子系统、ebs紧急制动子系统；

所述底层执行层，包括由dspace控制器、单片机、转向及制动执行机构构成的硬件子系统、由simulink和嵌入式系统构成的软件子系统。

进一步地，所述车辆自身运动参数，包括前轮转角和车轮转速；

所述车辆控制参数，包括转向机构期望输出角、期望制动压力、期望油门开度；

所述相关车辆，包括智能农业作业机械在内的一般无人驾驶车辆。

进一步地，所述ros控制系统，由gnss/ins、路径跟踪控制器、用户界面、控制中心、前轮转角、轮速等节点程序和相应话题组成。

所述ebs紧急制动系统，包括车载急停装置和遥控急停装置。

为了达到上述目的，本发明提供的基于ros的无人驾驶农机路径跟踪系统的运行方法，包括以下步骤：

1）车辆运行前准备工作，通过用户界面进行相应的需求设置和期望目标设定；

2）通过感知检测层的传感器，收集车辆位置信息、姿态信息及运动参数信息；

3）接收感知层数据，由决策控制系统进行相应控制参数的计算，并输出到底层执行层；

4）根据决策层控制参数，控制底层执行机构进行相应的加减速、转向及制动等操作。

进一步地，所述步骤1），包括以下步骤：

a.通过用户控制界面，用户设置期望路径上的各个目标，系统将这些目标连接成线形成期望路径；此外，用户还可以设置农机的预期车速和其他配置；

b.经用户确认设置后，预期路径等信息会发送给ros控制系统；

c.用户在用户控制界面上，按下开始按钮，农机开始行驶。

进一步地，所述步骤2），包括以下步骤：

d.gnss/ins系统接收卫星信号和惯性导航信号，得到农机目前的位置和姿态信息，将这些信息发送给ros控制系统；

e.前轮转角传感器获得农机左前轮转角，并将信息发送给ros控制系统；

f.轮速传感器获得农机的左前轮转速和右前轮转速，并将信息发送给ros控制系统。

进一步地，所述步骤3），包括以下步骤：

g.ros控制系统根据步骤a的预期路径和步骤d获得的实际路径，计算出农机路径跟踪所需要的期望前轮转角；

h.ros控制系统根据步骤g获得的期望前轮转角信息和步骤e获得的实际前轮转角，计算出期望前轮转角和实际转角的误差，使用pid控制器根据该误差，计算出发送给执行层的前轮转角增量；

i.ros系统根据步骤a获得的左前轮转速和右前轮转速，根据前轮转速和车速的近似关系式，计算农机的当前车速和根据步骤a获得的期望车速，计算出期望车速和当前车速的误差，使用pid控制器根据该误差，计算出发送给执行层的车速增量。

进一步地，所述步骤4），包括以下步骤：

j.执行层根据步骤h获得的前轮转角增量，根据前轮转角增量和方向盘转角增量的关系式，计算出所需的方向盘转角，利用转向电机驱动方向盘转到所需的角度；

k.执行层根据步骤i获得的车速增量，判断加速还是减速，根据油门和车速增加量的关系式，和刹车与车速减少量的关系式，计算出油门的开度或刹车的开度；

l.重复步骤d到步骤k，直至农机走完预期路径。

本发明的有益效果是：

本发明所述的一种基于ros的无人驾驶农机路径跟踪系统，包含感知监测层、决策控制层、底层执行层，对期望路径结合车辆动力学进行解算，控制相应执行机构，从而实现农机的路径跟踪。整个系统实现了无人驾驶农机的通用集成化控制，鲁棒性强且稳定，便于后续维护和升级，具有较强的实用性和广阔的应用前景。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。

附图说明

图1为本发明的系统架构图；

图2为根据本发明的ros决策控制系统原理框图；

图3为根据本发明的路径跟踪控制器原理流程图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

图1为根据本发明的基于ros的无人驾驶农机路径跟踪系统架构图，如图1所示，本发明所述的基于ros的无人驾驶农机路径跟踪系统，该系统包括：感知系统及装置、路径跟踪控制算法和控制执行装置，其中感知系统及装置，主要由gnss/ins系统，前轮传感器及轮速传感器等组成；路径跟踪控制算法，通过ros控制系统接收各种传感器的感知信号，结合车辆动力学模型，进行车辆控制参数的计算；控制执行装置，其接收到控制算法的输出结果，即控制电机的期望转角及转速等实际物理量，直接作用于车辆的油门转向及制动等机构。

所述感知监测层，包括gnss/ins组合导航子系统、前轮转角监测子系统、轮速监测子系统；

所述决策控制层，包括由工控机构成的硬件子系统、由ros控制系统及系统所有节点程序构成的软件子系统、ebs紧急制动子系统；

所述底层执行层，包括由dspace控制器、单片机、转向及制动执行机构构成的硬件子系统、由simulink和嵌入式系统构成的软件子系统。

图2为根据本发明的ros决策控制系统原理框图，如图2所示。本发明的ros决策控制系统，包括gnss/ins节点101、gnss/ins处理节点102、用户界面节点103、路径控制器节点104、控制中心节点105、前轮转角节点106、轮速节点107。其中，

gnss/ins节点101负责将gnss/ins原始的16进制数据转换为系统算法所需的基本数据类型，浮点型和双精度型。

gnss/ins处理节点102负责提取gnss/ins数据中的经纬度和航向角信息。

用户界面节点103负责接收用户输入的预期路径、车速和配置信息，并将预期路径发送给路径跟踪控制器，将车速和配置信息发送给控制中心节点105。

路径控制器节点104负责接收来自gnss/ins处理节点102发送来的经纬度信息和用户界面节点发送来的预期路径信息，处理接收的信息并计算出期望的农机前轮转角，将前轮转角信息发送给控制中心节点105。

控制中心节点105接收路径跟踪控制器节点发送的前轮转角，前轮转角节点106发送的农机实际前轮转角，用户界面发送的期望车速和轮速节点107发送的农机实际轮速，协调这些信息并计算出发送给执行层的前轮转角和车速信息，最终将这些信息发送给执行层。

图3为根据本发明的路径跟踪控制器原理流程图，如图3所示。路径跟踪控制器是基于gnss/ins惯性组合导航系统的。

控制器通过比较预期路径的经纬度和由组合导航得到的实际经纬度计算出位置偏差，再通过比较预期路径的航向角和实际航向角得到航向角偏差。

系统使用模糊自适应pid处理位置偏差和航向角偏差得出一个预设前轮转角。但预设前轮转角只能在较为理想的环境下，才能得到理想的效果。所以为了应对农机恶劣的车况，松软的路面以及振动等不利因素，需要在负反馈的基础上加上前馈控制，以尽量抵消这些不利因素带来的影响。

预设前轮转角和前馈补偿相结合将得出最终的前轮转角。

本发明所述的一种基于ros的无人驾驶农机路径跟踪系统，包含感知监测层、决策控制层、底层执行层，对期望路径结合车辆动力学进行解算，控制相应执行机构，从而实现农机的路径跟踪。

本领域的普通技术人员可以理解：以上所述仅为本发明的优选实例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。