CN115280960B - 一种基于田间视觉slam的联合收获机转向控制方法 - Google Patents

Abstract

本发明提供了一种基于田间视觉SLAM的联合收获机转向控制方法，工控机基于双目相机采集的图像，根据导航基准线将田间图像划分为已收割区域和未收割区域，前端视觉里程计对田间割茬丛和作物信息分析进行针对性的特征提取，估算出联合收获机的位姿，基于Ceres的后端优化对接收不同时刻联合收获机的位姿进行优化，得到联合收获机优化后的位姿；由联合收获机优化后的位姿与导航基准线的偏差，得到联合收获机转向调整角度，实现对联合收获机的转向控制。本发明使图像更容易被工控机分辨，对于联合收获机机身位姿判断更加准确，进而提高联合收获机的转向控制的精度。

Description

一种基于田间视觉SLAM的联合收获机转向控制方法

技术领域

本发明属于智能农机技术领域，具体涉及一种基于田间视觉SLAM的联合收获机转向控制方法。

背景技术

随着精细农业的提出，我国农业逐步向智能化方向发展，农业车辆自动导航技术成为其中一个重要的研究分支。目前主要的导航技术包括卫星导航、机器视觉、惯性导航、激光和雷达等。

视觉传感器成本较低，尤其随着近年来无人驾驶技术的不断探索和研究，高精度视觉导航技术不断地成熟。但在农业机械导航***中单独使用视觉技术有其局限性，例如田间作业环境复杂多变，受天气状况影响较为严重，在确定导航基准线和运动轨迹方面存在一定的误差。采用纯视觉导航方案进行田间作业，可以实时提取出当前作物行的特征信息，但由于易受光线影响，需结合惯导模块进行补偿，提高定位的精度。

现有技术公开了一种用于轮式联合收获机的转向控制方法，采用双目相机和惯导进行同步定位并输出实时位姿，采用针对农田环境的复杂多变、色彩单一、受光照影响较大等情况设计的图像处理及位姿估计SLAM算法对收获机状态估计，同时还利用非线性卡尔曼滤波针对收获机前方图像变化小特点进行非线性高斯***模型建立并进行优化，输出准确位姿。该方法无需作业前人工踩点，能对收获机驾驶进行辅助控制，但在行进速度较快时，会出现丢帧，对实时位姿输出有影响，影响导航精度。

发明内容

针对现有技术中存在不足，本发明提供了一种基于田间视觉SLAM的联合收获机转向控制方法，用于提高联合收获机的转向控制的精度。

本发明是通过以下技术手段实现上述技术目的的。

一种基于田间视觉SLAM的联合收获机转向控制方法，具体为：

工控机基于双目相机采集的图像，由前端视觉里程计估算出联合收获机的位姿，基于Ceres的后端优化对接收不同时刻联合收获机的位姿进行优化，得到联合收获机优化后的位姿；

由所述联合收获机优化后的位姿与导航基准线的偏差，得到联合收获机转向调整角度，实现对联合收获机的转向控制。

进一步的技术方案，由前端视觉里程计估算出联合收获机的位姿，即根据导航基准线将田间图像划分为已收割区域和未收割区域，对已收割区域和未收割区域分别进行特征点提取，将提取的图像特征点进行位姿解算。

更进一步的技术方案，对于已收割区域：

利用四叉树将已收割区域的图像分割成24块，针对割茬丛设定每块图像的感兴趣ROI搜索区域，对所有的感兴趣ROI搜索区域进行自适应FAST角点提取，获得割茬丛的FAST关键点，实现割茬丛特征点的筛选。

更进一步的技术方案，所述割茬丛的确定方法：

在每块感兴趣ROI搜索区域中同时选取标记多个像素点，并选择其中一像素点P，以P为圆心提取自适应半径为R的圆的像素点，如果有T个已选取标记的像素点，则判断为感兴趣ROI搜索区域范围内的一个割茬丛。

更进一步的技术方案，对于未收割区域：

