一种基于遥感地图辅助的无人机自主定位方法及系统

摘要

本发明公开一种基于遥感地图辅助的无人机自主定位方法及系统，该方法采用惯性/视觉组合里程计对无人机位置和姿态进行连续估计，在惯性/视觉组合里程计的基础上，还加入匹配定位算法，通过将相机图中的视觉特征与遥感地图的视觉特征进行匹配定位，消除惯性/视觉组合里程计中位置和姿态的积累误差；在匹配定位算法中利用惯性/视觉组合里程计估计的特征三维坐标信息对正确的匹配点进行筛选，提升了匹配定位算法的精度；此外，采用位姿图优化目标函数，将惯性/视觉组合里程计估计的导航结果和匹配定位算法解算的导航结果进行融合，提升了导航系统的整体精度。本发明提供的方法具有导航参数连续输出、导航误差不随时间积累等优点。

权利要求

1.一种基于遥感地图辅助的无人机自主定位方法，其特征在于，包括：根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，获得所述遥感地图的特征描述向量；

利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所述相机图视觉特征的特征描述向量；

利用匹配定位算法将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿态；

利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出的无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无人机的位置和姿态进行优化，包括：

将无人机在每一个相机图采样时刻的位置和姿态描述为一个节点，利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，建立两个节点之间的连通边残差；

利用所述根据匹配关系解算出的无人机的位置和姿态，构建匹配定位残差；

根据所述连通边残差和匹配定位残差，构建位姿图优化目标函数；

采用非线性优化算法对所述位姿图优化目标函数进行求解，获得使所述位姿图优化目标函数最小化的节点状态向量，所述节点状态向量即为无人机的位置和姿态。

2.如权利要求1所述的基于遥感地图辅助的无人机自主定位方法，其特征在于，根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，包括：根据无人机飞行任务，载入无人机任务轨迹沿途的遥感地图，所述遥感地图为无人机飞行任务的全程区域或者无人机飞行任务的局部区域；

采用特征检测算法对所述遥感地图中的视觉特征进行探测，获得所述遥感地图的视觉特征，并记录所述视觉特征在所述遥感地图中的位置；

根据所述视觉特征在所述遥感地图中的位置，建立所述遥感地图视觉特征的特征描述向量。

3.如权利要求1所述的基于遥感地图辅助的无人机自主定位方法，其特征在于，利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所述相机图视觉特征的特征描述向量，包括：定义导航坐标系的原点和方向；

对机载相机输出的相机图中的视觉特征进行跟踪，得到同一视觉特征在不同相机图中的位置；

利用惯性测量单元的输出，计算两帧相机图采集时间间隔内的惯性预积分量；

根据所述同一视觉特征在不同相机图中的位置以及所述惯性预积分量，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置、速度和姿态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所述相机图视觉特征的特征描述向量。

4.如权利要求3所述的基于遥感地图辅助的无人机自主定位方法，其特征在于，对机载相机输出的相机图中的视觉特征进行跟踪，得到同一视觉特征在不同相机图中的位置，包括：

利用特征检测算法检测机载相机输出的相机图中的视觉特征；

采用特征跟踪方式对相机图中的视觉特征进行特征跟踪，得到同一视觉特征在不同相机图中出现的位置。

5.如权利要求3所述的基于遥感地图辅助的无人机自主定位方法，其特征在于，利用惯性测量单元的输出，计算两帧相机图采集时间间隔内的惯性预积分量，包括：利用惯性测量单元，获取无人机的比力和角速率；

根据无人机的比力和角速率，计算两帧相机图采样时刻 和 之间的惯性预积分量。

6.如权利要求1所述的基于遥感地图辅助的无人机自主定位方法，其特征在于，利用匹配定位算法将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿态，包括：对于所述相机图视觉特征的特征描述向量中的每一个元素，利用匹配定位算法在所述遥感地图的特征描述向量中搜索与其最为相似的特征，并建立特征匹配对；

利用所述相机图中视觉特征在导航坐标系中的三维坐标，对所述特征匹配对进行筛选，获得无人机的位置和姿态。

7.一种基于遥感地图辅助的无人机自主定位系统，其特征在于，包括：遥感地图模块，用于根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，获得所述遥感地图的特征描述向量；

惯性/视觉组合里程计模块，用于利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所述相机图视觉特征的特征描述向量；

特征匹配模块，用于利用匹配定位算法将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿态；

优化模块，用于利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出的无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无人机的位置和姿态进行优化，包括：

将无人机在每一个相机图采样时刻的位置和姿态描述为一个节点，利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，建立两个节点之间的连通边残差；

利用所述根据匹配关系解算出的无人机的位置和姿态，构建匹配定位残差；

根据所述连通边残差和匹配定位残差，构建位姿图优化目标函数；

采用非线性优化算法对所述位姿图优化目标函数进行求解，获得使所述位姿图优化目标函数最小化的节点状态向量，所述节点状态向量即为无人机的位置和姿态。

