CN114419109B - 一种基于视觉和气压信息融合的飞行器定位方法 - Google Patents

Abstract

本发明公布了一种基于视觉和气压信息融合的飞行器定位方法，包括初始化设置和数据采集、相邻时刻位姿解算和状态更新三个步骤，通过循环递推可获取飞行器的实时定位。该定位方法通过将气压信息和视觉导航信息进行智能融合，不仅可以通过气压信息可获取垂向位置的量测，解决了单目视觉技术中存在的尺度因子问题；另一方面通过气压信息的变化，可反映当前飞行器的不同飞行阶段，采用不同的气压和视觉信息融合方法，以获取飞行器的实时位置估计，定位更加准确快速。该方法还提升了此类拒止条件下飞行器定位的可靠性和可信度，通过气压传感器信息判断飞行阶段，进而采用不同的定位计算和组合策略，以获取最优的定位结果。

Description

一种基于视觉和气压信息融合的飞行器定位方法

技术领域

本发明涉及一种定位方法，具体涉及一种基于视觉和气压信息融合的飞行器定位方法；属于飞行器定位技术领域。

背景技术

当前，飞行器的导航定位多采用卫星导航和惯性导航相结合的方式开展，其中惯性导航自主性较高、不依赖外部信号，但误差随时间累积，因此需要基于卫星导航信息进行结果的修正。在遭遇信号遮挡、信号干扰等情况下，卫星导航结果就不可用，需要一种能够抑制惯导误差发散，协同惯导进行组合定位的方式，为飞行器提供可信位置。视觉导航作为一种自主性较好的导航方式，可以通过匹配不同时刻采集的图像，解算出相对位姿移动，用于导航定位，能够弥补卫星导航的不足之处。

视觉导航通过匹配不同时刻采集的图像，解算出相对位姿移动，可用于导航定位，分为单目、多目等方式，其中单目视觉导航方式结构简单、计算量相对较小，被业界广泛研究用于移动载体的导航。在车载导航领域，相机高度相对地面不变，可基于地面特征点高度一致且垂直距离均等于相机高度的假设，估算出相关尺度因子。但在飞行器导航领域，飞行高度时刻变化，不适用上述假设，因此仅依赖单目视觉本身中无法解决视觉导航中尺度因子s的问题，需引入其他自主性较好的手段进行辅助。

气压传感器信息由于其普适性，被广泛应用于高度的量测。相较于汽车的近似平面运动，飞行器的运动在三维空间中自由，需要灵活判断飞行器的飞行高度，气压传感器是一种经济且可靠的手段。然而通过单一气压信息仅能获取高度，无法提供飞行器的精确位置。因此现有技术中无法解决飞行器定位的问题，有待克服。

发明内容

为解决现有技术的不足，本发明的目的在于一种基于视觉和气压信息融合的飞行器定位方法。

为了实现上述目标，本发明采用如下的技术方案：

一种基于视觉和气压信息融合的飞行器定位方法，包括如下步骤：

S1、初始化设置和数据采集：构建坐标系，相机采集初始时刻的基准图像，气压传感器获取初始时刻的气压值；

S2、相邻时刻位姿解算：达到状态更新条件时，系统采集最新时刻数据，进行位置和姿态解算，确定飞行器状态更新方法，获取最新时刻的位置，对飞行器进行定位；

S3、状态更新：将更新后的最新位置和最新姿态，作为新的基准，用于下一时刻状态累积更新的基准，迭代至步骤S2，循环递推获取实时定位。

优选地，前述步骤S1中，构建的坐标系包括：世界坐标系、载体坐标系和相机坐标系；所述气压传感器的安装位置靠近相机，所述相机镜头向下安装。所述世界坐标系以初始时刻载体上相机的中心位置为零点，北、东、地向为三轴方向，满足右手定则；所述载体坐标系和相机坐标系均满足右手定则，两者固连重合，定义为前、右、下，中心位于相机的中心位置；相机坐标系和载体坐标系间的旋转矩阵转换关系

恒定为单位阵

；初始时刻，相机坐标系和世界坐标系之间存在旋转矩阵转换关系

。

更优选地，前述步骤S2包括如下子步骤：

（1）气压传感器获取最新时刻的气压值，并计算与上一时刻的气压差值；

