一种多光谱图像和惯导融合的定位方法和计算系统

技术领域

本发明属于无人系统定位方法的技术领域。

背景技术

无人系统自主定位技术是智能机器人、无人驾驶领域的关键技术。在GPS不可用的环境中，无人系统主要基于视觉或激光雷达进行自主定位。大部分现有的基于视觉的无人系统自主定位技术只能在光照良好，视野清晰的情况下运行。基于激光雷达的自主定位技术在烟尘雨雾遮挡的环境中会出现位姿估计失败的问题。

针对昏暗、烟尘雨雾遮挡环境下无人系统位姿估计失败的问题，现有研究主要采用红外相机定位、可见光相机与惯性传感器融合定位、可见光相机与红外相机融合定位三种定位方法。其中，红外相机定位方法中，因红外图像存在分辨率低、信噪比低、纹理不清晰的缺陷，容易导致红外图像特征跟踪丢失，定位失效，并且单一依靠红外相机进行位姿估计的方法还存在鲁棒性不足的问题；可见光相机与惯性传感器融合定位方法可根据纯惯导定位应对短时间的视觉失效，但在长时间的昏暗环境下运行时，由于缺乏视觉约束，容易发生定位漂移的问题，且时间越长，惯导累计的零偏误差越大，导致定位精度低下，同时该方法不能有效利用视觉的感知信息，不能对惯导观测添加视觉约束，无法校正惯导误差；可见光相机与红外相机融合位姿估计方法中，可见光相机与红外相机融合模式较为固定，不能根据环境变化做出调整，无法在复杂多变的环境中实现精准定位，如在光线剧烈变化的场景中，可见光图像的跟踪精度较差，且按照单一融合方式将可见光图像进行融合将有损系统的定位精度。

总的来说，在昏暗条件下或光照多变等复杂场景下，由于成像设备捕获的光子数量有限，成像后的图像普遍存在能见度较差、细节丢失严重等问题，直接影响了现有方法中无人系统的定位性能，是无人系统定位方法中亟需解决的技术问题。

发明内容

针对现有技术的缺陷，本发明的目的在于提供一种通过多光谱图像与惯导融进行精准定位的方法，该方法可实现无人系统在昏暗或者光照多变场景下的精准定位。

本发明的技术方案如下：

一种多光谱图像和惯导融合的定位方法，其包括：

S1对获得的当前帧可见光图像及红外图像分别进行特征提取，得到可见光图像特征点和红外图像特征点，并将当前帧可见光图像特征点及红外图像特征点分别与上一帧的可见光图像特征点和红外图像特征点进行特征匹配，匹配完成后，根据当前帧可见光图像及红外图像的亮度、纹理及其中稳定跟踪的特征点的数量确定当前帧可见光图像与红外图像的融合权重；

S2对获得的惯导测量数据进行预积分，获得预积分数据；

S3根据所述当前帧可见光图像、红外图像及所述预积分数据，进行相机和惯导的联合初始化，获得视觉-惯性的联合初始化值，并将联合初始化过程中建立的关键帧及其三维特征点加入局部地图中；

S4基于所述当前帧可见光图像与红外图像的融合权重及所述视觉-惯性的联合初始化值，构建含有帧状态量和三维特征点状态量的优化状态量，基于所述优化状态量，通过紧耦合的视觉-惯性里程计获得局部地图中全部帧的优化位姿和三维特征点的优化位置；其中，所述帧状态量含有位姿信息和惯导参数信息，所述三维特征点状态量含有三维特征点的位置信息；

S5基于优化后的局部地图中的关键帧的帧状态量和三维特征点状态量，通过词袋检测法进行回环检测；

S6当在所述回环检测中检测到了回环，则将局部地图内的三维特征点与回环帧进行匹配，建立回环帧与局部地图中的三维特征点的特征匹配关系，并据此进行重定位优化，得到重定位优化后的局部地图中的帧状态量和三维特征点状态量；

S7在完成所述重定位优化后，进行全局位姿优化，所述全局位姿优化只优化所有关键帧的位姿不优化三维特征点状态量。

根据本发明的一些具体实施方式，所述多光谱图像和惯导融合的定位方法还包括：

