阅读说明：本技术 卫星与视觉紧耦合定位方法、系统及介质 (satellite and vision tight coupling positioning method, system and medium ) 是由 龚正 应忍冬 刘佩林 薛午阳 缪瑞航 于 2019-09-18 设计创作，主要内容包括：本发明提供了一种卫星与视觉紧耦合定位方法、系统及介质,包括：融合系统初始化步骤：进行视觉SLAM系统初始化,获取视觉地图与卫星定位坐标系相对位姿信息；融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息,对当前载体运动进行对应坐标系下的运动分解确定状态量,并基于传感器的所观测的状态量进行传感器状态跟踪,确定关键帧并建立局部地图；融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上,结合视觉局部地图与卫星伪距数据进行联合优化定位解算。本发明利用基于导航坐标系分解的异构异步传感器定位数据更新算法,使得即使在室内外复杂环境中可以仍然可以鲁棒精确地进行导航定位。(the invention provides a method, a system and a medium for positioning a satellite and vision tight coupling, which comprises the following steps: initializing a fusion system: initializing a visual SLAM system, and acquiring relative pose information of a visual map and a satellite positioning coordinate system; and (3) fusing asynchronous tracking steps of the system: according to different sensor data and relative pose information of the obtained visual map and a satellite positioning coordinate system, motion decomposition is carried out on the motion of the current carrier under a corresponding coordinate system to determine state quantity, sensor state tracking is carried out on the basis of the observed state quantity of the sensor, a key frame is determined, and a local map is established; a step of fusion system joint optimization: and performing joint optimization positioning calculation by combining the visual local map and the satellite pseudo-range data on the basis of the state quantity obtained after the navigation coordinate system is decomposed. The invention utilizes the heterogeneous asynchronous sensor positioning data updating algorithm based on the decomposition of the navigation coordinate system, so that the navigation positioning can still be robustly and accurately carried out even in indoor and outdoor complex environments.)

卫星与视觉紧耦合定位方法、系统及介质

技术领域

本发明涉及定位技术领域，具体地，涉及卫星与视觉紧耦合定位方法、系统及介质。尤其地，涉及一种基于同步定位与建图(Simultaneous Localization and Maping，SLAM)的卫星与视觉紧耦合定位算法，可用于自主无人系统在复杂室内外环境下的定位，提高定位精度及可靠性。

背景技术

当前自主无人系统(包含无人驾驶车辆、自主机器人、微型无人机等)的工作场景随着应用的增长而日趋复杂。单一的定位导航系统，如全球卫星导航系统(GlobalNavigation Satelitte System，GNSS)，惯性导航系统(Inertial Navigation System,INS)，视觉同步定位及建图技术(Vision-based Simultaneous Localization andMaping，V-SLAM)均只能解决部分环境场景以及应用需求。在这样的应用需求与技术背景下，为了最终实现在复杂环境下高精度与鲁棒稳定的载体定位，传感器以及传感方法的融合技术应运而生，旨在不同的场景下发挥每种传感器及其方法的最大作用，实现互补地提升系统鲁棒性及精度的目的。本算法在此背景下，着重研究了在复杂场景下异质异构传感器信息的统一抽象方法与融合架构，实现感知效能最大化与定位结果的优化融合，以期最终实现高精度与鲁棒稳定的载体定位。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种卫星与视觉紧耦合定位方法、系统及介质。

根据本发明提供的一种卫星与视觉紧耦合定位方法，包括：

融合系统初始化步骤：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；

融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；

融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

优选地，所述融合系统初始化步骤：

进行融合系统初始化，若初始化成功则通过外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态，记为视觉地图与卫星定位坐标系相对位姿信息，进入融合系统异步跟踪步骤继续执行；若初始化不成功，则继续初始化直到初始化成功；

判断所接收到的传感器数据类型：若为卫星伪距数据，则对其进行单点定位解算，进入融合系统异步跟踪步骤继续执行；若为图像数据，则对其进行标准视觉SLAM初始化流程，进入融合系统异步跟踪步骤继续执行；

所述外部器件包括：陀螺仪、磁力计。

优选地，所述融合系统初始化步骤：

所述获取视觉地图与卫星定位坐标系相对位姿信息指：

导航坐标系包括：地心地固ECEF坐标系本地地理坐标系由SLAM系统在建立初始地图时创建的局部地图坐标系以及移动载体坐标系

导航坐标系之间的转换关系通过如下公式表示：

其中，

p^B为在载体坐标系下的一三维点，为此点对应在世界坐标系下的齐次表示；

表示本地地理坐标系至世界坐标系的变换关系；

表示局部坐标系至本地地理坐标系的变换关系；

