基于视觉导航的农机多智体相对导航算法研究

孟福军，岳胜如，胡雪菲

（塔里木大学水利与建筑工程学院，新疆 阿拉尔 843300）

在精准农业技术驱动下，农业机械自主导航的现代化有了十足的发展［1，2］。作为精准农业的一项核心关键技术，利用农机多智体（即多个农业机械智能机器人）的协同作业去完成田间的智能播种、喷药、施肥、收割等或在收割机提出卸粮请求时，运粮农机跟随收割机智能行进并配合收割机完成卸粮作业，既能降低能源消耗，又能提高田间作业效率。在田间作业中，还可减少人员的直接参与。而同类型农机多智体之间、各农机智能体与被操作目标（如运粮农机、农作物等）之间的相对导航问题必须首先解决。

目前，无线电导航、惯性导航（INS）、视觉导航、卫星导航等技术是农业机械导航定位的主要技术［3-6］。视觉自主相对导航具有低成本、探测信息范围宽、完整的目标信息、高精度、强自主性、易于实现等优势，非常适用于农机多智体相对导航需求。在目标农机智能体上安装形状和大小已知的光学特征装置，在主动农机智能体上安装CCD相机，要进行主动农机智能体和目标农机智能体之间的相对导航，就需通过对特征装置在CCD相机上成像的分析和计算。因此，本研究提出基于视觉导航的农机多智体相对导航算法，以机器视觉中的四元数模型作为农机多智体相对导航算法，四元数位姿确定参数只有7个，相比较Hall算法12个参数［6］降低了相关矩阵的阶数，减轻了计算量。

1 坐标系定义及其坐标变换

1.1 坐标系定义

如图1所表示，本研究假设相机坐标系的三轴分别与主动农机智能体的三轴方向一致，其中，OB-xByBzB为主动农机智能体本体坐标系、OW-xWyWzW为被动农机智能体本体坐标系、OC-xCyCzC为相机坐标系，在基于机器视觉的相对导航中，目标与相机的相对位姿关系一般通过求解所选的目标参考坐标系OW-xWyWzW与相机坐标系的相对关系得到。根据实际要将目标农机智能体的坐标系定义为目标参考坐标系。像素坐标系为o-uv，u为列数，v为行数。OI-xIyI为图像物理坐标系。

图1 各坐标系及其相互关系

1.2 四元数对三维物体旋转变换的描述

四元数在描述目标的姿态方面具有特有的优势，它既能避免欧拉角所产生的奇异问题，又具有简单、系统的运算法则［7-10］。

假设x为空间三维向量，经过旋转矩阵R变换为x′，则有如下关系：

式中，旋转矩阵R可由四元数Q的元素表示为：

式中，mij(i=1,2,3;j=1,2,3)为：

2 基于视觉导航的农机多智体相对导航算法

2.1 确定农机多智体相对位姿关系

基于机器视觉的农机多智体相对位姿关系确定是确定主动农机智能体相机与被动农机智能体目标间的相对位姿，即根据目标特征点在目标参考坐标系的坐标与其相应的像平面坐标系求解相机的6个外方位元素。即：

将式（5）进一步转化得：

式中，（xWi,yWi,zWi）为空间第i个点的坐标；（xi,yi,1）为第i个点的像平面坐标；mij为姿态矩阵M的第i行第j列元素，f为相机的焦距。

用xˉ表示(xi,yi)T，矢量xˉ′表示齐次坐标(xi,yi,1)T，矢 量Xˉ表 示(xWi,yWi,zWi)T，矢 量Xˉ′表 示 齐 次 坐 标(xWi,yWi,zWi,1)T，式（6）可以表示为：

将式（7）两边同除以zCi就可得到共线方程：

基于机器视觉农机多智体位姿确定的基本方程，mij(i=1,2,3;j=1,2,3)表示相对姿态，ti(i=1,2,3)表示相对位置。

2.2 农机多智体机器视觉相对位姿确定模型

式（8）中，独立的参数有6个：3个旋转和3个平移参数。传统的Hall算法将mij(i=1,2,3;j=1,2,3)和ti(i=1,2,3)全部作为参数，这样就有12个参数，只有当观测点数n≥6时，方程才有解。引入四元数算法，利用式（4）将mij(i=1,2,3;j=1,2,3)换为四元数的元素，则由Hall算法的12个参数减少为7个参数。令：

则式（8）可表示为：

将上式非线性函数按泰勒级数展开，取小值一次项得到线性化方程：

式中，Fx0、Fy0是将q0,q1,q2,q3,t1,t2,t3的初始值代入式（10）所得，ΔFx、ΔFy的表达式为：

式中，Δq0,Δq1,Δq2,Δq3,Δt1,Δt2,Δt3为q0,q1,q2,q3,t1,t2,t3初始值的相应改正值。式（11）、式（12）解算必须要进行反复迭代趋近，直至改正数ΔFx、ΔFy小于某一允许值为止。获得的参数减少，但当观测点的个数n＞4时，应按最小二乘原理解算参数，参数的最小二乘解为：

为减少观测点数量和解算参数，先通过采用基于四元数的最小二乘法求解农机多智体相对位置和姿态参数，接着利用求差法计算农机智能体和相机的相对位置，再根据四元数和农机智能体坐标系的转换关系求得相对姿态，将得到的数据作为真实值，采用式（10）计算15个特征点位的像平面坐标，并给这些点位的像平面坐标加上白噪声误差作为模拟观测值，最后利用式（11）和式（12）计算主动农机智能体和被动农机智能体的相对位置和姿态，可以进行农机多智体的相对位姿确定。

3 地面实物仿真与分析

3.1 数据处理结果

将目标架设在机器视觉云台MV-5959上，通过计算机串口实现对目标的运动控制，目标由图2所示，设置了15个特征点位、1个相机三脚架，计算机由云台控制计算机和一台专门的数据处理计算机（数据的离线处理，其参数为Intel Celeron Dual-Core T3000 1.8 G，2.0 GB内存）。MV-5959云台上的目标设置特征线，并给随动云台跟踪系统MV-5959设置一定的运动规律，利用云台系统自带的控制软件控制云台水平匀速转动8 000步。同时，在预先定位好的三脚架上架设专业单目视CCD相机（MVVEM500SM最高分辨率为640×480；焦距为27 mm），并对云台目标进行跟踪拍照，每1秒记录1次数据，产生一系列CCD观测影像。最后，对采集的数据进行离线处理，进行农机主动智能体相机与被动智能体目标之间的相对位置和姿态的确定。

图2 MV-5959云台上的目标特征点

1）目标坐标系的建立及目标特征点的精确量测。以MV-5959底座中心为坐标原点OW，xW轴平行于目标特征点11和12的连线，zW轴平行于特征点1方向垂直向上，yW垂直于xWOWzW平面，并与xW、zW轴组成左手坐标系。经测量得到在云台旋转0、4 000、8 000步时，特征标记十字中心在目标坐标系中的坐标见表1。

表1 y=0，x=0、4 000、8 000步目标坐标系中特征点坐标

2）基于四元数的视觉导航位姿确定模型计算目标和相机的相对位置和姿态关系。初始值位置参数为：平移矢量t=［0 0 7.269m］T，初始值位置参数为：q=［1 0 0 0］T，计算相对导航算法中误差m0量级达到10-4，其结果如图3、图4所示。

图3 农机多智体视觉导航算法各时刻中误差

图4 基于视觉导航的农机多智体导航算法计算的相对位姿成果

3.2 结果与分析

在仿真时间内，计算中误差上下变化区间不大。随着仿真时间的增加，相对姿态均呈下滑趋势。同时，x、y轴相对坐标有一定的上下浮动，总体来看均呈下滑趋向。而z轴相对坐标逐渐升高。可以看出农机多智体视觉导航模型进行位置和姿态确定时，计算误差在允许的范围内，并且误差在很小的范围（10-4）内，说明该模型高效、准确。

4 小结

通过采用基于四元数的视觉导航位姿确定模型，进行农机多智能体相对位姿估计，计算量小，并且减少了迭代次数，提高了相对位置姿态的计算效率和计算精度，能够满足农机自主相对导航精准性和实时性的要求。该算法模型为中国农田作业的农机自动化耕种、收割、施肥、灌溉等工作的精准化理论研究提供一定的参考价值。