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

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

Info

Publication number
CN116007609A
CN116007609A CN202211632072.9A CN202211632072A CN116007609A CN 116007609 A CN116007609 A CN 116007609A CN 202211632072 A CN202211632072 A CN 202211632072A CN 116007609 A CN116007609 A CN 116007609A
Authority
CN
China
Prior art keywords
frame
visible light
inertial navigation
infrared
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211632072.9A
Other languages
English (en)
Inventor
蒋朝阳
叶学通
郑晓妮
高九龙
兰天然
孙海滨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202211632072.9A priority Critical patent/CN116007609A/zh
Publication of CN116007609A publication Critical patent/CN116007609A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Image Analysis (AREA)

Abstract

本发明公开了一种多光谱图像和惯导融合的定位方法和计算系统，所述定位方法包括：对当前帧可见光及红外图像进行二维特征点提取及特征点的同源匹配，根据匹配情况及环境等对图像的影响因素确定可见光和红外图像的融合权重；进行可见光、红外相机和惯导的联合初始化；通过紧耦合的视觉‑惯性里程计获得可见光图像、红外图像及惯导融合的局部地图优化位姿及含有空间坐标系信息的三维特征点的优化位置。本发明可融合可见光相机、红外相机和惯导信息，解决了无人系统在昏暗、光照多变等复杂场景下定位难的问题。

Description

