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

权利要求书

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