8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1 6中任一项所述方法的步骤。

~

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1 6中任一项所述方法的步骤。

~

说明书

一种基于遥感地图辅助的无人机自主定位方法及系统

技术领域

[0001] 本发明涉及自主导航技术领域，尤其是一种基于遥感地图辅助的无人机自主定位方法及系统。

背景技术

[0002] 定位系统是无人机载体顺利完成任务的重要保障。目前，在无人机中广泛使用的定位方式是“惯性+卫星（无线电）”组合导航。卫星导航和无线电导航信号极易被干扰，过分
依赖于卫星或无线电导航对无人机的导航系统存在巨大的潜在风险。因此，利用自身携带
的传感器，实现自主定位，对无人机具有重要的意义和广泛的应用前景。

[0003] 惯性/视觉组合里程计是一种广泛使用的无人机自主定位方法，它利用惯性传感器具有尺度敏感特性、输出频率高、动态性好等优点和视觉传感器测量精度高的优点对无
人机的导航参数进行估计。惯性/视觉组合里程计在估计无人机导航参数的同时，也能够将
观测到的图像特征进行三维重建。然而，视觉/惯性组合里程计的定位误差将随无人机飞行
的行程而积累，这严重制约了视觉/惯性组合里程计的精度。

发明内容

[0004] 本发明提供一种基于遥感地图辅助的无人机自主定位方法及系统，用于克服现有技术中惯性/视觉组合导航方法中无人机位置误差随时间积累等缺陷。

[0005] 为实现上述目的，本发明提出一种基于遥感地图辅助的无人机自主定位方法，包括：

[0006] 根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，获得所述遥感地图的特征描述向量；

[0007] 利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿态，并估计所述相机图中视觉特征
在导航坐标系中的三维坐标，根据所述三维坐标建立所述相机图视觉特征的特征描述向
量；

[0008] 利用匹配定位算法将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿态；

[0009] 利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出的无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无人机的位置和
姿态进行优化。

[0010] 为实现上述目的，本发明还提出一种基于遥感地图辅助的无人机自主定位系统，包括：

[0011] 遥感地图模块，用于根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，获得所述遥感地图的特征描述向量；

[0012] 惯性/视觉组合里程计模块，用于利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿
态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所
述相机图视觉特征的特征描述向量；

[0013] 特征匹配模块，用于利用匹配定位算法将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿
态；

[0014] 优化模块，用于利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出的无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无
人机的位置和姿态进行优化。

[0015] 为实现上述目的，本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

[0016] 为实现上述目的，本发明还提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述方法的步骤。

[0017] 与现有技术相比，本发明的有益效果有：

[0018] 本发明提供的基于遥感地图辅助的无人机自主定位方法，采用惯性/视觉组合里程计对无人机位置和姿态进行连续估计；惯性/视觉组合里程计具有较好的精度和鲁棒性，
不仅可以估计无人机的导航参数，还可以估计相机图视觉特征的三维坐标；在惯性/视觉组
合里程计的基础上，本发明还加入了匹配定位算法，通过将机载相机图中的视觉特征与遥
感地图中的视觉特征进行匹配定位，消除惯性/视觉组合里程计中位置和姿态的积累误差；
本发明在匹配定位算法中利用惯性/视觉组合里程计估计的特征三维坐标信息对正确的匹
配点进行筛选，有效提升了匹配定位算法的精度；此外，本发明采用位姿图优化目标函数，
将惯性/视觉组合里程计估计的导航结果和匹配定位算法解算的导航结果进行融合，提升
了导航系统的整体精度。与现有的无人机自主导航方法相比，本发明具有导航参数连续输
出、导航误差不随时间积累等优点。

附图说明

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

[0020] 图1为本发明提供的基于遥感地图辅助的无人机自主定位方法的流程图；

[0021] 图2为本发明实施例中相机图视觉特征的特征描述向量与遥感地图的特征描述向量进行特征匹配的示意图。

[0022] 本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

[0023] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基
于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其
他实施例，都属于本发明保护的范围。

[0024] 另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种
技术方案的结合不存在，也不在本发明要求的保护范围之内。

[0025] 无特殊说明，所使用的药品/试剂均为市售。

[0026] 本发明提出一种基于遥感地图辅助的无人机自主定位方法，如图1所示，包括：

[0027] 101：根据无人机飞行任务，载入无人机飞行区域的遥感地图并对遥感地图进行离线预处理，获得遥感地图的特征描述向量；

[0028] 遥感地图，是一种视觉地图。遥感地图中的每一个像素点都具有准确的地理位置标签，因此可以为无人机自主定位提供可靠的位置参考。

[0029] 102：利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用惯性/视觉组合里程计求解无人机的位置和姿态，并估计相机图中视觉特征在导
航坐标系中的三维坐标，根据三维坐标建立所述相机图视觉特征的特征描述向量；

