本发明专利技术公开了一种基于非完整性约束的多源鲁棒融合定位方法，基于车载导航场景下的特征，引入非完整性因子对惯性机械编排解算结果进行约束，对于GNSS卫星观测值基于残差卡方检验法RCTM算法进行卫星信号故障检测与排除，最后利用动基座初始化算法加速多源融合系统的快速部署，通过非线性优化的方式来实现卫星导航/惯性/视觉的多源鲁棒融合定位。航/惯性/视觉的多源鲁棒融合定位。航/惯性/视觉的多源鲁棒融合定位。
【技术实现步骤摘要】
基于非完整性约束的多源鲁棒融合定位方法
[0001]本专利技术属于多源传感器融合定位
，具体涉及一种基于非完整性约束的多源鲁棒融合定位方法。
技术介绍
[0002]随着数字化科技的飞速发展，越来越多的无人车、无人机、无人快递配送、自主机器人等智慧系统慢慢走进人们的生活，人们对于导航定位技术也提出了更高的要求，如何实现城市复杂环境下GNSS(Global Navigation Satellite System, GNSS)信号失锁甚至不可用的情况下仍能保持高精度、连续性、稳健、高可靠的导航定位成为了学术界和工业界关注的热点。目前，GNSS/INS（Inertial Navigation System, INS）组合导航系统技术已经成熟且应用广泛，能够连续地提供高精度的定位信息，许多学者也提出抗差自适应滤波算法来保证系统的稳健可靠性，但是只能保证GNSS短时间中断的高精度定位，当GNSS信号长时间中断不可用时，该系统的导航误差会迅速累积，无法满足高精度定位的需求。因此，这时就需要借助别的传感器来辅助导航定位。相机由于其成本低、方便携带、自主无源、信息丰富的特点，被人们广泛应用于计算机视觉和机器人领域。在城市复杂环境中相机所获得的信息往往比较丰富，而GNSS信号质量较差，视觉惯性导航表现出很好的优势，一方面，INS/视觉的组合可以在线估计陀螺零偏误差，有效减小航向的发散速度，另一方面，单目相机存在的尺度缺失问题可以通过IMU有效的解决；而在相机特征点较少的场合，GNSS的信号往往较好，GNSS/INS组合则能提供高精度定位，可见GNSS、INS、视觉这三种导航定位技术存在极好的互补性，克服了单一传感器或两种传感器融合定位的局限性。GNSS/INS/视觉融合定位技术的基本思想是利用事先确定的最优准则，对各传感器在不同时间、不同的空间观测的数据信息进行融合，获得系统的状态参数，提高导航解算精度，改进定位系统的质量控制，具有重要的现实意义和实际价值。
[0003]目前GNSS/INS/视觉融合定位的方式主要分为基于滤波和基于优化两种方式。采用松耦合的滤波融合方式由于对于视觉信息利用不充分，定位定姿的精度还需要进一步的提高；另一方面，利用GNSS的观测值来估计相机帧之间的尺度信息，当GNSS卫星不可用时该方法则失效。基于优化采用因子图的方式充分利用系统的稀疏性实现最优解算，但随着时间的增长存在误差向下累积的情况，另外，由于因子图优化的函数强非线性，如果初始化的参数不准确可能导致迭代估计时间较长甚至不收敛的问题。传感器的数据是否可靠是决定融合算法的根源，多源信息数据的质量评估、故障排除及权重分配将会是鲁棒性算法的未来方向。
技术实现思路
[0004]为了解决上述问题，本专利技术提出一种基于非完整性约束的多源鲁棒融合定位方法，基于车载导航场景下的特征，引入非完整性因子对惯性机械编排解算结果进行约束，对于GNSS观测值基于残差卡方检验法RCTM（Residual Chi
‑
square Test Method, RCTM）算法
进行卫星信号故障检测与排除，最后利用动基座初始化算法加速多源融合系统的快速部署，通过非线性优化的方式来实现卫星导航/惯性/视觉的多源鲁棒融合定位。
[0005]本专利技术实现其目的所采取的技术方案是：一种基于非完整性约束的多源鲁棒融合定位方法，包括如下步骤：步骤（1） 通过残差卡方检验法对GNSS的所有卫星测量值进行检验，判断是否卫星存在故障；如果不存在故障，则将解算RTK定位结果与视觉惯性里程计结果进行优化，如果存在故障，则使用残差卡方检验法对故障卫星进行检测、排除故障卫星；步骤（2） 引入非完整性约束，认为车辆在行驶的过程中不会发生侧滑和跳跃，则载体坐标系b系下车辆的右向速度和天向速度近似为0，对INS机械编排结果进行约束；步骤（3） 估计相机与惯性测量器件之间的旋转和陀螺的零偏，构建最小二乘计算相机与惯性测量器件之间的相对旋转最优估计值；步骤（4） 利用重力及载体运动的相对平移估计卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵；步骤（5） 估计加速度计的零偏；步骤（6） 卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵趋于平稳之后，即完成动基座初始化。
[0006]进一步地，所述步骤（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机械编排结果进行约束。
[0007]进一步地，所述步骤（3）包括：构建最小二乘问题得到相机与惯性测量器件之间的相对旋转最优估计值：(9)，式中，为预先采用手眼标定法得到的相机与惯性测量器件之间的相对旋转估计值，表示相机与惯性测量器件之间的最优旋转估计值，表示目标函数，为约束条件； argmin表示使函数达到最小值时取值的函数；以相机与惯性测量器件之间的最优旋转估计值和相机位姿估计值作为估计陀螺零偏的基准，记陀螺零偏的扰动误差为：(10)，式中，为陀螺零偏的真值，为计算姿态预积分时的陀螺零偏；则零偏校正后的姿态预积分与基准之间满足：(11)，式中，表示k与k
‑
1时刻之间的姿态变换矩阵，表示k
‑
1到k时刻的姿态预积分，为雅可比矩阵，表示四元数的乘法符号，表示陀螺仪零偏误差导致的姿态预积分误差，通过QR分解得到陀螺的零偏误差，进而得到修正后的零偏，并根据该修正后的值重新惯性测量器件预积分。
[0008]进一步地，所述步骤（4）包括：假设GNSS的定位发生在和时刻，则惯性测量器件在参考坐标系下的位姿与GNSS定位结果之间的关系写成以下差分形式：(12)，式中，表示i时刻IMU相对于相机在参考坐标系下的位置，表示j时刻IMU相对于相机在参考坐标系下的位置，表示i时刻IMU和相机在参考坐标系下的姿态变换矩阵，表示j时刻IMU和相机在参考坐标系下的姿态变换矩阵，表示GNSS与IMU之间的杆臂，表示i时刻GNSS解算的系下的位置，表示j时刻GNSS解算的系下的位置。
[0009]进一步地，所述步骤（5）包括本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，包括如下步骤：步骤（1） 通过残差卡方检验法对GNSS的所有卫星测量值进行检验，判断卫星是否存在故障；如果不存在故障，则将解算RTK定位结果与视觉惯性里程计结果进行优化，如果存在故障，则使用残差卡方检验法对故障卫星进行检测、排除故障卫星；步骤（2） 引入非完整性约束，认为车辆在行驶的过程中不会发生侧滑和跳跃，则载体坐标系b系下车辆的右向速度和天向速度近似为0，对惯性机械编排结果进行约束；步骤（3） 估计相机与惯性测量器件之间的旋转和陀螺的零偏，构建最小二乘计算相机与惯性测量器件之间的相对旋转最优估计值；步骤（4） 利用重力及载体运动的相对平移估计卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵；步骤（5）估计加速度计的零偏；步骤（6）卫星导航/惯性/视觉的世界坐标系与视觉里程计的参考坐标系之间的相对旋转矩阵趋于平稳之后，即完成动基座初始化。2.根据权利要求1所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（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.根据权利要求2所述的一种基于非完整性约束的多源鲁棒融合定位方法，其特征在于，所述步骤（3）包括：构建最小二乘问题得到相机与惯性测量器件之间的相对旋转最优估计值：(9)，式中，为预先采用手眼标定法得到的相机与惯性测量器件之间的相对旋转估计值，表示...
【专利技术属性】
技术研发人员：汪亮，蔚科，李子申，王宁波，孟磊，刘振耀，王亮亮，尹心彤，
申请(专利权)人：齐鲁空天信息研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。