表示载体坐标系至局部坐标系的变换关系；

表示在载体坐标系下的三维点位置；

表示本地地理坐标系至世界坐标系的旋转关系；

t_o表示表示本地地理坐标系在世界坐标系下的原点；

表示局部坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至局部坐标系的旋转关系；

表示载体坐标系至局部坐标系的平移关系；

通过分析该方程，得到在导航坐标系下所关心的基础运动分解变量，如以下公式所示：

其中，

表示载体坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至世界坐标系的平移关系；

与这两个变量为在实际应用中最为关心的两个状态量，分别为载体坐标系至本地地理坐标系之间的姿态转换与载体坐标与地心地固坐标系之间的平动转换，由公式可以看出，主要由局部地图坐标系至本地地理坐标系的旋转变换与载体坐标系至局部地图坐标系之间的旋转变换组成；

则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点t_o组成；

当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始t_o值。

优选地，所述融合系统异步跟踪步骤：

卫星及视觉联合异步跟踪步骤：

若为卫星伪距数据，则对其进行单点定位解算：

对于卫星系统，其状态量为x_GNSS＝[(t_rel+t_o)^Tδ]^T

其中，

x_GNSS表示卫星定位系统的状态量；

t_rel表示相对位置；

·^T表示矩阵的转置；

δ表示接收机钟差；

其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点t_o进行更新。求解卫星系统状态量等同于求解以下优化问题：

表示卫星接收机的后验状态量；

表示在j时刻的卫星测量方程；

若为视觉数据，则对其进行标准视觉SLAM初始化流程：

视觉系统的状态量为在李群切空间下的投影即

其中，

x_Cam表示视觉系统的状态量；

表示视觉系统的状态量在李群切空间下的投影；

·^∨表示反对称矩阵转换；

求解视觉系统的状态量等效于求解以下优化问题：

其中，

表示视觉系统的后验状态量；

表示视觉传感器在第i帧的测量方程；

异构关键帧确定步骤：

若为图像数据，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信：若可信，则创建包含视觉数据及卫星伪距信息的关键帧；否则，则创建只有视觉数据的关键帧；

若为卫星伪距数据，则根据最小时间差阈值以及视觉特征点数量判断最近的视觉帧是否可信；若是，则创建包含卫星伪距信息及视觉数据的关键帧；否则，则创建只有卫星伪距信息的关键帧。

优选地，所述融合系统联合优化步骤：

第k帧的关键帧状态量可表示为：

其中，

表示第k帧的关键帧状态量；

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影；

t_ok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点；

δ_k表示第k帧的关键帧中的接收机钟差；

表示十元空间；

求解该状态量等同于解决优化方程，具体如下所示：

其中，

表示所有关键帧状态的集合的后验状态；

N表示视觉关键帧个数；

表示在第i帧观察到的特征点集合，在局部坐标系下表示；

K表示包含卫星的关键帧个数；

M_k表示在第k帧内观察到的卫星个数。

根据本发明提供的一种卫星与视觉紧耦合定位系统，包括：

融合系统初始化模块：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；

融合系统异步跟踪模块：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；

融合系统联合优化模块：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

优选地，所述融合系统初始化模块：

进行融合系统初始化，若初始化成功则通过外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态，记为视觉地图与卫星定位坐标系相对位姿信息，进入融合系统异步跟踪模块继续执行；若初始化不成功，则继续初始化直到初始化成功；

判断所接收到的传感器数据类型：若为卫星伪距数据，则对其进行单点定位解算，调用融合系统异步跟踪模块；若为图像数据，则对其进行标准视觉SLAM初始化流程，调用融合系统异步跟踪模块；

所述外部器件包括：陀螺仪、磁力计。

优选地，所述融合系统初始化模块：

所述获取视觉地图与卫星定位坐标系相对位姿信息指：

导航坐标系包括：地心地固ECEF坐标系本地地理坐标系由SLAM系统在建立初始地图时创建的局部地图坐标系以及移动载体坐标系

导航坐标系之间的转换关系通过如下公式表示：

其中，

p^B为在载体坐标系下的一三维点，为此点对应在世界坐标系下的齐次表示；

表示本地地理坐标系至世界坐标系的变换关系；

表示局部坐标系至本地地理坐标系的变换关系；

表示载体坐标系至局部坐标系的变换关系；

表示在载体坐标系下的三维点位置；

表示本地地理坐标系至世界坐标系的旋转关系；

t_o表示表示本地地理坐标系在世界坐标系下的原点；

表示局部坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至局部坐标系的旋转关系；

表示载体坐标系至局部坐标系的平移关系；