一种多光谱图像和惯导融合的定位方法和计算系统
技术领域
本发明属于无人系统定位方法的技术领域。
背景技术
无人系统自主定位技术是智能机器人、无人驾驶领域的关键技术。在GPS不可用的环境中，无人系统主要基于视觉或激光雷达进行自主定位。大部分现有的基于视觉的无人系统自主定位技术只能在光照良好，视野清晰的情况下运行。基于激光雷达的自主定位技术在烟尘雨雾遮挡的环境中会出现位姿估计失败的问题。
针对昏暗、烟尘雨雾遮挡环境下无人系统位姿估计失败的问题，现有研究主要采用红外相机定位、可见光相机与惯性传感器融合定位、可见光相机与红外相机融合定位三种定位方法。其中，红外相机定位方法中，因红外图像存在分辨率低、信噪比低、纹理不清晰的缺陷，容易导致红外图像特征跟踪丢失，定位失效，并且单一依靠红外相机进行位姿估计的方法还存在鲁棒性不足的问题；可见光相机与惯性传感器融合定位方法可根据纯惯导定位应对短时间的视觉失效，但在长时间的昏暗环境下运行时，由于缺乏视觉约束，容易发生定位漂移的问题，且时间越长，惯导累计的零偏误差越大，导致定位精度低下，同时该方法不能有效利用视觉的感知信息，不能对惯导观测添加视觉约束，无法校正惯导误差；可见光相机与红外相机融合位姿估计方法中，可见光相机与红外相机融合模式较为固定，不能根据环境变化做出调整，无法在复杂多变的环境中实现精准定位，如在光线剧烈变化的场景中，可见光图像的跟踪精度较差，且按照单一融合方式将可见光图像进行融合将有损系统的定位精度。
总的来说，在昏暗条件下或光照多变等复杂场景下，由于成像设备捕获的光子数量有限，成像后的图像普遍存在能见度较差、细节丢失严重等问题，直接影响了现有方法中无人系统的定位性能，是无人系统定位方法中亟需解决的技术问题。
发明内容
针对现有技术的缺陷，本发明的目的在于提供一种通过多光谱图像与惯导融进行精准定位的方法，该方法可实现无人系统在昏暗或者光照多变场景下的精准定位。
本发明的技术方案如下：
一种多光谱图像和惯导融合的定位方法，其包括：
S1对获得的当前帧可见光图像及红外图像分别进行特征提取，得到可见光图像特征点和红外图像特征点，并将当前帧可见光图像特征点及红外图像特征点分别与上一帧的可见光图像特征点和红外图像特征点进行特征匹配，匹配完成后，根据当前帧可见光图像及红外图像的亮度、纹理及其中稳定跟踪的特征点的数量确定当前帧可见光图像与红外图像的融合权重；
S2对获得的惯导测量数据进行预积分，获得预积分数据；
S3根据所述当前帧可见光图像、红外图像及所述预积分数据，进行相机和惯导的联合初始化，获得视觉-惯性的联合初始化值，并将联合初始化过程中建立的关键帧及其三维特征点加入局部地图中；
S4基于所述当前帧可见光图像与红外图像的融合权重及所述视觉-惯性的联合初始化值，构建含有帧状态量和三维特征点状态量的优化状态量，基于所述优化状态量，通过紧耦合的视觉-惯性里程计获得局部地图中全部帧的优化位姿和三维特征点的优化位置；其中，所述帧状态量含有位姿信息和惯导参数信息，所述三维特征点状态量含有三维特征点的位置信息；
S5基于优化后的局部地图中的关键帧的帧状态量和三维特征点状态量，通过词袋检测法进行回环检测；
S6当在所述回环检测中检测到了回环，则将局部地图内的三维特征点与回环帧进行匹配，建立回环帧与局部地图中的三维特征点的特征匹配关系，并据此进行重定位优化，得到重定位优化后的局部地图中的帧状态量和三维特征点状态量；
S7在完成所述重定位优化后，进行全局位姿优化，所述全局位姿优化只优化所有关键帧的位姿不优化三维特征点状态量。
根据本发明的一些具体实施方式，所述多光谱图像和惯导融合的定位方法还包括：
S8在每新建一帧关键帧后，将该关键帧的相关数据加入关键帧数据库中，所述相关数据包括组成该关键帧的可见光图像和红外图像的特征点、词袋向量，及该帧的位姿，其中，各关键帧的位姿在每一次优化后进行更新，由更新后的每一帧关键帧的位姿作为定位系统的输出轨迹。
根据本发明的一些具体实施方式：所述S1包括：
S11采用角点检测法分别提取RGB形式的当前帧可见光图像和含红外测试温度T的当前帧红外图像的特征，得到可见光特征点和红外特征点；
S12将提取得到的可见光特征点和红外特征点分别与上一帧的可见光特征点和红外特征点进行特征匹配，标记在连续多帧图像中均匹配成功的特征点为稳定跟踪的特征点；
S13将当前帧可见光图像进行亮度的定量分析，获得该可见光图像的当前帧亮度均值和基于该亮度均值设置的亮度权重因子；
S14将当前帧可见光图像进行结构纹理的定量分析，获得该可见光图像的当前帧与上一帧的结构相似度和基于该结构相似度设置的纹理权重因子；
S15根据所述亮度权重因子、纹理权重因子及所述稳定跟踪的特征点的数量，确定可见光图像和红外图像的自适应融合权重α和β。
根据本发明的一些具体实施方式：所述自适应融合权重α和β通过以下权重计算模型确定：
Figure BDA0004005974540000031
β＝1-α
其中，Wi为亮度权重因子，Ws为纹理权重因子，SR为可见光图像中稳定跟踪的特征点的数量，ST为红外图像中稳定跟踪的特征点的数量，且：
Figure BDA0004005974540000032
其中，Ii为第i帧的亮度均值，Ii-1为第i-1帧的亮度均值；
Figure BDA0004005974540000041
其中，SSIM表示SSIM值。
根据本发明的一些具体实施方式：所述S3包括：
S31对可见光相机和红外相机进行纯视觉初始化，所述纯视觉初始化包括位姿初始化和相机的外参初始化，得到位姿初始化值及不同相机间的平移外参的初始化值；
S32通过紧耦合方式对惯导系统进行纯惯导初始化，得到惯导参数的初始化值；
S33对可见光相机、红外相机、惯导系统进行视觉-惯性联合初始化，得到联合后的位姿及惯导参数初始化值，即所述视觉-惯性的联合初始化值。
根据本发明的一些具体实施方式：所述S31包括：
根据当前帧可见光图像或红外图像Fi，在系统的过往帧堆栈中寻找可见光或红外参考帧图像Fj；
计算当前帧可见光图像或红外图像Fi与参考帧图像Fj之间的基础矩阵和单应矩阵，并求解两帧之间的运动参数，根据获得的两帧之间的位姿变化信息，将可见光或红外图像Fi中的二维的特征点进行三角化，得到其在世界坐标系下的三维空间点，即三维特征点；
遍历堆栈中的过往帧，若所得三维特征点和堆栈中的过往帧的二维特征点匹配足够多，则通过PnP算法恢复过往帧的位姿；
通过BA优化完善初始化过程所有帧的位姿，完成所述位姿初始化，得到纯视觉初始化过程中所有帧的位姿初始值；
求解如下的参数关系式，获得可见光相机和红外相机之间的旋转和平移外参估计值：
Figure BDA0004005974540000042
即：
Figure BDA0004005974540000043
其中，
Figure BDA0004005974540000051
表示k,k+1相邻两帧之间的可见光相机的旋转估计值，
Figure BDA0004005974540000052
表示k,k+1相邻两帧之间红外相机的旋转估计值，以上两个旋转估计值可由完成所述位姿初始化后的图像的第k帧和第k+1帧的姿态计算得到；qRT为四元数表示的旋转外参估计值，
Figure BDA0004005974540000053
[·]L,[·]R表示四元数的左乘和右乘；通过SVD分解法，可以求解得到可见光相机到红外相机旋转外参qRT；
将可见光相机与红外相机进行尺度统一，包括：
获得如下式所示的红外相机和可见光相机之间尺度的比值sratio：
Figure BDA0004005974540000054
其中，
Figure BDA0004005974540000055
表示由红外相机估计得到的第i-1帧到第i帧图像的位移向量；
Figure BDA0004005974540000056
表示由可见光相机估计得到的第i-1帧到第i帧图像的有尺度的位移向量；||·||2表示取向量的二范数，n表示初始化过程中的帧数；各上标横线代表有尺度；
根据所得比值sratio，将红外相机坐标系下所有点以及所有帧的坐标同时除sratio，使其统一到可见光相机尺度下；
尺度统一后，设
Figure BDA0004005974540000057
为可见光相机第i帧在世界坐标系下的位置，
Figure BDA0004005974540000058
为红外相机第i帧在世界坐标系下的位置，根据
Figure BDA0004005974540000059
和
Figure BDA00040059745400000510
的坐标变换关系，构造以下第一优化问题：
Figure BDA00040059745400000511
其中，
Figure BDA00040059745400000512
表示在可见光相机尺度下，可见光相机到红外相机的平移量，RRT为旋转矩阵表示的可见光相机坐标系到红外相机坐标系的旋转，||·||2表示取向量的二范数；
通过最小二乘法求解所述第一优化问题，以得到的平移量
Figure BDA00040059745400000513
的值，作为可见光相机尺度下的平移外参的初始化值。
根据本发明的一些具体实施方式：所述S32包括：
以离线标定的惯导偏差作为惯导系统的参数中加速度计的偏置ba和陀螺仪的偏置bg的初始值；
定义纯惯导初始化中待优化的状态量为Xinertial-only：
Xinertial-only＝{s,Rwg,b,v0:k}
其中，s表示可见光相机的尺度；Rwg∈SO(3)表示重力方向，其中，SO(3)表示李群，则在世界坐标系下，重力可表达为g＝RwggI，其中gI＝(0,0,G)T表示重力向量，G是重力的幅值；
Figure BDA0004005974540000061
表示加速度计和陀螺仪的组合偏置，其中ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure BDA0004005974540000062
表示六维实向量；
Figure BDA0004005974540000063
表示在从第0帧到第k帧之间的k+1个时刻时，惯导在世界坐标系下运动的速度，该速度具有尺度信息，其中，
Figure BDA0004005974540000064
表示三维实向量；
构建惯导的相邻帧之间的预积分残差，如下：
Figure BDA0004005974540000065
Figure BDA0004005974540000066
Figure BDA0004005974540000067
其中，
Figure BDA0004005974540000068
分别表示惯导的第i-1帧到第i帧的旋转预积分、速度预积分、位置预积分的残差；ΔRi-1,i，Δvi-1,i，Δpi-1,i分别表示从惯导的第i-1帧到第i帧的预积分；Ri-1表示第i-1帧时的惯导姿态，Ri表示第i帧时的惯导姿态；vi-1表示第i-1帧时的惯导速度，vi表示第i帧时的惯导速度；Δti-1,i表示相邻惯导测量之间的时间间隔；
Figure BDA0004005974540000069
表示第i-1帧时的惯导位置，
Figure BDA00040059745400000610
表示第i帧时的惯导位置；log代表从李群到李代数的映射；
根据预积分残差构建惯导参数的第一优化函数，如下：
Figure BDA0004005974540000071
其中，
Figure BDA0004005974540000072
为预积分残差项，rp＝ba表示惯导先验残差，Σp和
Figure BDA00040059745400000710
分别为rp、
Figure BDA00040059745400000711
的协方差；
通过最小二乘法求解所述第一优化函数，以所得惯导参数的最优估计值作为惯导参数的初始化值。
根据本发明的一些具体实施方式：所述S33包括：
S331在完成所述纯惯导初始化后，根据解算得到的重力方向Rwg旋转世界坐标系，使世界坐标系z轴与重力方向一致，并通过Rwg将在原世界坐标系下所有帧的位姿以及三维特征点坐标变换至旋转后的世界坐标系下；
S332根据纯惯导初始化中估计得到的可见光相机的尺度s，将有尺度的可见光相机、红外相机、三维特征点的位置坐标恢复到米制尺度下；
S333基于纯惯导初始化获得的惯导参数的初始化值，在第二优化问题即联合优化的优化问题中添加视觉残差项，对视觉估计的位姿和惯导参数进行联合优化，以获得的位姿和惯导参数的联合最优估计作为联合后的初始化值。
根据本发明的一些具体实施方式：所述S333包括：
(1)设视觉-惯性联合初始化过程中，第i帧的状态量为Si＝{Ti,vi}，其中，Ti表示第i帧的位姿，vi表示第i帧的惯导速度，则对k+1帧进行的联合初始化中，所有帧的状态量为
Figure BDA0004005974540000073
设待优化的参数向量
Figure BDA0004005974540000074
其中，
Figure BDA0004005974540000075
表示初始化过程中的惯导组合偏置，ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure BDA0004005974540000076
表示六维实向量；
(2)构建惯导预积分残差
Figure BDA0004005974540000077
可见光观测残差
Figure BDA0004005974540000078
和红外观测残差
Figure BDA00040059745400000712
(3)根据所得惯导预积分残差
Figure BDA0004005974540000079
可见光观测残差
Figure BDA00040059745400000713
红外观测残差
Figure BDA00040059745400000714
及当前帧可见光图像与红外图像的融合权重，构建视觉-惯性联合优化函数，即所述第二优化问题，如下：
Figure BDA0004005974540000081
其中，l表示三维特征点的观测数；
Figure BDA0004005974540000082
表示初始化过程中观测到第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为第i帧可见光图像和红外图像的自适应融合权重；
Figure BDA0004005974540000083
为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
(4)通过最小二乘法求解所述联合优化函数，以获得的最优参数向量XVI_init作为视觉-惯性联合后的初始化值。
根据本发明的一些具体实施方式：所述紧耦合的视觉-惯性里程计的计算过程包括：
(1)构建惯导的预积分残差，如下：
Figure BDA0004005974540000084
Figure BDA0004005974540000085
Figure BDA0004005974540000086
Figure BDA0004005974540000087
其中，
Figure BDA0004005974540000088
为所述惯导预积分残差；ΔRi-1,i、Δvi-1,i、Δpi-1,i为第i-1帧到第i帧的惯导预积分值；
(2)将局部地图中的第j个三维特征点投影到第i帧可见光图像和红外图像中，分别构建可见光观测残差
Figure BDA00040059745400000811
和红外观测残差
Figure BDA00040059745400000812
如下：
Figure BDA0004005974540000089
Figure BDA00040059745400000810
其中，RΠ,
Figure BDA0004005974540000091
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换；Ti为第i帧的位姿；
Figure BDA0004005974540000092
为李群SE(3)在
Figure BDA0004005974540000093
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
将所述惯导预积分残差
Figure BDA0004005974540000094
可见光观测残差
Figure BDA00040059745400000910
红外观测残差
Figure BDA00040059745400000911
与可见光图像与红外图像的融合权重结合，构造视觉-惯性紧耦合里程计优化函数，即第二优化函数，如下：
Figure BDA0004005974540000095
其中，
Figure BDA0004005974540000096
表示能观测到局部地图中第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的自适应融合权重，ΣIi-1,i为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
通过最小二乘法求解所述第二优化函数，获得当前帧位姿、局部地图中其他帧位姿和三维特征点位置的最优估计。
根据本发明的一些具体实施方式：所述重定位优化包括：
设局部地图中有n个三维特征点与回环帧成功匹配，且成功匹配的n个三维特征点的状态量为
Figure BDA0004005974540000097
将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到局部地图中第i帧的可见光图像和红外图像中，分别构建第一可见光观测残差和红外观测残差，如下：
Figure BDA0004005974540000098
Figure BDA0004005974540000099
(3)将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到回环帧的可见光图像和红外图像中，分别构建第二可见光观测残差和红外观测残差，如下：
Figure BDA0004005974540000101
Figure BDA0004005974540000102
其中，RΠ,
Figure BDA0004005974540000103
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图与回环帧匹配的特征点集的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；Ruloop,j,Tuloop,j分别为局部地图与回环帧匹配的特征点集的第j个特征点在回环帧图像中的观测值，TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换可见光图像和红外图像中的观测值；Ti为第i帧的位姿，Tloop为回环帧的位姿；
Figure BDA0004005974540000104
为李群SE(3)在
Figure BDA0004005974540000105
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
(4)定义重定位优化状态量包括局部地图中第i帧状态量
Figure BDA0004005974540000106
回环帧的状态量
Figure BDA0004005974540000107
和三维特征点状态量xi＝{x,y,z}；设局部地图中有k+1帧，其状态为
Figure BDA0004005974540000108
设局部地图与回环帧成功匹配了n个三维特征点，匹配点集的状态量为
Figure BDA0004005974540000109
则重重定位优化状态量
Figure BDA00040059745400001010
其对应的重定位优化问题即第三优化问题设置如下：
Figure BDA00040059745400001011
其中，
Figure BDA00040059745400001012
表示能观测到局部地图与回环帧匹配的第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的自适应融合权重；αloop和βloop为回环帧的可见光图像和红外图像的权重系数；
Figure BDA0004005974540000111
为预积分残差的信息矩阵；RΣij,TΣij为局部地图中可见光观测残差和红外观测残差的信息矩阵，RΣloop,j,TΣloop,j为回环帧中可见光观测残差和红外观测残差的信息矩阵；
通过最小二乘法求解所述第三优化问题，获得重定位后当前帧位姿的最优估计，并消除局部地图中其他关键帧位姿和其他三维特征点位置的累计误差。
根据本发明的一些具体实施方式：所述回环帧的可见光图像和红外图像的权重系数αloop和βloop通过以下计算模型得到：
Figure BDA0004005974540000112
βloop＝1-αloop
其中，Icur为当前帧的亮度均值，Iloop为回环帧的亮度均值。
本发明进一步提供了可实现上述定位方法的定位计算系统，其包括：对可见光图像和红外图像进行特征点提取、对惯导测量数据进行预积分，并采用光流法进行特征点追踪的前端；根据环境变化，动态调整可见光图像和红外图像的融合权重的自适应调整模块；将不同帧可见光特征点和红外特征点分别进行匹配的匹配模块；完成所述相机和惯导的联合初始化的初始化模块；进行所述紧耦合的视觉-惯性里程计的计算的视觉-惯性紧耦合里程计模块；实现所述回环检测和重定位的回环检测和重定位模块；对重定位后的全局位姿图进行优化，保证重定位后的全局一致性，并输出最终定位位姿的全局优化及输出模块。
针对红外图像存在分辨率低、信噪比低、纹理不清晰的缺陷及可见光相机与惯导融合后在昏暗环境中无法有效利用视觉观测的问题，本发明的上述定位方法融合了红外相机、可见光相机及惯导观测，不仅为定位系统提供了更丰富多源的感知信息，同时可使系统在昏暗环境中利用红外相机的观测信息，并通过红外相机的观测修正惯导误差。
针对可见光相机与红外相机融合模式固定，不能根据环境变化做出调整的问题，本发明的上述定位方法可将可见光图像和红外图像进行自适应融合，根据环境变化调整可见光图像和红外图像融合权重，避免了误差较大的观测对系统精度的损害。
本发明具有以下有益效果：
(1)本发明可融合可见光图像及红外图像信息，感知来源的信息更广，使无人系统在复杂环境尤其是昏暗环境下定位性能更好；
(2)根据本发明的一些具体实施方式，本发明可进行高效的视觉-惯性初始化，能够快速地将多相机位姿估计及惯导位姿估计对齐，并将可见光相机、红外相机、惯导三者尺度统一；
(3)相对于现有的定位方案中视觉惯性紧耦合里程计仅利用可见光图像构造视觉残差，低照度环境下可见光图像观测误差较大，根据本发明的一些具体实施方式，本发明的视觉惯性紧耦合里程计同时构造了可见光图像和红外图像的视觉残差，并且能根据环境变化自适应地调整可见光图像残差和红外图像残差融合权重，得到更高精度的视觉惯性紧耦合里程计；
(4)相对于现有的回环检测中仅通过可见光图像检测重复经过的地点，在光照剧烈变化的情况下很可能造成漏检，根据本发明的一些具体实施方式，本发明的回环检测可同时检测可见光图像和红外图像的相似程度，检测效果更好；
(5)相对于现有的的重定位方法只根据可见光图像进行特征匹配，昏暗场景或光照变化容易导致重定位失败，根据本发明的一些具体实施方式，本发明的重定位可同时对可见光图像和红外图像进行特征匹配，自适应地调整可见光图像残差和红外图像残差融合权重，能够确保复杂环境下重定位的可靠性。
附图说明
图1为具体实施方式中所述本发明的融合框架示意图。
图2为具体实施方式中所述视觉-惯性联合初始化过程示意图。
图3为具体实施方式中所述视觉-惯性紧耦合里程计过程示意图。
图4为具体实施方式中所述重定位过程示意图。
具体实施方式
以下结合实施例和附图对本发明进行详细描述，但需要理解的是，所述实施例和附图仅用于对本发明进行示例性的描述，而并不能对本发明的保护范围构成任何限制。所有包含在本发明的发明宗旨范围内的合理的变换和组合均落入本发明的保护范围。
参照附图1，根据本发明的技术方案，所述多光谱图像和惯导融合的定位方法的一些具体实施方式包括如下步骤：
S1对无人系统获得的当前帧可见光图像及红外图像分别进行特征提取，得到可见光图像特征点和红外图像特征点，并将当前帧可见光图像特征点及红外图像特征点分别与上一帧的可见光图像特征点和红外图像特征点进行特征匹配，匹配完成后，根据当前帧可见光图像及红外图像的亮度、纹理及其中稳定跟踪的特征点的数量确定当前帧可见光图像与红外图像的融合权重。
进一步的，在一些具体实施例中，S1进一步包括：
S11采用角点检测法分别提取RGB形式的当前帧可见光图像和含红外测试温度T的当前帧红外图像的特征，得到可见光特征点和红外特征点。
在一些具体实施例中，角点检测法可通过Fast角点检测器实现。
S12将提取得到的可见光特征点和红外特征点分别与上一帧的可见光特征点和红外特征点进行特征匹配，标记在连续多帧图像中均匹配成功的特征点为稳定跟踪的特征点，匹配方法为：对于可见光特征点，当特征点之间的可见光信息差值小于阈值时，特征匹配成功，对于红外特征点，当特征点之间的热感信息差值小于阈值时，特征匹配成功。
进一步的，在一具体实施例中，所述连续多帧设置为7帧。
S13将当前帧可见光图像进行亮度的定量分析，获得该可见光图像的当前帧亮度均值和基于该亮度均值设置的亮度权重因子。
进一步的，在一些具体的实施例中，所述亮度的定量分析包括：
将所述可见光图像进行灰度转换，将其三通道RGB图像转换为8位的单通道灰度图像，根据转换后的灰度图计算亮度均值，获得所述当前帧亮度均值；
基于所得亮度均值，根据以下亮度权重因子计算模型获得当前帧亮度权重因子Wi：
Figure BDA0004005974540000131
其中，Ii为第i帧可见光图像的亮度均值，Ii-1为第i-1帧可见光图像的亮度均值。
根据该计算模型，若当前帧亮度均值和上一帧亮度均值之差超过50，则认为图像间的亮度变化较大。
S14将当前帧可见光图像进行结构纹理的定量分析，获得该可见光图像的当前帧与上一帧的结构相似度和基于该结构相似度设置的纹理权重因子。
进一步的，在一些具体的实施例中，所述结构纹理的定量分析包括：
计算当前帧可见光图像与其上一帧可见光图像的SSIM值，以该SSIM值作为结构相似度的度量；
基于所得SSIM值，根据以下纹理权重因子计算模型获得当前帧可见光图像的纹理权重因子Ws：
Figure BDA0004005974540000141
根据该计算模型，若当前帧和上一帧结构相似度低于0.25，则认为结构纹理变化较大。
S15根据所述亮度权重因子、纹理权重因子及所述稳定跟踪的特征点的数量，确定可见光图像和红外图像的自适应融合权重α和β。
进一步的，在一些具体实施例中，所述自适应融合权重α和β通过以下权重计算模型确定：
Figure BDA0004005974540000142
β＝1-α
其中，SR为可见光图像中稳定跟踪的特征点的数量，ST为红外图像中稳定跟踪的特征点的数量。
通过以上权重计算模型，本发明可根据可见光图像亮度变化、结构纹理变化、特征点在可见光图像和红外图像中追踪成功的帧数，动态地调整特征追踪时可见光图像和红外图像的权重。
S2对无人系统获得的惯导测量数据进行预积分；
S3根据当前帧可见光图像、红外图像及预积分数据，进行无人系统的相机和惯导的联合初始化，获得视觉-惯性的联合初始化值，并将联合初始化过程中建立的当前帧、其之前的关键帧及关键帧中的三维特征点加入局部地图中。
进一步的，在一些具体实施例中，S3包括：
S31对可见光相机和红外相机进行纯视觉初始化，所述纯视觉初始化包括位姿初始化和相机的外参初始化，得到位姿初始化值及不同相机间的平移外参的初始化值。
进一步的，在一些具体实施例中，S31包括：
根据当前帧可见光或红外图像Fi，在系统的过往帧堆栈中寻找可见光或红外参考帧图像Fj；
计算当前帧可见光或红外图像Fi与参考帧图像Fj之间的基础矩阵和单应矩阵，并求解两帧之间的运动参数，根据获得的两帧之间的位姿变化信息，将可见光或红外图像Fi中的二维的特征点进行三角化，得到其在世界坐标系下的三维空间点，即三维特征点；
遍历堆栈中的过往帧，若所得三维特征点和堆栈中的过往帧的二维特征点匹配足够多，则通过PnP算法恢复过往帧的位姿；
通过BA优化完善初始化过程所有帧的位姿，完成所述位姿初始化，得到纯视觉初始化过程中所有帧的位姿初始值；
求解如下的参数关系式，获得可见光相机和红外相机之间的旋转和平移外参估计值：
Figure BDA0004005974540000151
即：
Figure BDA0004005974540000152
其中，
Figure BDA0004005974540000153
表示k,k+1相邻两帧之间的可见光相机的旋转估计值，
Figure BDA0004005974540000154
表示k,k+1相邻两帧之间红外相机的旋转估计值，以上两个旋转估计值可由完成所述位姿初始化后的图像的第k帧和第k+1帧的姿态计算得到；qRT为四元数表示的旋转外参估计值，
Figure BDA0004005974540000161
[·]L,[·]R表示四元数的左乘和右乘；通过SVD分解法，可以求解得到可见光相机到红外相机旋转外参qRT；
将可见光相机与红外相机进行尺度统一，包括：
获得如下式所示的红外相机和可见光相机之间尺度的比值sratio：
Figure BDA0004005974540000162
其中，
Figure BDA0004005974540000163
表示由红外相机估计得到的第i-1帧到第i帧图像的有尺度的位移向量(上标横线代表有尺度，下同)；
Figure BDA0004005974540000164
表示由可见光相机估计得到的第i-1帧到第i帧图像的有尺度的位移向量；||·||2表示取向量的二范数，n表示初始化过程中的帧数；
根据所得比值sratio，将红外相机坐标系下所有点以及所有帧的坐标同时除sratio，使其统一到可见光相机尺度下；
尺度统一后，设
Figure BDA0004005974540000165
为可见光相机第i帧在世界坐标系下的位置，
Figure BDA0004005974540000166
为红外相机第i帧在世界坐标系下的位置，根据
Figure BDA0004005974540000167
和
Figure BDA0004005974540000168
的坐标变换关系，构造以下优化问题：
Figure BDA0004005974540000169
其中，
Figure BDA00040059745400001610
表示在可见光相机尺度下，可见光相机到红外相机的平移量，RRT为旋转矩阵表示的可见光相机坐标系到红外相机坐标系的旋转，||·||2表示取向量的二范数；
通过调用Ceres库的LM法求解得
Figure BDA00040059745400001611
值，将其作为可见光相机尺度下的平移外参的初始化值。
以上过程中，通过位姿初始化估计得到的各图像帧及其中的各特征点的位置带有相机的尺度信息，而红外相机的尺度和可见光相机的尺度并不一样，因此本发明进一步将红外相机的尺度统一到可见光相机的尺度下。
S32通过紧耦合方式对惯导系统进行纯惯导初始化，得到惯导参数的初始化值。
进一步的，在一些具体实施例中，S32包括：
以离线标定的惯导偏差作为惯导系统的参数中加速度计的偏置ba和陀螺仪的偏置bg的初始值；
定义纯惯导初始化中待优化的状态量为Xinertial-only：
Xinertial-only＝{s,Rwg,b,v0:k}
其中，s表示可见光相机的尺度；Rwg∈SO(3)表示重力方向，其中，SO(3)表示李群，则在世界坐标系下，重力可表达为g＝RwggI，其中gI＝(0,0,G)T表示重力向量，G是重力的幅值；
Figure BDA0004005974540000171
表示加速度计和陀螺仪的组合偏置，其中ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure BDA0004005974540000172
表示六维实向量；
Figure BDA0004005974540000173
表示在从第0帧到第k帧之间的k+1个时刻时，惯导在世界坐标系下运动的速度，该速度具有尺度信息，其中，
Figure BDA0004005974540000174
表示三维实向量；
构建惯导的相邻帧之间的预积分残差，如下：
Figure BDA0004005974540000175
Figure BDA0004005974540000176
Figure BDA0004005974540000177
其中，
Figure BDA0004005974540000178
分别表示惯导的第i-1帧到第i帧的旋转预积分、速度预积分、位置预积分的残差；ΔRi-1,i，Δvi-1,i，Δpi-1,i分别表示从惯导的第i-1帧到第i帧的预积分；Ri-1表示第i-1帧时的惯导姿态，Ri表示第i帧时的惯导姿态；vi-1表示第i-1帧时的惯导速度，vi表示第i帧时的惯导速度；Δti-1,i表示相邻惯导测量之间的时间间隔；
Figure BDA0004005974540000179
表示第i-1帧时的惯导位置，
Figure BDA00040059745400001710
表示第i帧时的惯导位置(上标横线代表有尺度)；log代表从李群到李代数的映射；
根据预积分残差构建惯导参数的优化函数，如下：
Figure BDA0004005974540000181
其中，
Figure BDA0004005974540000182
为预积分残差项，rp＝ba表示惯导先验残差，Σp和
Figure BDA0004005974540000183
分别为rp、
Figure BDA0004005974540000184
的协方差；
通过调用Ceres库LM法求解上述纯惯导优化步骤中的优化函数，获得惯导参数的最优估计，以所得最优估计作为惯导参数的初始值。
本步骤采用紧耦合方式进行惯导初始化，可对所有惯导参数同步求解，保证了估计的准确性。
S33对可见光相机、红外相机、惯导系统进行视觉-惯性联合初始化，得到联合后的位姿及惯导参数初始化值，即所述视觉-惯性的联合初始化值。
进一步的，参照附图2，在一些具体实施例中，S33包括：
S331纯惯导初始化过程后，根据解算得到的重力方向Rwg旋转世界坐标系，使得世界坐标系z轴与重力方向一致，并通过Rwg将在原世界坐标系下所有帧的位姿以及三维特征点坐标变换到旋转后的世界坐标系下；
S332根据纯惯性初始化估计得到的可见光相机的尺度s，将有尺度的可见光相机、红外相机、三维特征点的位置坐标恢复到米制尺度下；
S333以纯惯导初始化获得的惯导参数的最优估计作为优化的初值，在优化问题中添加视觉残差项，对视觉估计的位姿和惯导参数进行联合优化，以获得的位姿和惯导的参数的联合最优估计作为联合后的初始化值，其可具体包括如：
(1)设视觉-惯性联合初始化过程中，第i帧的状态量为Si＝{Ti,vi}，其中，Ti表示第i帧的位姿，vi表示第i帧的惯导速度，则对k+1帧进行的联合初始化中，所有帧的状态量为
Figure BDA0004005974540000185
设待优化的参数XVI_init包括带尺度的所有帧的状态量
Figure BDA0004005974540000186
和初始化过程中的惯导组合偏置
Figure BDA0004005974540000187
其中ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure BDA0004005974540000191
表示六维实向量，即
Figure BDA0004005974540000192
(2)在构造优化函数之前，先构建惯导预积分残差
Figure BDA0004005974540000193
可见光观测残差
Figure BDA0004005974540000194
和红外观测残差
Figure BDA0004005974540000199
具体的，可采用与所述纯惯导初始化中构建惯导的相邻帧之间的预积分残差相同的方式构造预积分残差；采用与后文视觉-惯性紧耦合里程计中相同的可见光观测残差和红外观测残差构建方式构造可见光观测残差和红外观测残差，不同的仅为：视觉-惯性紧耦合里程计使用关键帧构造残差，由于初始化过程中的帧数较少，视觉-惯性联合初始化使用初始化过程中的每一帧构造残差；
(3)根据所得惯导预积分残差
Figure BDA0004005974540000195
可见光观测残差
Figure BDA00040059745400001911
红外观测残差
Figure BDA00040059745400001910
及当前帧可见光图像与红外图像的融合权重，构建视觉-惯性联合优化函数，如下：
Figure BDA0004005974540000196
其中，假设初始化过程中有k+1帧，总共观测到了l个三维特征点。
Figure BDA0004005974540000197
表示初始化过程中观测到第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为初始化过程中的第i帧可见光图像和红外图像的权重系数，αi和βi在确定融合权重步骤中计算得到，
Figure BDA0004005974540000198
为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
(4)通过调用Ceres库LM法求解完上述联合优化函数，以获得的最优参数XVI_init作为视觉-惯性联合后的初始化值，至此初始化过程结束。
S34将联合初始化过程中建立的关键帧帧和三维特征点加入到局部地图中。
其中，局部地图是指当前帧周围的地图，由当前帧(不包括当前帧的特征点)和当前帧之前的若干帧关键帧以及这些帧所观测到的特征点组成，即除了当前帧外，局部地图中的所有帧都是关键帧。
其中，所述关键帧可通过如下机制建立：若当前帧与上一帧关键帧之间的视差较大，或者距离上一帧关键帧的距离较远，则将当前帧建立为关键帧。若当前帧被建立为关键帧，则三角化当前帧提取到的新特征点，将当前帧以及当前帧的新特征点加入到局部地图中，若当前帧不是关键帧，则在解算完当前帧位姿后，将当前帧从局部地图中剔除。此外，距离最新帧太远的关键帧以及该帧的特征点将会从局部地图中移除。
S4基于所述当前帧可见光图像与红外图像的融合权重及所述视觉-惯性的联合初始化值，构建含有帧状态量和三维特征点状态量的优化状态量，基于所述优化状态量，通过紧耦合的视觉-惯性里程计(VIO)获得局部地图中全部帧的优化位姿和三维特征点的优化位置；其中，所述帧状态量含有位姿信息和惯导参数信息，所述三维特征点状态量含有三维特征点的位置信息。
其中，进一步的，局部地图中第i帧的帧状态量定义为
Figure BDA0004005974540000201
其中，Ti＝[Ri,Pi]∈SE(3)为第i帧的位姿，SE(3)表示李代数，则含有k+1帧的局部地图的状态量为
Figure BDA0004005974540000202
设其中第0帧到第k-1帧为关键帧，第k帧为当前帧；vi为惯导在世界坐标系下的速度；
Figure BDA0004005974540000203
分别是第i帧时刻下，陀螺仪和加速度计的偏置。
三维特征点的状态量定义为xi＝{x,y,z}，其中，x、y、z分别表示三维坐标的三个分量，则含有l个三维特征点的局部地图的三维特征点的状态量为
Figure BDA0004005974540000204
则紧耦合的视觉-惯性里程计的优化状态量
Figure BDA0004005974540000205
进一步的，在一些具体实施例中，参照附图3，所述紧耦合的视觉-惯性里程计(VIO)的计算过程包括：
(1)采用与所述纯惯导初始化优化中构建所述相邻帧之间的预积分残差相同方式构建惯导预积分残差，如下：
Figure BDA0004005974540000206
Figure BDA0004005974540000207
Figure BDA0004005974540000208
Figure BDA0004005974540000211
其中，
Figure BDA0004005974540000212
为所述惯导预积分残差；ΔRi-1,i、Δvi-1,i、Δpi-1,i为第i-1帧到第i帧的惯导预积分值。
(2)为充分利用可见光图像和红外图像信息，将局部地图中的第j个三维特征点投影到第i帧可见光图像和红外图像中，分别构建可见光观测残差
Figure BDA00040059745400002112
和红外观测残差
Figure BDA00040059745400002113
如下：
Figure BDA0004005974540000213
Figure BDA0004005974540000214
其中，RΠ,
Figure BDA0004005974540000215
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换；Ti为第i帧的位姿；
Figure BDA0004005974540000216
为李群SE(3)在
Figure BDA0004005974540000217
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
(3)将所述惯导预积分残差
Figure BDA0004005974540000218
可见光观测残差
Figure BDA00040059745400002114
红外观测残差
Figure BDA00040059745400002115
与可见光图像与红外图像的融合权重结合，构造视觉-惯性紧耦合里程计优化函数，并以通过最小二乘优化法使结合后的残差和最小，其中，优化函数设置如下：
Figure BDA0004005974540000219
其中，
Figure BDA00040059745400002110
表示能观测到局部地图中第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的权重系数，
Figure BDA00040059745400002111
为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
(4)通过调用Ceres库LM法求解上述视觉-惯性紧耦合里程计的优化函数，获得当前帧位姿、局部地图中其他帧位姿和三维特征点位置的最优估计。
S5基于优化后的局部地图中的关键帧的帧状态量和三维特征点状态量，进行回环检测。
进一步的，在一些具体实施例中，采用基于DBoW库的词袋检测方法在关键帧数据库寻找当前帧的潜在回环帧，回环检测时两帧的相似度函数定义如下：
Stotal＝αloopSrgb+βloopSt
其中，Srgb,St分别为当前帧可见光图像词袋向量和红外图像词袋向量与潜在回环帧可见光图像词袋向量和红外图像词袋向量的相似度，αloop和βloop分别为回环检测时可见光图像相似度和红外图像相似度的权重，其计算方式如下：
Figure BDA0004005974540000221
βloop＝1-αloop
其中，Icur为当前帧的亮度均值，Iloop为回环帧的亮度均值；
若Stotal大于设定的阈值，则认为检测到闭环。
通过以上实施方式，本发明不仅考虑了当前帧和潜在回环帧之间可见光图像的相似度，还同时考虑了红外图像的相似度。
S6若在所述回环检测中检测到了回环，则将局部地图内的三维特征点与回环帧进行匹配，建立回环帧与局部地图中的三维特征点的特征匹配关系，并据此进行无人系统的重定位优化，得到重定位优化后的局部地图中帧状态量和三维特征点状态量。
进一步的，在一些具体实施例中，参照附图4，所述重定位优化包括：
(1)假设局部地图中有n个三维特征点与回环帧成功匹配，为方便表示，设成功匹配的n个三维特征点的状态量为
Figure BDA0004005974540000222
(2)与视觉惯性紧耦合里程计视觉残差构建方法类似，将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到局部地图中第i帧的可见光图像和红外图像中，分别构建可见光观测残差和红外观测残差，如下：
Figure BDA0004005974540000223
Figure BDA0004005974540000224
(3)将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到回环帧的可见光图像和红外图像中，分别构建可见光观测残差和红外观测残差，如下：
Figure BDA0004005974540000231
Figure BDA0004005974540000232
其中，RΠ,
Figure BDA0004005974540000233
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图与回环帧匹配的特征点集的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；
Figure BDA0004005974540000234
分别为局部地图与回环帧匹配的特征点集的第j个特征点在回环帧图像中的观测值，TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换可见光图像和红外图像中的观测值；Ti为第i帧的位姿，Tloop为回环帧的位姿；
Figure BDA0004005974540000235
为李群SE(3)在
Figure BDA0004005974540000236
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
(4)定义重定位优化状态量包括局部地图中第i帧状态量Si＝{Ti,vi,bi g,bi a}、回环帧的状态量
Figure BDA0004005974540000237
和三维特征点状态量xi＝{x,y,z}。假设局部地图中有k+1帧。设局部地图中k+1帧的状态为
Figure BDA00040059745400002311
假设局部地图与回环帧成功匹配了n个三维特征点，设匹配点集的状态量为
Figure BDA00040059745400002312
则重定位优化状态量
Figure BDA0004005974540000238
将预积分残差和视觉残差结合后得到的重定位优化问题如下：
Figure BDA0004005974540000239
其中，假设局部地图中有k+1帧，局部地图与回环帧匹配了n个三维特征点。
Figure BDA00040059745400002310
表示能观测到局部地图与回环帧匹配的第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的权重系数，αi和βi在融合权重自适应调整步骤中计算得到，αloop和βloop为回环帧的可见光图像和红外图像的权重系数，αloop和βloop在回环检测步骤中计算得到。
Figure BDA0004005974540000241
为预积分残差的信息矩阵；RΣij,TΣij为局部地图中可见光观测残差和红外观测残差的信息矩阵，RΣloop,j,TΣloop,j为回环帧中可见光观测残差和红外观测残差的信息矩阵。
重定位优化问题的前三项和紧耦合视觉惯导里程计相同，充分利用了局部地图信息用于优化求解当前帧位姿，最后两项利用了回环帧的历史信息，为当前帧位姿优化求解添加了回环约束。
通过调用Ceres库LM法求解上述重定位步骤的优化函数，获得重定位后当前帧位姿的最优估计，并消除局部地图中其他关键帧位姿和其他三维特征点位置的累计误差。
S7在完成所述重定位优化后，进行全局位姿优化，保证重定位后的全局一致性，其中，全局位姿优化只优化所有关键帧的位姿不优化三维特征点状态量。
进一步的，在一些具体实施例中，采用四自由度位姿图优化的方法调整所有关键帧位姿的x坐标、y坐标、z坐标和航向角。
S8每新建一帧关键帧，将该关键帧加入关键帧数据库中，其存储的数据包括可见光图像和红外图像的特征点、词袋向量，以及该帧的位姿。关键帧数据库中关键帧位姿会在每一次优化后更新，最后定位系统输出的轨迹即为关键帧数据库中每一帧关键帧的位姿。
以上实施例仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例。凡属于本发明思路下的技术方案均属于本发明的保护范围。应该指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下的改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种多光谱图像和惯导融合的定位方法，其特征在于：其包括：
S1对获得的当前帧可见光图像及红外图像分别进行特征提取，得到可见光图像特征点和红外图像特征点，并将当前帧可见光图像特征点及红外图像特征点分别与上一帧的可见光图像特征点和红外图像特征点进行特征匹配，匹配完成后，根据当前帧可见光图像及红外图像的亮度、纹理及其中稳定跟踪的特征点的数量确定当前帧可见光图像与红外图像的融合权重；
S2对获得的惯导测量数据进行预积分，获得预积分数据；
S3根据所述当前帧可见光图像、红外图像及所述预积分数据，进行相机和惯导的联合初始化，获得视觉-惯性的联合初始化值，并将联合初始化过程中建立的关键帧及其三维特征点加入局部地图中；
S4基于所述当前帧可见光图像与红外图像的融合权重及所述视觉-惯性的联合初始化值，构建含有帧状态量和三维特征点状态量的优化状态量，基于所述优化状态量，通过紧耦合的视觉-惯性里程计获得局部地图中全部帧的优化位姿和三维特征点的优化位置；其中，所述帧状态量含有位姿信息和惯导参数信息，所述三维特征点状态量含有三维特征点的位置信息；
S5基于优化后的局部地图中的关键帧的帧状态量和三维特征点状态量，通过词袋检测法进行回环检测；
S6当在所述回环检测中检测到了回环，则将局部地图内的三维特征点与回环帧进行匹配，建立回环帧与局部地图中的三维特征点的特征匹配关系，并据此进行重定位优化，得到重定位优化后的局部地图中的帧状态量和三维特征点状态量；
S7在完成所述重定位优化后，进行全局位姿优化，所述全局位姿优化只优化所有关键帧的位姿不优化三维特征点状态量；
S8在每新建一帧关键帧后，将该关键帧的相关数据加入关键帧数据库中，所述相关数据包括组成该关键帧的可见光图像和红外图像的特征点、词袋向量，及该帧的位姿，其中，各关键帧的位姿在每一次优化后进行更新，由更新后的每一帧关键帧的位姿作为定位系统的输出轨迹。
2.根据权利要求1所述的多光谱图像和惯导融合的定位方法，其特征在于：所述S1包括：
S11采用角点检测法分别提取RGB形式的当前帧可见光图像和含红外测试温度T的当前帧红外图像的特征，得到可见光特征点和红外特征点；
S12将提取得到的可见光特征点和红外特征点分别与上一帧的可见光特征点和红外特征点进行特征匹配，标记在连续多帧图像中均匹配成功的特征点为稳定跟踪的特征点；
S13将当前帧可见光图像进行亮度的定量分析，获得该可见光图像的当前帧亮度均值和基于该亮度均值设置的亮度权重因子；
S14将当前帧可见光图像进行结构纹理的定量分析，获得该可见光图像的当前帧与上一帧的结构相似度和基于该结构相似度设置的纹理权重因子；
S15根据所述亮度权重因子、纹理权重因子及所述稳定跟踪的特征点的数量，确定可见光图像和红外图像的自适应融合权重α和β。
3.根据权利要求2所述的多光谱图像和惯导融合的定位方法，其特征在于：所述自适应融合权重α和β通过以下权重计算模型确定：
Figure FDA0004005974530000021
β＝1-α
其中，Wi为亮度权重因子，Ws为纹理权重因子，SR为可见光图像中稳定跟踪的特征点的数量，ST为红外图像中稳定跟踪的特征点的数量，且：
Figure FDA0004005974530000022
其中，Ii为第i帧的亮度均值，Ii-1为第i-1帧的亮度均值；
Figure FDA0004005974530000023
其中，SSIM表示SSIM值。
4.根据权利要求1所述的多光谱图像和惯导融合的定位方法，其特征在于：所述S3包括：
S31对可见光相机和红外相机进行纯视觉初始化，所述纯视觉初始化包括位姿初始化和相机的外参初始化，得到位姿初始化值及不同相机间的平移外参的初始化值；
S32通过紧耦合方式对惯导系统进行纯惯导初始化，得到惯导参数的初始化值；
S33对可见光相机、红外相机、惯导系统进行视觉-惯性联合初始化，得到联合后的位姿及惯导参数初始化值，即所述视觉-惯性的联合初始化值。
5.根据权利要求4所述的多光谱图像和惯导融合的定位方法，其特征在于：其中，S31包括：
根据当前帧可见光图像或红外图像Fi，在系统的过往帧堆栈中寻找可见光或红外参考帧图像Fj；
计算当前帧可见光图像或红外图像Fi与参考帧图像Fj之间的基础矩阵和单应矩阵，并求解两帧之间的运动参数，根据获得的两帧之间的位姿变化信息，将可见光或红外图像Fi中的二维的特征点进行三角化，得到其在世界坐标系下的三维空间点，即三维特征点；
遍历堆栈中的过往帧，若所得三维特征点和堆栈中的过往帧的二维特征点匹配足够多，则通过PnP算法恢复过往帧的位姿；
通过BA优化完善初始化过程所有帧的位姿，完成所述位姿初始化，得到纯视觉初始化过程中所有帧的位姿初始值；
求解如下的参数关系式，获得可见光相机和红外相机之间的旋转和平移外参估计值：
Figure FDA0004005974530000031
即：
Figure FDA0004005974530000032
其中，
Figure FDA0004005974530000033
表示k,k+1相邻两帧之间的可见光相机的旋转估计值，
Figure FDA0004005974530000034
表示k,k+1相邻两帧之间红外相机的旋转估计值，以上两个旋转估计值可由完成所述位姿初始化后的图像的第k帧和第k+1帧的姿态计算得到；qRT为四元数表示的旋转外参估计值，
Figure FDA0004005974530000035
[·]L,[·]R表示四元数的左乘和右乘；通过SVD分解法，可以求解得到可见光相机到红外相机旋转外参qRT；
将可见光相机与红外相机进行尺度统一，包括：
获得如下式所示的红外相机和可见光相机之间尺度的比值sratio：
Figure FDA0004005974530000041
其中，
Figure FDA0004005974530000042
表示由红外相机估计得到的第i-1帧到第i帧图像的位移向量；
Figure FDA0004005974530000043
表示由可见光相机估计得到的第i-1帧到第i帧图像的有尺度的位移向量；||·||2表示取向量的二范数，n表示初始化过程中的帧数；各上标横线代表有尺度；
根据所得比值sratio，将红外相机坐标系下所有点以及所有帧的坐标同时除sratio，使其统一到可见光相机尺度下；
尺度统一后，设
Figure FDA0004005974530000044
为可见光相机第i帧在世界坐标系下的位置，
Figure FDA0004005974530000045
为红外相机第i帧在世界坐标系下的位置，根据
Figure FDA0004005974530000046
和
Figure FDA0004005974530000047
的坐标变换关系，构造以下第一优化问题：
Figure FDA0004005974530000048
其中，
Figure FDA0004005974530000049
表示在可见光相机尺度下，可见光相机到红外相机的平移量，RRT为旋转矩阵表示的可见光相机坐标系到红外相机坐标系的旋转，||·||2表示取向量的二范数；
通过最小二乘法求解所述第一优化问题，以得到的平移量
Figure FDA00040059745300000410
的值，作为可见光相机尺度下的平移外参的初始化值；
和/或，
S32包括：
以离线标定的惯导偏差作为惯导系统的参数中加速度计的偏置ba和陀螺仪的偏置bg的初始值；
定义纯惯导初始化中待优化的状态量为Xinertial-only：
Xinertial-only＝{s,Rwg,b,v0:k}
其中，s表示可见光相机的尺度；Rwg∈SO(3)表示重力方向，其中，SO(3)表示李群，则在世界坐标系下，重力可表达为g＝RwggI，其中gI＝(0,0,G)T表示重力向量，G是重力的幅值；
Figure FDA0004005974530000051
表示加速度计和陀螺仪的组合偏置，其中ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure FDA0004005974530000052
表示六维实向量；
Figure FDA0004005974530000053
表示在从第0帧到第k帧之间的k+1个时刻时，惯导在世界坐标系下运动的速度，该速度具有尺度信息，其中，
Figure FDA0004005974530000054
表示三维实向量；
构建惯导的相邻帧之间的预积分残差，如下：
Figure FDA0004005974530000055
Figure FDA0004005974530000056
Figure FDA0004005974530000057
其中，
Figure FDA0004005974530000058
分别表示惯导的第i-1帧到第i帧的旋转预积分、速度预积分、位置预积分的残差；ΔRi-1,i，Δvi-1,i，Δpi-1,i分别表示从惯导的第i-1帧到第i帧的预积分；Ri-1表示第i-1帧时的惯导姿态，Ri表示第i帧时的惯导姿态；vi-1表示第i-1帧时的惯导速度，vi表示第i帧时的惯导速度；Δti-1,i表示相邻惯导测量之间的时间间隔；
Figure FDA0004005974530000059
表示第i-1帧时的惯导位置，
Figure FDA00040059745300000510
表示第i帧时的惯导位置；log代表从李群到李代数的映射；
根据预积分残差构建惯导参数的第一优化函数，如下：
Figure FDA00040059745300000511
其中，
Figure FDA0004005974530000061
为预积分残差项，rp＝ba表示惯导先验残差，Σp和
Figure FDA0004005974530000062
分别为rp、
Figure FDA0004005974530000063
的协方差；
通过最小二乘法求解所述第一优化函数，以所得惯导参数的最优估计值作为惯导参数的初始化值；
和/或，
S33包括：
S331在完成所述纯惯导初始化后，根据解算得到的重力方向Rwg旋转世界坐标系，使世界坐标系z轴与重力方向一致，并通过Rwg将在原世界坐标系下所有帧的位姿以及三维特征点坐标变换至旋转后的世界坐标系下；
S332根据纯惯导初始化中估计得到的可见光相机的尺度s，将有尺度的可见光相机、红外相机、三维特征点的位置坐标恢复到米制尺度下；
S333基于纯惯导初始化获得的惯导参数的初始化值，在第二优化问题即联合优化的优化问题中添加视觉残差项，对视觉估计的位姿和惯导参数进行联合优化，以获得的位姿和惯导参数的联合最优估计作为联合后的初始化值。
6.根据权利要求5所述的多光谱图像和惯导融合的定位方法，其特征在于：其中，S333包括：
(1)设视觉-惯性联合初始化过程中，第i帧的状态量为Si＝{Ti,vi}，其中，Ti表示第i帧的位姿，vi表示第i帧的惯导速度，则对k+1帧进行的联合初始化中，所有帧的状态量为
Figure FDA0004005974530000064
设待优化的参数向量
Figure FDA0004005974530000065
其中，
Figure FDA0004005974530000066
表示初始化过程中的惯导组合偏置，ba表示惯导加速度计偏置，bg表示惯导陀螺仪偏置，
Figure FDA0004005974530000067
表示六维实向量；
(2)构建惯导预积分残差
Figure FDA0004005974530000068
可见光观测残差
Figure FDA0004005974530000069
和红外观测残差
Figure FDA00040059745300000610
(3)根据所得惯导预积分残差
Figure FDA00040059745300000611
可见光观测残差
Figure FDA00040059745300000612
红外观测残差
Figure FDA00040059745300000613
及当前帧可见光图像与红外图像的融合权重，构建视觉-惯性联合优化函数，即所述第二优化问题，如下：
Figure FDA0004005974530000071
其中，l表示三维特征点的观测数；
Figure FDA0004005974530000072
表示初始化过程中观测到第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为第i帧可见光图像和红外图像的自适应融合权重；
Figure FDA0004005974530000073
为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
(4)通过最小二乘法求解所述联合优化函数，以获得的最优参数向量XVI_init作为视觉-惯性联合后的初始化值。
7.根据权利要求5所述的多光谱图像和惯导融合的定位方法，其特征在于：其中，所述紧耦合的视觉-惯性里程计的计算过程包括：
(1)构建惯导的预积分残差，如下：
Figure FDA0004005974530000074
Figure FDA0004005974530000075
Figure FDA0004005974530000076
Figure FDA0004005974530000077
其中，
Figure FDA0004005974530000078
为所述惯导预积分残差；ΔRi-1,i、Δvi-1,i、Δpi-1,i为第i-1帧到第i帧的惯导预积分值；
(2)将局部地图中的第j个三维特征点投影到第i帧可见光图像和红外图像中，分别构建可见光观测残差
Figure FDA0004005974530000079
和红外观测残差
Figure FDA00040059745300000710
如下：
Figure FDA00040059745300000711
Figure FDA00040059745300000712
其中，
Figure FDA00040059745300000713
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换；Ti为第i帧的位姿；
Figure FDA0004005974530000081
为李群SE(3)在
Figure FDA0004005974530000082
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
将所述惯导预积分残差
Figure FDA0004005974530000083
可见光观测残差
Figure FDA0004005974530000088
红外观测残差
Figure FDA0004005974530000089
与可见光图像与红外图像的融合权重结合，构造视觉-惯性紧耦合里程计优化函数，即第二优化函数，如下：
Figure FDA0004005974530000084
其中，
Figure FDA00040059745300000810
表示能观测到局部地图中第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的自适应融合权重，
Figure FDA00040059745300000811
为预积分残差的信息矩阵；RΣij,TΣij为可见光观测残差和红外观测残差的信息矩阵；
通过最小二乘法求解所述第二优化函数，获得当前帧位姿、局部地图中其他帧位姿和三维特征点位置的最优估计。
8.根据权利要求5所述的多光谱图像和惯导融合的定位方法，其特征在于：其中，所述重定位优化包括：
(1)设局部地图中有n个三维特征点与回环帧成功匹配，且成功匹配的n个三维特征点的状态量为
Figure FDA0004005974530000085
(2)将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到局部地图中第i帧的可见光图像和红外图像中，分别构建第一可见光观测残差和红外观测残差，如下：
Figure FDA0004005974530000086
Figure FDA0004005974530000087
(3)将局部地图与回环帧匹配的特征点集的第j个特征点xj投影到回环帧的可见光图像和红外图像中，分别构建第二可见光观测残差和红外观测残差，如下：
Figure FDA0004005974530000091
Figure FDA0004005974530000092
其中，
Figure FDA0004005974530000093
分别为可见光和红外相机模型的投影函数；Ruij,Tuij分别为局部地图与回环帧匹配的特征点集的第j个特征点在局部地图的第i帧的可见光图像和红外图像中的观测值；Ruloop,j,Tuloop,j分别为局部地图与回环帧匹配的特征点集的第j个特征点在回环帧图像中的观测值，TRB,TTB∈SE(3)分别代表从可见光相机和红外相机坐标系到惯导坐标系的变换可见光图像和红外图像中的观测值；Ti为第i帧的位姿，Tloop为回环帧的位姿；
Figure FDA0004005974530000094
为李群SE(3)在
Figure FDA0004005974530000095
上的运算；xj表示局部地图中第j个特征点在世界坐标系下的三维坐标；
(4)定义重定位优化状态量包括局部地图中第i帧状态量
Figure FDA0004005974530000096
回环帧的状态量
Figure FDA0004005974530000097
和三维特征点状态量xi＝{x,y,z}；设局部地图中有k+1帧，其状态为
Figure FDA0004005974530000098
设局部地图与回环帧成功匹配了n个三维特征点，匹配点集的状态量为
Figure FDA0004005974530000099
则重重定位优化状态量
Figure FDA00040059745300000910
其对应的重定位优化问题即第三优化问题设置如下：
Figure FDA00040059745300000911
其中，
Figure FDA00040059745300000912
表示能观测到局部地图与回环帧匹配的第j个三维特征点的帧的集合；ρHub表示Huber核函数；αi和βi为局部地图的第i帧关键帧的可见光图像和红外图像的自适应融合权重；αloop和βloop为回环帧的可见光图像和红外图像的权重系数；
Figure FDA00040059745300000913
为预积分残差的信息矩阵；RΣij,TΣij为局部地图中可见光观测残差和红外观测残差的信息矩阵，RΣloop,j,TΣloop,j为回环帧中可见光观测残差和红外观测残差的信息矩阵；
(5)通过最小二乘法求解所述第三优化问题，获得重定位后当前帧位姿的最优估计，并消除局部地图中其他关键帧位姿和其他三维特征点位置的累计误差。
9.根据权利要求6所述的多光谱图像和惯导融合的定位方法，其特征在于：所述回环帧的可见光图像和红外图像的权重系数αloop和βloop通过以下计算模型得到：
Figure FDA0004005974530000101
βloop＝1-αloop
其中，Icur为当前帧的亮度均值，Iloop为回环帧的亮度均值。
10.实现权利要求1-9中任一项所述的定位方法的定位计算系统，其包括：对可见光图像和红外图像进行特征点提取、对惯导测量数据进行预积分，并采用光流法进行特征点追踪的前端；根据环境变化，动态调整可见光图像和红外图像的融合权重的自适应调整模块；将不同帧可见光特征点和红外特征点分别进行匹配的匹配模块；完成所述相机和惯导的联合初始化的初始化模块；进行所述紧耦合的视觉-惯性里程计的计算的视觉-惯性紧耦合里程计模块；实现所述回环检测和重定位的回环检测和重定位模块；对重定位后的全局位姿图进行优化，保证重定位后的全局一致性，并输出最终定位位姿的全局优化及输出模块。
CN202211632072.9A 2022-12-19 2022-12-19 一种多光谱图像和惯导融合的定位方法和计算系统 Pending CN116007609A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211632072.9A CN116007609A (zh) 2022-12-19 2022-12-19 一种多光谱图像和惯导融合的定位方法和计算系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211632072.9A CN116007609A (zh) 2022-12-19 2022-12-19 一种多光谱图像和惯导融合的定位方法和计算系统

