[发明专利]一种基于田间视觉SLAM的联合收获机转向控制方法

权利要求书

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

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

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

2.根据权利要求1所述的联合收获机转向控制方法，其特征在于，由前端视觉里程计估算出联合收获机的位姿，即根据导航基准线将田间图像划分为已收割区域和未收割区域，对已收割区域和未收割区域分别进行特征点提取，将提取的图像特征点进行位姿解算。

3.根据权利要求2所述的联合收获机转向控制方法，其特征在于，对于已收割区域：

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

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

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

5.根据权利要求3所述的联合收获机转向控制方法，其特征在于，对于未收割区域：

设置划分深度限制：

C＝4^x

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

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

6.根据权利要求5所述的联合收获机转向控制方法，其特征在于，当C低于设定的最小值时，选取点像素亮度为I_p的点P′，以P′为圆心、半径5的像素构成一个检测圆，提取检测圆周上28个像素点，当像素点11和像素点26、像素点12和像素点25、像素点13和像素点24、像素点4和像素点19、像素点5和像素点18、像素点6和像素点19中有一对像素满足约束条件：

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

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

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

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

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

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

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