[0030] 103：利用匹配定位算法将相机图视觉特征的特征描述向量与遥感地图的特征描述向量进行特征匹配，如图2所示，并根据匹配关系解算出无人机的位置和姿态；

[0031] 104：利用惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出的无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无人机的位置和
姿态进行优化。

[0032] 本发明提供的基于遥感地图辅助的无人机自主定位方法，采用惯性/视觉组合里程计对无人机位置、速度和姿态进行连续估计；惯性/视觉组合里程计具有较好的精度和鲁
棒性，不仅可以估计无人机的导航参数，还可以估计相机图视觉特征的三维坐标；在惯性/
视觉组合里程计的基础上，本发明还加入了匹配定位算法，通过将机载相机图中的视觉特
征与遥感地图中的视觉特征进行匹配定位，消除惯性/视觉组合里程计中位置和姿态的积
累误差；本发明在匹配定位算法中利用惯性/视觉组合里程计估计的特征三维坐标信息对
正确的匹配点进行筛选，有效提升了匹配定位算法的精度；此外，本发明采用位姿图优化目
标函数，将惯性/视觉组合里程计估计的导航结果和匹配定位算法解算的导航结果进行融
合，提升了导航系统的整体精度。与现有的无人机自主导航方法相比，本发明具有导航参数
连续输出、导航误差不随时间积累等优点。

[0033] 在其中一个实施例中，对于步骤101，根据无人机飞行任务，载入无人机飞行区域的遥感地图并对遥感地图进行离线预处理，包括：

[0034] 001：根据无人机飞行任务，载入无人机任务轨迹沿途的遥感地图，遥感地图为无人机飞行任务的全程区域或者无人机飞行任务的局部区域；

[0035] 002：采用特征检测算法对遥感地图中的视觉特征进行探测，获得遥感地图的视觉特征，并记录视觉特征在遥感地图中的位置；

[0036] 特征检测算法为基于SITF特征的特征提取算法、基于FAST角点的特征提取算法或者基于HARRIS角点的特征提取算法，等等。

[0037] 将视觉特征在遥感地图中的位置记为 ，其中 表示在遥感地图 中的像素坐标 ，表示第个视觉特征，表示视觉特征总数量。

[0038] 003：根据视觉特征在遥感地图中的位置，建立遥感地图视觉特征的特征描述向量。

[0039] 遥感地图视觉特征的特征描述向量记为 。

[0040] 特征描述向量采用SIFT特征、SURF特征或者ORB特征进行建立。

[0041] 在下一个实施例中，对于步骤102，利用机载相机的输出和惯性测量单元的输出，构建惯性/视觉组合里程计，利用惯性/视觉组合里程计求解无人机的位置、速度和姿态，并
估计相机图中视觉特征在导航坐标系中的三维坐标，根据三维坐标建立相机图视觉特征的
特征描述向量，包括：

[0042] 201：定义导航坐标系的原点和方向；

[0043] 一般将无人机的初始位置定义为导航坐标系的原点，XYZ坐标轴分别指向东向、北向和天向。

[0044] 202：对机载相机输出的相机图中的视觉特征进行跟踪，得到同一视觉特征在不同相机图中的位置；

[0045] 203：利用惯性测量单元的输出，计算两帧相机图采集时间间隔内的惯性预积分量；

[0046] 204：根据同一视觉特征在不同相机图中的位置以及惯性预积分量，构建惯性/视觉组合里程计，利用惯性/视觉组合里程计求解无人机的位置、速度和姿态，并估计相机图
中视觉特征在导航坐标系中的三维坐标，根据三维坐标建立所述相机图视觉特征的特征描
述向量。

[0047] 在某个实施例中，对于步骤202，对机载相机输出的相机图中的视觉特征进行跟踪，得到同一视觉特征在不同相机图中的位置，包括：

[0048] 2021：利用特征检测算法检测机载相机输出的相机图中的视觉特征；

[0049] 特征检测算法为基于SITF特征的特征提取算法、基于FAST角点的特征提取算法或者基于HARRIS角点的特征提取算法，等等。

[0050] 2022：采用特征跟踪方式对相机图中的视觉特征进行特征跟踪，得到同一视觉特征在不同相机图中出现的位置。

[0051] 特征跟踪方式采用现有方式，如KLT稀疏光流跟踪法、稠密光流跟踪法、特征匹配法等。

[0052] 编号为 的视觉特征在第 幅相机图中出现的位置记为 。

[0053] 在另一个实施例中，对于步骤203，利用惯性测量单元的输出，计算两帧相机图采集时间间隔内的惯性预积分量，包括：

[0054] 2031：利用惯性测量单元，获取无人机的比力和角速率；

[0055] 惯性测量单元的测量模型为：

[0056] （1）

[0057] 式中， 为惯性测量单元中加速度计测得的无人机在t时刻的比力； 为惯性测量单元中陀螺测得的无人机在t时刻的角速率； 为无人机在t时刻的实际比力； 为无人
机在t时刻的实际角速率； 和 为 时刻估计得到的加速度计和陀螺仪的零偏值；
为世界坐标系到t时刻体坐标系的转换矩阵， 为世界坐标系； 为世界坐标系下的重力；
陀螺仪的测量噪声 和加速度计的测量噪声 服从高斯分布， ，
和 分别为加速度计和陀螺仪测量噪声的方差。

[0058] 2032：根据无人机的比力和角速率，计算两帧相机图采样时刻 和 之间的惯性预积分量。

[0059] 预积分表达式为：

[0060] （2）

[0061] 式中， 为t时刻体坐标系到 时刻体坐标系的姿态矩阵； 为t时刻从体坐标系到世界坐标系的姿态矩阵；表示右乘四元数； 为姿态预积分量； 为速度预积
分量； 为位置预积分量。

[0062] 惯性预积分量的误差微分方程为：

[0063] （3）

[0064] 式中， 为预积分位置分量估计误差； 为预积分速度分量估计误差；为预积分姿态分量估计误差； 为加速度计零偏估计误差； 为陀螺仪零偏估计误
差； 为加速度计测量噪声； 陀螺仪测量噪声； 为加速度计零偏估计噪声；
为陀螺仪零偏估计噪声； 为状态误差传递系数矩阵； 为状态误差矩阵； 为噪声
传递系数矩阵； 为噪声矩阵；式（3）中符号所代表的量均在连续空间中。

[0065] 惯性预积分量的协方差矩阵 可以通过关于离散时间的一阶方程进行迭代求解。首先给协方差矩阵 赋初值为0，然后通过下式进行迭代求解：

[0066] （4）

[0067] 式中， 为惯性预积分量对应的采样时间间隔； 为噪声构成的对角矩阵；为单位矩阵； 为误差传递系数矩阵； 为噪声传递系数矩
阵。

[0068] 惯性预积分量关于误差量的雅各比矩阵可以通过下式迭代求解：

[0069] （5）

[0070] 式中， 赋初始值为单位矩阵 。利用式（4）计算得到的协方差矩阵 和式（5）计算得到的雅可比矩阵 ，预积分量 关于惯性测量单元零偏的一阶近似可以
写成：

[0071] （6）

[0072] 其中， 、 分别为预积分位置分量相对于加速度计零偏和陀螺仪零偏的雅克比矩阵； 、 分别为预积分速度分量相对于加速度计零偏和陀螺仪零偏的雅克比矩阵；
为预积分姿态分量相对于陀螺仪零偏的雅克比矩阵； 和 分别为加速度计和
陀螺仪零偏估计误差； 、 和 为常值零偏下惯性预积分量的预测值，计算方法
为：

[0073] （7）

[0074] 其中， 为将姿态四元数转化为姿态旋转矩阵； 为四元数乘法； 和 为时刻估计得到的加速度计和陀螺仪的零偏值。

[0075] 在下一实施例中，对于步骤204，根据同一视觉特征在不同相机图中的位置以及惯性预积分量，构建惯性/视觉组合里程计，利用惯性/视觉组合里程计求解无人机的位置和
姿态，并估计相机图中视觉特征在导航坐标系中的三维坐标，根据三维坐标建立所述相机
图视觉特征的特征描述向量，包括：

[0076] 设有 个时刻的导航状态需要估计，将这 个时刻的惯性/视觉组合导航状态向量 表示为：

[0077] （8）

[0078] 其中， 表示第 个时刻待估计的导航状态向量，其中包括无人机的位置向量速度向量 、姿态四元数 、滑动窗口内的加速度计零偏估计值 和陀螺仪零偏估计值 ；
导航状态向量 中还包括有 时间段内，相机图中视觉特征的逆深度信息 ，其中 即
为第 个相机图中视觉特征的逆深度， ，其中 为特征点总数，该特征在导航坐标
系下的三维坐标 可以通过逆深度计算得到，计算方法为：

[0079] （9）

[0080] 其中， 为第 个相机图中视觉特征在被首次观测到的相机图 中的位置； 为第帧体坐标系到世界坐标系的姿态矩阵； 为机载相机与惯性测量单元体坐标
系之间的姿态矩阵； 为机载相机与惯性测量单元体坐标系之间的位置矩阵； 表示
将图像坐标逆映射为单位深度的三维坐标。

[0081] 惯性/视觉组合导航的优化目标函数可表示为：

[0082] （10）

[0083] 式中， 为惯性预积分量残差，具体表达式为：

[0084] （11）

[0085] 式中， 为预积分位置分量估计误差； 为预积分速度分量估计误差；为预积分姿态分量估计误差； 为加速度计零偏估计误差； 为陀螺仪零偏估计
误差； 为世界坐标系到 时刻体坐标系的转换矩阵； 为 时刻体无人机位置向量；
为世界坐标系下的重力； 为时间窗口长度； 为 时刻体无人机速度向量； 、
、 为预积分三分量的测量值； 为 时刻体无人机姿态逆矩阵； 、 为 时
刻加速度计和陀螺仪的零偏；式（11）中符号所代表的量均在离散空间中。

