CN115979258A - 一种实时高精度定位定姿方法、装置与系统 - Google Patents

Abstract

本发明公开了一种大范围实时高精度定位定姿方法、装置与系统，涉及增强现实技术领域。该高精度定位定姿方法，包括采用高精度GNSS授时同步IMU和双目相机的时间，提高时间同步精度；对GNSS、IMU和双目相机等传感器进行事先联合标定，获得不同传感器之间的标定参数；利用GNSS RTK获取实时高精度绝对坐标信息，同时通过GNSS双天线解算，获得实时真北方位以及双目相机的实时绝对姿态。本发明利用GNSS RTK和双天线解算，能实时获取高精度的绝对位置和姿态信息，解决了大范围增强现实应用中难以准确确定绝对位置、姿态的问题；并且融合多传感器数据信息，提高了复杂环境下实时定位定姿的精度、稳定性和可靠性。

Description

一种实时高精度定位定姿方法、装置与系统

技术领域

本发明涉及增强现实定位定姿及导航定位测绘技术领域，尤其涉及一种实时高精度定位定姿方法、装置与系统。

背景技术

目前大范围增强现实(Augmented Reality，AR)应用广泛，AR技术是一种将虚拟信息与真实世界巧妙融合的技术，广泛运用了多媒体、三维建模、实时跟踪、智能交互、传感等多种技术手段，将计算机生成的文字、图像、三维模型、音乐、视频等虚拟信息模拟仿真后，应用到真实世界中，两种信息互为补充，从而实现对真实世界的“增强”。实时高精度定位定姿是AR技术核心关键技术之一，目前AR应用中，多采用视觉定位方法确定设备的位姿，但是这需要提前构建视觉地图，这对于室外大范围场景成本较高，而且视觉地图一般是相对的，与真实的地理位置没有关系，因此，需要提供一种实时高精度绝对定位定姿方法。

针对大范围AR定位定姿方法，公开号为CN110988947B的专利提出利用RTK提供高精度的绝对位置信息，融合视觉定位方法，该方法实现了大范围的高精度定位问题，也与真实地理位置想关联，但是没有提供绝对的真北方位姿态信息。公开号为CN114723924A的专利提出利用地图约束的方法提供真北方位，一方面该方法的精度难以保障，同时其没有利用高精度的RTK定位信息。上述方法没有较好地解决大范围AR应用对实时高精度定位定姿的需求。

发明内容

本申请实施例通过提供一种实时高精度定位定姿方法、装置与系统，融合GNSSRTK、GNSS双天线定向、IMU以及双目SLAM技术，解决了大范围AR应用时实时高精度定位定姿问题；同时采用双目SLAM，比单目SLAM具有更高的精度和可靠性。

本申请实施例提供了一种高精度定位定姿方法，包括以下步骤：

S1、利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

S2、统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

S3、融合算法和策略，分别对GNSS RTK，GNSS双天线定向、IMU和双目相机的精度进行评估，获得权比关系。

进一步的，所述S1中对IMU进行时间同步的控制包括以下步骤：

S101、GNSS接收同步时间控制指令后，向IMU发送串口数据形式的初始时间，并同时在IMU中标记粗时间的时间戳；

S102、IMU中的细时间计时器以粗时间的时间戳为起始点进行计时；

S103、GNSS持续定时向IMU发送脉冲信号；

S104、IMU接收到S103中发送的脉冲信号后，IMU中的细时间计时器归零并重新计时。

进一步的，所述S1中利用GNSS对双目相机进行时间同步的过程具体包括以下步骤：

S111、双目相机接收到时间同步控制指令后，向GNSS发送曝光信号，并同时接收GNSS上的时间戳；

S112、获得时间戳后，实现曝光时间与GNSS时间参考基准一致，完成时间同步。

进一步的，所述S2实现标定的具体过程包括以下步骤：

S21、获取双目相机轨迹，IMU轨迹以及GNSS轨迹；

S22、对双目相机，IMU以及GNSS传感器的轨迹进行时间同步；

S23、利用时间同步后的轨迹获得双目相机和IMU的联合外参以及GNSS天线和IMU的杆臂；

S24、根据新获得的联合外参和杆臂对原有的联合外参和杆臂进行更新。

