(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210798209.1 (22)申请日 2022.07.08 (71)申请人 江苏大学 地址 212013 江苏省镇江市京口区学府路 301号 (72)发明人 陈进　程凯　魏晓波　朱富豪　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 张明明 (51)Int.Cl. A01D 41/127(2006.01) A01B 69/00(2006.01) G06T 7/73(2017.01) G06V 10/25(2022.01) G06V 10/46(2022.01)G01C 21/00(2006.01) G01C 21/20(2006.01) (54)发明名称 一种基于田间视觉SLAM的联合收获机转向 控制方法 (57)摘要 本发明提供了一种基于田间视觉SLAM的联 合收获机转向控制方法， 工控机基于双目相机采 集的图像， 根据导航基准线将田间图像划分为已 收割区域和未收割区域， 前端视觉里程计对田间 割茬丛和作物信息分析进行针对性的特征提取， 估算出联合收获机的位姿， 基于Ceres的后端优 化对接收不同时刻联合收获机的位姿进行优化， 得到联合收获机优化后的位姿； 由联合收获机优 化后的位姿与导航基准线的偏 差， 得到联合收获 机转向调整角度， 实现对联合收获机的转向控 制。 本发明使 图像更容易被工控机分辨， 对于联 合收获机机身位姿判断更加准确， 进而提高联合 收获机的转向控制的精度。 权利要求书2页 说明书6页 附图4页 CN 115280960 A 2022.11.04 CN 115280960 A 1.一种基于田间视 觉SLAM的联合收获机转向控制方法， 其特 征在于： 工控机基于双目相机采集的图像， 由前端视觉里程计估算出联合收获机的位姿， 基于 Ceres的后端优化对接收不同时刻联合收获机的位姿进行优化， 得到联合收获机优化后的 位姿； 由所述联合收获机优化后的位姿与导航基准线的偏差， 得到联合收获机转向调整角 度， 实现对联合收获机的转向控制。 2.根据权利要求1所述的联合收获机转向控制方法， 其特征在于， 由前端视觉里程计估 算出联合收获机的位姿， 即根据导航基准线将田间图像划分为已收割区域和未收割区域， 对已收割区域和未收割区域分别进行 特征点提取， 将提取的图像特 征点进行位姿解 算。 3.根据权利要求2所述的联合收获机转向控制方法， 其特 征在于， 对于已收割区域： 利用四叉树将已收割区域的图像分割成24块， 针对割茬丛设定每块图像的感兴趣ROI 搜索区域， 对 所有的感兴趣ROI搜索区域进 行自适应FAST角点提取， 获得割茬丛的FAST关键 点， 实现割茬丛特 征点的筛 选。 4.根据权利要求3所述的联合收获机转向控制方法， 其特征在于， 所述割茬丛的确定方 法： 在每块感兴趣ROI搜索区域中 同时选取标记多个像素点， 并选择其中一像素点P， 以P为 圆心提取自适应半径为R的圆的像素点， 如果有T个已选取标记的像素点， 则判断为感兴趣 ROI搜索区域范围内的一个割茬丛。 5.根据权利要求3所述的联合收获机转向控制方法， 其特 征在于， 对于未收割区域： 设置划分深度限制： C＝4x 其中： C为要求 提取特征点的数目， x为划分的深度； 利用四叉树将未收割区域的图像分割成24块， 针对作物设定每块 图像的感兴趣ROI搜 索区域， 对所有的感兴趣ROI搜索区域进行自适应FAST角点提取， 获得作物的FAST关键点， 实现作物特 征点的筛 选。 6.根据权利要求5所述的联合收获机转向控制方法， 其特征在于， 当C低于设定的最小 值时， 选取点像素亮度为Ip的点P′， 以P′为圆心、 半径5的像素构成一个检测圆， 提取检测圆 周上28个像素点， 当像素点11和像素点26、 像素点12和像素点25、 像素点13和像素点24、 像 素点4和像素点19、 像素点5和像素点18、 像素点6和像素点19中有一对像素满足约束条件： 则判定P′是作物特 征点； 其中： I1和I2为像素点对的亮度， t为设置的阈值。 7.根据权利要求1所述的联合收获机转向控制方法， 其特征在于， 基于Ceres的后端优 化接收不同时刻联合收获机的位姿进 行优化： 将惯性测量单元采集的信息预积分结果的误 差与图像从3D 重投影到2D的误差进 行约束和计算， 得到联合收获机相对于世界坐标系优化 后的位姿。 8.根据权利要求7所述的联合收获机转向控制方法， 其特征在于， 后端优化还包括闭环 处理， 若当前帧与地图中的关键帧存在相似且相似度超过阈值， 则判断联合收获机重新回权　利　要　求　书 1/2 页 2 CN 115280960 A 2到地图中关键帧的位置 。 9.根据权利要求1所述的联合收获机转向控制方法， 其特征在于， 估算出联合收获机的 位姿通过以下 方法进行跟踪： 判断输入帧和上一帧是否有恒定的速度， 如果有， 根据恒速运动模型估计位姿， 对输入 帧和上一帧进 行特征点匹配计算， 如果匹配点个数大于 设定值， 判断跟踪成功， 否则判断跟 踪失败； 若没有恒定速度， 则进行参考帧估计位姿， 即通过优化图像从3D重投影到2D的误差， 对 当前帧和图库中参考帧进 行匹配， 匹配点个数大于 设定值判定跟踪成功， 否则跟踪失败， 进 行重定位估计位姿， 即重新计算两帧之 间的特征点匹配关系， 若跟踪仍然失败， 则判断当前 信息丢失。权　利　要　求　书 2/2 页 3 CN 115280960 A 3