垄作模式设施植保机器人设计与导航系统试验_冯昊栋.pdf

第3期冯昊栋 等 垄作模式设施植保机器人设计与导航系统试验7 1 D O I 1 0 1 3 7 3 3 j j c a m i s s n 2 0 9 5 5 5 5 3 2 0 2 5 0 3 0 1 2 冯昊栋 郑航 张毅 等 垄作模式设施植保机器人设计与导航系统试验 J 中国农机化学报 2 0 2 5 4 6 3 7 1 7 8 F e n g H a o d o n g Z h e n g H a n g Z h a n g Y i e t a l D e s i g n a n d n a v i g a t i o n s y s t e m e x p e r i m e n t a t i o n o f p l a n t p r o t e c t i o n r o b o t s i n r i d g e p l a n t i n g c o n d i t i o n s J J o u r n a l o f C h i n e s e A g r i c u l t u r a l M e c h a n i z a t i o n 2 0 2 5 4 6 3 7 1 7 8 垄作模式设施植保机器人设计与导航系统试验 冯昊栋1 郑航2 3 张毅4 薛向磊2 3 童俊华1 3 俞国红2 3 1 浙江理工大学机械工程学院 杭州市 3 1 0 0 1 8 2 浙江省农业科学院农业装备研究所 杭州市 3 1 0 0 2 1 3 农业农村部东南丘陵山地农业装备重点实验室 部省共建 杭州市 3 1 0 0 2 1 4 浙江钱塘机器人及智能装备研究有限公司 杭州市 3 1 0 0 0 2 摘要 针对设施垄作模式下空间结构狭小 环境密闭造成机器自主作业困难等问题 设计一种四轮独立驱动转向设施植保 机器人 开发基于U W B和I M U的组合导航控制系统 并对机器人进行室内垄间与换垄行走性能测试 首先根据设施垄 作模式特点 确定底盘垄间行走和换垄作业模式并设计相应的底盘结构部件 然后基于行走作业模式与底盘结构开发设 施植保作业机器人的导航控制系统 采用U W B定位技术与I M U模块进行组合导航 以机器人的位置信息和姿态信息作 为输入 在底盘运动学模型的基础上 采用纯跟踪算法实现导航控制 最后开展机器人室内导航实地测试 结果表明 在 行驶速度为0 5 m s 1 0 m s 1 5 m s时 该机器人的垄间直线行走最大横向偏差平均值分别为0 0 9 4 m 0 1 0 6 m 0 1 4 8 m 平均偏差分别为0 0 2 8 m 0 0 4 1 m 0 0 6 8 m 该机器人在室内具有较好的转向灵活性和导航行走精度 能够满 足设施垄作结构特点下的行走作业要求 为自主设施植保作业机器人的研制提供参考 关键词 设施温室 垄作模式 植保机器人 自主导航 中图分类号 S 6 2 5 T P 2 7 3 文献标识码 A 文章编号 2 0 9 5 5 5 5 3 2 0 2 5 0 3 0 0 7 1 0 8 收稿日期 2 0 2 3年9月2 9日 修回日期 2 0 2 3年1 1月1 3日 基金项目 浙江省 尖兵 研发技术攻关项目 2 0 2 3 C 0 2 0 1 2 浙江省 领雁 研发技术攻关项目 2 0 2 3 C 0 2 0 5 3 浙江省农业科学院成果推广项目 2 0 2 3 R 3 0 C B 0 0 1 第一作者 冯昊栋 男 1 9 9 8年生 河南鹤壁人 硕士研究生 研究方向为智能农机装备设计与制造 E m a i l Z h r o r y 1 2 6 c o m 通讯作者 俞国红 男 1 9 6 9年生 浙江萧山人 硕士 高级工程师 研究方向为农业机械化工程 E m a i l Y u g u o h o n g 1 6 3 c o m Design and navigation system experimentation of plant protection robots in ridge planting conditions F e n g H a o d o n g 1 Z h e n g H a n g 2 3 Z h a n g Y i 4 X u e X i a n g l e i 2 3 T o n g J u n h u a 1 3 Y u G u o h o n g 2 3 1 School of Mechanical Engineering Zhejiang Sci Tech University Hangzhou 3 1 0 0 1 8 China 2 Institute of Agricultural Equipment Zhejiang Academy of Agricultural Sciences Hangzhou 3 1 0 0 2 1 China 3 Key Laboratory of Agricultural Equipment for Hilly and Mountainous Areas in Southeastern China Co construction by Ministry and Province Ministry of Agriculture and Rural Affairs Hangzhou 3 1 0 0 2 1 China 4 Zhejiang Qiantang Robot and Intelligent Equipment Research Co Ltd Hangzhou 3 1 0 0 0 2 China Abstract T o s o l v e t h e p r o b l e m s o f a u t o n o m o u s o p e r a t i o n i n n a r r o w a n d e n c l o s e d s p a c e s u n d e r r i d g e p l a n t i n g c o n d i t i o n s a f o u r w h e e l i n d e p e n d e n t l y d r i v e n a n d s t e e r i n g p l a n t p r o t e c t i o n r o b o t i s d e s i g n e d a n d a c o m b i n e d n a v i g a t i o n c o n t r o l s y s t e m b a s e d o n U W B a n d I M U t e c h n o l o g i e s i s d e v e l o p e d T h e r o b o t s p e r f o r m a n c e i s t e s t e d f o r i n t e r r i d g e a n d r i d g e c h a n g i n g o p e r a t i o n s i n d o o r s F i r s t t h e c h a r a c t e r i s t i c s o f t h e r i d g e p l a n t i n g c o n d i t i o n s a r e a n a l y z e d t o d e f i n e t h e c h a s s i s i n t e r r i d g e w a l k i n g a n d r i d g e c h a n g i n g o p e r a t i o n m o d e s a n d c o r r e s p o n d i n g c h a s s i s s t r u c t u r a l c o m p o n e n t s a r e d e s i g n e d B a s e d o n t h e s e o p e r a t i o n m o d e s a n d t h e c h a s s i s s t r u c t u r e a n a v i g a t i o n c o n t r o l s y s t e m o f t h e p l a n t p r o t e c t i o n r o b o t i s d e v e l o p e d T h i s s y s t e m i n t e g r a t e s U W B p o s i t i o n i n g t e c h n o l o g y a n d a n I M U m o d u l e f o r c o m b i n e d n a v i g a t i o n u s i n g r o b o t s p o s i t i o n a l i n f o r m a t i o n a n d a t t i t u d e i n f o r m a t i o n a s t h e i n p u t s A p u r e t r a c k i n g a l g o r i t h m i s i m p l e m e n t e d o n t h e c h a s s i s k i n e m a t i c m o d e l t o a c h i e v e p r e c i s e n a v i g a t i o n c o n t r o l F i n a l l y i n d o o r n a v i g a t i o n t e s t s o f t h e r o b o t a r e c o n d u c t e d T h e r e s u l t s s h o w t h a t a t s p e e d s o f 0 5 m s 1 0 m s a n d 1 5 m s t h e m a x i m u m l a t e r a l d e v i a t i o n s o f t h e r o b o t s i n t e r r i d g e s t r a i g h t l i n e w a l k i n g a r e 0 0 9 4 m 0 1 0 6 m a n d 0 1 4 8 m a n d t h e a v e r a g e d e v i a t i o n s a r e 0 0 2 8 m 0 0 4 1 m a n d 第4 6卷 第3期 2 0 2 5年3月 中国农机化学报 J o u r n a l o f C h i n e s e A g r i c u l t u r a l M e c h a n i z a t i o n V o l 4 6 N o 3 M a r 2 0 2 5 7 2 中国农机化学报2 0 2 5年 0 0 6 8 m r e s p e c t i v e l y T h e r o b o t d e m o n s t r a t e s e x c e l l e n t s t e e r i n g f l e x i b i l i t y a n d n a v i g a t i o n a c c u r a c y i n d o o r s m e e t i n g t h e o p e r a t i o n a l r e q u i r e m e n t s o f r i d g e p l a n t i n g s t r u c t u r e s T h e f i n d i n g s p r o v i d e a v a l u a b l e r e f e r e n c e f o r t h e d e v e l o p m e n t o f a u t o n o m o u s p l a n t p r o t e c t i o n r o b o t s f o r f a c i l i t y b a s e d o p e r a t i o n s Keywords f a c i l i t y g r e e n h o u s e r i d g e p l a n t i n g p l a n t p r o t e c t i o n r o b o t a u t o n o m o u s n a v i g a t i o n 0 引言 设施农业是一种综合应用现代生物和信息技术的 高新技术产业 是可以实现农作物高产 优产的现代农 业模式 是未来农业的发展方向 1 相对大田种植 室 内空间封闭 高温高湿且作物种植密度大 更容易导致 作物病虫害的发生 目前温室植保基本采用人工通过 背负式手动 电动 喷雾器 人工喷洒随机性高 机具作 业效率低 机械化水平低 且密闭环境对植保工作人员 健康伤害大 2 植保作业是保证农作物高产 稳产的重 要环节 先进的植保装备和技术多处于试验阶段还未普 及 因此设施农业的高效率 高品质的优势无法充分体 现 温室垄间距狭窄且种植物密集 室内空间狭小 目 前市面上大多数传统的自走式喷杆植保机转向和掉头 所需空间大 操纵方便性较差 容易造成农作物的碾压 同时影响植保作业效率 无法直接应用在设施环境 自主导航是实现植保机器人在温室内无人化作业 的基础 3 温室内实现精准定位是植保机器人自主导 航的技术基础 4 在定位方面 全球定位系统 G P S 被广泛用于自动导航农机定位研究中 但大多 用于室外大田环境 设施温室遮挡物较多 G P S信号 严重衰减 定位精度低 5 常见的定位方法还有使用 视觉传感器或红外线传感器进行机器人定位 但是受 到光照条件影响较大 不适用于温室环境下的导航定 位 6 因此 研发和应用在温室环境下灵活性高的自 主植保作业机器人 对提高我国的设施作业机具自动 化程度和设施农业生产效率具有重要的现实意义 近年来 国内外针对温室智能植保作业技术与装 备开展了大量研究 管泽峰等 7 设计了电磁导式喷雾 机器人导航系统 利用磁导航传感器和测距传感器实 现温室移动平台的协同作业行走控制 前期需要铺设 诱导线及磁标等 应用受限且成本高 侯加林等 8 设计 了基于激光雷达的温室运输机器人 通过在底盘的头 尾部搭载二维激光雷达实现温室环境下的定位导航 但激光雷达在垄作环境下工作时 激光点云不能准确 扫描田垄的点云信息 从而难以保障植保机运行轨迹 的正确性 U W B技术具有较高的传输速率和时间分 辨率 其信号强度不受距离影响且功耗较低 视距通讯 下有着较高的定位精度 已被广泛应用于室内高精度 定位 9 虽然U W B定位技术能够实现温室环境内的 高精度定位 但受遮挡影响 惯性导航定位不受环境限 制 在温室环境下 U W B和I M U数据的有效组合可 为温室机器人作业过程提供稳定的高精度数据 本文针对设施温室空间狭小 植保机转向困难 温 室内定位精度差等问题 设计一种基于U W B I M U组 合导航的四轮独立转向的植保机器人 并通过纯跟踪 算法实现对预设轨迹的跟踪控制 以实现设施温室植 保环节的自动化 无人化作业 为设施农业机械化 智 能化发展提供技术与装备支撑 1 整机结构与工作原理 1 1 设施垄作模式结构特征 与露天田间环境不同 设施温室为半结构化的设施 农业环境 其由钢体框架搭建 顶部安装外遮阳系统 1 0 设施温室可被划分为道路区域和种植区域两部分 其中 道路区域为工作人员和运输设备通行的水泥硬化路面 路面宽度为1 8 m 种植区域为垄作模式 中间分布若干 金属立柱 设施温室环境如图1所示 图1 设施温室环境 F i g 1 G r e e n h o u s e f a c i l i t i e s e n v i r o n m e n t 设施温室内叶菜类种植的垄型多为宽平垄 如 图2所示 该垄型垄距为1 4 5 0 m m 垄沟宽3 0 0 m m 垄顶宽1 0 5 0 m m 垄高1 5 0 m m 植保机械作业过程 需要跨垄行走 目前设施蔬菜起垄主要依靠人工操作 机械起垄 垄沟直线度误差大 影响植保机械的垄间通 过性 同时温室内只有道路区域可进行掉头和换垄作 业 需要植保机具有较好的灵活转向性 图2 宽平垄结构 F i g 2 B r o a d b a n d r i d g e d s t r u c t u r e 1 2 整机结构 根据上述设施温室结构和转向要求 设计一款可全 方位行驶的植保作业机器人 该机器人主要由喷雾模 块 独立驱动 转向模块 导航模块以及控制模块组成 第3期冯昊栋 等 垄作模式设施植保机器人设计与导航系统试验7 3 总体结构如图3所示 其中喷雾模块由折叠喷杆 电推 杆 隔膜泵 喷头等组成 独立驱动 转向模块由车轮 转 向电机 蜗轮蜗杆减速器 角度传感器 驱动电机等组 成 如图4所示 导航模块由上位机 U W B模块 I M U传 感器 移动电源 安装支架等组成 控制模块由 S T M 3 2 F 4 0 7单片机 电机驱动器 继电器 串口显示器 等组成 其控制系统操作界面如图5所示 植保机有两 种控制模式 分别是遥控模式和自主导航模式 其中遥 控模式是通过遥控器发出信号 控制器接收信号后通过 控制电机驱动器和相应继电器远程实现植保机的前进 后退 转向 喷雾打开 关闭 喷杆折叠等功能 当植保 机到达指定位置时 通过切换操作界面启动自主模式 植保机开始跟踪预定轨迹 上位机实时处理偏差并将驱 动信号传输至底盘控制系统实现导航行走作业 图3 植保机结构示意图 F i g 3 S c h e m a t i c d i a g r a m o f p l a n t p r o t e c t i o n m a c h i n e s t r u c t u r e 1 独立驱动 转向模块 2 机架 3 喷杆结构 4 U W B模块 图4 独立驱动转向机构示意图 F i g 4 S c h e m a t i c d i a g r a m o f i n d e p e n d e n t s t e e r i n g d r i v i n g m e c h a n i s m 1 回转钢架 2 支撑钢架 3 支撑板 4 蜗轮蜗杆减速器 5 转向电机 6 驱动电机 7 驱动轮 图5 植保机控制系统操作界面 F i g 5 O p e r a t i o n i n t e r f a c e o f t h e c o n t r o l s y s t e m o f t h e p l a n t p r o t e c t i o n m a c h i n e 1 3 工作原理 设施植保机器人控制系统结构如图6所示 操作 人员通过遥控器控制植保机到达温室道路区域的指定 位置后 植保机切换到自主模式 然后上位机发出指 令 喷杆自动展开并喷雾 植保机开始跟踪预定轨迹 上位机通过U W B输入位置信息和I M U输入的航向 信息计算出横向偏差和航向偏差 通过纯跟踪算法计 算出车身角速度发送至底盘控制器 上位机与底盘控 制器通过C A N总线协议进行通讯 控制器根据接收到 的车身角速度通过运动学模型计算出四轮的转角和速 度 通过C A N接口与电机驱动器传输数据 实时调整 车轮转向 实现植保机沿作业轨迹导航行驶 S T M 3 2通过I O接口为喷雾模块提供控制信号 完成 喷雾启停控制 喷杆折叠控制 遥控模式下 遥控器信 号以R S 4 8 5通信方式发送遥控信号到底盘控制器 图6 控制系统结构框图 F i g 6 C o n t r o l s y s t e m s t r u c t u r e b l o c k d i a g r a m 设施植保机器人的主要技术参数如表1所示 表1 植保机主要技术参数 T a b 1 M a i n t e c h n i c a l p a r a m e t e r s o f p l a n t p r o t e c t i o n m a c h i n e 参数数值 整车尺寸 长 宽 高 m m m m m m 1 6 5 0 1 6 0 0 8 5 0 整机质量 k g 4 0 0 隔膜泵额定压力 M P a 1 0 2 0 药箱容量 L 1 5 最大爬坡度 3 0 工作电压 V 6 0 最大速度 m s 1 1 5 续航时间 h 5 2 温室调头 换垄工况与运动学模型 2 1 温室调头换垄工况分析与路径规划 当植保机在进入温室后需要在狭窄道路区域进行 调转车头动作和换行作业 设计的四轮独立转向植保 机可实现原地转向和平移换行作业 如图7所示 当 植保机行驶至位置点A时 切换到自主模式 上位机 打开串口 开始接收U W B定位数据和I M U数据 植 保机开始跟踪AB段轨迹 当到达B处时 控制器控 制植保机的各车轮转动至特定角度 以原地转向的方 式实现车头旋转9 0 令车头与喷雾作业路径平行 然 后转向轮回正 植保机继续沿着既定作业路径BC跟 踪 当植保机行驶至C点时 车轮反向行驶 植保机 开始回航 当植保机回航至B点时 控制器发出指 令 控制底盘的各轮相同方向转动9 0 以一种平行运 动的方式实现换垄作业 此时车头方向仍与路径BC 7 4 中国农机化学报2 0 2 5年 保持平行 当植保机移动到下一直线跟踪路径起点D 时 转向轮回正 同理 按上述控制方法完成剩余的既 定作业路径 当植保机回航行驶至点F点后再平移运 动到补给点处完成作业任务 植保机停止所有运动 图7 温室工况图 F i g 7 G r e e n h o u s e c o n d i t i o n s 综上 当植保机需要掉头运动时 如行至B点处 可以实现原地转向控制 当植保机需要换垄作业时 如行至D处 可实现平移换垄控制 原地转向和平移 换垄方式简单灵活 能缩短转向和换行时间 解决狭小 温室内需要较大转向空间的问题 有较强的适用性 植保机自主导航模式工作时 上位机发出控制信 号 植保机开始跟踪提前规划好的行驶路径 如图7所 示 行驶路径包括喷雾作业路径 回航路径和换行路 径 AB BD DF为预设换行路径 BC DE FG为喷 雾作业路径 CB ED GF IH为回航路径 植保机 在喷雾作业路径时 自动启动水泵并喷出药液 在横移 换垄路径和回航路径时 自动关闭水泵并停止喷药 图7中箭头方向表示植保机行驶方向 2 2 四轮转向植保机运动学模型 运动学模型是植保机运动控制的基础 1 5 设计 的植保机由4个直流电机实现驱动 由4个转向电机 提供转向力矩实现转向 在喷雾作业过程中 底盘能 按照上述所规划的路径行驶 误差小且轨迹平滑 每 个独立驱动模块的车轮转向角度和车轮的速度必须相 互协调 否则会导致车体变形 为满足以上要求 需要 建立底盘运动学模型 基于阿克曼原理的低速转向模型是农用底盘常用 的转向模式 1 2 为保持实时控制效果 使瞬心预设在 底盘中轴线的延长线上 简化后的低速转向模型如 图8所示 底盘低速向左绕旋转中心O 旋转 四轮转 向底盘的运动分解关系推导如式 1 式 4 所示 1 3 a r c t a nL2R B 1 2 4 a r c t a nL2R B 2 v1 v2 R B2 2 L 2 4 3 v3 v4 R B2 2 L 2 4 4 式中 L 底盘的轴距 B 底盘的轮距 vi 4个车轮的速度 i 1 2 3 4 i 4个车轮的转角 i 1 2 3 4 R 绕瞬心O 旋转半径 底盘角速度 当植保机底盘进行原地转向运动 图9 时 底盘 形心即为旋转中心 各轮的旋转半径和速度相同 且每 个车轮的速度与竖直方向的夹角相同 并根据底盘的 轮距和轴距 可由三角函数得出 即为O 图8 简化阿克曼低速转向模型 F i g 8 L o w s p e e d A c k e r m a n s t e e r i n g m o d e l 图9 原地转向运动学模型 F i g 9 I n s i t u s t e e r i n g k i n e m a t i c m o d e l t a n BL 5 由式 5 可得 a r c t a nBL 0 7 3 6 r a d 6 当斜行 图1 0 时各轮的转速相同 偏转角相同且 不为0 即 v1 v2 v3 v4 7 1 2 3 4 8 当横移换垄运动时 偏转角转至9 0 即 1 2 3 4 9 0 9 图10 斜行运动学模型 F i g 1 0 O b l i q u e k i n e m a t i c s m o d e l 第3期冯昊栋 等 垄作模式设施植保机器人设计与导航系统试验7 5 3 导航控制策略 3 1 导航功能实现原理 植保机导航系统组成如图1 1所示 包括上位机 U W B定位模块 I M U传感器 角度传感器和四轮转向 植保机 图11 植保机及导航系统组成 F i g 1 1 P l a n t p r o t e c t i o n a i r c r a f t a n d n a v i g a t i o n s y s t e m c o m p o n e n t s U W B定位模块包括1个U W B定位标签 4个 U W B定位基站 移动电源和基站安装支架 其中 U W B定位基站通过安装支架固定于定位区域边缘作 为定位参考点 U W B定位标签为1个U W B无线传 感器 安装于植保机车身顶部对称轴线上并通过串口 与上位机通讯 U W B标签通过与定位基站进行无线 通信 获得与各定位基站间的距离信息 植保机机身 上安装I M U传感器并通过串口与上位机通讯 用于实 时测量车身的航向信息 首先确定预设轨迹 将该轨迹以二维数组的形式 存储到上位机中 和上位机相连的U W B标签实时接 收与定位基站的距离信息 据此可计算出植保机当前 的位置坐标 再由前视距离来获得轨迹跟踪的目标点 通过分析I M U采集到的当前姿态信息得出航向信息 从而确定目标方向 通过纯跟踪算法计算出植保机角 速度并发送至底盘控制器 控制器根据运动学模型计 算出四轮的转角和速度 实现植保机导航 图1 2 图12 导航控制原理 F i g 1 2 N a v i g a t i o n c o n t r o l s c h e m a t i c 3 2 纯跟踪模型 目前农业机械导航控制方法为P I D控制 1 3 模糊 控制 1 4 模糊P I D控制 1 5 纯跟踪控制 1 6 和最优控 制 1 7 等 P I D控制可靠性高 但控制参数整定繁琐复 杂 模糊控制需要根据专家经验制定其控制规则 追 踪误差一般较大 难以快速修正 基于神经网络的最 优控制非常依赖训练样本 需要大量训练样本才能保 证路径追踪效果 设施内空间狭小 障碍物多 在提前规划好底盘行 驶轨迹的同时需要控制算法具有响应速度快 鲁棒性 好的优点 植保机室内作业轨迹多为直线轨迹 路径 较单一 选用简单的控制算法就能满足设施内导航要 求 并节省上位机程序处理的时间 纯跟踪算法主要 是通过对底盘进行运动学建模建立直观的几何模型进 行公式推导 只需要调整参数找到最合适的前视距离 就可以达到良好的跟踪效果 因此采用纯跟踪控制来 解决路径跟踪问题 纯跟踪模型如图1 3所示 AB线为跟踪路径 底盘 质点为C点 过C点作AB垂线 垂足为D 从D点沿 AB线一定距离为预瞄点P 过C点作Y轴的垂线 并 与CP的中垂线相交于点O 即为期望转向中心 图13 纯跟踪模型 F i g 1 3 P u r e t r a c k i n g m o d e l CP 转向半径和圆心角分别为R和 则有几 何关系 2 1 0 Ld s i n R s i n 2 1 1 整理得 R Ld2 s i n 1 2 Ld xP xC 2 yP yC 2 1 3 式中 xP yP 预瞄点P在U W B坐标系下的坐 标 xC yC 植保机质心C点的坐标 可通过 U W B测距值计算出具体位置 由图1 3可以推导出角速度的表达式为 2LX vL d 2 1 4 式中 质点处的角速度 v 控制点处的线速度 Ld 前视距离 LX 预瞄点在坐标系下的X轴向距离 结合推导出的运动分解公式 可以将质点处的线 速度和角速度分解为4个车轮的转速 转角控制量 横向偏差和航向偏差都是在U W B和I M U信息解 析计算得到的数值 由图1 3可知 前视距离Ld为直角 7 6 中国农机化学报2 0 2 5年 CDP的斜边CP 其中直角边DC为横向偏差d DP 为重要的控制模型参数 经实际测试取定值1 5 m 3 3 位置解算算法 将U W B定位基站安装在同一高度 采用T W T O F双向飞行时间法测量U W B定位标签与U W B定 位基站间的距离 1 8 根据标签与各定位基站的距离以 及各基站的位置坐标 得到平面4点的定位方程组 如 式 1 5 所示 x x0 2 y y0 2 d0 2 x x1 2 y y1 2 d1 2 x x2 2 y y2 2 d2 2 x x3 2 y y3 2 d3 2 1 5 式中 xi yi 基站坐标 i 1 2 3 x y 标签坐标 d0 d1 d2 d3 标签到基站Ai的测距值 对式 1 5 简化可得到关于标签坐标 x y 的线 性方程组 其矩阵表示形式为 Ax B 1 6 其中x xi yi T 式中 x 标签位置坐标 其中矩阵A和矩阵B具体形式如式 1 7 和 式 1 8 所示 A 2 x1 x0 2 y1 y0 2 x2 x1 2 y2 y1 2 x3 x2 2 y3 y2 1 7 B x1 2 x0 2 y1 2 y0 2 d1 2 d0 2 x2 2 x1 2 y2 2 y1 2 d2 2 d1 2 x3 2 x2 2 y3 2 y2 2 d3 2 d2 2 1 8 使用最小二乘法求解式 1 9 可得到基于当前 U W B标签的二位坐标计算值x x ATA 1ATB 1 9 3 4 卡尔曼滤波预处理U W B测距值 设施温室内存在若干金属支柱 植保机移动过程 中 安装在机身上的U W B标签与安装在温室内的 U W B定位基站之间由于金属立柱的遮挡会产生非视 距误差 同时由于传感器自身属性会产生随机系统误 差 所以植保机在温室作业过程中获得的U W B测距 数据需要进行预处理 卡尔曼滤波器用于预处理测距值 并抑制导致 U W B距离值突变的随机系统误差和一些非视距距离 误差 使用t 1和t的测距值的差值作为测量向量 将x1 0设置为初始状态 并建立离散状态模型用于 测距值滤波 xt Azxt 1 Wt zt Hxt Vt 2 0 式中 Az 系统状态转移矩阵 Az 1 dt 0 1 H 系统测量矩阵 并且H 1 0 Wt Vt 当前的过程噪声和测量噪声 Wt N 0 Qt Vt N 0 Rt 基于离散状态模型的先验估计产生预测状态x t 和预测协方差P t x t Azxt 1 2 1 P t AzPt 1AzT Qt 2 2 式中 xt 1 瞬时t 1最佳状态估计 Qt 瞬时t过程噪声的协方差矩阵 卡尔曼增益Kt从系统中获得 如式 2 3 所示 Kt P tH HP tHT Rt 1 2 3 式 2 3 表明 预测协方差P t与卡尔曼增益Kt呈 正相关 观测噪声Rt与卡尔曼增益Kt呈负相关 因 此 当测量矢量误差增大时 当前时刻的卡尔曼增益 Kt将减小 更新状态x t和协方差Pt产生结果如 式 2 4 和式 2 5 所示 x x t 1 Kt zt H x t x t 1 2 4 Pt 1 H Kt P t 2 5 x t表示当前状态下的最优估计 并且x t和Pt将 在时刻t 1参与先验估计 直到滤波结束 4 试验与结果分析 4 1 自动导航作业试验 为检验植保机的自动导航作业系统的稳定性 在 浙江省农业科学院杨渡试验基地进行试验 试验材 料 设施植保机 上位机 U W B定位模块 I M U传感 器等 温室内的试验场地为四边形垄田区域 尺寸为 2 0 m 2 0 m U W B定位基站位于试验场地4个角 落 U W B基站安装高度为2 m 建立导航坐标系 各 基站坐标分别为A0 0 0 A1 2 0 0 A2 0 2 0 A3 2 0 2 0 单位为m U W B定位标签安装在植保机 顶部对称轴线上 试验前进行上位机 传感器 控制电 源等各控制系统部件的安装和检查 设定喷雾路径 回 航路径和换行路径 植保机分别按照0 5 m s 1 0 m s 1 5 m s的速度行驶 采样频率为1 0 H z 植 保机出发的初始位置为A0 5 5 初始横向 航向偏差 均为0 沿指定方向行驶 同一初始状态的试验重复 3次 各数据求平均值 上位机记录实际跟踪轨迹并 和规划轨迹进行比较 计算横向偏差 植保机实际规 第3期冯昊栋 等 垄作模式设施植保机器人设计与导航系统试验7 7 划轨迹如图1 4所示 图14 植保机导航试验路径 F i g 1 4 P i l o t p a t h f o r p l a n t p r o t e c t i o n a i r c r a f t n a v i g a t i o n 4 2 试验结果分析 对导航横向偏差的平均值和最大值进行统计分 析 最大偏差是在整个试验过程中横向偏差最大值 平均偏差是从试验开始到结束的横向偏差均值 植保机可在喷雾路径下自动打开喷雾功能 在换 行路径处实现平移换行控制 在回航路径下植保机自 动关闭喷雾功能 不同速度下实际轨迹与规划轨迹的 横向偏差统计结果如表2所示 植保机分别以 0 5 m s 1 0 m s和1 5 m s的速度跟踪规划轨迹 最 大横向偏差平均值分别为0 0 9 4 m 0 1 0 6 m 0 1 4 8 m 平均横向偏差分别为0 0 2 8 m 0 0 4 1 m 0 0 6 8 m 在不同速度下 路径跟踪精度基本满足温室 植保作业要求 表2 横向偏差试验结果 T a b 2 T e s t r e s u l t s o f l a t e r a l d e v i a t i o n 速度 m s 1 试验序号最大偏差 m平均偏差 m 0 5 1 0 0 9 3 0 0 2 6 2 0 1 0 1 0 0 3 5 3 0 0 8 9 0 0 2 3 平均值0 0 9 4 0 0 2 8 1 0 1 0 1 0 9 0 0 3 5 2 0 0 9 6 0 0 2 9 3 0 1 1 3 0 0 4 8 平均值0 1 0 6 0 0 4 1 1 5 1 0 1 5 2 0 0 7 4 2 0 1 4 8 0 0 6 7 3 0 1 4 5 0 0 6 3 平均值0 1 4 8 0 0 6 8 从图1 5可以看出 植保机在前期实际作业行驶 时 横向偏差有所增大 主要原因 植保机底盘设施内 在作业时 由于道路区域和垄作区域之间有一定的高 度差 导致车身产生倾斜或滑移 植保机受到路面平 整度的影响 导致I M U传感器获取车体位姿信息产生 突变 影响导航性能 虽然颠簸地面不平对传感器的 定位精度造成一定影响 但是试验结果展现的导航精 度和稳定性对于植保作业是可以接受的 在不同速度 下的导航精度基本满足温室植保作业要求 后期将持 续改进控制算法 不断降低相关因素对导航控制的影 响 持续提高导航的控制精度 以适应更加复杂温室结 构环境的自主移动要求和机械化作业需求 图15 导航横向偏差 F i g 1 5 N a v i g a t i o n l a t e r a l d e v i a t i o n 5 结论 1 针对