CN112129281A - 一种基于局部邻域地图的高精度图像导航定位方法 - Google Patents

Abstract

本发明公开了一种基于局部邻域地图的高精度图像导航定位方法，属于视觉导航领域，具体包含离线地图测绘阶段和导航定位阶段两个阶段：具体包含：采集原始数据；建立视觉位置地图1和地理位置地图2；构建视觉/地理坐标映射表步骤四：计算当前帧图像在视觉坐标系下的位置；提取的邻域坐标映射表；计算相似变换模型参数，由参数得到载体位置信息。可以达到分米级的动态定位精度。通过离线建立导航地图并利用该地图进行载体在线实时的导航定位，实现仅依赖于图像进行车载的自主导航定位，并且达到分米级的动态定位精度。

Description

一种基于局部邻域地图的高精度图像导航定位方法

技术领域

本发明属于视觉导航领域，具体涉及一种基于局部邻域地图的高精度图像导航定位方法。

背景技术

对于战时GPS信号拒止环境下，车载全自主导航主要依赖于传统惯性导航系统。由于惯性导航系统长时间运行导航定位的累积误差较大，无法适用于长时间的高精度导航定位需求，需要寻求新的全自主导航定位方法。

近年来，视觉SLAM被广泛应用于无人机、机器人等领域，能够根据图像序列完成局部环境内的地图绘制以及定位，具有高度自主性和较高的定位精度。最早等人提出的并行跟踪与制图(parallel tracking and mapping，PTAM)的算法，首次将定位和制图分为两个独立的线程进行并使用非线性优化，表现出较好的实时性。随后，等人利用ORB特征，在上述PATM 算法的基础上提出了ORB-SLAM系统。该系统分为跟踪定位、局部地图创建和闭环检测三个线程，并引入本质图来提升回环检测的速度。由于ORB-SLAM算法在工程上具有较好的实用性，许多学者都是在ORB-SLAM算法的基础上进行完善，以提升SLAM定位性能。

以上研究的单目SLAM输出的定位数据是基于以初始帧位置为基准所建立的坐标系。对于导航定位系统来说，缺乏真实世界的尺度信息。因此需要在SLAM输出的基础上赋予物理尺度，从而可应用到实际导航系统中。另外，从原理上看，单目SLAM系统的定位误差会随着时间变化而逐渐发散，即使采用了回环检测与融合方法，也只能在一定程度上减小其发散的程度。在实际应用中，缺乏尺度信息且误差发散的SLAM定位数据难以满足导航定位需求。

基于视觉SLAM的自主导航方法可以实现较高精度的局部定位，无法提供地球坐标系下的地理坐标。卫星导航技术是一种全球定位导航技术，在天空无遮挡的场景下可以提供地理坐标。实时动态载波相位差分技术(Real Time Kinematic，RTK)结合基准站和卫星多种信号实现毫米级的静态定位精度，在军事、城市规划、土地测量等领域得到广泛应用。

为了解决上述问题，提出一种基于局部邻域地图下的高精度图像导航定位的方法。具体涉及一种利用RTK将SLAM输出的无尺度位置信息转换为全局定位数据的方法和利用定位点局部邻域范围内的RTK获得分米级精度的三维位置，避免了SLAM随时间发散带来的精度损失。

发明内容

本发明所要解决的技术问题是提出了一种基于局部邻域地图的图像高精度导航定位方法。通过离线建立导航地图并利用该地图进行载体在线实时的导航定位，实现仅依赖于图像进行车载的自主导航定位，并且达到分米级的动态定位精度。

本发明为解决上述技术问题采用以下技术方案

一种基于局部邻域地图的高精度图像导航定位方法，具体包含以下两个阶段；

阶段一：离线地图测绘阶段：

步骤一：采集原始数据；

步骤二：建立视觉位置地图1和地理位置地图2；

步骤三：构建视觉/地理坐标映射表；

阶段二：导航定位阶段：

步骤四：计算当前帧图像在视觉坐标系下的位置；

步骤五：提取的邻域坐标映射表；

步骤六：计算相似变换模型参数，由参数得到载体位置信息。

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，在步骤一中，采用测绘导航数据采集硬件平台采集原始数据：所述测绘导航数据采集硬件平台包含RTK数据采集模块、图像采集模块、数据处理模块和无线数据监控模块。

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，所述步骤二具体如下；

测绘阶段利用ORB-SLAM通过分析连续的图像序列来推算载体位置随时间的变化，图像序列之间的共视关系以及共视点的图像位置刻画了相机在某一约定坐标系下的运动参数，将这一约定坐标系定义为视觉坐标系；利用ORB-SLAM系统输入步骤一采集的图像以及时间序列，得到每一帧图像在视觉坐标系下的三维空间位置坐标记为{v(t_k)}，其中k＝1,2,3...,N，t_k为图像采集时刻，从而建立区域的视觉位置地图1，同时保存应用场景内的所有关键帧图像、关键帧图像对应的词袋向量以及与关键帧图像存在共视关系的三维空间特征点；

利用RTK设备得到的经纬度是基于WGS-84坐标系的，由于在计算相似变换模型参数时采用的是直角坐标，因此需要将WGS-84坐标系下的经度、纬度和高度转换到当地地理位置为原点的直角坐标系下；

在地球上经度、纬度、高度为(L,λ,h)的位置，将其转换为地球坐标系O_e-X_eY_eZ_e下的关系式为

式中，R_e＝6378137m为参考椭球赤道平面半径，f＝1/298.257223563为参考椭球扁率；

由于地球坐标系下的坐标值一般较大，不利于在局部地理区域内使用，通常采用某一位置为基准建立当地地理坐标系，若选择的参考位置经纬高度坐标为(L₀,λ₀,h₀)，根据式计算出来的地球坐标系下坐标为X_e0＝(X_e0,Y_e0,Z_e0)^T，则可根据下式将地球坐标系转换到当地地理坐标系下：

根据式和式就可以将采集的RTK位置的经纬高坐标转换到当地地理坐标系下，并记为

其中j＝1,2,...,M，

为RTK数据采集时刻，从而建立地理位置地图2。

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，所述步骤三具体如下：由步骤一所知图像采集频率与RTK采集频率不一致，对于各个图像采集点，根据步骤二建立的视觉位置地图1和地理位置地图2，建立视觉/地理坐标映射表，考虑在t_k时刻下对地理坐标的推算，采用一阶线性插值的方法，利用时间戳找到离t_k时刻最近的RTK采集时刻，如图3中的

和

利用如下公式可获得t_k时刻的地理坐标为：

通过对所有的图像采集时刻获得对应的地理坐标后，建立视觉/地理坐标映射表。

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，在步骤四中，计算当前帧图像在视觉坐标系下的位置v_t；具体如下；

输入当前帧图像，利用ORB-SLAM系统通过测绘阶段存储的关键帧图像，以构建词袋向量并用于图像帧搜索；首先利用视觉词袋模型的词典计算当前帧图像的词袋向量，并与测绘阶段存储的关键帧图像集合对应的词袋向量进行比较搜索，可获得与当前帧图像想近的关键帧图像，而后利用ORB-SLAM系统进行航位推算，当前帧图像在视觉坐标系下的定位过程包括特征跟踪匹配与2D-3D视觉位姿估计两个步骤：对于特征跟踪匹配，利用与当前帧图像想近的关键帧图像，根据图像之间的共视关系找到可能观察到的特征点，并利用单个特征点的特征描述符和多个特征点之间的几何结构关系进行匹配，获得当前帧图像ORB特征点与地图中ORB 三维特征点的匹配关系；接着利用上述得到的ORB三维特征点的三维位置信息(x_i,y_i,z_i)，根据式所示的相机的成像原理，ORB三维特征点映射到图像中点的二维位置(u_i,v_i)，利用PnP算法，可根据至少三对二维到三维的点对映射关系解算出当前帧图像的位姿信息R、t，最后利用式根据上一时刻图像位置v_t-1解算出当前帧图像在视觉坐标系下的位置v_t；

v_t＝Rv_t-1+t。 (5)

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，在步骤五中，提取v_t的邻域坐标映射表，具体如下；

在步骤三建立的视觉/地理坐标映射表中搜索与当前帧图形视觉位置v_t想近的N组位置点；考虑N组位置点的视觉坐标与地理坐标分为记为{(v_k,g_k)}，其中k＝1,2,...,N，计算这些地图位置点在视觉坐标系下与v_t之间的欧式距离，记为

通过对这些距离进行排序，提取出欧式距离最小的K组地图位置点，其索引值为

其中

表示集合

中第K小的距离；提取索引集合S对应的视觉坐标与地理坐标，构成邻域坐标映射表{(v_k,g_k)|k∈S}；

作为本发明一种基于局部邻域地图的高精度图像导航定位方法的进一步优选方案，在步骤六中，计算相似变换模型参数R,t,s，由参数得到载体的位置g_t，具体如下；

利用步骤二中提取的邻域坐标映射表{(v_k,g_k)|k∈S}，考虑k个地图位置点，其在视觉坐标系下的坐标点集为

对应的地理坐标系下坐标点集为

若不考虑测量噪声的影响，它们之间满足相似变换关系；在考虑测量噪声的情况下，需要估计相似变换的模型参数，使得下式反应的误差指标最小：

根据上式获得的最小二乘意义下的最优解即为相似变换模型参数，分别为旋转因数R、平移向量t和尺度因子s；

将式写成如下简化的矩阵形式：

式中，h＝1_N×1；

对两个坐标点集取均值，可得到点集中心

将两个坐标点集移动到各自的点集中心，以消除平移向量的影响；定义向点集中心偏移的坐标点集为

得到相似变换的所有参数估计结果如下：

(1)根据式和式计算点集中心和向点集中心偏移的坐标点集

(2)对

进行奇异值分解，即

得到U、D和V，其中UU^T＝VV^T＝I，D为对角矩阵，对角线元素满足d₁＞d₂＞d₃＞0；

(3)计算旋转矩阵R＝VU^T，根据

计算尺度因子s，最后利用R和s来计算平移向量t＝μ_g-sRμ_v；

利用上述估计的相似变换模型参数，计算出当前时刻载体的地理位置为g_t＝sRv_t+t。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明相比于传统的全局定位算法，本发明可以达到分米级的动态定位精度。传统的全局定位算法首先提取视觉/地理坐标映射表中所有一一对应的点集，计算两组点集的相似变换模型参数，分别为旋转因数R、平移向量t和尺度因子s，而后利用ORB-SLAM系统输入当前图像帧，输出当前帧图像在视觉坐标系下的无尺度三维空间直角坐标v，最后利用g＝sRv+t输出当前帧图像的地理位置坐标g；通过离线建立导航地图并利用该地图进行载体在线实时的导航定位，实现仅依赖于图像进行车载的自主导航定位，并且达到分米级的动态定位精度。

附图说明

图1是测绘导航数据采集硬件平台；

图2测绘导航数据采集软件平台；

图3测绘导航试验数据采集流程图；

图4视觉/地理坐标映射表；

图5邻域映射表的提取；

图6局部邻域、传统全局以及区域RTK轨迹点；

其中，采集区域RTK的轨迹如图6中的细实线所示，利用传统全局定位算法进行定位的轨迹如图6中的Vision-1对应的粗实线所示，利用基于局部邻域地图进行定位的轨迹如图6中的Vision-2对应的虚线所示；

图7局部邻域与传统全局定位误差曲线；其中，Vision-1表示基于传统全局方法在空间直角坐标系下的定位误差，Vision-2表示基于局部邻域地图方法在空间直角坐标系下的定位误差。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

一种基于局部邻域地图的高精度图像导航定位方法，具体包含以下两个阶段；

阶段1：离线地图测绘阶段：

步骤1.1：采集原始数据；

步骤1.2：建立视觉位置地图1和地理位置地图2；

步骤1.3：构建视觉/地理坐标映射表；

阶段2：导航定位阶段：

步骤2.1：计算当前帧图像在视觉坐标系下的位置；

步骤2.2：提取的邻域坐标映射表，如图5所示；

步骤2.3：计算相似变换模型参数，由参数得到载体位置信息

1.离线地图测绘阶段

1.1采集原始数据

采用如图1所示的测绘导航数据采集硬件平台，主要分为四大模块：RTK数据采集模块、图像采集模块、数据处理模块和无线数据监控模块。测绘导航数据采集软件架构如图2所示，该平台通过Manifold板载处理器搭载的ROS操作系统对获取的图像和RTK位置信息进行同时采集和保存，其中图像采集频率设置为20Hz和RTK的采集频率设置为5Hz。ROS操作系统是一种分布式的处理框架，可以实现不同传感器数据采用相同的通信方式进行交互。根据如图6所示的测绘导航试验数据采集流程图进行图像数据、RTK数据以及对应的时间戳等原始数据的保存。利用传统全局定位算法进行定位的轨迹如图6中的Vision-1对应的粗实线所示，利用基于局部邻域地图进行定位的轨迹如图6中的Vision-2对应的虚线所示。

1.2建立视觉位置地图1和地理位置地图2；

