CN107067437B - 一种基于多视几何和光束法平差的无人机定位系统及方法 - Google Patents

Abstract

本发明属于航空摄影测量领域，具体涉及一种基于多视几何和光束法平差的无人机定位系统及方法。相片选取模块选取两张相片进行特征提取与匹配；根据匹配的特征点计算两个摄像机的相对旋转与平移；根据特征点匹配数量以及两摄像机的基线距离来判断是否将该图像对加入目标定位计算模块。采用多视几何的算法进行摄像机相对外参与特征点三维坐标的计算，计算结果作为非线性平差系统的初始值进行平差，得到平差之后的相对摄像机外参与特征点三维坐标。根据相片实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵。最终得到摄像机绝对坐标系下的外参与特征点的经纬度。根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

Description

一种基于多视几何和光束法平差的无人机定位系统及方法

技术领域

本发明主要属于航空摄影测量领域，具体涉及一种基于多视几何和光束法平差的无人机定位系统及方法。

背景技术

目前对于无人机目标定位主要通过前方交汇算法计算初始值，采用非线性平差算法来得到高精度目标位置。前方交汇算法是一种通过在连续图像序列上跟踪目标，并根据传感器提供的摄像机外参来计算目标三维坐标的算法；非线性平差算法是一种对空中三角测量中各个系统误差进行平均和消除从而得到更精确测量结果的算法。整个目标定位系统由前方交汇初始值计算、误差方程最优解求解以及迭代优化三个模块组成。误差方程最优解计算模块根据多视几何估算出来的飞机姿态角、载荷姿态角中的误差源设计模型，使得误差模型的误差值最小的误差值就是求取的系统误差。消除系统误差之后可以计算出更精确的目标坐标。迭代优化模块不断加入新的数据参与平差，得到全局最优的系统误差，使得目标精度得到进一步的提升。

常规目标定位算法非常依赖传感器的精度。传感器精度会直接影响初始值的精度，进而影响光束法平差的迭代次数以及最终的定位结果。随着无人机可见光载荷分辨率越来越高，图像质量越来越清晰。可以充分利用视觉信息，不使用传感器测姿进行初始值计算。这样可以有效减少误差来源，从而增加对误差的可控性。但将视觉信息运用到初始值计算需要进一步解决以下问题：(1)如何仅使用图像序列得 到无人机内外参数以及目标点坐标初始值；(2)如何实现快速目标定位；(3)如何剔除误差较大数据，从而增加算法的抗干扰能力等问题。

发明内容

基于上述问题，本发明提供了一种基于多视几何和光束法平差的无人机定位系统及方法，通过计算机视觉中多视几何和摄影测量中光束法平差相结合的方法实现的无人机高精度连续帧定位。

本发明是通过以下技术方案实现的：

一种基于多视几何和光束法平差的无人机定位系统，所述定位系统包括图像选取模块、多视几何模块、非线性平差模块、全局变换矩阵计算模块；

所述图像选取模块选取多相机系统拍摄的图像中符合要求的图像送入所述多视几何模块；

所述多视几何模块计算多相机系统中摄像机的相对外参与特征点三维坐标；

所述非线性平差模块以多视几何模块计算的结果作为初始值进行平差计算得到平差之后的相对摄像机外参与特征点三维坐标；

所述全局变换矩阵计算模块，根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

一种基于多视几何和光束法平差的无人机定位方法，所述方法包括以下步骤，

图像选取：选取多相机系统拍摄的图像中符合要求的图像；

多视几何计算：计算多相机系统中摄像机的相对外参与特征点三维坐标；

平差计算：以多视几何计算的结果作为初始值进行平差计算得到平差之后的相对摄像机外参与特征点三维坐标；

全局变换矩阵：根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

进一步地，所述图像选取具体为，在多相机系统采集的图像上进行特征点的提取，从已经匹配好的图像集中选取拍摄时间上距离刚采集图像最近的图像进行特征点匹配，如果匹配上的特征点数量大于阈值之后，采用双目视觉算法进行两摄像机之间相对旋转与平移的计算，根据旋转与平移计算两摄像机之间的基线，满足匹配数量足够多且基线足够长的条件。

