CN111179303B - 基于粒子滤波的谷物收获机器人视觉导航方法及其应用 - Google Patents

Abstract

本发明公开了一种基于粒子滤波的谷物收获机器人视觉导航方法，包括：1、在谷物收获机器人顶部安装相机，实时获取收获机前方原始谷物图像序列，以及与之对应的鸟瞰图；2、初始化第一目标区域ROI1、特征区域、粒子位置、粒子区域、导航线横坐标；3、更新粒子区域、特征区域；4、计算粒子的权重，并对其归一化，计算k时刻粗分割的收割线横坐标；5、根据粗分割的结果在ROI1中截取第二目标区域ROI2，对ROI2进行处理，得到细分割的收割线上的两个点P₁、P₂；6、计算割幅并提取导航线，对粒子重采样，跳转到步骤3，继续提取下一时刻的导航线。该方法能够对收割线进行追踪和精确识别，有效提取收获机器人导航参数和割幅宽度。

Description

基于粒子滤波的谷物收获机器人视觉导航方法及其应用

技术领域

本发明属于农业机械自动化技术领域，具体涉及谷物收获机器人的导航线提取方法，以及该方法在收获机偏航角计算和位姿关系标定的应用。

背景技术

精准农业是农业科学研究的热点领域，智能收获机的使用，能有效提高农业生产的效率，解放劳动生产力。路径检测已经成为农业机械自动化的一个热点，为了获得较高的生产率，需要使收获机在满割幅的状况下进行田间作业，作业过程中由于长时间保持满割幅，驾驶员劳动强度大，容易造成疲劳驾驶，并且田间作业灰尘大，肉眼难以获取准确的边界线，因此需要实时获取导航线，对路径进行自动规划，实现满割幅作业。在路径检测时计算收获机的收割幅度，通过割幅、作物密度和收获机在田间的作业速度可以获得收获机的喂入量。因此准确、实时获取稻麦收割线对于实现农业自动化，提高农业生产率具有重要的意义。

目前智能收获机自动驾驶采用的卫星导航容易做到全球、全天候精准定位，但不能实时连续导航；惯性导航实时性好，不依赖任何外部信息，但是设备复杂，需要高精密的惯性元件，成本高，长时间使用必须进行额外校准；组合导航比单一的导航系统精度高，各种导航系统进行数据融合，能达到优势互补，但是采用组合导航如果不能及时正确判断并隔离掉出现故障的导航系统，会影响到其他系统的导航性能；多数算法没有将导航线的提取与智能收获机相对位姿相联系，研究停留在导航线的提取上；在导航线提取方面，采用2G-R-B算法实时分割田间作物以获取导航参数，适用于绿色作物，但不能应用于成熟稻麦的分割；利用HSV颜色空间的S分量结合OTSU算法进行分割，进而获取导航线，此方法可在颜色层次明显的区域进行分割，无法分割农田作业中整片的成熟稻麦区域；采用双目视觉获取深度信息，根据深度信息获取导航线，此方法计算量大，难以适应复杂的农田环境和在运动的收获机上实现收割线的提取。

发明内容

发明目的：针对现有技术中存在的问题，本发明旨在提供一种能够对收割线进行追踪和精确识别，有效提取收获机导航参数和割幅宽度的谷物收获机器人导航线提取方法。

技术方案：本发明一方面公开了一种基于粒子滤波的谷物收获机器人视觉导航方法，包括：

(1)在谷物收获机器人顶部安装相机并对相机进行标定获取比例因子SCALE，相机实时获取收获机前方原始谷物图像序列，获取与原始谷物图像对应的谷物图像鸟瞰图；

(2)在初始时刻k＝1时，在谷物图像的鸟瞰图Pic_air(k)中确定第一目标区域ROI1，所述第一目标区域ROI1的宽为固定值width，高为固定值height，width>height；将ROI1中未收割区域侧大小为height×height的未收割区域作为k时刻的特征区域

在ROI1的上边界设置N个粒子p_i，粒子与ROI1左边界的距离为均匀分布；以粒子p_i的位置为左上角，ROI1的右下角为右下角作为p_i的粒子区域Area_k(p_i)，i＝1,…,N；初始化导航线横坐标pre_x(1)为收割区域与未收割区域的边界上任一点的横坐标；

(3)当k>1时，在k时刻的谷物图像鸟瞰图Pic_air(k)中更新粒子区域Area_k(p_i)，更新未收割区域的特征区域

