基于ROS的智能割草机器人导航控制系统设计

王 全, 陈学海,2, 干 熊,2, 杨光友,2

(1 湖北工业大学农业机械工程研究设计院, 湖北 武汉 430068;2 湖北省农机装备智能化工程技术研究中心, 湖北 武汉 430068)

田间、果园和滩涂上的杂草会直接影响到农业生产,导致粮食减产和经济损失[1]。目前国内外的除草方法主要分为化学除草和非化学除草两类[2],化学除草的主要弊端在于化学药剂会残留在土壤内,会对自然环境和人体健康造成危害。非化学除草主要是用机械设备进行除草[3],而传统除草机械由于智能化程度不足,除草作业的全程都需要人工参与,导致其劳动强度大、作业效率较低。为了提高作业效率和降低人工成本,需要开发智能割草机器人,而导航控制系统是智能割草机器人的关键环节。

在导航控制系统方面,罗锡文等[4-6]在二自由度车辆转向模型的基础上,建立了基于GPS的导航控制系统,可以有效地追踪目标轨迹。熊斌等[7]采用气动转向系统,将果园施药机的运动学模型和纯追踪模型相结合,建立了基于北斗的自主导航系统,其作业精度可以满足果园作业精度要求。白晓鸽等[8]针对数学模型复杂的拖拉机运动学问题,采用基于神经网络的控制方法,设计了车辆控制的神经网络控制器,实验结果表明该控制器对拖拉机的运动有较好的控制效果。Peichen Huang等[9]研究了固定前视距离的PI路径跟踪算法并开展了实验,实验结果表明可以获取车辆的位置和航向误差。王辉等[10]采用预瞄准纯路径追踪模型作为路径跟踪方法,可以有效地提高车辆在复杂路面的追踪精度。Ahn Joonwoo[11-12]等在预瞄准纯路径追踪模型的基础上采用动态前视距离纯路径追踪模型的方法作为路径跟踪算法,可以进一步提高车辆在复杂路面的追踪精度。王玉亮等[13]为解决水稻插秧机无人驾驶的问题,采用RTK定位技术,以MPC算法作为路径追踪算法,能够在车速1m/s时,有效跟踪目标路线。Shuo Cheng等[14]针对在自动驾驶过程中车辆参数的不确定性,设计了基于MPC的车辆路径跟踪控制器,实验结果表明该方法可以有效追踪目标路径。

由于实际割草作业环境复杂且多变,导致上述导航控制系统不能直接迁移到智能割草机器人上。为了实现割草机智能化水平的提高,本文设计了差速驱动底盘的割草机器人导航控制系统,实现了底盘驱动、GNSS数据采集、IMU数据采集等功能。针对系统的定位精度和定位频率较低的问题,采用扩展卡尔曼滤波算法将GNSS和IMU的定位信息进行融合,提高了智能割草机器人的定位精度和定位频率。同时,采用基于预瞄准的纯路径追踪算法,实现了智能割草机器人对目标轨迹的自主追踪。

1 导航控制系统设计

割草机器人平台采用husky的差速底盘机器人,该平台采用4轮驱动,在其左右两侧沿对角线各布置一台电机,同一侧轮子采用同步带驱动如图1所示。相较于三轮模型该平台稳定性更好,且能够原地转向,适合在狭小空间作业。

图1 差速底盘机器人

1.1 系统总体框架

在机器人操作系统ROS下,根据经典的模块化分层思想构建割草机器人导航控制系统。其整体框架如图2所示。

图2 系统总体框架

如图2所示,系统共分为三层:控制层、数据处理层以及硬件层。控制层分布在远程PC端。PC端通过WiFi与树莓派连接,具有割草机器人的位置信息实时显示,割草机器人远程控制以及作业轨迹发布等功能。数据处理层分布在上位机ROS和下位机RT-Thread上,通过WiFi与控制层通信。数据处理层中ROS中具有控制算法和传感器数据处理等节点,并发布电机控制指令。数据处理层中的RT-Thread主要负责接受遥控器的指令或是来自ROS的控制指令,实现对电机控制。

1.2 导航控制系统上位机设计