测绘阶段利用ORB-SLAM通过分析连续的图像序列来推算载体位置随时间的变化，图像序列之间的共视关系以及共视点的图像位置刻画了相机在某一约定坐标系下的运动参数，将这一约定坐标系定义为视觉坐标系。利用ORB-SLAM系统输入1.1采集的图像以及时间序列，得到每一帧图像在视觉坐标系下的三维空间位置坐标记为{v(t_k)}，其中k＝1,2,3...,N，t_k为图像采集时刻，从而建立区域的视觉位置地图1，同时保存应用场景内的所有关键帧图像、关键帧图像对应的词袋向量以及与关键帧图像存在共视关系的三维空间特征点。

利用RTK设备得到的经纬度是基于WGS-84坐标系的，由于在计算相似变换模型参数时采用的是直角坐标，因此需要将WGS-84坐标系下的经度、纬度和高度转换到当地地理位置为原点的直角坐标系下。

在地球上经度、纬度、高度为(L,λ,h)的位置，将其转换为地球坐标系O_e-X_eY_eZ_e下的关系式为

式中，R_e＝6378137m为参考椭球赤道平面半径，f＝1/298.257223563为参考椭球扁率。

由于地球坐标系下的坐标值一般较大，不利于在局部地理区域内使用，通常采用某一位置为基准建立当地地理坐标系，若选择的参考位置经纬高度坐标为(L₀,λ₀,h₀)，根据式计算出来的地球坐标系下坐标为X_e0＝(X_e0,Y_e0,Z_e0)^T，则可根据下式将地球坐标系转换到当地地理坐标系下：

根据式和式就可以将采集的RTK位置的经纬高坐标转换到当地地理坐标系下，并记为

其中j＝1,2,...,M，

为RTK数据采集时刻，从而建立地理位置地图2。

2.3构建视觉/地理坐标映射表；

由1.1所知图像采集频率与RTK采集频率不一致，对于各个图像采集点，根据1.2建立的视觉位置地图1和地理位置地图2，利用如图4所示的方法建立视觉/地理坐标映射表，考虑在t_k时刻下对地理坐标的推算，采用一阶线性插值的方法，利用时间戳找到离t_k时刻最近的RTK采集时刻，如图3中的

和

利用如下公式可获得t_k时刻的地理坐标为：

通过对所有的图像采集时刻获得对应的地理坐标后，建立视觉/地理坐标映射表。

图7局部邻域与传统全局定位误差曲线

如图7的Vision-1表示基于传统全局方法在空间直角坐标系下的定位误差，Vision-2 表示基于局部邻域地图方法在空间直角坐标系下的定位误差。

表1局部邻域与传统全局误差统计特性

由表1和图7可知，传统全局算法定位误差平均值为11.58m，均方差为6.86m，而基于局部邻域地图算法定位误差平均值为0.36m，均方差为0.31m，结果表明基于局部邻域地图下的图像导航定位方法可以达到分米级的动态定位精度，优于传统的全局定位算法。

2.导航定位阶段

2.1计算当前帧图像在视觉坐标系下的位置v_t；

输入当前帧图像，利用ORB-SLAM系统通过测绘阶段存储的关键帧图像，以构建词袋向量并用于图像帧搜索。首先利用视觉词袋模型的词典计算当前帧图像的词袋向量，并与测绘阶段存储的关键帧图像集合对应的词袋向量进行比较搜索，可获得与当前帧图像想近的关键帧图像，而后利用ORB-SLAM系统进行航位推算，当前帧图像在视觉坐标系下的定位过程包括特征跟踪匹配与2D-3D视觉位姿估计两个步骤：对于特征跟踪匹配，利用与当前帧图像想近的关键帧图像，根据图像之间的共视关系找到可能观察到的特征点，并利用单个特征点的特征描述符和多个特征点之间的几何结构关系进行匹配，获得当前帧图像ORB特征点与地图中ORB 三维特征点的匹配关系；接着利用上述得到的ORB三维特征点的三维位置信息(x_i,y_i,z_i)，根据式所示的相机的成像原理，ORB三维特征点映射到图像中点的二维位置(u_i,v_i)，利用PnP算法，可根据至少三对二维到三维的点对映射关系解算出当前帧图像的位姿信息R、t，最后利用式根据上一时刻图像位置v_t-1解算出当前帧图像在视觉坐标系下的位置v_t。

v_t＝Rv_t-1+t\*MERGEFORMAT (5)

2.2提取v_t的邻域坐标映射表；