进一步地，所述多视几何计算具体为，通过双目视觉算法得到两两摄像机之间的旋转与平移，通过匹配特征点，找到每两对摄像机之间公共的特征点，通过公共特征点将两对摄像机计算出来的点云变换到同一个相对坐标系下，将所有的点云及摄像机都变换到第一张摄像机的坐标系下，从而得到相对于第一张相片的所有摄像机外参及特征点三维坐标。

进一步地，所述平差计算具体为，通过分析误差源，并将误差源 带入到投影模型中计算重投影误差；采用非线性最小二乘算法将该误差达到最小；这些误差源和待求的目标点三维坐标作为未知数，进行使得误差函数最小的非线性方程的求解。

进一步地，

将3x3的旋转矩阵转换为四元数，采用了按照XYZ的顺序相乘的方式来计算四元数；

对于围绕单一轴向的旋转可以采用如下的公式转换为四元数：

<θ|1 0 0>：x轴旋转；

<θ|0 1 0>：y轴旋转；

<θ|0 0 1>：z轴旋转；

两个旋转矩阵的相乘可以变成两个四元数的叉乘，所以可以将多个旋转矩阵的累乘转换为多个四元数的叉乘，最终将旋转矩阵转化为四元数Q。

误差函数如下所示：

e＝|proj3d(getQ(fd，sys_err)，Pt-uv|²；

其中fd表示摄像机的外参，SYSerr表示待求的系统误差，Pt表示待平差的目标三维坐标，uv表示观测到的该目标点在图像中的像素坐标；函数getQ可以将系统误差加入到当前摄像机外参上，并采用上文提到的算法计算该旋转矩阵的四元数形式；proj3d函数是将Pt通过旋转四元数Q，投影到图像平面上；该函数的计算流程如下：

1.将Pt转换为四元数的形式：qPt＝<0.0|Pt.x Pt.y Pt.z>，

2.采用如下公式将Pt按照旋转四元数Q进行旋转，得到旋转后 的三维坐标rotatedPt：

rotatedPt＝Q^-1·qPt·Q；

3.将rotatedPt进行投影，得到投影之后的像素坐标:

uv′与uv距离的平方作为该组数据的误差，非线性最小二乘算法就是要将这个误差源降低。采用lm算法迭代优化这些参数，目的是为了让所有数据的误差和最小。在实际使用中，调用lm的数学库进行非线性最小二乘的优化。该函数的使用非常简单，仅需要将误差函数以callback的形式传入到lm函数中。为了防止lm算法优化到另外一个局部最小值点上，需要对每个系统误差以及三维坐标的范围进行一定的限制。在误差函数中对输入的每个变量的值进行判断，如果超出预设的范围就设置为边界值。一般根据经验为每个系统误差项设置一个误差范围，以弧度为单位。采用这样的方法可以得到系统误差，以及平差之后的目标点三维坐标。

进一步地，所述全局变换矩阵具体为，通过实测的摄像机GPS经纬度，计算出从第一张相片的相对坐标到真实经纬度坐标的变换矩阵；变换矩阵如下所示：

其中R₁…R₉为3x3旋转矩阵的9个元素；Tx,Ty,Tz为XYZ方向上 的平移；通过函数R将三维旋转角

θ，ω变换成3x3旋转矩阵；通过非线性最小二乘算法，计算出

θ，ω以及Tx,Ty,Tz使得下面的代价函数最小：

其中XcYcZc为第一张摄像机下的摄像机位置坐标；Xgps,Ygps,Zgps为摄像机位置的东北天坐标；通过非线性优化计

θ，ω角度以及平移，从而得到变换矩阵；使用计算出来的变换矩阵，将所有特征点通过计算出来的变换矩阵变换到东北天坐标系下，最终目标点的经纬度通过KNN算法找到最近邻的四个已经变换到东北天坐标系下的点通过差值得到。

本发明的有益技术效果：

(1)本发明提供了一种基于多视几何和航空摄影测量相结合的高精度目标定位技术，该技术解决了在微小型无人机上目标定位中的定位速度缓慢、无精确高程数据以及平差迭代过多等问题；

(2)本发明中通过多视几何算法计算精度相对较高目标点坐标以及摄像机外参，为平差提供一个较好的起始搜索值；

(3)本发明解决了没有激光、地面高程数据情况下较高精度初始值计算，达到减少优化迭代次数的目的；

(4)本发明排除了在飞机飞行过程中受到外界环境影响产生的误差较大数据对平差算法的干扰和影响。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。

实施例1

一种基于多视几何和光束法平差的无人机定位方法，所述方法包括以下步骤，

(1)图像选取：选取多相机系统拍摄的图像中符合要求的图像；

在多相机系统采集的图像上进行特征点的提取，从已经匹配好的图像集中选取拍摄时间上距离刚采集图像最近的图像进行特征点匹配，如果匹配上的特征点数量大于阈值之后，采用双目视觉算法进行两摄像机之间相对旋转与平移的计算，根据旋转与平移计算两摄像机之间的基线，满足匹配数量足够多且基线足够长的条件。

其中，特征点是利用SIFT(Scale-invariant feature transform)算法，该算法通过求一幅图中的特征点(interest points,or corner points)及其有关scale和orientation的描述得到子特征并进行图像特征点匹配；

所述阈值不是定值，可根据具体情况设定，本实施例中的设置的阈值为100对；

所述图像满足的条件也跟据不同情况可调整设置，本实施例中的设置为匹配特征对数在100对以上，基线夹角在1°到3°之间。

(2)多视几何计算：计算多相机系统中摄像机的相对外参与特征点三维坐标；

通过双目视觉算法得到两两摄像机之间的旋转与平移，通过匹配特征点，找到每两对摄像机之间公共的特征点，通过公共特征点将两对摄像机计算出来的点云变换到同一个相对坐标系下，将所有的点云及摄像机都变换到第一张摄像机的坐标系下，从而得到相对于第一张相片的所有摄像机外参及特征点三维坐标。

(3)平差计算：以多视几何计算的结果作为初始值进行平差计算得到平差之后的相对摄像机外参与特征点三维坐标；

所述平差计算具体为，通过分析误差源，并将误差源带入到投影模型中计算重投影误差，从而计算出使得重投影误差最小的误差，通过抵消该误差达到提升目标定位精度的效果；采用非线性最小二乘算法将该误差达到最小；这些误差项和待求的目标点三维坐标作为未知数，进行使得误差函数最小的非线性方程的求解，得到摄像机外参的误差，目标点三维坐标的误差。

将3x3的旋转矩阵转换为四元数形式，

误差函数如下所示：

e＝|proj3d(getQ(fd，sys_err)，Pt)-uv|²；

其中，fd表示摄像机的外参，SYSerr表示待求的系统误差，Pt表示待平差的目标三维坐标，uv表示观测到的该目标点在图像中的像素坐标；

函数getQ将系统误差加入到当前摄像机外参上，并计算该旋转矩阵的四元数形式；

proj3d函数是将Pt通过旋转四元数Q，投影到图像平面上；

proj3d函数的计算流程如下：

将Pt转换为四元数的形式：qPt＝<0.0|Pt.x Pt.y Pt.z>，

将Pt按照旋转四元数Q进行旋转，得到旋转后的三维坐标rotatedPt：

rotatedPt＝Q^-1·qPt·Q；

将rotatedPt进行投影，得到投影之后的像素坐标:

uv′与uv距离的平方作为该组数据的误差，

非线性最小二乘算法将这个误差源降低，采用lm算法迭代优化这些参数，目的是为了让所有数据的误差和最小。

采用了按照XYZ的顺序相乘的方式来计算四元数；

对于围绕单一轴向的旋转可以采用如下的公式转换为四元数：

<θ|1 0 0>：x轴旋转；

<θ|0 1 0>：y轴旋转；

<θ|0 0 1>：z轴旋转；

两个旋转矩阵的相乘可以变成两个四元数的叉乘，所以可以将多个旋转矩阵的累乘转换为多个四元数的叉乘，最终将旋转矩阵转化为四元数Q。

为了防止lm算法优化到另外一个局部最小值点上，需要对每个系统误差以及三维坐标的范围进行一定的限制，在误差函数中对输入的每个变量的值进行判断，如果超出预设的范围就设置为边界值。

(4)全局变换矩阵：根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

所述全局变换矩阵具体为，通过实测的摄像机GPS经纬度，计算出从第一张相片的相对坐标到真实经纬度坐标的变换矩阵；变换矩阵如下所示：

其中R1…R9为3x3旋转矩阵的9个元素；Tx,Ty,Tz为XYZ方向上的平移；通过函数R将三维旋转角

θ，ω变换成3x3旋转矩阵；通过非线性最小二乘算法，计算出θ，ω以及Tx,Ty,Tz使得下面的代价函数最小：

其中XcYcZc为第一张摄像机下的摄像机位置坐标；Xgps,Ygps,Zgps为摄像机位置的东北天坐标；通过非线性优化计算出合适的

θ，ω角度以及平移，从而得到变换矩阵；使用计算出来的变换矩阵，将所有特征点通过计算出来的变换矩阵变换到东北天坐标系下，最终目标点的经纬度通过KNN算法找到最近邻的四个已经变换到东北天坐标系下的点通过差值得到。

一种基于多视几何和光束法平差的无人机定位系统，所述定位系统包括图像选取模块、多视几何模块、非线性平差模块、全局变换矩阵计算模块；

所述图像选取模块选取多相机系统拍摄的图像中符合要求的图像送入所述多视几何模块；

所述多视几何模块计算多相机系统中摄像机的相对外参与特征点三维坐标；

所述非线性平差模块以多视几何模块计算的结果作为初始值进行平差计算得到平差之后的相对摄像机外参与特征点三维坐标；

所述全局变换矩阵计算模块，根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

本实施例中的无人机定位方法可应用于本实施例中的无人机定位系统，利用图像选取模块、多视几何模块、非线性平差模块、全局变换矩阵计算模块分别实现方法中图像选取、多视几何计算、非线性 平差计算、全局变换矩阵计算。

Claims (6)

1.一种基于多视几何和光束法平差的无人机定位方法，其特征在于，所述方法包括以下步骤，

图像选取：在多相机系统采集的图像上进行特征点的提取，从已经匹配好的图像集中选取拍摄时间上距离刚采集图像最近的图像进行特征点匹配，匹配上的特征点数量大于100对之后，采用双目视觉算法进行两摄像机之间相对旋转与平移的计算，根据旋转与平移计算两摄像机之间的基线，满足匹配特征对数在100对以上，基线夹角在1°到3°之间的条件；

多视几何计算：通过双目视觉算法得到两两摄像机之间的旋转与平移，通过匹配特征点，找到每两对摄像机之间公共的特征点，通过公共特征点将两对摄像机计算出来的点云变换到同一个相对坐标系下，将所有的点云及摄像机都变换到第一张摄像机的坐标系下，从而得到相对于第一张相片的所有摄像机外参及特征点三维坐标；

平差计算：通过分析误差源，并将误差源带入到投影模型中计算重投影误差；采用非线性最小二乘算法将该误差达到最小；这些误差源和待求的目标点三维坐标作为未知数，进行使得误差函数最小的非线性方程的求解；

全局变换矩阵：根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。

2.如权利要求1所述定位方法，其特征在于，

将3x3的旋转矩阵转换为四元数形式，

误差函数如下所示：

e＝|proj3d(getQ(fd,syserr),Pt)-uv|²

其中，fd表示摄像机的外参，SYSerr表示待求的系统误差，Pt表示待平差的目标三维坐标，uv表示观测到的该目标点在图像中的像素坐标；

函数getQ将系统误差加入到当前摄像机外参上，并计算该旋转矩阵的四元数形式；

proj3d函数是将Pt通过旋转四元数Q，投影到图像平面上；

proj3d函数的计算流程如下：

将Pt转换为四元数的形式：qPt＝<0.0|Pt.x Pt.y Pt.z>，

将Pt按照旋转四元数Q进行旋转，得到旋转后的三维坐标rotatedPt：

rotatedPt＝Q^-1·qPt·Q；

将rotatedPt进行投影，得到投影之后的像素坐标:

uv′与uv距离的平方作为该组数据的误差，

非线性最小二乘算法将这个误差源降低，采用lm算法迭代优化摄像机外参，目标点三维坐标参数使所有数据的误差和最小。

3.如权利要求2所述定位方法，其特征在于，

采用了按照XYZ的顺序相乘的方式来计算四元数；

对于围绕单一轴向的旋转可以采用如下的公式转换为四元数：

<θ|1 0 0>:x轴旋转；

<θ|0 1 0>:y轴旋转；

<θ|0 0 1>:z轴旋转；

两个旋转矩阵的相乘可以变成两个四元数的叉乘，将多个旋转矩阵的累乘转换为多个四元数的叉乘，最终将旋转矩阵转化为四元数Q。

4.如权利要求2所述定位方法，其特征在于，

为了防止lm算法优化到另外一个局部最小值点上，需要对每个系统误差以及三维坐标的范围进行一定的限制，在误差函数中对输入的每个变量的值进行判断，如果超出预设的范围就设置为边界值。

5.如权利要求1所述定位方法，其特征在于，

所述全局变换矩阵具体为，通过实测的摄像机GPS经纬度，计算出从第一张相片的相对坐标到真实经纬度坐标的变换矩阵；变换矩阵如下所示：

其中R₁…R₉为3x3旋转矩阵的9个元素；Tx,Ty,Tz为X、Y、Z方向上的平移；通过函数R将三维旋转角

θ，ω变换成3x3旋转矩阵；通过非线性最小二乘算法，计算出

θ，ω以及Tx,Ty,Tz使得代价函数最小，所述代价函数为：

其中，XcYcZc为第一张摄像机下的摄像机位置坐标；Xgps,Ygps,Zgps为摄像机位置的东北天坐标；通过非线性优化计算出合适的

θ，ω角度以及平移，从而得到变换矩阵；使用计算出来的变换矩阵，将所有特征点通过计算出来的变换矩阵变换到东北天坐标系下，最终目标点的经纬度通过KNN算法找到最近邻的四个已经变换到东北天坐标系下的点通过差值得到。

6.一种基于多视几何和光束法平差的无人机定位系统，所述定位系统包括图像选取模块、多视几何模块、非线性平差模块、全局变换矩阵计算模块；

所述图像选取模块在多相机系统采集的图像上进行特征点的提取，从已经匹配好的图像集中选取拍摄时间上距离刚采集图像最近的图像进行特征点匹配，匹配上的特征点数量大于100对之后，采用双目视觉算法进行两摄像机之间相对旋转与平移的计算，根据旋转与平移计算两摄像机之间的基线，满足匹配特征对数在100对以上，基线夹角在1°到3°之间的条件，选取多相机系统拍摄的图像中符合要求的图像送入所述多视几何模块；

所述多视几何模块通过双目视觉算法得到两两摄像机之间的旋转与平移，通过匹配特征点，找到每两对摄像机之间公共的特征点，通过公共特征点将两对摄像机计算出来的点云变换到同一个相对坐标系下，将所有的点云及摄像机都变换到第一张摄像机的坐标系下，从而得到相对于第一张相片的所有摄像机外参及特征点三维坐标；

所述非线性平差模块通过分析误差源，并将误差源带入到投影模型中计算重投影误差；采用非线性最小二乘算法将该误差达到最小；这些误差源和待求的目标点三维坐标作为未知数，进行使得误差函数最小的非线性方程的求解，得到平差之后的相对摄像机外参与特征点三维坐标；

所述全局变换矩阵计算模块，根据每个图像实测的gps坐标，计算相对坐标系与经纬度坐标系的旋转平移矩阵，最终得到摄像机绝对坐标系下的外参与特征点的经纬度，根据距离目标点最近的4个特征点经纬度，经过差值，得到目标点经纬度。