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

本发明专利技术提供了一种卫星与视觉紧耦合定位方法、系统及介质，包括：融合系统初始化步骤：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。本发明专利技术利用基于导航坐标系分解的异构异步传感器定位数据更新算法，使得即使在室内外复杂环境中可以仍然可以鲁棒精确地进行导航定位。

The method, system and medium of close coupling positioning between satellite and vision

The invention provides a satellite and vision tightly coupled positioning method, system and medium, including: initialization steps of the fusion system: initializing visual SLAM system, obtaining relative posture information of vision map and satellite positioning coordinate system; and tracking asynchronous steps of fusion system: simultaneous interpreting of relative position and orientation information of different visual sensor and satellite positioning coordinate system based on different sensor data. At present, the motion of the carrier is decomposed in the corresponding coordinate system to determine the state quantity, and the state of the sensor is tracked based on the observed state quantity of the sensor, the key frame is determined and the local map is established; the joint optimization steps of the fusion system are: Based on the state quantity obtained after the decomposition of the navigation coordinate system, the joint optimization positioning solution is carried out by combining the visual local map and the satellite pseudo range data Calculate. The invention utilizes the heterogeneous asynchronous sensor positioning data update algorithm based on the navigation coordinate system decomposition, so that the navigation positioning can still be carried out robustly and accurately even in the indoor and outdoor complex environment.

【技术实现步骤摘要】
卫星与视觉紧耦合定位方法、系统及介质
本专利技术涉及定位
，具体地，涉及卫星与视觉紧耦合定位方法、系统及介质。尤其地，涉及一种基于同步定位与建图(SimultaneousLocalizationandMaping，SLAM)的卫星与视觉紧耦合定位算法，可用于自主无人系统在复杂室内外环境下的定位，提高定位精度及可靠性。
技术介绍
当前自主无人系统(包含无人驾驶车辆、自主机器人、微型无人机等)的工作场景随着应用的增长而日趋复杂。单一的定位导航系统，如全球卫星导航系统(GlobalNavigationSatelitteSystem，GNSS)，惯性导航系统(InertialNavigationSystem,INS)，视觉同步定位及建图技术(Vision-basedSimultaneousLocalizationandMaping，V-SLAM)均只能解决部分环境场景以及应用需求。在这样的应用需求与技术背景下，为了最终实现在复杂环境下高精度与鲁棒稳定的载体定位，传感器以及传感方法的融合技术应运而生，旨在不同的场景下发挥每种传感器及其方法的最大作用本文档来自技高网...

【技术保护点】
1.一种卫星与视觉紧耦合定位方法，其特征在于，包括：/n融合系统初始化步骤：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；/n融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；/n融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。/n

【技术特征摘要】
1.一种卫星与视觉紧耦合定位方法，其特征在于，包括：
融合系统初始化步骤：进行视觉SLAM系统初始化，获取视觉地图与卫星定位坐标系相对位姿信息；
融合系统异步跟踪步骤：根据不同传感器数据及获得的视觉地图与卫星定位坐标系相对位姿信息，对当前载体运动进行对应坐标系下的运动分解确定状态量，并基于传感器的所观测的状态量进行传感器状态跟踪，确定关键帧并建立局部地图；
融合系统联合优化步骤：在导航坐标系分解后得到的状态量基础上，结合视觉局部地图与卫星伪距数据进行联合优化定位解算。


2.根据权利要求1所述的卫星与视觉紧耦合定位方法，其特征在于，所述融合系统初始化步骤：
进行融合系统初始化，若初始化成功则通过外部器件记录当前视觉定位系统地图原点相对于当地地理坐标系的姿态，记为视觉地图与卫星定位坐标系相对位姿信息，进入融合系统异步跟踪步骤继续执行；若初始化不成功，则继续初始化直到初始化成功；
判断所接收到的传感器数据类型：若为卫星伪距数据，则对其进行单点定位解算，进入融合系统异步跟踪步骤继续执行；若为图像数据，则对其进行标准视觉SLAM初始化流程，进入融合系统异步跟踪步骤继续执行；
所述外部器件包括：陀螺仪、磁力计。


3.根据权利要求2所述的卫星与视觉紧耦合定位方法，其特征在于，所述融合系统初始化步骤：
所述获取视觉地图与卫星定位坐标系相对位姿信息指：
导航坐标系包括：地心地固ECEF坐标系本地地理坐标系由SLAM系统在建立初始地图时创建的局部地图坐标系以及移动载体坐标系
导航坐标系之间的转换关系通过如下公式表示：



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

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

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

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

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

表示本地地理坐标系至世界坐标系的旋转关系；
to表示表示本地地理坐标系在世界坐标系下的原点；

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

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

表示载体坐标系至局部坐标系的平移关系；
通过分析该方程，得到在导航坐标系下所关心的基础运动分解变量，如以下公式所示：






其中，

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

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

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

则主要由部地图坐标系至本地地理坐标系的旋转变换载体坐标系至局部地图坐标系之间的旋转变换平移变换以及本地地理坐标系在地心地固全局坐标系下的坐标原点to组成；
当系统初始化时，视觉定位系统按照传统SLAM方式进行初始化，得到初始并通过IMU或其它传感器测量得到卫星定位系统则通过传统伪距解算方法得到初始to值。


4.根据权利要求1所述的卫星与视觉紧耦合定位方法，其特征在于，所述融合系统异步跟踪步骤：
卫星及视觉联合异步跟踪步骤：
若为卫星伪距数据，则对其进行单点定位解算：
对于卫星系统，其状态量为
xGNSS＝[(trel+to)Tδ]T
其中，
xGNSS表示卫星定位系统的状态量；
trel表示相对位置；
.T表示矩阵的转置；
δ表示接收机钟差；
其中相对位置来源于视觉定位系统。在跟踪阶段我们只利用卫星定位系统对系统原点to进行更新。求解卫星系统状态量等同于求解以下优化问题：




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

表示在j时刻的卫星测量方程；
若为视觉数据，则对其进行标准视觉SLAM初始化流程：
视觉系统的状态量为在李群切空间下的投影即



其中，
xCam表示视觉系统的状态量；

表示视觉系统的状态量在李群切空间下的投影；
·∨表示反对称矩阵转换；
求解视觉系统的状态量等效于求解以下优化问题：



其中，

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

表示视觉传感器在第i帧的测量方程；
异构关键帧确定步骤：
若为图像数据，则根据最小时间差阈值以及卫星数量判断距其最近的卫星帧是否可信：若可信，则创建包含视觉数据及卫星伪距信息的关键帧；否则，则创建只有视觉数据的关键帧；
若为卫星伪距数据，则根据最小时间差阈值以及视觉特征点数量判断最近的视觉帧是否可信；若是，则创建包含卫星伪距信息及视觉数据的关键帧；否则，则创建只有卫星伪距信息的关键帧。


5.根据权利要求4所述的卫星与视觉紧耦合定位方法，其特征在于，所述融合系统联合优化步骤：
第k帧的关键帧状态量可表示为：






其中，

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

表示在第k帧的关键帧中视觉系统的状态量为在李群切空间下的投影；
tok表示在第k帧的关键帧中本地地理坐标系在世界坐标系下的原点；
δk表示第k帧的关键帧中的接收机钟差；

表示十元空间；
求解该状态量等同于解决优化方程，具体如下所示：



其中，

表示所有关键帧状态的集合的后验状态；
N表示视觉关键帧个数；

表示在第i帧观察到的特征点集合，在局部坐标系下表示；
K表示包含卫星的关键帧个数；
Mk表示在第k帧内观察到的卫星个数。


6.一种卫星与视觉紧耦合定位...

【专利技术属性】
技术研发人员：龚正，应忍冬，刘佩林，薛午阳，缪瑞航，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：上海;31