CN116540286B - 基于非完整性约束的多源鲁棒融合定位方法 - Google Patents
基于非完整性约束的多源鲁棒融合定位方法 Download PDFInfo
- Publication number
- CN116540286B CN116540286B CN202310821874.2A CN202310821874A CN116540286B CN 116540286 B CN116540286 B CN 116540286B CN 202310821874 A CN202310821874 A CN 202310821874A CN 116540286 B CN116540286 B CN 116540286B
- Authority
- CN
- China
- Prior art keywords
- representing
- coordinate system
- error
- zero offset
- camera
- 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
- 238000000034 method Methods 0.000 title claims abstract description 89
- 230000004927 fusion Effects 0.000 title claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims description 40
- 230000008569 process Effects 0.000 claims description 32
- 230000010354 integration Effects 0.000 claims description 24
- 238000005259 measurement Methods 0.000 claims description 20
- 230000000007 visual effect Effects 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 230000033001 locomotion Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 3
- 238000005457 optimization Methods 0.000 abstract description 7
- 238000001514 detection method Methods 0.000 abstract description 6
- 230000008030 elimination Effects 0.000 abstract description 5
- 238000003379 elimination reaction Methods 0.000 abstract description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000001914 filtration Methods 0.000 description 3
- 230000001131 transforming effect Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000000546 chi-square test Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000001303 quality assessment method Methods 0.000 description 1
- 238000003908 quality control method Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009012 visual motion Effects 0.000 description 1
Classifications
-
- 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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
-
- 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/20—Integrity monitoring, fault detection or fault isolation of space segment
-
- 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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Security & Cryptography (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于非完整性约束的多源鲁棒融合定位方法，基于车载导航场景下的特征，引入非完整性因子对惯性机械编排解算结果进行约束，对于GNSS卫星观测值基于残差卡方检验法RCTM算法进行卫星信号故障检测与排除，最后利用动基座初始化算法加速多源融合系统的快速部署，通过非线性优化的方式来实现卫星导航/惯性/视觉的多源鲁棒融合定位。
Description
技术领域
本发明属于多源传感器融合定位技术领域，具体涉及一种基于非完整性约束的多源鲁棒融合定位方法。
背景技术
随着数字化科技的飞速发展，越来越多的无人车、无人机、无人快递配送、自主机器人等智慧系统慢慢走进人们的生活，人们对于导航定位技术也提出了更高的要求，如何实现城市复杂环境下GNSS(Global Navigation Satellite System, GNSS)信号失锁甚至不可用的情况下仍能保持高精度、连续性、稳健、高可靠的导航定位成为了学术界和工业界关注的热点。目前，GNSS/INS（Inertial Navigation System, INS）组合导航系统技术已经成熟且应用广泛，能够连续地提供高精度的定位信息，许多学者也提出抗差自适应滤波算法来保证系统的稳健可靠性，但是只能保证GNSS短时间中断的高精度定位，当GNSS信号长时间中断不可用时，该系统的导航误差会迅速累积，无法满足高精度定位的需求。因此，这时就需要借助别的传感器来辅助导航定位。相机由于其成本低、方便携带、自主无源、信息丰富的特点，被人们广泛应用于计算机视觉和机器人领域。在城市复杂环境中相机所获得的信息往往比较丰富，而GNSS信号质量较差，视觉惯性导航表现出很好的优势，一方面，INS/视觉的组合可以在线估计陀螺零偏误差，有效减小航向的发散速度，另一方面，单目相机存在的尺度缺失问题可以通过IMU有效的解决；而在相机特征点较少的场合，GNSS的信号往往较好，GNSS/INS组合则能提供高精度定位，可见GNSS、INS、视觉这三种导航定位技术存在极好的互补性，克服了单一传感器或两种传感器融合定位的局限性。GNSS/INS/视觉融合定位技术的基本思想是利用事先确定的最优准则，对各传感器在不同时间、不同的空间观测的数据信息进行融合，获得系统的状态参数，提高导航解算精度，改进定位系统的质量控制，具有重要的现实意义和实际价值。
目前GNSS/INS/视觉融合定位的方式主要分为基于滤波和基于优化两种方式。采用松耦合的滤波融合方式由于对于视觉信息利用不充分，定位定姿的精度还需要进一步的提高；另一方面，利用GNSS的观测值来估计相机帧之间的尺度信息，当GNSS卫星不可用时该方法则失效。基于优化采用因子图的方式充分利用系统的稀疏性实现最优解算，但随着时间的增长存在误差向下累积的情况，另外，由于因子图优化的函数强非线性，如果初始化的参数不准确可能导致迭代估计时间较长甚至不收敛的问题。传感器的数据是否可靠是决定融合算法的根源，多源信息数据的质量评估、故障排除及权重分配将会是鲁棒性算法的未来方向。
发明内容
为了解决上述问题，本发明提出一种基于非完整性约束的多源鲁棒融合定位方法，基于车载导航场景下的特征，引入非完整性因子对惯性机械编排解算结果进行约束，对于GNSS观测值基于残差卡方检验法RCTM（Residual Chi-square Test Method, RCTM）算法进行卫星信号故障检测与排除，最后利用动基座初始化算法加速多源融合系统的快速部署，通过非线性优化的方式来实现卫星导航/惯性/视觉的多源鲁棒融合定位。
本发明实现其目的所采取的技术方案是：
一种基于非完整性约束的多源鲁棒融合定位方法，包括如下步骤：
步骤（1） 通过残差卡方检验法对GNSS的所有卫星测量值进行检验，判断是否卫星存在故障；如果不存在故障，则将解算RTK定位结果与视觉惯性里程计结果进行优化，如果存在故障，则使用残差卡方检验法对故障卫星进行检测、排除故障卫星；
步骤（2） 引入非完整性约束，认为车辆在行驶的过程中不会发生侧滑和跳跃，则载体坐标系b系下车辆的右向速度和天向速度近似为0，对INS机械编排结果进行约束；
步骤（3） 估计相机与惯性测量器件之间的旋转和陀螺的零偏，构建最小二乘计算相机与惯性测量器件之间的相对旋转最优估计值；
步骤（4） 利用重力及载体运动的相对平移估计卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵；
步骤（5） 估计加速度计的零偏；
步骤（6） 卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵趋于平稳之后，即完成动基座初始化。
进一步地，所述步骤（2）具体包括：
认为车辆在行驶的过程中不会发生侧滑和跳跃，此时在载体坐标系b系下，车辆的右向速度和天向速度近似为0，即：
(1)，
式中，的上标b表示在载体坐标系b系下的速度，下标分别表示右向和天向的速度；
(2)，
式中，表示地心地固坐标系e系到载体坐标系b系的方向余弦矩阵，/>表示地心地固坐标系e系下的速度，/>表示载体坐标系b系下的速度；
对式(2)添加扰动误差得到：
(3)，
式中，、/>分别表示b系和e系下的速度误差项，/>表示单位矩阵，/>表示失准角，/>表示反对称矩阵符号，对式(3)展开并去掉二次误差项得到：
(4)，
因为，则式(4)写成：
(5)，
将式(2)代入式(5)得到：
(6)，
将式(6)写成矩阵形式得到：
(7)，
(8)，
式中，表示非完整性约束的速度，/>表示INS机械编排推算的速度，/>表示加速计的零偏，/>表示陀螺的零偏，/>表示地心地固坐标系e系下的失准角误差，分别表示地心地固坐标系e系下的速度误差和位置误差，/>表示非完整性约束噪声，/>表示INS推算的载体坐标系b系下的前向速度，也表明不对前向速度加入约束；
式(7)即对INS机械编排结果进行约束。
进一步地，所述步骤（3）包括：
构建最小二乘问题得到相机与惯性测量器件之间的相对旋转最优估计值：
(9)，
式中，为预先采用手眼标定法得到的相机与惯性测量器件之间的相对旋转估计值，/>表示相机与惯性测量器件之间的最优旋转估计值，/>表示目标函数，/>为约束条件； argmin表示使/>函数达到最小值时/>取值的函数；
以相机与惯性测量器件之间的最优旋转估计值和相机位姿估计值作为估计陀螺零偏的基准，记陀螺零偏的扰动误差为：
(10)，
式中，为陀螺零偏的真值，/>为计算姿态预积分时的陀螺零偏；
则零偏校正后的姿态预积分与基准之间满足：
(11)，
式中，表示k与k-1时刻之间的姿态变换矩阵，/>表示k-1到k时刻的姿态预积分，/>为雅可比矩阵，/>表示四元数的乘法符号，/>表示陀螺仪零偏误差导致的姿态预积分误差，通过QR分解得到陀螺的零偏误差，进而得到修正后的零偏，并根据该修正后的值重新惯性测量器件预积分。
进一步地，所述步骤（4）包括：
假设GNSS的定位发生在和/>时刻，则惯性测量器件在参考坐标系/>下的位姿与GNSS定位结果之间的关系写成以下差分形式：
(12)，
式中，表示i时刻IMU相对于相机在参考坐标系/>下的位置，/>表示j时刻IMU相对于相机在参考坐标系/>下的位置，/>表示i时刻IMU和相机在参考坐标系下的姿态变换矩阵，/>表示j时刻IMU和相机在参考坐标系/>下的姿态变换矩阵，表示GNSS与IMU之间的杆臂，/>表示i时刻GNSS解算的/>系下的位置，表示j时刻GNSS解算的/>系下的位置。
进一步地，所述步骤（5）包括：
记加速度计零偏的扰动误差为：
(13)，
式中，为加速度计零偏的真值，/>为加速度计零偏的当前估计；
假设GNSS/INS/视觉的世界坐标系与视觉里程计的参考坐标系/>之间的当前相对旋转矩阵为/>，估计的误差为/>，则：
(14)，
位置和速度预积分误差可以通过加速计零偏进行补偿：
(15)，
式中，分别表示预积分估计的理想位置和速度，分别表示预积分的估计位置与速度，/>分别表示位置和速度预积分估计的加速度计零偏的雅可比矩阵，/>表示加速度计零偏的扰动误差，通过QR分解方式求解，进而得到修正后的零偏，并根据该修正后的值重新惯性测量器件预积分。
进一步地，所述步骤（6）包括：
将当前估计的相对旋转矩阵写成四元数的形式/>，假设当前估计的相对旋转矩阵/>连续的n个估计值为/>，则任意两个之间的均方根误差为：
(16)，
在n个估计值中，若随机挑选2个估计值之间的均方根误差均小于阈值，则认为收敛。
本发明的优点在于：
针对卫星导航/惯性/视觉的多源传感器融合鲁棒定位，提出对于GNSS观测卫星进行RTCM故障检测与排除，并基于车载导航的特性，在IMU与积分阶段引入非完整性约束，最后为了应对非线性优化的准确迭代初始值要求，提出一种动基座的初始化算法，加速多源融合系统的快速部署，提高定位的精度及鲁棒性。
附图说明
图1为本发明的基于非完整性约束的多源鲁棒融合定位方法流程示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明引入非完整性因子对惯性机械编排解算结果进行约束，对于GNSS观测值基于残差卡方检验法RCTM算法进行卫星信号故障检测与排除，最后利用动基座初始化算法加速多源融合系统的快速部署，通过非线性优化的方式来实现卫星导航/惯性/视觉的鲁棒定位。
如图1所示，本发明的一种基于非完整性约束的多源鲁棒融合定位方法，第一方面，对GNSS卫星进行故障探测与排除，在原始数据层面进行把关，保障融合定位精度及可靠性，第二方面，在惯性测量单元预积分的阶段基于车载导航系统的特性，引入非完整性约束，第三方面，卫星导航/惯性/视觉动基座初始化；具体包括如下步骤：
步骤（1） 首先通过残差卡方检验法RCTM对GNSS的所有卫星测量值进行检验，判断其是否存在故障，如果不存在故障，则直接解算RTK结果，与视觉惯性里程计VIO(visual-inertial odometry, VIO）结果进行后端非线性优化；如果检测有故障，则通过随机搜索方式，从当前历元观测到的N颗卫星中选取n颗卫星，再次对其进行RCTM检测，如果存在故障，返回上一步重新随机选取n颗卫星进行检测，如果不存在故障，则将这n颗卫星进行记录保存为无故障组；将当前历元观测的其余（N-n）颗卫星依次加入到算法中并进行RCTM检测，如果出现故障，则将该卫星进行排除，如果没有故障，则保存并更新到之前的无故障卫星组。
步骤（2）在INS预积分的阶段基于车载导航系统的特性，引入非完整性约束，具体包括：
认为车辆在行驶的过程中不会发生侧滑和跳跃，此时在载体坐标系b系下（本发明采用右前上坐标系）车辆的右向速度和天向速度近似为0，即：
(1)，
式中，的上标b表示在载体坐标系b系下的速度，下标分别表示右向和天向的速度。
式(1)可作为对INS的机械编排估算结果的一种约束，但是由于安装角误差会对影响，进一步地，会影响/>，因为：
(2)，
式中，表示地心地固坐标系e系到载体坐标系b系的方向余弦矩阵，/>表示地心地固坐标系e系下的速度，/>表示载体坐标系b系下的速度。对式(2)添加扰动误差可得：
(3)，
式中，、/>分别表示b系和e系下的速度误差项，/>表示单位矩阵，/>表示失准角，/>表示反对称矩阵符号，对式(3)展开并去掉二次误差项可得：
(4)，
又因为，则式(4)可以写成：
(5)，
将式(2)代入式(5)可得：
(6)，
将式(6)写成矩阵形式可得：
(7)，
(8)，
式中，表示非完整性约束的速度，/>表示INS机械编排推算的速度，/>表示加速计的零偏，/>表示陀螺的零偏，/>表示地心地固坐标系e系下的失准角误差，分别表示地心地固坐标系e系下的速度误差和位置误差，/>表示非完整性约束噪声，/>表示INS推算的载体坐标系b系下的前向速度，也表明不对前向速度加入约束。式(7)即可对INS机械编排结果进行约束。
步骤（3） 估计相机与惯性测量器件IMU(Inertial Measurement Unit, IMU)之间的旋转和陀螺的零偏。由于IMU预积分与相机的位姿估计可能存在一定的误差，可构建最小二乘问题来得到相机与IMU之间的相对旋转最优估计值。
(9)，
式中，为预先采用手眼标定法得到的相机与IMU之间的相对旋转估计值，/>表示相机与IMU之间的最优旋转估计值，/>表示目标函数，/>为约束条件。argmin表示使/>函数达到最小值时/>取值的函数。
由于陀螺零偏随时间缓慢变化，在预积分的时间段可将其看作常值。以相机与IMU之间的最优旋转估计值和相机位姿估计值作为估计陀螺零偏的基准，若记陀螺零偏的扰动误差为：
(10)，
式中，为陀螺零偏的真值，/>为计算姿态预积分时的陀螺零偏。则零偏校正后的姿态预积分与基准之间满足：
(11)，
式中，表示k与k-1时刻之间的姿态变换矩阵，/>表示k-1到k时刻的姿态预积分，/>表示四元数的乘法符号，/>为雅可比矩阵，/>表示陀螺仪零偏误差导致的姿态预积分误差，可通过QR分解得到陀螺的零偏误差，进而得到修正后的零偏，并根据该修正后的值重新IMU预积分。
步骤（4） 利用重力及载体运动的相对平移估计卫星导航/惯性/视觉的世界坐标系（地心地固坐标系）与视觉里程计的参考坐标系/>之间的相对旋转矩阵/>。假设GNSS的定位发生在/>和/>时刻，则IMU在参考坐标系/>下的位姿与GNSS定位结果之间的关系可写成以下差分形式：
(12)，
式中，表示i时刻IMU相对于相机在参考坐标系/>下的位置，/>表示j时刻IMU相对于相机在参考坐标系/>下的位置，/>表示i时刻IMU和相机在参考坐标系下的姿态变换矩阵，/>表示j时刻IMU和相机在参考坐标系/>下的姿态变换矩阵，表示GNSS与IMU之间的杆臂，/>表示i时刻GNSS解算的/>系下的位置，表示j时刻GNSS解算的/>系下的位置。
步骤（5）由于IMU器件的零偏误差会影响速度和位置的预积分，进而导致重力估计误差和相对旋转矩阵，因此需要估计加速度计的零偏。若记加速度计零偏的扰动误差为：
(13)，
式中，为加速度计零偏的真值，/>为加速度计零偏的当前估计。假设GNSS/INS/视觉的世界坐标系（地心地固坐标系）/>与视觉里程计的参考坐标系/>之间的当前相对旋转矩阵为/>，估计的误差为/>，则：
(14)，
位置和速度预积分误差可以通过加速计零偏进行补偿：
(15)，
式中，分别表示预积分估计的理想位置和速度，分别表示预积分的估计位置与速度，/>分别表示位置和速度预积分估计的加速度计零偏的雅可比矩阵，/>表示加速度计零偏的扰动误差，其可通过QR分解方式求解，进而得到修正后的零偏，并根据该修正后的值重新IMU预积分。
步骤（6）认为卫星导航/惯性/视觉的世界坐标系（地心地固坐标系）与视觉里程计的参考坐标系/>之间的当前估计相对旋转矩阵/>趋于平稳之后则认为动基座初始化已完成。进一步地，认为在n个估计值中，若随机挑选2个估值之间的均方根误差都小于一定的阈值，则认为已经收敛。当前估计相对旋转矩阵/>写成四元数的形式/>，假设当前估计相对旋转矩阵连续的n个估计值为/>，则任意两个之间的均方根误差为：
(16)。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，包括如下步骤：
步骤（1） 通过残差卡方检验法对GNSS的所有卫星测量值进行检验，判断卫星是否存在故障；如果不存在故障，则将解算RTK定位结果与视觉惯性里程计结果进行优化，如果存在故障，则使用残差卡方检验法对故障卫星进行检测、排除故障卫星；
步骤（2） 引入非完整性约束，认为车辆在行驶的过程中不会发生侧滑和跳跃，则载体坐标系b系下车辆的右向速度和天向速度近似为0，对惯性机械编排结果进行约束，包括：
认为车辆在行驶的过程中不会发生侧滑和跳跃，此时在载体坐标系b系下，车辆的右向速度和天向速度近似为0，即：
(1)
式中，的上标b表示在载体坐标系b系下的速度，下标分别表示右向和天向的速度；
(2)
式中，表示地心地固坐标系e系到载体坐标系b系的方向余弦矩阵，/>表示地心地固坐标系e系下的速度，/>表示载体坐标系b系下的速度；
对式(2)添加扰动误差得到：
(3)
式中，、/>分别表示b系和e系下的速度误差项，/>表示单位矩阵，/>表示失准角，/>表示反对称矩阵符号，对式(3)展开并去掉二次误差项得到：
(4)
因为，则式(4)写成：
(5)
将式(2)代入式(5)得到：
(6)
将式(6)写成矩阵形式得到：
(7)
(8)
式中，表示非完整性约束的速度，/>表示INS机械编排推算的速度，/>表示加速计的零偏，/>表示陀螺的零偏，/>表示地心地固坐标系e系下的失准角误差，分别表示地心地固坐标系e系下的速度误差和位置误差，/>表示非完整性约束噪声，/>表示INS推算的载体坐标系b系下的前向速度，也表明不对前向速度加入约束；
式(7)即对INS机械编排结果进行约束；
步骤（3） 估计相机与惯性测量器件之间的旋转和陀螺的零偏，构建最小二乘计算相机与惯性测量器件之间的相对旋转最优估计值；
步骤（4） 利用重力及载体运动的相对平移估计卫星导航/惯性/视觉的世界坐标系与视觉惯性里程计的参考坐标系之间的相对旋转矩阵；
步骤（5）估计加速度计的零偏；
步骤（6）卫星导航/惯性/视觉的世界坐标系与视觉惯性里程计的参考坐标系之间的相对旋转矩阵趋于平稳之后，即完成动基座初始化。
2.根据权利要求1所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（3）包括：
构建最小二乘问题得到相机与惯性测量器件之间的相对旋转最优估计值：
(9)
式中，为预先采用手眼标定法得到的相机与惯性测量器件之间的相对旋转估计值，/>表示相机与惯性测量器件之间的最优旋转估计值，/>表示目标函数，为约束条件； argmin表示使/>函数达到最小值时/>取值的函数；
以相机与惯性测量器件之间的最优旋转估计值和相机位姿估计值作为估计陀螺零偏的基准，记陀螺零偏的扰动误差为：
(10)
式中，为陀螺零偏的真值，/>为计算姿态预积分时的陀螺零偏；
则零偏校正后的姿态预积分与基准之间满足：
(11)
式中，表示k与k-1时刻之间的姿态变换矩阵，/>表示k-1到k时刻的姿态预积分，/>为雅可比矩阵，/>表示四元数的乘法符号，/>表示陀螺仪零偏误差导致的姿态预积分误差，通过QR分解得到陀螺的零偏误差，进而得到修正后的零偏，并根据该修正后的值重新惯性测量器件预积分。
3.根据权利要求2所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（4）包括：
假设GNSS的定位发生在和/>时刻，则惯性测量器件在参考坐标系/>下的位姿与GNSS定位结果之间的关系写成以下差分形式：
(12)
式中，表示i时刻IMU相对于相机在参考坐标系/>下的位置，/>表示j时刻IMU相对于相机在参考坐标系/>下的位置，/>表示i时刻IMU和相机在参考坐标系/>下的姿态变换矩阵，/>表示j时刻IMU和相机在参考坐标系/>下的姿态变换矩阵，/>表示GNSS与IMU之间的杆臂，/>表示i时刻GNSS解算的/>系下的位置，/>表示j时刻GNSS解算的/>系下的位置。
4.根据权利要求3所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（5）包括：
记加速度计零偏的扰动误差为：
(13)
式中，为加速度计零偏的真值，/>为加速度计零偏的当前估计；
假设GNSS/INS/视觉的世界坐标系与视觉惯性里程计的参考坐标系/>之间的当前相对旋转矩阵为/>，估计的误差为/>，则：
(14)
位置和速度预积分误差通过加速计零偏进行补偿：
(15)
式中，分别表示预积分估计的理想位置和速度，/>分别表示预积分的估计位置与速度，/>分别表示位置和速度预积分估计的加速度计零偏的雅可比矩阵，/>表示加速度计零偏的扰动误差，通过QR分解方式求解，进而得到修正后的零偏，并根据该修正后的值重新惯性测量器件预积分。
5.根据权利要求4所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（6）包括：
将当前估计的相对旋转矩阵写成四元数的形式/>，假设当前估计的相对旋转矩阵/>连续的n个估计值为/>，则任意两个估计值之间的均方根误差为：
(16)
在n个估计值中，若随机挑选2个估计值之间的均方根误差均小于阈值，则认为收敛。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310821874.2A CN116540286B (zh) | 2023-07-06 | 2023-07-06 | 基于非完整性约束的多源鲁棒融合定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310821874.2A CN116540286B (zh) | 2023-07-06 | 2023-07-06 | 基于非完整性约束的多源鲁棒融合定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116540286A CN116540286A (zh) | 2023-08-04 |
| CN116540286B true CN116540286B (zh) | 2023-08-29 |
Family
ID=87456407
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310821874.2A Active CN116540286B (zh) | 2023-07-06 | 2023-07-06 | 基于非完整性约束的多源鲁棒融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116540286B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117368946B (zh) * | 2023-10-19 | 2024-05-28 | 齐鲁空天信息研究院 | 基于多基准站实时数据流的卫星广播星历综合方法和系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2893717A1 (en) * | 2015-06-03 | 2016-12-03 | Novatel Inc. | Method for using partially occluded images for navigation and positioning |
| CN110411462A (zh) * | 2019-07-22 | 2019-11-05 | 武汉大学 | 一种gnss/惯性/车道线约束/里程计多源融合方法 |
| CN111780755A (zh) * | 2020-06-30 | 2020-10-16 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
| AU2020103576A4 (en) * | 2019-12-27 | 2021-02-04 | Wuhan University | Autonomous orbit and attitude determination method of low-orbit satellite based on non-navigation satellite signal |
| CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
-
2023
- 2023-07-06 CN CN202310821874.2A patent/CN116540286B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2893717A1 (en) * | 2015-06-03 | 2016-12-03 | Novatel Inc. | Method for using partially occluded images for navigation and positioning |
| CN110411462A (zh) * | 2019-07-22 | 2019-11-05 | 武汉大学 | 一种gnss/惯性/车道线约束/里程计多源融合方法 |
| AU2020103576A4 (en) * | 2019-12-27 | 2021-02-04 | Wuhan University | Autonomous orbit and attitude determination method of low-orbit satellite based on non-navigation satellite signal |
| CN111780755A (zh) * | 2020-06-30 | 2020-10-16 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
| CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| 城市环境下GNSS/INS/视觉集成定位技术研究;蔚科;《中国优秀硕士学位论文全文数据库 基础科学》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116540286A (zh) | 2023-08-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7199163B2 (ja) | 位置推定方法、装置及びコンピュータプログラム | |
| CN110702104B (zh) | 一种基于车辆零速检测的惯性导航误差修正方法 | |
| CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
| CN111854740B (zh) | 能够在交通工具中进行航位推算的惯性导航系统 | |
| JP2009019992A (ja) | 位置検出装置及び位置検出方法 | |
| CN116540286B (zh) | 基于非完整性约束的多源鲁棒融合定位方法 | |
| Jung et al. | Monocular visual-inertial-wheel odometry using low-grade IMU in urban areas | |
| CN110207693B (zh) | 一种鲁棒立体视觉惯性预积分slam方法 | |
| CN114002725A (zh) | 一种车道线辅助定位方法、装置、电子设备及存储介质 | |
| JP2020169872A (ja) | 慣性航法装置 | |
| CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN111928847A (zh) | 惯性测量单元位姿数据优化方法、装置及电子设备 | |
| CN108613675B (zh) | 低成本无人飞行器移动测量方法及系统 | |
| CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
| CN112797985A (zh) | 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统 | |
| JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
| CN112880669A (zh) | 一种航天器星光折射和单轴旋转调制惯性组合导航方法 | |
| Zhao et al. | A high-accuracy autonomous navigation scheme for the Mars rover | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| Forno et al. | Techniques for improving localization applications running on low-cost IoT devices | |
| CN112923934A (zh) | 一种适用于非结构化场景中结合惯导的激光slam技术 | |
| Atia et al. | A novel systems integration approach for multi-sensor integrated navigation systems | |
| CN113375665B (zh) | 基于多传感器松紧耦合的无人机位姿估计方法 |
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 |