CN114459506B - 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 - Google Patents
在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 Download PDFInfo
- Publication number
- CN114459506B CN114459506B CN202210191523.3A CN202210191523A CN114459506B CN 114459506 B CN114459506 B CN 114459506B CN 202210191523 A CN202210191523 A CN 202210191523A CN 114459506 B CN114459506 B CN 114459506B
- Authority
- CN
- China
- Prior art keywords
- navigation satellite
- coordinate system
- global navigation
- receiver
- satellite system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
- G01S19/235—Calibration of receiver components
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法与系统，方法包括：获取全球导航卫星系统接收机与视觉惯性里程计外参的初始值，以及全球导航卫星系统初始化得到的局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；根据初始值计算滑动窗口中每一时刻的全球导航卫星系统接收机的天线在地心地固坐标系下的位置；根据伪距测量值和多普勒频移测量值，计算伪距测量值和多普勒频移测量值相对于全球导航卫星系统接收机与视觉惯性里程计之间外参的雅可比矩阵和残差，实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参。实现对外参进行实时标定，并且不断对外参进行优化。
Description
技术领域
本发明涉及标定外参技术领域，尤其涉及一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法与系统。
背景技术
视觉相机因为其低成本，易安装等优点，使得同时定位与地图构建(SLAM,Simultaneous Localization and Mapping)的研究变得越来越广泛。但是单纯的视觉SLAM系统会有很多的缺点，比如在遇到弱纹理、天黑，有过多动态物体等场景时视觉相机不能很好地提取特征点，这将会极大地影响视觉SLAM系统的定位精度。所以，一般都会将视觉相机与其他传感器结合使用以克服上面提到的一些缺点。目前，最常用的传感器就是惯性测量单元(IMU)。IMU体积小、价格低廉，并且能提供实时加速度和角速度，可以和视觉相机起到互补的作用。现阶段有非常多的经典视觉惯性里程计(VIO,visual-inertial odometry)系统，有基于优化的方法和基于滤波的方法。VIO短时定位效果很好，但是由于存在四自由度不可观的问题，所以系统运行的时间越长，VIO系统的轨迹就会偏离真实的轨迹越远。另外，由于视觉相机和IMU都不能提供绝对坐标，这就使得VIO系统只能进行相对定位，而不能进行绝对定位。正是由于VIO系统有上面提到的一些缺点，在实际的自动驾驶的定位系统中并不会直接使用VIO系统，而是要加上一些其他的传感器。一个比较好的选择就是全球导航卫星系统(GNSS,Global Navigation Satellite System)接收机，因为GNSS可以提供测站的全局坐标，并且由于GNSS与VIO的紧耦合系统在地心地固坐标系下是全自由度可观的，不会出现轨迹累积漂移的问题。但是在GNSS与VIO系统中要面临的一个问题就是GNSS接收机与IMU的外参标定。因为在传感器实际的安装过程中，GNSS与IMU并不是安装在同一个位置的，GNSS坐标系的原点与IMU坐标系的原点在空间上会有一段距离。如何标定这个距离是GNSS与VIO紧耦合系统中的一个关键问题。
目前的一些GNSS与VIO融合的多传感器融合定位系统并没有标定GNSS到IMU之间的外参，因为这些系统在安装传感器的时候GNSS和IMU的空间距离非常接近，所以就认为GNSS的原点与IMU的原点在同一位置。由于它们的中心距离比较近，即使认为它们的中心在同一位置，对在地心坐标系下的位置也不会造成太大的影响(因为这个距离相对于GNSS接收机到卫星的距离是可以忽略不计的)。但是当GNSS与IMU之间的安装距离比较远时，此时外参的影响就不可以忽略了。如果此时还认为GNSS与IMU的中心在同一个位置，那么解算到地心坐标系下的位置就会有比较大的误差。此外，与IMU与相机之间的外参不同，一般情况下，GNSS与IMU之间的外参是不会预先知道的，这样的话就必须在系统开始运行之后对外参进行在线标定。为了不影响系统长时间的定位精度，在线标定方法应该能够在短时间内就得到外参的初始化值，并且在后续的运行中不断通过优化算法来优化GNSS与IMU之间的外参。
以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。
发明内容
本发明为了解决现有的问题，提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法与系统。
为了解决上述问题，本发明采用的技术方案如下所述：
一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，包括以下步骤：S1：获取全球导航卫星系统接收机与视觉惯性里程计外参的初始值，所述初始值包括视觉惯性里程计初始化得到的惯性测量单元坐标系相对于局部世界坐标系的位置、速度和旋转矩阵，以及全球导航卫星系统初始化得到的局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；S2：根据所述初始值计算滑动窗口中每一时刻的全球导航卫星系统接收机的天线在地心地固坐标系下的位置；S3：根据全球导航卫星系统测量得到的伪距测量值和多普勒频移测量值，计算所述伪距测量值和所述多普勒频移测量值相对于所述全球导航卫星系统接收机与视觉惯性里程计之间外参的雅可比矩阵和残差，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参。
优选地，所述全球导航卫星系统接收机与视觉惯性里程计的外参为全球导航卫星系统初始化得到的局部世界坐标系在地心地固坐标系下的表示/>局部世界坐标系相对于导航坐标系的偏航角θ；局部世界坐标系相对于导航坐标系的旋转矩阵/>所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的位移为/>所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的旋转矩阵为/>导航坐标系到地心地固坐标系的旋转矩阵为/>所述滑动窗口中k时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移为/>则/>的表达式为：
其中，局部世界坐标系到导航坐标系的旋转矩阵与偏航角θ的关系为：
计算一个所述滑动窗口中所有时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移
其中，k＝1,2,3,…,N，N为所述滑动窗口数量。
优选地，在所述滑动窗口中的所述接收机看到4颗以上的导航卫星时为有效的接收机；对所述滑动窗口中所有所述有效的接收机计算伪距残差、伪距雅可比矩阵、多普勒频移残差和多普勒频移雅可比矩阵。
优选地，根据全球导航卫星系统测量得到的接收机的所述伪距测量值为伪距残差/>的表达式为：
式中，为全球导航卫星系统的导航卫星在地心地固坐标系下的坐标；c为光速；为k时刻接收机的时钟误差；Δts为导航卫星的时钟误差；/>为k时刻对流层延迟；/>为k时刻电离层延迟；
所述多普勒频移测量值为多普勒频移残差/>的表达式为：
其中，λ为载波波长；l为接收机到卫星的单位向量；为导航卫星在地心地固坐标系下的速度；/>为k时刻接收机在地心地固坐标系下的速度；/>为k时刻接收机钟差的变化率；/>为导航卫星钟差的变化率。
优选地，伪距雅可比矩阵表达式为：
多普勒频移雅可比矩阵的表达式为：
优选地，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参包括：S31：计算全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代值当迭代次数达到最大值或者迭代值小于一个阈值停止迭代；S32：根据所述迭代值实时在线优化更新全球导航卫星系统接收机与视觉惯性里程计之间外参/>S33：随着所述滑动窗口移动实时计算全球导航卫星系统的所述接收机天线在地心地固坐标系下的坐标/>
优选地，最大迭代次数设置为10，更新阈值设置为10-5。
优选地，根据所述伪距残差、所述多普勒频移残差、所述伪距雅可比矩阵和所述多普勒频移雅可比矩阵得到伪距-多普勒频移残差rk表达式为：
伪距-多普勒频移雅可比矩阵Jk表达式为：
高斯牛顿法方程为：
全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代优化方程为：
本发明还提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的系统，用于实现如上任一所述的方法，包括：数据处理单元、视觉惯性里程计初始化单元、全球导航卫星系统初始化单元、全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元以及全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元；所述数据预处理单元包括图像数据预处理单元和惯性测量数据预处理单元，用于将处理后的图片和惯性测量数据输出到所述视觉惯性里程计初始化单元；所述图像数据预处理单元主要对单目相机采集的图片进行特征点的检测和跟踪；所述惯性测量数据预处理单元主要对惯性测量单元采集的数据进行预积分；所述的视觉惯性里程计初始化单元接收所述数据预处理单元的数据，得到惯性测量单元坐标系相对于局部世界坐标系的位置、速度、旋转矩阵以及单目相机的尺度和重力向量等数据；所述全球导航卫星系统初始化单元接收卫星星历以及伪距，多普勒频移测量值，并估计局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；所述全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元，利用所述的视觉惯性里程计初始化单元以及所述全球导航卫星系统初始化单元输出的计算值对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行初始估计；全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元在系统运行时对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行实时优化解算。
本发明再提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。
本发明的有益效果为：提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法与系统，通过在GNSS与IMU外参初始化阶段，利用VIO初始化输出的关键帧位姿，GNSS初始化输出的局部世界坐标系原点在地心地固坐标系下的坐标和局部世界坐标系相对于导航坐标系的偏航角，对GNSS与IMU之间的外参进行初始化估计；在GNSS与IMU外参实时优化阶段，利用GNSS得到的伪距和多普勒频移测量值，计算伪距-多普勒频移残差和雅可比矩阵，在一个滑动窗口中根据高斯牛顿法实时迭代更新GNSS与IMU之间的外参；实现对外参进行实时标定，并且在系统运行时不断对外参进行优化，可以极大地提升全球导航卫星系统与视觉惯性里程计耦合系统的定位精度。
本发明可以广泛应用于GNSS与VIO紧耦合的SLAM定位系统中对GNSS与IMU进行在线标定。
附图说明
图1是本发明实施例中一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法的示意图。
图2是本发明实施例中全球导航卫星系统接收机与视觉惯性里程计的坐标系的位置。
图3是本发明实施例中利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参的方法示意图。
图4是本发明实施例中一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的流程的示意图。
具体实施方式
为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。
需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
针对上述问题，本发明的目的是提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法与系统，其能够在系统运行时对外参进行实时标定，并且在系统运行时不断对外参进行优化，可以极大地提升全球导航卫星系统与视觉惯性里程计耦合系统的定位精度。
如图1所示，本发明提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，包括以下步骤：
S1：获取全球导航卫星系统接收机与视觉惯性里程计外参的初始值，所述初始值包括视觉惯性里程计初始化的惯性测量单元坐标系相对于局部世界坐标系的位置、速度和旋转矩阵，以及全球导航卫星系统初始化得到的局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；
S2：根据所述初始值计算滑动窗口中每一时刻的全球导航卫星系统接收机的天线在地心地固坐标系下的位置；
S3：根据全球导航卫星系统测量得到的伪距测量值和多普勒频移测量值，计算所述伪距测量值和所述多普勒频移测量值相对于所述全球导航卫星系统接收机与视觉惯性里程计之间外参的雅可比矩阵和残差，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参。
如图2所示，是本发明实施例中全球导航卫星系统接收机与视觉惯性里程计的坐标系的位置。
本发明通过在GNSS与IMU外参初始化阶段，利用VIO初始化输出的关键帧位姿，GNSS初始化输出的局部世界坐标系原点在地心地固坐标系下的坐标和局部世界坐标系相对于导航坐标系的偏航角，对GNSS与IMU之间的外参进行初始化估计；在GNSS与IMU外参实时优化阶段，它利用GNSS得到的伪距和多普勒频移测量值，计算伪距-多普勒频移残差和雅可比矩阵，在一个滑动窗口中根据高斯牛顿法实时迭代更新GNSS与IMU之间的外参，实现对外参进行实时标定，并且在系统运行时不断对外参进行优化，可以极大地提升全球导航卫星系统与视觉惯性里程计系统的定位精度。
本发明可以广泛应用于GNSS与VIO紧耦合的SLAM定位系统中对GNSS与IMU进行在线标定。
在本发明的一种实施例中，全球导航卫星系统接收机与视觉惯性里程计的外参为全球导航卫星系统初始化得到的局部世界坐标系在地心地固坐标系下的表示/>局部世界坐标系相对于导航坐标系的偏航角θ；局部世界坐标系相对于导航坐标系的旋转矩阵所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的位移为/>所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的旋转矩阵为/>导航坐标系到地心地固坐标系的旋转矩阵为/>所述滑动窗口中k时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移为/>则/>的表达式为：
其中，局部世界坐标系到导航坐标系的旋转矩阵与偏航角θ的关系为：
计算一个所述滑动窗口中所有时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移
其中，k＝1,2,3,…,N，N为所述滑动窗口数量。
可以理解的是，在所述滑动窗口中的所述接收机看到4颗以上的导航卫星时为有效的接收机；对所述滑动窗口中所有所述有效的接收机计算伪距残差、伪距雅可比矩阵、多普勒频移残差和多普勒频移雅可比矩阵。
根据全球导航卫星系统测量得到的接收机的所述伪距测量值为伪距残差的表达式为：
式中，为全球导航卫星系统的导航卫星在地心地固坐标系下的坐标；c为光速；为k时刻接收机的时钟误差；Δts为导航卫星的时钟误差；/>为k时刻对流层延迟；/>为k时刻电离层延迟；
所述多普勒频移测量值为多普勒频移残差/>的表达式为：
其中，λ为载波波长；l为接收机到卫星的单位向量；为导航卫星在地心地固坐标系下的速度；/>为k时刻接收机在地心地固坐标系下的速度；/>为k时刻接收机钟差的变化率；/>为导航卫星钟差的变化率。
伪距雅可比矩阵表达式为：
多普勒频移雅可比矩阵的表达式为：
如图3所示，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参包括：
S31：计算全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代值当迭代次数达到最大值或者迭代值小于一个阈值停止迭代；
S32：根据所述迭代值实时在线优化更新全球导航卫星系统接收机与视觉惯性里程计之间外参
S33：随着所述滑动窗口移动实时计算全球导航卫星系统的所述接收机天线在地心地固坐标系下的坐标
在一种具体的实施例中，由于GNSS与IMU外参的初始化是一个近似的估计值，最大迭代次数设置为10，更新阈值设置为10-5。当迭代大于10，或者迭代值的二范数小于10-5时，停止更新。
每计算一次迭代值更新一次GNSS接收机天线在地心地固坐标系下的坐标
根据所述伪距残差、所述多普勒频移残差、所述伪距雅可比矩阵和所述多普勒频移雅可比矩阵得到伪距-多普勒频移残差rk表达式为：
伪距-多普勒频移雅可比矩阵Jk表达式为：
高斯牛顿法方程为：
全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代优化方程为：
在系统运行的时候，根据上式就可以不断迭代优化GNSS与IMU的外参。
本发明还提供一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的系统，用于实现如上任一所述的方法，包括：数据处理单元、视觉惯性里程计初始化单元、全球导航卫星系统初始化单元、全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元以及全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元；
所述数据预处理单元包括图像数据预处理单元和惯性测量数据预处理单元，用于将处理后的图片和惯性测量数据输出到所述视觉惯性里程计初始化单元；所述图像数据预处理单元主要对单目相机采集的图片进行特征点的检测和跟踪；所述惯性测量数据预处理单元主要对惯性测量单元采集的数据进行预积分；
所述的视觉惯性里程计初始化单元接收所述数据预处理单元的数据，得到惯性测量单元坐标系相对于局部世界坐标系的位置、速度、旋转矩阵以及单目相机的尺度和重力向量等数据；
所述全球导航卫星系统初始化单元接收卫星星历以及伪距、载波、多普勒频移测量值，并估计局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；
所述全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元，利用所述的视觉惯性里程计初始化单元以及所述全球导航卫星系统初始化单元输出的计算值对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行初始估计；
全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行实时优化解算。
本发明的系统在运行时对外参进行实时标定，并且在系统运行时不断对外参进行优化，可以极大地提升GNSS与VIO系统的定位精度。
如图4所示，在一种具体的实施例中，本发明的一种在线标定GNSS与VIO外参的系统包括三个步骤：数据初始化阶段，包括VIO的初始化和GNSS的初始化；GNSS与IMU外参初始化阶段；GNSS与IMU外参实时在线优化阶段。
其中,VIO初始化主要包括视觉图片预处理(特征点检测与跟踪)和IMU初始化(IMU预积分)阶段和滑动窗口中关键帧数据初始化阶段；GNSS初始化阶段主要包括求解局部世界坐标系在地心地固世界坐标系的坐标和求解局部世界坐标系相对与导航坐标系的偏航角。
在数据预处理阶段图片序列由单目相机采集得到，加速度和角速度由IMU采集得到。从单目相机中采集的关键点为Shi-Tomas角点，描述子为BRIRF描述子，特征点跟踪使用LK光流法；IMU的加速度和角速度进行预积分。使用预处理后的数据计算初始滑动窗口中关键帧的位姿。对于GNSS初始化阶段，根据伪距残差和多普勒频移残差计算局部世界坐标系原点在地心地固坐标系下的坐标和局部世界坐标系到导航坐标系的偏航角。GNSS与VIO外参初始化阶段根据前一阶段关键帧的位姿和GNSS初始化的输出，计算GNSS与IMU之间外参的初始值。在外参实时优化阶段对GNSS与IMU外参实时优化。
基于本发明提供的在线标定GNSS与VIO外参的系统，本发明还提出了一种在线标定GNSS与VIO外参的方法，其包括以下步骤：
1)数据预处理阶段包括图片预处理和IMU数据预处理，图片预处理包括图片特征点检测和跟踪，IMU数据预处理包括加速度和角速度预积分。
2)所述步骤1)中，输出检测和跟踪到的特征点和IMU预积分值，利用预处理的值初始化所述滑动窗口中所有关键帧的位姿。
3)根据GNSS接收机得到的测量值和星历数据。计算导航卫星钟差Δts、对流层延迟电离层延迟/>导航卫星在地心地固坐标系下的坐标/>
4)根据所所述步骤3)得到的值计算局部世界坐标系原点在地心地固坐标系下的坐标局部世界坐标系到导航坐标系的偏航角θ，局部世界坐标系到导航坐标系的旋转矩阵/>表达式为：
5)估计所述步骤2)和所述步骤4)输出的值，以及伪距测量值为对GNSS与IMU之间的外参/>进行初始估计。具体步骤为：
①计算所述滑动窗口中k时刻(k＝1,2,…,N)的伪距残差和伪距雅可比矩阵表达式分别为：
②根据残差和雅可比得到的高斯牛顿方程为：
GNSS与IMU之间的外参迭代方程为：
k表示所述滑动窗口中关键帧的个数。
6)在得到GNSS与IMU之间的外参初始值后，系统进入后端优化，对GNSS与IMU之间的外参进行在线实时标定。具体步骤为：
①与所述步骤5)中阶段①相同。
②计算所述滑动窗口中k时刻的多普勒频移残差多普勒频移雅可比矩阵，表达式分别为：
③根据所述步骤6)步骤①和步骤②，高斯牛顿方程为：
其中，Jk为伪距-多普勒频移雅可比矩阵；rk为伪距-多普勒频移残差。
则优化变量的迭代方程表达式为：
k表示滑动窗口的大小，随着滑动窗口的不断移动，窗口大小不变，但是优化过程中使用的测量值不断变化。
现有的GNSS与VIO紧耦合的SLAM定位系统没有标定GNSS与IMU之间的外参，主要是由于在这些系统中两者的安装位置比较相近，两者的距离相比与GNSS接收机到导航卫星的距离非常小，由此引起的坐标差在地心地固坐标系下可以忽略不计。所以考虑在导航坐标系下的定位误差，比较了GVINS和基于本发明的系统，由于使用的数据集GNSS与IMU距离比较近，所以最终定位结果非常接近，比较结果如表1所示。
表1 SLAM定位系统与加上本发明的定位系统效果对比
从表1可以看出，由于使用的数据集GNSS接收机安装的位置与IMU非常接近，两者的定位结果误差不大。当两个传感器安装的位置相隔越远，本发明的定位精度越准确。
本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。
本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。
本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。
所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，ErasableProgrammable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，ElectricallyErasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，FerromagneticRandom Access Memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random Access Memory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，SynchronousStatic Random Access Memory)、动态随机存取存储器(DRAM，Dynamic Random AccessMemory)、同步动态随机存取存储器(SDRAM，Synchronous Dynamic Random AccessMemory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data RateSynchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAMEnhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，Sync Link Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。
上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。
另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
或者，本发明上述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。
本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。
本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。
以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。
Claims (10)
1.一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，包括以下步骤：
S1：获取全球导航卫星系统接收机与视觉惯性里程计外参的初始值，所述初始值包括视觉惯性里程计初始化得到的惯性测量单元坐标系相对于局部世界坐标系的位置、速度和旋转矩阵，以及全球导航卫星系统初始化得到的局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；
S2：根据所述初始值计算滑动窗口中每一时刻的全球导航卫星系统接收机的天线在地心地固坐标系下的位置；
S3：根据全球导航卫星系统测量得到的伪距测量值和多普勒频移测量值，计算所述伪距测量值和所述多普勒频移测量值相对于所述全球导航卫星系统接收机与视觉惯性里程计之间外参的雅可比矩阵和残差，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参。
2.如权利要求1所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，所述全球导航卫星系统接收机与视觉惯性里程计的外参为全球导航卫星系统初始化得到的局部世界坐标系在地心地固坐标系下的表示/>局部世界坐标系相对于导航坐标系的偏航角θ；局部世界坐标系相对于导航坐标系的旋转矩阵/>所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的位移为/>所述滑动窗口中k时刻的惯性测量单元坐标系到局部世界坐标系的旋转矩阵为/>导航坐标系到地心地固坐标系的旋转矩阵为/>所述滑动窗口中k时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移为/>则/>的表达式为：
其中，局部世界坐标系到导航坐标系的旋转矩阵与偏航角θ的关系为：
计算一个所述滑动窗口中所有时刻的全球导航卫星系统的接收机天线到地心地固坐标系的位移
其中，k＝1,2,3,…,N，N为所述滑动窗口数量。
3.如权利要求2所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，在所述滑动窗口中的所述接收机看到4颗以上的导航卫星时为有效的接收机；对所述滑动窗口中所有所述有效的接收机计算伪距残差、伪距雅可比矩阵、多普勒频移残差和多普勒频移雅可比矩阵。
4.如权利要求3所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，根据全球导航卫星系统测量得到的接收机的所述伪距测量值为伪距残差/>的表达式为：
式中，为全球导航卫星系统的导航卫星在地心地固坐标系下的坐标；c为光速；/>为k时刻接收机的时钟误差；Δts为导航卫星的时钟误差；/>为k时刻对流层延迟；/>为k时刻电离层延迟；
所述多普勒频移测量值为多普勒频移残差/>的表达式为：
其中，λ为载波波长；l为接收机到卫星的单位向量；为导航卫星在地心地固坐标系下的速度；/>为k时刻接收机在地心地固坐标系下的速度；/>为k时刻接收机钟差的变化率；为导航卫星钟差的变化率。
5.如权利要求4所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，伪距雅可比矩阵表达式为：
多普勒频移雅可比矩阵的表达式为：
6.如权利要求5所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，利用高斯牛顿法或者列文伯格-马夸尔特方法实时优化全球导航卫星系统接收机与视觉惯性里程计之间的外参包括：
S31：计算全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代值当迭代次数达到最大值或者迭代值小于一个阈值停止迭代；
S32：根据所述迭代值实时在线优化更新全球导航卫星系统接收机与视觉惯性里程计之间外参
S33：随着所述滑动窗口移动实时计算全球导航卫星系统的所述接收机天线在地心地固坐标系下的坐标
7.如权利要求6所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，最大迭代次数设置为10，更新阈值设置为10-5。
8.如权利要求7所述的在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法，其特征在于，根据所述伪距残差、所述多普勒频移残差、所述伪距雅可比矩阵和所述多普勒频移雅可比矩阵得到伪距-多普勒频移残差rk表达式为：
伪距-多普勒频移雅可比矩阵Jk表达式为：
高斯牛顿法方程为：
全球导航卫星系统接收机与视觉惯性里程计之间外参的迭代优化方程为：
9.一种在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的系统，其特征在于，用于实现如权利要求1-8任一所述的方法，包括：数据处理单元、视觉惯性里程计初始化单元、全球导航卫星系统初始化单元、全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元以及全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元；
所述数据预处理单元包括图像数据预处理单元和惯性测量数据预处理单元，用于将处理后的图片和惯性测量数据输出到所述视觉惯性里程计初始化单元；所述图像数据预处理单元主要对单目相机采集的图片进行特征点的检测和跟踪；所述惯性测量数据预处理单元主要对惯性测量单元采集的数据进行预积分；
所述的视觉惯性里程计初始化单元接收所述数据预处理单元的数据，得到惯性测量单元坐标系相对于局部世界坐标系的位置、速度、旋转矩阵以及单目相机的尺度和重力向量数据；
所述全球导航卫星系统初始化单元接收卫星星历以及伪距，多普勒频移测量值，并估计局部世界坐标系的原点在地心地固坐标系下的位置以及局部世界坐标系到导航坐标系的旋转矩阵；
所述全球导航卫星系统接收机与视觉惯性里程计估计外参初值单元，利用所述的视觉惯性里程计初始化单元以及所述全球导航卫星系统初始化单元输出的计算值对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行初始估计；
全球导航卫星系统接收机与视觉惯性里程计联合实时优化外参单元在系统运行时对全球导航卫星系统接收机与视觉惯性里程计之间的外参进行实时优化解算。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-8任一所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210191523.3A CN114459506B (zh) | 2022-02-28 | 2022-02-28 | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210191523.3A CN114459506B (zh) | 2022-02-28 | 2022-02-28 | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114459506A CN114459506A (zh) | 2022-05-10 |
| CN114459506B true CN114459506B (zh) | 2023-08-08 |
Family
ID=81414996
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210191523.3A Active CN114459506B (zh) | 2022-02-28 | 2022-02-28 | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114459506B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115343730B (zh) * | 2022-10-18 | 2023-02-17 | 苏州挚途科技有限公司 | Gnss天线外参确定方法、装置与计算机可读存储介质 |
| CN116026316B (zh) * | 2023-03-30 | 2023-08-29 | 山东科技大学 | 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110542916A (zh) * | 2019-09-18 | 2019-12-06 | 上海交通大学 | 卫星与视觉紧耦合定位方法、系统及介质 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN111356902A (zh) * | 2017-11-29 | 2020-06-30 | 高通股份有限公司 | Radar辅助视觉惯性测程初始化 |
| CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150219767A1 (en) * | 2014-02-03 | 2015-08-06 | Board Of Regents, The University Of Texas System | System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates |
| US11754726B2 (en) * | 2020-07-13 | 2023-09-12 | Qualcomm Incorporated | Method and apparatus for in motion initialization of global navigation satellite system—inertial navigation system |
-
2022
- 2022-02-28 CN CN202210191523.3A patent/CN114459506B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111356902A (zh) * | 2017-11-29 | 2020-06-30 | 高通股份有限公司 | Radar辅助视觉惯性测程初始化 |
| CN110542916A (zh) * | 2019-09-18 | 2019-12-06 | 上海交通大学 | 卫星与视觉紧耦合定位方法、系统及介质 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
Non-Patent Citations (1)
| Title |
|---|
| Optimization-based visual-inertial SLAM tightly coupled with raw GNSS measurements;Liu J. et al.;2021 IEEE International Conference on Robotics and Automation;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114459506A (zh) | 2022-05-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11543541B2 (en) | Systems and methods for high-integrity satellite positioning | |
| CN114459506B (zh) | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 | |
| CN109477900B (zh) | 全球导航卫星系统接收器中用于模糊度解算的频率间偏差的估算 | |
| US9035826B2 (en) | Satellite differential positioning receiver using multiple base-rover antennas | |
| WO2010074777A1 (en) | Navigation receiver and method for combined use of a standard rtk system and a global carrier-phase differential positioning system | |
| US11781868B2 (en) | System and method for GNSS reflective surface mapping and position fix estimation | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN114396943A (zh) | 一种融合定位方法与终端 | |
| JP2008039691A (ja) | 搬送波位相式測位装置 | |
| CN115435779A (zh) | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 | |
| US11650324B2 (en) | Navigation apparatus and method in which measurement quantization errors are modeled as states | |
| Wen et al. | 3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons | |
| CN107270904B (zh) | 基于图像配准的无人机辅助引导控制系统及方法 | |
| CN113436261A (zh) | 一种面向封闭园区自动驾驶的单目视觉惯性定位方法 | |
| Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
| CN112924999B (zh) | 一种无人机的定位方法、系统、装置及介质 | |
| CN115096309A (zh) | 融合定位方法、装置、电子设备及存储介质 | |
| CN114861725A (zh) | 一种目标感知跟踪的后处理方法、装置、设备及介质 | |
| Zimmermann et al. | Accurate georeferencing of TLS point clouds with short GNSS observation durations even under challenging measurement conditions | |
| US11859979B2 (en) | Delta position and delta attitude aiding of inertial navigation system | |
| CN113721248B (zh) | 一种基于多源异构传感器的融合定位方法及系统 | |
| Xu et al. | An innovation-based cycle-slip, multipath estimation, detection and mitigation method for tightly coupled GNSS/INS/Vision navigation in urban areas | |
| Li et al. | An improved ambiguity resolution algorithm based on particle filter for ins/rtk integration in urban environments | |
| Li et al. | An ambiguity-free smoothing algorithm for multipath mitigation in INS/RTK tightly-coupled integration | |
| Li et al. | Accuracy-and Simplicity-Oriented Self-Calibration Approach for In-Vehicle GNSS/INS/Vision System With Observability Analysis |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |