基于视觉补充的水稻插秧机多传感器组合定位研究

杨圣语，宋悦，薛金林，王培晓; YANG Shengyu; SONG Yue; XUE Jinlin; WANG Peixiao

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

摘要

为改善基于GNSS/INS组合定位的水稻插秧机在遇到遮挡、电磁干扰、传感器失效等情况时的导航效果，在原有GNSS/INS组合定位的基础上，提出一种视觉导航系统（vision navigation system，VNS）补充的水稻插秧机多传感器组合定位方法。首先设计改进的Otsu法和改进的Hough变换算法用于视觉定位信息提取，并构建插秧机和相机坐标系关系方程以求解位姿值；然后采用具有容错功能的联邦卡尔曼滤波算法将VNS输出的定位信息和GNSS、INS输出的定位信息进行融合；最后分别在水泥地和水田进行试验。结果显示，空旷水泥地场景下，GNSS/INS/VNS组合定位和GNSS/INS组合定位精度相近，而在遮挡水泥地场景下，GNSS/INS/VNS组合定位解算出的位置误差和航向误差的平均值分别为1.77 cm和0.99°，相较于GNSS/INS组合定位方法分别提高46.8%和61.5%；水田试验中，经过视觉补充后导航系统的横向偏差和航向偏差平均值分别降低45.7%和67.9%，横向偏差平均值为1.97 cm，航向偏差平均值为0.49°。试验结果表明，基于视觉补充的多传感器组合定位方法能有效降低导航系统的定位误差和跟踪偏差，满足插秧机自动驾驶作业的要求。

关键词

插秧机; 视觉导航; 信息融合; 导航定位; 卡尔曼滤波; 图像处理; 容错性

随着传感器技术和自动控制技术在农业生产上的应用，农业车辆正逐步向自动化、智能化方向发展^［

1-4］。水稻作为世界上主要的粮食作物之一，实现水稻插秧机的自动驾驶与作业，对提高水稻种植生产效率具有重要意义，这也对插秧机的导航定位系统提出了更高的要求。

现有的插秧机自动驾驶技术成果主要基于全球导航卫星系统（GNSS）和惯性导航系统（INS）提供的农机位置信息和航向信息为插秧机提供定位信息。GNSS是实现精准农业作业的基础，在农用车辆中的应用极大地提高了农业生产效率^［

5］。何杰等^{［参考文献 6

百度学术}6］开发了一种插秧机自动作业策略，利用GNSS获取插秧机定位信息，完成了对水稻插秧机自动作业系统的设计。在多传感器组合导航技术中，GNSS和INS的组合应用较为普遍，并且在农业领域应用广泛^{［参考文献 7-9}7-9］。惯性测量单元作为INS系统中的核心部件，不需要接收外部信息，能够以相对较高的采样频率运行，可以有效提高定位信息输出频率。邱权等^{［参考文献 10

百度学术}10］采用自适应系数卡尔曼滤波算法对GNSS和INS的定位信息进行融合，利用GNSS对INS解算的位姿信息进行修正，有效解决了INS运行过程中的误差积累问题。但是在我国复杂的农田环境中，卫星定位容易受到遮挡或电磁干扰而出现信号丢失或多路径效应的现象，导致GNSS/INS组合定位系统的定位精度下降。视觉导航系统（VNS）因其价格便宜、适用性强的优势被广泛应用于农业领域，尤其是农用机械的视觉导航方面^{［参考文献 11-14}11-14］。张泽清等^{［参考文献 15

百度学术}15］提出了一种基于视觉的插秧机导航线提取方法，该方法根据秧苗行的作物特征计算插秧机的导航参数。相比之下，VNS不依赖于其他传感器，能够独立实时采集环境信息从而获取插秧机的相对定位信息^{［参考文献 16

百度学术}16］。

因此，为改善基于GNSS/INS组合定位的水稻插秧机在遇到遮挡、电磁干扰、传感器失效等情况时的导航效果，本研究在原有GNSS/INS组合定位的基础上，开展基于视觉补充的水稻插秧机多传感器组合定位并针对插秧机设计视觉系统定位信息提取方法，提高定位系统的精度和容错性能，旨在为进一步开展插秧机自动作业研究提供理论参考。

1 材料与方法

1.1 总体方案设计

本研究以水稻插秧机为载体，在原有的GNSS/INS组合导航的基础上增加视觉实现插秧机的组合定位，以提高插秧机抗遮挡与抗电磁干扰的能力。系统总体方案如图1所示，通过INS的惯导力学编排解算出插秧机的速度、位置和姿态，通过GNSS获取插秧机的绝对位置和航向信息，通过相机获取水稻秧苗图像，得到插秧机相对于秧苗行的位姿信息。在经过时钟同步后，GNSS和VNS在联邦卡尔曼滤波器中使用间接法与INS的输出参数求差，在主滤波器中得到最优估计，以矫正INS的时间累积误差，输出精准定位信息。

图1 总体方案示意图

Fig.1 Schematic diagram of the overall scheme

系统的硬件平台主要包括洋马RR6高速插秧机、苏测导航卫星接收机、HG-GOYH卫星天线、工控一体机、林柏视S907工业摄像头、非接触式陀螺仪等。系统的软件设计主要包括视觉定位信息提取方法和联邦卡尔曼滤波器设计，本研究在VS2019上基于OpenCV完成了视觉系统的图像处理和位姿求解，并通过Matlab2021搭建联邦卡尔曼滤波器对采集到的多种传感器信息进行融合处理。

1.2 视觉定位信息提取方法

在分析水稻秧苗行成像特点的基础上，设计一种视觉定位信息提取方法。首先是选择合适的色彩空间将秧苗与背景特征区分开来，并采用改进的Otsu法对图像进行二值化；再利用改进的Hough变换算法进行直线检测，拟合出1条导航基准线；最后分析插秧机与相机之间的投影关系并进一步得到插秧机相对于秧苗行的位姿信息。

1）色彩空间转换。本研究采用Lab色彩空间对图像进行分割。在Lab色彩空间中，L分量表示亮度，a分量表示红绿对立维度，b分量表示黄蓝对立维度。RGB图像无法直接转换为Lab图像，需要先转换为XYZ图像，转换公式如式（1）所示：

[\begin{matrix} X^{} \\ Y^{} \\ Z^{} \end{matrix}] = M_{0} [\begin{matrix} R \\ G \\ B \end{matrix}]

（1）

式（1）中，R、G、B为RGB色彩空间各分量值；X、Y、Z为XYZ色彩空间各分量值，M₀=［0.412 4，0.357 6，0.180 5；0.212 6，0.715 2，0.072 2；0.019 3，0.119 2，0.950 5］。XYZ色彩空间转换为Lab色彩空间的公式如式（2）所示：

\{\begin{matrix} L = 116 f (Y / Y_{n}) - 6 \\ a = 500 [f (X / X_{n}) - f (Y / Y_{n})] \\ b = 200 [f (Y / Y_{n}) - f (Z / Z_{n})] \end{matrix}

（2）

式（2）中，L、a、b为Lab色彩空间各分量值；X_n、Y_n、Z_n为标准照明体照射下白体的三刺激值。

图2和图3为秧苗行图片经过色彩空间转换后的Lab空间各分量灰度图和灰度直方图。由图2和图3可知，a分量能够明显的将秧苗的绿色特征与背景分割（图2C），并且峰值像素分布范围较小（图3B），便于图像二值化过程中阈值的搜索。a分量剔除了图像中的亮度信息，能够有效解决水田中反光导致的高亮噪声。

A：原图像 Original image；B：L分量 L componen；C：a分量 a component；D：b分量 b component.图2　Lab空间各分量灰度图

Fig.2 Gray scale image of each component in Lab space

A：L分量L componen；B：a分量a component；C：b分量b component.图3　Lab空间各分量灰度直方图

Fig.3 Gray level histogram of each component in Lab space

2）改进Otsu法阈值分割。本研究通过分析秧田秧苗图像的特征，缩小Otsu法的阈值搜索范围，从而减小算法耗时。由图2C中a分量的灰度图可以看出，秧苗图像背景部分的面积明显大于秧苗行部分的面积，因此，像素数目最多的像素值一定属于背景部分，结合图3B中a分量的灰度直方图可以得出，秧苗部分像素值应小于像素数目最多的像素值，因此，最优阈值可以把像素数目最多的像素值作为搜索上限。假设阈值为K，则有式（3）：

U \leq K \leq N

（3）

式（3）中，N为灰度图中像素数目最多的像素值，U为小于像素数目最多的像素值的第1个像素数目不为零的像素值，则使用改进Otsu法确定秧苗图像分割阈值方法如下：

在进行阈值搜索时，先对像素值进行统计，从像素数目最多的像素值向下搜索，直到搜索到像素数目为零的像素值。根据像素值是否大于K值将灰度图中的像素分为背景和秧苗行两部分，记p₁为背景部分像素数目和总的像素数目的比值，p₂为秧苗行部分像素数目和总的像素数目的比值，全图像素均值则可由式（4）表示：

M = p_{1} \times M_{1} + p_{2} \times M_{2}

（4）

式（4）中，M₁是背景部分像素均值；M₂是秧苗行部分像素均值。背景和秧苗行两部分的类间方差可由式（5）表示：

σ^{2} = p_{1} \times (M_{1} - M) + p_{2} \times (M_{2} - M)

（5）

最终确定的最优阈值由式（6）表示：

K = A r g \underset{U \leq k \leq N}{m a x} \{p_{1} \times {(M_{1} - M)}^{2} + p_{2} \times {(M_{2} - M)}^{2}\}

（6）

对秧田图像灰度图分别使用Otsu法和改进Otsu法进行阈值分割，得到的二值化图像如图4所示，可以看出2种算法分割出的二值化图像结果相近。

图4 秧田二值化图像

Fig.4 Binary image

使用改进Otsu法分别对反光、阴影、昏暗条件下的秧苗图像灰度图进行图像分割，效果如图5所示。由图5可知，无论在何种光照条件下，图像分割的效果都较好。分别使用传统Otsu法和改进Otsu法对6张1 280像素×720像素的秧苗图像进行阈值分割，耗时平均值分别为35.40、21.27 ms，改进的Otsu法由于大幅度缩小了阈值搜索的范围，阈值分割耗时较传统的Otsu法平均缩短了14 ms，有利于提高视觉系统的采样频率。

图5 不同光照条件下的图像分割效果

Fig.5 Image segmentation effect under different illumination conditions

A：反光环境原图 Original drawing of reflective environment；B：阴影环境原图 Shadow environment original picture；C：昏暗环境原图Dark environment original picture；D：反光环境二值图 Reflective environment binary map；E：阴影环境二值图 Shadow environment binary graph； F：昏暗环境二值图 Dim environment binary graph.

3）改进Hough变换直线检测。Hough变换是检测直线的经典算法，它可以分析二值图像中经过每个像素点的所有直线，并把穿过足够多像素点的直线筛选出来。利用Hough变换对图像进行直线检测，检测出的每条直线都可由式（7）表示：

ρ = x c o s θ + y s i n θ

（7）

式（7）中，ρ为直线到图像左上角的像素距离；θ为直线与竖直方向的夹角，顺时针为正。本研究通过提前划定感兴趣区域来限制Hough变换的像素搜索区域和角度的方法对其进行改进：（1）导航线取自离插秧机最近的1条秧苗行，对二值图像使用连通区域提取的方法，通过计算每个形状的力矩筛选出图像最右侧秧苗行的矩形描述符，作为Hough变换像素搜索区域；（2）在秧苗行的矩形描述符中，2对角线的角度分别为θ₁和θ₂，如图6所示，在此区域检测出的秧苗行直线角度应属于［θ₁，θ₂］，故将［θ₁，θ₂］作为Hough变换搜索角度。

图6 秧苗行矩形描述符

Fig.6 Seedling row rectangular descriptor

改进Hough变换算法的流程如图7所示。秧苗行二值图像在经过轮廓提取、去除无效区域后，挑选出图像最右侧秧苗行矩形描述符作为感兴趣区域（RoI），并计算2对角线的角度θ₁和θ₂，定义累加器A（ρ，θ）以映射所有可能的ρ和θ值，同时将初始值设为零，从RoI中选取任一像素的坐标（x，y），并在［θ₁，θ₂］内以π/180为步长遍历所有的角度θ，按照式（8）计算ρ值，每计算出1组（ρ，θ）值则将累加器中对应的A（ρ，θ）值加1，不断循环遍历秧苗行矩形描述符中的所有像素，当A（ρ，θ）大于设定的最小投票数时，即可确定1条直线。

图7 改进Hough变换算法流程图

Fig.7 Flow chart of improved Hough transform algorithm

4）相对位姿求解。假定插秧机在近似平坦的地面上活动，图8为构建的插秧机坐标系和相机坐标系的示意图。在插秧机坐标系O_t-x_ty_tz_t（transplanter）中，原点O_t位于插秧机质心下方的地面上，x_t指向插秧机左侧，y_t垂直地面向上，z_t指向插秧机正前方；相机坐标系O_c-x_cy_cz_c（camera）中，x_cO_cy_c平面为相机镜头平面，z_c为相机主光轴方向。相机与地面之间距离为H，相机主光轴与地面之间的倾角为Φ。

图8 插秧机和相机坐标系示意图

Fig.8 Schematic diagram of rice transplanter and camera coordinate system

相机坐标系与插秧机坐标系之间的齐次变换则可用式（8）表示：

[\begin{matrix} \begin{matrix} x_{c} \\ y_{c} \end{matrix} \\ \begin{matrix} z_{c} \\ 1 \end{matrix} \end{matrix}] = [\begin{matrix} \begin{matrix} \begin{matrix} \begin{matrix} 1 \\ 0 \end{matrix} \\ \begin{matrix} 0 \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ c o s Φ \end{matrix} \\ \begin{matrix} - H s i n Φ \\ 0 \end{matrix} \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} \begin{matrix} 0 \\ s i n Φ \end{matrix} \\ \begin{matrix} c o s Φ \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ - H c o s Φ \end{matrix} \\ \begin{matrix} H s i n Φ \\ 1 \end{matrix} \end{matrix} \end{matrix} \end{matrix}] [\begin{matrix} \begin{matrix} x_{t} \\ y_{t} \end{matrix} \\ \begin{matrix} z_{t} \\ 1 \end{matrix} \end{matrix}]