设置划分深度限制：

C＝4^x

其中：C为要求提取特征点的数目，x为划分的深度；

利用四叉树将未收割区域的图像分割成24块，针对作物设定每块图像的感兴趣ROI搜索区域，对所有的感兴趣ROI搜索区域进行自适应FAST角点提取，获得作物的FAST关键点，实现作物特征点的筛选。

更进一步的技术方案，当C低于设定的最小值时，选取点像素亮度为I_p的点P′，以P′为圆心、半径为5.5个像素构成一个检测圆，提取检测圆周上28个像素点，当像素点11和像素点26、像素点12和像素点25、像素点13和像素点24、像素点4和像素点19、像素点5和像素点18、像素点6和像素点19中有一对像素满足约束条件：

则判定P′是作物特征点；

其中：I₁和I₂为像素点对的亮度，t为设置的阈值。

进一步的技术方案，基于Ceres的后端优化接收不同时刻联合收获机的位姿进行优化：将惯性测量单元采集的信息预积分结果的误差与图像从3D重投影到2D的误差进行约束和计算，得到联合收获机相对于世界坐标系优化后的位姿。

更进一步的技术方案，后端优化还包括闭环处理，若当前帧与地图中的关键帧存在相似且相似度超过阈值，则判断联合收获机重新回到地图中关键帧的位置。

进一步的技术方案，估算出联合收获机的位姿通过以下方法进行跟踪：

判断输入帧和上一帧是否有恒定的速度，如果有，根据恒速运动模型估计位姿，对输入帧和上一帧进行特征点匹配计算，如果匹配点个数大于设定值，判断跟踪成功，否则判断跟踪失败；

若没有恒定速度，则进行参考帧估计位姿，即通过优化图像从3D重投影到2D的误差，对当前帧和图库中参考帧进行匹配，匹配点个数大于设定值判定跟踪成功，否则跟踪失败，进行重定位估计位姿，即重新计算两帧之间的特征点匹配关系，若跟踪仍然失败，则判断当前信息丢失。

本发明的有益效果为：

(1)本发明根据双目相机和惯性测量单元采集的信息，获取联合收获机优化后的位姿信息，再作为转向控制的输入，抗干扰能力强，可实现联合收获机在复杂环境中精确作业；

(2)本发明采用SLAM算法对田间割茬丛和作物信息分析进行针对性的特征提取，根据导航基准线将田间图像划分为已收割区域和未收割区域，对于已收割区域，利用四叉树将已收割区域的图像分割成24块，针对割茬丛设定每块图像的感兴趣ROI搜索区域，对所有的感兴趣ROI搜索区域进行自适应FAST角点提取，获得割茬丛的FAST关键点，实现割茬丛特征点的筛选；对于未收割区域，若使用四叉树划分，先设置划分深度限制，若不使用四叉树划分，选取点像素亮度为I_p的点P′，以P′为圆心、半径为5.5个像素构成一个检测圆，提取检测圆周上28个像素点，当像素点11和像素点26、像素点12和像素点25、像素点13和像素点24、像素点4和像素点19、像素点5和像素点18、像素点6和像素点19中有一对像素满足约束条件，则判定P′是作物特征点；上述提取特征的方法，使图像更容易被工控机分辨，对于联合收获机机身位姿判断更加准确，提高了测量精度；

(3)本发明根据联合收获机优化后的位姿与导航基准线的偏差，得到联合收获机转向调整角度，提高导航***可靠性，改善导航***抑制干扰的能力。

附图说明

图1为本发明所述基于田间视觉SLAM的联合收获机转向控制流程图；

图2为本发明所述田间视觉SLAM算法流程图；

图3为本发明所述前端视觉里程计的特征点提取流程图；

图4为本发明所述FAST作物特征点提取示意图；

图5为本发明所述坐标转换示意图；

图6为本发明所述联合收获机位姿的跟踪流程图；

图7为本发明所述联合收获机转向控制***框图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