在测绘阶段1.3建立的视觉/地理坐标映射表中搜索与当前帧图形视觉位置v_t想近的N 组位置点。考虑N组位置点的视觉坐标与地理坐标分为记为{(v_k,g_k)}，其中k＝1,2,...,N，计算这些地图位置点在视觉坐标系下与v_t之间的欧式距离，记为

通过对这些距离进行排序，提取出欧式距离最小的K组地图位置点，其索引值为

其中

表示集合

中第K小的距离。提取索引集合S对应的视觉坐标与地理坐标，构成邻域坐标映射表{(v_k,g_k)|k∈S}。

2.3计算相似变换模型参数R,t,s，由参数得到载体的位置g_t

利用2.2中提取的邻域坐标映射表{(v_k,g_k)|k∈S}，考虑k个地图位置点，其在视觉坐标系下的坐标点集为

对应的地理坐标系下坐标点集为

若不考虑测量噪声的影响，它们之间满足相似变换关系。在考虑测量噪声的情况下，需要估计相似变换的模型参数，使得下式反应的误差指标最小：

根据上式获得的最小二乘意义下的最优解即为相似变换模型参数，分别为旋转因数R、平移向量t和尺度因子s。

将式写成如下简化的矩阵形式：

式中，h＝1_N×1。

对两个坐标点集取均值，可得到点集中心

将两个坐标点集移动到各自的点集中心，以消除平移向量的影响。定义向点集中心偏移的坐标点集为

得到相似变换的所有参数估计结果如下：

(1)根据式和式计算点集中心和向点集中心偏移的坐标点集

(2)对

进行奇异值分解，即

得到U、D和V，其中UU^T＝VV^T＝I，D为对角矩阵，对角线元素满足d₁＞d₂＞d₃＞0；

(3)计算旋转矩阵R＝VU^T，根据

计算尺度因子s，最后利用R和s来计算平移向量t＝μ_g-sRμ_v。

利用上述估计的相似变换模型参数，计算出当前时刻载体的地理位置(直角坐标)为 g_t＝sRv_t+t。

Claims (7)

1.一种基于局部邻域地图的高精度图像导航定位方法，其特征在于：具体包含以下两个阶段；

阶段一：离线地图测绘阶段：

步骤一：采集原始数据；

步骤二：建立视觉位置地图1和地理位置地图2；

步骤三：构建视觉/地理坐标映射表；

阶段二：导航定位阶段：

步骤四：计算当前帧图像在视觉坐标系下的位置；

步骤五：提取的邻域坐标映射表；

步骤六：计算相似变换模型参数，由参数得到载体位置信息。

2.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：在步骤一中，采用测绘导航数据采集硬件平台采集原始数据：所述测绘导航数据采集硬件平台包含RTK数据采集模块、图像采集模块、数据处理模块和无线数据监控模块。

3.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：所述步骤二具体如下；

测绘阶段利用ORB-SLAM通过分析连续的图像序列来推算载体位置随时间的变化，图像序列之间的共视关系以及共视点的图像位置刻画了相机在某一约定坐标系下的运动参数，将这一约定坐标系定义为视觉坐标系；利用ORB-SLAM系统输入步骤一采集的图像以及时间序列，得到每一帧图像在视觉坐标系下的三维空间位置坐标记为{v(t_k)}，其中k＝1,2,3...,N，t_k为图像采集时刻，从而建立区域的视觉位置地图1，同时保存应用场景内的所有关键帧图像、关键帧图像对应的词袋向量以及与关键帧图像存在共视关系的三维空间特征点；

利用RTK设备得到的经纬度是基于WGS-84坐标系的，由于在计算相似变换模型参数时采用的是直角坐标，因此需要将WGS-84坐标系下的经度、纬度和高度转换到当地地理位置为原点的直角坐标系下；

在地球上经度、纬度、高度为(L,λ,h)的位置，将其转换为地球坐标系O_e-X_eY_eZ_e下的关系式为

式中，R_e＝6378137m为参考椭球赤道平面半径，f＝1/298.257223563为参考椭球扁率；

由于地球坐标系下的坐标值一般较大，不利于在局部地理区域内使用，通常采用某一位置为基准建立当地地理坐标系，若选择的参考位置经纬高度坐标为(L₀,λ₀,h₀)，根据式计算出来的地球坐标系下坐标为X_e0＝(X_e0,Y_e0,Z_e0)^T，则可根据下式将地球坐标系转换到当地地理坐标系下：

