首页> 中国专利> 基于直线匹配的农用无人机自主导航方法

基于直线匹配的农用无人机自主导航方法

页面导航

摘要
著录项
法律信息
说明书
相似文献

摘要

本发明提出一种基于直线匹配的农用无人机自主导航方法，该方法首先对常见的田间直线纹理特征进行提取、匹配，得到相应的几何约束方程，然后使用最小二乘法和奇异值分解对方程求解，并对求出的位置姿态参数进行二次校正，最终得到农用无人机的位置、姿态。本发明的优点在于：结合农田地形特征，提出基于直线匹配对农用无人机导航的新思路，采用本方法导航具有较高实时性和准确性，并且精度高、噪声干扰小，具有广泛的应用前景。

著录项

公开/公告号CN105004337A

专利类型发明专利
公开/公告日2015-10-28

原文格式PDF
申请/专利权人青岛科技大学;
展开▼

申请/专利号CN201510512916.X
发明设计人邵巍;赵立业;
展开▼

申请日2015-08-19
分类号G01C21/00(20060101);G01C11/04(20060101);
代理机构青岛致嘉知识产权代理事务所;
代理人刘晓
地址 266061 山东省青岛市崂山区松岭路99号
入库时间 2023-12-18 11:33:29

法律信息

法律状态公告日

法律状态信息

法律状态
2019-06-18

专利权的转移 IPC(主分类):G01C21/00 登记生效日:20190529 变更前: 变更后: 申请日:20150819

专利申请权、专利权的转移
2017-12-08

授权

授权
2015-11-25

实质审查的生效 IPC(主分类):G01C21/00 申请日:20150819

实质审查的生效
2015-10-28

公开

公开

说明书

技术领域

本发明属于无人机导航领域，尤其涉及一种基于直线匹配的农用无人机自主导航方法。

背景技术

随着现代化农业技术的发展，农用无人机的应用无疑是跨进高科技农业时代的重要标志，现在农用无人机越来越多地应用于喷洒农药、施肥等方面，农用无人机的出现节省了大量的人力劳动，而导航方法的精确性直接影响农用无人机工作效率，因此找到一种精确度高、噪声影响小的导航方法十分必要。

目前，较常使用的导航方法主要有惯性导航、无线电导航以及卫星导航等，这些导航技术应用在工作在山区、丘陵等地区的农用无人机都存在一定的局限性。申请号为【201410128459.X】的发明专利公开一种基于景象匹配/视觉里程的惯性组合导航方法，根据视觉里程原理，计算无人机航拍实时图像序列的单应矩阵，通过累积连续两帧实时图之间的相对位移，递推计算出无人机的当前位置；由于导航信息经积分产生，定位误差随时间增长，误差积累较大导致定位精度随时间降低，并且要对误差进行修正，导致高精度的惯导系统质量高、体积大、造价昂贵，不适用于农用无人机的导航。

使用无线电导航发送无线电波实现导航时，需要建立地面基站来辅助导航，只有地面基站正常工作时，无线电导航系统才能正常工作，在山区、盆地或丘陵地区易受地形遮挡，容易产生信号干扰，卫星导航方法的传输信号易被山峰阻挡导致导航精度降低，不适用于有遮挡物的山区。

利用计算机视觉进行无人机的自主导航成为近年的研究热点，该方法具有精度高，不受电子干扰等特点。申请号为【201410596775.X】公开一种无人机视觉导航方法，首先利用SIFT算法构建图像的多尺度空间，然后精确定位特征点的位置，根据图像中提取的最稳定特征点采用最近/次近邻距离匹配方法对特征点进行匹配，利用已经匹配的特征点对求解基础矩阵再进一步解算本质矩阵从而求解了无人机的姿态变换，此类方法是基于搜寻像素多尺度局部信息极值，并形成特征点描述符从而进行匹配的，对一定范围内的缩放、光照、尺度变化具有一定的鲁棒性，但是不适用于农用无人机导航，由于农田纹理特征极为相似、大尺度的缩放和角度变化时容易出现误匹配，另一方面，这些特征点提取算法能够在每幅图像得到几百甚至上千个特征点，计算量大，实时性难以保证，并且不利于进行地理位置等信息的直观表述。因此，利用点特征等信息进行农用无人机的自主导航存在诸多局限性。

