CN113483755A - 一种基于非全局一致地图的多传感器组合定位方法及系统 - Google Patents

一种基于非全局一致地图的多传感器组合定位方法及系统 Download PDF

Info

Publication number
CN113483755A
CN113483755A CN202110780435.2A CN202110780435A CN113483755A CN 113483755 A CN113483755 A CN 113483755A CN 202110780435 A CN202110780435 A CN 202110780435A CN 113483755 A CN113483755 A CN 113483755A
Authority
CN
China
Prior art keywords
pose
map
time
scale factor
positioning
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.)
Granted
Application number
CN202110780435.2A
Other languages
English (en)
Other versions
CN113483755B (zh
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 Yihang Yuanzhi Technology Co Ltd
Original Assignee
Beijing Yihang Yuanzhi Technology Co Ltd
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 Yihang Yuanzhi Technology Co Ltd filed Critical Beijing Yihang Yuanzhi Technology Co Ltd
Priority to CN202110780435.2A priority Critical patent/CN113483755B/zh
Publication of CN113483755A publication Critical patent/CN113483755A/zh
Application granted granted Critical
Publication of CN113483755B publication Critical patent/CN113483755B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

一种基于非全局一致地图的多传感器组合定位方法及系统，该方法包括以下步骤：步骤S1、建立轮式里程计/陀螺仪在离散时间下的联合运动学模型；步骤S2、构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；步骤S3、在逻辑地图中利用轮式里程计的数据分段估计标度因子，确定地图位置与标度因子的对应关系；步骤S4、对视觉定位情况进行判断，如获取视觉定位正常，则优化轮式里程计与陀螺仪的关键内参，同时借助优化信息同步更新最新的位姿；如视觉定位丢失，则利用步骤S3中获取的先验标度因子进行状态传播。该方法能够在尺度不一致的视觉地图中利用物理传感器进行有效定位，且鲁棒性好，准确性高，适应性强。

Description

一种基于非全局一致地图的多传感器组合定位方法及系统
技术领域
本发明属于智能机器人多传感器定位导航技术领域，具体的说是一种基于尺度非全局一致的单目视觉地图/轮速/陀螺仪的多传感器组合定位方法，该方法可应用于自动驾驶车辆的定位模块，智能机器人多传感器融合模块，机器人里程计模块等。
现有技术
组合定位导航是机器人定位导航领域的研究热点，而在视觉地图下提供快速鲁棒的位姿推算是其中重要的组成部分，但由于稀疏特征地图的特征不鲁棒，会不可避免的出现定位丢失，并且短时无法重定位；另外，由于单目视觉地图尺度不统一，难免出现物理里程计的轨迹与地图形态差距很大的问题。
为了解现有技术的发展状况，本发明对已有的专利和论文进行了检索、比较和分析，筛选出如下3篇与本发明相关度比较高的技术方案。
技术方案1：专利号为CN108489482A的《视觉惯性里程计的实现方法及系统》，涉及一种视觉惯性里程计的实现方法，该方法主要包括以下步骤：(1)通过相机实时采集图像数据，通过IMU采集陀螺仪和加速度的数据；(2)场景初始化模块建立视觉惯性里程计系统的初始空间三维地图；(3)地图扩展模块对空间三维地图进行实时更新；(4)姿态估计模块接收计算出每一帧图像对应的设备的位置和姿态。该方法属于单目视觉惯性里程计，引入了惯性测量单元，而且能够进行地图的增量构建，但是没有引入位姿推算中马尔科夫过程的协方差，因此没有误差传递模型。此外，还会面临由车辆运动特性所导致的里程计尺度退化的问题，如果和原始地图定位的尺度不一致而导致尺度偏差叠加，就会造成严重的定位误差。
技术方案2：专利号为CN112050809A的《轮式里程计与陀螺仪信息融合的无人车定向定位方法》，涉及用于轮式里程计与陀螺仪信息融合的无人车定向定位方法，该方法主要包括以下步骤：(1)建立无人车运动学模型；(2)建立精确的无人车离散时间下的运动学模型，采用几何法求解无人车离散时间下的运动学模型；(3)建立传感器数学模型；(4)轮式里程计与陀螺仪信息融合估算方法；(5)改进卡尔曼滤波的航向角估计方法。该方法引入了噪声和误差传递模型，通过协方差矩阵来进行位姿优化的比率判定，但是没有估计轮速与imu或者陀螺的关键内部参数等策略，车辆的每一次运行或运行过程当中都有可能由于乘坐人数等原因造成负重变化，从而引起车轮滚动半径的变化，带来了轮式里程计标定因子的误差。此外，该方法有一定的尺度全局一致性，没有去适配已知地图的尺度变化。
技术方案3：专利号为CN112066983A的《惯性/里程计组合导航滤波方法、电子设备及存储介质》，涉及组合导航非线性滤波技术领域，特别是涉及一种基于状态变换卡尔曼滤波(ST-EKF)的惯性/里程计组合导航的在线平滑方法。该方法主要包括以下步骤：(1)基于捷联惯导和轮式里程计各自接收的数据信息，建立基于ST-EKF的惯性/里程计组合导航模型；(2)基于ST-EKF的惯性/里程计组合导航模型完成前向滤波，并将输出的滤波信息实时存入滑动窗口空间；(3)利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航系统输出的姿态、速度、位置信息。该发明是一种具备实时航迹校正能力和更好的滤波鲁棒性的在线平滑滤波方法，是改善复杂环境下无人驾驶车辆导航定位精度的可行方法。该方法在技术方案2的基础上，能够利用滑动窗口和RTS平滑算法解决尺度统一的物理地图下的定位和里程计问题，并采用ST-EKF估计了里程计标度因子等误差。但是，ST-EKF是基于滤波的估计方案，可以利用的先验信息较少，误差估计可以进一步提高；其次，从其建立的EKF数学模型可以看出，该方法是利用惯性单元来估计标度因子等误差，会将惯性单元本身的估计误差(尤其是重力向量)引入里程计的标度因子误差；再次，该方法并没有估计惯性单元的加速度计和陀螺仪的偏置并进行参数的最佳逼近；最后，改方案依然没有引入地图的先验信息，只是一个平滑的定位算法。
综上可知，现有的组合定位方法仍无法解决在尺度不一致的视觉地图中利用物理传感器进行定位的难题。因此，如何设计出一个自适应地图的里程计，充分利用地图的先验信息，实时对里程计的关键参数(如物理传感器适应地图的尺度，偏置等)进行最佳逼近，并且在视觉定位丢失的情况下利用这些信息进行长时间的精确定位，是具有重要理论意义和实际应用价值的。
发明内容
鉴于上述问题，本发明的目的在于提供一种基于非全局一致地图的多传感器组合定位方法，该方法能够在尺度不一致的视觉地图中利用物理传感器进行有效定位，且鲁棒性好，准确性高，适应性强。
为实现上述目的，本发明采用如下技术方案：
一种基于非全局一致地图的多传感器组合定位方法，该方法包括以下步骤：
步骤S1、建立轮式里程计/陀螺仪在离散时间下的联合运动学模型；
步骤S2、构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
步骤S3、在优化过的逻辑地图中利用轮式里程计的数据分段估计标度因子，确定地图位置与标度因子的对应关系；
步骤S4、对视觉定位情况进行判断，如获取视觉定位正常，则执行步骤S5；如视觉定位丢失，则执行步骤S6；
步骤S5、在既有视觉地图正常定位的过程中，利用优化求解器并联合步骤S2的数学模型优化轮式里程计与陀螺仪的关键内参，估计轮式里程计标度因子的变化，同时借助优化信息同步更新最新的位姿；
步骤S6、在视觉定位丢失时，利用步骤S5获取的最新的优化信息与步骤S3中获取的先验标度因子进行状态传播。
作为本发明的优选，所述步骤S1建立的联合运动学模型的计算方式如下：
Figure BDA0003156576770000031
Figure BDA0003156576770000032
积分过程如下：
Figure BDA0003156576770000033
Figure BDA0003156576770000034
Figure BDA0003156576770000035
式中，Δq为Δt时间内的姿态增量，Δp为Δt时间内的位移增量；ΔPl，ΔPr分别为Δt时间内左轮和右轮的脉冲增量，s为标度因子；ω为Δt时间段的平滑角速度，
Figure BDA0003156576770000037
为旋转的角度；t为时间的积分限，pt为位置积分最终结果，qt为姿态积分最终结果，Δpi为一个时间间隔内的位置增量，qi为i时刻的姿态，Δti为i时刻的时间间隔，Δqj为j时刻的姿态增量，Twb为位姿，Rq为姿态q以旋转矩阵的表示，p为位置；n为Δt时间段内记录的角速度数据个数，ωi为i时刻的角速度。
作为本发明的优选，所述步骤S2建立的数学模型的目标函数为：
Figure BDA0003156576770000036
式中，
Figure BDA0003156576770000041
表示i时刻绝对位置残差,
Figure BDA0003156576770000042
表示i时刻绝对姿态残差，
Figure BDA0003156576770000043
表示ij时刻之间的相对位置残差，
Figure BDA0003156576770000044
表示ij时刻之间的相对姿态残差，
Figure BDA0003156576770000045
为优化的滑动窗口，χ为优化变量，为每个优化窗口
Figure BDA0003156576770000046
内的位姿T，标度因子s，以及陀螺仪角速度偏置引起的姿态偏置qbias，
Figure BDA0003156576770000047
为i时刻的绝对位姿观测的位置部分和姿态部分，
Figure BDA0003156576770000048
为i时刻和j时刻之间的相对位姿观测的位置部分和姿态部分，ρ为损失核函数。
作为本发明的优选，所述步骤S2建立的数学模型中所需的残差向量的计算方式如下：
Figure BDA0003156576770000049
式中，ei表示i时刻的残差向量，[Ro，po]为body系下作为优化变量的姿态，[Rb，pb]为内部传感器的观测，[Rc，pc]为相机观测，R表示的是姿态，为旋转矩阵形式，q表示的也是姿态，为四元数形式，p表示的是位置，w作为参数的上标，表示的是描述该参数的坐标系为世界坐标系，下标b和c分别表示的是被描述的坐标系为车体系和相机系；下标o表示的是该参数属于需要被优化的优化变量，下标i和j表示的是参数的索引，
Figure BDA00031565767700000410
为相机到车体的外部参数平移部分，
Figure BDA00031565767700000411
表示相机到车体坐标的外部参数旋转部分。
作为本发明的优选，引入标定误差和陀螺仪的角速度偏置后，初始预测的位姿则需要根据优化后的关键内参进行更新；
其中，所述位置qt的更新方式为：
Figure BDA00031565767700000412
式中，qbias为陀螺仪的角速度偏置引起的姿态偏置；
所述姿态pt的更新方式为：
Figure BDA00031565767700000413
ΔS＝Snew-Sold
式中，snew为更新后的标度因子，sold为更新前的标度因子。
作为本发明的优选，所述步骤S3在确定地图位置与标度因子的对应关系时，具体步骤如下：
步骤S3.1、对建立好的地图做一次全程定位，设定Δt时间内且位移量超过Δp的窗口；
步骤S3.2、在窗口内计算视觉定位轨迹位移量和轮式里程计位移量的标度因子；
Figure BDA0003156576770000051
其中，下标i和j表示i时刻和j时刻，w作为参数的上标，表示的是描述该参数的坐标系为世界坐标系，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系，
Figure BDA0003156576770000052
为相机到车体的外部参数位置部分，Δt表示的是两时刻的时间间隔；
步骤S3.3、根据时间确定地图位置与标度因子的对应关系。
作为本发明的优选，所述步骤S5借助优化信息同步更新最新位姿的具体方法为：
步骤S5.1、将视觉定位转换成系统定位；其中，所述视觉定位是指相机在地图下的定位，所述系统定位是指车辆在地图下的定位；
步骤S5.2、根据步骤S1建立的联合运动学模型，通过积分和插值的方式，获取对应轮式里程计观测，即获取轮式里程计在视觉定位时刻下的位姿；
步骤S5.3、移动窗口，读取历史状态变量；
步骤S5.4、更新视觉位姿和关键参数，将最新的观测和预测加入状态变量所组成的向量当中；
步骤S5.5、构建优化求解器，联合步骤S2的数学模型融合求解；其中，所述优化求解器在一个滑动窗口范围之内的目标函数为：
Figure BDA0003156576770000053
步骤S5.6、根据步骤S5.5获取的优化参数推导由轮式里程计决定的系统位姿。
作为本发明的优选，所述步骤S6更新最新位姿的具体方法为：
步骤S6.1、利用步骤S3所产生的分段标度因子，找出视觉定位丢失位置所对应的历史标度因子；
步骤S6.2、根据步骤S5中保存的最新标度因子来确定二者标度因子比率；
步骤S6.3、根据步骤S6.2的标度因子比率更新当前标度因子；其中，所述当前标度因子是指i时刻地图位置对应的历史标度因子；
步骤S6.4、结合步骤S5获取的优化变量与步骤S6.3更新后的标度因子更新最新位姿。
作为本发明的优选，根据优化参数对位姿进行解算的方式如下：
Figure BDA0003156576770000054
式中，下标k表示起始时刻的索引，n表示终止时刻的索引，i表示遍历视觉定位没有达到的位姿的每一个索引，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系，下标p表示的是该参数由被优化的优化变量推导传播而来，
Figure BDA0003156576770000061
表示更新完毕后的姿态结果，
Figure BDA0003156576770000062
表示更新完毕后的位置结果，
Figure BDA0003156576770000063
为视觉定位没有达到的位姿的集合，
Figure BDA0003156576770000064
为i时刻的姿态更新量，
Figure BDA0003156576770000065
为i时刻位置更新量，sn表示当前最优的标度因子。
本发明的另一目的在于一种基于非全局一致地图的多传感器组合定位系统，该系统包括位姿预测模块、状态变量关联模块、预测位姿更新模块、标度与地图构建模块、优化求解器、位姿解算模块；其中,所述位姿预测模块，用于根据传感器输出的增量编码器信息和角速度信息来解算车辆运行过程中任意时刻的位置和姿态信息；解算时利用已建立的轮式里程计/陀螺仪在离散时间下的联合运动学模型进行解算；
所述状态变量关联模块，用于根据内部传感器观测下和相机观测下的误差，构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
所述预测位姿更新模块，用于根据标度误差以及陀螺仪角速度偏置引起的姿态偏置对位姿预测模块解算的位置和姿态进行更新；
所述标度与地图构建模块，用于构建地图位置与标度因子的对应关系；
所述优化求解器，用于根据观测信息、历史状态变量、误差函数进行求解关键参数，并将获得的关键参数发送给位姿解算模块；
所述位姿解算模块，视觉定位正常时，根据优化求解器发送的参数推导出车辆在地图中的位姿；视觉定位丢失后，根据优化求解器中最新的标度因子和该位置的历史标度因子求解标度因子比率，根据标度因子比率以及历史标度因子更新标度因子，利用更新后的标度因子推导出车辆在地图中的位姿。
本发明的优点和技术效果是：
1、本发明对物理传感器的参数在逻辑地图中进行分段估计，充分考虑逻辑地图的先验信息，利用先验信息将标度因子对齐到尺度不一致的视觉地图中，以获取更好的适配性，保证在视觉定位丢失的情况下，能够利用物理传感器在逻辑地图中进行有效定位。
2、本发明提供的定位方法在已知视觉地图的情况下，利用更多的有效信息在线估计当前传感器关键内参，对物理传感器关键参数进行调优，并适配到地图的不同路段，保持定位的鲁棒性和准确性。
3、本发明利用既有逻辑地图的实时定位信息，实现估计轮式里程计标度因子的优化策略，用以在线估计当前里程计的标度因子，并由此对地图中不同路段的标度因子进行数据关联。
4、本发明利用滑动窗口位姿约束因子图的优化算法，通过对窗口内的某些关键位姿建立相对位姿约束，在线估计更为准确的陀螺仪角速度偏置。
5、本发明通过更新参数更新后的状态传播模型，大大减少位姿推算的时间，减少系统计算资源的消耗。
6、本发明提出传感器误差的简化参数化模型，在最优估计完成后，大大提高状态传播过程的计算效率，并能保持同一数量级的位姿精度。
附图说明
图1是本发明方法的整体流程图。
图2是本发明步骤S5的具体流程图。
图3是本发明系统的结构框图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
实施例1一种基于非全局一致地图的多传感器组合定位方法
本发明旨在提供一种基于非全局一致地图的多传感器组合定位方法，图1描述了本发明的整体流程图。
如图1所示，本发明提供的一种基于非全局一致地图的多传感器组合定位方法，包括以下步骤：
步骤S1、建立轮式里程计/陀螺仪在离散时间下的联合运动学模型；
步骤S2、构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
步骤S3、在优化过的逻辑地图中利用轮式里程计的数据分段估计标度因子，确定地图位置与标度因子的对应关系；
步骤S4、对视觉定位情况进行判断，如获取视觉定位正常，则执行步骤S5；如视觉定位丢失，则执行步骤S6；
步骤S5、在既有视觉地图正常定位的过程中，利用优化求解器并联合步骤S2的数学模型优化轮式里程计与陀螺仪的关键内参，估计轮式里程计标度因子的变化，同时借助优化信息同步更新最新的位姿；
步骤S6、在视觉定位丢失时，利用步骤S5获取的最新的优化信息与步骤S3中获取的先验标度因子进行状态传播。
注意：
a.特别的，为了方便符号表示，П中的乘法与和四元数相关的乘法及其缩写，均代表四元数乘法，而不写为广义加法，或某个二元运算，没有歧义。
b.涉及系统状态的所有初值，是该状态关于所在代数系统中对应的二元运算的幺元，本发明中所有状态变量的左幺元等于右幺元。
为了使本领域技术人员清楚了解本申请上述步骤具体是如何实现的，下面分别对上述步骤进行详细介绍。
(1)建立轮式里程计/陀螺仪在离散时间下的联合运动学模型
(1.1)坐标系定义
将轮式里程计的坐标系(如车辆后轴中心)，即body坐标系，用下标b表示。
陀螺仪坐标系，简称imu坐标系，用下标i表示。
相机坐标系，即camera坐标系，用下标c表示。
导航坐标系或称为建立的世界地图，即world坐标系，用下标w表示。
车辆坐标系采用ENU，所有坐标系均采用右手系。
(1.2)运动传播方程
根据所采用的轮式里程计和陀螺仪的传感器性质，建立车辆的非完整性约束运动模型，将离散时间下的状态传播的过程分解为位移+姿态的推算方法：
Figure BDA0003156576770000081
Figure BDA0003156576770000082
积分过程如下：
Figure BDA0003156576770000083
Figure BDA0003156576770000084
Figure BDA0003156576770000091
其中，Δq为Δt时间内的姿态增量，Δp为Δt时间内的位移增量，ΔPl，ΔPr分别为Δt时间内左轮和右轮的脉冲增量，s为标度因子，ω为Δt时间段的平滑角速度，θ为旋转的角度；t为时间的积分限，pt为位置积分最终结果，qt为姿态积分最终结果，Δpi为一个时间间隔内的位置增量，qi为i时刻的姿态，Δti为i时刻的时间间隔，Δqj为j时刻的姿态增量，Twb为位姿，Rq为姿态q以旋转矩阵的表示，p为位置；n为Δt时间段内记录的角速度数据个数，ωi为i时刻的角速度。
建立好联合运动学模型公式，就可以利用传感器输出的增量编码信息和角速度信息，通过积分和线性插值的方式，来解算车辆/机器人运行过程中任意时刻的位置和姿态信息。
(2)构建标度因子，陀螺仪偏置与状态变量等参数最优化问题的数学模型
为了能够实现状态变量的数值最优化，只拥有运动学模型是不够的，还需要引入误差模型，而误差模型当中的目标函数在残差的计算过程中，需要结合运动学模型的解算结果和其他外部传感器(如：相机)的观测结果。由于优化变量当中包含了imu的某些内部参数(如：bias)，在完成最优化问题的解算之后，可以用得到的最优内部参数，结合运动模型，来实现位姿传播这一部分的增量更新。
(2.1)更新方程
为了简化优化之后的积分过程，首先将偏置建模为Δt时间内的姿态变化量，而不是角速度偏置，避免了Δti时间内陀螺的重新积分。
引入bias的姿态更新方程：
Figure BDA0003156576770000092
其中，qbias为陀螺仪的角速度偏置引起的姿态偏置；
引入尺度误差项的位移更新方程：
Figure BDA0003156576770000093
Δs＝shew-sold (8)
其中，shew为更新后的标度因子，sold为更新前的标度因子；
(2.2)确定目标函数
需要优化的滑动窗口符号为
Figure BDA0003156576770000094
需要优化的变量为每个优化窗口
Figure BDA0003156576770000095
内的位姿T，标度因子s，以及陀螺仪角速度偏置引起的姿态偏置qbias；滑动窗口
Figure BDA0003156576770000096
内总共k+1个位姿。
定义优化变量χ：
χk＝[Tk s qbias]＝[Ti Ti+1 … Ti+k s qbias]
＝[qi qi+1 … qi+k pi pi+1 … pi+k s qbias]
(9)
其中，下标k表示的是滑动窗口W中位姿T的个数，下标i表示的是窗口的起点，下标的取值范围不会超过滑动窗口Wk的个数范围，s是标度因子，qbias是陀螺仪角速度偏置引起的姿态偏置，T是姿态[p，q]的简写形式；
误差模型：
两个姿态下的误差模型，其中观测来自camera对视觉地图的定位，本申请将body系下作为优化变量的姿态写为Ro，po，Rb，pb代表内部传感器的观测，Rc，pc代表相机观测，其中R表示的是姿态，为旋转矩阵形式，q表示的也是姿态，为四元数形式，p表示的是位置，形如[q]xyz，表示四元数虚部部分：
Figure BDA0003156576770000101
其中，等式左边，ei表示i时刻的残差向量，具体为：
Figure BDA0003156576770000102
表示i时刻绝对位置残差，
Figure BDA0003156576770000103
表示i时刻绝对姿态残差，
Figure BDA0003156576770000104
表示ij时刻之间的相对位置残差，
Figure BDA0003156576770000105
表示ij时刻之间的相对姿态残差；等式右边，R表示的是姿态，p表示的是位置，w作为参数的上标，表示的是描述该参数的坐标系为世界坐标系，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系；下标o表示的是该参数属于需要被优化的优化变量，下标i和j表示的是参数的索引，
Figure BDA0003156576770000106
为相机到车体的外部参数平移部分，
Figure BDA0003156576770000107
表示相机到车体坐标的外部参数旋转部分；
因此，定义目标函数为：
Figure BDA0003156576770000108
其中，
Figure BDA0003156576770000109
χ，
Figure BDA00031565767700001010
的意义同上，
Figure BDA00031565767700001011
为i时刻的绝对位姿观测的位置部分和姿态部分，
Figure BDA00031565767700001012
为i时刻和j时刻之间的相对位姿观测的位置部分和姿态部分，ρ为损失核函数。
(3)在优化过的逻辑地图中利用轮式里程计的数据分段估计标度因子，确定地图位置与标度因子对应关系
数学模型构建完成之后，需要获取一次定位过程中的视觉定位和轮式里程计分别在地图当中的对应关系，只有获取到视觉定位和轮式里程计在地图中不同路段的对应关系之后，方能开始进行组合定位，其具体步骤如下：
步骤S3.1、对于建立好的地图做一次全程定位，设定Δt时间内且位移量超过Δp的窗口；
步骤S3.2、在窗口内采用基于优化的方法，或滑动平均的方法，计算视觉定位轨迹位移量和轮式里程计位移量的标度因子。
Figure BDA0003156576770000111
其中，下标i和j表示i时刻和j时刻，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系,下标k表示的是第k次的窗口计算，tk表示的是第k次计算所处于的时刻，
Figure BDA0003156576770000112
表示的是tk时刻标度因子的估计，
Figure BDA0003156576770000113
为相机到车体的外部参数位置部分，Δt表示的是两时刻的时间间隔；
步骤S3.3、由于是同时定位，可以根据时间可以找到位置上的对应关系；
Figure BDA0003156576770000114
最终得到地图位置与标度因子的对应关系；即在定位过程中有了可以根据地图不同位置采用不同的标度因子进行计算的先决条件；
Figure BDA0003156576770000115
其中，下标k表示的是第k次的窗口计算，tk表示的是所处于的时刻，
Figure BDA0003156576770000116
表示的是tk时刻标度因子的估计，
Figure BDA0003156576770000117
表示的是tk时刻相机在地图上的位置，
Figure BDA0003156576770000118
表示的是通过
Figure BDA0003156576770000119
计算出来的车体在地图上的位置，
Figure BDA00031565767700001110
表示由
Figure BDA00031565767700001111
位置可以找出对应的标度因子，
Figure BDA00031565767700001112
表示的是一个相关和统一的性质，
Figure BDA00031565767700001113
表示的是一个映射的性质，
Figure BDA00031565767700001114
表示的是一个推导和从属的性质。
(4)在既有视觉地图正常定位的过程中，利用优化求解器并联合步骤S2的数学模型优化轮式里程计与陀螺仪的关键内参，估计轮式里程计标度因子的变化，同时借助优化信息同步更新最新的位姿。
根据上述步骤已经获取到了地图中不同路段的尺度和建图过程中所属车辆/机器人的轮式里程计尺度之间的标度因子。在同一车辆不同负载，或者是不同车辆在该地图下运行的时候，轮式里程计的尺度存在发生较大变化的可能性，因此在一般的定位过程中，需要估计当前在地图中实际运行的车辆的轮式里程计尺度，并记录最新的标度因子，以防备在视觉定位丢失时建立与历史标度因子的对应关系。
再者，由于视觉定位比轮式里程计的直接推算速度要慢，因此还要借助定位来实时解算并更新最新的位姿。
如图2描述了步骤S5的具体流程，如图2所示，步骤S5的具体流程如下：
步骤S5.1、将视觉定位转换乘系统定位。视觉能够定位出传感器(即相机)在当前地图中的姿态，通过该姿态和相机与body的外部参数，可以获取系统(车辆/机器人)在地图下的定位；
步骤S5.2、根据步骤S1建立的运动学模型，通过积分和插值的方式，获取对应里程计观测，即获取轮式里程计在视觉定位时刻下的位姿。
视觉传感器的工作瞬时时间和其他传感器很难严格同频，轮速imu类传感器频率较视觉传感器要高出太多，因此为了得到对应时刻的其他传感器观测，则需要通过插值获取来进行近似处理。
步骤S5.3、移动窗口，读取历史状态变量。优化变量需要包含窗口中的大量先验信息，进行联合优化，而不是只进行马尔科夫式的状态传播和协方差更新。
步骤S5.4、更新视觉位姿和关键参数，将最新的观测和预测加入状态变量所组成的向量当中。
步骤S5.5、构建优化求解器，融合求解。将优化变量，观测信息，误差函数等模型构建成梯度下降问题进行迭代求解；所述优化求解器在一个滑动窗口的范围之内的目标函数如下式：
Figure BDA0003156576770000121
步骤S5.6、根据步骤S5.5获取的优化参数推导由里程计决定的系统位姿。即得到最优结果之后，通过更新的参数来进行状态传播，从而得到最新的系统位姿。
采用里程计和得到的内参对视觉定位没有达到的位姿进行正向更新的解算方式如下：
Figure BDA0003156576770000122
其中，下标k表示起始时刻的索引，n表示终止时刻的索引，i表示遍历视觉定位没有达到的位姿的每一个索引，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系，下标p表示的是该参数由被优化的优化变量推导传播而来，
Figure BDA0003156576770000131
表示更新完毕后的姿态结果，
Figure BDA0003156576770000132
表示更新完毕后的位置结果，
Figure BDA0003156576770000133
为视觉定位没有达到的位姿的集合，
Figure BDA0003156576770000134
为i时刻的姿态更新量，
Figure BDA0003156576770000135
为i时刻位置更新量，sn表示当前最优的标度因子。
(5)在视觉定位丢失时，利用步骤S5获取的最新的优化信息与步骤S3中获取的先验标度因子进行状态传播。
步骤S6在视觉定位丢失后，没有观测值供给目标函数进行更新，此时利用步骤S3所产生的分段标度因子来作为不同位置标度因子的先验，并结合步骤S5中保存的最新标度因子来确定二者标度因子比率，从而得到最适配当前系统的尺度，以进行最优尺度的位姿解算。
步骤S6的具体步骤如下：
步骤S6.1、找出该位置(视觉定位丢失位置)对应的历史标度因子
Figure BDA0003156576770000136
步骤S6.2、结合步骤S5中保存的最新标度因子来确定二者标度因子比率
Figure BDA0003156576770000137
步骤S6.3、运动过程中位置发生变化，导致先验标度因子改变时，根据步骤S6.2的标度因子比率更新当前标度因子；
Figure BDA0003156576770000138
步骤S6.4、结合步骤S5获取的优化变量与步骤S6.3更新后的标度因子更新最新位姿；
位姿更新算法变为：
Figure BDA0003156576770000139
其中，sn表示的最新的标度因子，
Figure BDA00031565767700001310
表示的是更新的意义，sratio表示的是标度因子的比率，
Figure BDA00031565767700001311
表示的是i时刻所在地图位置对应的历史标度因子。
实施例2一种基于非全局一致地图的多传感器组合定位系统
如图3所示，本发明提供的一种基于非全局一致地图的多传感器组合定位系统，该系统包括位姿预测模块1、状态变量关联模块2、预测位姿更新模块3、标度与地图构建模块4、优化求解器5、位姿解算模块6；
其中,所述位姿预测模块1，用于根据传感器输出的增量编码器信息和角速度信息来解算车辆运行过程中任意时刻的位置和姿态信息；解算时利用已建立的轮式里程计/陀螺仪在离散时间下的联合运动学模型进行解算；
所述状态变量关联模块2，用于根据内部传感器观测下和相机观测下的误差，构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
所述预测位姿更新模块3，用于根据标度误差以及陀螺仪角速度偏置引起的姿态偏置对位姿预测模块解算的位置和姿态进行更新；
所述标度与地图构建模块4，用于构建地图位置与标度因子的对应关系；
所述优化求解器5，用于根据观测信息、历史状态变量、误差函数进行求解关键参数，并将获得的关键参数发送给位姿解算模块；
所述位姿解算模块6，视觉定位正常时，根据优化求解器发送的参数推导出车辆在地图中的位姿；视觉定位丢失后，根据优化求解器中最新的标度因子和该位置的历史标度因子求解标度因子比率，根据标度因子比率以及历史标度因子更新标度因子，利用更新后的标度因子推导出车辆在地图中的位姿。
本发明提供的定位方法及系统可用于自动驾驶、全自动机器人等领域，当用于自动驾驶时，其可实现对自动驾驶汽车的定位；当用于全自动机器人领域时，可实现对机器人的定位，该方法鲁棒性好，准确性高，适应性强；因此，凡涉及本申请技术方案的，无论应用在何种领域，均属于本申请保护的范围。

Claims (10)

1.一种基于非全局一致地图的多传感器组合定位方法，其特征在于：该方法包括以下步骤：
步骤S1、建立轮式里程计/陀螺仪在离散时间下的联合运动学模型；
步骤S2、构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
步骤S3、在逻辑地图中利用轮式里程计的数据分段估计标度因子，确定地图位置与标度因子的对应关系；
步骤S4、对视觉定位情况进行判断，如获取视觉定位正常，则执行步骤S5；如视觉定位丢失，则执行步骤S6；
步骤S5、在既有视觉地图正常定位的过程中，利用优化求解器并联合步骤S2的数学模型优化轮式里程计与陀螺仪的关键内参，估计轮式里程计标度因子的变化，同时借助优化信息同步更新最新的位姿；
步骤S6、在视觉定位丢失时，利用步骤S5获取的最新的优化信息与步骤S3中获取的先验标度因子进行状态传播。
2.根据权利要求1所述的多传感器组合定位方法，其特征在于：所述步骤S1建立的联合运动学模型的计算方式如下：
Figure FDA0003156576760000011
Figure FDA0003156576760000012
积分过程如下：
Figure FDA0003156576760000013
Twb=Rq，
Figure FDA0003156576760000014
Figure FDA0003156576760000015
式中，Δq为Δt时间内的姿态增量，Δp为Δt时间内的位移增量；ΔPl，ΔPr分别为Δt时间内左轮和右轮的脉冲增量，s为标度因子；ω为Δt时间段的平滑角速度，θ为旋转的角度；t为时间的积分限，pt为位置积分最终结果，qt为姿态积分最终结果，Δpi为一个时间间隔内的位置增量，qi为i时刻的姿态，Δti为i时刻的时间间隔，Δqj为j时刻的姿态增量，Twb为位姿，Rq为姿态q以旋转矩阵的表示，p为位置；n为Δt时间段内记录的角速度数据个数，ωi为i时刻的角速度。
3.根据权利要求1所述的多传感器组合定位方法，其特征在于：所述步骤S2建立的数学模型的目标函数为：
Figure FDA0003156576760000021
式中，
Figure FDA0003156576760000022
表示i时刻绝对位置残差,
Figure FDA0003156576760000023
表示i时刻绝对姿态残差，
Figure FDA0003156576760000024
表示ij时刻之间的相对位置残差，
Figure FDA0003156576760000025
表示ij时刻之间的相对姿态残差，
Figure FDA0003156576760000026
为优化的滑动窗口，χ为优化变量，为每个优化窗口
Figure FDA0003156576760000027
内的位姿T，标度因子s，以及陀螺仪角速度偏置引起的姿态偏置qbias，
Figure FDA0003156576760000028
为i时刻的绝对位姿观测的位置部分和姿态部分，
Figure FDA0003156576760000029
为i时刻和j时刻之间的相对位姿观测的位置部分和姿态部分，ρ为损失核函数。
4.根据权利要求3所述的多传感器组合定位方法，其特征在于：所述步骤S2建立的数学模型中所需的残差向量的计算方式如下：
Figure FDA00031565767600000210
式中，ei表示i时刻的残差向量，[Ro，po]为body系下作为优化变量的姿态，[Rb，pb]为内部传感器的观测,[Rc，pc]为相机观测，R表示的是姿态，为旋转矩阵形式，q表示的也是姿态，为四元数形式，p表示的是位置，w作为参数的上标，表示的是描述该参数的坐标系为世界坐标系，下标b和c分别表示的是被描述的坐标系为车体系和相机系；下标o表示的是该参数属于需要被优化的优化变量，下标i和j表示的是参数的索引，
Figure FDA00031565767600000211
为相机到车体的外部参数平移部分，
Figure FDA00031565767600000212
表示相机到车体坐标的外部参数旋转部分。
5.根据权利要求3所述的多传感器组合定位方法，其特征在于：引入标定误差和陀螺仪的角速度偏置后，初始预测的位姿则需要根据优化后的关键内参进行更新；
其中，所述位置qt的更新方式为：
Figure FDA00031565767600000213
式中，qbias为陀螺仪的角速度偏置引起的姿态偏置；
所述姿态pt的更新方式为：
Figure FDA0003156576760000031
Δs＝snew-sold
式中，snew为更新后的标度因子，sold为更新前的标度因子。
6.根据权利要求1所述的多传感器组合定位方法，其特征在于：所述步骤S3在确定地图位置与标度因子的对应关系时，具体步骤如下：
步骤S3.1、对建立好的地图做一次全程定位，设定Δt时间内且位移量超过Δp的窗口；
步骤S3.2、在窗口内计算视觉定位轨迹位移量和轮式里程计位移量的标度因子；
Figure FDA0003156576760000032
其中，下标i和j表示i时刻和j时刻，w作为参数的上标，表示的是描述该参数的坐标系为世界坐标系，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系，
Figure FDA0003156576760000033
为相机到车体的外部参数位置部分，Δt表示的是两时刻的时间间隔；
步骤S3.3、根据时间确定地图位置与标度因子的对应关系。
7.根据权利要求1所述的多传感器组合定位方法，其特征在于：所述步骤S5借助优化信息同步更新最新位姿的具体方法为：
步骤S5.1、将视觉定位转换成系统定位；其中，所述视觉定位是指相机在地图下的定位，所述系统定位是指车辆在地图下的定位；
步骤S5.2、根据步骤S1建立的联合运动学模型，通过积分和插值的方式，获取对应轮式里程计观测，即获取轮式里程计在视觉定位时刻下的位姿；
步骤S5.3、移动窗口，读取历史状态变量；
步骤S5.4、更新视觉位姿和关键参数，将最新的观测和预测加入状态变量所组成的向量当中；
步骤S5.5、构建优化求解器，联合步骤S2的数学模型融合求解；其中，所述优化求解器在一个滑动窗口范围之内的目标函数为：
Figure FDA0003156576760000034
步骤S5.6、根据步骤S5.5获取的优化参数推导由轮式里程计决定的系统位姿。
8.根据权利要求1所述的多传感器组合定位方法，其特征在于：所述步骤S6更新最新位姿的具体方法为：
步骤S6.1、利用步骤S3所产生的分段标度因子，找出视觉定位丢失位置所对应的历史标度因子；
步骤S6.2、根据步骤S5中保存的最新标度因子来确定二者标度因子比率；
步骤S6.3、根据步骤S6.2的标度因子比率更新当前标度因子；其中，所述当前标度因子是指i时刻地图位置对应的历史标度因子；
步骤S6.4、结合步骤S5获取的优化变量与步骤S6.3更新后的标度因子更新最新位姿。
9.根据权利要求7或8所述的多传感器组合定位方法，其特征在于：根据优化参数对位姿进行解算的方式如下：
Figure FDA0003156576760000041
式中，下标k表示起始时刻的索引，n表示终止时刻的索引，i表示遍历视觉定位没有达到的位姿的每一个索引，下标b和c分别表示的是被描述的坐标系分别为车体系和相机系，下标表示的是该参数由被优化的优化变量推导传播而来，
Figure FDA0003156576760000042
表示更新完毕后的姿态结果，
Figure FDA0003156576760000043
表示更新完毕后的位置结果，
Figure FDA0003156576760000044
为视觉定位没有达到的位姿的集合，
Figure FDA0003156576760000045
为i时刻的姿态更新量，
Figure FDA0003156576760000046
为i时刻位置更新量，sn表示当前最优的标度因子。
10.一种基于非全局一致地图的多传感器组合定位系统，其特征在于，该系统包括位姿预测模块、状态变量关联模块、预测位姿更新模块、标度与地图构建模块、优化求解器、位姿解算模块；其中,所述位姿预测模块，用于根据传感器输出的增量编码器信息和角速度信息来解算车辆运行过程中任意时刻的位置和姿态信息；解算时利用已建立的轮式里程计/陀螺仪在离散时间下的联合运动学模型进行解算；
所述状态变量关联模块，用于根据内部传感器观测下和相机观测下的误差，构建标度因子、陀螺仪偏置与状态变量最优化问题的数学模型；
所述预测位姿更新模块，用于根据标度误差以及陀螺仪角速度偏置引起的姿态偏置对位姿预测模块解算的位置和姿态进行更新；
所述标度与地图构建模块，用于构建地图位置与标度因子的对应关系；
所述优化求解器，用于根据观测信息、历史状态变量、误差函数进行求解关键参数，并将获得的关键参数发送给位姿解算模块；
所述位姿解算模块，视觉定位正常时，根据优化求解器发送的参数推导出车辆在地图中的位姿；视觉定位丢失后，根据优化求解器中最新的标度因子和该位置的历史标度因子求解标度因子比率，根据标度因子比率以及历史标度因子更新标度因子，利用更新后的标度因子推导出车辆在地图中的位姿。
CN202110780435.2A 2021-07-09 2021-07-09 一种基于非全局一致地图的多传感器组合定位方法及系统 Active CN113483755B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110780435.2A CN113483755B (zh) 2021-07-09 2021-07-09 一种基于非全局一致地图的多传感器组合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110780435.2A CN113483755B (zh) 2021-07-09 2021-07-09 一种基于非全局一致地图的多传感器组合定位方法及系统

Publications (2)

Publication Number Publication Date
CN113483755A true CN113483755A (zh) 2021-10-08
CN113483755B CN113483755B (zh) 2023-11-07

Family

ID=77938382

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110780435.2A Active CN113483755B (zh) 2021-07-09 2021-07-09 一种基于非全局一致地图的多传感器组合定位方法及系统

Country Status (1)

Country Link
CN (1) CN113483755B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114199281A (zh) * 2021-12-13 2022-03-18 哈尔滨工业大学芜湖机器人产业技术研究院 基于速度优化的多传感器联合标定方法及系统
WO2023142353A1 (zh) * 2022-01-26 2023-08-03 奥比中光科技集团股份有限公司 一种位姿预测方法及装置

Citations (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3056926A1 (en) * 2015-02-12 2016-08-17 Novatel, Inc. Navigation system with rapid gnss and inertial initialization
US20170199040A1 (en) * 2016-01-07 2017-07-13 Caterpillar Inc. Positioning system having smoothed kalman filter update
US20180045519A1 (en) * 2016-08-09 2018-02-15 Nauto, Inc. System and method for precision localization and mapping
US20180051990A1 (en) * 2016-08-18 2018-02-22 Toyota Jidosha Kabushiki Kaisha Position estimation system, position estimation method and mobile unit
CN109269497A (zh) * 2018-07-31 2019-01-25 哈尔滨工程大学 基于auv切法向速度模型的多尺度无迹卡尔曼滤波估计方法
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN111156987A (zh) * 2019-12-18 2020-05-15 东南大学 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人
CN111707261A (zh) * 2020-04-10 2020-09-25 南京非空航空科技有限公司 一种微型无人机高速感知和定位方法
CN111862673A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN111986506A (zh) * 2020-07-20 2020-11-24 苏州易航远智智能科技有限公司 基于多视觉系统的机械式停车位泊车方法
CN112050809A (zh) * 2020-10-08 2020-12-08 吉林大学 轮式里程计与陀螺仪信息融合的无人车定向定位方法
CN112066983A (zh) * 2020-09-08 2020-12-11 中国人民解放军国防科技大学 惯性/里程计组合导航滤波方法、电子设备及存储介质
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置

Patent Citations (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3056926A1 (en) * 2015-02-12 2016-08-17 Novatel, Inc. Navigation system with rapid gnss and inertial initialization
US20170199040A1 (en) * 2016-01-07 2017-07-13 Caterpillar Inc. Positioning system having smoothed kalman filter update
US20180045519A1 (en) * 2016-08-09 2018-02-15 Nauto, Inc. System and method for precision localization and mapping
US20180051990A1 (en) * 2016-08-18 2018-02-22 Toyota Jidosha Kabushiki Kaisha Position estimation system, position estimation method and mobile unit
CN109269497A (zh) * 2018-07-31 2019-01-25 哈尔滨工程大学 基于auv切法向速度模型的多尺度无迹卡尔曼滤波估计方法
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN111156987A (zh) * 2019-12-18 2020-05-15 东南大学 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人
CN111707261A (zh) * 2020-04-10 2020-09-25 南京非空航空科技有限公司 一种微型无人机高速感知和定位方法
CN111862673A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN111986506A (zh) * 2020-07-20 2020-11-24 苏州易航远智智能科技有限公司 基于多视觉系统的机械式停车位泊车方法
CN112066983A (zh) * 2020-09-08 2020-12-11 中国人民解放军国防科技大学 惯性/里程计组合导航滤波方法、电子设备及存储介质
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN112050809A (zh) * 2020-10-08 2020-12-08 吉林大学 轮式里程计与陀螺仪信息融合的无人车定向定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
刘志励: "基于双目视觉与激光雷达的局部路径规划算法研究", 中国优秀硕士学位论文全文数据库 *
张琳;曾成;王羿帆;: "一种多时间尺度融合的视觉辅助惯性定姿算法", 现代电子技术, no. 12 *
潘世光;尚建华;罗远;马鸿斌;贺岩;: "一种小型化、远距离、平面扫描式激光雷达避障系统", 信息技术与网络安全, no. 05 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114199281A (zh) * 2021-12-13 2022-03-18 哈尔滨工业大学芜湖机器人产业技术研究院 基于速度优化的多传感器联合标定方法及系统
WO2023142353A1 (zh) * 2022-01-26 2023-08-03 奥比中光科技集团股份有限公司 一种位姿预测方法及装置

Also Published As

Publication number Publication date
CN113483755B (zh) 2023-11-07

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN113483755A (zh) 一种基于非全局一致地图的多传感器组合定位方法及系统
Heo et al. EKF-based visual inertial navigation using sliding window nonlinear optimization
US9482536B2 (en) Pose estimation
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN112083726A (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN111795692B (zh) 经由多模式slam融合过程进行并行跟踪和定位的方法和设备
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN113436261B (zh) 一种面向封闭园区自动驾驶的单目视觉惯性定位方法
CN112099378B (zh) 考虑随机测量时滞的前车侧向运动状态实时估计方法
CN112697138A (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN113503873B (zh) 一种多传感器融合的视觉定位方法
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN115046545A (zh) 一种深度网络与滤波结合的定位方法
Lamon et al. 3D position tracking in challenging terrain
CN115577320A (zh) 一种基于数据插值的多传感器异步数据融合方法
CN115388899A (zh) 基于变分贝叶斯的移动机器人视觉惯性融合slam方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN113091754A (zh) 一种非合作航天器位姿一体化估计和惯性参数确定方法
Zhang et al. Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN115077517A (zh) 一种基于立体相机与imu融合的低速无人车定位方法及系统

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
GR01 Patent grant
GR01 Patent grant