CN114387341A - 通过单张航拍观测图像计算飞行器六自由度位姿的方法 - Google Patents

Abstract

本发明公开了一种通过单张航拍观测图像计算飞行器六自由度位姿的方法，属于飞行器位姿估计领域，包括步骤：通过在单张飞行器航拍观测图像与卫星地图之间提取并匹配特征点，然后根据匹配结果计算飞行器的六自由度位姿，从而计算出飞行器的位置和姿态。本发明不但可以得到更精确的位置数据，也可以得到飞行器的精确旋转角度。

Description

通过单张航拍观测图像计算飞行器六自由度位姿的方法

技术领域

本发明涉及飞行器位姿估计领域，更为具体的，涉及一种通过单张航拍观测图像计算飞行器六自由度位姿的方法。

背景技术

飞行器视觉定位是飞行器通过机载相机的图像数据进行自身定位的技术，这项技术在GPS失效的情况下，对飞行器的安全作业具有十分重要的价值。大部分的飞行器视觉定位系统只能估计飞行器的相对运动，这在飞行器长时间飞行过程中会不断产生累积误差，导致定位误差越来越大。

发明内容

本发明的目的在于克服现有技术的不足，提供一种通过单张航拍观测图像计算飞行器六自由度位姿的方法，不但可以得到更精确的位置数据，也可以得到飞行器的精确旋转角度等。

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

一种通过单张航拍观测图像计算飞行器六自由度位姿的方法，包括步骤：通过在单张飞行器航拍观测图像与卫星地图之间提取并匹配特征点，然后根据匹配结果计算飞行器的六自由度位姿，从而计算出飞行器的位置和姿态。

进一步地，包括子步骤：

S1，根据视觉里程计或惯性测量单元计算出飞行器的第一位置，根据第一位置的GPS提取第一位置的卫星地图，然后分别提取并匹配卫星地图与观测图像的特征点；

S2，基于步骤S1得到飞行器观测图像与卫星地图之间的特征点匹配后，根据卫星地图上特征点的像素坐标

按照如下公式计算出卫星地图上每个特征点的经纬度坐标

其中，w和h分别是局部卫星地图图像的宽度和高度，C_lon和C_lat是局部卫星地图中心点像素的经度和纬度；ρ_lon和ρ_lat分别是卫星地图上每个像素的经度和纬度跨度；

S3，将特征点的经纬度坐标变换到地心坐标系下，再由地心坐标系变换到以飞行器起飞点为站点的东北天坐标系下，特征点的东北天坐标系坐标表示为：

其中，N是匹配特征点的数量；

S4，将旋转矩阵R和三维平移t作为待求解的未知量，得到卫星地图上特征点在相机坐标系下的坐标，表示为：

相机观测图像上的匹配特征点

通过相机内参矩阵K变换到相机坐标系下，表示为：

观测图像上的匹配特征点与相机中心的连线形成向量，向量

与

的方向相同，以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

进一步地，在步骤S4中，所述以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态包括子步骤：

是一个三自由度向量，存在两个与之正交的方向向量，这两个与之正交的方向向量存在于

的零空间中，则记为：

其中，r和s是与

相正交的两个方向向量，则存在：

在

与

的方向相同的条件下，则存在：

上式中，共包含R和t的12个未知量，每对匹配特征点提供两条等式约束，使用至少6对匹配特征点即能求解出相机坐标，然后采用一般求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

进一步地，在步骤S4中，包括子步骤：将旋转矩阵R设为：

进一步地，在步骤S1中，所述特征点包括SURF特征点。

进一步地，所述飞行器包括无人机。

本发明的有益效果是：

本发明提供一种方法，通过对飞行器观测图像和卫星地图之间进行特征点匹配，然后直接求解飞行器在真实世界坐标下的六自由度绝对位姿，该方法不但可以得到更精确的位置数据(经度、纬度、高度)，也可以得到飞行器的精确旋转角度(航向角、滚动角、俯仰角)。

附图说明

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

图1为本发明的方法原理示意图；

图中，1-机载相机坐标系，2-机载相机像平面，3-深度归一化平面，4-卫星地图点在像平面上的投影点，5-卫星地图点在像平面上的实际观测点，6-卫星地图点投影的方向向量，7-相机原点到观测点的方向向量，8-相机原点到观测点的方向向量7在深度归一化平面3上的第一切向量，9-相机原点到观测点的方向向量7在深度归一化平面3上的第二切向量，10-地面。

具体实施方式

本说明书中所有实施例公开的所有特征，或隐含公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合和/或扩展、替换。下面根据附图1，对本发明的技术构思、工作原理、功效和工作过程作进一步详细说明。

本发明提供的技术方案，只需利用单张飞行器(例如无人机)航拍时观测到的图像，通过在飞行器观测图像与卫星地图之间提取并匹配特征点，建立如图1所示的匹配关系，包括载相机坐标系1、机载相机像平面2、深度归一化平面3、卫星地图点在像平面上的投影点4、卫星地图点在像平面上的实际观测点5和卫星地图点投影的方向向量6。然后根据匹配结果，即可比较准确地计算出飞行器的位置和姿态。在具体实施过程中，可以计算飞行器在以起飞点为站点的东北天坐标系下的六自由度位姿，从而可以比较准确地计算出飞行器的位置和姿态。

在实际应用中，当飞行器飞行到一个新位置后，首先根据视觉里程计或IMU(惯性测量单元)推测出飞行器的大致位置(本发明第一位置)，根据大致位置的GPS，提取附近的卫星地图，然后分别提取并匹配卫星地图与观测图像的特征点(例如SURF特征点)。

在得到飞行器观测图像与卫星地图之间的特征点匹配后，本发明首先根据卫星地图上特征点的像素坐标

计算出卫星地图上每个特征点的经纬度坐标

计算方法为：

其中，w和h分别是局部卫星地图图像的宽度和高度，C_lon和C_lat是局部卫星地图中心点像素的经度和纬度；ρ_lon和ρ_lat分别是卫星地图上每个像素的经度和纬度跨度；

特征点的经纬度坐标可以变换到地心坐标系下，再由地心坐标系变换到以飞行器起飞点为站点的东北天坐标系下，这些特征点的东北天坐标系坐标表示为：

本发明在具体实施过程中，例如求解飞行器在东北天坐标系下的姿态，包括一个旋转矩阵R和一个三维平移t＝(t_x,t_y,t_z)。将R和t作为待求解的未知量，可以得到卫星地图上特征点在相机坐标系下的坐标，表示为：

相机观测图像上的匹配特征点也可以通过相机内参矩阵K变换到相机坐标系下：

这些点与相机中心(即相机原点)的连线形成向量，如图1中相机原点到观测点的方向向量7。理论上，在相机位姿正确的情况下，向量

与

的方向相同，即图1中的地图点投影的方向向量6和相机原点到观测点的方向向量7相重合，本发明以此构造线性方程。

是一个三自由度向量，存在两个与之正交的方向向量，如图1中的相机原点到观测点的方向向量7在深度归一化平面3上的第一切向量8和相机原点到观测点的方向向量7在深度归一化平面3上的第二切向量9，这两个方向向量存在于

的零空间中，记为：

其中r和s是与

相正交的两个方向向量，存在：

于是，若想要

与

的方向相同，则需要满足：

将旋转矩阵R设为：

于是，式(8)中共包含R和t的12个未知量，每对匹配特征点提供两条等式约束，因此需要至少6对匹配特征点就能求解出相机坐标。6对点匹配特征点得到6组等式(8)，一共12条等式组成的方程组。采用一般求解线性方程组的方法就可以求解出飞行器在东北天坐标系下的位置和姿态。

实施例1：一种通过单张航拍观测图像计算飞行器六自由度位姿的方法，包括步骤：通过在单张飞行器观测图像与卫星地图之间提取并匹配特征点，然后根据匹配结果计算飞行器的六自由度位姿，从而计算出飞行器的位置和姿态。

在实施例1的基础上，包括子步骤：

S1，根据视觉里程计或惯性测量单元计算出飞行器的第一位置，根据第一位置的GPS提取第一位置的卫星地图，然后分别提取并匹配卫星地图与观测图像的特征点；

S2，基于步骤S1得到飞行器观测图像与卫星地图之间的特征点匹配后，根据卫星地图上特征点的像素坐标

按照如下公式计算出卫星地图上每个特征点的经纬度坐标

