CN114184200B - 一种结合动态建图的多源融合导航方法 - Google Patents

一种结合动态建图的多源融合导航方法 Download PDF

Info

Publication number
CN114184200B
CN114184200B CN202210133071.3A CN202210133071A CN114184200B CN 114184200 B CN114184200 B CN 114184200B CN 202210133071 A CN202210133071 A CN 202210133071A CN 114184200 B CN114184200 B CN 114184200B
Authority
CN
China
Prior art keywords
camera
map
image
guide line
point
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.)
Active
Application number
CN202210133071.3A
Other languages
English (en)
Other versions
CN114184200A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210133071.3A priority Critical patent/CN114184200B/zh
Publication of CN114184200A publication Critical patent/CN114184200A/zh
Application granted granted Critical
Publication of CN114184200B publication Critical patent/CN114184200B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种结合动态建图的多源融合导航方法，包括以下步骤：步骤1，构建高精度3D点云地图和引导线地图；步骤2，对步骤1中所构建的3D点云地图进行辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算；步骤3，采用隐马尔可夫链模型将定位轨迹匹配到步骤1中所构建的引导线地图上；步骤4，用匹配后的位姿修正3D点云地图和引导线地图。本发明提出的方法消除了组合导航系统中VO算法的不稳定性以及视觉定位中未充分挖掘图像中有用信息辅助定位的问题；有效提高了组合导航系统速度的可靠性；充分挖掘了图像中有用信息辅助定位，实现对构建的地图进行实时更新，并有效的用于载体的场景感知与定位。

Description

一种结合动态建图的多源融合导航方法
技术领域
本发明涉及一种多源融合导航方法，特别是一种结合动态建图的多源融合导航方法。
背景技术
随着智能交通系统的发展，高性能车载导航定位技术需求不断增加。传统的GNSS/IMU组合导航是目前使用最为广泛的导航定位技术，在一定程度上解决了惯性测量元件（Inertial Measurement Unit，IMU）误差累积和全球导航卫星系统（Global NavigationSatellite System，GNSS）信号中断的问题。但是在城市峡谷环境下，随着GNSS信号中断时间变长，GNSS/IMU组合导航性能不断下降，不能满足精准定位的需求。视觉传感器具有体积小、功耗低、价格低廉，能够实现自主定位，地图构建，目标识别等优势，同时移动平台行驶受到道路等路面信息的约束，因此将视觉传感器和地图匹配来提高GNSS/IMU组合导航系统精度成为近年来研究的热点。
Chen和Chiang提出了一种用双目视觉传感器协助低精度惯性导航系统（InertialNavigation System，INS）/全球定位系统（Global Positioning System，GPS）组合系统的算法，利用视觉里程计（Visual Odometer，VO）的位置、速度信息辅助INS/GPS组合定位，虽然组合导航系统能够在一定程度上提高GPS信号中断时的定位精度，但是该系统的VO算法存在不稳定性，影响了系统最终定位的可靠性。Sun提出了一种使用非完整约束的基于两次扩展卡尔曼滤波的GNSS/IMU/VO组合导航方案，实验结果表明，在城市复杂环境下可以得到3.285m的三维位置误差。Liao、Li等人开发了一种基于扩展卡尔曼滤波的GNSS/INS/Vision紧耦合组合方案，提出了一种结合旋转二进制鲁棒独立基本特征(Binary RobustIndenpendent Elementary Features，BRIEF)描述子和稀疏光流跟踪法(Kanade-Lucas-Tomasi Tracking Method，KLT)的特征跟踪算法（参考：Liao J, Li X, Wang X, et al.Enhancing navigation performance through visual-inertial odometry in GNSS-degraded environment[J]. GPS Solutions, 2021, 25(2): 1-18.），实验结果证明在城市峡谷中，所提算法的导航性能相比于GNSS/INS松耦合组合导航有极大的提高，由于缺少地图辅助，当存在特征不匹配和复杂驾驶环境下的动态目标时，导航性能会变得比GNSS/INS松耦合组合导航更糟糕的情况。
Tao和Bonnifait等人提出了一种利用摄像机的车道检测功能检索道路标记的横向和方向信息进行自主实时定位的算法（参考：Tao Z, Bonnifait P, Fremont V, et al.Mapping and localization using GPS, lane markings and proprioceptive sensors[C]//2013 IEEE/RSJ International Conference on Intelligent Robots andSystems. IEEE, 2013: 406-412.），但是该方法需要移动平台在定位前通过高精度定位装置进行移动测绘。Nedevschi等人提出了一种基于贝叶斯网络的结合视觉和数字地图信息来识别当前车道的定位方案（参考：Nedevschi S, Popescu V, Danescu R, et al.Accurate ego-vehicle global localization at intersections through alignmentof visual data with digital map[J]. IEEE transactions on intelligenttransportation systems, 2012, 14(2): 673-687.），实现了亚米级全局定位精度；Wu提出了一种利用视觉识别道路上各种交通标志（箭头、人行横道和限速牌等）进行车道级定位的方法（参考：Wu T, Ranganathan A. Vehicle localization using road markings[C]//2013 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2013: 1185-1190.）；但是这两种车道级定位方案都依赖先验高精度3D数字地图信息。Zhu和Shen等人提出了一种基于视觉匹配路标点和车道线约束的GNSS/INS/Vision紧耦合组合导航算法（参考：ZhuF, Shen Y, Wang Y, et al. Fusing GNSS/INS/Vision with a Priori Feature Mapfor High-precision and Continuous Navigation[J]. IEEE Sensors Journal, 2021,21(20): 23370-23381.），在一定程度上可以提高定位精度。Liu等人提出了一种与GNSS原始观测量紧耦合的基于优化的视觉/惯性即时定位与建图（Simultaneous Localizationand Mapping，SLAM）组合导航方案（参考：Liu J, Gao W, Hu Z. Optimization-basedvisual-inertial SLAM tightly coupled with raw GNSS measurements[C]//2021 IEEEInternational Conference on Robotics and Automation (ICRA). IEEE, 2021:11612-11618.）,将GNSS测量误差、重投影误差和IMU预积分误差进行联合图优化获得最终导航定位结果，但该算法在建图时并未充分利用图像中的有用信息，定位精度有望进一步提高。
现有的GNSS/IMU/Vision组合导航方案中，视觉测速存在不稳定性，当存在特征不匹配或复杂环境下的动态目标时，在缺少地图辅助的情况下，相机定位精度及视觉测速精度急剧下降，因此无法保证组合导航定位系统的可靠性；
现有的基于视觉匹配的GNSS/IMU/Vision组合导航方案中通常使用静态3D电子地图，而3D高精度数字电子地图存在更新滞后、精度不变的问题，虽然有些算法利用建立的3D点云地图协助定位，但是并未充分挖掘视觉图像中的有用信息，不能实现载体有效的场景感知，最终无法进一步提高组合导航定位精度。
发明内容
发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种结合动态建图的多源融合导航方法。
为了解决上述技术问题，本发明公开了一种结合动态建图的多源融合导航方法，包括以下步骤：
步骤1，构建高精度3D点云地图和引导线地图；
步骤2，对步骤1中所构建的3D点云地图进行辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算；
步骤3，采用隐马尔可夫链模型将定位轨迹匹配到步骤1中所构建的引导线地图上；
步骤4，用匹配后的位姿修正3D点云地图和引导线地图。
本发明步骤1中，利用高精度全球导航卫星系统及惯性测量元件组合导航设备和相机构建高精度3D点云地图和引导线地图，包括以下步骤：
步骤1-1，构建3D点云地图；
步骤1-2，构建引导线地图；
步骤1-3，建立字典；
其中，步骤1-1中所述构建3D点云地图的方法，包括以下步骤：
步骤1-1-1，对相机拍摄的图像进行特征提取和特征匹配，使用尺度不变特征变换的特征来描述环境特征；
步骤1-1-2，利用相机内置矩阵
Figure 87379DEST_PATH_IMAGE001
将图像中匹配的特征点u转换到相机坐标系，内置矩阵
Figure 434440DEST_PATH_IMAGE001
通过相机标定获得，其值为：
Figure 382804DEST_PATH_IMAGE002
其中，
Figure 22864DEST_PATH_IMAGE003
和
Figure 791100DEST_PATH_IMAGE004
为相机焦距分量，
Figure 938922DEST_PATH_IMAGE005
和
Figure 335268DEST_PATH_IMAGE006
为相机投影中心在图像坐标系中的坐标；
步骤1-1-3，利用高精度全球导航卫星系统及惯性测量元件组合导航设备解算出相机的位姿信息
Figure 146230DEST_PATH_IMAGE007
，并根据高精度相机位姿将匹配的特征点从相机坐标系转换到世界坐标系构建3D点云地图；
构建得到的3D点云地图中匹配的特征点在世界坐标系中的坐标G为：
Figure 401762DEST_PATH_IMAGE008
其中，
Figure 713794DEST_PATH_IMAGE009
为特征点 u的深度信息，
Figure 105592DEST_PATH_IMAGE010
为特征点u的齐次坐标。
本发明步骤1-2中所述构建引导线地图包括以下步骤：
步骤1-2-1，图像预处理：
在图像中设置引导线所在的区域并进行截取；使用高斯滤波器对截取的图像进行平滑；使用仿射变换将图像转换成鸟瞰图；通过设置颜色阈值，对图形进行二值化处理，得到二值化图像；
步骤1-2-2，引导线检测：
对上述二值化图像进行高斯直方图统计，将两个极大峰值作为左右引导线的搜索起点；设置滑动窗口的大小，将行驶引导道离散为多个窗口，利用高斯直方图统计窗口，将高斯分布曲线的最大峰值所在点作为窗口的每个中心；寻找所有滑动窗口的中心位置，将这些中心点通过直线拟合或曲线拟合得到当前引导线曲线参数；
步骤1-2-3，引导线跟踪：
引导线在前后两帧图像中的位置相互关联，根据前一帧中引导线的位置预测当前帧中引导线的坐标；采用速度模型建立上一帧引导线位置与当前帧中引导线的位置关系：
Figure 323341DEST_PATH_IMAGE011
其中，
Figure 190802DEST_PATH_IMAGE012
和
Figure 650734DEST_PATH_IMAGE013
分别是上一帧和当前帧拟合的引导线的四个端点的坐标，
Figure 162618DEST_PATH_IMAGE014
和
Figure 174436DEST_PATH_IMAGE015
分别是上一帧和当前帧四个端点的速度，
Figure 404560DEST_PATH_IMAGE016
和
Figure 166718DEST_PATH_IMAGE017
分别代表像素点位置系统噪声和速度系统噪声；
观测方程为：
Figure 392163DEST_PATH_IMAGE018
其中 ，
Figure 715828DEST_PATH_IMAGE019
为观测噪声，
Figure 292302DEST_PATH_IMAGE020
为当前帧中观测的四个端点的坐标；
获得第一帧引导线位置后，根据当前帧的观测值利用卡尔曼滤波对后续帧进行连续跟踪；
步骤1-2-4，建立引导线地图：
选择4个路标点作为控制点求解单应矩阵
Figure 94036DEST_PATH_IMAGE021
：
Figure 580512DEST_PATH_IMAGE022
其中，
Figure 934133DEST_PATH_IMAGE023
为控制点在图像平面中的像素坐标，
Figure 374735DEST_PATH_IMAGE024
为控制点在相机坐标系中的坐标，
Figure 104794DEST_PATH_IMAGE025
为比例因子；
得到单应矩阵后，将检测的引导线像素坐标通过单应矩阵变换到相机坐标系，再利用高精度全球导航卫星系统及惯性测量元件求解的相机位姿，将引导线点从相机坐标系转换到世界坐标系，最终完成引导线地图的构建。
本发明步骤1-3中所述建立字典，包括以下步骤：
使用词袋模型计算图像间的相似性，采用k叉树结构建立字典，对每一帧图像均用单词描述，实现地图数据库中快速最近邻搜索构建一个深度d=7、每次分叉数k=10的树：
步骤1-3-1，采用k-means算法把所有单词分为k类，得到字典的第一层；
步骤1-3-2，对第一层的每个节点，把属于该节点的样本用k-means算法聚类成k类，得到下一层；
步骤1-3-3，对每一层采用上述方法，直到得到最后的叶子层；
经过以上步骤，建立一个可以容纳
Figure 180197DEST_PATH_IMAGE026
个单词的字典，字典中单词的集合表示为：
Figure 845665DEST_PATH_IMAGE027
其中，特征点个数
Figure 131153DEST_PATH_IMAGE028
,
Figure 540269DEST_PATH_IMAGE029
为建立的单词，其中自然数
Figure 594812DEST_PATH_IMAGE030
；
在完成字典的构建后，对于某一帧图像A，将图像中提取的特征点用单词表中的单词近似替代，根据图像中单词出现的频率构建Bow向量
Figure 195296DEST_PATH_IMAGE031
，即：
Figure 843446DEST_PATH_IMAGE032
其中，
Figure 915307DEST_PATH_IMAGE033
为单词
Figure 965303DEST_PATH_IMAGE029
在图像A中的权重。
本发明步骤2中所述3D点云地图辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算方法，包括以下步骤：
步骤2-1，3D点云地图辅助求解相机速度；
步骤2-2，惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束；
步骤2-3，全球导航卫星系统或惯性测量元件或视觉松组合导航解算。
本发明步骤2-1中所述3D点云地图辅助求解相机速度方法，包括：
在当前帧图像经尺度不变特征变换的特征提取和构建Bow向量后，计算图像A和图像B的Bow向量即
Figure 831627DEST_PATH_IMAGE031
和
Figure 232653DEST_PATH_IMAGE034
的匹配相似度
Figure 231176DEST_PATH_IMAGE035
：
Figure 525891DEST_PATH_IMAGE036
其中，
Figure 704062DEST_PATH_IMAGE037
和
Figure 326805DEST_PATH_IMAGE038
分别为单词
Figure 740469DEST_PATH_IMAGE029
在图像A和B中的权重
Figure 765056DEST_PATH_IMAGE033
；
解算得到的最大相似度即为匹配到的地图数据库中的图片，找到匹配图片后，则获得图像像素坐标系中坐标和与之对应的路标点在世界坐标系中的坐标，接着利用PnP算法求解相机位姿，设相机的位姿为
Figure 238763DEST_PATH_IMAGE039
和
Figure 581757DEST_PATH_IMAGE040
，位姿的李代群用
Figure 533533DEST_PATH_IMAGE041
表示，即
Figure 678206DEST_PATH_IMAGE042
，
Figure 198181DEST_PATH_IMAGE043
为
Figure 920149DEST_PATH_IMAGE044
的全零矩阵的转置，像素坐标的归一化平面齐次坐标为
Figure 550982DEST_PATH_IMAGE045
，其深度为
Figure 143637DEST_PATH_IMAGE046
，其对应的路标点在世界坐标系中的齐次坐标为
Figure 601556DEST_PATH_IMAGE047
，则两者之间的关系为：
Figure 686187DEST_PATH_IMAGE048
写成矩阵形式为：
Figure 386290DEST_PATH_IMAGE049
上式中，相机位姿未知并且存在观测噪声，存在重投影误差，PnP算法通过将误差求和，寻找使误差最小的相机位姿，即
Figure 99031DEST_PATH_IMAGE050
在解算出相机的位移
Figure 960808DEST_PATH_IMAGE040
后，通过对时间差分求解相机速度，即：
Figure 391789DEST_PATH_IMAGE051
其中，
Figure 128539DEST_PATH_IMAGE040
为移动平台在相机坐标系中的位移，
Figure 836732DEST_PATH_IMAGE052
为时间间隔，
Figure 994043DEST_PATH_IMAGE053
为由相机坐标系到载体坐标系的转换矩阵，
Figure 53266DEST_PATH_IMAGE054
为由相机解算的移动平台在相机坐标系中的速度，
Figure 829592DEST_PATH_IMAGE055
为由相机解算的移动平台在载体坐标系中的速度。
本发明步骤2-2中所述惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束，方法如下：
利用惯性测量元件输出的纵向和横向角速度判断移动平台是否发生换道和转弯，当惯性测量元件输出的沿载体横向的角速度
Figure 159336DEST_PATH_IMAGE056
时，判定移动平台横向速度为0，沿载体纵向的角速度
Figure 487549DEST_PATH_IMAGE057
时，判定移动平台纵向速度为0，即移动平台保持直行，此时，对相机求解的速度增加非完整约束：
Figure 768489DEST_PATH_IMAGE058
其中，
Figure 473140DEST_PATH_IMAGE059
为移动平台沿x轴即沿载体横向向右的速度，
Figure 890346DEST_PATH_IMAGE060
为移动平台沿y轴即沿载体前向的速度，
Figure 530406DEST_PATH_IMAGE061
为移动平台沿z轴即沿载体纵向向上的速度。
本发明步骤2-3中所述全球导航卫星系统或惯性测量元件或视觉松组合导航解算，方法如下：
选取三维位置误差
Figure 157696DEST_PATH_IMAGE062
、三维速度误差
Figure 39939DEST_PATH_IMAGE063
、三维姿态角误差
Figure 436285DEST_PATH_IMAGE064
、加速度计偏差
Figure 247247DEST_PATH_IMAGE066
和比例因子
Figure 237199DEST_PATH_IMAGE068
、陀螺仪偏差
Figure 283653DEST_PATH_IMAGE070
和比例因子
Figure 941030DEST_PATH_IMAGE072
为状态量，即：
Figure 158778DEST_PATH_IMAGE073
根据惯性导航的力学编排，构建基于扩展卡尔曼滤波的全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程，即：
Figure 26240DEST_PATH_IMAGE074
其中
Figure 486171DEST_PATH_IMAGE075
是状态转移矩阵，
Figure 998055DEST_PATH_IMAGE076
是系统噪声；
在全球导航卫星系统接收到星历文件和观测文件后，根据伪距单点定位和多普勒测速解算移动平台的三维位置和速度，得到全球导航卫星系统的观测方程：
Figure 275453DEST_PATH_IMAGE077
其中，
Figure 974419DEST_PATH_IMAGE078
和
Figure 2155DEST_PATH_IMAGE079
分别为全球导航卫星系统解算的移动平台位置和速度，
Figure 227600DEST_PATH_IMAGE080
和
Figure 285686DEST_PATH_IMAGE081
分别为惯导系统解算的移动平台位置和速度，
Figure 127740DEST_PATH_IMAGE082
，
Figure 195053DEST_PATH_IMAGE083
为
Figure 150371DEST_PATH_IMAGE084
的单位矩阵，
Figure 35150DEST_PATH_IMAGE085
和
Figure 198454DEST_PATH_IMAGE086
分别为
Figure 69458DEST_PATH_IMAGE084
和
Figure 269496DEST_PATH_IMAGE087
的全零矩阵，
Figure 934963DEST_PATH_IMAGE088
为全球导航卫星系统观测误差；
经非完整约束的相机测速观测方程为：
Figure 361397DEST_PATH_IMAGE089
其中，
Figure 895146DEST_PATH_IMAGE090
、
Figure 323591DEST_PATH_IMAGE091
和
Figure 425539DEST_PATH_IMAGE092
为惯导系统解算的移动平台速度分别在x轴、y轴和z轴的分量，
Figure 198323DEST_PATH_IMAGE093
，
Figure 145551DEST_PATH_IMAGE094
为相机测速误差；
得到全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程和观测方程后，利用卡尔曼滤波解算车辆的导航参数。
本发明步骤3中所述用隐马尔可夫链模型将定位轨迹匹配到引导线地图上，方法如下：
采用隐马尔可夫链模型实现地图匹配的步骤如下：
步骤3-1，在引导线地图中寻找组合导航解算的移动平台位置在设定半径范围内的所有候选路段；
步骤3-2，针对每个候选路段计算观测概率和状态转移概率：
观测概率：
设组合导航定位点与候选点之间的距离误差服从零均值高斯分布，观测概率为：
Figure 320180DEST_PATH_IMAGE095
其中，
Figure 327450DEST_PATH_IMAGE096
为松组合系统解算的移动平台位置，
Figure 964361DEST_PATH_IMAGE097
为t时刻第i条候选路段，假设
Figure 839914DEST_PATH_IMAGE098
为第i条候选路段的候选点，
Figure 744416DEST_PATH_IMAGE099
为观测点
Figure 781642DEST_PATH_IMAGE096
与候选路段
Figure 404384DEST_PATH_IMAGE097
之间的距离，
Figure 958993DEST_PATH_IMAGE100
为当 t时刻的移动平台真实位置在引导道中的位置
Figure 108215DEST_PATH_IMAGE098
时观测到
Figure 955823DEST_PATH_IMAGE101
的概率，
Figure 924916DEST_PATH_IMAGE102
是组合导航解算位置的标准差，e为自然对数的底数， π是圆周率；
状态转移概率：
状态转移概率为组合导航解算的移动平台位置在引导线网络中从t时刻的一条候选路段
Figure 17637DEST_PATH_IMAGE103
移动到
Figure 896731DEST_PATH_IMAGE104
时刻的一条候选路段
Figure 541339DEST_PATH_IMAGE105
的概率；当前时刻组合导航定位点与下一时刻组合导航定位点的候选点之间的距离越小时，选择这条道路的可能性越大，反之则越小；构建转移概率：
Figure 873095DEST_PATH_IMAGE106
其中，
Figure 628561DEST_PATH_IMAGE107
为状态转移概率，
Figure 129206DEST_PATH_IMAGE108
为当前组合导航定位点与下一时刻组合导航定位点的候选点之间的距离，
Figure 820081DEST_PATH_IMAGE109
为当前组合导航定位点与下一个组合导航定位点的候选点之间的最大距离；
步骤3-3，利用维特比算法求解最有可能的隐藏序列即最优路径，最终实现将定位轨迹匹配到引导线地图上。
本发明步骤4中所述用匹配后的位姿修正3D点云地图和引导线地图，方法如下：
在完成步骤3中所述定位轨迹与引导线地图匹配后，根据解算的移动平台高精度位姿，按照步骤1所述方法进一步修正3D点云地图和引导线地图，完成所述地图实时更新。
有益效果：
本发明提出的方法消除了组合导航中VO算法的不稳定性以及视觉定位中未充分挖掘图像中有用信息辅助定位的问题；
(1)针对GNSS/IMU/Vision组合导航系统中视觉获取速度存在不稳定性，利用构建的高精度3D点云地图辅助相机位姿估计，同时利用IMU输出的纵向和横向角速度判断移动平台是否发生换道和转弯，当移动平台保持直行时，对相机位置差分求解的移动平台速度增加NHC约束，使用经NHC约束的相机速度与GNSS/IMU系统进行组合导航求解移动平台的位姿，有效提高了组合导航系统速度的可靠性;
(2)针对现有的基于匹配的GNSS/IMU/Vision组合导航方案大多依赖高精度3D电子地图，而3D电子地图更新不及时和定位精度有限的问题以及融合多传感器的SLAM组合导航方案只对3D特征点进行建图，并未充分挖掘视觉图像中的其他有用信息辅助定位的问题，在利用高精度GNSS/IMU设备和视觉设备构建高精度3D点云地图的同时，挖掘图像中的引导线信息构建引导线地图，将GNSS/IMU/Vision组合导航解算地存在着一定误差的定位轨迹通过隐马尔可夫链模型精确匹配到引导线地图上，从而获得更准确的车载位置信息和所在路段信息, 充分挖掘了图像中有用信息辅助定位，实现对构建的地图进行实时更新，并有效的用于载体的场景感知与定位。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
图1为本发明一种结合动态建图的多源融合导航方法流程图。
具体实施方式
本发明的核心内容为一种结合动态建图的多源融合导航方法，包括如下4个阶段：（1）构建高精度3D点云地图和引导线地图；（2）点云地图辅助的GNSS/IMU/Vision组合导航解算；（3）隐马尔可夫链模型将定位轨迹匹配到引导线地图上；（4）用匹配后的位姿修正3D点云地图和引导线地图。其总体流程如图1所示：
（1）构建高精度3D点云地图和引导线地图
首先，利用高精度GNSS/IMU组合导航设备和相机构建高精度3D点云地图和引导线地图。具体步骤如下：
1）构建3D点云地图
第一步：对相机拍摄的图像进行特征提取和特征匹配，由于尺度不变特征变换（Scale Invariant Feature Transform，SIFT）特征具有尺度不变性、旋转不变性，有着较好的稳定性、可靠性和准确性，在本发明中，使用SIFT特征来描述环境特征；
第二步：利用相机内置矩阵
Figure 29346DEST_PATH_IMAGE110
将图像中匹配的特征点
Figure 198290DEST_PATH_IMAGE111
转换到相机坐标系，内置矩阵
Figure 51977DEST_PATH_IMAGE110
可通过相机标定获得，其值为：
Figure 38387DEST_PATH_IMAGE112
其中，
Figure 108849DEST_PATH_IMAGE113
和
Figure 206118DEST_PATH_IMAGE114
分别为相机焦距分量，
Figure 914311DEST_PATH_IMAGE115
和
Figure 946989DEST_PATH_IMAGE116
分别为相机投影中心在图像坐标系中的坐标。
第三步：利用高精度GNSS/IMU组合导航设备解算出相机的位姿信息
Figure 130846DEST_PATH_IMAGE117
，并根据高精度相机位姿将匹配特征点从相机坐标系转到世界坐标系，从而构建3D点云地图。
构建的3D点云地图中匹配特征点在世界坐标系中的坐标
Figure 641593DEST_PATH_IMAGE118
为：
Figure 328926DEST_PATH_IMAGE119
其中，
Figure 299549DEST_PATH_IMAGE120
为特征点
Figure 314910DEST_PATH_IMAGE111
的深度信息，
Figure 285140DEST_PATH_IMAGE121
为特征点
Figure 702346DEST_PATH_IMAGE111
的齐次坐标。
2）构建引导线地图
第一步：图像预处理。在图像中设置感兴趣区域即引导线所在的区域并进行截取；使用高斯滤波器对截取的图像进行平滑；使用仿射变换将图像转换成鸟瞰图；通过设置颜色阈值，对图形进行二值化处理。
第二步：引导线检测。对二值化图像进行高斯直方图统计，将两个极大峰值作为左右引导线的搜索起点；设置滑动窗口的大小，将车道离散为多个窗口，利用高斯直方图统计窗口，将高斯分布曲线的最大峰值所在点作为窗口的每个中心；寻找所有滑动窗口的中心位置，将这些中心点通过直线拟合或曲线拟合得到当前引导线曲线参数。
第三步：引导线跟踪。引导线在前后两帧图像中的位置是相互关联的，可以根据前一帧中引导线的位置预测当前帧中引导线的坐标。采用速度模型建立上一帧引导线位置与当前帧中引导线的位置关系：
Figure 342406DEST_PATH_IMAGE122
其中，
Figure 969696DEST_PATH_IMAGE123
和
Figure 586360DEST_PATH_IMAGE124
分别是上一帧和当前帧拟合的引导线的四个端点的坐标，
Figure 248286DEST_PATH_IMAGE125
和
Figure 793668DEST_PATH_IMAGE126
分别是上一帧和当前帧四个端点的速度
Figure 49200DEST_PATH_IMAGE127
和
Figure 830074DEST_PATH_IMAGE128
分别代表像素点位置系统噪声和速度系统噪声；
观测方程为：
Figure 487451DEST_PATH_IMAGE129
其中
Figure 982497DEST_PATH_IMAGE130
为观测噪声，
Figure 584380DEST_PATH_IMAGE131
为当前帧中观测的四个端点的坐标。
在获得第一帧引导线的位置后，根据当前帧的观测值即可利用卡尔曼滤波对后续帧进行连续跟踪。
第四步：建立引导线地图。选择4个路标点做为控制点求解单应矩阵
Figure 309891DEST_PATH_IMAGE132
：
Figure 680829DEST_PATH_IMAGE133
其中，
Figure 568014DEST_PATH_IMAGE134
为在控制点在图像平面中的像素坐标，
Figure 532559DEST_PATH_IMAGE135
为控制点在相机坐标系中的坐标，
Figure 920815DEST_PATH_IMAGE136
为比例因子。
在得到单应矩阵后，可以将检测的引导线像素坐标通过单应矩阵变换到相机坐标系，再利用高精度GNSS/IMU求解的相机位姿，将引导线点从相机坐标系转换到世界坐标系，最终完成引导线地图的构建。
3）建立字典
在巨大的地图数据库中实现快速最近邻搜索是极其困难的，为了提高匹配效率，使用词袋模型计算图像间的相似性，采用k叉树结构建立字典，对每一帧图像都用单词来描述。构建一个深度d=7、每次分叉数k=10的树：
第一步：采用k-means算法（参考：杨淑莹,郑清春.模式识别与智能计算—MATLAB技术实现[M].第4版.电子工业出版社, 2019.11 :202-206.）把所有单词分为k个类，得到字典的第一层；
第二步：对第一层的每个节点，把属于该节点的样本再次用k-means算法聚类成k类，得到下一层；
第三步：依次类推，直到得到最后的叶子层。
经过以上步骤，可以建立一个可以容纳
Figure 785740DEST_PATH_IMAGE137
个单词的字典，字典中单词的集合表示为：
Figure 968460DEST_PATH_IMAGE138
其中，特征点个数
Figure 420301DEST_PATH_IMAGE139
,
Figure 487614DEST_PATH_IMAGE140
为建立的单词，其中自然数
Figure 567565DEST_PATH_IMAGE141
。
在完成字典的构建后，对于某一帧图像A，将图像中提取的特征点用单词表中的单词近似替代，根据图像中单词出现的频率构建Bow向量
Figure 62132DEST_PATH_IMAGE142
，即：
Figure 125903DEST_PATH_IMAGE143
其中，
Figure 967213DEST_PATH_IMAGE144
为单词
Figure 308196DEST_PATH_IMAGE140
在图像A中的权重。
（2）点云地图辅助的GNSS/IMU/Vision组合导航解算
第一步：3D点云地图辅助求解相机速度。在当前帧图像经尺度不变特征变换的特征提取和构建Bow向量后，计算图像A和图像B的Bow向量即
Figure 363876DEST_PATH_IMAGE142
和
Figure 259151DEST_PATH_IMAGE145
的匹配相似度
Figure 933846DEST_PATH_IMAGE146
：
Figure 253969DEST_PATH_IMAGE147
其中，
Figure 323294DEST_PATH_IMAGE148
和
Figure 237023DEST_PATH_IMAGE149
分别为单词
Figure 308885DEST_PATH_IMAGE140
在图像A和B中的权重
Figure 358880DEST_PATH_IMAGE150
；
解算的最大相似度即为匹配到的地图数据库中的图片，找到匹配图片后，则可获得图像像素坐标系中坐标和与之对应的路标点在世界坐标系中的坐标，接着利用PnP算法（参考：高翔,张涛.视觉SLAM十四讲[M].第2版.电子工业出版社, 2017.3 :180-184.）求解相机位姿，PnP算法是通过迭代求出使重投影误差即像素坐标与3D点坐标按照当前估计的位姿进行投影得到位姿相比较得到的误差最小的相机外参的方法。假设相机的位姿为
Figure 631730DEST_PATH_IMAGE151
和
Figure 626231DEST_PATH_IMAGE152
，位姿的李代群用
Figure 144193DEST_PATH_IMAGE153
表示，即
Figure 173329DEST_PATH_IMAGE154
，
Figure 351500DEST_PATH_IMAGE155
为
Figure 239822DEST_PATH_IMAGE156
的全零矩阵的转置，像素坐标的归一化平面齐次坐标为
Figure 653486DEST_PATH_IMAGE157
，像素点的深度为
Figure 678074DEST_PATH_IMAGE158
，其对应的路标点在世界坐标系中的齐次坐标为
Figure 525682DEST_PATH_IMAGE159
，则两者之间的关系为：
Figure 494775DEST_PATH_IMAGE160
写成矩阵形式为：
Figure 587496DEST_PATH_IMAGE161
上式中，相机位姿未知并且存在观测噪声，存在重投影误差，PnP算法通过将误差求和，寻找使误差最小的相机位姿，即
Figure 466590DEST_PATH_IMAGE162
在解算出相机的位移
Figure 111198DEST_PATH_IMAGE152
后，通过对时间差分求解相机速度，即：
Figure 442953DEST_PATH_IMAGE163
其中，
Figure 463999DEST_PATH_IMAGE152
为移动平台在相机坐标系中的位移，
Figure 964644DEST_PATH_IMAGE164
为时间间隔，
Figure 655519DEST_PATH_IMAGE165
为由相机坐标系到载体坐标系的转换矩阵，
Figure 599204DEST_PATH_IMAGE166
为由相机解算的移动平台在相机坐标系中的速度，
Figure 33728DEST_PATH_IMAGE167
为由相机解算的移动平台在载体坐标系中的速度。
第二步：IMU输出判断相机解算速度是否使用NHC约束。
利用IMU输出的纵向和横向角速度判断移动平台是否发生换道和转弯，当IMU输出的角速度
Figure 746469DEST_PATH_IMAGE168
时，默认移动平台横向速度为0，
Figure 873825DEST_PATH_IMAGE169
时，默认移动平台纵向速度为0，即移动平台保持直行，此时，对相机求解的速度增加NHC约束：
Figure 678708DEST_PATH_IMAGE170
其中，
Figure 775977DEST_PATH_IMAGE171
为移动平台沿x轴（即沿车体横向向右）的速度，
Figure 218590DEST_PATH_IMAGE172
为移动平台沿y轴（即沿车体前向）的速度，
Figure 641482DEST_PATH_IMAGE173
为移动平台沿z轴（即沿车体纵向向上）的速度。
第三步：GNSS/IMU/Vision松组合导航解算。
该GNSS/IMU/Vision组合导航系统选取选取三维位置误差
Figure 169546DEST_PATH_IMAGE174
、三维速度误差
Figure 211451DEST_PATH_IMAGE175
、三维姿态角误差
Figure 898785DEST_PATH_IMAGE176
、加速度计偏差
Figure 592110DEST_PATH_IMAGE066
和比例因子
Figure 997684DEST_PATH_IMAGE068
、陀螺仪偏差
Figure 312121DEST_PATH_IMAGE070
和比例因子
Figure 853961DEST_PATH_IMAGE178
为状态量，即
Figure 759600DEST_PATH_IMAGE179
根据惯性导航的力学编排原理，构建基于扩展卡尔曼滤波的GNSS/IMU/Vision松组合导航系统状态方程，即
Figure 527836DEST_PATH_IMAGE180
其中
Figure 144500DEST_PATH_IMAGE181
是状态转移矩阵，
Figure 806426DEST_PATH_IMAGE182
是系统噪声。
在GNSS接收机接收到星历文件和观测文件后，根据伪距单点定位和多普勒测速解算移动平台的3维位置和速度，可以得到GNSS观测方程：
Figure 351807DEST_PATH_IMAGE183
其中，
Figure 200815DEST_PATH_IMAGE184
、
Figure 388214DEST_PATH_IMAGE185
分别为GNSS解算移动平台位置和速度，
Figure 904646DEST_PATH_IMAGE186
、
Figure 620929DEST_PATH_IMAGE187
分别为惯导系统解算的移动平台位置、速度，
Figure 599643DEST_PATH_IMAGE188
，其中，
Figure 449787DEST_PATH_IMAGE189
为
Figure 961671DEST_PATH_IMAGE190
的单位矩阵，
Figure 848855DEST_PATH_IMAGE191
和
Figure 203613DEST_PATH_IMAGE192
分别为
Figure 467236DEST_PATH_IMAGE193
和
Figure 332161DEST_PATH_IMAGE194
的全零矩阵，
Figure 514881DEST_PATH_IMAGE195
为GNSS观测误差。
经NHC约束的相机测速观测方程为：
Figure 966722DEST_PATH_IMAGE196
其中，
Figure 893090DEST_PATH_IMAGE197
、
Figure 113986DEST_PATH_IMAGE198
、
Figure 608553DEST_PATH_IMAGE199
为惯导系统解算的移动平台速度分别在x轴、y轴和z轴的分量，
Figure 406745DEST_PATH_IMAGE200
，
Figure 513634DEST_PATH_IMAGE201
为相机测速误差。
在得到GNSS/IMU/Vision松组合导航系统状态方程和观测方程后，可以利用卡尔曼滤波解算车辆的导航参数。
（3）用隐马尔可夫链模型将定位轨迹匹配到引导线地图上
由于城市环境的复杂性，GNSS/INS/Vision松组合导航系统解算的移动平台位置存在着一定的误差，本发明利用构建的引导线地图，基于隐马尔可夫链(Hidden MarkovModels，HMM）模型（参考：Hansson A, Korsberg E, Maghsood R, et al. Lane-level mapmatching based on HMM[J]. IEEE Transactions on Intelligent Vehicles, 2020.）将定位轨迹准确地匹配到该地图上，从而提高导航定位精度。
基于HMM模型的地图匹配算法主要解决在已知模型参数和观测序列的情况下，寻找使观测序列出现概率最大的状态序列即最优路径。HMM模型主要包含三个参数：初始状态概率向量、状态转移概率矩阵和观测概率矩阵。用HMM模型实现地图匹配的步骤如下：
第一步：在车道线地图中寻找组合导航解算的车辆位置10m半径范围内的所有候选路段；
第二步：针对每个候选路段计算观测概率和状态转移概率：
观测概率：
假设组合导航定位点与候选点之间的距离误差服从零均值高斯分布，观测概率为：
Figure 713671DEST_PATH_IMAGE202
其中，
Figure 113560DEST_PATH_IMAGE203
为松组合系统解算的移动平台位置，
Figure 399048DEST_PATH_IMAGE204
为t时刻第i条候选路段，假设
Figure 808163DEST_PATH_IMAGE205
为第i条候选路段的候选点，
Figure 128286DEST_PATH_IMAGE206
为观测点
Figure 699076DEST_PATH_IMAGE203
与候选路段
Figure 471860DEST_PATH_IMAGE204
之间的距离
Figure 917623DEST_PATH_IMAGE207
为当
Figure 826673DEST_PATH_IMAGE208
时刻的移动平台真实位置在引导道中的位置
Figure 99522DEST_PATH_IMAGE205
时观测到
Figure 969389DEST_PATH_IMAGE209
的概率，
Figure 844941DEST_PATH_IMAGE211
是组合导航解算位置的标准差， e为自然对数的底数， π是圆周率。状态转移概率：
状态转移概率是指组合导航解算的载体位置在引导线网络中从
Figure 483864DEST_PATH_IMAGE208
时刻的一条候选路段
Figure 786670DEST_PATH_IMAGE212
移动到
Figure 910877DEST_PATH_IMAGE213
时刻的一条候选路段
Figure 324541DEST_PATH_IMAGE214
的概率。当前时刻组合导航定位点与下一时刻组合导航定位点的候选点之间的距离越小时，选择这条道路的可能性越大，反之则越小。根据以上，可以构建转移概率：
Figure 83549DEST_PATH_IMAGE215
其中，
Figure 698201DEST_PATH_IMAGE216
为状态转移概率，
Figure 667294DEST_PATH_IMAGE217
为当前组合导航定位点与下一时刻组合导航定位点的候选点之间的距离，
Figure 760015DEST_PATH_IMAGE218
为当前组合导航定位点与下一个组合导航定位点的候选点之间的最大距离。
第三步：利用维特比算法(Viterbi Algorithm ，VA)求解最有可能的隐藏序列即最优路径（参考：Forney G D. The viterbi algorithm[J]. Proceedings of the IEEE,1973, 61(3): 268-278.）。
（4）用最终解算的位姿修正3D点云地图和引导线地图
在通过GNSS/IMU/Vision松组合导航解算和地图匹配后，可以获得移动平台的高精度位姿，根据解算的移动平台高精度位姿，进一步修正3D点云地图和引导线地图（步骤同3D点云地图构建和引导线地图构建），从而实现地图的实时更新，并用于下一个历元的计算。
本发明提供了一种结合动态建图的多源融合导航方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (1)

1.一种结合动态建图的多源融合导航方法，其特征在于，包括以下步骤：
步骤1，构建高精度3D点云地图和引导线地图；
步骤2，对步骤1中所构建的3D点云地图进行辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算；
步骤3，采用隐马尔可夫链模型将定位轨迹匹配到步骤1中所构建的引导线地图上；
步骤4，用匹配后的位姿修正3D点云地图和引导线地图；
步骤1中，利用高精度全球导航卫星系统及惯性测量元件组合导航设备和相机构建高精度3D点云地图和引导线地图，包括以下步骤：
步骤1-1，构建3D点云地图；
步骤1-2，构建引导线地图；
步骤1-3，建立字典；
其中，步骤1-1中所述构建3D点云地图的方法，包括以下步骤：
步骤1-1-1，对相机拍摄的图像进行特征提取和特征匹配，使用尺度不变特征变换的特征来描述环境特征；
步骤1-1-2，利用相机内置矩阵
Figure 356120DEST_PATH_IMAGE001
将图像中匹配的特征点u转换到相机坐标系，内置矩阵
Figure 640209DEST_PATH_IMAGE001
通过相机标定获得，其值为：
Figure 96598DEST_PATH_IMAGE002
其中，
Figure 993010DEST_PATH_IMAGE003
和
Figure 851244DEST_PATH_IMAGE004
为相机焦距分量，
Figure 542120DEST_PATH_IMAGE005
和
Figure 361171DEST_PATH_IMAGE006
为相机投影中心在图像坐标系中的坐标；
步骤1-1-3，利用高精度全球导航卫星系统及惯性测量元件组合导航设备解算出相机的位姿信息
Figure 185908DEST_PATH_IMAGE007
，并根据高精度相机位姿将匹配的特征点从相机坐标系转换到世界坐标系构建3D点云地图；
构建得到的3D点云地图中匹配的特征点在世界坐标系中的坐标G为：
Figure 275480DEST_PATH_IMAGE008
其中，
Figure 402836DEST_PATH_IMAGE009
为特征点 u的深度信息，
Figure 568238DEST_PATH_IMAGE010
为特征点u的齐次坐标；
步骤1-2中所述构建引导线地图包括以下步骤：
步骤1-2-1，图像预处理：
在图像中设置引导线所在的区域并进行截取；使用高斯滤波器对截取的图像进行平滑；使用仿射变换将图像转换成鸟瞰图；通过设置颜色阈值，对图形进行二值化处理，得到二值化图像；
步骤1-2-2，引导线检测：
对上述二值化图像进行高斯直方图统计，将两个极大峰值作为左右引导线的搜索起点；设置滑动窗口的大小，将行驶引导道离散为多个窗口，利用高斯直方图统计窗口，将高斯分布曲线的最大峰值所在点作为窗口的每个中心；寻找所有滑动窗口的中心位置，将这些中心点通过直线拟合或曲线拟合得到当前引导线曲线参数；
步骤1-2-3，引导线跟踪：
引导线在前后两帧图像中的位置相互关联，根据前一帧中引导线的位置预测当前帧中引导线的坐标；采用速度模型建立上一帧引导线位置与当前帧中引导线的位置关系：
Figure 540873DEST_PATH_IMAGE011
其中，
Figure 639279DEST_PATH_IMAGE012
和
Figure 671957DEST_PATH_IMAGE013
分别是上一帧和当前帧拟合的引导线的四个端点的坐标，
Figure 964136DEST_PATH_IMAGE014
和
Figure 865096DEST_PATH_IMAGE015
分别是上一帧和当前帧四个端点的速度，
Figure 427795DEST_PATH_IMAGE016
和
Figure 21588DEST_PATH_IMAGE017
分别代表像素点位置系统噪声和速度系统噪声；
观测方程为：
Figure 302527DEST_PATH_IMAGE018
其中 ，
Figure 616965DEST_PATH_IMAGE019
为观测噪声，
Figure 158805DEST_PATH_IMAGE020
为当前帧中观测的四个端点的坐标；
获得第一帧引导线位置后，根据当前帧的观测值利用卡尔曼滤波对后续帧进行连续跟踪；
步骤1-2-4，建立引导线地图：
选择4个路标点作为控制点求解单应矩阵
Figure 312049DEST_PATH_IMAGE021
：
Figure 939339DEST_PATH_IMAGE022
其中，
Figure 323047DEST_PATH_IMAGE023
为控制点在图像平面中的像素坐标，
Figure 594759DEST_PATH_IMAGE024
为控制点在相机坐标系中的坐标，
Figure 530354DEST_PATH_IMAGE025
为比例因子；
得到单应矩阵后，将检测的引导线像素坐标通过单应矩阵变换到相机坐标系，再利用高精度全球导航卫星系统及惯性测量元件求解的相机位姿，将引导线点从相机坐标系转换到世界坐标系，最终完成引导线地图的构建；
步骤1-3中所述建立字典，包括以下步骤：
使用词袋模型计算图像间的相似性，采用k叉树结构建立字典，对每一帧图像均用单词描述，实现地图数据库中快速最近邻搜索构建一个深度d=7、每次分叉数k=10的树：
步骤1-3-1，采用k-means算法把所有单词分为k类，得到字典的第一层；
步骤1-3-2，对第一层的每个节点，把属于该节点的样本用k-means算法聚类成k类，得到下一层；
步骤1-3-3，对每一层采用上述方法，直到得到最后的叶子层；
经过以上步骤，建立一个容纳
Figure 520307DEST_PATH_IMAGE026
个单词的字典，字典中单词的集合表示为：
Figure 566761DEST_PATH_IMAGE027
其中，特征点个数
Figure 457094DEST_PATH_IMAGE028
,
Figure 173377DEST_PATH_IMAGE029
为建立的单词，其中自然数
Figure 775260DEST_PATH_IMAGE030
；
在完成字典的构建后，对于某一帧图像A，将图像中提取的特征点用单词表中的单词近似替代，根据图像中单词出现的频率构建Bow向量
Figure 500770DEST_PATH_IMAGE031
，即：
Figure 871709DEST_PATH_IMAGE032
其中，
Figure 758893DEST_PATH_IMAGE033
为单词
Figure 848072DEST_PATH_IMAGE029
在图像A中的权重；
步骤2中所述3D点云地图辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算方法，包括以下步骤：
步骤2-1，3D点云地图辅助求解相机速度；
步骤2-2，惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束；
步骤2-3，全球导航卫星系统或惯性测量元件或视觉松组合导航解算；
步骤2-1中所述3D点云地图辅助求解相机速度方法，包括：
在当前帧图像经尺度不变特征变换的特征提取和构建Bow向量后，计算图像A和图像B的Bow向量即
Figure 613159DEST_PATH_IMAGE031
和
Figure 979550DEST_PATH_IMAGE034
的匹配相似度
Figure 427848DEST_PATH_IMAGE035
：
Figure 879690DEST_PATH_IMAGE036
其中，
Figure 681423DEST_PATH_IMAGE037
和
Figure 292533DEST_PATH_IMAGE038
分别为单词
Figure 20056DEST_PATH_IMAGE029
在图像A和B中的权重
Figure 959193DEST_PATH_IMAGE033
；
解算得到的最大相似度即为匹配到的地图数据库中的图片，找到匹配图片后，则获得图像像素坐标系中坐标和与之对应的路标点在世界坐标系中的坐标，接着利用PnP算法求解相机位姿，设相机的位姿为
Figure 954831DEST_PATH_IMAGE039
和
Figure 30234DEST_PATH_IMAGE040
，位姿的李代群用
Figure 695702DEST_PATH_IMAGE041
表示，即
Figure 981190DEST_PATH_IMAGE042
，
Figure 891770DEST_PATH_IMAGE043
为
Figure 87259DEST_PATH_IMAGE044
的全零矩阵的转置，像素坐标的归一化平面齐次坐标为
Figure 48262DEST_PATH_IMAGE045
，其深度为
Figure 696412DEST_PATH_IMAGE046
，其对应的路标点在世界坐标系中的齐次坐标为
Figure 768273DEST_PATH_IMAGE047
，则两者之间的关系为：
Figure 818269DEST_PATH_IMAGE048
写成矩阵形式为：
Figure 324074DEST_PATH_IMAGE049
上式中，相机位姿未知并且存在观测噪声，存在重投影误差，PnP算法通过将误差求和，寻找使误差最小的相机位姿，即
Figure 584154DEST_PATH_IMAGE050
在解算出相机的位移
Figure 69494DEST_PATH_IMAGE040
后，通过对时间差分求解相机速度，即：
Figure 364209DEST_PATH_IMAGE051
其中，
Figure 542380DEST_PATH_IMAGE040
为移动平台在相机坐标系中的位移，
Figure 165123DEST_PATH_IMAGE052
为时间间隔，
Figure 578786DEST_PATH_IMAGE053
为由相机坐标系到载体坐标系的转换矩阵，
Figure 104839DEST_PATH_IMAGE054
为由相机解算的移动平台在相机坐标系中的速度，
Figure 578546DEST_PATH_IMAGE055
为由相机解算的移动平台在载体坐标系中的速度；
步骤2-2中所述惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束，方法如下：
利用惯性测量元件输出的纵向和横向角速度判断移动平台是否发生换道和转弯，当惯性测量元件输出的沿载体横向的角速度
Figure 688584DEST_PATH_IMAGE056
时，判定移动平台横向速度为0，沿载体纵向的角速度
Figure 515726DEST_PATH_IMAGE057
时，判定移动平台纵向速度为0，即移动平台保持直行，此时，对相机求解的速度增加非完整约束：
Figure 519454DEST_PATH_IMAGE058
其中，
Figure 305007DEST_PATH_IMAGE059
为移动平台沿x轴即沿载体横向向右的速度，
Figure 400877DEST_PATH_IMAGE060
为移动平台沿y轴即沿载体前向的速度，
Figure 421923DEST_PATH_IMAGE061
为移动平台沿z轴即沿载体纵向向上的速度；
步骤2-3中所述全球导航卫星系统或惯性测量元件或视觉松组合导航解算，方法如下：
选取三维位置误差
Figure 889944DEST_PATH_IMAGE062
、三维速度误差
Figure 705454DEST_PATH_IMAGE063
、三维姿态角误差
Figure 790084DEST_PATH_IMAGE064
、加速度计偏差
Figure 224608DEST_PATH_IMAGE066
和比例因子
Figure 937349DEST_PATH_IMAGE068
、陀螺仪偏差
Figure 288872DEST_PATH_IMAGE070
和比例因子
Figure 860798DEST_PATH_IMAGE072
为状态量，即：
Figure 958067DEST_PATH_IMAGE074
根据惯性导航的力学编排，构建基于扩展卡尔曼滤波的全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程，即：
Figure 931840DEST_PATH_IMAGE075
其中
Figure 823572DEST_PATH_IMAGE076
是状态转移矩阵，
Figure 882795DEST_PATH_IMAGE077
是系统噪声；
在全球导航卫星系统接收到星历文件和观测文件后，根据伪距单点定位和多普勒测速解算移动平台的三维位置和速度，得到全球导航卫星系统的观测方程：
Figure 423236DEST_PATH_IMAGE078
其中，
Figure 844990DEST_PATH_IMAGE079
和
Figure 48569DEST_PATH_IMAGE080
分别为全球导航卫星系统解算的移动平台位置和速度，
Figure 188563DEST_PATH_IMAGE081
和
Figure 34160DEST_PATH_IMAGE082
分别为惯导系统解算的移动平台位置和速度，
Figure 716945DEST_PATH_IMAGE083
，
Figure 216059DEST_PATH_IMAGE084
为
Figure 220181DEST_PATH_IMAGE085
的单位矩阵，
Figure 603889DEST_PATH_IMAGE086
和
Figure 235DEST_PATH_IMAGE087
分别为
Figure 811196DEST_PATH_IMAGE085
和
Figure 925783DEST_PATH_IMAGE088
的全零矩阵，
Figure 847602DEST_PATH_IMAGE089
为全球导航卫星系统观测误差；
经非完整约束的相机测速观测方程为：
Figure 737936DEST_PATH_IMAGE090
其中，
Figure 578853DEST_PATH_IMAGE091
、
Figure 321681DEST_PATH_IMAGE092
和
Figure 906246DEST_PATH_IMAGE093
为惯导系统解算的移动平台速度分别在x轴、y轴和z轴的分量，
Figure 152551DEST_PATH_IMAGE094
，
Figure 305314DEST_PATH_IMAGE095
为相机测速误差；
得到全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程和观测方程后，利用卡尔曼滤波解算车辆的导航参数；
步骤3中所述用隐马尔可夫链模型将定位轨迹匹配到引导线地图上，方法如下：
采用隐马尔可夫链模型实现地图匹配的步骤如下：
步骤3-1，在引导线地图中寻找组合导航解算的移动平台位置在设定半径范围内的所有候选路段；
步骤3-2，针对每个候选路段计算观测概率和状态转移概率：
观测概率：
设组合导航定位点与候选点之间的距离误差服从零均值高斯分布，观测概率为：
Figure 128914DEST_PATH_IMAGE096
其中，
Figure 159580DEST_PATH_IMAGE097
为松组合系统解算的移动平台位置，
Figure 385025DEST_PATH_IMAGE098
为t时刻第i条候选路段，假设
Figure 443111DEST_PATH_IMAGE099
为第i条候选路段的候选点，
Figure 19586DEST_PATH_IMAGE100
为观测点
Figure 86899DEST_PATH_IMAGE097
与候选路段
Figure 307796DEST_PATH_IMAGE098
之间的距离，
Figure 926996DEST_PATH_IMAGE101
为当
Figure 364668DEST_PATH_IMAGE102
时刻的移动平台真实位置在引导道中的位置
Figure 970093DEST_PATH_IMAGE099
时观测到
Figure 170130DEST_PATH_IMAGE103
的概率，
Figure 101177DEST_PATH_IMAGE105
是组合导航解算位置的标准差，e为自然对数的底数， π是圆周率；
状态转移概率：
状态转移概率为组合导航解算的移动平台位置在引导线网络中从t时刻的一条候选路段
Figure 262031DEST_PATH_IMAGE106
移动到
Figure 795781DEST_PATH_IMAGE107
时刻的一条候选路段
Figure 227156DEST_PATH_IMAGE108
的概率；当前时刻组合导航定位点与下一时刻组合导航定位点的候选点之间的距离越小时，选择这条道路的可能性越大，反之则越小；构建转移概率：
Figure 329104DEST_PATH_IMAGE109
其中，
Figure DEST_PATH_IMAGE110
为状态转移概率，
Figure DEST_PATH_IMAGE111
为当前组合导航定位点与下一时刻组合导航定位点的候选点之间的距离，
Figure DEST_PATH_IMAGE112
为当前组合导航定位点与下一个组合导航定位点的候选点之间的最大距离；
步骤3-3，利用维特比算法求解最有可能的隐藏序列即最优路径，最终实现将定位轨迹匹配到引导线地图上；
步骤4中所述用匹配后的位姿修正3D点云地图和引导线地图，方法如下：
在完成步骤3中所述定位轨迹与引导线地图匹配后，根据解算的移动平台高精度位姿，按照步骤1所述方法进一步修正3D点云地图和引导线地图，完成所述地图实时更新。
CN202210133071.3A 2022-02-14 2022-02-14 一种结合动态建图的多源融合导航方法 Active CN114184200B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210133071.3A CN114184200B (zh) 2022-02-14 2022-02-14 一种结合动态建图的多源融合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210133071.3A CN114184200B (zh) 2022-02-14 2022-02-14 一种结合动态建图的多源融合导航方法

Publications (2)

Publication Number Publication Date
CN114184200A CN114184200A (zh) 2022-03-15
CN114184200B true CN114184200B (zh) 2022-06-17

Family

ID=80545860

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210133071.3A Active CN114184200B (zh) 2022-02-14 2022-02-14 一种结合动态建图的多源融合导航方法

Country Status (1)

Country Link
CN (1) CN114184200B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114782539B (zh) * 2022-06-21 2022-10-11 中航金城无人系统有限公司 一种多云天气下基于云层观测的视觉定位系统及方法
CN115426401B (zh) * 2022-08-31 2023-05-19 国家开放大学 基于教学空间的讯息推送方法与装置
CN116086453B (zh) * 2022-12-12 2024-03-12 运来智能装备(无锡)有限公司 一种基于概率优化计算的惯导和地图组合定位方法
CN116086469B (zh) * 2023-01-16 2023-10-24 禾多科技（北京）有限公司 一种车道定位方法及装置
CN117492056B (zh) * 2023-12-26 2024-03-22 合众新能源汽车股份有限公司 车辆融合定位方法、系统、装置和计算机可读介质
CN118067148B (zh) * 2024-04-17 2024-07-09 深圳市科乐达电子科技有限公司 一种基于大数据分析的车载导航系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595680A (zh) * 2016-12-15 2017-04-26 福州大学 一种基于隐马尔可夫模型的车辆gps数据地图匹配方法
CN109376785B (zh) * 2018-10-31 2021-09-24 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
US11243081B2 (en) * 2019-03-29 2022-02-08 Trimble Inc. Slam assisted INS
CN109974739B (zh) * 2019-04-15 2020-11-10 西安交通大学 基于高精度地图的全局导航系统及导航信息生成方法
CN111811506B (zh) * 2020-09-15 2020-12-01 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质

Also Published As

Publication number Publication date
CN114184200A (zh) 2022-03-15

Similar Documents

Publication Publication Date Title
CN114184200B (zh) 一种结合动态建图的多源融合导航方法
Rozenberszki et al. LOL: Lidar-only Odometry and Localization in 3D point cloud maps
Mohamed et al. A survey on odometry for autonomous navigation systems
CN113485441B (zh) 结合无人机高精度定位和视觉跟踪技术的配网巡检方法
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
Poddar et al. Evolution of visual odometry techniques
CN109945858A (zh) 用于低速泊车驾驶场景的多传感融合定位方法
Parra et al. Robust visual odometry for vehicle localization in urban environments
CN111862673A (zh) 基于顶视图的停车场车辆自定位及地图构建方法
Dumble et al. Airborne vision-aided navigation using road intersection features
US20220398825A1 (en) Method for generating 3d reference points in a map of a scene
CN115407357A (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
Zhou et al. Lane information extraction for high definition maps using crowdsourced data
Yan et al. SensorX2car: Sensors-to-car calibration for autonomous driving in road scenarios
CN108921896B (zh) 一种融合点线特征的下视视觉罗盘
Hoang et al. A simplified solution to motion estimation using an omnidirectional camera and a 2-D LRF sensor
Lu et al. Vision-based localization methods under GPS-denied conditions
Tang et al. Environmental perception for intelligent vehicles
García-García et al. 3D visual odometry for road vehicles
Kogan et al. Lane-level positioning with sparse visual cues
Aggarwal Machine vision based SelfPosition estimation of mobile robots
Lan et al. Highly robust and accurate multi-sensor fusion localization system for complex and challenging scenarios
Mao et al. Precise visual-inertial localization for UAV with the aid of a 2D georeferenced map
Evlampev et al. Map relative localization based on road lane matching with Iterative Closest Point algorithm

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
CB03 Change of inventor or designer information

Inventor after: Sun Rui

Inventor after: Wang Yuanyuan

Inventor after: Mao Yi

Inventor after: Liu Xiaofeng

Inventor after: Yang Yi

Inventor before: Sun Rui

Inventor before: Wang Yuanyuan

Inventor before: Mao Yi

Inventor before: Liu Xiaofeng

Inventor before: Yang Yi

CB03 Change of inventor or designer information