根据式和式就可以将采集的RTK位置的经纬高坐标转换到当地地理坐标系下，并记为

其中j＝1,2,...,M，

为RTK数据采集时刻，从而建立地理位置地图2。

4.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：所述步骤三具体如下：由步骤一所知图像采集频率与RTK采集频率不一致，对于各个图像采集点，根据步骤二建立的视觉位置地图1和地理位置地图2，建立视觉/地理坐标映射表，考虑在t_k时刻下对地理坐标的推算，采用一阶线性插值的方法，利用时间戳找到离t_k时刻最近的RTK采集时刻，如图三中的

和

利用如下公式可获得t_k时刻的地理坐标为：

通过对所有的图像采集时刻获得对应的地理坐标后，建立视觉/地理坐标映射表。

5.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：在步骤四中，计算当前帧图像在视觉坐标系下的位置v_t；具体如下；

输入当前帧图像，利用ORB-SLAM系统通过测绘阶段存储的关键帧图像，以构建词袋向量并用于图像帧搜索；首先利用视觉词袋模型的词典计算当前帧图像的词袋向量，并与测绘阶段存储的关键帧图像集合对应的词袋向量进行比较搜索，可获得与当前帧图像想近的关键帧图像，而后利用ORB-SLAM系统进行航位推算，当前帧图像在视觉坐标系下的定位过程包括特征跟踪匹配与2D-3D视觉位姿估计两个步骤：对于特征跟踪匹配，利用与当前帧图像想近的关键帧图像，根据图像之间的共视关系找到可能观察到的特征点，并利用单个特征点的特征描述符和多个特征点之间的几何结构关系进行匹配，获得当前帧图像ORB特征点与地图中ORB三维特征点的匹配关系；接着利用上述得到的ORB三维特征点的三维位置信息(x_i,y_i,z_i)，根据式所示的相机的成像原理，ORB三维特征点映射到图像中点的二维位置(u_i,v_i)，利用PnP算法，可根据至少三对二维到三维的点对映射关系解算出当前帧图像的位姿信息R、t，最后利用式根据上一时刻图像位置v_t-1解算出当前帧图像在视觉坐标系下的位置v_t；

v_t＝Rv_t-1+t。 (5)

6.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：在步骤五中，提取v_t的邻域坐标映射表，具体如下；

在步骤三建立的视觉/地理坐标映射表中搜索与当前帧图形视觉位置v_t想近的N组位置点；考虑N组位置点的视觉坐标与地理坐标分为记为{(v_k,g_k)}，其中k＝1,2,...,N，计算这些地图位置点在视觉坐标系下与v_t之间的欧式距离，记为

通过对这些距离进行排序，提取出欧式距离最小的K组地图位置点，其索引值为

其中

表示集合

中第K小的距离；提取索引集合S对应的视觉坐标与地理坐标，构成邻域坐标映射表{(v_k,g_k)|k∈S}。

7.根据权利要求1所述的一种基于局部邻域地图的高精度图像导航定位方法,其特征在于：在步骤六中，计算相似变换模型参数R,t,s，由参数得到载体的位置g_t，具体如下；

利用步骤二中提取的邻域坐标映射表{(v_k,g_k)|k∈S}，考虑k个地图位置点，其在视觉坐标系下的坐标点集为

对应的地理坐标系下坐标点集为

若不考虑测量噪声的影响，它们之间满足相似变换关系；在考虑测量噪声的情况下，需要估计相似变换的模型参数，使得下式反应的误差指标最小：

根据上式获得的最小二乘意义下的最优解即为相似变换模型参数，分别为旋转因数R、平移向量t和尺度因子s；

将式写成如下简化的矩阵形式：

式中，h＝1_N×1；

对两个坐标点集取均值，可得到点集中心

将两个坐标点集移动到各自的点集中心，以消除平移向量的影响；定义向点集中心偏移的坐标点集为

得到相似变换的所有参数估计结果如下：

(1)根据式和式计算点集中心和向点集中心偏移的坐标点集

(2)对

进行奇异值分解，即

得到U、D和V，其中UU^T＝VV^T＝I，D为对角矩阵，对角线元素满足d₁＞d₂＞d₃＞0；

(3)计算旋转矩阵R＝VU^T，根据

计算尺度因子s，最后利用R和s来计算平移向量t＝μ_g-sRμ_v；

利用上述估计的相似变换模型参数，计算出当前时刻载体的地理位置为g_t＝sRv_t+t。

Images