（2）相机采集新的一帧图像，并使用特征点匹配的方法与上一时刻图像进行对比分析，获取相机相邻时刻的姿态角变化及位移比例关系；

（3）判断相邻时刻气压传感器输出差值及根据视觉解算得到的载体姿态角变化值与预设阈值之间的关系，视不同场景利用不同的方法进行载体姿态和位置更新计算。

优选地，前述的特征点匹配方法具体为：首先计算两幅图像中的特征点，随后剔除两幅图像中的相同像素位置的特征点，进而依据特征点的描述子进行特征点匹配，最后，使用改进的随机采样方法，对于错误匹配进行剔除。

进一步优选地，前述步骤S2中，包括如下三种场景：

（a）气压差值小于预设阈值且载体姿态角变化超过预定阈值；（b）气压差值小于预设阈值且载体姿态角变化小于预定阈值；（c）气压差值超过预设阈值。

更优选地，预设阈值为：气压差值为0.1hPa，姿态角变化为1度。

更进一步优选地，前述场景（a）的更新计算方法为（a）方法：首先根据前一更新周期中世界坐标系下的三轴位移

和间隔时间，获得前一更新周期中的运动速度

；再根据前后时刻飞行器姿态的

的差值，进行补偿，重新构建本时刻的运动趋势：

其中k为补偿系数，

和

分别表示平面x和y方向的运动速度趋势，得到相应的运动趋势后，乘以相应的时间则可获取平面位移，累加到上一时刻的位置上，据此得到最新时刻的位置。

场景（b）的更新方法为（b）方法：根据前一更新周期中世界坐标系下的三轴位移

和间隔时间

，获得前一更新周期中的运动速度

，默认本时刻和上一时刻的运动趋势一致，得出状态递推方程：

，

其中，

表示噪声向量；

该状态递推方程中，使用三维的运动趋势进行状态递推，递推后使用光流信息的输出作为平面位置量测，气压传感器输出求解的气压高度值作为高度量测，对状态参数进行滤波，量测方程构建如下：

，

其中，

表示量测误差，取决于气压传感器的精度值；

为单位矩阵，

为世界坐标系下三维位置的量测值，通过气压传感器的输出当前位置的气压值

，换算为高度的量测值

；结合当前气压高度值，配合当地地面高修正，通过光流运算，得到x和y方向上的速度，将其乘以时间得到

，累加到上一时刻位置

上，得到

的值，即为当前最新时刻的位置；

表示量测误差，取决于气压传感器的精度值。

场景（c）的更新方法为（c）方法：根据前后两个时刻气压传感器的输出，分别求得两个时刻的高度值

和

，得到高度差

；

前后两个时刻之间载体的位移在世界坐标系纵轴下的投影，理想情况下等于该高度差：

即

，

其中，

表示上一时刻相机坐标系和世界坐标系的旋转转换关系，

表示k-1时刻相机坐标系下，三轴的真实位移，

表示k-1时刻相机坐标系下，三轴的真实位移比例关系。

得到尺度因子s，以及真实世界坐标系下的位移

；

根据上一时刻的位置，叠加上位移，获取当前时刻的位置信息

。

本发明的有益之处在于：

本发明的飞行器定位方法通过将气压信息和视觉导航信息进行智能融合，不仅可以通过气压信息可获取垂向位置的量测，解决了单目视觉技术中存在的尺度因子问题；另一方面通过气压信息的变化，可反映当前飞行器的不同飞行阶段，采用不同的气压和视觉信息融合方法，以获取飞行器的实时位置估计，定位更加准确快速。

本发明的方法通过引入视觉和气压信息进行组合定位，提升了此类拒止条件下飞行器定位的可靠性和可信度，通过气压传感器信息判断飞行阶段，进而采用不同的定位计算和组合策略，以获取最优的定位和导航结果。

附图说明

图1是本发明的一种基于视觉和气压信息融合的飞行器定位方法的流程框图；

图2是本发明的方法中设计运动趋势的计算流程示意图。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