的横坐标范围，

的横坐标从pre_x(k-1)+T₁到ROI1的右侧边界；pre_x(k-1)为k-1时刻导航线的横坐标，T₁为预设的特征区域调节值；

(4)计算

的纹理特征值

计算每个粒子区域Area_k(p_i)的纹理特征值

计算k时刻粗分割的收割线横坐标X_c(k)；

(5)以X_c(k)为中心线，向左右分别扩展距离x_roi2，得到宽度为2*x_roi2，高度为height的区域为第二目标区域ROI2；对ROI2进行处理，得到细分割的收割线上的两个点P₁、P₂；更新当前时刻导航线的横坐标pre_x(k)为过P₁、P₂点的直线与ROI1下边界的交点的横坐标；

(6)计算割幅并提取导航线，对粒子进行重采样，令k＝k+1，跳转到步骤3，继续提取下一时刻的导航线。

另一方面，本发明公开了应用上述导航线提取方法来计算收获机偏航角的方法，包括：

S1、根据上述谷物收获机器人视觉导航方法实时提取谷物收获机器人上的导航线点P₁₁和P₂₂；

在原始谷物图像上建立坐标系C_p，所述坐标系C_p以相机采集的原图中心为原点，Y轴指向图像上方，X轴指向图像左侧，Z轴与X,Y轴成右手定则关系；将P₁₁和P₂₂变换到坐标C_p下，得到P′₁₁和P′₂₂：

其中WIDTH为谷物图像鸟瞰图的宽度；HEIGHT为谷物图像鸟瞰图的高度；x_m、y_m对应P₁₁、P₂₂的坐标，x′_m、y′_m为坐标系变换之后的坐标；

S2、收获机收割线与Y轴的夹角θ为：

S3、计算过P′₁₁和P′₂₂的直线L_k′，计算直线L_k′与C_p坐标系的X轴的交点P₀(X₀，0)，其在C_w坐标系中的坐标为：

其中x_w、y_w、z_w表示交点P₀在C_w坐标系的坐标；

d_x表示原始谷物图像中一个像素在X轴上的物理尺寸；h表示相机距离地面的高度；f是焦距；

是相机的俯仰角；

S4、C_p坐标系中的收割线与Y轴的夹角为θ，根据小孔成像和坐标系转换得偏航角θ_w：

其中

表示摄像机的横向因子，

表示摄像机的纵向因子，a_x、a_y可以通过相机标定获取。

本发明还公开了一种谷物收获机器人与导航路径相对位姿关系标定方法，包括：

根据上述谷物收获机器人偏航角提取方法计算谷物收获机器人的偏航角θ_w；

地面上实际导航路径投影的像素坐标与智能收获机相对位姿之间的关系方程如下：

其中λ为偏航距；X,Y为C_p坐标系下导航线的坐标。

有益效果：与现有技术相比，本发明公开的基于粒子滤波的谷物收获机器人视觉导航方法具有以下优点：1、能准确、快速的分割已收割的稻麦区域和未收割的稻麦区域；2、基于粒子滤波进行收割线的跟踪，能有效定位收割线，实现导航线的提取，并实时获得割幅宽度，为收获机进行产量分析和路径规划提供了重要的数据信息。

附图说明

图1为本发明公开的谷物收获机器人视觉导航方法的流程图；

图2为实施例中坐标系示意图；

图3为实施例中谷物图像鸟瞰图示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明的具体实施案例做说明。

如图1所示，本发明公开了一种基于粒子滤波的谷物收获机器人视觉导航方法，包括：

步骤1、在谷物收获机器人顶部安装相机，如图2所示，定义收获机坐标系C_w，其坐标原点O_w在收获机下方的地面上，Z_w轴沿收获机中心线指向行进前方，X_w轴指向收获机前进方向的左侧，Y_w轴垂直地面向上；相机坐标系C_c的原点O_c在O_w的正上方，距离地面高度为h，Z_c轴与相机光轴保持一致，倾斜角为

且

X_c轴经过O_c指向收获机前进方向的左侧，Y_c轴垂直于面X_cO_cZ_c指向农机上方，X_c、Y_c、Z_c三个坐标轴成右手定则。

标定相机，获取比例因子SCALE，相机实时获取收获机前方的原始谷物图像序列pic(k)，并经过逆透视变换获得鸟瞰图Pic_air(k)，在Pic_air(k)的图像坐标系C_air中确定左侧分禾器的坐标X_l；