通过分析该方程，得到在导航坐标系下所关心的基础运动分解变量，如以下公式所示：

其中，

表示载体坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至世界坐标系的平移关系；

与这两个变量为在实际应用中最为关心的两个状态量，分别为载体坐标系至本地地理坐标系之间的姿态转换与载体坐标与地心地固坐标系之间的平动转换，由公式可以看出，主要由局部地图坐标系至本地地理坐标系的旋转变换与载体坐标系至局部地图坐标系之间的旋转变换组成；

则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点t_o组成；

当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始t_o值。

优选地，所述融合系统异步跟踪模块：

卫星及视觉联合异步跟踪模块：

若为卫星伪距数据，则对其进行单点定位解算：

对于卫星系统，其状态量为x_GNSS＝[(t_rel+t_o)^Tδ]^T

其中，

x_GNSS表示卫星定位系统的状态量；

trel表示相对位置；

·^T表示矩阵的转置；

δ表示接收机钟差；

其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点t_o进行更新。求解卫星系统状态量等同于求解以下优化问题：

表示卫星接收机的后验状态量；

表示在j时刻的卫星测量方程；

若为视觉数据，则对其进行标准视觉SLAM初始化流程：

视觉系统的状态量为在李群切空间下的投影即

其中，

x_Cam表示视觉系统的状态量；

表示视觉系统的状态量在李群切空间下的投影；

·^∨表示反对称矩阵转换；

求解视觉系统的状态量等效于求解以下优化问题：

其中，

表示视觉系统的后验状态量；

表示视觉传感器在第i帧的测量方程；

异构关键帧确定模块：

若为图像数据，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信：若可信，则创建包含视觉数据及卫星伪距信息的关键帧；否则，则创建只有视觉数据的关键帧；

若为卫星伪距数据，则根据最小时间差阈值以及视觉特征点数量判断最近的视觉帧是否可信；若是，则创建包含卫星伪距信息及视觉数据的关键帧；否则，则创建只有卫星伪距信息的关键帧；

所述融合系统联合优化模块：

第k帧的关键帧状态量可表示为：

其中，

表示第k帧的关键帧状态量；

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影；

t_ok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点；

δ_k表示第k帧的关键帧中的接收机钟差；

表示十元空间；

求解该状态量等同于解决优化方程，具体如下所示：

其中，

表示所有关键帧状态的集合的后验状态；

N表示视觉关键帧个数；

表示在第i帧观察到的特征点集合，在局部坐标系下表示；

K表示包含卫星的关键帧个数；

M_k表示在第k帧内观察到的卫星个数。

根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的卫星与视觉紧耦合定位方法的步骤。

与现有技术相比，本发明具有如下的有益效果：

1、本发明利用基于导航坐标系分解的异构异步传感器定位数据更新算法，使得即使在室内外复杂环境中可以仍然可以鲁棒精确地进行导航定位。

2、本发明利用图优化算法解算卫星伪距定位信息，并且利用将卫星伪距数据作为特殊视觉特征点的方法，实现了SLAM架构下的卫星与视觉定位紧耦合融合定位算法，解决了异质异构传感器在同一算法框架下的融合问题，提高了系统定位精度。

3、本发明利用异构传感器关键帧合成与选择算法解决了异质异构传感器的时间同步问题，使得不同传感器信息不需要进行严格时间同步即可进行定位解算，提高了定位的精确度。

4、采用基于SLAM算法结构下的视觉-卫星联合优化定位算法，可以降低对现有程度算法的改动，降低实现成本。

5、将卫星伪距信号作为特殊的视觉特征点参与计算。

6、在定位跟踪阶段卫星只负责更新全局坐标原点而非最终定位结果。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明提供的一实施例中视觉特征点地图与卫星伪距观测示意图；

图2为本发明提供的一实施例中算法流程实施框图；

图3本发明提供的一实施例中不同导航坐标系下不同传感器的运动位姿分解示意图；

图4为本发明提供的一实施例中同一载体坐标系下不同传感器的运动位姿分解示意图；

图5为本发明提供的一实施例中不同传感器对运动位姿分解后状态量的观测示意图；

图6为发明提供的一实施例中异步跟踪算法流程实施框图；

图7为发明提供的一实施例中关键帧选取算法框图；

图8为本发明提供的一实施例中融合定位与独立传感器定位的结果对比示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的一种卫星与视觉紧耦合定位方法，包括：

融合系统初始化步骤：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；

融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；

融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

优选地，所述融合系统初始化步骤：

进行融合系统初始化，若初始化成功则通过外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态，记为视觉地图与卫星定位坐标系相对位姿信息，进入融合系统异步跟踪步骤继续执行；若初始化不成功，则继续初始化直到初始化成功；