为此，亟待提出一种针对农田地形特征，实现精确、实时导航的方法。

发明内容

本发明的目的是针对上述导航方法导航精度低、易受信号干扰，计算量大、实时性难以保证等技术问题提出的一种基于直线匹配的农用无人机自主导航方法，基于农田中因作物分垄形成的纹理丰富、田间道路明显，并利用这些纹理特征便于提取出直线的特点进行导航。

为了达到上述目的，本发明提出一种基于直线匹配的农用无人机自主导航方法，主要包括以下步骤：

S1、对航拍的田间直线纹理特征进行提取、并与基准图像进行直线匹配，所述基准图像为目标作业区的完整图像；

S2、根据上述匹配结果求取农用无人机的位姿参数，过程如下：

S21、建立世界坐标系和局部坐标系，标定匹配直线L₁、L₂…L_n(n>3)；

S22、从图像中检测出匹配直线的对应投影l₁、l₂…l_n(n>3)；

S23、建立相机坐标系，描述投影直线的图像坐标；

S24、根据地面匹配直线与像平面上投影直线间的几何关系，建立约束方程；

S25、求解约束方程得到农用无人机的位姿参数候选初始解；

S3、对上述农用无人机的位姿参数候选初始解进行校正，并选取最优解；

S4、剔除导航过程中出现的粗大误差，进而确定农用无人机的导航数据，所述导航数据为农用无人机的位姿参数终值。

作为优选，所述步骤S21中建立局部坐标系时，为了减少噪声的影响，选取像平面上内最长的一条直线记为l₁，对应的空间直线为L₁，以直线L₁作为局部坐标系的X^m轴，以地平面内与X^m轴垂直且指向的右方向作为局部坐标系Y^m轴，利用匹配直线近似位于同一地平面的特点，建立的局部坐标系Z^m轴与世界坐标系的Z^w轴方向一致，将所有局部坐标值在Z^m轴上的分量近似为零，降低了计算的复杂度，提高实时性。

作为优选，为了取得更为稳定可靠的导航结果，利用无人机在飞行的过程中，无论在角度上还是方向位移上不会在极短时间内发生极大的变化特点，所述步骤S4中剔除粗大误差采用3σ准则去除产生粗大误差的结果，若当前时刻位姿参数值超过误差范围，则当前时刻位姿参数值使用上一时刻的位姿参数值替代。

作为优选，所述步骤S1包括：S11、对航拍图像进行高斯滤波及图像金字塔处理，对不同尺度下的图像进行边缘提取，提取的边缘具有单像素特性，便于直线特征符的描述；S12、直线特征提取之后，利用尺度不变特征点描述符的匹配思想，对提取出的直线进行局部特征描述，并利用最近邻距离比率的方法进行匹配，该描述符对图像的缩放、旋转、光照变化都具有鲁棒性。

作为优选，对所述约束方程求解时采用最小二乘法和奇异值分解法，最小二乘法通过将误差最小化，实现最大程度地拟合出真实数据的走向，提高计算精度，奇异值分解原理中不涉及复数，计算简单，速度快。

作为优选，所述农用无人机的位姿参数包括旋转矩阵和平移向量T。

作为优选，所述步骤S3中对农用无人机的位姿参数进行校正时，先设定求得的旋转矩阵和平移向量T与真实值相差ΔR和ΔT，将ΔR和ΔT作为对农用无人机位姿参数的修正，根据修正后的参数满足直线上的点与相应投影平面的法向量的点乘加权求和为零，反求ΔR和ΔT，进而对位姿参数校正。

与现有技术相比，本发明的优点和积极效果在于：

1、根据田间作物分垄以及农田中道路形成的直线纹理清晰丰富等特征，提出根据直线匹配进行农用无人机导航的新思路，受外界环境约束较少，且不受电子信号干扰，该方法适用于任何位置关系的直线。

2、通过对直线纹理特征的提取与匹配，并基于平面直线特征建立局部坐标系，使z轴方向的坐标值近似为零，降低了计算的复杂度，计算速度快、实时性好，对地面高度变化的鲁棒性好。

3、利用3σ准则去除导航过程中产生的粗大误差，获得更为稳定可靠的导航结果，提高了导航精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为实施例中基于直线匹配的农用无人机自主导航方法流程图；

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

图3-1为基准图像直线提取结果示意图；

图3-2为无人机导航过程中拍摄图像的直线提取结果示意图；

图4为直线匹配结果示意图；

图5-1为剔除粗大误差后位移误差仿真结果图；

图5-2为剔除粗大误差后角度误差仿真结果图；

图6-1为实施例中导航方法位移误差与直线条数的关系曲线图；

图6-2为实施例中导航方法角度误差与直线条数的关系曲线图；

图7为实施例中导航方法计算时间示意图；

图8-1为实施例中导航方法位移误差与地面起伏的关系图；

图8-2为实施例中导航方法角度误差与地面起伏的关系图。

具体实施方式

本发明提出一种基于直线匹配的农用无人机自主导航方法，下面结合附图详细说明本发明的具体实施方式。

如图1所示，为本实施例提供的基于直线匹配的农用无人机自主导航方法流程图，基于直线匹配的农用无人机自主导航方法主要包括：步骤S1、对航拍的田间直线纹理特征进行提取、并与基准图像进行直线匹配，所述基准图像为目标作业区的完整图像；步骤S2、根据上述匹配结果求取农用无人机的位姿参数；步骤S3、对上述农用无人机的位姿参数进行校正，并选取最优解；步骤 S4、剔除导航过程中出现的粗大误差，进而确定农用无人机的导航数据，所述导航数据为无人机的位姿参数。步骤S2求取农用无人机位姿参数的过程如下：步骤S21、建立世界坐标系和局部坐标系，标定匹配直线L₁、L₂…L_n(n>3)；步骤S22、从图像中检测出匹配直线的对应投影l₁、l₂…ln(n>3)；步骤S23、建立相机坐标系，描述投影直线的图像坐标；步骤S24、根据地面匹配直线与像平面上投影直线间的几何关系，建立约束方程；步骤S25、求解约束方程得到农用无人机的位姿参数初值。

为了获得更为稳定的导航结果，所述步骤S4中剔除粗大误差，采用3σ 准则去除粗大误差后的结果，先假设导航得到的一组位姿参数数据只含有随机误差，对其进行计算处理得到标准差σ，以3σ为误差区间范围，超过此范围的为粗大误差并予以剔除，即若当前时刻位姿参数值超过此误差范围，则当前时刻位姿参数值使用上一时刻的位姿参数值替代。

1)本实施例中，对图像中的直线纹理特征提取与匹配时，首先采用SIFT 特征点尺度空间的思想，将原始图像灰度化，为了保证能够在不同缩放比例下对边缘信息进行提取，采用连续降采样获取图像金字塔。对各层图像利用高斯滤波滤除噪声，然后采用索贝尔算子包含的横向及纵向两组3×3的矩阵，与图像作平面卷积，得到横向和纵向的灰度差分近似值，然后通过判断像素梯度变化方向并进行相邻关键像素的连接，进而对不同尺度下的图像进行边缘提取，此方法比传统的canny边缘提取算法计算速度快10％，且提取的边缘具有单像素特性、连续性，且运行速度很快，便于直线特征符的描述。如图3-1和3-2 所示，为对基准图像与航拍图像进行多尺度边缘提取并进行直线最小二乘拟合的效果，从图中可以看出，图像中的大多数直线纹理都被较好地提取出来。

