CN117760427A - 一种基于环境地标检测的惯导-地图融合定位方法 - Google Patents
一种基于环境地标检测的惯导-地图融合定位方法 Download PDFInfo
- Publication number
- CN117760427A CN117760427A CN202410009428.6A CN202410009428A CN117760427A CN 117760427 A CN117760427 A CN 117760427A CN 202410009428 A CN202410009428 A CN 202410009428A CN 117760427 A CN117760427 A CN 117760427A
- Authority
- CN
- China
- Prior art keywords
- key frame
- landmark
- inertial navigation
- gnss
- current
- 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
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 33
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000001514 detection method Methods 0.000 title claims abstract description 23
- 230000007613 environmental effect Effects 0.000 claims abstract description 56
- 238000000605 extraction Methods 0.000 claims abstract description 6
- 230000000007 visual effect Effects 0.000 claims description 32
- 239000013598 vector Substances 0.000 claims description 29
- 238000005259 measurement Methods 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000003287 optical effect Effects 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 description 8
- 238000009825 accumulation Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于环境地标检测的惯导‑地图融合定位方法，属于融合定位技术领域，该方法包括获取全球导航卫星系统GNSS观测数据，并建立地标数据库；根据全球导航卫星系统GNSS观测数据，完成惯性导航系统的初始化，并根据惯性导航系统，得到系统状态预测结果；获取卫星图像数据，并根据卫星图像数据和系统状态预测结果，通过特征提取算法，得到关键帧集合；根据关键帧集合与地标数据库，得到关键帧基于匹配的有效性结果集合；根据惯性导航系统和关键帧基于匹配的有效性结果集合，得到载具当前位姿，完成基于环境地标检测的惯导‑地图融合定位。本发明解决了现有融合定位技术对跟踪卫星数量有要求以及容易受天气和环境干扰的问题。
Description
技术领域
本发明属于融合定位技术领域，尤其涉及一种基于环境地标检测的惯导-地图融合定位方法。
背景技术
现有的一种技术利用全球导航卫星系统(GNSS)、惯性测量单元(IMU)和视觉的互补性，通过紧耦合的集成方法实现城市环境下的连续和精确导航。但紧耦合的方法引入了GNSS的全部原始测量数据与视觉惯导系统(VINS)融合，使得该系统对跟踪卫星的数量有所依赖。具体表现为在系统初始化过程中若少于四颗卫星跟踪将导致系统表现随卫星数量减少而下降，并在无卫星跟踪时退化为VINS，从而重新产生原始的VIO易出现的轨迹漂移或开环问题。
现有的一种技术利用GNSS全球导航卫星系统的RTK实时动态信息对IMU惯性测量单元进行地球旋转补偿来提高高级惯性测量单元的精度。但使用GNSS辅助初始化的方法虽然在仅引用GNSS的RTK数据后未表现出对跟踪卫星数量的依赖性，然而对IMU和视觉数据的依赖却导致其面对室外的复杂场景可能被天气及其他环境因素干扰，从而导致漂移问题。
现有的一种技术将原始的GNSS载波相位和伪距测量、IMU数据和视觉特征直接在观测层面上使用如集中式的扩展卡尔曼滤波器(EKF)的方法进行融合，从而充分利用多传感器信息并排除潜在的异常测量值。但使用EKF的融合算法则对传感器之间的同步和校准要求较高，否则将无法确保准确的融合结果，而更加复杂的融合算法也带来了相比于前两种方法更多的计算资源消耗。
发明内容
针对现有技术中的上述不足，本发明提供的一种基于环境地标检测的惯导-地图融合定位方法解决了现有融合定位技术对跟踪卫星数量有要求以及容易受天气和环境干扰的问题。
为了达到上述发明目的，本发明采用的技术方案为：一种基于环境地标检测的惯导-地图融合定位方法，包括以下步骤：
S1、获取全球导航卫星系统GNSS观测数据，并建立地标数据库；
S2、根据全球导航卫星系统GNSS观测数据，完成惯性导航系统的初始化，并根据惯性导航系统，得到系统状态预测结果；
S3、获取卫星图像数据，并根据卫星图像数据和系统状态预测结果，通过特征提取算法，得到关键帧集合；
S4、根据关键帧集合与地标数据库，得到关键帧基于匹配的有效性结果集合；
S5、根据惯性导航系统和关键帧基于匹配的有效性结果集合，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
本发明的有益效果为：基于环境地标的特征提取和关联可以通过地标的位置信息来校正和修正惯性导航系统的漂移和误差累积问题；本发明在室内外不同天气的复杂环境下均能维持稳定。由于环境地标往往有易识别，特征点多等特点，在识别到图像数据的环境地标特征点后，由惯性导航里程计产生的定位结果将更可靠。
进一步地，所述步骤S2具体为：
S201、保持载具直线移动一段时间，使惯性导航系统保持绝对姿态；
S202、在步骤S201的基础上，在融合惯性测量单元IMU中进行地球自转补偿，完成惯性导航系统的初始化；
S203、根据惯性导航系统的惯性测量单元IMU，获取系统状态预测结果。
上述进一步方案的有益效果为：通过利用全球导航卫星系统(GNSS)观测数据和惯性导航系统的初始化，可以提供更准确的导航定位和轨迹跟踪，从而改善导航的精度。而根据IMU进行地球自转补偿能够减小惯性导航系统在地球自转过程中造成的误差，提高导航系统的准确性和稳定性。
进一步地，所述步骤S203中系统状态预测结果的表达式为：
其中，为系统状态预测结果；/>为预测的位姿矩阵的导数信息；/>为预测的速度的导数信息；/>为预测的位置误差的导数信息；/>为向量积；/>为在世界坐标系中的地球自转速率；/>为上一次预测的位姿矩阵；/>为载具坐标系相对于世界坐标系的旋转矩阵；/>为陀螺仪测量的误差；fb为加速度计测量的误差；/>为上一次预测的速度信息；gw为重力矢量；δ为变分符号。
上述进一步方案的有益效果为：通过上述的系统状态预测结果，可以实时获取载具的位置、速度和姿态等信息，为后续状态向量的最大后验估计提供初始数据。
进一步地，所述步骤S3具体为：
S301、利用全球导航卫星系统GNSS，得到卫星图像数据；
S302、根据卫星图像数据，获取当前图像帧；
S303、利用Harris角点检测器获取当前图像帧的环境地标特征点；
S304、根据系统状态预测结果和当前图像帧的环境地标特征点，利用Lukas-Kanade光流算法进行特征追踪，得到光流速度矢量；
S305、将光流速度矢量与图像数据的帧间隔相乘，得到当前图像帧环境地标与载具的相对位置；
S306、根据当前图像帧的环境地标特征点，判断是否出现新的环境地标特征点，若是，直接进入步骤S307，否则，返回步骤S302，获取下一图像帧；
S307、根据当前图像帧环境地标与载具的相对位置，判断出现平均视差变化是否小于第一阈值，若是，将当前图像帧作为关键帧插入关键帧集合，否则，返回步骤S302，获取下一图像帧。
上述进一步方案的有益效果为：Harris角点检测器和Lukas-Kanade光流算法对环境地标特征点的提取与追踪充分利用了图像数据，在确定关键帧的同时同样为后续状态向量的最大后验估计提供了初始数据。
进一步地，所述步骤S4具体为：
S401、根据关键帧集合，获取与地标数据库中的地标数据成功匹配的第一个关键帧，并以第一个关键帧为参考关键帧；
S402、从关键帧集合中获取下一关键帧作为当前关键帧；
S403、判断地标数据库中是否有地标数据与当前关键帧的环境地标特征点成功匹配，若是，直接进入步骤S404，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S404、根据参考关键帧的系统状态预测结果，将参考关键帧的环境地标特征点与当前关键帧的环境地标特征点进行三角测量，得到环境地标在当前关键帧中的深度；
S405、将参考关键帧更新为当前关键帧；
S406、判断环境地标在当前关键帧中的深度是否小于第二阈值，若是，直接进入步骤S407，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S407、根据关键帧集合，对当前关键帧的前后若干关键帧进行深度计算，得到前后帧的深度结果；
S408、根据前后帧的深度结果，判断当前关键帧的环境地标特征点是否为离群值，若是，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧，否则，判断当前关键帧为有效帧，得到当前关键帧的有效性结果；
S409、根据各关键帧的有效性结果，得到关键帧基于匹配的有效性结果集合。
上述进一步方案的有益效果为：通过对参考关键帧和当前关键帧的特征点进行三角测量，可以得到环境地标在当前关键帧中的深度信息。这有助于提供更精确的相对位置和姿态估计，从而改善整体方法的定位和导航性能。而基于深度的有效性判断也进一步提高了整体方法的可扩展性和应用性。
进一步地，所述步骤S5具体为：
S501、根据关键帧基于匹配的有效性结果集合，设置视觉和GNSS伪距信息的权重；
S502、根据视觉和GNSS伪距信息的权重，得到满足最大后验估计的状态向量；
S503、根据满足最大后验估计的状态向量中世界坐标系下的位置与姿态四元数信息，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
上述进一步方案的有益效果为：通过将不同传感器(视觉和惯性导航系统)的测量数据进行融合，提高了定位结果的准确性和鲁棒性。而通过基于环境地标检测的惯导-地图融合定位方法，将惯性导航系统的测量结果与环境地标的信息相结合，可以在没有GNSS信号或视觉遮挡的情况下提高定位的精度和鲁棒性。
进一步地，所述步骤S501中若关键帧的有效性结果为有效帧或当GNSS信号短暂丢失，则将视觉残差的动态权重设置为3，GNSS残差的动态权重设置为0.8；否则，将视觉残差的动态权重设置为0.8，GNSS残差的动态权重设置为3。
上述进一步方案的有益效果为：通过权重的设置，可以根据不同的场景和可靠性考虑，在定位过程中更合理地利用视觉和GNSS伪距信息，从而提高定位的精确性和稳定性。
进一步地，所述步骤S502中满足最大后验估计的状态向量的表达式为：
其中，χ*为满足最大后验估计的状态向量；argminχ(·)为当最小时χ的取值；rp为来自上一滑动窗口的先验残差矩阵；Hp为来自上一滑动窗口的先验雅可比矩阵；χ为状态向量，包含IMU测量时每个时间节点的状态，Yaw角偏差，和地标深度的倒数；k为视觉滑动窗口中的时间节点编号；n为视觉滑动窗口中时间节点的数量；rI为IMU预积分；为视觉滑动窗口中第k-1个时间节点处的系统状态；/>为IMU预积分的马氏范数的平方；/>为第k-1个和第k个时间节点处IMU预积分的协方差；αVM为视觉残差的动态权重；l为地图中的地标；L为在滑窗中的地图地标；rV为视觉重投影；/>为由关键帧i和关键帧j得出的系统状态；i和j均为包含地标l的关键帧；/>为视觉重投影的马氏范数的平方；/>为取关键帧i和关键帧j时视觉重投影的协方差；αGM为GNSS残差的动态权重；rG为GNSS-RTK的残差因子；为滑动窗口中GNSS-RTK测量时的系统状态；/>为GNSS-RTK残差因子的马氏范数的平方；m为滑动窗口中GNSS-RTK测量的次数；h为GNSS-RTK测量编号。
上述进一步方案的有益效果为：对视觉、惯性导航和地标信息的综合利用，提高了定位的精确性、鲁棒性和可靠性，使这种融合定位方法可以应对复杂环境下的定位需求，为导航、自主驾驶和机器人等领域提供更可靠的定位解决方案。
附图说明
图1为本发明的方法流程图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
如图1所示，在本发明的一个实施例中，一种基于环境地标检测的惯导-地图融合定位方法，包括以下步骤：
S1、获取全球导航卫星系统GNSS观测数据，并建立地标数据库；
S2、根据全球导航卫星系统GNSS观测数据，完成惯性导航系统的初始化，并根据惯性导航系统，得到系统状态预测结果；
S3、获取卫星图像数据，并根据卫星图像数据和系统状态预测结果，通过特征提取算法，得到关键帧集合；
S4、根据关键帧集合与地标数据库，得到关键帧基于匹配的有效性结果集合；
S5、根据惯性导航系统和关键帧基于匹配的有效性结果集合，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
所述步骤S2具体为：
S201、保持载具直线移动一段时间，使惯性导航系统保持绝对姿态；
S202、在步骤S201的基础上，在融合惯性测量单元IMU中进行地球自转补偿，完成惯性导航系统的初始化；
S203、根据惯性导航系统的惯性测量单元IMU，获取系统状态预测结果。
所述步骤S203中系统状态预测结果的表达式为：
其中，为系统状态预测结果；/>为预测的位姿矩阵的导数信息；/>为预测的速度的导数信息；/>为预测的位置误差的导数信息；/>为向量积；/>为在世界坐标系中的地球自转速率；/>为上一次预测的位姿矩阵；/>为载具坐标系相对于世界坐标系的旋转矩阵；/>为陀螺仪测量的误差；fb为加速度计测量的误差；/>为上一次预测的速度信息；gw为重力矢量；δ为变分符号。
所述步骤S3具体为：
S301、利用全球导航卫星系统GNSS，得到卫星图像数据；
S302、根据卫星图像数据，获取当前图像帧；
S303、利用Harris角点检测器获取当前图像帧的环境地标特征点；
S304、根据系统状态预测结果和当前图像帧的环境地标特征点，利用Lukas-Kanade光流算法进行特征追踪，得到光流速度矢量；
S305、将光流速度矢量与图像数据的帧间隔相乘，得到当前图像帧环境地标与载具的相对位置；
S306、根据当前图像帧的环境地标特征点，判断是否出现新的环境地标特征点，若是，直接进入步骤S307，否则，返回步骤S302，获取下一图像帧；
S307、根据当前图像帧环境地标与载具的相对位置，判断出现平均视差变化是否小于第一阈值，若是，将当前图像帧作为关键帧插入关键帧集合，否则，返回步骤S302，获取下一图像帧。
所述步骤S4具体为：
S401、根据关键帧集合，获取与地标数据库中的地标数据成功匹配的第一个关键帧，并以第一个关键帧为参考关键帧；
S402、从关键帧集合中获取下一关键帧作为当前关键帧；
S403、判断地标数据库中是否有地标数据与当前关键帧的环境地标特征点成功匹配，若是，直接进入步骤S404，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S404、根据参考关键帧的系统状态预测结果，将参考关键帧的环境地标特征点与当前关键帧的环境地标特征点进行三角测量，得到环境地标在当前关键帧中的深度；
S405、将参考关键帧更新为当前关键帧；
S406、判断环境地标在当前关键帧中的深度是否小于第二阈值，若是，直接进入步骤S407，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S407、根据关键帧集合，对当前关键帧的前后若干关键帧进行深度计算，得到前后帧的深度结果；
S408、根据前后帧的深度结果，判断当前关键帧的环境地标特征点是否为离群值，若是，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧，否则，判断当前关键帧为有效帧，得到当前关键帧的有效性结果；
S409、根据各关键帧的有效性结果，得到关键帧基于匹配的有效性结果集合。
所述步骤S5具体为：
S501、根据关键帧基于匹配的有效性结果集合，设置视觉和GNSS伪距信息的权重；
S502、根据视觉和GNSS伪距信息的权重，得到满足最大后验估计的状态向量；
S503、根据满足最大后验估计的状态向量中世界坐标系下的位置与姿态四元数信息，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
所述步骤S501中若关键帧的有效性结果为有效帧或当GNSS信号短暂丢失，则将视觉残差的动态权重设置为3，GNSS残差的动态权重设置为0.8；否则，将视觉残差的动态权重设置为0.8，GNSS残差的动态权重设置为3。
所述步骤S502中满足最大后验估计的状态向量的表达式为：
其中，χ*为满足最大后验估计的状态向量；argminχ(·)为当最小时χ的取值；rp为来自上一滑动窗口的先验残差矩阵；Hp为来自上一滑动窗口的先验雅可比矩阵；χ为状态向量，包含IMU测量时每个时间节点的状态，Yaw角偏差，和地标深度的倒数；k为视觉滑动窗口中的时间节点编号；n为视觉滑动窗口中时间节点的数量；rI为IMU预积分；为视觉滑动窗口中第k-1个时间节点处的系统状态；/>为IMU预积分的马氏范数的平方；/>为第k-1个和第k个时间节点处IMU预积分的协方差；αVM为视觉残差的动态权重；l为地图中的地标；L为在滑窗中的地图地标；rV为视觉重投影；/>为由关键帧i和关键帧j得出的系统状态；i和j均为包含地标l的关键帧；/>为视觉重投影的马氏范数的平方；/>为取关键帧i和关键帧j时视觉重投影的协方差；αGM为GNSS残差的动态权重；rG为GNSS-RTK的残差因子；为滑动窗口中GNSS-RTK测量时的系统状态；/>为GNSS-RTK残差因子的马氏范数的平方；m为滑动窗口中GNSS-RTK测量的次数；h为GNSS-RTK测量编号。
本实施例中，本发明基于动态权重和环境地标进行了全球导航卫星系统GNSS，惯性测量单元IMU，卫星图像数据的融合定位，包括获取GNSS数据，使用其辅助惯性导航系统进行初始化；获取图像数据，采用特征提取算法提取环境地标的特征点，并将其与预先建立的地标数据库进行匹配和关联；根据当前关键帧与检测到环境地标的第一张关键帧计算地标在关键帧中的深度并决定其有效性；根据匹配和关联结果调整图像数据和GNSS伪距信息的权重，从而调整因子图在当前时刻的滑动窗口优化器，并求解最大后验估计以得到当前位姿。
Claims (8)
1.一种基于环境地标检测的惯导-地图融合定位方法，其特征在于，包括以下步骤：
S1、获取全球导航卫星系统GNSS观测数据，并建立地标数据库；
S2、根据全球导航卫星系统GNSS观测数据，完成惯性导航系统的初始化，并根据惯性导航系统，得到系统状态预测结果；
S3、获取卫星图像数据，并根据卫星图像数据和系统状态预测结果，通过特征提取算法，得到关键帧集合；
S4、根据关键帧集合与地标数据库，得到关键帧基于匹配的有效性结果集合；
S5、根据惯性导航系统和关键帧基于匹配的有效性结果集合，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
2.根据权利要求1所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S2具体为：
S201、保持载具直线移动一段时间，使惯性导航系统保持绝对姿态；
S202、在步骤S201的基础上，在融合惯性测量单元IMU中进行地球自转补偿，完成惯性导航系统的初始化；
S203、根据惯性导航系统的惯性测量单元IMU，获取系统状态预测结果。
3.根据权利要求2所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S203中系统状态预测结果的表达式为：
其中，为系统状态预测结果；/>为预测的位姿矩阵的导数信息；/>为预测的速度的导数信息；/>为预测的位置误差的导数信息；/>为向量积；/>为在世界坐标系中的地球自转速率；/>为上一次预测的位姿矩阵；/>为载具坐标系相对于世界坐标系的旋转矩阵；/>为陀螺仪测量的误差；fb为加速度计测量的误差；/>为上一次预测的速度信息；gw为重力矢量；δ为变分符号。
4.根据权利要求1所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S3具体为：
S301、利用全球导航卫星系统GNSS，得到卫星图像数据；
S302、根据卫星图像数据，获取当前图像帧；
S303、利用Harris角点检测器获取当前图像帧的环境地标特征点；
S304、根据系统状态预测结果和当前图像帧的环境地标特征点，利用Lukas-Kanade光流算法进行特征追踪，得到光流速度矢量；
S305、将光流速度矢量与图像数据的帧间隔相乘，得到当前图像帧环境地标与载具的相对位置；
S306、根据当前图像帧的环境地标特征点，判断是否出现新的环境地标特征点，若是，直接进入步骤S307，否则，返回步骤S302，获取下一图像帧；
S307、根据当前图像帧环境地标与载具的相对位置，判断出现平均视差变化是否小于第一阈值，若是，将当前图像帧作为关键帧插入关键帧集合，否则，返回步骤S302，获取下一图像帧。
5.根据权利要求4所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S4具体为：
S401、根据关键帧集合，获取与地标数据库中的地标数据成功匹配的第一个关键帧，并以第一个关键帧为参考关键帧；
S402、从关键帧集合中获取下一关键帧作为当前关键帧；
S403、判断地标数据库中是否有地标数据与当前关键帧的环境地标特征点成功匹配，若是，直接进入步骤S404，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S404、根据参考关键帧的系统状态预测结果，将参考关键帧的环境地标特征点与当前关键帧的环境地标特征点进行三角测量，得到环境地标在当前关键帧中的深度；
S405、将参考关键帧更新为当前关键帧；
S406、判断环境地标在当前关键帧中的深度是否小于第二阈值，若是，直接进入步骤S407，否则，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧；
S407、根据关键帧集合，对当前关键帧的前后若干关键帧进行深度计算，得到前后帧的深度结果；
S408、根据前后帧的深度结果，判断当前关键帧的环境地标特征点是否为离群值，若是，判断当前关键帧为无效，得到当前关键帧的有效性结果，并返回步骤S402，获取下一关键帧，否则，判断当前关键帧为有效帧，得到当前关键帧的有效性结果；
S409、根据各关键帧的有效性结果，得到关键帧基于匹配的有效性结果集合。
6.根据权利要求1所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S5具体为：
S501、根据关键帧基于匹配的有效性结果集合，设置视觉和GNSS伪距信息的权重；
S502、根据视觉和GNSS伪距信息的权重，得到满足最大后验估计的状态向量；
S503、根据满足最大后验估计的状态向量中世界坐标系下的位置与姿态四元数信息，得到载具当前位姿，完成基于环境地标检测的惯导-地图融合定位。
7.根据权利要求6所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S501中若关键帧的有效性结果为有效帧或当GNSS信号短暂丢失，则将视觉残差的动态权重设置为3，GNSS残差的动态权重设置为0.8；否则，将视觉残差的动态权重设置为0.8，GNSS残差的动态权重设置为3。
8.根据权利要求7所述基于环境地标检测的惯导-地图融合定位方法，其特征在于，所述步骤S502中满足最大后验估计的状态向量的表达式为：
其中，χ*为满足最大后验估计的状态向量；argminχ(·)为当最小时χ的取值；rp为来自上一滑动窗口的先验残差矩阵；Hp为来自上一滑动窗口的先验雅可比矩阵；χ为状态向量，包含IMU测量时每个时间节点的状态，Yaw角偏差，和地标深度的倒数；k为视觉滑动窗口中的时间节点编号；n为视觉滑动窗口中时间节点的数量；rI为IMU预积分；为视觉滑动窗口中第k-1个时间节点处的系统状态；/>为IMU预积分的马氏范数的平方；/>为第k-1个和第k个时间节点处IMU预积分的协方差；αVM为视觉残差的动态权重；l为地图中的地标；L为在滑窗中的地图地标；rV为视觉重投影；/>为由关键帧i和关键帧j得出的系统状态；i和j均为包含地标l的关键帧；/>为视觉重投影的马氏范数的平方；/>为取关键帧i和关键帧j时视觉重投影的协方差；αGM为GNSS残差的动态权重；rG为GNSS-RTK的残差因子；/>为滑动窗口中GNSS-RTK测量时的系统状态；/>为GNSS-RTK残差因子的马氏范数的平方；m为滑动窗口中GNSS-RTK测量的次数；h为GNSS-RTK测量编号。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311606713 | 2023-11-28 | ||
| CN2023116067138 | 2023-11-28 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117760427A true CN117760427A (zh) | 2024-03-26 |
Family
ID=90325866
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410009428.6A Pending CN117760427A (zh) | 2023-11-28 | 2024-01-03 | 一种基于环境地标检测的惯导-地图融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117760427A (zh) |
-
2024
- 2024-01-03 CN CN202410009428.6A patent/CN117760427A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Heo et al. | EKF-based visual inertial navigation using sliding window nonlinear optimization | |
| CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
| US8761439B1 (en) | Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 | |
| CN113436261B (zh) | 一种面向封闭园区自动驾驶的单目视觉惯性定位方法 | |
| CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN115388884A (zh) | 一种智能体位姿估计器联合初始化方法 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| CN115435779A (zh) | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 | |
| CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| US11874116B2 (en) | Range image aided inertial navigation system (INS) with map based localization | |
| CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN116380070A (zh) | 一种基于时间戳优化的视觉惯性定位方法 | |
| CN116105729A (zh) | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 | |
| CN116482735A (zh) | 一种受限空间内外高精度定位方法 | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN114440877B (zh) | 一种异步多相机视觉惯性里程计定位方法 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| EP4047310A1 (en) | Range image aided ins | |
| CN117760427A (zh) | 一种基于环境地标检测的惯导-地图融合定位方法 | |
| CN115344033A (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 | |
| Li et al. | Accuracy-and Simplicity-Oriented Self-Calibration Approach for In-Vehicle GNSS/INS/Vision System With Observability Analysis | |
| CN117739972B (zh) | 一种无全球卫星定位系统的无人机进近阶段定位方法 |
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 |