应平面内的向量都垂直，可以得到方程 j=U…/〇，其中，T为世界坐标系下农用无人机的平移向{\ni} -T) =o 量位置坐标，在直线1^上任取一点记为if，T为世界坐标系中相机的位置，将式
[0056] q=[cosysinyxcyczc 1]T，将仏奇异值分解如下[USV] =svd(Mt)，
[0057] 根据奇异值分解原理知，V的第6列向量V6与向量q线性相关，存在如下 关系V6=kq，其中k为常数，因为向量q的最后一个元素为1，所以k与向量V6中的 最后一个元素^66相等，S卩k=V66，所以有q=V6/V66，相应地得到：cosy=V16/V66， siBF-匕/仏，，所以r= 现在得到m^sinM矣选值，并得到对应 的Y三角函数值及平移向量T的解。
[0058] 5)求出旋转矩阵扣，和平移向量T的候选初值后，由于噪声干扰，旋转矩阵私并不 符合旋转矩阵为正交单位阵的基本要求，因此，在选取最精确位姿参数之前，应先对位姿参 数进行处理以满足旋转矩阵要求。
[0059] 首先在直线Q上任选一点，其世界坐标为if,并通过焉'^ =If-:(iff< _ 计算得到直线Q上离世界坐标系原点最近的一点if,利用已求出的馬:和T得到 相机坐标系下点的坐标关
<、疗在投影平面n,上投影 坐标为
,
卑}.校准，r=(\c/r，估计出规范 化的C和T'，在以下的叙述中和T=T'。
[0060] 将位姿参数规范化后，为了简化选取最优解的计算量，首先对％组旋转矩阵作初 步筛选，并设定旋转矩阵误差表示为：乓= fi下面是对连续2000次随机仿真 /-I. 试验结果的数据分析：
[0061] 2000次随机仿真试验共得到候选旋转矩阵7413个，对应得到的Ei值分布如下：
[0062] 表1-1候选旋转矩阵对应E值
[0063]
[0064] 选取的2000个最优旋转矩阵对应的Ei值分布如下表：
[0065]表1-2最优旋转矩阵对应已丨值
[0066]
[0067]由表中的数据分布可知，在筛选的过程中，将Ei大于102对应的候选旋转矩阵 较为合理。设经过筛选后剩余m3组位姿参数，将其代入式
得到叫组 if、牙值，并相应地得到叫组if和野％在投影平面上画出由野^和巧构成的直线记做Ui，设其投影为ri，LJPL'i偏离程度表示了位姿参数的准确度。将两直线之间的 偏差定义为E2，
其中1,表示像平面上投影直线1 ,的 长度，hls、hle分别表示1 的点到ri的距离，并将其作为评价位姿参数准确性的标准。 选出m3组位姿参数对应E2最小的一组，并根据相机只拍摄前方物体这一基本原则，只保 留r轴坐标分量为正时对应的和T值。利用ni应与if垂直的几何关系，进一步修正 ?二和T。假设祀、T与真实值分别相差AR和AT，直线上点if对应？1的真实值坐标为
求取其点乘的加权平方和最小值对应的AR和AT，作 为对和T的修正，.if)2 =0，其中《馮1啲倒数，用于优化估计过程中噪声对直 线影响的权重。
[0068]标定结果，以仿真环境为MatlabR2010a，CPU采用Intel(R)Core(TM)i5_323M，内 存4GB为例，假设农用无人机飞行高度为2000m，相机视场角45°，分辨率为1024X1024,焦 距为2. 5mm，每秒拍摄1帧图像。设田间道路宽度为10m，匹配直线的两个端点加上标准差 为9个像素的尚斯白噪声。
[0069] 农用无人机连续航拍过程中，采用蒙特卡洛模拟仿真方法，进行2000次随机仿真 实验并统计计算结果，进而获得无人机位姿参数最终导航数据。参考图5-1和图5-2,为经 过2000次随机仿真实验剔除粗大误差后位移误差和角度误差仿真结果图。在选取最优解 的过程中，由于某些角度的候选解与真实值十分接近，偶尔选错最优解而出现发散现象。考 虑到无人机短时间内位置、角度变化小，可以采用3〇准则剔除粗大误差。本实施例优选从 当前时刻算起，对前10次的位姿参数求标准差〇，并比较第11次的值与之前10次的平均 值之差与3 〇的大小，差值小于3 〇的在误差范围内，则第11次的位姿参数值保留，否则使 用第10次的位姿参数替代，依次对之后的数据采用此方法处理，最后取平均值作为最终的 导航数据。表2-U2-2为使用该导航方法仿真得到的旋转角度及平移向量的精度值，可以 看出该方法稳定性较强且导航精度较高。
[0070] 表2-1旋转角精度
[0071]
[0072] 表2-2平移向量精度
[0073]
[0074] 图6-1和图6-2为本发明导航方法导航精度与匹配直线条数的关系曲线图，图7 为本方法计算时间示意图。参考图8-1、图8-2,为本发明导航方法的导航精度与地面起伏 关系图，其中农用无人机飞行高度由高度计获取。
[0075] 以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任 何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等 效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质 对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
【主权项】
1. 一种基于直线匹配的农用无人机自主导航方法，其特征在于，包括如下步骤： 51、 对航拍的田间直线纹理特征进行提取、并与基准图像进行直线匹配， 所述基准图像为目标作业区的完整图像； 52、 根据上述匹配结果求取农用无人机的位姿参数，过程如下： 521、 建立世界坐标系和局部坐标系，标定匹配直线Li、L2. . .L"(n〉3); 522、 从图像中检测出匹配直线的对应投影li、I2. . .l"(n〉3); 523、 建立相机坐标系，描述直线在图像上的投影坐标； 524、 根据地面匹配直线与像平面上投影直线间的几何关系，建立约束方程； 525、 求解约束方程得到农用无人机的位姿参数候选初始解； 53、 对上述农用无人机的位姿参数候选初始解进行校正，并从中选取最优解； 54、 剔除连续导航过程中出现的粗大误差，进而确定农用无人机的最终导航数据。2. 根据权利要求1所述的基于直线匹配的农用无人机自主导航方法，其特征在于，所 述步骤S21中，建立局部坐标系时，选取像平面上最长的一条直线记为li，对应的空间直线 为Li，W直线Li作为局部坐标系的X"轴，W地平面内与X"轴垂直且指向的右方向作为局部 坐标系Y"轴，局部坐标系的Zm轴与世界坐标系的Z"轴重合。3. 根据权利要求1所述的基于直线匹配的农用无人机自主导航方法，其特征在于，所 述步骤S4中采用30准则剔除粗大误差，若当前时刻位姿参数值超过误差范围，则当前时 刻位姿参数值使用上一时刻的位姿参数值替代。4. 根据权利要求1所述的基于直线匹配的农用无人机自主导航方法，其特征在于，所 述步骤S1包括： 511、 对航拍图像进行高斯滤波及图像金字塔处理，对不同缩放比例下的图像进行边缘 提取； 512、 直线特征提取之后，利用尺度不变特征点描述符的匹配思想，对提取出的直线进 行局部特征的描述，并利用最近邻距离比率的方法进行匹配。5. 根据权利要求4所述的基于直线匹配的农用无人机自主导航方法，其特征在于，对 所述约束方程求解时采用最小二乘法和奇异值分解法。6. 根据权利要求5所述的基于直线匹配的农用无人机自主导航方法，其特征在于，所 述农用无人机的位姿参数包括旋转矩阵RI,和平移向量T。7. 根据权利要求6所述的基于直线匹配的农用无人机自主导航方法，其特征在于，所 述步骤S3中对农用无人机的位姿参数进行二次校正时，先设定位姿参数旋转矩阵R;;和平 移向量T与真实值相差AR和AT，并将AR和AT作为对无人机位姿参数的修正，根据修 正后的参数满足直线上的点与相应投影平面法向量的点乘加权求和为零，求AR和AT，进 而对位姿参数校正。
【专利摘要】本发明提出一种基于直线匹配的农用无人机自主导航方法，该方法首先对常见的田间直线纹理特征进行提取、匹配，得到相应的几何约束方程，然后使用最小二乘法和奇异值分解对方程求解，并对求出的位置姿态参数进行二次校正，最终得到农用无人机的位置、姿态。本发明的优点在于：结合农田地形特征，提出基于直线匹配对农用无人机导航的新思路，采用本方法导航具有较高实时性和准确性，并且精度高、噪声干扰小，具有广泛的应用前景。
【IPC分类】G01C21/00, G01C11/04
【公开号】CN105004337
【申请号】CN201510512916
【发明人】邵巍, 赵立业
【申请人】青岛科技大学
【公开日】2015年10月28日
【申请日】2015年8月19日