判断所接收到的传感器数据类型：若为卫星伪距数据，则对其进行单点定位解算，进入融合系统异步跟踪步骤继续执行；若为图像数据，则对其进行标准视觉SLAM初始化流程，进入融合系统异步跟踪步骤继续执行；

所述外部器件包括：陀螺仪、磁力计。

优选地，所述融合系统初始化步骤：

所述获取视觉地图与卫星定位坐标系相对位姿信息指：

导航坐标系包括：地心地固ECEF坐标系本地地理坐标系由SLAM系统在建立初始地图时创建的局部地图坐标系以及移动载体坐标系

导航坐标系之间的转换关系通过如下公式表示：

其中，

p^B为在载体坐标系下的一三维点，为此点对应在世界坐标系下的齐次表示；

表示本地地理坐标系至世界坐标系的变换关系；

表示局部坐标系至本地地理坐标系的变换关系；

表示载体坐标系至局部坐标系的变换关系；

表示在载体坐标系下的三维点位置；

表示本地地理坐标系至世界坐标系的旋转关系；

t_o表示表示本地地理坐标系在世界坐标系下的原点；

表示局部坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至局部坐标系的旋转关系；

表示载体坐标系至局部坐标系的平移关系；

通过分析该方程，得到在导航坐标系下所关心的基础运动分解变量，如以下公式所示：

其中，

表示载体坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至世界坐标系的平移关系；

与这两个变量为在实际应用中最为关心的两个状态量，分别为载体坐标系至本地地理坐标系之间的姿态转换与载体坐标与地心地固坐标系之间的平动转换，由公式可以看出，主要由局部地图坐标系至本地地理坐标系的旋转变换与载体坐标系至局部地图坐标系之间的旋转变换组成；

则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点t_o组成；

当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始t_o值。

优选地，所述融合系统异步跟踪步骤：

卫星及视觉联合异步跟踪步骤：

若为卫星伪距数据，则对其进行单点定位解算：

对于卫星系统，其状态量为x_GNSS＝[(t_rel+t_o)^Tδ]^T

其中，

x_GNSS表示卫星定位系统的状态量；

t_rel表示相对位置；

·^T表示矩阵的转置；

δ表示接收机钟差；

其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点t_o进行更新。求解卫星系统状态量等同于求解以下优化问题：

表示卫星接收机的后验状态量；

表示在j时刻的卫星测量方程；

若为视觉数据，则对其进行标准视觉SLAM初始化流程：

视觉系统的状态量为在李群切空间下的投影即

其中，

x_Cam表示视觉系统的状态量；

表示视觉系统的状态量在李群切空间下的投影；

·^∨表示反对称矩阵转换；

求解视觉系统的状态量等效于求解以下优化问题：

其中，

表示视觉系统的后验状态量；

表示视觉传感器在第i帧的测量方程；

异构关键帧确定步骤：

若为图像数据，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信：若可信，则创建包含视觉数据及卫星伪距信息的关键帧；否则，则创建只有视觉数据的关键帧；

若为卫星伪距数据，则根据最小时间差阈值以及视觉特征点数量判断最近的视觉帧是否可信；若是，则创建包含卫星伪距信息及视觉数据的关键帧；否则，则创建只有卫星伪距信息的关键帧。

优选地，所述融合系统联合优化步骤：

第k帧的关键帧状态量可表示为：

其中，

表示第k帧的关键帧状态量；

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影；

t_ok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点；

δ_k表示第k帧的关键帧中的接收机钟差；

表示十元空间；

求解该状态量等同于解决优化方程，具体如下所示：

其中，

表示所有关键帧状态的集合的后验状态；

N表示视觉关键帧个数；

表示在第i帧观察到的特征点集合，在局部坐标系下表示；

K表示包含卫星的关键帧个数；

M_k表示在第k帧内观察到的卫星个数。

本发明提供的卫星与视觉紧耦合定位系统，可以通过本发明给的卫星与视觉紧耦合定位方法的步骤流程实现。本领域技术人员可以将所述卫星与视觉紧耦合定位方法，理解为所述卫星与视觉紧耦合定位系统的一个优选例。

根据本发明提供的一种卫星与视觉紧耦合定位系统，包括：

融合系统初始化模块：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；

融合系统异步跟踪模块：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；

融合系统联合优化模块：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

优选地，所述融合系统初始化模块：

进行融合系统初始化，若初始化成功则通过外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态，记为视觉地图与卫星定位坐标系相对位姿信息，进入融合系统异步跟踪模块继续执行；若初始化不成功，则继续初始化直到初始化成功；

判断所接收到的传感器数据类型：若为卫星伪距数据，则对其进行单点定位解算，调用融合系统异步跟踪模块；若为图像数据，则对其进行标准视觉SLAM初始化流程，调用融合系统异步跟踪模块；

所述外部器件包括：陀螺仪、磁力计。

优选地，所述融合系统初始化模块：

所述获取视觉地图与卫星定位坐标系相对位姿信息指：

导航坐标系包括：地心地固ECEF坐标系本地地理坐标系由SLAM系统在建立初始地图时创建的局部地图坐标系以及移动载体坐标系

导航坐标系之间的转换关系通过如下公式表示：

其中，

p^B为在载体坐标系下的一三维点，为此点对应在世界坐标系下的齐次表示；

表示本地地理坐标系至世界坐标系的变换关系；

表示局部坐标系至本地地理坐标系的变换关系；

表示载体坐标系至局部坐标系的变换关系；

表示在载体坐标系下的三维点位置；

表示本地地理坐标系至世界坐标系的旋转关系；

t_o表示表示本地地理坐标系在世界坐标系下的原点；

表示局部坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至局部坐标系的旋转关系；

表示载体坐标系至局部坐标系的平移关系；

通过分析该方程，得到在导航坐标系下所关心的基础运动分解变量，如以下公式所示：

其中，

表示载体坐标系至本地地理坐标系的旋转关系；

表示载体坐标系至世界坐标系的平移关系；

与这两个变量为在实际应用中最为关心的两个状态量，分别为载体坐标系至本地地理坐标系之间的姿态转换与载体坐标与地心地固坐标系之间的平动转换，由公式可以看出，主要由局部地图坐标系至本地地理坐标系的旋转变换与载体坐标系至局部地图坐标系之间的旋转变换组成；

则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点t_o组成；

当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始t_o值。

优选地，所述融合系统异步跟踪模块：

卫星及视觉联合异步跟踪模块：

若为卫星伪距数据，则对其进行单点定位解算：

对于卫星系统，其状态量为x_GNSS＝[(t_rel+t_o)^Tδ]^T

其中，

x_GNSS表示卫星定位系统的状态量；

t_rel表示相对位置；

·^T表示矩阵的转置；

δ表示接收机钟差；

其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点t_o进行更新。求解卫星系统状态量等同于求解以下优化问题：

表示卫星接收机的后验状态量；

表示在j时刻的卫星测量方程；

若为视觉数据，则对其进行标准视觉SLAM初始化流程：

视觉系统的状态量为在李群切空间下的投影即

其中，

x_Cam表示视觉系统的状态量；

表示视觉系统的状态量在李群切空间下的投影；

·^∨表示反对称矩阵转换；

求解视觉系统的状态量等效于求解以下优化问题：

其中，

表示视觉系统的后验状态量；

表示视觉传感器在第i帧的测量方程；

异构关键帧确定模块：

若为图像数据，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信：若可信，则创建包含视觉数据及卫星伪距信息的关键帧；否则，则创建只有视觉数据的关键帧；

若为卫星伪距数据，则根据最小时间差阈值以及视觉特征点数量判断最近的视觉帧是否可信；若是，则创建包含卫星伪距信息及视觉数据的关键帧；否则，则创建只有卫星伪距信息的关键帧；

所述融合系统联合优化模块：

第k帧的关键帧状态量可表示为：

其中，

表示第k帧的关键帧状态量；

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影；

t_ok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点；

δ_k表示第k帧的关键帧中的接收机钟差；

表示十元空间；

求解该状态量等同于解决优化方程，具体如下所示：

其中，

表示所有关键帧状态的集合的后验状态；

N表示视觉关键帧个数；

表示在第i帧观察到的特征点集合，在局部坐标系下表示；

K表示包含卫星的关键帧个数；

M_k表示在第k帧内观察到的卫星个数。

根据本发明提供的一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述中任一项所述的卫星与视觉紧耦合定位方法的步骤。

下面通过优选例，对本发明进行更为具体地说明。

优选例1：

一种基于同步定位与建图(Simultaneous Localization and Maping，SLAM)的卫星与视觉紧耦合定位算法，其中，提供了用以采集周围环境图像的摄像设备；用于接收卫星信号的GNSS接收机；基于SLAM算法架构的卫星与视觉紧耦合定位方法

该方法包括如下步骤：

融合系统初始化：视觉SLAM系统初始化；确定视觉地图与卫星定位坐标系相对位姿；

融合系统异步跟踪：根据不同传感器数据，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪；确定关键帧并建立局部地图。

融合系统联合优化：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

融合系统初始化时，会判断所接收到的传感器数据类型：若为卫星伪距数据，则尝试对其进行单点定位解算；若为图像数据，则对其进行标准视觉SLAM初始化流程。若初始化成功则通过陀螺仪或磁力计等外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态；

若初始化不成功，系统会继续初始化直到成功。

采用了先对载***移进行坐标系相关的拆分，使用视觉SLAM对其中的相对位移进行更新；继而使用相对位移与相对位移坐标原点重新合成在地球坐标系下的坐标位置作为卫星定位解算的初始值。

在跟踪阶段创建关键帧时，使用如下判定条件：若视觉SLAM系统需要建立关键帧，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信。可信则创建包含视觉及卫星伪距信息的关键帧，反之则创建只有视觉数据的关键帧。另外地，若有新的卫星单点跟踪解算完成，则根据最小时间差阈值以及视觉特征点数量判断是否需要加入视觉帧数据，反之则创建没有任何视觉数据的纯卫星关键帧。

在卫星-视觉数据融合的处理方法中，卫星作为具有唯一识别码的点状路标，可以作为特殊的特征点加入视觉SLAM系统中的优化求解过程。

在视觉-卫星联合优化解算时，将系统状态量设定为局部地图所涉及的所有关键帧中的局部坐标系中的位置与姿态、局部坐标系全局原点以及接收机钟差。将卫星伪距测量与视觉特征点投影测量方程分别对上述系统状态量在其三维流形切空间上进行求导以得到其优化下降方向。

优选例2：

以下将结合图1至图7对本发明提供的一种基于SLAM的卫星与视觉紧耦合定位算法进行详细的描述，其为本发明一可选的实施例，可以认为，本领域的技术人员在不改变本发明精神和内容的范围内能够对其进行修改和润色。

本实施例是传统融合算法的改进以及传统SLAM算法的拓展，其提供了一种基于SLAM算法架构的卫星与视觉紧耦合定位方法，其中，提供了用以采集周围环境图像的摄像设备；用于接收卫星信号的GNSS接收机；基于SLAM算法架构的卫星与视觉紧耦合定位方法；

主要过程通过C++来实现，该方法详细实施框图如图2所示，包括如下步骤：

融合系统初始化：视觉SLAM系统初始化；确定视觉地图与卫星定位坐标系相对位姿；

融合系统异步跟踪：根据不同传感器数据，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪；确定关键帧并建立局部地图。

融合系统联合优化：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

其中所述的状态量可重新组合与映射为在全局绝对坐标系或局部相对坐标系下的位置与姿态。至于如何在局部相对坐标系下进行视觉特征点的跟踪，进而实现三维的地图构建以及具体的定位；以及如何单独对卫星伪距信号进行单点解算，在本领域都已有诸多文献有所记载，本实施例仅就与现有技术的区别部分进行详细阐述，故而不应因为本发明和实施例中未记载现有技术中已有的内容而认为本发明未充分公开，换言之，本领域的技术人员在将现有技术与本发明的技术方案结合的情况下，完全可以实现本发明的技术手段和技术效果。

融合系统初始化时，会判断所接收到的传感器数据类型：若为卫星伪距数据，则尝试对其进行单点定位解算；若为图像数据，则对其进行标准视觉SLAM初始化流程。若初始化成功则通过陀螺仪或磁力计等外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态。

在完成系统初始化后，系统进入传感器异步跟踪以及联合优化解算的两个并行阶段。在进行传感器定位跟踪阶段，由于不同传感器所观测的测量量不同，如何最大化利用每种传感器的信息进行传感器定位跟踪成为异构传感器融合的一大挑战。在本方法中，本发明创造性地发现到卫星定位的解算数据包含了了局部位移与局部位移原点相对于地球坐标系的位置，与此同时视觉SLAM的定位结果则仅仅包含局部定位的结果。由于视觉SLAM在局部相对位移的定位结果上有着原告与卫星伪距定位解算的精度，因此本方法中采用了先对载***移进行坐标系相关的拆分，使用视觉SLAM对其中的相对位移进行更新；继而使用相对位移与相对位移坐标原点重新合成在地球坐标系下的坐标位置作为卫星定位解算的初始值。由于卫星定位跟踪算法选择信任视觉SLAM的相对定位结果，因而在跟踪阶段只更新相对定位的坐标原点。

在跟踪阶段创建关键帧时，使用如下判定条件：若视觉SLAM系统需要建立关键帧，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信。可信则创建包含视觉及卫星伪距信息的关键帧，反之则创建只有视觉数据的关键帧。另外地，若有新的卫星单点跟踪解算完成，则根据最小时间差阈值以及视觉特征点数量判断是否需要加入视觉帧数据，反之则创建没有任何视觉数据的纯卫星关键帧。