[0086] 为视觉观测残差，具体表达式为：

[0087] （12）

[0088] （13）

[0089] 式中， 、 为 和 时刻在世界坐标系下的位置向量。

[0090] 为边缘化残差，代表 时刻以前的导航状态向量的影响。

[0091] 式中， 、 为先验误差和海森矩阵。

[0092] 采用非线性优化算法对式（10）进行求解，可得到惯性/视觉组合里程计估计的无人机位置向量 速度向量 、姿态四元数 和视觉特征的逆深度 。通过逆深度和求解
得到的位置、姿态信息，即可恢复出视觉特征在导航坐标系下的三维坐标，具体计算方法为
参见式（9）。

[0093] 在另一个实施例中，对于步骤204，根据三维坐标建立相机图视觉特征的特征描述向量，包括：

[0094] 根据相机图中估计的三维坐标的重投影误差，选取惯性/视觉组合里程计中的相机图中视觉特征用于匹配定位。重投影误差可根据式（12）计算，仅筛选重投影误差小于设
定阈值的特征将用于匹配定位，将筛选出的特征集记为 ；

[0095] 对特征集记为 进行特征描述，得到相机图视觉特征的特征描述向量 。

[0096] 特征描述向量采用SIFT特征、SURF特征或者ORB特征进行建立。相机图视觉特征的特征描述向量与遥感地图的特征描述向量应为同一种特征描述向量。

[0097] 在下一个实施例中，对于步骤103，利用匹配定位算法将相机图视觉特征的特征描述向量与遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和
姿态，包括：

[0098] 301：对于相机图视觉特征的特征描述向量 中的每一个元素，利用匹配定位算法在遥感地图的特征描述向量 中搜索与其最为相似的特征，并建立
特征匹配对 ； 表示从相机图视觉特征中筛选出的第
个特征与遥感地图视觉特征中的第 个元素之间形成一个匹配对。

[0099] 302：利用相机图中视觉特征在导航坐标系中的三维坐标，对特征匹配对进行筛选，获得无人机的位置和姿态。

[0100] 在某个实施例中，对于步骤302，利用相机图中视觉特征在导航坐标系中的三维坐标，对特征匹配对进行筛选，获得无人机的位置和姿态，包括：

[0101] 3021：每一个特征匹配对 ，计算一个位置平移量，计算方法为：

[0102] （14）

[0103] 其中， 、 为相机图中视觉特征在导航坐标系中的三维坐标点的 、轴坐标值； 遥感地图左上角点在导航坐标系下的坐标； 、 为遥感地图视
觉特征在遥感地图中的坐标；为地图分辨率，单位为像素/米。

[0104] 3022：对于每一个匹配对 ，初始化一个空集 。搜索在匹配集（所有的匹配对构成匹配集）中与匹配对 具有相接近平移量的匹配对，搜索方法根据两个匹配对平移
量的一致性进行判断，一致性判断参数的计算方法为：

[0105] （15）

[0106] 当一致性参数小于设定阈值时，将相应的平移量 加入到集合 中；

[0107] 3023：在所有匹配对中，筛选对应集合 元素最多的集合，记为 。当中的元素个数超过设定阈值时，匹配定位成功。若匹配成功则进行匹配定位解算；若未匹配
成功，则返回步骤301，对下一幅相机图进行匹配。

[0108] 3024：若匹配成功，则求集合 所有元素的均值 ，记为 。无人机的匹配位置可用如下公式表示：

[0109] （16）

[0110] 其中， 为惯性/视觉组合里程计估计的无人机位置。

[0111] 在下一个实施例中，对于步骤104，利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出无人机的位置和姿态，构建无人机位姿图优化目
标函数，从而对无人机的位置和姿态进行优化，包括：

[0112] 401：将无人机在每一个相机图采样时刻的位置和姿态描述为一个节点，表示为，利用惯性/视觉组合里程计求解的无人机的位置和姿态，建立两个节点之间
的连通边残差；节点 和 之间的连通边可以表示为 ，具体表达式为：

[0113] （17）

[0114] 其中， 、 为节点， 表示将姿态四元数转化为姿态旋转矩阵；该连通边的残差项 即为与惯性/视觉里程计测量值之间的残差项，表达式为：

[0115] （18）

[0116] 其中， 表示用惯性/视觉组合里程计估计结果， 与 分别代表位置向量的估计值和姿态向量的估计值，具体表达式与（17）相同。

[0117] 402：利用所述根据匹配关系解算出无人机的位置和姿态，构建匹配定位残差 ；具体表达示为：

[0118] （19）

[0119] 其中， 表示惯性/视觉组合里程计估计的无人机位置， 表示无人机的匹配位置。

[0120] 若第 个节点没有对应的匹配定位结果，则没有相应的残差项。