如图1所示，本发明一种基于田间视觉SLAM的联合收获机转向控制方法，双目相机和惯性测量单元采集的信息传输给工控机，工控机由采集的信息，利用SLAM算法获取联合收获机优化后的位姿信息，再根据姿态偏差信息进行调节，使联合收获机进行转向调节，让割台边缘更加贴近作物边界线，达到全幅收割的效果。

如图2为SLAM算法流程图，主要针对联合收获机在未知环境中移动过程的自主定位和导航(通过双目相机和惯性测量单元实现)，田间前端视觉里程计从双目相机拍摄的图像中估算出联合收获机的位姿，基于Ceres的后端优化接收不同时刻前端视觉里程计获取的联合收获机位姿以及回环检测的信息，进行优化，得到联合收获机优化后的位姿；回环检测主要是通过双目相机判断是否到过当前位置，进行闭环处理。

图3为前端视觉里程计的特征点提取流程图，分别对左、右目相机采集的图像进行处理，首先根据导航基准线将田间图像划分为已收割区域和未收割区域；采用四叉树将已收割区域的图像分割成24块，针对割茬丛设定每块的感兴趣ROI搜索区域，获取图像的对比度D，计算公式为：

其中：δ(i-j)为图像相邻像素之间的灰度差，P_δ(ij)为图像相邻灰度差为δ的像素分布概率；

对24块图像选取的感兴趣ROI搜索区域进行自适应FAST角点提取，设计自适应半径公式：

R＝αD+β

其中：α和β为根据割茬从设置的自适应参数；

由于割茬丛的特殊相似性，在每块的感兴趣ROI搜索区域中同时选取标记多个像素点，并选择其中一像素点P，以P为圆心提取自适应半径为R的圆的像素点，在自适应半径为R的圆内如果有T个已选取标记的像素点(T的取值为经验值)，则判断为感兴趣ROI搜索区域范围内的一个割茬丛；并通过FAST角点提取算法获得割茬丛的FAST关键点，实现割茬丛特征点的筛选。

由于未收割区域的特征点提取较为困难，四叉树划分层次过多会导致特征点提取过少，导致跟踪丢失，本发明设置划分深度限制，即：

C＝4^x

其中：C为要求提取特征点的数目，x为划分的深度；

然后按照已收割区域提取特征的方法进行作物特征点提取。

当在未收割区域提取的特征点C低于设定的最小值(为经验值)时，取消四叉树划分，选取点像素亮度为I_p的点P′，以P′为圆心、半径为5.5个像素构成一个检测圆，设置阈值t，如图4所示，提取检测圆周上28个像素点，当像素点11和像素点26、像素点12和像素点25、像素点13和像素点24、像素点4和像素点19、像素点5和像素点18、像素点6和像素点19中有一对像素满足下述约束条件，则判定P′是作物特征点：

其中：I₁和I₂为像素点对的亮度。

当割茬丛特征点和作物特征点筛选后，对筛选的特征点使用灰度质心法计算特征点的主方向，然后通过Brief描述子对特征点周边像素进行描述并将信息存储；然后进行双目匹配(双目匹配的过程为现有技术)，获取当前帧均匀分布的特征点。

前端视觉里程计提取图像特征点后，工控机对惯性测量单元采集的信息进行预处理，包含去噪滤波处理，由于传感器采集频率的区别，需要将惯性测量单元数据与双目相机采集的图像进行时间和空间上的对齐。

将提取的图像特征点进行初始化，并进行位姿解算。位姿解算即对双目相机帧间的运动估计，图5为坐标转换示意图，将提取的图像特征点坐标转换到双目相机像素坐标，提取的图像特征点(3D点)为A、B、C，对应双目相机图像(2D点)中a、b、c，其中小写字母代表的点为大写字母在相机成像平面上的投影；记验证点对为D-d，双目相机光心为O。已知A、B、C在世界坐标系中的坐标，由于三角形之间存在对应关系：ΔOab-ΔOAB、ΔObc-ΔOBC、ΔOac-ΔOAC，利用余弦定理，有：