如图1所示，本发明的基于视觉和气压信息融合的飞行器定位方法整体流程分为初始化、相邻时刻位姿解算和状态更新三个过程，在图1中分别以①②③结合虚线框的形式来标明。其中，第②过程（步骤S2相邻时刻位姿解算）和第③过程（步骤S3状态更新）循环递推，以实现载体位置和姿态的不断更新，进而实现实时定位。其中，相邻时刻位姿解算过程需根据气压传感器的输出及视觉的姿态匹配结果进行区分处理，大大优化了定位结果的可靠性和准确性。

下面针对以上三个过程和其中的具体步骤进行详细说明：

S1、初始化过程

该过程为飞行器飞行前的准备工作，用于确定基准条件。气压传感器安装贴近视觉相机，相机镜头朝下安装，默认相机坐标系与载体坐标系固连。本方法中，相机相对于世界坐标系的姿态和位移与载体（飞行器）相对于世界坐标系的姿态和位移同义。

具体过程包括：

（1）选取当前载体上相机中心所在位置点为零点构建三轴直角坐标系，作为本方法中的世界坐标系，后续递推的位置及姿态均基于定义的世界坐标系；

（2）相机获取初始时刻的基准图像，气压传感器初始时刻的气压值;

S2、相邻时刻位姿解算过程

达到系统状态更新条件时，传感器采集最新时刻数据，用于位置和姿态解算。根据传感器输出及解算信息，确定飞行器状态信息更新方法。

具体过程包括：

（1）气压传感器获取当前时刻气压值，并计算与上一时刻的气压差值；

（2）相机采集新的一帧图像，并使用特征点匹配的方法与上一时刻图像进行对比分析，获取相机相邻时刻的姿态角变化及位移比例关系；

（3）判断相邻时刻气压传感器输出差值及根据视觉解算得到的载体姿态角变化值与预设阈值之间的关系，利用不同的方法进行载体姿态和位置的更新计算。

具体地，根据不同的场景使用不同的方法，具体包括下述的(a)、(b)及(c)三种方法，以获取最优的结果。相邻时刻气压差值较小、姿态变化较大时，使用下面的(a)方法。气压差值较小、姿态变化也较小时，使用下面的(b)方法。气压差值较大时，则使用下面的(c)方法。

(a)方法：将相邻时刻的姿态变化关系，叠加到上一时刻载体相对于世界坐标系的姿态变化中，获得最新的载体姿态角信息。根据运动趋势，进行相机速度估计，并基于此速度进行位移递推，并将推算位移累加到上一时刻载体相对与世界坐标系的位置上，获取载体最新时刻的位置。

(b)方法：认为载体的姿态和高度在相邻时刻并未发生变化，使用上一时刻载体的姿态角作为本时刻的姿态角，上一时刻的高度值作为本时刻的高度值。将当前时刻的图像与上一时刻图像进行对比，使用光流法获取平面的运动速度，从而计算出平面位移，累加到上一时刻载体相对与世界坐标系的位置上，获取载体最新时刻的位置。

(c)方法：将相邻时刻的姿态变化关系，叠加到上一时刻载体相对于世界坐标系的姿态变化中，获得最新的载体姿态角信息。将相邻时刻的位移比例关系，根据载体姿态转换为世界坐标系下的载体位移比例关系，利用气压差换算得出的高度差值作为垂向的位移参考量，计算出世界坐标系下另外两轴的位移。将三轴的位移累加到上一时刻载体相对与世界坐标系的位置上，获取载体最新时刻的位置。

前述运动趋势的获取，如图2所示，由上一时刻的位移结合姿态信息换算得到。

在前述不同的三个方法中，气压传感器的用途不一样。根据气压传感器输出判断当前处于哪个飞行阶段，其中，在起飞降落阶段，气压高度差用于计算尺度因子s；在平飞阶段，气压高度被用于直接作为高度量测，用于计算高度值，结合飞行器的运动趋势建立的状态递推方程，进行滤波获得位置估算。因此，在本申请中，能够根据不同的飞行状态（阶段），使用灵活的视觉和气压高度组合方法，以获取最优的定位结果;

S3、状态更新过程

将更新后的载体最新位置、最新姿态，作为新的基准，用于下一时刻状态累积更新的基准。

实施例1