S8在每新建一帧关键帧后，将该关键帧的相关数据加入关键帧数据库中，所述相关数据包括组成该关键帧的可见光图像和红外图像的特征点、词袋向量，及该帧的位姿，其中，各关键帧的位姿在每一次优化后进行更新，由更新后的每一帧关键帧的位姿作为定位系统的输出轨迹。

根据本发明的一些具体实施方式：所述S1包括：

S11采用角点检测法分别提取RGB形式的当前帧可见光图像和含红外测试温度T的当前帧红外图像的特征，得到可见光特征点和红外特征点；

S12将提取得到的可见光特征点和红外特征点分别与上一帧的可见光特征点和红外特征点进行特征匹配，标记在连续多帧图像中均匹配成功的特征点为稳定跟踪的特征点；

S13将当前帧可见光图像进行亮度的定量分析，获得该可见光图像的当前帧亮度均值和基于该亮度均值设置的亮度权重因子；

S14将当前帧可见光图像进行结构纹理的定量分析，获得该可见光图像的当前帧与上一帧的结构相似度和基于该结构相似度设置的纹理权重因子；

S15根据所述亮度权重因子、纹理权重因子及所述稳定跟踪的特征点的数量，确定可见光图像和红外图像的自适应融合权重α和β。

根据本发明的一些具体实施方式：所述自适应融合权重α和β通过以下权重计算模型确定：

β＝1-α

其中，W

其中，I

其中，SSIM表示SSIM值。

根据本发明的一些具体实施方式：所述S3包括：

S31对可见光相机和红外相机进行纯视觉初始化，所述纯视觉初始化包括位姿初始化和相机的外参初始化，得到位姿初始化值及不同相机间的平移外参的初始化值；

S32通过紧耦合方式对惯导系统进行纯惯导初始化，得到惯导参数的初始化值；

S33对可见光相机、红外相机、惯导系统进行视觉-惯性联合初始化，得到联合后的位姿及惯导参数初始化值，即所述视觉-惯性的联合初始化值。

根据本发明的一些具体实施方式：所述S31包括：

根据当前帧可见光图像或红外图像F

计算当前帧可见光图像或红外图像F

遍历堆栈中的过往帧，若所得三维特征点和堆栈中的过往帧的二维特征点匹配足够多，则通过PnP算法恢复过往帧的位姿；

通过BA优化完善初始化过程所有帧的位姿，完成所述位姿初始化，得到纯视觉初始化过程中所有帧的位姿初始值；

求解如下的参数关系式，获得可见光相机和红外相机之间的旋转和平移外参估计值：

即：

其中，

将可见光相机与红外相机进行尺度统一，包括：

获得如下式所示的红外相机和可见光相机之间尺度的比值s

其中，

根据所得比值s

尺度统一后，设

其中，

通过最小二乘法求解所述第一优化问题，以得到的平移量

根据本发明的一些具体实施方式：所述S32包括：

以离线标定的惯导偏差作为惯导系统的参数中加速度计的偏置b

定义纯惯导初始化中待优化的状态量为X

X

其中，s表示可见光相机的尺度；R

构建惯导的相邻帧之间的预积分残差，如下：

/>

其中，

根据预积分残差构建惯导参数的第一优化函数，如下：

其中，

通过最小二乘法求解所述第一优化函数，以所得惯导参数的最优估计值作为惯导参数的初始化值。

根据本发明的一些具体实施方式：所述S33包括：

S331在完成所述纯惯导初始化后，根据解算得到的重力方向R

S332根据纯惯导初始化中估计得到的可见光相机的尺度s，将有尺度的可见光相机、红外相机、三维特征点的位置坐标恢复到米制尺度下；

S333基于纯惯导初始化获得的惯导参数的初始化值，在第二优化问题即联合优化的优化问题中添加视觉残差项，对视觉估计的位姿和惯导参数进行联合优化，以获得的位姿和惯导参数的联合最优估计作为联合后的初始化值。

根据本发明的一些具体实施方式：所述S333包括：

(1)设视觉-惯性联合初始化过程中，第i帧的状态量为S