提取直线特征之后，根据尺度不变特征点描述符的匹配思想，对提取出的直线进行局部特征的描述，并利用最近邻距离比率的方法进行匹配，为使特征描述符具有良好的独特性，采用的直线描述符不仅包含直线本身，也包括直线邻域的有关像素信息。首先对不同直线的描述符在搜索空间中进行层次划分，导出一个索引结构，进而快速寻找到一个给定特征的邻近特征，在欧式空间中进行快速匹配。广泛使用的索引结构是多维搜索树，这里采用K-D树最近邻搜索算法的改进：BBF(Best-Bin-First)查询算法，此算法将“查询路径”上的节点进行排序，如按各自分割超平面与查询点的距离排序，能够确保优先检索包含最近邻点可能性较高的空间。采用BBF查询机制后，K-D树便可以有效的扩展到高维数据集上。利用该搜索算法可快速找到最邻近以及次邻近的直线描述符。

设待匹配特征描述符为LBD_A，其最邻近特征为LBD_B，次邻近特征为LBD_c，那么判断该特征匹配的条件为：其中N为比例阈值，本实施例中N取0.6。如图4所示，对航拍农田图像与基准图像进行直线匹配的结果可以看出，由于对直线特征较为详细的描述，达到很好的匹配效果。并且利用该直线描述符可以对提取的直线特征进行不同缩放、旋转的正确匹配，为利用直线导航奠定了良好的基础。

2)对图像中的直线特征提取并匹配后，标定匹配出的直线，根据匹配结果求取农用无人机的位姿参数，所述位姿参数包括旋转矩阵R^c_w和平移向量T。

标定匹配直线、求取农用无人机位姿参数时，首先建立坐标系，如图2所示，图2中左边为相机坐标系和局部坐标系的关系图，右边为局部坐标系和世界坐标系的关系图。实施例中，以东北天坐标系作为世界坐标系O^wX^wY^wZ^w，以相机光心为相机坐标系原点O^c，相机坐标系Z^c轴方向为相机光轴方向，并指向像平面外，像平面的行和列平行且方向相同的方向为X^c轴和Y^c轴，建立相机坐标系O^cX^cY^cZ^c。为方便相机在世界坐标系中姿态的计算，还需要建立局部坐标系，设由相机采集并匹配得到的空间直线记为L_i(i＝1,2,3…n)以及在像平面上的投影直线记为l_i(i＝1,2,3…n)。为了减少噪声的影响，在像平面上选取最长的一条直线记为l₁，对应的地面空间直线为L₁。设平面Π₁是相机坐标系的坐标原点O^c和直线L₁组成的平面，称为投影平面，其法向量为n₁。为方便相机在世界坐标系中姿态的计算，并考虑到减少噪声的干扰，选取直线L₁作为局部坐标系X^m轴，地平面内与X^m轴垂直且指向的右方向作为局部坐标系Y^m轴，局部坐标系Z^m轴与世界坐标系的Z^m轴方向一致，从而建立局部坐标系O^mX^mY^mZ^m，利用农田种植基地地势较为平缓的特点，将直线的空间坐标近似为平面坐标值，降低了计算的复杂度，本实施例中，由于农田直线特征多为平面直线，本导航方法将近似于平面的直线按平面算法处理。

3)获得直线的世界坐标值及对应投影的图像坐标值，根据地面直线与在像平面上投影直线间的几何关系，建立约束方程。直线L₁的单位方向向量在世界坐标系下记为设其与轴O^wX^w的夹角为α，直线的方向向量在世界坐标系中的坐标值转化成局部坐标系中的坐标值的旋转矩阵记为根据直线L₁作为局部坐标系的X^m轴这一特点，向量与向量存在如下关系如果的取值记为必须满足以下条件：向量e₁和e₂是一对单位正交向量；向量e₁和e₂满足又因为是围绕世界坐标系Z^w的旋转，所以有 $R_{w}^{m} = (\begin{matrix} c o s α & - s i n α & 0 \\ s i n α & \cos α & 0 \\ 0 & 0 & 1 \end{matrix}),$ 旋转矩阵同样满足实现其他直线方向向量的世界坐标向局部坐标转换。

像平面上投影直线l₁的端点在相机坐标系下记为平面Π₁的法向量在n₁可由p_1s，p_1e叉乘获得，即n₁＝p_1s×p_1e，因为直线L₁位于平面Π₁上，直线L₁的方向向量与平面Π₁的法向量正交，并将相机坐标系转换为局部坐标系旋转矩阵记为实现n₁从相机坐标值向局部坐标系的转化为局部坐标，其中，为局部坐标系到相机坐标系的旋转矩阵，为直线L₁在局部坐标系下的方向向量，且满足条件由以上限制条件，可以将参数化为其中，R为第二列为n₁的单位正交矩阵，R_x(β)表示围绕O^mX^m轴旋转β角度的旋转矩阵，R_y(γ)表示围绕 O^mY^m轴旋转γ角度的旋转矩阵，向量n₁已知量，而向量为在直线提取匹配后获得，那么也为已知量，记相机坐标系转化成世界坐标系的旋转矩阵只需将角度β和γ求出，即可得到旋转矩阵

4)求取位姿参数时，在像平面上选取投影直线长度仅次于l₁的直线记为l₂，对应的空间直线为L₂，记除L₁、L₂外的n-2条直线为L_i(i＝3,4…n)，同样为了减少像平面上像素噪声对直线向量的影响，选取L₂作为在求解方程中的辅助线，设L₂和L_i的方向向量在局部坐标系下表示为L_i(i＝1,2,3…n)与相机的坐标原点构成平面 Π_i(i＝1,2,3…n)，记Π_i单位法向量为n_i＝[x′_i y′_i z′_i]^T，除直线L₁外，剩余n-1条直线，将直线L₂分别和L₃、L₄…L_n组合，共构成n-2组直线对，以直线L₂和L₃组为第一组，获得约束方程组 $(\begin{matrix} n_{2}^{T} \cdot R_{m}^{c} \cdot v_{2}^{m} = 0 \\ n_{3}^{T} \cdot R_{m}^{c} \cdot v_{3}^{m} = 0 \end{matrix}),$ 将向量和的坐标表示代入上式得到 $(\begin{matrix} σ_{1} c o s γ + σ_{2} s i n γ + σ_{3} = 0 \\ σ_{4} c o s γ + σ_{5} s i n γ + σ_{6} = 0 \end{matrix})$

其中，

$(\begin{matrix} σ_{1} = a_{2}^{m} x_{2}^{'} + b_{2}^{m} z_{2}^{'} s i n β \\ σ_{2} = b_{2}^{m} x_{2}^{'} - a_{2}^{m} z_{2}^{'} \\ σ_{3} = b_{2}^{m} y_{2}^{'} c o s β \\ σ_{4} = a_{i}^{m} x_{i}^{'} + b_{i}^{m} z_{i}^{'} s i n β \\ σ_{5} = b_{i}^{m} x_{i}^{'} - a_{i}^{m} z_{i}^{'} \\ σ_{6} = b_{i}^{m} y_{i}^{'} \cos β \end{matrix})$

求解上式得到 $c o s γ = \frac{σ_{2} σ_{6} - σ_{3} σ_{5}}{σ_{1} σ_{5} - σ_{2} σ_{4}}, s i n γ = \frac{σ_{3} σ_{4} - σ_{1} σ_{6}}{σ_{1} σ_{5} - σ_{2} σ_{4}},$ 利用三角函数关系 cos²γ+sin²γ＝1消去未知参数γ，得到仅含未知参数β的一元方程 (σ₂σ₆-σ₃σ₅)²+(σ₃σ₄-σ₁σ₆)²＝(σ₁σ₅-σ₂σ₄)²，将三角函数关系cos²β＝1-sin²β代入式，记 r＝sinβ，得其中a₁₀、a₁₁…a₁₄是由σ_j(j＝1,2…6) 构成的多项式。与第一组直线的计算方法同理，分别对其他n-3组直线对列写约束方程组，共得到n-2个关于r的一元方程，将n-2个方程联立得到 $(\begin{matrix} f_{1} (r) = Σ_{k = 0}^{4} a_{1 k} r^{k} = 0 \\ f_{2} (r) = Σ_{k = 0}^{4} a_{2 k} r^{k} = 0 \\ ... \\ f_{n - 2} (r) = Σ_{k = 0}^{4} a_{(n - 2) k} r^{k} = 0 \end{matrix}),$ 其中k＝0,1,2,3,4对方程中各等式两边平方并相加得到 $F (r) = Σ_{i = 1}^{n - 2} f_{i}^{2} (r) = 0,$ 求导得到关于r的7阶方程式 $F^{'} (r) = Σ_{i = 1}^{n - 2} f_{i} (r) f_{i}^{'} (r) = 0,$ 可以从中解得7个r根，设其中存在m₁(m₁≤7)个实数。为了判断所求的是极大或极小值点，需要对上式 $F (r) = Σ_{i = 1}^{n - 2} f_{i}^{2} (r) = 0$ 求二阶导数 $F^{''} (r) = Σ_{i = 1}^{n - 2} f_{i}^{'} (r) f_{i}^{'} (r) + f_{i} (r) f_{i}^{''} (r),$ 利用极小值判定定理F″(r)≥0，从求出的m₁个实数中选出m₂(m₂≤m₁)个极小值，因此求出m₂个sinβ的候选值，接下来就可以利用sinβ的候选值求出γ值及旋转矩阵的候选值。