[0121] 403：根据连通边残差 和匹配定位残差 ，构建位姿图优化目标函数 ，具体表达式为：

[0122] （20）

[0123] 其中， 表示所有节点的集合； 表示仅匹配定位成功的节点； 表示里惯性/视觉里程计测量的误差协方差矩阵，可根据经验和算法精度设置； 为匹配定位误差协方
差矩阵，可根据经验和算法精度设置。

[0124] 404：采用非线性优化算法对所述位姿图优化目标函数进行求解，获得使所述位姿图优化目标函数最小化的节点状态向量，所述节点状态向量即为无人机的位置和姿态。

[0125] 本发明还提出一种基于遥感地图辅助的无人机自主定位系统，包括：

[0126] 遥感地图模块，用于根据无人机飞行任务，载入无人机飞行区域的遥感地图并对所述遥感地图进行离线预处理，获得所述遥感地图的特征描述向量；

[0127] 惯性/视觉组合里程计模块，用于利用机载相机输出的相机图和惯性测量单元的输出，构建惯性/视觉组合里程计，利用所述惯性/视觉组合里程计求解无人机的位置和姿
态，并估计所述相机图中视觉特征在导航坐标系中的三维坐标，根据所述三维坐标建立所
述相机图视觉特征的特征描述向量；

[0128] 特征匹配模块，用于将所述相机图视觉特征的特征描述向量与所述遥感地图的特征描述向量进行特征匹配，并根据匹配关系解算出无人机的位置和姿态；

[0129] 优化模块，用于利用所述惯性/视觉组合里程计求解的无人机的位置和姿态，以及根据匹配关系解算出无人机的位置和姿态，构建无人机位姿图优化目标函数，从而对无人
机的位置和姿态进行优化。

[0130] 在其中一个实施例中，遥感地图模块还包括：

[0131] 001：根据无人机飞行任务，载入无人机任务轨迹沿途的遥感地图，遥感地图为无人机飞行任务的全程区域或者无人机飞行任务的局部区域；

[0132] 002：采用特征检测算法对遥感地图中的视觉特征进行探测，获得遥感地图的视觉特征，并记录视觉特征在遥感地图中的位置；

[0133] 特征检测算法为基于SITF特征的特征提取算法、基于FAST角点的特征提取算法或者基于HARRIS角点的特征提取算法，等等。

[0134] 将视觉特征在遥感地图中的位置记为 ，其中 表示在遥感地图 中的像素坐标 ，表示第 个视觉特征， 表示视觉特征总数量。

[0135] 003：根据视觉特征在遥感地图中的位置，建立遥感地图视觉特征的特征描述向量。

[0136] 遥感地图视觉特征的特征描述向量记为 。

[0137] 特征描述向量采用SIFT特征、SURF特征或者ORB特征进行建立。

[0138] 在下一个实施例中，惯性/视觉组合里程计模块还包括：

[0139] 201：定义导航坐标系的原点和方向；

[0140] 一般将无人机的初始位置定义为导航坐标系的原点，XYZ坐标轴分别指向东向、北向和天向。

[0141] 202：对机载相机输出的相机图中的视觉特征进行跟踪，得到同一视觉特征在不同相机图中的位置；

[0142] 203：利用惯性测量单元的输出，计算两帧相机图采集时间间隔内的惯性预积分量；

[0143] 204：根据同一视觉特征在不同相机图中的位置以及惯性预积分量，构建惯性/视觉组合里程计，利用惯性/视觉组合里程计求解无人机的位置、速度和姿态，并估计相机图
中视觉特征在导航坐标系中的三维坐标，根据三维坐标建立所述相机图视觉特征的特征描
述向量。

[0144] 在某个实施例中，惯性/视觉组合里程计模块还包括：

[0145] 2021：利用特征检测算法检测机载相机输出的相机图中的视觉特征；

[0146] 特征检测算法为基于SITF特征的特征提取算法、基于FAST角点的特征提取算法或者基于HARRIS角点的特征提取算法，等等。

[0147] 2022：采用特征跟踪方式对相机图中的视觉特征进行特征跟踪，得到同一视觉特征在不同相机图中出现的位置。

[0148] 特征跟踪方式采用现有方式，如KLT稀疏光流跟踪法、稠密光流跟踪法、特征匹配法等。

[0149] 编号为 的视觉特征在第 幅相机图中出现的位置记为 。

[0150] 在另一个实施例中，惯性/视觉组合里程计模块还包括：

[0151] 2031：利用惯性测量单元，获取无人机的比力和角速率；

[0152] 惯性测量单元的测量模型为：

[0153] （1）

[0154] 式中， 为惯性测量单元中加速度计测得的无人机在t时刻的比力； 为惯性测量单元中陀螺测得的无人机在t时刻的角速率； 为无人机在t时刻的实际比力； 为无人机
在t时刻的实际角速率； 和 为 时刻估计得到的加速度计和陀螺仪的零偏值； 为
世界坐标系到t时刻体坐标系的转换矩阵， 为世界坐标系； 为世界坐标系下的重力；陀
螺仪的测量噪声 和加速度计的测量噪声 服从高斯分布， ，
和 分别为加速度计和陀螺仪测量噪声的方差。