导航控制系统以树莓派4B为上位机,安装Ubuntu16.04系统,运行ROS,其总体框架如图3所示。

图3 上位机软件框架

ROS系统中主要有5个节点,分别为轨迹追踪节点、底盘驱动节点、GNSS数据采集节点、IMU数据采集节点、卡尔曼滤波节点。其中:1)轨迹追踪节点:主要通过轨迹追踪算法计算车辆转向角速度w和车辆行驶速度v,并发布到话题/cmd_msg上;2)底盘驱动节点:通过订阅话题/cmd_msg中的指令来完成机器人的行驶和转向控制;3)GNSS数据采集节点:首先将GNSS的原始数据转化到地心大地坐标系下及转化成经纬度,在将经纬度转化到地心空间直角坐标系下及转化成x,y,z,最后将地心空间直角坐标系转化为导航坐标系及东北天坐标系并将坐标信息发布到话题/GNSS_msg上;4)IMU数据采集节点:将原始数据解析为三轴加速度和三轴角速度,并通过DR算法得到基于机体坐标系的坐标,最后将机体坐标系下的坐标转化为导航坐标系下的坐标及东北天坐标系下的坐标并发布到话题/IMU_msg上;5)卡尔曼滤波节点:订阅话题/GNSS_msg、/IMU_msg中的坐标信息并进行融合,以得到最优估计坐标。

1.3 导航控制系统下位机设计

导航控制系统以STM32F103C8T6单片机为下位机,采用嵌入式实时操作系统RT-Thread作为运行环境,其总体框架如图4所示。

图4 下位机软件设计

下位机主要负责接收上位机控制指令以及接收遥控器指令。分为三个线程:分别为遥控线程、电机线程和ROS通讯线程。遥控线程主要负责接收遥控器信号并转换成相应的控制指令。电机线程通过PID算法来完成电机的控制指令。ROS通讯线程通过rosserial把单片机做为ROS下的一个节点,以实现通过话题的方式与上位机的ROS系统通信,并订阅话题/cmd_vel中的电机控制指令。

2 导航控制方法研究

2.1 差速转向模型

假设前进速度不变、忽略车辆离心力和侧滑,建立以割草机器人中心为控制点的运动学模型;依据模型可以确定左右两轮的转速与转向半径的关系以及割草机器人相对于起始坐标系运动的运动学方程。如图5所示为差速转向机器人运动学模型。

图5 差速转向运动学模型

割草机器人在笛卡尔坐标系下的坐标为(x0,y0),航向角为θ。割草机器人的运动学模型为:

(1)

(2)

(3)

可得两轮线速度与转弯半径的关系为:

(4)

式中,(x0,y0)为割草机器人质心坐标,m;v1,v2为割草机器人左右轮线速度,m/s;R为割草机器人转向半径,m。

实际作业中由于轮胎侧滑、地面不平坦等因素会对车轮的实际速度产生较大影响,从而降低转向的准确度,因此对车轮速度采用PID控制,以确保转向的准确度。

2.2 预瞄准纯路径追踪模型

建立割草机器人的预瞄准纯追踪模型几何学示意图如图6所示。

图6 预瞄准纯追踪模型原理

建立割草机器人相对坐标系,割草机器人中心点p为割草机器人当前位置,O1为割草机器人转向时的瞬时圆心;R为瞬时转向半径;选取目标轨迹上的预瞄准点G(x1,y1)为当前目标位置,割草机器人中心点p到预瞄准点G的距离ld为前视距离;pG对应的弧长为割草机器人到达目标位置所需要行驶的圆弧,根据圆弧可以计算出转向半径,在根据转向半径即可计算出左右两轮的理论转速;在下个采样周期根据新的当前位置信息和目标位置信息计算出新的左右轮转速,如此反复即可完成轨迹追踪。

依据图中几何关系可得:

(5)

e=ldsin(θ-yaw)

(6)

(7)

结合式(4)-(7)可得:

(8)

式中,(x0,y0)为割草机器人质心坐标,m;(x1,y1)标点坐标,m;v1,v2为割草机器人左右轮线速度,m/s;l-人左右轮间距,m;ld为前视距离,m;R为割草机器人转向半径,m;θ为pG与x轴夹角,(°);yaw为初始航向角,(°);e为横向误差,m。