首先，定义三个坐标系：世界坐标系（w）、载体坐标系（b）和相机坐标系（c）。世界坐标系中，以初始时刻载体上相机的中心位置为零点，北、东、地向为三轴方向，满足右手定则。相机垂直下视，载体坐标系和相机坐标系均满足右手定则，固连重合，定义为前、右、下，中心位于相机的中心位置。由此，相机坐标系和载体坐标系间的旋转矩阵转换关系

恒定为单位阵

。初始时刻，相机坐标系和世界坐标系之间存在旋转矩阵转换关系

。

载体在初始时刻采集一帧图像并获取初始时刻的气压值，用于和下一时刻采集图像和气压值进行对比。初始化工作完成后，载体可以正常起飞进行作业。

当达到采样时间间隔条件时，采集新一帧图像并进行特征点匹配。将前后两幅图像进行特征提取，计算出关键点及其描述子。其中，关键点描述了像素的位置，描述子则描述了该像素与周边像素的灰度值等关系。

为了更好地理解和实施本发明，将特征点匹配的具体过程进行如下详述：

首先，取第一幅图像的关键点集合成集合1；再将两幅图进行像素灰度差分，确定两幅图像像素灰度一致点，将这些点集合成集合2；接着，取集合1中与集合2的差集，并将其中的点更新第一幅图像的关键点，这些关键点及其描述子重新构成第一幅图像的特征点。取差集，将两幅图像中相同位置相同像素的点去除，以避免机架等固连于机体平台的物体在画面中造成的干扰。

将两幅图像中的特征点，根据描述子最相似的原则进行匹配。使用RANSAC方法进行错误匹配点剔除，从而获得两幅图像中特征点的最佳匹配效果，该方法能够大大减少计算量，进而提高计算效率，使得飞行器定位更加准确快速。

错误匹配点剔除的具体过程如下：

由摄像机投影模型可知：

其中，K为相机的内参矩阵，与相机的硬件有关，

为

分别为投影点的像素齐次坐标，

为点M的世界坐标，

和

分别是M点在各自摄像机坐标系中的Z轴值，

和

是第一个摄像机坐标系相对于世界坐标系的旋转矩阵和位移向量，

表示一个矩阵，

和

第二个摄像机坐标系相对于世界坐标的旋转矩阵和位移向量。在运动视觉中，通常将世界坐标系临时取在第一个摄像机坐标系上，则上式可转换为如下形式：

其中，

表示单位矩阵，

和

分别表示旋转矩阵和平移向量；

为相机内参矩阵，

表示

矩阵乘以

矩阵，

表示

矩阵乘以

矩阵。由于临时世界坐标系取在第一个摄像机坐标系上，因此第一个相机坐标系与临时世界坐标系重合，R为I，t为0，即不用旋转也不用移动。

将上述两式联立可得：

其中，

表示矩阵的转置，

表示求逆后转置，

表示矩阵的叉乘。

取

随机选取10个匹配点对，即可根据最小二乘方法求得F矩阵的解。

在求得F矩阵后，将现有的匹配点集回带入方程，获得最大值最为后续判断的阈值：

Choose表示随机选取的10个匹配点对的数据集。

计算所有的匹配点对，满足下述要求的则作为内点对，其余的作为外点对：

从内点对中重新随机选取10对，进行上述计算循环。

若本次计算求得的Gate值小于上一次的Gate值，则选用本次计算获得F为最优模型，同时从本次满足要求的内点对中选取新的用于下一次迭代计算的10个匹配点对。RANSAC方法循环终止的条件是内点对数量稳定，或满足要求的内点对数少于10对。

本方案RANSAC方法中，内外点的判断采用直接判据。直接计算匹配点对代入基础矩阵公式 求得的值，以此判断模型匹配度。相比于计算投影极线与匹配点的误差，该方式计算量较小。同时Gate值不使用固定值，而采用根据实际情况生成并逐渐逼近的方式，可保证获取每次获取最优解，避免出现不满足阈值要求，直接无解退出的情况。

在剔除错误匹配点后，即可根据匹配关系求得F阵，从而根据F阵可以求解相机的R和 t，容易看出，R和t的求解存在一个比例系数的影响，其中R和R的逆矩阵相乘为单位阵，可以消除比例系数的影响。

令：

由此获取两幅图像的拍摄时刻，相机的相对姿态转换矩阵为