[0155] 2032：根据无人机的比力和角速率，计算两帧相机图采样时刻 和 之间的惯性预积分量。

[0156] 预积分表达式为：

[0157] （2）

[0158] 式中， 为t时刻体坐标系到 时刻体坐标系的姿态矩阵； 为t时刻从体坐标系到世界坐标系的姿态矩阵； 表示右乘四元数； 为姿态预积分量； 为速度预积分
量； 为位置预积分量。

[0159] 惯性预积分量的误差微分方程为：

[0160] （3）

[0161] 式中， 为预积分位置分量估计误差； 为预积分速度分量估计误差； 为预积分姿态分量估计误差； 为加速度计零偏估计误差； 为陀螺仪零偏估计误差；
为加速度计测量噪声； 陀螺仪测量噪声； 为加速度计零偏估计噪声； 为陀螺仪
零偏估计噪声； 为状态误差传递系数矩阵； 为状态误差矩阵； 为噪声传递系数矩
阵； 为噪声矩阵；式（3）中符号所代表的量均在连续空间中。

[0162] 惯性预积分量的协方差矩阵 可以通过关于离散时间的一阶方程进行迭代求解。首先给协方差矩阵 赋初值为0，然后通过下式进行迭代求解：

[0163] （4）

[0164] 式中， 为惯性预积分量对应的采样时间间隔； 为噪声构成的对角矩阵；为单位矩阵； 为误差传递系数矩阵；为噪声传递系数矩阵。

[0165] 惯性预积分量关于误差量的雅各比矩阵可以通过下式迭代求解：

[0166] （5）

[0167] 式中， 赋初始值为单位矩阵 。利用式（4）计算得到的协方差矩阵 和式（5）计算得到的雅可比矩阵 ，预积分量 关于惯性测量单元零偏的一阶近似可以
写成：

[0168] （6）

[0169] 其中， 、 分别为预积分位置分量相对于加速度计零偏和陀螺仪零偏的雅克比矩阵； 、 分别为预积分速度分量相对于加速度计零偏和陀螺仪零偏的雅克比矩
阵； 为预积分姿态分量相对于陀螺仪零偏的雅克比矩阵； 和 分别为加速度计
和陀螺仪零偏估计误差； 、 和 为常值零偏下惯性预积分量的预测值，计算方法
为：

[0170] （7）

[0171] 其中， 为将姿态四元数转化为姿态旋转矩阵；为四元数乘法； 和 为时刻估计得到的加速度计和陀螺仪的零偏值。

[0172] 在下一实施例中，惯性/视觉组合里程计模块还包括：

[0173] 设有 个时刻的导航状态需要估计，将这 个时刻的惯性/视觉组合导航状态向量 表示为：

[0174] （8）

[0175] 其中， 表示第 个时刻待估计的导航状态向量，其中包括无人机的位置向量速度向量 、姿态四元数 、滑动窗口内的加速度计零偏估计值 和陀螺仪零偏估计值 ；
导航状态向量 中还包括有 时间段内，相机图中视觉特征的逆深度信息 ，其中 即
为第 个相机图中视觉特征的逆深度， ，其中 为特征点总数，该特征在导航坐标系
下的三维坐标 可以通过逆深度计算得到，计算方法为：

[0176] （9）

[0177] 其中， 为第 个相机图中视觉特征在被首次观测到的相机图 中的位置； 为第帧体坐标系到世界坐标系的姿态矩阵； 为机载相机与惯性测量单元体坐标
系之间的姿态矩阵； 为机载相机与惯性测量单元体坐标系之间的位置矩阵； 表示
将图像坐标逆映射为单位深度的三维坐标。

[0178] 惯性/视觉组合导航的优化目标函数可表示为：

[0179] （10）

[0180] 式中， 为惯性预积分量残差，具体表达式为：

[0181] （11）

[0182] 式中， 为预积分位置分量估计误差； 为预积分速度分量估计误差；为预积分姿态分量估计误差； 为加速度计零偏估计误差； 为陀螺仪零偏估计
误差； 为世界坐标系到 时刻体坐标系的转换矩阵； 为 时刻体无人机位置向量；
为世界坐标系下的重力； 为时间窗口长度； 为 时刻体无人机速度向量； 、
、 为预积分三分量的测量值； 为 时刻体无人机姿态逆矩阵； 、 为 时刻
加速度计和陀螺仪的零偏；式（11）中符号所代表的量均在离散空间中。

[0183] 为视觉观测残差，具体表达式为：

[0184] （12）

[0185] （13）

[0186] 式中， 、 为 和 时刻在世界坐标系下的位置向量。

[0187] 为边缘化残差，代表 时刻以前的导航状态向量的影响。