步骤2、在初始时刻k＝1时，在谷物图像的鸟瞰图Pic_air(k)中确定第一目标区域ROI1，如图3所示，第一目标区域ROI1的宽为固定值width，高为固定值height，左上角坐标为(x_tl,y_tl)，且width>height；将ROI1中未收割区域侧大小为height×height的未收割区域作为k时刻的特征区域

在ROI1的上边界设置N个粒子p_i，粒子与ROI1左边界的距离为均匀分布；由于粒子都在ROI1区域内，为便于计算，以ROI1的左上角为原点，水平向右为X轴，竖直向下为Y轴，建立坐标系C_ROI1，每个粒子的位置为

以粒子p_i的位置为左上角，ROI1的右下角为右下角作为p_i的粒子区域Area_k(p_i)，i＝1,…,N；Area_k(p_i)的初始大小为

初始化导航线横坐标pre_x(1)为收割区域与未收割区域的边界上任一点的横坐标；

步骤3、当k>1时，在k时刻的谷物图像鸟瞰图Pic_air(k)中更新粒子区域Area_k(p_i)，更新未收割区域的特征区域

的横坐标范围，

的横坐标从pre_x(k-1)+T₁到ROI1的右侧边界；pre_x(k-1)为k-1时刻导航线的横坐标，T₁为预设的特征区域调节值，以保证特征区域

始终处于未收割部分；

步骤4、计算

的纹理特征值

计算每个粒子区域Area_k(p_i)的纹理特征值

计算k时刻粗分割的收割线横坐标X_c(k)；

图像区域Area的纹理特征值的计算步骤如下：

(2.1)对区域Area降低灰度级；

(2.2)计算区域Area的灰度共生矩阵P；

(2.3)计算灰度共生矩阵的熵：

eng(Area)即为区域Area的纹理特征值。

按照步骤(2.1)-(2.3)，计算

的纹理特征值

和每个粒子区域Area_k(p_i)的纹理特征值

计算当前时刻粗分割的收割线横坐标X_c(k)的具体步骤为：

(4.1)计算粒子的权重：

其中，

表示k时刻第i个粒子对应的权重；

表示k时刻第i个粒子的横坐标；

表示k时刻第i个粒子的纹理特征值；

(4.2)粒子权重归一化，得到

(4.3)计算粗分割的收割线横坐标X_c(k)：

步骤5、根据粗分割的结果在ROI1中截取第二目标区域ROI2，以缩小收割线区域；以X_c(k)为中心线，向左右分别扩展距离x_roi2，得到宽度为2*x_roi2，高度为height的区域为第二目标区域ROI2；

对ROI2进行处理，得到细分割的收割线上的两个点P₁、P₂，具体步骤为：

(5.1)对第二目标区域ROI2利用反向投影算法投影出已收割区域与未收割区域的灰度图G_b；

(5.2)利用拉普拉斯算子对灰度图G_b进行边缘增强得到图像G；

(5.3)对增强后的图像G进行图像处理，得到细分割的收割线上的点，具体包括：

对图像G依次进行如下处理：使用21*21的中值滤波对图像G进行模糊；大津法对图像二值化；对图像进行腐蚀；采用基于单点的区域生长，寻找y轴方向的边缘线；霍夫变换求得边缘线上的两个点P₁＝(x₁,y₁)和P₂＝(x₂,y₂)。

更新当前时刻导航线的横坐标pre_x(k)为过P₁、P₂点的直线与ROI1下边界的交点的横坐标；

步骤6、计算割幅W：

其中L为谷物收获机器人割台的宽度；X_c(k)为粗分割的收割线横坐标；P₁₁＝(x₁₁,y₁₁)，P₂₂＝(x₂₂，y₂₂)为P₁、P₂在谷物图像鸟瞰图坐标系C_air中的对应点；X_l为左侧分禾器在图像中的坐标；SCALE是标定获取的尺度因子；

提取导航线：将P₁₁和P₂₂变换到当前时刻的原始谷物图像上，过点P₁₁和P₂₂的直线即为当前时刻的导航线；令k＝k+1，跳转到步骤3，继续提取下一时刻的导航线。

对粒子进行重采样，令k＝k+1，跳转到步骤(3)，继续提取下一时刻的导航线。

对粒子进行重采样的步骤为：