(2)构建惯导预积分残差

(3)根据所得惯导预积分残差

/>

其中，l表示三维特征点的观测数；

(4)通过最小二乘法求解所述联合优化函数，以获得的最优参数向量X

根据本发明的一些具体实施方式：所述紧耦合的视觉-惯性里程计的计算过程包括：

(1)构建惯导的预积分残差，如下：

其中，

(2)将局部地图中的第j个三维特征点投影到第i帧可见光图像和红外图像中，分别构建可见光观测残差

其中，

将所述惯导预积分残差

其中，

通过最小二乘法求解所述第二优化函数，获得当前帧位姿、局部地图中其他帧位姿和三维特征点位置的最优估计。

根据本发明的一些具体实施方式：所述重定位优化包括：

设局部地图中有n个三维特征点与回环帧成功匹配，且成功匹配的n个三维特征点的状态量为

将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到局部地图中第i帧的可见光图像和红外图像中，分别构建第一可见光观测残差和红外观测残差，如下：

(3)将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到回环帧的可见光图像和红外图像中，分别构建第二可见光观测残差和红外观测残差，如下：

其中，

(4)定义重定位优化状态量包括局部地图中第i帧状态量

其中，

通过最小二乘法求解所述第三优化问题，获得重定位后当前帧位姿的最优估计，并消除局部地图中其他关键帧位姿和其他三维特征点位置的累计误差。

根据本发明的一些具体实施方式：所述回环帧的可见光图像和红外图像的权重系数α

β

其中，I

本发明进一步提供了可实现上述定位方法的定位计算系统，其包括：对可见光图像和红外图像进行特征点提取、对惯导测量数据进行预积分，并采用光流法进行特征点追踪的前端；根据环境变化，动态调整可见光图像和红外图像的融合权重的自适应调整模块；将不同帧可见光特征点和红外特征点分别进行匹配的匹配模块；完成所述相机和惯导的联合初始化的初始化模块；进行所述紧耦合的视觉-惯性里程计的计算的视觉-惯性紧耦合里程计模块；实现所述回环检测和重定位的回环检测和重定位模块；对重定位后的全局位姿图进行优化，保证重定位后的全局一致性，并输出最终定位位姿的全局优化及输出模块。

针对红外图像存在分辨率低、信噪比低、纹理不清晰的缺陷及可见光相机与惯导融合后在昏暗环境中无法有效利用视觉观测的问题，本发明的上述定位方法融合了红外相机、可见光相机及惯导观测，不仅为定位系统提供了更丰富多源的感知信息，同时可使系统在昏暗环境中利用红外相机的观测信息，并通过红外相机的观测修正惯导误差。

针对可见光相机与红外相机融合模式固定，不能根据环境变化做出调整的问题，本发明的上述定位方法可将可见光图像和红外图像进行自适应融合，根据环境变化调整可见光图像和红外图像融合权重，避免了误差较大的观测对系统精度的损害。

本发明具有以下有益效果：

(1)本发明可融合可见光图像及红外图像信息，感知来源的信息更广，使无人系统在复杂环境尤其是昏暗环境下定位性能更好；

(2)根据本发明的一些具体实施方式，本发明可进行高效的视觉-惯性初始化，能够快速地将多相机位姿估计及惯导位姿估计对齐，并将可见光相机、红外相机、惯导三者尺度统一；

(3)相对于现有的定位方案中视觉惯性紧耦合里程计仅利用可见光图像构造视觉残差，低照度环境下可见光图像观测误差较大，根据本发明的一些具体实施方式，本发明的视觉惯性紧耦合里程计同时构造了可见光图像和红外图像的视觉残差，并且能根据环境变化自适应地调整可见光图像残差和红外图像残差融合权重，得到更高精度的视觉惯性紧耦合里程计；

(4)相对于现有的回环检测中仅通过可见光图像检测重复经过的地点，在光照剧烈变化的情况下很可能造成漏检，根据本发明的一些具体实施方式，本发明的回环检测可同时检测可见光图像和红外图像的相似程度，检测效果更好；