（8）

图7中，获得的基准线如图9所示，ρ和θ在Hough变换过程中给出，且直线L上的点则可在点D（x₀，0）处用点斜式表示为式（9）：

[\begin{matrix} x \\ y \end{matrix}] = [\begin{matrix} \frac{λ}{c o s θ} \\ 0 \end{matrix}] + k [\begin{matrix} - t a n θ \\ 1 \end{matrix}]

（9）

图9 直线检测输出示意图

Fig.9 Schematic diagram of linear detection output

根据图8的几何关系可得出式（10）：

λ + ρ = A B c o s θ + B C s i n θ

（10）

且相机坐标系中直线的方程可以用式（11）表示：

λ = x_{c} c o s θ + y_{c} s i n θ

（11）

在计算机的帧存图像中，相邻像素之间的实际距离为d_x和d_y，那么相机坐标系中的点D（λd_x/cosθ，0）对应于插秧机坐标系中的点D_t（x_t，y_t，z_t），由（8）式结合小孔成像原理，得：

\{\begin{matrix} x_{t} = \frac{λ d_{x} H}{f c o s θ s i n Φ} \\ y_{t} = 0 \\ z_{t} = \frac{H}{t a n Φ} \end{matrix}

（12）

插秧机坐标系中的直线用式（13）表示：

λ_{t} = x_{t} c o s θ_{t} + z_{t} s i n θ_{t}

（13）

插秧机坐标系中的直线方向角θ_t和相机图像中的直线方向角θ之间的关系则可由式（14）表示：

\frac{d_{x}}{d_{y}} t a n θ = \frac{f t a n θ_{t} + \frac{λ d_{x} c o s Φ}{c o s θ}}{f s i n Φ}

（14）

将式（11）、（12）、（14）式代入（13）式，得：

λ_{t} = \frac{x_{c} H f_{y}}{f_{x} f_{y} s i n Φ - y_{c} f_{x} c o s Φ} c o s θ_{t} + \frac{f_{y} H c o s Φ + y_{c} H s i n Φ}{f_{y} s i n Φ - y_{c} c o s Φ} s i n θ_{t}

（15）

其中，f_x=f/d_x、f_y=f/d_y，分别用于定义水平像素和垂直像素表示的相机焦距，在相机标定的过程中给出。将点（λ/cosθ，0）代入上式并与（14）式联立，即可得到由相机图像中的λ、θ值计算插秧机坐标系中λ_t、θ_t的方程，如式（16）、（17）所示：

λ_{t} = \frac{λ H + f_{y} H s i n Φ c o s Φ s i n θ - λ c o s Φ}{f_{x} c o s θ s i n Φ}

（16）

θ_{t} = a r c t a n \frac{f_{y} s i n Φ s i n θ - λ c o s Φ}{f_{x} c o s θ}

（17）

5）视觉系统实时采样试验。为了将VNS和GNSS、INS融合，视觉系统在保证识别精度的同时必须具有足够高的采样频率。本试验使用林柏视S907工业摄像头获取图像，相机分辨率为1 280像素×720像素。采用张氏标定法求解出的相机内参数矩阵为：

M = [\begin{matrix} 2011.58 & 0 & 695.717 \\ 0 & 2010.48 & 377.576 \\ 0 & 0 & 1 \end{matrix}]

（18）

将相机安装在三脚架上，相机距地面垂直高度为1.5 m，与水平线的夹角为45°，在地面上设置3条仿真秧苗行，如图10所示。改变相机主光轴方向与仿真秧苗行的角度和距离，设置7组采样试验，每组设置好的角度和距离取10次采样结果并计算平均值，对测量得到的位姿和计算得到的位姿进行比较，得到计算误差。结果显示，λ的计算误差的均值为1.53 cm，标准差为0.48 cm；θ的计算误差的均值为0.87°，标准差为0.20°；总体误差率保持在11%以内，并且在系统运行10 s内，采集处理了约60张图片，频率达到6 Hz，视觉导航系统满足组合导航定位系统数据融合所需输出频率的要求。

图10 试验场景图

Fig.10 Test scene diagram

1.3 联邦卡尔曼滤波器设计

本研究设计了一种具有容错功能的最优信息融合联邦卡尔曼滤波器。联邦滤波器选择INS作为公共参考系统，与GNSS和VNS分别构成2个卡尔曼子滤波器。

1）状态方程的建立。联邦滤波器的状态向量为：

\begin{matrix} X = [δ p_{e}, δ p_{n}, δ v_{e}, δ v_{n}, δ φ_{e}, δ φ_{n}, δ φ_{u}, \\ \nabla_{x}, \nabla_{y}, \nabla_{z}, ε_{x}, ε_{y}, ε_{z}] \end{matrix}

（19）

式（19）中，δp_e、δp_n为经度、纬度的误差；δv_e、δv_n为东、北方向上移动速度的误差；δφ_e、δφ_n、δφ_u为东、北、天方向上姿态角的误差；∇_x、∇_y、∇_z为加速度计零偏；ε_x、ε_y、ε_z为陀螺仪角速度计零偏。

状态向量共13维，其状态方程为：

\hat{X} = F X + G w

（20）

式（20）中，F为状态X的系数矩阵，w为系统噪声，G为统噪声的分布矩阵。

系统噪声w和系统噪声的分布矩阵G如式（21）和式（22）所示：

w = [\begin{matrix} \begin{matrix} \begin{matrix} \begin{matrix} w_{g x} & w_{g y} \end{matrix} & w_{g z} \end{matrix} & w_{b x} & w_{b y} & w_{b z} \end{matrix} & w_{a x} & w_{a y} & w_{a z} \end{matrix}]

（21）

式（21）中，w_gx、w_gy、w_gz为陀螺漂移白噪声；w_bx、w_by、w_bz为随机飘逸马尔科夫过程白噪声；w_ax、w_ay、w_az为角速度计零偏一阶马尔科夫白噪声。

G = [0_{6 \times 9}; I_{3 \times 3}, 0_{3 \times 6}; 0_{3 \times 3}, I_{3 \times 3}, 0_{3 \times 3}; 0_{3 \times 6}, I_{3 \times 3}]

（22）

2）GNSS/INS测量方程。在GNSS和INS融合子系统中，系统的量测值有3种：位置量测差值、速度量测差值、航向量测差值。将INS输出的位置、速度和航向信息与GNSS输出的对应信息相减即可得量测方程如式（23）所示：

Z_{G N S S} = [\begin{matrix} p_{I N S} - p_{G N S S} \\ v_{I N S} - v_{G N S S} \\ φ_{I N S} - φ_{G N S S} \end{matrix}] = [\begin{matrix} H_{1} & H_{G N S S} \end{matrix}] [\begin{matrix} X_{I N S} \\ X_{G N S S} \end{matrix}] + V_{G N S S}

（23）

其中，H₁=［I_9×9 0_9×27］，H_GNSS=［ $-$ I_9×9］，V_GNSS是卫星接收器的测量白噪声。

3）VNS/INS测量方程。视觉系统获得的量测值包括插秧机相对于秧苗行导航线的航向偏差β和横向偏差λ_VNS，而INS系统获得的量测值为插秧机绝对位姿，无法直接与视觉系统获得的相对位姿求差值。将INS获得的绝对位置信息与导航系统预设路径结合，求出插秧机航向偏差和横向偏差后，再与视觉系统获得的相对位姿信息求差值，即可作为VNS/INS子系统的位置量测差值和航向量测差值。位置量测差值和航向量测差值可由式（24）表示：

\{\begin{matrix} x_{V N S} - x_{I N S} = λ^{*} s i n θ \\ y_{V N S} - y_{I N S} = - λ^{*} c o s θ \\ φ_{V N S} - φ_{I N S} = β + θ - φ_{I N S} \end{matrix}

（24）

其中，λ^*=λ_VNS-λ_INS，为VNS和INS获得的横向偏差的差值，β为视觉系统获得的航向偏差，θ为导航预设路径方向，可得VNS/INS子系统的测量方程为：

Z_{V N S} = [\begin{matrix} x_{V N S} - x_{I N S} \\ y_{V N S} - y_{I N S} \\ φ_{V N S} - φ_{I N S} \end{matrix}] = H_{V N S} [\begin{matrix} X_{I N S} \\ X_{G P S} \end{matrix}] + V_{V N S}

（25）

其中，H_VNS是由式（24）得到的观测矩阵，V_VNS是相机的测量白噪声。

4）滤波器信息更新。在具有2层融合结构的最优信息融合联邦卡尔曼滤波器中，时间更新在主滤波器和子滤波器中独立进行，量测更新则只在子滤波器中进行，各个子滤波器的时间更新和量测更新分别用式（26）和式（27）表示：

\{\begin{matrix} {\hat{x}}_{i} (t + 1 | t) = Φ_{i} (t) {\hat{x}}_{i} (t | t) + B (t) u (t) \\ P_{i} (t + 1 | t) = Φ_{i} (t) P_{i} (t | t) Φ_{i}^{T} (t) + γ_{i} Q (t) \end{matrix}

（26）

\{\begin{matrix} K_{i} (t + 1) = P_{i} (t + 1 | t) H_{i}^{T} (t + 1) {[H_{i} (t + 1) P_{i} (t + 1 | t) H_{i}^{T} (t + 1) + R_{i} (t + 1)]}^{- 1} \\ {\hat{x}}_{i} (t + 1 | t + 1) = {\hat{x}}_{i} (t + 1 | t) + K_{i} (t + 1) ε_{i} (t + 1) \\ P_{i} (t + 1 | t + 1) [I_{n} K_{i} (t + 1) H_{i} (t + 1)] P_{i} (t + 1 | t) \end{matrix}

（27）

式（26）～（27）中， ${\hat{x}}_{i}$ （t）为t时刻第i个子滤波器的状态向量；Φ_i（t）为状态转换矩阵；u（t）为系统噪声输入；B（t）为输入映射矩阵；P_i（t）为第i个子滤波器中状态向量协方差矩阵；Q（t）为预测值高斯噪声的协方差矩阵；K_i（t）为第i个子滤波器的滤波增益矩阵；R_i（t）为第i个子滤波器测量值高斯噪声的协方差矩阵；γ_i为协方差阵放大倍数。

状态转换矩阵Φ和输入映射矩阵B可由式（28）和式（29）表示：

Φ (t) = [I_{3 \times 3}, Δ t \times I_{3 \times 3}, 0_{3 \times 9}; 0_{3 \times 3}, 0_{3 \times 3}, I_{3 \times 9}; 0_{9 \times 6}, I_{9 \times 9}]

（28）

B = [I_{6 \times 6}; 0_{9 \times 6}]

（29）

P矩阵、Q矩阵和R矩阵的初始值分别设置为：

\{\begin{matrix} P (0) = d i a g [0,0, 1,1, 1,1, 1, {(0.1)}^{2}, {(0.1)}^{2}, {(0.1)}^{2}, 10^{- 4}, 10^{- 4}, 10^{- 4}] \\ Q (0) = d i a g [{(0.01)}^{2}, {(0.01)}^{2}, {(0.01)}^{2}, {(0.05)}^{2}, {(0.05)}^{2}, {(0.05)}^{2}] \\ R (0) = d i a g [{(0.05)}^{2}, {(0.05)}^{2}, 1, {(0.1)}^{2}, {(0.3)}^{2}, {(0.3)}^{2}] \end{matrix}

（30）

主滤波器中的全局状态估计 ${\hat{X}}_{g}$ 和状态协方差矩阵P_g可由式（31）表示：

\{\begin{matrix} {\hat{X}}_{g} = {(P_{1}^{- 1} + P_{2}^{- 1})}^{- 1} (P_{1}^{- 1} {\hat{X}}_{1} + P_{2}^{- 1} {\hat{X}}_{2}) \\ P_{g} = {(P_{1}^{- 1} + P_{2}^{- 1})}^{- 1} \end{matrix}

（31）

式（26）中的协方差阵放大倍数γ_i应满足信息守恒原则，即式（32）：

\begin{matrix} \frac{1}{γ_{_{1}}} + \frac{1}{γ_{_{2}}} = 1 & 0 \leq \end{matrix} \frac{1}{γ_{i}} \leq 1, i = 1,2

（32）

联邦滤波器结构如图11所示。

图11 联邦滤波器结构图

Fig.11 Structure diagram of federated filter

图11中主滤波器到子滤波器的反馈环节为全局滤波对子滤波器滤波值和滤波协方差的重置，重置规则服从式（33）、（34）：

\begin{matrix} {\hat{X}}_{i} = \{\begin{matrix} \begin{matrix} {\hat{X}}_{g}, & | r_{j} | \leq k_{j} \end{matrix} \\ \begin{matrix} {\hat{X}}_{i}, & | r_{j} | > k_{j} \end{matrix} \end{matrix} & i, j = 1,2 且 i \neq j \end{matrix}

（33）

\begin{matrix} P_{i} = \{\begin{matrix} \begin{matrix} γ_{i} P_{g}, & | r_{j} | \leq k_{j} \end{matrix} \\ \begin{matrix} P_{i}, & | r_{j} | > k_{j} \end{matrix} \end{matrix} & i, j = 1,2 且 i \neq j \end{matrix}

（34）

式（33）~（34）中，k为各子系统的容错阈值，r为测量值残差，计算公式如式（35）所示：

\begin{matrix} r_{i} (t) = Z_{i} (t) - H_{i} {\hat{X}}_{i} (t - 1) & i = 1,2 \end{matrix}

（35）

式（33）和（34）表示：当子滤波器1中r₁（t）大于阈值k₁时，认为此子系统测量误差较大，则关闭t时刻子滤波器2的信息重置通道，以防止测量误差污染子滤波器2；当子滤波器2中r₂（t）大于阈值k₂时，认为此子系统测量误差较大，则关闭t时刻子滤波器1的信息重置通道，以防止测量误差污染子滤波器2。

k_i取为3σ_i，σ为r的实验标准差，计算公式如式（36）所示：

σ = \sqrt[]{\frac{\sum_{m = 1}^{n} r_{m}^{2}}{n - 1}}

（36）

1.4 时钟同步

本研究使用GNSS自带的脉冲发生器作为各个传感器的时钟源进行时钟同步，以保证传感器之间的时间差没有累计漂移。将INS（100 Hz）、GNSS（10 Hz）和视觉系统（6 Hz）统一到同一时间戳上，以INS的采样频率为基准进行匹配，如图12所示。在某一匹配时刻，当INS完成1次采样时，寻找GNSS和视觉系统在同一时刻的等效信息。其他两系统的等效信息则通过索引这一时间点前后的2帧信息，进行线性差值计算得到。

图12 时钟同步示意图

Fig.12 Clock synchronization diagram

2 结果与分析

2.1 水泥地试验

为验证多传感器融合算法在不同环境下的定位效果，在南京农业大学校内分别选取1块空旷地面和1块遮挡地面作为试验区域，在2块试验区域内分别测试GNSS/INS融合定位、GNSS/INS/VNS融合定位的定位精度。在地面上铺设3条模拟秧苗行作为视觉识别目标，模拟秧苗行距30 cm、株距20 cm。由于插秧机栽植装置宽度180 cm，因此模拟秧苗设置在预设路径左侧90 cm处用于视觉系统获取信息（图13）。将INS系统初始对准后让插秧机沿预设路径自南向北以0.7 m/s的速度移动，导航系统预设路径信息如表1所示，插秧机行进过程中用石灰标记插秧机行驶路径并用手持RTK定位仪测量得到插秧机实际轨迹。首先单独使用子滤波器1对求解出的GNSS与INS定位信息进行融合处理，作为GNSS/INS融合定位的输出结果。再使用联邦卡尔曼滤波器对INS、GNSS和VNS的定位信息进行融合处理。在Matlab中计算同一时刻下GNSS/INS融合定位、GNSS/INS/VNS融合定位2种定位信息解算模式与实际轨迹之间的位置误差和航向误差，将误差的平均值、最大值和标准差作为定位效果的评价指标。

图13 水泥地试验场景

Fig.13 Test scene of cement ground

A：空旷场景 Empty scene； B：遮挡场景 Occlusion scene.

表1 导航系统预设路径

Table 1 Navigation system default route ( ° )

场地 Space

起始点坐标

Starting point coordinates

结束点坐标

End point coordinates

方向(EbN)

Direction

空旷场景Empty scene

32.07954685/118.41734462

32.07983439/118.41574435

遮挡场景Occlusion scene

32.07944899/118.41735266

32.07972582/118.41577258

2种定位方法分别在空旷场景和遮挡场景的定位效果如图14所示。图14A、14B为2种定位方法解算轨迹与插秧机实际运行轨迹对比图。图14C、14D为2种定位方法解算出的位置信息与插秧机实际运行轨迹之间的位置误差，以定位点在实际轨迹的左侧为正。图14E、14F为2种定位方法解算出的插秧机航向信息与实际轨迹之间的航向误差，以东偏北方向为正方向。空旷场景试验中，卫星天线的搜星数稳定为25颗，GNSS系统中的RTK能够解算出毫米级定位精度，定位精度较高，并能够解算出较为精确的插秧机航向信息。分析GNSS/INS/VNS融合定位中联邦滤波器的运行情况，GNSS子滤波器中的误差协方差阵P₁小于视觉子滤波器中的误差协方差阵P₂，滤波结果更接近GNSS子滤波器中的状态信息。从图14C和图14E可以看出，GNSS/INS/VNS融合定位和GNSS/INS融合定位精度相近。

图14 2种定位方法在水泥地的定位效果

Fig.14 Positioning effect in two scenes and two positioning methods of cement ground

遮挡场景试验中，分析GNSS输出数据串可知，在9.5 s后，由于信号遮挡严重，平均只能接收到15颗卫星信号，GNSS输出的信号变为浮点解（float），并在37.5 s时恢复固定解（fix），搜星数变为20颗。对比图14C、14D可以看出，在遮挡环境下GNSS/INS组合定位解算出的位置信息振荡幅度远大于空旷环境，尤其是在float段，而在融合视觉系统后振荡幅度降低。从图14F可以看出，在float段，GNSS/INS组合定位解算出的航向信息存在大量极值，定位精度与位置信息相比，效果更差。而在融入视觉系统之后，则对航向信息的精度有所改善。

表2为2种定位信息解算模式分别在空旷和遮挡试验场景下的定位精度统计结果。由表2可知，空旷场景下GNSS/INS组合定位精度较高，位置误差和航向误差的平均误差绝对值分别为0.57 cm和0.61°，在与视觉数据进行融合后，位置误差和航向误差的平均误差绝对值分别为0.51 cm和0.56°，分别降低了约10.5%和8.2%。在遮挡场景下，GNSS/INS组合定位解算出的位置误差和航向误差的标准差分别为4.00 cm和3.16°，定位精度较差，最大误差绝对值达到了13.48 cm和10.65°，可靠性较差。GNSS/INS/VNS组合定位解算出的位置误差和航向误差的标准差分别为2.05 cm和1.84°，分别降低了48.8%和41.8%，位置平均误差绝对值和航向平均误差绝对值分别得到了1.56 cm和1.61°的改善，定位精度分别提高了46.8%和61.5%。结果表明，原有GNSS/INS组合定位方法能够在空旷场景下发挥较好的定位效果，但在遇到遮挡时，精度下降较严重，而融入视觉后的GNSS/INS/VNS组合定位则可以解决这一问题，大幅度提高导航定位系统的定位精度和容错性能。

表2 2种定位方法在水泥地的定位误差

Table 2 Positioning deviation of two positioning methods in open scene of cement ground

试验场景Test scenario	定位方法 Positioning	位置误差/cm Position error			航向误差/(°) Course error
试验场景Test scenario	定位方法 Positioning	平均误差绝对值 Average	最大误差绝对值 Maximum	标准差 Standard deviation	平均误差绝对值Average	最大误差绝对值 Maximum	标准差 Standard deviation
空旷场景 Empty	GNSS/INS	0.57	1.42	0.65	0.61	1.65	1.28
空旷场景 Empty	GNSS/INS/VNS	0.51	1.40	0.60	0.56	1.48	1.25
遮挡场景 Occlusion	GNSS/INS	3.33	13.48	4.00	2.60	10.65	3.16
遮挡场景 Occlusion	GNSS/INS/VNS	1.77	5.77	2.05	0.99	5.51	1.84

2.2 田间试验

为验证多传感器组合导航定位方法在水稻插秧机自动驾驶与作业系统中的适用性，在南京八卦洲农场开展水稻插秧机自动驾驶田间试验，试验场景如图15A所示。试验开始前先对导航进行校正，完成陀螺仪的初始对准。导航系统人机交互界面如图15B所示，新建直线作业后在种植好的秧苗行左侧设定导航系统的A、B点，使导航系统预设路径与已种植秧苗行保持平行，即开始插秧机自动驾驶，在有秧苗行引导和无秧苗行引导的情况下分别行走20 m，比较GNSS/INS/VNS和GNSS/INS两种定位方法下的路径跟踪效果。

图15 田间试验场景

Fig.15 Field experiment

A:田间试验场景 Field experiment scene;B:导航系统用户界面 Navigation system user interface.

视觉导航系统采集的秧苗图像处理结果如图16所示。在有秧苗行引导和没有秧苗行引导的情况下导航系统的横向偏差和航向偏差分别如图17所示。其中，有秧苗行引导时横向偏差平均值为1.97 cm、标准差为2.39 cm，航向偏差平均值为0.49°、标准差为0.61°；无秧苗行引导时横向偏差平均值为3.63 cm、标准差为2.86 cm，航向偏差平均值为1.53°、标准差为1.85°。在经过视觉补充后导航系统的横向偏差和航向偏差平均值分别降低45.7%和67.9%，标准差分别降低16.4%和67.0%。结果表明，视觉补充的多传感器组合定位能有效降低导航系统跟踪偏差，满足插秧机的性能要求。

A:秧苗原图 Seedling original picture; B:秧苗二值图 Seedling binary map; C:秧苗导航线 Seedling navigation line.图16　秧苗图像处理结果

Fig.16 Seedling image processing results

图17 2种定位方法的田间定位效果试验

Fig.17 Positioning effect in two scenes and two positioning methods

A：有秧苗行引导时的横向偏差 Lateral deviation in guiding seedlings in rows；B：有秧苗行引导时的航向偏差 Course deviation in guiding seedlings in rows；C：无秧苗行引导时的横向偏差 Lateral deviation of no-seedling row guidance；D：无秧苗行引导时的航向偏差Course deviation of no-seedling row guidance.

3 讨论

本研究分析了水稻秧苗行成像特点，设计了一种视觉定位信息提取方法。相机直接获取的图像为RGB图像，但RGB图像中单独的任何一种颜色分量都不能有效地将秧苗的绿色特征区分开来。一般常用的色彩空间有XYZ、HIS、Luv和Lab等^［

17］，本研究采用Lab色彩空间将秧苗与背景特征区分开来。图像阈值分割中Otsu法通过遍历0~225的所有值来寻找使图像前景和背景类间方差最大的值作为阈值对图像进行分割^{［参考文献 18

百度学术}18］，计算量相对较大。本研究采用限定范围的Otsu法对图像进行二值化，降低计算量。Hough变换对整幅图像中的像素点系统地进行逐行扫描，会产生大量的时间开销和内存占用^{［参考文献 19-20}19-20］，本研究利用叠加约束的Hough变换进行直线检测，有效提高视觉系统输出频率。

信息融合中的卡尔曼滤波理论已经被广泛应用于飞机、汽车、机器人等的自动驾驶导航系统中^［

21-23］。针对插秧机的工作特点，本研究设计了具有容错功能的联邦卡尔曼滤波算法，用于融合来自VNS、GNSS和INS的定位信息，该方法结合了视觉系统不易受外界因素干扰、GNSS精度高以及INS输出频率高的优势，提高了插秧机定位系统的精度和稳定性。为了验证组合定位效果的容错性能，基于插秧机搭建了试验平台，对搭建的插秧机试验平台进行多传感器的组合定位试验。试验结果表明，VNS能够有效弥补GNSS在信号遮挡严重时的定位误差。在遮挡场景试验中，GNSS/INS/VNS组合定位解算出的位置误差和航向误差的平均值分别为1.77 cm和0.99°，相较于GNSS/INS组合定位方法分别提高了46.8%和61.5%。水田试验结果表明：在经过视觉补充后，导航系统的横向偏差和航向偏差平均值分别为1.97 cm和0.49°，相较于GNSS/INS组合定位方法分别降低了45.7%和67.9%，视觉补充的多传感器组合定位能有效降低导航系统跟踪偏差。

参考文献 References

周俊，何永强．农业机械导航路径规划研究进展［J］.农业机械学报，2021，52（9）：1-14.ZHOU J，HE Y Q.Research progress on navigation path planning of agricultural machinery［J］.Transactions of the CSAM，2021，52（9）：1-14 （in Chinese with English abstract）. [百度学术]

胡子谦，王登辉，胡瑞，等.基于拟人驾驶模型的联合收获机导航控制器设计与试验［J］.华中农业大学学报，2022，41（4）：248-258.HU Z Q，WANG D H，HU R，et al.Design and test of navigation controller for combine harvester based on humanoid driving model［J］.Journal of Huazhong Agricultural University，2022，41（4）：248-258 （in Chinese with English abstract）. [百度学术]

张漫，季宇寒，李世超，等.农业机械导航技术研究进展［J］.农业机械学报，2020，51（4）：1-18.ZHANG M，JI Y H，LI S C，et al.Research progress of agricultural machinery navigation technology［J］.Transactions of the CSAM，2020，51（4）：1-18 （in Chinese with English abstract）. [百度学术]

SALEEM M H，POTGIETER J，ARIF K M.Automation in agriculture by machine and deep learning techniques：a review of recent developments［J］.Precision agriculture，2021，22（6）：2053-2091. [百度学术]

杨卫中，吴才聪.国际GNSS精准农业应用概况［J］.农业工程技术，2018，38（18）：20-21.YANG W Z，WU C C.General situation of international GNSS precision agriculture application［J］.Agricultural engineering technology，2018，38（18）：20-21 （in Chinese）. [百度学术]

何杰，朱金光，张智刚，等.水稻插秧机自动作业系统设计与试验［J］.农业机械学报，2019，50（3）：17-24.HE J，ZHU J G，ZHANG Z G，et al.Design and experiment of automatic operation system for rice transplanter［J］.Transactions of the CSAM，2019，50（3）：17-24 （in Chinese with English abstract）. [百度学术]

YIN X，DU J，NOGUCHI N，et al.Development of autonomous navigation system for rice transplanter［J］.International journal of agricultural and biological engineering，2018，11（6）：89-94. [百度学术]

GAO P，LEE H，JEON C W，et al.Improved position estimation algorithm of agricultural mobile robots based on multisensor fusion and autoencoder neural network［J/OL］.Sensors （Basel，Switzerland），2022，22（4）：1522［2023-02-09］. https：//doi.org/10.3390/s22041522. [百度学术]

YAN Y X，ZHANG B H，ZHOU J，et al.Real-time localization and mapping utilizing multi-sensor fusion and visual-IMU-wheel odometry for agricultural robots in unstructured，dynamic and GPS-denied greenhouse environments［J/OL］.Agronomy，2022，12（8）：1740［2023-02-09］.https：//doi.org/10.3390/agronomy12081740. [百度学术]

邱权，胡青含，樊正强，等.基于自适应系数卡尔曼滤波的农业移动机器人组合定位［J］.农业机械学报，2022，53（S1）：36-43.QIU Q，HU Q H，FAN Z Q，et al.Integrated positioning of agricultural mobile robots based on adaptive coefficient Kalman filter［J］.Transactions of the CSAM，2022，53（S1）：36-43 （in Chinese with English abstract）. [百度学术]

关卓怀，陈科尹，丁幼春，等.水稻收获作业视觉导航路径提取方法［J］.农业机械学报，2020，51（1）：19-28.GUAN Z H，CHEN K Y，DING Y C，et al.Visual navigation path extraction method in rice harvesting［J］.Transactions of the CSAM，2020，51（1）：19-28 （in Chinese with English abstract）. [百度学术]

ZHANG Q，SHAOJIE CHEN M E，LI B.A visual navigation algorithm for paddy field weeding robot based on image understanding［J］.Computers and electronics in agriculture，2017，143：66-78. [百度学术]

LI J， ZHU R， CHEN B. Image detection and verification of visual navigation route during cotton field management period［J］. International journal of agricultural and biological engineering， 2018， 11（6）： 159-165. [百度学术]

TIAN H K，WANG T H，LIU Y D，et al.Computer vision technology in agricultural automation：a review［J］.Information processing in agriculture，2020，7（1）：1-19. [百度学术]

张泽清，陈海，蒋浩，等.基于视觉的插秧机导航线提取方法研究［J］.中国稻米，2020，26（5）：34-39.ZHANG Z Q，CHEN H，JIANG H，et al.Extraction of guiding line in unmanned driving technology of rice transplanter［J］.China rice，2020，26（5）：34-39 （in Chinese with English abstract）. [百度学术]

贺静，何杰，罗锡文，等.基于多传感器融合的水稻行识别与跟踪导航研究［J］.农业机械学报，2022，53（3）：18-26.HE J，HE J，LUO X W，et al.Rice row recognition and navigation control based on multi-sensor fusion［J］.Transactions of the CSAM，2022，53（3）：18-26（in Chinese with English abstract）. [百度学术]

李伟斌，马洪林，易贤，等.基于色彩空间变换的彩色图像分割方法［J］.计算机工程与应用，2019，55（9）：162-167.LI W B，MA H L，YI X，et al.Segmentation method for color image based on transformed color space［J］.Computer engineering and applications，2019，55（9）：162-167 （in Chinese with English abstract）. [百度学术]

OTSU N.A threshold selection method from gray-level histograms［J］.IEEE transactions on systems，man，and cybernetics，1979，9（1）：62-66. [百度学术]

MUKHOPADHYAY P，CHAUDHURI B B.A survey of Hough transform［J］.Pattern recognition，2015，48（3）：993-1010. [百度学术]

段汝娇，赵伟，黄松岭，等.一种基于改进Hough变换的直线快速检测算法［J］.仪器仪表学报，2010，31（12）：2774-2780.DUAN R J，ZHAO W，HUANG S L，et al.Fast line detection algorithm based on improved Hough transformation［J］.Chinese journal of scientific instrument，2010，31（12）：2774-2780 （in Chinese with English abstract）. [百度学术]

ROTH M，FRITSCHE C，HENDEBY G，et al.The ensemble Kalman filter and its relations to other nonlinear filters［C］//2015 23rd European Signal Processing Conference （EUSIPCO）.August 31 - September 4，2015，Nice，France.Nice：IEEE，2015：1236-1240. [百度学术]

JIN T，WANG C Y，LU X Y，et al.Analysis of a federal Kalman filter-based tracking loop for GPS signals［J/OL］.GPS solutions，2019，23（4）：119［2023-02-09］. https：//doi.org/10.1007/s10291-019-0911-6. [百度学术]

LI S C，ZHANG M，JI Y H，et al.Agricultural machinery GNSS/IMU-integrated navigation based on fuzzy adaptive finite impulse response Kalman filtering algorithm［J/OL］.Computers and electronics in agriculture，2021，191：106524［2023-02-09］.https：//doi.org/10.1016/j.compag.2021.106524. [百度学术]

联系我们

电话： 027-87287437，027-87287364
邮政编码： 430070
E-mail：hnlkxb@mail.hzau.edu.cn
地址：武汉市洪山区狮子山街1号

友情链接

期刊订阅 Email Alert 国际标准刊号：ISSN 1000-2421 国外邮发代号：BM 3816 国内刊号：CN 42-1181/S 国内发行代号：38-120

基于视觉补充的水稻插秧机多传感器组合定位研究 PDF

摘要

关键词