根据法向量n_i与相应平面内的向量都垂直，可以得到方程 $(\begin{matrix} {(n_{i})}^{T} \cdot R_{m}^{c} \cdot v_{2}^{m} = 0 \\ {(n_{i})}^{T} \cdot R_{m}^{c} \cdot (P_{i}^{w} - T) = 0 \end{matrix}), (i = 1, 2 ... n),$ 其中，T为世界坐标系下农用无人机的平移向量位置坐标，在直线L_i上任取一点记为T为世界坐标系中相机的位置，将式及第t(t＝1,2…m₂)个sinβ值代入式 $(\begin{matrix} {(n_{i})}^{T} \cdot R_{m}^{c} \cdot v_{2}^{m} = 0 \\ {(n_{i})}^{T} \cdot R_{m}^{c} \cdot (P_{i}^{w} - T) = 0 \end{matrix}),$ 得到 $(\begin{matrix} {(n_{i})}^{T} \cdot R \cdot R_{y} (γ) \cdot R_{x} (β_{t}) \cdot R_{w}^{m} \cdot v_{i}^{w} = 0 \\ {(n_{i})}^{T} \cdot [R \cdot R_{y} (γ) \cdot R_{x} (β_{t}) \cdot R_{w}^{m} \cdot P_{i}^{w} - R \overline{T}] = 0 \end{matrix}), (t = 1, 2 ... m_{2}),$ 其中， $\overline{T} = R_{w}^{c} \cdot T = {(\begin{matrix} x^{c} & y^{c} & z^{c} \end{matrix})}^{T} .$ 设 $(\begin{matrix} R_{x} (β_{t}) \cdot R_{w}^{m} \cdot v_{i}^{w} = {(\begin{matrix} a_{i t} & b_{i t} & c_{i t} \end{matrix})}^{T}, & R_{x} (β_{t}) \cdot R_{w}^{m} \cdot P_{i}^{w} = {(\begin{matrix} x_{i t}^{m} & y_{i t}^{m} & z_{i t}^{m} \end{matrix})}^{T}, \end{matrix})$ $n_{i}^{T} \cdot R = {(\begin{matrix} {\overline{x}}_{i} & {\overline{y}}_{i} & {\overline{z}}_{i} \end{matrix})}^{T},$ 并代入式 $(\begin{matrix} {(n_{i})}^{T} \cdot R \cdot R_{y} (γ) \cdot R_{x} (β_{t}) \cdot R_{w}^{m} \cdot v_{i}^{w} = 0 \\ {(n_{i})}^{T} \cdot [R \cdot R_{y} (γ) \cdot R_{x} (β_{t}) \cdot R_{w}^{m} \cdot P_{i}^{w} - R \overline{T}] = 0 \end{matrix})$ 得到M_t·Q＝0，其中， $M_{t} = {(\begin{matrix} ... \\ {\overline{x}}_{i} a_{i t} + {\overline{y}}_{i} b_{i t} & {\overline{y}}_{i} a_{i t} - {\overline{x}}_{i} b_{i t} & 0 & 0 & 0 & {\overline{z}}_{i} c_{i t} \\ {\overline{x}}_{i} x_{i t}^{m} + {\overline{y}}_{i} y_{i t}^{m} & {\overline{y}}_{i} x_{i t}^{m} - {\overline{x}}_{i} y_{i t}^{m} & - x_{i}^{'} & - y_{i}^{'} & - z_{i}^{'} & {\overline{z}}_{i} z_{i t}^{m} \\ ... \end{matrix})}_{2n \times 6}$

q＝[cosγ sinγ x^c y^c z^c 1]^T，将M_t奇异值分解如下[U S V]＝svd(M_t)，

根据奇异值分解原理知，V的第6列向量V₆与向量q线性相关，存在如下关系V₆＝kq，其中k为常数，因为向量q的最后一个元素为1，所以k与向量V₆中的最后一个元素V₆₆相等，即k＝V₆₆，所以有q＝V₆/V₆₆，相应地得到：cosγ＝V₁₆/V₆₆，所以现在得到m₂个sinβ候选值，并得到对应的γ三角函数值及平移向量T的解。

5)求出旋转矩阵和平移向量T的候选初值后，由于噪声干扰，旋转矩阵并不符合旋转矩阵为正交单位阵的基本要求，因此，在选取最精确位姿参数之前，应先对位姿参数进行处理以满足旋转矩阵要求。

首先在直线L_i上任选一点，其世界坐标为并通过计算得到直线L_i上离世界坐标系原点最近的一点利用已求出的和T得到相机坐标系下点的坐标为 $(\begin{matrix} P_{i}^{c} = R_{m}^{c} \cdot (P_{i}^{w} - T) \\ {\overline{P}}_{i}^{c} = R_{m}^{c} \cdot ({\overline{P}}_{i}^{w} - T) \end{matrix}), P_{i}^{c}, {\overline{P}}_{i}^{c}$ 在投影平面Π_i上投影坐标为 $(\begin{matrix} P_{i}^{p} = P_{i}^{c} - (P_{i}^{c} \cdot n_{i}) \cdot n_{i} \\ {\overline{P}}_{i}^{p} = {\overline{P}}_{i}^{c} - ({\overline{P}}_{i}^{c} \cdot n_{i}) \cdot n_{i} \end{matrix}),$ 根据 $[P_{1}^{p}, P_{2}^{p} ... P_{i}^{p}] = [P_{1}^{w}, P_{2}^{w} ... P_{i}^{w}] \cdot R_{w}^{' c} + {\overline{T}}^{'},$ $[{\overline{P}}_{1}^{p}, {\overline{P}}_{2}^{p} ... {\overline{P}}_{i}^{p}] = [{\overline{P}}_{1}^{w}, {\overline{P}}_{2}^{w} ... {\overline{P}}_{i}^{w}] \cdot R_{w}^{' c} + {\overline{T}}^{'},$ 对点校准， $T^{'} = {(R_{w}^{' c})}^{T} {\overline{T}}^{'},$ 估计出规范化的和Τ′，在以下的叙述中和Τ＝Τ′。

将位姿参数规范化后，为了简化选取最优解的计算量，首先对m₂组旋转矩阵作初步筛选，并设定旋转矩阵误差表示为：下面是对连续 2000次随机仿真试验结果的数据分析：

2000次随机仿真试验共得到候选旋转矩阵7413个，对应得到的E₁值分布如下：

表1-1候选旋转矩阵对应E值

E1误差值小于10^-6(10^-6,10^-5] (10^-5,10^-4] (10^-4,10^-3] (10^-3,10^-2] 大于10^-2数量 300 466 1458 2774 1660 755 概率 4.0469％ 6.2863％ 19.6682％ 37.4207％ 22.3931％ 10.1848％

选取的2000个最优旋转矩阵对应的E₁值分布如下表：

表1-2最优旋转矩阵对应E₁值

E₁误差值小于10^-6(10^-6,10^-5] (10^-5,10^-4] (10^-4_,10^-3] (10^-3,10^-2] 大于10^-2数量 214 367 1028 354 37 0 概率 10.7％ 18.35％ 51.4％ 17.7％ 1.85％ 0

由表中的数据分布可知，在筛选的过程中，将E₁大于10^-2对应的候选旋转矩阵较为合理。设经过筛选后剩余m₃组位姿参数，将其代入式 $(\begin{matrix} P_{i}^{c} = R_{w}^{c} \cdot (P_{i}^{w} - T) \\ {\overline{P}}_{i}^{c} = R_{w}^{c} \cdot ({\overline{P}}_{i}^{w} - T) \end{matrix}),$ 得到m₃组值，并相应地得到m₃组和在投影平面上画出由和构成的直线记做L′_i，设其投影为l′_i，L_i和L′_i偏离程度表示了位姿参数的准确度。将两直线之间的偏差定义为E₂，其中l_i表示像平面上投影直线l_i的长度，h_is、h_ie分别表示l_i上的点到l′_i的距离，并将其作为评价位姿参数准确性的标准。选出m₃组位姿参数对应E₂最小的一组，并根据相机只拍摄前方物体这一基本原则，只保留Z^c轴坐标分量为正时对应的和T值。利用n_i应与垂直的几何关系，进一步修正和T。假设T与真实值分别相差ΔR和ΔT，直线上点对应P_i的真实值坐标为求取其点乘的加权平方和最小值对应的ΔR和ΔT，作为对和T的修正，其中ω_i是l_i的倒数，用于优化估计过程中噪声对直线影响的权重。

标定结果，以仿真环境为Matlab R2010a，CPU采用Intel(R)Core(TM) i5-323M，内存4GB为例，假设农用无人机飞行高度为2000m，相机视场角45°，分辨率为1024×1024，焦距为2.5mm，每秒拍摄1帧图像。设田间道路宽度为 10m，匹配直线的两个端点加上标准差为9个像素的高斯白噪声。

农用无人机连续航拍过程中，采用蒙特卡洛模拟仿真方法，进行2000次随机仿真实验并统计计算结果，进而获得无人机位姿参数最终导航数据。参考图5-1和图5-2，为经过2000次随机仿真实验剔除粗大误差后位移误差和角度误差仿真结果图。在选取最优解的过程中，由于某些角度的候选解与真实值十分接近，偶尔选错最优解而出现发散现象。考虑到无人机短时间内位置、角度变化小，可以采用3σ准则剔除粗大误差。本实施例优选从当前时刻算起，对前 10次的位姿参数求标准差σ，并比较第11次的值与之前10次的平均值之差与 3σ的大小，差值小于3σ的在误差范围内，则第11次的位姿参数值保留，否则使用第10次的位姿参数替代，依次对之后的数据采用此方法处理，最后取平均值作为最终的导航数据。表2-1、2-2为使用该导航方法仿真得到的旋转角度及平移向量的精度值，可以看出该方法稳定性较强且导航精度较高。

表2-1旋转角精度

表2-2平移向量精度

图6-1和图6-2为本发明导航方法导航精度与匹配直线条数的关系曲线图，图7为本方法计算时间示意图。参考图8-1、图8-2，为本发明导航方法的导航精度与地面起伏关系图，其中农用无人机飞行高度由高度计获取。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

去获取专利，查看全文>

相似文献

专利
中文文献
外文文献

1. 基于直线匹配的农用无人机自主导航方法 [P] . 中国专利： CN105004337B . 2017.12.08
2. 基于直线匹配的农用无人机自主导航方法 [P] . 中国专利： CN105004337A . 2015-10-28
3. SYSTEM AND METHOD FOR AUTONOMOUS NAVIGATION OF UNMANNED AERIAL VEHICLE (UAV) IN GPS DENIED ENVIRONMENT [P] . WO2021038485A1 . 2021-03-04

机译：在GPS拒绝环境中无人机（UAV）的自主导航系统和方法
4. An autonomous navigation system of unmanned surface vehicle and method thereof [P] . 韩国专利： KR102112098B1 . 2020-05-18

机译：无人机的自主导航系统及其方法
5. An autonomous navigation system of unmanned surface vehicle and method thereof [P] . 韩国专利： KR20190090251A . 2019-08-01

机译：无人机的自主导航系统及其方法