本发明创造性地发现到，在卫星-视觉数据融合的处理方法中，卫星作为具有唯一识别码的点状路标，可以作为特殊的特征点加入视觉SLAM系统中的优化求解过程。在视觉-卫星联合优化解算时，将系统状态量设定为局部地图所涉及的所有关键帧中的局部坐标系中的位置与姿态、局部坐标系全局原点以及接收机钟差。将卫星伪距测量与视觉特征点投影测量方程分别对上述系统状态量在其三维流形切空间上进行求导以得到其优化下降方向。同时，本发明创造性地利用了基于图优化的卫星伪距解算方法以将卫星位置解算与图像视觉定位所利用的SLAM算法进行融合统一。

步骤一、系统初始化及针对不同导航坐标系的运动分解

如图3所示，主要导航坐标系有四个，分别为地心地固(Earth-Centered Earth-Fixed,ECEF)坐标系本地地理坐标系例如东北天(East-North-Up,ENU)坐标系；局部地图坐标系由SLAM系统在建立初始地图时创建；以及移动载体坐标系进一步地，在同一个载体坐标系下，若载体搭载了多种传感器，则第i个传感器的坐标系与载体坐标系之间的关系如图4所示。

这些坐标系之间的转换关系可通过公式(1)表示。其中p^B为在载体坐标系下的一三维点，为此点对应在世界坐标系下的齐次表示。

(1)

其中，

为此点对应在世界坐标系下的齐次表示

表示本地地理坐标系至世界坐标系的变换关系

表示局部坐标系至本地地理坐标系的变换关系

表示载体坐标系至局部坐标系的变换关系

表示在载体坐标系下的三维点位置

表示本地地理坐标系至世界坐标系的旋转关系

t_o表示表示本地地理坐标系在世界坐标系下的原点

表示局部坐标系至本地地理坐标系的旋转关系

表示载体坐标系至局部坐标系的旋转关系

表示载体坐标系至局部坐标系的平移关系

通过分析该方程，我们可以得到在导航坐标系下所关心的基础运动分解变量，如公式(2)所示。其中与这两个变量为在实际应用中最为关心的两个状态量，分别为载体坐标系至本地地理坐标系之间的姿态转换与载体坐标与地心地固坐标系之间的平动转换。由公式(2)可以看出，主要由局部地图坐标系至本地地理坐标系的旋转变换与载体坐标系至局部地图坐标系之间的旋转变换组成；则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点to组成。

其中，

表示载体坐标系至本地地理坐标系的旋转关系

表示载体坐标系至世界坐标系的平移关系

在完成对位姿的分解后，不同传感器对系统状态量之间的观测关系由图5给出。

当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始t_o值。

步骤二、卫星-视觉联合异步跟踪

在传感器跟踪更新阶段，本方法针对不同的传感器，进行了异步但数据互补的联合解算跟踪，其具体流程图如图6所示。其中，视觉系统的状态量为在李群切空间下的投影即

其中，

x_Cam表示视觉系统的状态量

表示视觉系统的状态量在李群切空间下的投影；

·^∨表示反对称矩阵转换

求解视觉系统的状态量等效于求解以下优化问题：

其中，

表示视觉系统的后验状态量

表示视觉传感器在第i帧的测量方程

对于卫星系统，其状态量为x_GNSS＝[(t_rel+t_o)^Tδ]^T (5)

其中，

x_GNSS表示卫星定位系统的状态量

t_erl表示相对位置

·^T表示矩阵的转置

δ表示接收机钟差

其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点t_o进行更新。求解卫星系统状态量等同于求解以下优化问题：

表示卫星接收机的后验状态量

表示在j时刻的卫星测量方程

步骤三、异构关键帧确定

在跟踪阶段创建关键帧时，使用如图7所示条件：若视觉SLAM系统需要建立关键帧，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信。可信则创建包含视觉及卫星伪距信息的关键帧，反之则创建只有视觉数据的关键帧。另外地，若有新的卫星单点跟踪解算完成，则根据最小时间差阈值以及视觉特征点数量判断是否需要加入视觉帧数据，反之则创建没有任何视觉数据的纯卫星关键帧。

步骤四、卫星-视觉联合优化

在视觉-卫星联合优化解算时，系统状态量为局部地图所涉及的所有关键帧中的局部坐标系中的位置与姿态、局部坐标系全局原点以及接收机钟差。其中第k帧的关键帧状态量可表示为

其中，

表示第k帧的关键帧状态量

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影

t_ok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点

δ_k表示第k帧的关键帧中的接收机钟差

表示十元空间