(5)相对于现有的的重定位方法只根据可见光图像进行特征匹配，昏暗场景或光照变化容易导致重定位失败，根据本发明的一些具体实施方式，本发明的重定位可同时对可见光图像和红外图像进行特征匹配，自适应地调整可见光图像残差和红外图像残差融合权重，能够确保复杂环境下重定位的可靠性。

附图说明

图1为具体实施方式中所述本发明的融合框架示意图。

图2为具体实施方式中所述视觉-惯性联合初始化过程示意图。

图3为具体实施方式中所述视觉-惯性紧耦合里程计过程示意图。

图4为具体实施方式中所述重定位过程示意图。

具体实施方式

以下结合实施例和附图对本发明进行详细描述，但需要理解的是，所述实施例和附图仅用于对本发明进行示例性的描述，而并不能对本发明的保护范围构成任何限制。所有包含在本发明的发明宗旨范围内的合理的变换和组合均落入本发明的保护范围。

参照附图1，根据本发明的技术方案，所述多光谱图像和惯导融合的定位方法的一些具体实施方式包括如下步骤：

S1对无人系统获得的当前帧可见光图像及红外图像分别进行特征提取，得到可见光图像特征点和红外图像特征点，并将当前帧可见光图像特征点及红外图像特征点分别与上一帧的可见光图像特征点和红外图像特征点进行特征匹配，匹配完成后，根据当前帧可见光图像及红外图像的亮度、纹理及其中稳定跟踪的特征点的数量确定当前帧可见光图像与红外图像的融合权重。

进一步的，在一些具体实施例中，S1进一步包括：

S11采用角点检测法分别提取RGB形式的当前帧可见光图像和含红外测试温度T的当前帧红外图像的特征，得到可见光特征点和红外特征点。

在一些具体实施例中，角点检测法可通过Fast角点检测器实现。

S12将提取得到的可见光特征点和红外特征点分别与上一帧的可见光特征点和红外特征点进行特征匹配，标记在连续多帧图像中均匹配成功的特征点为稳定跟踪的特征点，匹配方法为：对于可见光特征点，当特征点之间的可见光信息差值小于阈值时，特征匹配成功，对于红外特征点，当特征点之间的热感信息差值小于阈值时，特征匹配成功。

进一步的，在一具体实施例中，所述连续多帧设置为7帧。

S13将当前帧可见光图像进行亮度的定量分析，获得该可见光图像的当前帧亮度均值和基于该亮度均值设置的亮度权重因子。

进一步的，在一些具体的实施例中，所述亮度的定量分析包括：

将所述可见光图像进行灰度转换，将其三通道RGB图像转换为8位的单通道灰度图像，根据转换后的灰度图计算亮度均值，获得所述当前帧亮度均值；

基于所得亮度均值，根据以下亮度权重因子计算模型获得当前帧亮度权重因子W

其中，I

根据该计算模型，若当前帧亮度均值和上一帧亮度均值之差超过50，则认为图像间的亮度变化较大。

S14将当前帧可见光图像进行结构纹理的定量分析，获得该可见光图像的当前帧与上一帧的结构相似度和基于该结构相似度设置的纹理权重因子。

进一步的，在一些具体的实施例中，所述结构纹理的定量分析包括：

计算当前帧可见光图像与其上一帧可见光图像的SSIM值，以该SSIM值作为结构相似度的度量；

基于所得SSIM值，根据以下纹理权重因子计算模型获得当前帧可见光图像的纹理权重因子W

根据该计算模型，若当前帧和上一帧结构相似度低于0.25，则认为结构纹理变化较大。

S15根据所述亮度权重因子、纹理权重因子及所述稳定跟踪的特征点的数量，确定可见光图像和红外图像的自适应融合权重α和β。

进一步的，在一些具体实施例中，所述自适应融合权重α和β通过以下权重计算模型确定：

β＝1-α

其中，S

通过以上权重计算模型，本发明可根据可见光图像亮度变化、结构纹理变化、特征点在可见光图像和红外图像中追踪成功的帧数，动态地调整特征追踪时可见光图像和红外图像的权重。

S2对无人系统获得的惯导测量数据进行预积分；

S3根据当前帧可见光图像、红外图像及预积分数据，进行无人系统的相机和惯导的联合初始化，获得视觉-惯性的联合初始化值，并将联合初始化过程中建立的当前帧、其之前的关键帧及关键帧中的三维特征点加入局部地图中。

进一步的，在一些具体实施例中，S3包括：

S31对可见光相机和红外相机进行纯视觉初始化，所述纯视觉初始化包括位姿初始化和相机的外参初始化，得到位姿初始化值及不同相机间的平移外参的初始化值。

进一步的，在一些具体实施例中，S31包括：

根据当前帧可见光或红外图像F

计算当前帧可见光或红外图像F

遍历堆栈中的过往帧，若所得三维特征点和堆栈中的过往帧的二维特征点匹配足够多，则通过PnP算法恢复过往帧的位姿；

通过BA优化完善初始化过程所有帧的位姿，完成所述位姿初始化，得到纯视觉初始化过程中所有帧的位姿初始值；

求解如下的参数关系式，获得可见光相机和红外相机之间的旋转和平移外参估计值：

即：

其中，

将可见光相机与红外相机进行尺度统一，包括：

获得如下式所示的红外相机和可见光相机之间尺度的比值s

其中，

根据所得比值s

尺度统一后，设

其中，

通过调用Ceres库的LM法求解得

以上过程中，通过位姿初始化估计得到的各图像帧及其中的各特征点的位置带有相机的尺度信息，而红外相机的尺度和可见光相机的尺度并不一样，因此本发明进一步将红外相机的尺度统一到可见光相机的尺度下。

S32通过紧耦合方式对惯导系统进行纯惯导初始化，得到惯导参数的初始化值。

进一步的，在一些具体实施例中，S32包括：

以离线标定的惯导偏差作为惯导系统的参数中加速度计的偏置b

定义纯惯导初始化中待优化的状态量为X

X

其中，s表示可见光相机的尺度；R

构建惯导的相邻帧之间的预积分残差，如下：

其中，

根据预积分残差构建惯导参数的优化函数，如下：

其中，

通过调用Ceres库LM法求解上述纯惯导优化步骤中的优化函数，获得惯导参数的最优估计，以所得最优估计作为惯导参数的初始值。

本步骤采用紧耦合方式进行惯导初始化，可对所有惯导参数同步求解，保证了估计的准确性。

S33对可见光相机、红外相机、惯导系统进行视觉-惯性联合初始化，得到联合后的位姿及惯导参数初始化值，即所述视觉-惯性的联合初始化值。

进一步的，参照附图2，在一些具体实施例中，S33包括：

S331纯惯导初始化过程后，根据解算得到的重力方向R

S332根据纯惯性初始化估计得到的可见光相机的尺度s，将有尺度的可见光相机、红外相机、三维特征点的位置坐标恢复到米制尺度下；

S333以纯惯导初始化获得的惯导参数的最优估计作为优化的初值，在优化问题中添加视觉残差项，对视觉估计的位姿和惯导参数进行联合优化，以获得的位姿和惯导的参数的联合最优估计作为联合后的初始化值，其可具体包括如：

(1)设视觉-惯性联合初始化过程中，第i帧的状态量为S

(2)在构造优化函数之前，先构建惯导预积分残差

(3)根据所得惯导预积分残差

其中，假设初始化过程中有k+1帧，总共观测到了l个三维特征点。

(4)通过调用Ceres库LM法求解完上述联合优化函数，以获得的最优参数X

S34将联合初始化过程中建立的关键帧帧和三维特征点加入到局部地图中。

其中，局部地图是指当前帧周围的地图，由当前帧(不包括当前帧的特征点)和当前帧之前的若干帧关键帧以及这些帧所观测到的特征点组成，即除了当前帧外，局部地图中的所有帧都是关键帧。

其中，所述关键帧可通过如下机制建立：若当前帧与上一帧关键帧之间的视差较大，或者距离上一帧关键帧的距离较远，则将当前帧建立为关键帧。若当前帧被建立为关键帧，则三角化当前帧提取到的新特征点，将当前帧以及当前帧的新特征点加入到局部地图中，若当前帧不是关键帧，则在解算完当前帧位姿后，将当前帧从局部地图中剔除。此外，距离最新帧太远的关键帧以及该帧的特征点将会从局部地图中移除。

S4基于所述当前帧可见光图像与红外图像的融合权重及所述视觉-惯性的联合初始化值，构建含有帧状态量和三维特征点状态量的优化状态量，基于所述优化状态量，通过紧耦合的视觉-惯性里程计(VIO)获得局部地图中全部帧的优化位姿和三维特征点的优化位置；其中，所述帧状态量含有位姿信息和惯导参数信息，所述三维特征点状态量含有三维特征点的位置信息。

其中，进一步的，局部地图中第i帧的帧状态量定义为

三维特征点的状态量定义为x

则紧耦合的视觉-惯性里程计的优化状态量

进一步的，在一些具体实施例中，参照附图3，所述紧耦合的视觉-惯性里程计(VIO)的计算过程包括：

(1)采用与所述纯惯导初始化优化中构建所述相邻帧之间的预积分残差相同方式构建惯导预积分残差，如下：

/>

其中，

(2)为充分利用可见光图像和红外图像信息，将局部地图中的第j个三维特征点投影到第i帧可见光图像和红外图像中，分别构建可见光观测残差

其中，

(3)将所述惯导预积分残差

其中，

(4)通过调用Ceres库LM法求解上述视觉-惯性紧耦合里程计的优化函数，获得当前帧位姿、局部地图中其他帧位姿和三维特征点位置的最优估计。

S5基于优化后的局部地图中的关键帧的帧状态量和三维特征点状态量，进行回环检测。

进一步的，在一些具体实施例中，采用基于DBoW库的词袋检测方法在关键帧数据库寻找当前帧的潜在回环帧，回环检测时两帧的相似度函数定义如下：

S

其中，S

β

其中，I

若S

通过以上实施方式，本发明不仅考虑了当前帧和潜在回环帧之间可见光图像的相似度，还同时考虑了红外图像的相似度。

S6若在所述回环检测中检测到了回环，则将局部地图内的三维特征点与回环帧进行匹配，建立回环帧与局部地图中的三维特征点的特征匹配关系，并据此进行无人系统的重定位优化，得到重定位优化后的局部地图中帧状态量和三维特征点状态量。

进一步的，在一些具体实施例中，参照附图4，所述重定位优化包括：

(1)假设局部地图中有n个三维特征点与回环帧成功匹配，为方便表示，设成功匹配的n个三维特征点的状态量为

(2)与视觉惯性紧耦合里程计视觉残差构建方法类似，将局部地图与回环帧匹配的特征点集的第j个特征点x

(3)将局部地图与回环帧匹配的特征点集的第j个特征点x

其中，

(4)定义重定位优化状态量包括局部地图中第i帧状态量S

其中，假设局部地图中有k+1帧，局部地图与回环帧匹配了n个三维特征点。

重定位优化问题的前三项和紧耦合视觉惯导里程计相同，充分利用了局部地图信息用于优化求解当前帧位姿，最后两项利用了回环帧的历史信息，为当前帧位姿优化求解添加了回环约束。

通过调用Ceres库LM法求解上述重定位步骤的优化函数，获得重定位后当前帧位姿的最优估计，并消除局部地图中其他关键帧位姿和其他三维特征点位置的累计误差。

S7在完成所述重定位优化后，进行全局位姿优化，保证重定位后的全局一致性，其中，全局位姿优化只优化所有关键帧的位姿不优化三维特征点状态量。

进一步的，在一些具体实施例中，采用四自由度位姿图优化的方法调整所有关键帧位姿的x坐标、y坐标、z坐标和航向角。

S8每新建一帧关键帧，将该关键帧加入关键帧数据库中，其存储的数据包括可见光图像和红外图像的特征点、词袋向量，以及该帧的位姿。关键帧数据库中关键帧位姿会在每一次优化后更新，最后定位系统输出的轨迹即为关键帧数据库中每一帧关键帧的位姿。

以上实施例仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例。凡属于本发明思路下的技术方案均属于本发明的保护范围。应该指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下的改进和润饰，这些改进和润饰也应视为本发明的保护范围。