2.3 扩展卡尔曼滤波融合GNSS/IMU定位数据

全球导航卫星系统GNSS具有较高的定位精度,更新频率一般为10 Hz,因其每次的定位信息不依赖于上次的值,所以不存在累计误差。惯性传感器IMU,对其输出的角速度和加速度进行积分可以得到相较于起点的位置和姿态信息,但由于每次的定位信息依赖于上次的值,所以存在累计误差,短时间内累计误差较小定位精度很高,但长时间累计误差较大导致定位精度很低。IMU更新频率很快,可达100 Hz以上,为此可以将IMU和GNSS定位系统结合起来,利用GNSS较高的定位精度来修正IMU的累计误差。再结合IMU短时间内精度较高和频率快的特点即可得到高精度高频率的导航信息。通过传感器信息融合技术卡尔曼滤波即可实现GNSS/IMU的定位信息融合,如图7所示。采用GNSS定位结果辅助IMU系统的初始化,给出基于GNSS精度的初始位置。在稳定运行过程中可以通过GNSS的位置信息动态修正IMU的累计误差;另一方面,当GNSS无法定位或定位误差太大时,便切换至IMU系统,短时间内修正GNSS定位信息,当GNSS定位恢复正常后,再用GNSS定位信息对IMU经行校正,如此即可实现高精度高频率的导航。

图7 GNSS/IMU融合流程

2.3.1GNSS/IMU组合系统状态方程的建立GNSS/IMU组合导航系统状态变量为X=[xe,ve,ae,xn,vn,an,w],可以得到组合系统的状态方程:

(9)

2.3.2GNSS/IMU组合系统观测方程的建立系统的观测量包括GNSS接收机输出的车辆东向位置信息xe,北向位置信息xn和惯性传感器IMU输出的z角速度w,东向加速度ae和北向加速度an。可得系统离散的观测方程为:

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

3 机器人导航控制实验与结果分析

为验证智能割草机器人导航系统工作效果,以husky为实验平台在湖北省农机鉴定站试验场地对智能割草机器人进行路径跟踪实验,测试现场如图8所示。

图8 导航系统实验

在试验地块上选取机器人起点为图9a中的点T(-0.1 m, 0),终点为图9a中的点F(4.5 m, 20.0 m)。规划路径的起点为图9a中的点A(0.5 m, 0),中间点依次为点B(0.5 m, 20 m)、C(2.5 m, 20 m)、D(2.5 m, 0)、E(4.5 m, 0),终点为F点。将点A、B、C、D、E、F确定的5条直线作为机器人跟踪的目标路径,如图9a中的黑实线所示。

(a) 轨迹追踪

实验方法:设置割草机器人速度恒定为1 m/s,前视距离恒定为0.4 m,采样周期为0.01 s。在点T(-0.1 m, 0)启动机器人,割草机器人自动追踪图9a中所规划的路径,到达F点后结束路径跟踪实验。

割草机器人轨迹追踪如图9a红色点所示,其横向误差如图9b所示。由图9b可知,横向误差最大为0.35 m,平均横向误差为0.11 m。如图9a所示,在进行直线①②③④⑤,5条直线追踪时误差较小,在每个直角转弯处则出现最大误差。在固定前视距离的情况下,割草机器人进行直线追踪时不会出现较大的超调。但在进行直角转弯时,由于前视距离的设置为定值,导致控制系统算法不能很好地适应当前状态,进而对割草机器人直线追踪精度产生较大影响。

4 结论

本文设计了一种智能割草机器人的导航控制系统。基于机器人操作系统ROS,实现了底盘驱动、GNSS数据采集、IMU数据采集等功能。通过扩展卡尔曼滤波器实现了GNSS/IMU的组合定位,有效地提高了割草机器人定位精度与定位频率。建立了基于差速转向模型的预瞄准纯路径追踪运动学模型,基于husky机器人平台对算法进行了测试实验,实验验证了导航控制系统的有效性。下一步将深入研究提高路径追踪精度的算法,同时加入避障功能模块,进一步提高割草机器人的智能化程度。