，其中上下标

和

分别表示本时刻和上一时刻的相机坐标系。一同求得的还有

，表示相机在这段时间间隔中，在前一时刻相机坐标系下的位移比例关系，该比例关系和真实的位移

之间存在一个尺度因子S影响，即

。

将相机相对姿态转换矩阵

乘以上一时刻相机坐标系和世界坐标系的旋转转换关系

，得到最新时刻的相机坐标系相对世界坐标系的转换关系

，即

。

根据如下姿态角与姿态转换矩阵时间的关系，得到更新后的姿态角信息：

其中，

分别表示载体俯仰、航向、横滚角。

将本时刻的姿态角

，与上一时刻的姿态角

进行比较，获得差值

。

将本时刻的气压传感器输出值

，与上一时刻的气压传感器输出

进行比较，获得差值

。

当气压差值小于预设阈值时且载体姿态角变化超过预定阈值时，采用如下的步骤，即（a）方法更新载体位置。这里所述的“预定阈值”是操作人员自定义的，具体到本实施例中，预定阈值可设定为“气压0.1hPa，姿态角变化1度”。

（a）方法：认为高度值没有变化，将前一更新周期中世界坐标系下的三轴位移

，除以间隔时间，获得前一更新周期中的运动速度

，根据前后时刻飞行器姿态的

的差值，进行补偿，重新构建本时刻的运动趋势，其中k为补偿系数，通常位于[0,2]区间内，优选为1，具体可根据实施过程中器件的精度来调整。

得到相应的运动趋势后，乘以相应的时间则可获取平面位移。累加到上一时刻的位置上，即可得到最新时刻的位置。在该（a）方法情况下，飞行器在同一高度情况下转弯，因此只需要通过视觉获得的姿态变化信息，对原有的偏航进行修正。

当气压差值小于预设阈值，且载体姿态角变化小于预定阈值时，采用如下步骤，即（b）方法更新载体位置。

（b）方法：将前一更新周期中世界坐标系下的三轴位移

，除以间隔时间，获得前一更新周期中的运动速度

，默认本时刻和上一时刻的运动趋势一致，得出如下状态递推方程：

。

该状态递推方程中，使用三维的运动趋势进行状态递推，递推后使用光流信息的输出作为平面位置量测，气压传感器输出求解的气压高度值作为高度量测，对于状态参数进行滤波，量测方程构建如下：

，

其中，

表示量测误差，取决于气压传感器的精度值；

为单位矩阵，

为世界坐标系下三维位置的量测值。通过气压传感器的输出

，可以换算为高度的量测值

，结合当前气压高度值，配合当地地面高修正，通过光流运算，可以得到x和y方向上的速度，将其乘以时间可以得到

，累加到上一时刻位置

上，即可得到

的值。

由此，通过一次状态递推+一次量测更新，即可得到当前最新时刻的位置。在该（b）方法情况下，飞行器在匀速运动状态，因此可以使用上一时刻的运动趋势进行位置递推。

当气压差值超过预设阈值时，采用如下步骤，即方法（c）更新载体位置。

（c）方法：根据前后两个时刻气压传感器的输出，分别求得两个时刻的高度值

和

，得到高度差

。

前后两个时刻之间载体的位移在世界坐标系纵轴下的投影，理想情况下等于该高度差：

即

，

其中，

表示上一时刻相机坐标系和世界坐标系的旋转转换关系。

由此，我们可以得到尺度因子s，以及真实世界坐标系下的位移

。

根据上一时刻的位置，叠加上位移，获取当前时刻的位置信息，即

。

综上，本发明的飞行器定位方法能够更好地结合飞行器的实际情况，将飞行器分为起飞降落阶段（c方法）、平飞阶段（b方法）、转弯阶段（a方法），各阶段采用不同的方法进行位置估算，在高度变化较大的起飞降落阶段能够准确计算出尺度因子s；在平飞和转弯阶段，气压高度则被用于高度量测，结合飞行器的运动趋势建立的状态递推方程，进行滤波获得位置估算。本发明中通过引入视觉和气压信息进行组合定位，大大提升了此类拒止条件下飞行器定位的可靠性和可信度，获取最优的定位结果。

以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。

Claims (5)