OA²+OB²-2OA·OB·cos(a，b)＝AB² (1)

对于其他两个三角形亦有类似性质，于是有：

OB²+OC²-2OB·OC·cos(b，c)＝BC² (2)

OA²+OC²-2OA·OC·cos(a，c)＝AC² (3)

将式(1)、(2)、(3)均除以OC²，且记x＝OA/OC、y＝OB/OC，则有：

x²+y²-2xy cos(a，b)＝AB²/OC² (4)

y²+1²-2y cos(b，c)＝BC²/OC² (5)

x²+1²-2x cos(a，c)＝AC²/OC² (6)

记v＝AB²/OC²、uv＝BC²/OC²、wv＝AC²/OC²，则有：

x²+y²-2xy cos(a，b)-v＝0 (7)

y²+1²-2y cos(b，c)-uv＝0 (8)

x²+1²-2x cos(a，c)-wv＝0 (9)

把式(7)中的v移到等式右边，并代入式(8)、(9)中，得：

(1-u)y²-ux²-2y cos(b，c)+2uxy cos(a，b)+1＝0 (10)

(1-w)x²-wy²-2x cos(a，c)+2wxy cos(a，b)+1＝0 (11)

已知2D点的图像位置，则三个余弦角cos(a，b)、cos(b，c)、cos(a，c)是已知的，同时u＝BC²/AB²、w＝AC²/AB²，因此，可以通过A、B、C在世界坐标系下的坐标计算出u、w，变换到相机坐标系下之后，u、w的值不变。式中的x、y是未知的，随着双目相机移动会发生变化，通过验证对点来计算最可能的解，进而根据x＝OA/OC、y＝OB/OC，得到A、B、C在相机坐标系下的3D坐标；然后，根据两坐标系下的点对关系，计算双目相机一帧图像的旋转和平移，当双目相机所有图像计算后，即得到联合收获机的位姿。

后端优化主要是状态估计问题，在接收前端视觉里程计传输的联合收获机位姿和惯性测量单元采集的信息预积分结果，将预积分结果的误差与图像从3D重投影到2D的误差使用非线性最小二乘法进行约束和计算，得到联合收获机相对于世界坐标系优化后的位姿。

后端优化中的闭环处理主要是通过在关键帧库中检索，判断当前帧是否与地图(存储在工控机内)中的关键帧存在相似，若相似度超过阈值，则判断联合收获机重新回到关键帧的位置；如果检测到存在闭环，则会将当前状态传递给后端进行优化。

在前端视觉里程计进行图像特征点提取过程中，针对田间特征点难提取、视觉***跟踪容易丢失等问题，采用图6的联合收获机位姿的跟踪方法，提高联合收获机位姿获取的准确性。首先工控机处理后的图像，判断输入帧(即当前处理图像)和上一帧(关键帧库中保存的)是否有恒定的速度，如果有，根据恒速运动模型估计位姿，对输入帧和上一帧进行特征点匹配计算，如果匹配点个数大于设定值，判断跟踪成功，否则判断跟踪失败；若没有恒定速度，则进行参考帧估计位姿，即通过优化图像从3D重投影到2D的误差，对当前帧和图库中参考帧进行匹配，匹配点个数大于设定值判定跟踪成功，否则跟踪失败，进行下一步重定位估计位姿，即重新计算两帧之间的特征点匹配关系，若跟踪仍然失败(匹配点个数大于设定值的方法进行判定)，则判断当前信息丢失，需要重启；三种位姿估计中有一种跟踪成功，则记录位姿信息并更新三种位姿估计信息，剔除失效的特征点。位姿跟踪信息用于判断收获机上一时刻行进到当前时刻的运动轨迹和位姿变换信息。其中根据恒速运动模型估计位姿、参考帧估计位姿以及重定位估计位姿的过程均为现有技术。