其中，w和h分别是局部卫星地图图像的宽度和高度，C_lon和C_lat是局部卫星地图中心点像素的经度和纬度；ρ_lon和ρ_lat分别是卫星地图上每个像素的经度和纬度跨度；

S3，将特征点的经纬度坐标变换到地心坐标系下，再由地心坐标系变换到以飞行器起飞点为站点的东北天坐标系下，特征点的东北天坐标系坐标表示为：

其中，N是匹配特征点的数量；

S4，将旋转矩阵R和三维平移t作为待求解的未知量，得到卫星地图上特征点在相机坐标系下的坐标，表示为：

相机观测图像上的匹配特征点

通过相机内参矩阵变换到相机坐标系下，表示为：

观测图像上的匹配特征点与相机中心的连线形成向量，向量

与

的方向相同，以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

实施例3：在实施例2的基础上，在步骤S4中，以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态包括子步骤：

是一个三自由度向量，存在两个与之正交的方向向量，这两个与之正交的方向向量存在于

的零空间中，则记为：

其中，r和s是与

相正交的两个方向向量，则存在：

在

与

的方向相同的条件下，则存在：

上式中，共包含R和t的12个未知量，每对匹配特征点提供两条等式约束，使用至少6对匹配特征点即能求解出相机坐标，然后采用一般求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

实施例4：在实施例3的基础上，在步骤S4中，包括子步骤：将旋转矩阵R设为：

在实际应用时，在步骤S1中，特征点包括SURF特征点。

在实际应用时，飞行器包括无人机，且不限于无人机。

本发明功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，在一台计算机设备(可以是个人计算机，服务器，或者网络设备等)以及相应的软件中执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、或者光盘等各种可以存储程序代码的介质，进行测试或者实际的数据在程序实现中存在于只读存储器(Random Access Memory，RAM)、随机存取存储器(Random Access Memory，RAM)等。

Claims (6)

1.一种通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，包括步骤：通过在单张飞行器航拍观测图像与卫星地图之间提取并匹配特征点，然后根据匹配结果计算飞行器的六自由度位姿，从而计算出飞行器的位置和姿态。

2.根据权利要求1所述的通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，包括子步骤：

S1，根据视觉里程计或惯性测量单元计算出飞行器的第一位置，根据第一位置的GPS提取第一位置的卫星地图，然后分别提取并匹配卫星地图与观测图像的特征点；

S2，基于步骤S1得到飞行器观测图像与卫星地图之间的特征点匹配后，根据卫星地图上特征点的像素坐标

按照如下公式计算出卫星地图上每个特征点的经纬度坐标

其中，w和h分别是局部卫星地图图像的宽度和高度，C_lon和C_lat是局部卫星地图中心点像素的经度和纬度；ρ_lon和ρ_lat分别是卫星地图上每个像素的经度和纬度跨度；

S3，将特征点的经纬度坐标变换到地心坐标系下，再由地心坐标系变换到以飞行器起飞点为站点的东北天坐标系下，特征点的东北天坐标系坐标表示为：

其中，N是匹配特征点的数量；

S4，将旋转矩阵R和三维平移t作为待求解的未知量，得到卫星地图上特征点在相机坐标系下的坐标，表示为：

相机观测图像上的匹配特征点

通过相机内参矩阵K变换到相机坐标系下，表示为：

观测图像上的匹配特征点与相机中心的连线形成向量，向量

与

的方向相同，以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

3.根据权利要求2所述的通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，在步骤S4中，所述以此构造线性方程，并采用求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态包括子步骤：

是一个三自由度向量，存在两个与之正交的方向向量，这两个与之正交的方向向量存在于

的零空间中，则记为：

其中，r和s是与

相正交的两个方向向量，则存在：

在

与

的方向相同的条件下，则存在：

上式中，共包含R和t的12个未知量，每对匹配特征点提供两条等式约束，使用至少6对匹配特征点即能求解出相机坐标，然后采用一般求解线性方程组的方法求解出飞行器在东北天坐标系下的位置和姿态。

4.根据权利要求3述的通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，在步骤S4中，包括子步骤：将旋转矩阵R设为：

5.根据权利要求1所述的通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，在步骤S1中，所述特征点包括SURF特征点。

6.根据权利要求1～5任一项所述的通过单张航拍观测图像计算飞行器六自由度位姿的方法，其特征在于，所述飞行器包括无人机。

Images