【技术实现步骤摘要】
本专利技术涉及同时定位与地图构建，具体涉及一种基于因子图优化的gnss惯导融合定位方法。
技术介绍
1、同时定位与建图(simultaneous localization and mapping，slam)是一种在机器人和自动驾驶领域中广泛应用的先进技术。它利用传感器信息，让机器人在未知环境中自我导航并构建出详细的地图。slam技术对于实现精准定位和建立环境模型具有重要意义，可以帮助机器人在复杂的现实世界中实现高效的导航和路径规划。
2、尽管传统的惯性导航具有体积小、重量轻、非接触和小范围内定位精度高等诸多优势，但它只能提供局部框架的相对位姿变化，无法恢复4自由度的全局偏航和位置信息，而且存在估计误差累积(漂移)的问题。因此，在惯性导航系统中引入其他传感器来提高定位精度和鲁棒性变得至关重要。为了弥补惯性导航的不足，全球导航卫星系统(globalnavigation satellite system，gnss)被引入作为slam的潜在传感器融合对象。gnss具有无飘移特性，可以提供高精度的全球定位信息，与惯性导航的互补特性相结合，可以极大地提高定位的平滑性、一致性和鲁棒性。通过将gnss数据与惯性测量单元(imu)的数据进行融合，可以有效地消除imu的漂移问题，并实现高精度的定位和导航。然而，gnss-惯导融合定位导航方法存在一些弊端。首先，gnss信号在室内、城市峡谷和密集的建筑物遮挡区域等复杂环境下容易受到干扰和衰减，导致定位精度下降甚至无法获取有效的定位信息。这种信号受限的问题限制了传统gnss-惯导融合方法在一
技术实现思路
1、针对现有技术中的上述不足，本专利技术提供了一种基于因子图优化的gnss惯导融合定位方法。
2、为了达到上述专利技术目的，本专利技术采用的技术方案为：
3、一种基于因子图优化的gnss惯导融合定位方法，包括以下步骤：
4、s1、获取惯性测量单元的测量数据、全球导航卫星系统接收器的gnss信号数据；
5、s2、对步骤s1中获取的惯性测量单元的测量数据进行预积分，得到积分处理的测量数据；
6、s3、检查步骤s1中获取的全球导航卫星系统接收器的gnss信号数据的有效性，得到最优的gnss信号数据；
7、s4、根据步骤s2中得到的积分处理的测量数据与步骤s3中得到的最优的gnss信号数据，构建基于测量数据和gnss信号数据的因子图模型；
8、s5、采用增量平滑算法对步骤s4中构建的基于测量数据和gnss信号数据的因子图模型进行求解，得到搭载物在地球坐标系下的位姿状态。
9、进一步地，步骤s1具体包括：
10、s11、读取惯性测量单元的加速度测量值与角速度测量值，得到惯性测量单元的测量数据；
11、s12、读取全球导航卫星系统接收器的伪码距、多普勒频移、导航信息以及载波相位数据，得到全球导航卫星系统接收器的gnss信号数据。
12、进一步地，步骤s2具体包括：
13、将设定时间段内采集的加速度测量值与角速度测量值利用积分公式进行预积分，得到积分处理的测量数据。
14、进一步地，积分处理的测量数据为搭载物相对于世界坐标系的相对位置、速度与旋转位置，具体计算过程如下：
15、
16、其中，表示第i时刻到第j时刻自身参考坐标系b下搭载物相对于世界坐标系的相对位置，表示从自身参考坐标系bi到自身参考坐标系bt的坐标变换矩阵，表示第t时刻的加速度测量值，表示第t时刻加速度的随机游走，表示第t时刻的加速度计白噪声，表示第i时刻到第j时刻自身参考坐标系b下搭载物相对于世界坐标系的相对速度，na表示加速度计白噪声，表示第i时刻到第j时刻搭载物相对于世界坐标系的相对旋转位置，ω表示中间临时变量，无实际意义，表示第t时刻的陀螺仪测量的角速度测量值，表示第t时刻角速度随机游走，nw表示陀螺仪角速度白噪声，表示第t时刻在自身参考坐标系bi下的方向信息，at表示第t时刻不含噪声的加速度，表示从自身参考坐标系bt到世界坐标系w的坐标变换矩阵，gw表示世界坐标系w下的加速度，wt表示第t时刻不含噪声的角速度，wx、wy、wz分别表示在x轴、y轴与z轴的世界坐标系，表示第j时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的位置，表示第i时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的位置，表示第i时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的速度，δti表示第i到i+1时刻之间的时间差，表示从世界坐标系w到自身参考坐标系bt的坐标变化矩阵，表示第j时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的速度，表示从世界坐标系w到自身参考坐标系bi的坐标变化矩阵，表示第t时刻在自身参考坐标系bi下的方向，表示汉密尔顿四元数的乘法操作。
17、进一步地，步骤s3具体包括：
18、s31、根据步骤s12中得到的全球导航卫星系统接收器的gnss信号数据，获取gnss信号数据中的轨道参数、时钟误差修正、电力层延迟系数以及卫星状态信息；
19、s32、根据步骤s31中获取的卫星状态信息，剔除全球导航卫星系统中卫星偏角小以及锁定次数少的卫星提供的gnss信号数据，得到最优的gnss信号数据。
20、进一步地，步骤s4具体包括：
21、s41、根据步骤s2中得到的积分处理的测量数据与定位系统初始状态，构建惯性测量单元因子，得到系统状态与积分处理的测量数据之间的残差模型，即：
22、
23、其中，rb(·)表示系统状态与积分处理的测量数据之间的残差模型，表示从第i到第j时刻惯性测量单元的给定观测值，χ表示定位系统的未知状态，δ表示误差，表示第i时刻到第j时刻自身参考坐标系b下搭载物相对于世界坐标系的相对位置，表示第i时刻到第j时刻自身参考坐标系b下搭载物相对于世界坐标系的相对速度，表示第i时刻到第j时刻搭载物相对于世界坐标系的相对旋转位置，ba表示加速度误差，bw表示角速度误差，表示从自身参考坐标系bi到世界坐标系w的坐标变化矩阵，表示第j时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的位置，表示第i时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的位置，表示第i时刻自身参考坐标系b下惯性测量单元的搭载物在世界坐标系w下的速度，δti表示第i到i+1时刻之间的时间差，gw表示世界坐标系w下的加速度，表示第i时刻到第j时刻所测量的相对位置，表示第j本文档来自技高网...
【技术保护点】
1.一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S1具体包括：
3.根据权利要求2所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S2具体包括：
4.根据权利要求3所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，积分处理的测量数据为搭载物相对于世界坐标系的相对位置、速度与旋转位置，具体计算过程如下：
5.根据权利要求2所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S3具体包括：
6.根据权利要求1所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S4具体包括：
7.根据权利要求6所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S46中基于测量数据和GNSS信号数据的因子图模型为：
8.根据权利要求1所述的一种基于因子图优化的GNSS惯导融合定位方法，其特征在于，步骤S5具体包括：
【技术特征摘要】
1.一种基于因子图优化的gnss惯导融合定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于因子图优化的gnss惯导融合定位方法，其特征在于，步骤s1具体包括：
3.根据权利要求2所述的一种基于因子图优化的gnss惯导融合定位方法，其特征在于，步骤s2具体包括：
4.根据权利要求3所述的一种基于因子图优化的gnss惯导融合定位方法，其特征在于，积分处理的测量数据为搭载物相对于世界坐标系的相对位置、速度与旋转位置，具体计算过程如下：
【专利技术属性】
技术研发人员：杨廷勇，程天满，谷彩香，刘准，何宏江，
申请(专利权)人：中国长江电力股份有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。