进一步的，所述S21中获取双目相机轨迹的过程包括以下步骤：

S211、获取图像信息中的ORB特征；

S212、根据获得的ORB特征将左眼图像和右眼图像进行双目立体匹配，获得立体关键点；

S213、将立体关键点输入到视觉SLAM系统中，推算出双目相机的轨迹。

进一步的，所述S21中获得IMU轨迹的过程包括以下步骤：

S2101、获取速度，位置以及姿态矩阵的初始值；

S2102、获取IMU姿态信息；

S2103、对加速度计输出的比力信息进行坐标变换，将IMU坐标系下的比力信息变换到当地水平坐标系下；

S2104、坐标变换后的比力信息经二次积分得到位置信息，即IMU轨迹。

进一步的，所述S3中获得权比关系的过程包括以下步骤：

S31、联合双目相机和IMU获取被定位对象的第一位姿数据；

S32、联合IMU和GNSS获得被定位对象的第二位姿数据；

S33、根据GNSS信号的质量确定权重值。

进一步的、所述S33中获得权重值的过程包括以下步骤：

S331、获取卫星测得的被定位对象的第三位姿数据；

S332、根据第三位姿数据获得伪距残差平方和，并根据伪距残差平方和获得最小二乘残差统计量和检测门限值；

S333、对比最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏；

S334、根据GNSS信号的质量好坏的判断结果，确定权重值。

本申请实施例提供了一种高精度定位定姿装置，时间同步模块，坐标系统一模块和融合模块，其中：

所述时间同步模块，用于利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

所述坐标系统一模块，用于统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

所述融合模块，用于融合算法和策略，分别对GNSS RTK，IMU和双目相机的精度进行评估，获得权比关系。

本申请实施例提供了一种高精度定位定姿系统，包括时间同步控制模块，轨迹推算模块，联合标定模块和评估模块，其中：

时间同步控制模块，用来接收时间同步控制指令，利用GNSS时间对IMU和双目相机确定时间；

轨迹推算模块，用来根据双目相机输出的信息推算双目相机轨迹，对IMU输出的信息进行定位解算，推算IMU轨迹，将GNSS传感器输出的信息转换到当地水平坐标系，得GNSS轨迹；

联合标定模块，用来利用时间同步后的轨迹求解相机和IMU的联合外参以及GNSS天线和IMU的杆臂；

评估模块，用来根据GNSS信号的质量好坏的判断结果，确定权重值。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

采用高精度的GNSS授时方法，其精度可以达到几十ns，针对低速场景，时间精度可以满足应用场景，便于进行同步时间，统一采用地理坐标系，实现对GNSS、IMU和双目相机的坐标系的统一，进行事先标定，融合简单、可靠性高，定位信息准确，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度，解决了大范围AR应用时实时高精度定位定姿问题；同时采用双目SLAM，比单目SLAM具有更高的精度和可靠性。

附图说明

图1为本申请实施例一提供的高精度定位定姿方法流程图；

图2为本申请实施例二提供的高精度定位定姿装置结构图；

图3为本申请实施例三提供的高精度定位定姿系统结构示意图。

具体实施方式

本申请实施例通过提供一种高精度定位定姿方法、装置与系统，解决了GNSS/IMU融合定位技术不适用于对GNSS信号具有长时间遮挡作用的复杂环境中的问题，实现了在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度。

实施例一

如图1所示，为本申请实施例提供的高精度定位定姿方法流程图，该方法包括：

S1、利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

S2、统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

S3、融合算法和策略，分别对GNSS RTK，IMU和双目相机的精度进行评估，获得权比关系。

进一步的，本申请实施例提供的方法，可以在视频处理中实现提升检索速度，同时保证了识别的准确性，具体步骤如下：

所述S1中对IMU进行时间同步的控制包括以下步骤：

S101、GNSS接收同步时间控制指令后，向IMU发送串口数据形式的初始时间，并同时在IMU中标记粗时间的时间戳；

S102、IMU中的细时间计时器以粗时间的时间戳为起始点进行计时；

S103、GNSS持续定时向IMU发送脉冲信号；

S104、IMU接收到S103中发送的脉冲信号后，IMU中的细时间计时器归零并重新计时；

进一步的，所述S1中利用GNSS对双目相机进行时间同步的过程具体包括以下步骤：

S111、双目相机接收到时间同步控制指令后，向GNSS发送曝光信号，并同时接收GNSS上的时间戳；

S112、获得时间戳后，实现曝光时间与GNSS时间参考基准一致，完成时间同步。

在本实施例中，GNSS中的指令接收器接收控制系统发送的同步时间指令后，向IMU发送UTC初始时间，该初始时间以串口数据的形式发送，同时在IMU中标记UTC粗时间的时间戳，随后IMU中的细时间计时器以粗时间的时间戳为起始点进行计时，GNSS中的指令接收器也会定时向IMU发送pps脉冲信号，以此来减少扫描仪的累计误差，IMU在接收到脉冲信号后，其细时间计时器会归零并重新计时，使IMU的时间与GNSS中的指令接收器的时间一致；

控制系统向GNSS中的指令接收器的同时也会向双目相机发送同步时间指令，两者是同时进行的。

进一步的，所述S2实现标定的具体过程包括以下步骤：

S21、获取双目相机轨迹，IMU轨迹以及GNSS轨迹，将GNSS传感器输出的位置信息转换到当地水平坐标系下，得GNSS轨迹；

S22、对双目相机，IMU以及GNSS传感器的轨迹进行时间同步；

S23、利用时间同步后的轨迹获得双目相机和IMU的联合外参以及GNSS天线和IMU的杆臂；

S24、根据新获得的联合外参和杆臂对原有的联合外参和杆臂进行更新。

在本实施例中，对双目相机，IMU以及GNSS传感器的轨迹进行时间同步同步后，利用同步后的轨迹获取双目相机和IMU的联合外参以及GNSS天线和IMU的杆臂，从而更新原有的联合外参和杆臂，解决运动过程中因碰撞、振动等因素造成的双目相机、IMU和GNSS接收机位置关系的改变问题。

进一步的，所述S21中获取双目相机轨迹的过程包括以下步骤：

S211、获取图像信息中的ORB特征；

S212、根据获得的ORB特征将左眼图像和右眼图像进行双目立体匹配，获得立体关键点；

S213、将立体关键点输入到视觉SLAM系统中，推算出双目相机的轨迹。

在本实施例中，先利用双目相机的内参对双目相机输出的原始图像进行去畸变和双目矫正，将矫正后的图像输入到视觉SLAM系统中就可以得到双目相机的轨迹，原始图像包括左眼图像和右眼图像，将原始图像信息的三位空间点投影到归一化图像平面，对归一化图像平面上的点去畸变，再将去畸变的点通过双目相机的内参矩阵投影到像素平面上，得到该点在图像上的正确位置；

提取ORB特征可以采用图像金字塔，图像金字塔是计算机视觉中常用的一种处理方法，根据ORB特征将左眼图像和右眼图像进行双目立体匹配，在右眼图像中搜索每个左眼ORB的匹配点，再利用左眼ORB的坐标和其右眼匹配点的水平坐标生成立体关键点，根据立体关键点推算双目相机轨迹。

进一步的，所述S21中获得IMU轨迹的过程包括以下步骤：

S2101、获取速度，位置以及姿态矩阵的初始值；

S2102、获取IMU姿态信息；

S2103、对加速度计输出的比力信息进行坐标变换，将IMU坐标系下的比力信息变换到当地水平坐标系下；

S2104、坐标变换后的比力信息经二次积分得到位置信息，即IMU轨迹

在本实施例中，通过对IMU输出的信息进行定位解算，推算IMU轨迹，初始对准，获取速度、位置和姿态矩阵的初值，利用陀螺仪输出数据对IMU做姿态解算并更新，得到IMU的姿态信息，对加速度计输出的比力信息做坐标变换，将IMU坐标系下的比力信息变换到当地水平坐标系下。当地水平坐标系又称之为地理坐标系，坐标原点为GNSS接收机初始位置，X轴指东，Y轴指北，Z轴指天顶，：坐标变换后的比力信息经一次积分得到速度信息，二次积分得到位置信息，位置信息即IMU轨迹。

进一步的，所述S3中获得权比关系的过程包括以下步骤：

S31、联合双目相机和IMU获取被定位对象的第一位姿数据；

S32、联合IMU和GNSS获得被定位对象的第二位姿数据；

S33、根据GNSS信号的质量确定权重值。

在本实施例中，将GNSS，IMU以及双目相机进行融合，根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值，先分别结合视觉相机和IMU模块得到第一位姿数据、结合IMU模块和GNSS模块得到第二位姿数据，再根据GNSS信号的质量好坏确定第一位姿数据和第二位姿数据融合时的权重值，最后对第一位姿数据和第二位姿数据进行加权融合计算得到最终的位姿数据，在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果。

进一步的，所述S33中获得权重值的过程包括以下步骤：

S331、获取卫星测得的被定位对象的第三位姿数据；

S332、根据第三位姿数据获得伪距残差平方和，并根据伪距残差平方和获得最小二乘残差统计量和检测门限值；

S333、对比最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏；

S334、根据GNSS信号的质量好坏的判断结果，确定权重值。

在本实施例中，获取卫星定位系统中各个卫星测得的被定位对象的第三位姿数据，根据第三位姿数据获得伪距残差平方和，并根据伪距残差平方和获得最小二乘残差统计量和检测门限值，对比最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏，最后根据GNSS信号的质量好坏的判断结果，确定权重值。

上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：

采用高精度的GNSS授时方法，其精度可以达到几十ns，针对低速场景，时间精度可以满足应用场景，便于进行同步时间，统一采用地理坐标系，实现对GNSS、IMU和双目相机的坐标系的统一，进行事先标定，融合简单、可靠性高，定位信息准确。

实施例二

如图2所示，为本申请实施例提供的高精度定位定姿装置结构图，本申请实施例提供的高精度定位定姿装置包括时间同步模块，坐标系统一模块和融合模块，其中：

所述时间同步模块，用于利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

所述坐标系统一模块，用于统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

所述融合模块，用于融合算法和策略，分别对GNSS RTK，IMU和双目相机的精度进行评估，获得权比关系。

上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：

采用高精度的GNSS授时方法，其精度可以达到几十ns，针对低速场景，时间精度可以满足应用场景，便于进行同步时间，统一采用地理坐标系，实现对GNSS、IMU和双目相机的坐标系的统一，进行事先标定，融合简单、可靠性高，定位信息准确。

实施例三

如图3所示，为本申请实施例提供的高精度定位定姿系统的结构示意图，包括时间同步控制模块，轨迹推算模块，联合标定模块和评估模块，其中：

时间同步控制模块，用来接收时间同步控制指令，利用GNSS时间对IMU和双目相机确定时间；

轨迹推算模块，用来根据双目相机输出的信息推算双目相机轨迹，对IMU输出的信息进行定位解算，推算IMU轨迹，将GNSS传感器输出的信息转换到当地水平坐标系，得GNSS轨迹；

联合标定模块，用来利用时间同步后的轨迹求解相机和IMU的联合外参以及GNSS天线和IMU的杆臂；

评估模块，用来根据GNSS信号的质量好坏的判断结果，确定权重值。

在本实施例中，时间同步控制模块中的控制系统发送时间同步指令后，GNSS授时，对IMU和双目相机进行时间同步，利用双目相机轨迹，IMU轨迹以及GNSS轨迹进行同步，利用时间同步后的轨迹求解相机和IMU的联合外参以及GNSS天线和IMU的杆臂，从而对被测物体进行标定，根据GNSS信号的好坏确定权重，依次来确定被测物体的定位信息。

上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：

采用高精度的GNSS授时方法，其精度可以达到几十ns，针对低速场景，时间精度可以满足应用场景，便于进行同步时间，统一采用地理坐标系，实现对GNSS、IMU和双目相机的坐标系的统一，进行事先标定，融合简单、可靠性高，定位信息准确。

本申请实施例还提供的一种存储介质，存储介质上存储有处理器执行时实现高精度定位定姿方法的程序。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种高精度定位定姿方法，其特征在于，包括以下步骤：

S1、利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

S2、统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

S3、融合算法和策略，分别对GNSS RTK，IMU和双目相机的精度进行评估，获得权比关系。

2.如权利要求1所述高精度定位定姿方法，其特征在于：所述S1中对IMU进行时间同步的控制包括以下步骤：

S101、GNSS接收同步时间控制指令后，向IMU发送串口数据形式的初始时间，并同时在IMU中标记粗时间的时间戳；

S102、IMU中的细时间计时器以粗时间的时间戳为起始点进行计时；

S103、GNSS持续定时向IMU发送脉冲信号；

S104、IMU接收到S103中发送的脉冲信号后，IMU中的细时间计时器归零并重新计时。

3.如权利要求1所述高精度定位定姿方法，其特征在于：所述S1中利用GNSS对双目相机进行时间同步的过程具体包括以下步骤：

S111、双目相机接收到时间同步控制指令后，向GNSS发送曝光信号，并同时接收GNSS上的时间戳；

S112、获得时间戳后，实现曝光时间与GNSS时间参考基准一致，完成时间同步。

4.如权利要求1所述高精度定位定姿方法，其特征在于：所述S2实现标定的具体过程包括以下步骤：

S21、获取双目相机轨迹，IMU轨迹以及GNSS轨迹；

S22、对双目相机，IMU以及GNSS传感器的轨迹进行时间同步；

S23、利用时间同步后的轨迹获得双目相机和IMU的联合外参以及CNGG天线和IMU的杆臂；

S24、根据新获得的联合外参和杆臂对原有的联合外参和杆臂进行更新。

5.如权利要求4所述高精度定位定姿方法，其特征在于：所述S21中获取双目相机轨迹的过程包括以下步骤：

S211、获取图像信息中的ORB特征；

S212、根据获得的ORB特征将左眼图像和右眼图像进行双目立体匹配，获得立体关键点；

S213、将立体关键点输入到视觉SLAM系统中，推算出双目相机的轨迹。

6.如权利要求4所述高精度定位定姿方法，其特征在于：所述S21中获得IMU轨迹的过程包括以下步骤：

S2101、获取速度，位置以及姿态矩阵的初始值；

S2102、获取IMU姿态信息；

S2103、对加速度计输出的比力信息进行坐标变换，将IMU坐标系下的比力信息变换到当地水平坐标系下；

S2104、坐标变换后的比力信息经二次积分得到位置信息，即IMU轨迹。

7.如权利要求1所述高精度定位定姿方法，其特征在于：所述S3中获得权比关系的过程包括以下步骤：

S31、联合双目相机和IMU获取被定位对象的第一位姿数据；

S32、联合IMU和GNSS获得被定位对象的第二位姿数据；

S33、根据GNSS信号的质量确定权重值。

8.如权利要求7所述高精度定位定姿方法，其特征在于：所述S33中获得权重值的过程包括以下步骤：

S331、获取卫星测得的被定位对象的第三位姿数据；

S332、根据第三位姿数据获得伪距残差平方和，并根据伪距残差平方和获得最小二乘残差统计量和检测门限值；

S333、对比最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏；

S334、根据GNSS信号的质量好坏的判断结果，确定权重值。

9.一种高精度定位定姿装置，其特征在于，包括时间同步模块，坐标系统一模块和融合模块，其中：

所述时间同步模块，用于利用GNSS时间对IMU和双目相机确定时间，进行时间同步；

所述坐标系统一模块，用于统一采用地理坐标系，对GNSS，IMU和双目相机的坐标系进行统一，对GNSS、IMU和双目相机进行实现标定；

所述融合模块，用于融合算法和策略，分别对GNSS RTK，IMU和双目相机的精度进行评估，获得权比关系。

10.一种高精度定位定姿系统，其特征在于，包括时间同步控制模块，轨迹推算模块，联合标定模块和评估模块，其中：

时间同步控制模块，用来接收时间同步控制指令，利用GNSS时间对IMU和双目相机确定时间；

轨迹推算模块，用来根据双目相机输出的信息推算双目相机轨迹，对IMU输出的信息进行定位解算，推算IMU轨迹，将GNSS传感器输出的信息转换到当地水平坐标系，得GNSS轨迹；

联合标定模块，用来利用时间同步后的轨迹求解相机和IMU的联合外参以及GNSS天线和IMU的杆臂；

评估模块，用来根据GNSS信号的质量好坏的判断结果，确定权重值。

Images