求解该状态量等同于解决优化方程

其中，

表示所有关键帧状态的集合的后验状态

N表示视觉关键帧个数

表示在第i帧观察到的特征点集合，在局部坐标系下表示。

K表示包含卫星的关键帧个数

M_k表示在第k帧内观察到的卫星个数

求解过程涉及将卫星伪距测量与视觉特征点投影测量方程分别对上述系统状态量在其三维流形切空间上进行求导以得到其优化下降方向。

我们利用以上步骤对一组在复杂的近楼宇场景数据中进行实验，对比我们的方法和单独传感器的方法，结果如图8。图8是结果本文所提出的融合定位算法与其它传感器定位方法的比较。红色轨迹代表本文所示方法结果，绿色轨迹代表纯视觉SLAM(ORB-SLAM2)的结果，蓝色轨迹为卫星定位结果，黑色轨迹为差分RTK卫星定位结果作为参考。由于测试轨迹处于建筑峡谷内，因此卫星定位结果有着非常明显的漂移。继而，由于测试场景后半段场景较为开阔，因此单纯视觉SLAM出现了明显的尺度漂移。而本文所提出的融合方法可以使用在卫星定位收到建筑等遮挡干扰时明显提高定位精度与稳定性；反之，在场景开阔区域使用相对准确的卫星定位系统修正视觉定位系统产生的尺度漂移问题。由于其效果如图8所示。

优选例3：

本发明所解决的技术问题是提供一种可以在SLAM架构基础上融合卫星与视觉信息进行联合定位的方法，同时使得定位结果在复杂场景下的提升精度及稳定性，确保在环境变化情境下的定位无缝切换能力。

为了解决这一技术问题，本发明提供了一种基于SLAM架构下的视觉-卫星联合优化定位方法，其中包含了：基于导航坐标系分解的异构异步传感器定位数据更新算法；SLAM架构下异构传感器关键帧合成与选择算法；基于SLAM算法结构下的视觉-卫星联合优化定位算法。

该方法包括以下步骤：

融合系统初始化：视觉SLAM系统初始化；确定视觉地图与卫星定位坐标系相对位姿；

融合系统异步跟踪：根据不同传感器数据，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪；确定关键帧并建立局部地图。

融合系统联合优化：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。

融合系统初始化时，会判断所接收到的传感器数据类型：若为卫星伪距数据，则尝试对其进行单点定位解算；若为图像数据，则对其进行标准视觉SLAM初始化流程。若初始化成功则通过陀螺仪或磁力计等外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态。

在完成系统初始化后，系统进入传感器异步跟踪以及联合优化解算的两个并行阶段。在进行传感器定位跟踪阶段，由于不同传感器所观测的测量量不同，如何最大化利用每种传感器的信息进行传感器定位跟踪成为异构传感器融合的一大挑战。

在本方法中，本发明创造性地发现到卫星定位的解算数据包含了了局部位移与局部位移原点相对于地球坐标系的位置，与此同时视觉SLAM的定位结果则仅仅包含局部定位的结果。由于视觉SLAM在局部相对位移的定位结果上有着远高于卫星伪距定位解算的精度，因此本方法中采用了先对载***移进行坐标系相关的拆分，使用视觉SLAM对其中的相对位移进行更新；继而使用相对位移与相对位移坐标原点重新合成在地球坐标系下的坐标位置作为卫星定位解算的初始值。由于卫星定位跟踪算法选择信任视觉SLAM的相对定位结果，因而在跟踪阶段只更新相对定位的坐标原点。

在跟踪阶段创建关键帧时，使用如下判定条件：若视觉SLAM系统需要建立关键帧，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信。可信则创建包含视觉及卫星伪距信息的关键帧，反之则创建只有视觉数据的关键帧。另外地，若有新的卫星单点跟踪解算完成，则根据最小时间差阈值以及视觉特征点数量判断是否需要加入视觉帧数据，反之则创建没有任何视觉数据的纯卫星关键帧。

本发明创造性地发现到，在卫星-视觉数据融合的处理方法中，卫星作为具有唯一识别码的点状路标，可以作为特殊的特征点加入视觉SLAM系统中的优化求解过程。在视觉-卫星联合优化解算时，将系统状态量设定为局部地图所涉及的所有关键帧中的局部坐标系中的位置与姿态、局部坐标系全局原点以及接收机钟差。将卫星伪距测量与视觉特征点投影测量方程分别对上述系统状态量在其三维流形切空间上进行求导以得到其优化下降方向。同时，本发明创造性地利用了基于图优化的卫星伪距解算方法以将卫星位置解算与图像视觉定位所利用的SLAM算法进行融合统一。

在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。