若X_c(k)<t，则产生N个随机数rand_i∈U(0,σ)，其中t为预设的重采样阈值；否则产生N个随机数rand_i∈U(-σ,σ)；每个粒子横坐标

重采样为：

纵坐标

粒子所在区域大小重采样为

U(·)表示均匀分布，±σ为随机数上下区间，σ为粒子重采样范围阈值。

在获取导航线后，可以计算收获机的偏航角，以及得到导航路径与收获机相对位姿之间的关系，具体步骤为：

S1、在原始谷物图像上建立坐标系C_p，此坐标系以相机采集的原图中心为原点，Y轴指向图像上方，X轴指向图像左侧，Z轴与X,Y轴成右手定则关系；将P₁₁和P₂₂变换到坐标C_p下，得到P′₁₁和P′₂₂：

其中WIDTH为鸟瞰图的宽度；HEIGHT为鸟瞰图的高度；x_m、y_m对应P₁₁、P₂₂的坐标，x′_m、y′_m为坐标系变换之后的坐标；

S2、收获机收割线与Y轴的夹角θ为：

S3、计算过P′₁₁和P′₂₂的直线L_k′，计算直线L_k′与C_p坐标系的X轴的交点P₀(X₀，0)，其在C_w坐标系中的坐标为：

其中x_w、y_w、z_w表示交点P₀在C_w坐标系的坐标；

d_x表示原始谷物图像中一个像素在X轴上的物理尺寸；h表示相机距离地面的高度；f是焦距；

是相机的俯仰角；

S4、C_p坐标系中的收割线与Y轴的夹角为θ，斜率为负时，θ>0，斜率为正时，θ<0，根据小孔成像和坐标系转换得偏航角θ_w：

其中

表示摄像机的横向因子，

表示摄像机的纵向因子，a_x、a_y可以通过相机标定获取；

S5、地面上实际导航路径投影的像素坐标与智能收获机相对位姿之间的关系方程如下：

其中λ为偏航距；X,Y为C_p坐标系下导航线的坐标。

Claims (8)

1.基于粒子滤波的谷物收获机器人视觉导航方法，其特征在于，包括：

(1)在谷物收获机器人顶部安装相机并对相机进行标定获取比例因子SCALE，相机实时获取收获机前方原始谷物图像序列，获取与原始谷物图像对应的谷物图像鸟瞰图；

(2)在初始时刻k＝1时，在谷物图像的鸟瞰图Pic_air(k)中确定第一目标区域ROI1，第一目标区域ROI1的宽为固定值width，高为固定值height，左上角坐标为(x_tl,y_tl)，且width>height；第一目标区域ROI1中包括收割区域与未收割区域的边界，且未收割区域位于ROI1的右侧部分；将ROI1中未收割区域侧大小为height×height的未收割区域作为k时刻的特征区域

在ROI1的上边界从左向右均匀设置N个粒子p_i；以粒子p_i的位置为左上角，ROI1的右下角为右下角作为p_i的粒子区域Area_k(p_i)，i＝1,…,N；初始化导航线横坐标pre_x(1)为收割区域与未收割区域的边界上任一点的横坐标；

(3)当k>1时，在k时刻的谷物图像鸟瞰图Pic_air(k)中更新粒子区域Area_k(p_i)，更新未收割区域的特征区域

的横坐标范围，

的横坐标从pre_x(k-1)+T₁到ROI1的右侧边界；pre_x(k-1)为k-1时刻导航线的横坐标，T₁为预设的特征区域调节值；

(4)计算

的纹理特征值

计算每个粒子区域Area_k(p_i)的纹理特征值

计算k时刻粗分割的收割线横坐标X_c(k)；

(5)以X_c(k)为中心线，向左右分别扩展距离x_roi2，得到宽度为2*x_roi2，高度为height的区域为第二目标区域ROI2；对ROI2进行处理，得到细分割的收割线上的两个点P₁、P₂；更新当前时刻导航线的横坐标pre_x(k)为过P₁、P₂点的直线与ROI1下边界的交点的横坐标；

(6)计算割幅并提取导航线，对粒子进行重采样，令k＝k+1，跳转到步骤(3)，继续提取下一时刻的导航线。

2.根据权利要求1所述的谷物收获机器人视觉导航方法，其特征在于，所述图像区域Area的纹理特征值的计算步骤如下：

(2.1)对区域Area降低灰度级；

(2.2)计算区域Area的灰度共生矩阵P；