1.一种基于视觉和气压信息融合的飞行器定位方法，其特征在于，包括如下步骤：

S1、初始化设置和数据采集：构建坐标系，相机采集初始时刻的基准图像，气压传感器获取初始时刻的气压值；

S2、相邻时刻位姿解算：达到状态更新条件时，系统采集最新时刻数据，进行位置和姿态解算，确定飞行器状态更新方法，获取最新时刻的位置，对飞行器进行定位；

具体地，包括如下三种场景：

（a）气压差值小于预设阈值且载体姿态角变化超过预定阈值；

（b）气压差值小于预设阈值且载体姿态角变化小于预定阈值；

（c）气压差值超过预设阈值；

其中，场景（a）的更新计算方法为（a）方法：

首先，根据前一更新周期中世界坐标系下的三轴位移

和间隔时间，获得前一更新周期中的运动速度

；

再根据前后时刻飞行器姿态的

的差值，进行补偿，重新构建本时刻的运动趋势：

其中k为补偿系数，

和

分别表示平面x和y方向的运动速度趋势，据此得到最新时刻的位置；

场景（b）的更新方法为（b）方法：

根据前一更新周期中世界坐标系下的三轴位移

和间隔时间

，获得前一更新周期中的运动速度

，默认本时刻和上一时刻的运动趋势一致，得出状态递推方程：

，

其中，

表示噪声向量；

使用光流信息的输出作为平面位置量测，气压传感器输出求解的气压高度值作为高度量测，对状态参数进行滤波，量测方程构建如下：

，

其中，

表示量测误差，取决于气压传感器的精度值；

为单位矩阵，；

，为世界坐标系下三维位置的量测值，通过气压传感器的输出当前位置的气压值

，换算为高度的量测值

；结合当前气压高度值，配合当地地面高修正，通过光流运算，得到x和y方向上的速度，进而得到

，累加到上一时刻位置

上，得到

的值，即为当前最新时刻的位置；

场景（c）的更新方法为（c）方法：

根据相邻两个时刻气压传感器的输出，分别求得高度值

和

，得到高度差

；

相邻两个时刻之间载体的位移在世界坐标系纵轴下的投影，理想情况下等于该高度差：

即

，

其中，

表示上一时刻相机坐标系和世界坐标系的旋转转换关系，

表示k-1时刻相机坐标系下，三轴的真实位移，

表示k-1时刻相机坐标系下，三轴的真实位移比例关系；

进而得到尺度因子s，以及真实世界坐标系下的位移

；

根据上一时刻的位置，叠加位移，获取当前时刻的位置信息

；

S3、状态更新：将更新后的最新位置和最新姿态，作为新的基准，用于下一时刻状态累积更新的基准，迭代至步骤S2，循环递推获取实时定位。

2.根据权利要求1所述的一种基于视觉和气压信息融合的飞行器定位方法，其特征在于，所述步骤S1中，构建的坐标系包括：世界坐标系、载体坐标系和相机坐标系；所述气压传感器的安装位置靠近相机，所述相机镜头向下安装。

3.根据权利要求1所述的一种基于视觉和气压信息融合的飞行器定位方法，其特征在于，所述步骤S2包括如下子步骤：

（1）通过气压传感器获得当前最新时刻与上一时刻的气压差值；

（2）相机采集新的一帧图像，并使用特征点匹配的方法与上一时刻图像进行对比分析，获取相机相邻时刻的姿态角变化及位移比例关系；

（3）判断相邻时刻气压传感器输出的气压差值，并根据视觉解算得到的载体姿态角变化值与预设阈值之间的关系，视不同场景采用不同的方法进行载体姿态和位置更新计算，对飞行器进行定位。

4.根据权利要求3所述的一种基于视觉和气压信息融合的飞行器定位方法，其特征在于，所述特征点匹配方法中，首先计算两幅图像中的特征点，随后剔除两幅图像中的相同像素位置的特征点，进而依据特征点的描述子进行特征点匹配，最后，使用改进的随机采样方法，对于错误匹配进行剔除。

5.根据权利要求1所述的一种基于视觉和气压信息融合的飞行器定位方法，其特征在于，所述预设阈值为：气压差值为0.1hPa，姿态角变化为1度。

Images