[发明专利]一种结合动态建图的多源融合导航方法

权利要求书

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，利用相机内置矩阵将图像中匹配的特征点u转换到相机坐标系，内置矩阵通过相机标定获得，其值为：

其中， 和 为相机焦距分量，和为相机投影中心在图像坐标系中的坐标；

步骤1-1-3，利用高精度全球导航卫星系统及惯性测量元件组合导航设备解算出相机的位姿信息 ，并根据高精度相机位姿将匹配的特征点从相机坐标系转换到世界坐标系构建3D点云地图；

构建得到的3D点云地图中匹配的特征点在世界坐标系中的坐标G为：

其中，为特征点 u的深度信息，为特征点u的齐次坐标；

步骤1-2中所述构建引导线地图包括以下步骤：

步骤1-2-1，图像预处理：

在图像中设置引导线所在的区域并进行截取；使用高斯滤波器对截取的图像进行平滑；使用仿射变换将图像转换成鸟瞰图；通过设置颜色阈值，对图形进行二值化处理，得到二值化图像；

步骤1-2-2，引导线检测：

对上述二值化图像进行高斯直方图统计，将两个极大峰值作为左右引导线的搜索起点；设置滑动窗口的大小，将行驶引导道离散为多个窗口，利用高斯直方图统计窗口，将高斯分布曲线的最大峰值所在点作为窗口的每个中心；寻找所有滑动窗口的中心位置，将这些中心点通过直线拟合或曲线拟合得到当前引导线曲线参数；

步骤1-2-3，引导线跟踪：

引导线在前后两帧图像中的位置相互关联，根据前一帧中引导线的位置预测当前帧中引导线的坐标；采用速度模型建立上一帧引导线位置与当前帧中引导线的位置关系：

其中，和 分别是上一帧和当前帧拟合的引导线的四个端点的坐标， 和 分别是上一帧和当前帧四个端点的速度， 和 分别代表像素点位置系统噪声和速度系统噪声；

观测方程为：

其中 ，为观测噪声， 为当前帧中观测的四个端点的坐标；

获得第一帧引导线位置后，根据当前帧的观测值利用卡尔曼滤波对后续帧进行连续跟踪；

步骤1-2-4，建立引导线地图：

选择4个路标点作为控制点求解单应矩阵：

其中，为控制点在图像平面中的像素坐标，为控制点在相机坐标系中的坐标，为比例因子；

得到单应矩阵后，将检测的引导线像素坐标通过单应矩阵变换到相机坐标系，再利用高精度全球导航卫星系统及惯性测量元件求解的相机位姿，将引导线点从相机坐标系转换到世界坐标系，最终完成引导线地图的构建；

步骤1-3中所述建立字典，包括以下步骤：

使用词袋模型计算图像间的相似性，采用k叉树结构建立字典，对每一帧图像均用单词描述，实现地图数据库中快速最近邻搜索构建一个深度d=7、每次分叉数k=10的树：

步骤1-3-1，采用k-means算法把所有单词分为k类，得到字典的第一层；

步骤1-3-2，对第一层的每个节点，把属于该节点的样本用k-means算法聚类成k类，得到下一层；

步骤1-3-3，对每一层采用上述方法，直到得到最后的叶子层；

经过以上步骤，建立一个容纳个单词的字典，字典中单词的集合表示为：

其中，特征点个数,为建立的单词，其中自然数 ；

在完成字典的构建后，对于某一帧图像A，将图像中提取的特征点用单词表中的单词近似替代，根据图像中单词出现的频率构建Bow向量，即：

其中， 为单词 在图像A中的权重；

步骤2中所述3D点云地图辅助的全球导航卫星系统或惯性测量元件或视觉组合导航解算方法，包括以下步骤：

步骤2-1，3D点云地图辅助求解相机速度；

步骤2-2，惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束；

步骤2-3，全球导航卫星系统或惯性测量元件或视觉松组合导航解算；

步骤2-1中所述3D点云地图辅助求解相机速度方法，包括：

在当前帧图像经尺度不变特征变换的特征提取和构建Bow向量后，计算图像A和图像B的Bow向量即和的匹配相似度：

其中， 和 分别为单词在图像A和B中的权重 ；