Publications (1)

Publication Number Publication Date
CN116007609A true CN116007609A (zh) 2023-04-25

Family

ID=86032700

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211632072.9A Pending CN116007609A (zh) 2022-12-19 2022-12-19 一种多光谱图像和惯导融合的定位方法和计算系统

Country Status (1)

Country Link
CN (1) CN116007609A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116576850A (zh) * 2023-07-12 2023-08-11 北京集度科技有限公司 一种位姿确定方法、装置、计算机设备及存储介质
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116576850A (zh) * 2023-07-12 2023-08-11 北京集度科技有限公司 一种位姿确定方法、装置、计算机设备及存储介质
CN116576850B (zh) * 2023-07-12 2023-10-20 北京集度科技有限公司 一种位姿确定方法、装置、计算机设备及存储介质
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法
CN117760428B (zh) * 2024-02-22 2024-04-30 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam系统及机器人
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN110044354B (zh) 一种双目视觉室内定位与建图方法及装置
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN108700946B (zh) 用于并行测距和建图的故障检测和恢复的系统和方法
CN111595333A (zh) 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN112197770B (zh) 一种机器人的定位方法及其定位装置
CN111561923A (zh) 基于多传感器融合的slam制图方法、系统
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN110553648A (zh) 一种用于室内导航的方法和系统
CN109781092B (zh) 一种危险化工事故中移动机器人定位与建图方法
WO2019104571A1 (zh) 图像处理方法和设备
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN111366153B (zh) 一种激光雷达与imu紧耦合的定位方法
CN110533719B (zh) 基于环境视觉特征点识别技术的增强现实定位方法及装置
CN111882602B (zh) 基于orb特征点和gms匹配过滤器的视觉里程计实现方法
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
US11195297B2 (en) Method and system for visual localization based on dual dome cameras
Seiskari et al. Hybvio: Pushing the limits of real-time visual-inertial odometry
CN114623817B (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination