取最优解；步骤S4、剔除导航过程中出现的粗大误差，进而确定农用无人 机的导航数据，所述导航数据为无人机的位姿参数。步骤S2求取农用无人机位姿参数的过 程如下：步骤321、建立世界坐标系和局部坐标系，标定匹配直线1^、1^1^(11>3);步骤522、 从图像中检测出匹配直线的对应投影h、V"In(n>3);步骤S23、建立相机坐标系，描述投 影直线的图像坐标；步骤S24、根据地面匹配直线与像平面上投影直线间的几何关系，建立 约束方程；步骤S25、求解约束方程得到农用无人机的位姿参数初值。
[0043] 为了获得更为稳定的导航结果，所述步骤S4中剔除粗大误差，采用3 〇准则去除 粗大误差后的结果，先假设导航得到的一组位姿参数数据只含有随机误差，对其进行计算 处理得到标准差以3〇为误差区间范围，超过此范围的为粗大误差并予以剔除，即若当 前时刻位姿参数值超过此误差范围，则当前时刻位姿参数值使用上一时刻的位姿参数值替 代。
[0044] 1)本实施例中，对图像中的直线纹理特征提取与匹配时，首先采用SIFT特征点尺 度空间的思想，将原始图像灰度化，为了保证能够在不同缩放比例下对边缘信息进行提取， 采用连续降采样获取图像金字塔。对各层图像利用高斯滤波滤除噪声，然后采用索贝尔算 子包含的横向及纵向两组3X3的矩阵，与图像作平面卷积，得到横向和纵向的灰度差分近 似值，然后通过判断像素梯度变化方向并进行相邻关键像素的连接，进而对不同尺度下的 图像进行边缘提取，此方法比传统的canny边缘提取算法计算速度快10%，且提取的边缘 具有单像素特性、连续性，且运行速度很快，便于直线特征符的描述。如图3-1和3-2所示， 为对基准图像与航拍图像进行多尺度边缘提取并进行直线最小二乘拟合的效果，从图中可 以看出，图像中的大多数直线纹理都被较好地提取出来。
[0045] 提取直线特征之后，根据尺度不变特征点描述符的匹配思想，对提取出的直线进 行局部特征的描述，并利用最近邻距离比率的方法进行匹配，为使特征描述符具有良好的 独特性，采用的直线描述符不仅包含直线本身，也包括直线邻域的有关像素信息。首先对不 同直线的描述符在搜索空间中进行层次划分，导出一个索引结构，进而快速寻找到一个给 定特征的邻近特征，在欧式空间中进行快速匹配。广泛使用的索引结构是多维搜索树，这 里采用K-D树最近邻搜索算法的改进：BBF(Best-Bin-First)查询算法，此算法将"查询路 径"上的节点进行排序，如按各自分割超平面与查询点的距离排序，能够确保优先检索包含 最近邻点可能性较高的空间。采用BBF查询机制后，K-D树便可以有效的扩展到高维数据 集上。利用该搜索算法可快速找到最邻近以及次邻近的直线描述符。
[0046] 设待匹配特征描述符为LBDA，其最邻近特征为LBDb，次邻近特征为LBD。，那么判断 该特征匹配的条件为
V，其中N为比例阈值，本实施例中N取0.6。如图 4所示，对航拍农田图像与基准图像进行直线匹配的结果可以看出，由于对直线特征较为详 细的描述，达到很好的匹配效果。并且利用该直线描述符可以对提取的直线特征进行不同 缩放、旋转的正确匹配，为利用直线导航奠定了良好的基础。
[0047] 2)对图像中的直线特征提取并匹配后，标定匹配出的直线，根据匹配结果求取农 用无人机的位姿参数，所述位姿参数包括旋转矩阵ITW和平移向量T。
[0048] 标定匹配直线、求取农用无人机位姿参数时，首先建立坐标系，如图2所示，图2中 左边为相机坐标系和局部坐标系的关系图，右边为局部坐标系和世界坐标系的关系图。实 施例中，以东北天坐标系作为世界坐标系〇wXwYwZw，以相机光心为相机坐标系原点0%相机坐 标系F轴方向为相机光轴方向，并指向像平面外，像平面的行和列平行且方向相同的方向 为r轴和f轴，建立相机坐标系crrrr。为方便相机在世界坐标系中姿态的计算，还需要 建立局部坐标系，设由相机采集并匹配得到的空间直线记为1^(i= 1，2, 3…n)以及在像平 面上的投影直线记为Mi= 1，2, 3…n)。为了减少噪声的影响，在像平面上选取最长的一 条直线记为1:，对应的地面空间直线为Q。设平面是相机坐标系的坐标原点(T和直线 Li组成的平面，称为投影平面，其法向量为ni。为方便相机在世界坐标系中姿态的计算，并 考虑到减少噪声的干扰，选取直线1^作为局部坐标系Xm轴，地平面内与Xm轴垂直且指向的 右方向作为局部坐标系Ym轴，局部坐标系Zm轴与世界坐标系的Zm轴方向一致，从而建立局 部坐标系〇mXmYmZm，利用农田种植基地地势较为平缓的特点，将直线的空间坐标近似为平面 坐标值，降低了计算的复杂度，本实施例中，由于农田直线特征多为平面直线，本导航方法 将近似于平面的直线按平面算法处理。
[0049] 3)获得直线的世界坐标值及对应投影的图像坐标值，根据地面直线与在像平面 上投影直线间的几何关系，建立约束方程。直线1^的单位方向向量在世界坐标系下记为 <=[<1Of，设其与轴(TXW的夹角为a，直线的方向向量在世界坐标系中的坐标值 <转化成局部坐标系中的坐标值< 的旋转矩阵记为W根据直线Q作为局部坐标系的Xm 轴这一特点，向量vr与向量存在如下关系< =oc)T=[i0 0r，如果氧?的取值记 为，必须满足以下条件：向量ejpe2是一对单位正交向量；向量e挪e2满足 ，又因为< 是围绕世界坐标系Z"的旋转，所以芊
3旋 转矩阵夏 =同样满足< =(C)T ?<，实现其他直线方向向量的世界坐标向局部坐标转换。
[0050] 像平面上投影直线1:的端点在相机坐标系下记为腐,=[感兑, ，平面ni的法向量在ni可由pls，plf；叉乘获得，即nplsXplf；，因 为直线1^位于平面n ，直线Li的方向向量与平面ni的法向量正交，并将相机坐 标系转换为局部坐标系旋转矩阵记为《。实现h从相机坐标值向局部坐标系的转 化《f为局部坐标，其中，及纟为局部坐标系到相机坐标系的旋转矩 阵，彳为直线Li在局部坐标系下的方向向量，且满足条件,由以上限制条 件，可以将疋参数化为尾其中，r为第二列为叫的单位正交矩阵，Rx(0)表示围绕0mXm轴旋转0角度的旋转矩阵，Ry(y)表示围绕0T1轴旋转y角 度的旋转矩阵，及=[%xhxv^),%x，向量ni已知量，而向量vf为在直线 提取匹配后获得，那么 <也为已知量，记相机坐标系转化成世界坐标系的旋转矩阵 巧=疋101=1&(外足(外(0'只需将角度0和丫求出，即可得到旋转矩阵尺:。
[0051] 4)求取位姿参数时，在像平面上选取投影直线长度仅次于h的直线记为1 2，对应 的空间直线为L2，记除LpL2外的n-2条直线为1^(1 = 3,4~n)，同样为了减少像平面上 像素噪声对直线向量的影响，选取L2作为在求解方程中的辅助线，设L2和Li的方向向量在 局部坐标系下表示为fC(卜3,4,…11) 5 Lji= 1，2, 3*"n)与相机的坐标原点构成平面nji= 1，2, 3*"n)，记n;单位法向量为n; = [X' iy' lZ'i]T，除直线L#，剩余n-1条直线，将直线1^2分别和L3、L4*"Ln组合，共 构成n-2组直线对，以直线LjPL3组为第一组，获得约束方程组
丨向量vf 和m的坐标表示代入上式得至I
[0052] 其中，
[0053] 、_ 0 i. .i.
- '
[0054] 求解上式得到《
、，利用三角函 数关系cos2y+sin2y= 1消去未知参数y，得到仅含未知参数0的一元方程 (〇 2 〇 6- 〇 3 〇 5) 2+ ( 〇 3 〇 4- 〇 丄 〇 6) 2= ( 〇 丄 〇 5- 〇 2 〇 4)2,将三角函数关系 cos2 0 = 1-sin2 员 代入式，记r=sinP，得 /1(r)=fl14r4+fl13,3+o12r2+o11r+ <%=0,其中a10、an…a14是 由〇,(j= 1，2…6)构成的多项式。与第一组直线的计算方法同理，分别对其他n-3 组直线对列写约束方程组，共得到n-2个关于r的一元方程，将n-2个方程联立得到
，其中k= 0, 1，2, 3, 4对方程中各等式两边平方并相加得到 F(厂)=5rrl2(r)=0，求导得到关于r的7阶方程式，(r)=IL2x(r)<f(r)二0，可以从中解 得7个r根，设其中存在mi (mi< 7)个实数。为了判断所求的是极大或极小值点，需要对上 式冲扣^^彳㈧^求二阶导数"卜卜^^你如切+义仍乃小利用极小值判定定理 F" (r)彡0,从求出的叫个实数中选出mXnii)个极小值，因此求出m2个sinP的候选 值，接下来就可以利用sinP的候选值求出y值及旋转矩阵t的候选值。
[0055] 根据法向量ni与相