解算得到的最大相似度即为匹配到的地图数据库中的图片，找到匹配图片后，则获得图像像素坐标系中坐标和与之对应的路标点在世界坐标系中的坐标，接着利用PnP算法求解相机位姿，设相机的位姿为和，位姿的李代群用表示，即， 为的全零矩阵的转置，像素坐标的归一化平面齐次坐标为，其深度为，其对应的路标点在世界坐标系中的齐次坐标为，则两者之间的关系为：

写成矩阵形式为：

上式中，相机位姿未知并且存在观测噪声，存在重投影误差，PnP算法通过将误差求和，寻找使误差最小的相机位姿，即

在解算出相机的位移后，通过对时间差分求解相机速度，即：

其中，为移动平台在相机坐标系中的位移， 为时间间隔，为由相机坐标系到载体坐标系的转换矩阵，为由相机解算的移动平台在相机坐标系中的速度，为由相机解算的移动平台在载体坐标系中的速度；

步骤2-2中所述惯性测量元件输出判断相机解算速度是否使用非完整约束进行约束，方法如下：

利用惯性测量元件输出的纵向和横向角速度判断移动平台是否发生换道和转弯，当惯性测量元件输出的沿载体横向的角速度时，判定移动平台横向速度为0，沿载体纵向的角速度时，判定移动平台纵向速度为0，即移动平台保持直行，此时，对相机求解的速度增加非完整约束：

其中， 为移动平台沿x轴即沿载体横向向右的速度，为移动平台沿y轴即沿载体前向的速度，为移动平台沿z轴即沿载体纵向向上的速度；

步骤2-3中所述全球导航卫星系统或惯性测量元件或视觉松组合导航解算，方法如下：

选取三维位置误差、三维速度误差、三维姿态角误差、加速度计偏差和比例因子 、陀螺仪偏差和比例因子为状态量，即：

根据惯性导航的力学编排，构建基于扩展卡尔曼滤波的全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程，即：

其中是状态转移矩阵， 是系统噪声；

在全球导航卫星系统接收到星历文件和观测文件后，根据伪距单点定位和多普勒测速解算移动平台的三维位置和速度，得到全球导航卫星系统的观测方程：

其中，和分别为全球导航卫星系统解算的移动平台位置和速度，和分别为惯导系统解算的移动平台位置和速度，， 为的单位矩阵，和分别为和的全零矩阵，为全球导航卫星系统观测误差；

经非完整约束的相机测速观测方程为：

其中， 、和为惯导系统解算的移动平台速度分别在x轴、y轴和z轴的分量，， 为相机测速误差；

得到全球导航卫星系统或惯性测量元件或视觉松组合导航系统状态方程和观测方程后，利用卡尔曼滤波解算车辆的导航参数；

步骤3中所述用隐马尔可夫链模型将定位轨迹匹配到引导线地图上，方法如下：

采用隐马尔可夫链模型实现地图匹配的步骤如下：

步骤3-1，在引导线地图中寻找组合导航解算的移动平台位置在设定半径范围内的所有候选路段；

步骤3-2，针对每个候选路段计算观测概率和状态转移概率：

观测概率：

设组合导航定位点与候选点之间的距离误差服从零均值高斯分布，观测概率为：

其中， 为松组合系统解算的移动平台位置， 为t时刻第i条候选路段，假设为第i条候选路段的候选点， 为观测点与候选路段之间的距离，为当 时刻的移动平台真实位置在引导道中的位置时观测到的概率，是组合导航解算位置的标准差，e为自然对数的底数， π是圆周率；

状态转移概率：

状态转移概率为组合导航解算的移动平台位置在引导线网络中从t时刻的一条候选路段 移动到 时刻的一条候选路段的概率；当前时刻组合导航定位点与下一时刻组合导航定位点的候选点之间的距离越小时，选择这条道路的可能性越大，反之则越小；构建转移概率：

其中， 为状态转移概率， 为当前组合导航定位点与下一时刻组合导航定位点的候选点之间的距离，为当前组合导航定位点与下一个组合导航定位点的候选点之间的最大距离；

步骤3-3，利用维特比算法求解最有可能的隐藏序列即最优路径，最终实现将定位轨迹匹配到引导线地图上；

步骤4中所述用匹配后的位姿修正3D点云地图和引导线地图，方法如下：

在完成步骤3中所述定位轨迹与引导线地图匹配后，根据解算的移动平台高精度位姿，按照步骤1所述方法进一步修正3D点云地图和引导线地图，完成所述地图实时更新。