图7为联合收获机转向控制***框图，首先，预处理双目相机采集到的图像信息得到导航基准线，其次将导航基准线以及优化后的联合收获机位姿输出作为导航控制器输入，进行偏差计算，得到联合收获机转向调整角度，以方向盘的旋转角度作为导航控制器输出并输入轮式收获机的转向控制器，实现对收获机后轮的转向控制，最后将角度传感器测量的后轮转向角度作为反馈，对整体的转向控制实现闭环。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (5)

1.一种基于田间视觉SLAM的联合收获机转向控制方法，其特征在于：

工控机基于双目相机采集的图像，由前端视觉里程计估算出联合收获机的位姿，基于Ceres的后端优化对接收不同时刻联合收获机的位姿进行优化，得到联合收获机优化后的位姿；

由所述联合收获机优化后的位姿与导航基准线的偏差，得到联合收获机转向调整角度，实现对联合收获机的转向控制；

由前端视觉里程计估算出联合收获机的位姿，即根据导航基准线将田间图像划分为已收割区域和未收割区域，对已收割区域和未收割区域分别进行特征点提取，将提取的图像特征点进行位姿解算；

对于已收割区域：利用四叉树将已收割区域的图像分割成24块，针对割茬丛设定每块图像的感兴趣搜索区域，对所有的感兴趣搜索区域进行自适应FAST角点提取，获得割茬丛的FAST关键点，实现割茬丛特征点的筛选；

对于未收割区域：设置划分深度限制：

C＝4^x

其中：C为要求提取特征点的数目，x为划分的深度；

利用四叉树将未收割区域的图像分割成24块，针对作物设定每块图像的感兴趣搜索区域，对所有的感兴趣搜索区域进行自适应FAST角点提取，获得作物的FAST关键点，实现作物特征点的筛选；

当C低于设定的最小值时，取消四叉树划分，选取点像素亮度为I_p的点P′，以P′为圆心、半径为5.5个像素构成一个检测圆，提取检测圆周上28个像素点，当像素点11和像素点26、像素点12和像素点25、像素点13和像素点24、像素点4和像素点19、像素点5和像素点18、像素点6和像素点17中有一对像素满足约束条件：

则判定P′是作物特征点；

其中：I₁和I₂为像素点对的亮度，t为设置的阈值。

2.根据权利要求1所述的联合收获机转向控制方法，其特征在于，所述割茬丛的确定方法：

在每块感兴趣搜索区域中同时选取标记多个像素点，并选择其中一像素点P，以P为圆心提取自适应半径为R的圆的像素点，如果有T个已选取标记的像素点，则判断为感兴趣搜索区域范围内的一个割茬丛。

3.根据权利要求1所述的联合收获机转向控制方法，其特征在于，基于Ceres的后端优化接收不同时刻联合收获机的位姿进行优化：将惯性测量单元采集的信息预积分结果的误差与图像从3D重投影到2D的误差进行约束和计算，得到联合收获机相对于世界坐标系优化后的位姿。

4.根据权利要求3所述的联合收获机转向控制方法，其特征在于，后端优化还包括闭环处理，若当前帧与地图中的关键帧存在相似且相似度超过阈值，则判断联合收获机重新回到地图中关键帧的位置。

5.根据权利要求1所述的联合收获机转向控制方法，其特征在于，估算出联合收获机的位姿通过以下方法进行跟踪：

判断输入帧和上一帧是否有恒定的速度，如果有，根据恒速运动模型估计位姿，对输入帧和上一帧进行特征点匹配计算，如果匹配点个数大于设定值，判断跟踪成功，否则判断跟踪失败；

若没有恒定速度，则进行参考帧估计位姿，即通过优化图像从3D重投影到2D的误差，对当前帧和图库中参考帧进行匹配，匹配点个数大于设定值判定跟踪成功，否则跟踪失败，进行重定位估计位姿，即重新计算两帧之间的特征点匹配关系，若跟踪仍然失败，则判断当前信息丢失。