[0188] 式中， 、 为先验误差和海森矩阵。

[0189] 采用非线性优化算法对式（10）进行求解，可得到惯性/视觉组合里程计估计的无人机位置向量 速度向量 、姿态四元数 和视觉特征的逆深度 。通过逆深度和求解
得到的位置、姿态信息，即可恢复出视觉特征在导航坐标系下的三维坐标，具体计算方法为
参见式（9）。

[0190] 在另一个实施例中，惯性/视觉组合里程计模块还包括：

[0191] 根据相机图中估计的三维坐标的重投影误差，选取惯性/视觉组合里程计中的相机图中视觉特征用于匹配定位。重投影误差可根据式（12）计算，仅筛选重投影误差小于设
定阈值的特征将用于匹配定位，将筛选出的特征集记为 ；

[0192] 对特征集记为 进行特征描述，得到相机图视觉特征的特征描述向量。

[0193] 特征描述向量采用SIFT特征、SURF特征或者ORB特征进行建立。相机图视觉特征的特征描述向量与遥感地图的特征描述向量应为同一种特征描述向量。

[0194] 在下一个实施例中，特征匹配模块还包括：

[0195] 301：对于相机图视觉特征的特征描述向量 中的每一个元素，利用匹配定位算法在遥感地图的特征描述向量 中搜索与其最为相似的特征，并建立
特征匹配对 ； 表示从相机图视觉特征中筛选出的第
个特征与遥感地图视觉特征中的第 个元素之间形成一个匹配对。

[0196] 302：利用相机图中视觉特征在导航坐标系中的三维坐标，对特征匹配对进行筛选，获得无人机的位置和姿态。

[0197] 在某个实施例中，特征匹配模块还包括：

[0198] 3021：每一个特征匹配对 ，计算一个位置平移量，计算方法为：

[0199] （14）

[0200] 其中， 、 为相机图中视觉特征在导航坐标系中的三维坐标点 的、 轴坐标值； 遥感地图左上角点在导航坐标系下的坐标； 、 为遥感地图视觉特
征在遥感地图中的坐标； 为地图分辨率，单位为像素/米。

[0201] 3022：对于每一个匹配对 ，初始化一个空集 。搜索在匹配集（所有的匹配对构成匹配集）中与匹配对 具有相接近平移量的匹配对，搜索方法根据两个匹配对平移
量的一致性进行判断，一致性判断参数的计算方法为：

[0202] （15）

[0203] 当一致性参数小于设定阈值时，将相应的平移量 加入到集合 中；

[0204] 3023：在所有匹配对中，筛选对应集合 元素最多的集合，记为 。当 中的元素个数超过设定阈值时，匹配定位成功。若匹配成功则进行匹配定位解算；若未匹配成
功，则返回步骤301，对下一幅相机图进行匹配。

[0205] 3024：若匹配成功，则求集合 所有元素的均值 ，记为 。无人机的匹配位置可用如下公式表示：

[0206] （16）

[0207] 其中， 为惯性/视觉组合里程计估计的无人机位置。

[0208] 在下一个实施例中，优化模块还包括：

[0209] 401：将无人机在每一个相机图采样时刻的位置和姿态描述为一个节点，表示为，利用惯性/视觉组合里程计求解的无人机的位置和姿态，建立两个节点之间
的连通边残差；节点 和 之间的连通边可以表示为 ，具体表达式为：

[0210] （17）

[0211] 其中， 、 为节点， 表示将姿态四元数转化为姿态旋转矩阵；该连通边的残差项 即为与惯性/视觉里程计测量值之间的残差项，表达式为：

[0212] （18）

[0213] 其中， 表示用惯性/视觉组合里程计估计结果， 与 分别代表位置向量的估计值和姿态向量的估计值，具体表达式与（17）相同。

[0214] 402：利用所述根据匹配关系解算出无人机的位置和姿态，构建匹配定位残差 ；具体表达示为：

[0215] （19）

[0216] 其中， 表示惯性/视觉组合里程计估计的无人机位置， 表示无人机的匹配位置。

[0217] 若第 个节点没有对应的匹配定位结果，则没有相应的残差项。

[0218] 403：根据连通边残差 和匹配定位残差 ，构建位姿图优化目标函数 ，具体表达式为：

[0219] （20）

[0220] 其中， 表示所有节点的集合； 表示仅匹配定位成功的节点； 表示里惯性/视觉里程计测量的误差协方差矩阵，可根据经验和算法精度设置； 为匹配定位误差协方
差矩阵，可根据经验和算法精度设置。

[0221] 404：采用非线性优化算法对所述位姿图优化目标函数进行求解，获得使所述位姿图优化目标函数最小化的节点状态向量，所述节点状态向量即为无人机的位置和姿态。

[0222] 本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

[0223] 本发明还提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述方法的步骤。

[0224] 以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用
在其他相关的技术领域均包括在本发明的专利保护范围内。