(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211512861.9 (22)申请日 2022.11.30 (71)申请人 西北工业大 学 地址 710072 陕西省西安市碑林区友谊西 路127号 (72)发明人 张通　李嘉奇　许建宇　杨韬　 许涛　 (74)专利代理 机构 西安凯多 思知识产权代理事 务所(普通 合伙) 61290 专利代理师 刘新琼 (51)Int.Cl. G01C 21/16(2006.01) G01S 19/45(2010.01) G01S 19/47(2010.01) G01C 21/00(2006.01)G06V 10/44(2022.01) G06T 7/246(2017.01) (54)发明名称 一种基于视觉惯性GNSS紧耦合的无缝定位 方法 (57)摘要 本发明涉及一种基于视觉惯性GNSS紧耦合 的无缝定位方法， 使用SPP算法计算载体初始绝 对位置， 使用多普勒频移与视觉惯 性系统解算出 的载体速度对齐， 获得载体在ENU坐标系下的初 始位置、 姿态和速度。 分别计算IMU残差、 视觉重 投影误差、 伪距残差、 多普勒频移残差和接收机 时钟残差， 将其作为因子加入到优化项中， 优化 求解获得载体的绝对位姿。 提出一种针对GNSS拒 止环境的状态切换策略， 在无可用的GNSS信息 时， 锁定与GNSS相关的状态量并在因子图中隔离 伪距残差因子、 多普勒频移残差因子和接收机时 钟残差因子， 系统退化为视觉惯 性融合的定位方 法。 此方法在GNSS信号弱的拒止环境下能够获得 可靠精确的绝对位姿， 并能在 进出拒止环境时进 行状态的无缝切换。 权利要求书2页 说明书5页 附图1页 CN 115523920 A 2022.12.27 CN 115523920 A 1.一种基于视觉惯性GNSS紧耦合的无缝定位方法， 其特征在于： 实现视觉惯性GNSS紧 耦合并在进入GNS S拒止环境 仍能进行 无缝的绝对位置的获取； 步骤如下： 步骤1： 对IMU信息进行预积分处理， 对视觉图像信息进行特征点的提取与跟踪， 使用星 历信息对GNS S信息进行 可靠性筛 选， 得到可用的传感器信息； 步骤2： 进行初始化， 使用仅视觉的三维运动重建获得载体的相对于初始位置的初始相 对位姿， 进行视觉IMU联合初始化得到IMU加 速度计与陀螺仪的偏置以及重力矢量的对齐； 利用SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置， 利用多普 勒频移与视觉惯性系统得到的相对速度对齐， 进 行视觉惯 性系统与GNSS信息的联合初始 化 得到载体在ENU坐标系下的绝对位姿； 步骤3： 计算视觉重投影误差、 IMU预积分残差和GNSS信息的伪距残差、 多普勒频移残 差、 接收机时钟残差并分别作为因子构建因子图， 构建滑动窗口对其中所有的状态量 进行优化求解， 其中 n为滑动窗口中的帧数， m为滑动 窗口中所有特征点的数量 ， 分别为载体的位 置、 速度和姿态， 为加速度计和陀螺仪的偏置， 为接收机时钟钟差， 为接收机时 钟钟差变化率， 为相机外参， 为特征点的逆深度， 为载体坐标系和ENU坐标系之间 的偏航角； 通过星历信息对卫星GNSS信息进行筛选， 当无可用的GNSS信息时， 锁定与GNSS相 关的状态量并在因子图中隔离 GNSS信息的伪距残差因子、 多普勒频移残差因子和接收机时 钟残差因子， 切换到视觉惯性定位系统解算载体相对位姿并根据有 可用GNSS信息时的先验 信息推算ENU坐标系下的绝对位姿， 待重新接收到可用得GNSS信息时， 再对GNSS相关的状态 量进行解锁， 并加入伪距残差因子、 多普勒频移残差因子和接 收机时钟残差因子进行联合 优化， 令偏航角 、 接收机时钟钟差 、 接收机时钟钟差变化率 继续被各优化项优化 更新。 2.根据权利要求1所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法， 其特征在于： 步骤3所述的伪距 残差因子： 其中 为地球自转角速度， 为卫星信号传输时间， 为地球在卫星信号传输 过程中的旋转， 为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置， 表示接收机的位置， c代表光速， 为接收机时钟钟差， 为卫星时钟误差， 和 分别代表对流层和电流层带来的延迟误差， 为伪距的量测值。 3.根据权利要求1所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法， 其特征在于： 步骤3所述的多普勒频移 残差因子： 权　利　要　求　书 1/2 页 2 CN 115523920 A 2其中 表示载波信号的波长， 表示从接收机到卫星的单位向量， 和 分别表示 卫星和接收机的速度， 表示接收机时钟钟差变化率， 表示卫星时钟误差漂移率， 为多普勒频移量测值。 4.根据权利要求1所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法， 其特征在于： 步骤3所述的接收机时钟残差因子： 其中 和 分别为第 k时刻和第 k‑1时刻的接收机时钟钟差， 为第k‑1时刻的 接收机时钟 钟差变化 率， 为第k时刻和第 k‑1时刻的时间差 。 5.根据权利要求1所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法， 其特征在于： 步骤3所述的联合优化具体如下： 将残差项因子加入到优化项中， 建立优化模型： 其中 表示的是 z里面的第 i项，z表示所有 n个传感器量测信息的集合， r表示残差项； 问题被建模成非线性最小二乘问题， 使用梯度下降法对该优化模型进行求解， 获得载体的 位姿信息 。 6.一种计算机系统， 其特征在于包括： 一个或多个处理器， 计算机可读存储介质， 用于 存储一个或多个程序， 其中， 当所述一个或多个程序被所述一个或多个处理器执行时， 使得 所述一个或多个处 理器实现权利要求1所述的方法。 7.一种计算机可读存储介质， 其特征在于存储有计算机可执行指令， 所述指令在被执 行时用于实现权利要求1所述的方法。权　利　要　求　书 2/2 页 3 CN 115523920 A 3