(2.3)计算灰度共生矩阵的熵：

eng(Area)即为区域Area的纹理特征值。

3.根据权利要求1所述的谷物收获机器人视觉导航方法，其特征在于，所述步骤(4)中计算粗分割的收割线横坐标X_c(k)的具体步骤为：

(4.1)计算粒子的权重：

其中

表示k时刻第i个粒子对应的权重；

表示k时刻第i个粒子的横坐标；

表示k时刻第i个粒子的纹理特征值；

(4.2)粒子权重归一化，得到

(4.3)计算粗分割的收割线横坐标X_c(k)：

4.根据权利要求1所述的谷物收获机器人视觉导航方法，其特征在于，所述步骤(5)中获取细分割的收割线的步骤为：

(5.1)对第二目标区域ROI2利用反向投影算法投影出已收割区域与未收割区域的灰度图G_b；

(5.2)利用拉普拉斯算子对灰度图G_b进行边缘增强得到图像G；

(5.3)对增强后的图像G进行图像处理，得到细分割的收割线上的点，具体包括：

对图像G依次进行如下处理：使用21*21的中值滤波对图像G进行模糊；大津法对图像二值化；对图像进行腐蚀；采用基于单点的区域生长，寻找y轴方向的边缘线；霍夫变换求得边缘线上的两个点P₁＝(x₁,y₁)和P₂＝(x₂,y₂)。

5.根据权利要求1所述的谷物收获机器人视觉导航方法，其特征在于，所述步骤(6)中割幅W为：

其中L为谷物收获机器人割台的宽度；X_c(k)为粗分割的收割线横坐标；P₁₁＝(x₁₁,y₁₁)，P₂₂＝(x₂₂，y₂₂)为谷物图像鸟瞰图坐标系中P₁、P₂的对应点；X_l为左侧分禾器在图像中的坐标；SCALE是标定获取的尺度因子；

将P₁₁和P₂₂变换到原始谷物图像上，过点P₁₁和P₂₂的直线即为当前时刻的导航线。

6.根据权利要求1所述的谷物收获机器人视觉导航方法，其特征在于，所述步骤(6)中对粒子进行重采样的步骤为：

若X_c(k)<t，则产生N个随机数rand_i∈U(0,σ)，其中t为预设的重采样阈值；否则产生N个随机数rand_i∈U(-σ,σ)；每个粒子横坐标

重采样为：

纵坐标

粒子所在区域大小重采样为

U(·)表示均匀分布，±σ为随机数上下区间，σ为粒子重采样范围阈值。

7.一种谷物收获机器人偏航角提取方法，其特征在于，包括：

S1、根据权利要求1-6中任一项所述的谷物收获机器人视觉导航方法实时提取谷物收获机器人上的导航线点P₁₁和P₂₂；

在原始谷物图像上建立坐标系C_p，所述坐标系C_p以相机采集的原图中心为原点，Y轴指向图像上方，X轴指向图像左侧，Z轴与X,Y轴成右手定则关系；将P₁₁和P₂₂变换到坐标C_p下，得到P′₁₁和P′₂₂：

其中WIDTH为谷物图像鸟瞰图的宽度；HEIGHT为谷物图像鸟瞰图的高度；x_m、y_m对应P₁₁、P₂₂的坐标，x′_m、y′_m为坐标系变换之后的坐标；

S2、收获机收割线与Y轴的夹角θ为：

S3、计算过P′₁₁和P′₂₂的直线L_k′，计算直线L_k′与C_p坐标系的X轴的交点P₀(X₀，0)，其在C_w坐标系中的坐标为：

其中x_w、y_w、z_w表示交点P₀在C_w坐标系的坐标；

d_x表示原始谷物图像中一个像素在X轴上的物理尺寸；h表示相机距离地面的高度；f是焦距；

是相机的俯仰角；

S4、C_p坐标系中的收割线与Y轴的夹角为θ，根据小孔成像和坐标系转换得偏航角θ_w：

其中

表示摄像机的横向因子，

表示摄像机的纵向因子，a_x、a_y可以通过相机标定获取。

8.一种谷物收获机器人与导航路径相对位姿关系标定方法，其特征在于，包括：

根据权利要求7计算谷物收获机器人的偏航角θ_w；

地面上实际导航路径投影的像素坐标与智能收获机相对位姿之间的关系方程如下：

其中λ为偏航距；X,Y为C_p坐标系下导航线的坐标。

Images