【技术实现步骤摘要】
本专利技术属于多源融合导航。
技术介绍
1、因子图优化算法可以根据系统状态、量测等导航信息构建因子图节点模型，基于最大后验概率估计理论实现导航数据的融合。因子图提供了一个非常灵活的组合导航框架，可以融合多源组合导航系统中导航传感器数据，提升组合导航系统的适应性。
2、在复杂城市环境中，单一的导航信息源易受到外部环境干扰导致无法使用，车载系统往往采用多源信息融合的方式进行导航。目前常用的信息融合方法采用集中式滤波、联邦滤波或因子图优化，当复杂环境引起的部分传感器故障时，集中式滤波和联邦滤波必须对系统主滤波器进行重构，这将影响导航系统的实时性和可靠性。因子图优化虽然具有即插即用特性容易实现模型重组，但无法主动判别外部干扰，导致定位精度大大降低。因此，有效的抗干扰导航手段具有良好的车载应用前景。
技术实现思路
1、专利技术目的：为了解决上述现有技术存在的问题，本专利技术提供了一种基于因子图的交互模型自适应组合导航方法。
2、技术方案：本专利技术公开了一种基于因子图的交互模型自适应组合导航方法，具体包括如下步骤：
3、步骤1：获传感器系统中每个传感器的初始量测信息并进行解算，所述传感器系统包括imu，卫星和激光雷达；
4、步骤2：构建因子图组合导航系统模型：构建系统状态方程，通过滑动窗口进行系统状态优化，将系统状态变量构建为变量节点，根据imu预积分解算出的位置、速度和姿态构建imu因子节点，根据卫星解算出的位置和速度构建gnss因子节点，根
5、步骤3：获取gnss因子节点和lidar因子节点的位置解算信息并构建交互式模型算法子系统模型，该子模型包括gnss导航系统模型i和lidar导航系统模型j；
6、步骤4：基于因子图组合导航系统模型和上一个时刻gnss导航系统模型和lidar导航系统模型在因子图组合导航系统模型中的模型转移概率，求解当前时刻k时系统状态变量最优值
7、步骤5：依据交互模型算法计算当前时刻lidar导航系统模型的模型转移概率，并以相同的方法计算当前时刻gnss导航系统模型的模型转移概率，将计算得到的当前时刻的模型转移概率用于求解下一个时刻系统状态变量。
8、进一步的，所述步骤1中imu的初始量测信息包括初始陀螺仪和加速度计零偏，imu的解算方法为imu预积分解算；卫星的初始量测信息为卫星的星历数据，卫星的解算方法为伪距差分解算；激光雷达的初始量测信息为激光雷达外参标定，激光雷达的解算方法为点云匹配迭代解算。
9、进一步的，所述步骤2中系统状态方程表达式相同，具体表达式如下所示：
10、xk＝fk/k-1xk-1+wk-1
11、其中，xk表示当前时刻k时系统的状态变量，xk-1表示时刻k-1时系统的状态变量，fk/k-1表示传感器系统状态转移矩阵，wk-1表示时刻k-1时传感器系统的噪声矩阵；
12、xk的表达式为：
13、
14、其中，是姿态角误差，δv是速度误差，δp是位置误差,ba和bg分别为imu的加度计和陀螺仪的偏差，t表示转置；
15、步骤2中，imu因子节点的表达式如下所示：
16、
17、其中，表示时刻k时imu的imu因子节点方程，exp(.)表示指数函数，σimu表示im的协方差矩阵,表示时刻k时imu的量测向量，的表达式如下所示：
18、
19、其中，himu表示imu的量测矩阵，表示时刻k时imu的量测噪声矩阵；
20、gnss因子节点和lidar因子节点的表达式如下所示：
21、
22、
23、其中，表示时刻k时卫星的gnss因子节点方程，表示时刻k时激光雷达的lidar因子节点方程，∑gnss表示卫星的协方差矩阵，∑lidar表示激光雷达的协方差矩阵，时刻k时卫星的量测向量，表示时刻k激光雷达的量测向量；和的表达式如下所示：
24、
25、
26、其中，hk表示传感器量测矩阵，为时刻k时卫星的量测噪声矩阵，为时刻k时激光雷达的量测噪声矩阵；
27、步骤2中的先验节点和误差因子节点的表达式形式相同，具体如下所示：
28、
29、其中，fk(x)表示先验节点方程或者误差因子节点方程，当fk(x)表示先验节点方程时，σ’表示先验节点的协方差矩阵，x表示先验节点参数，μx表示先验节点参数的均值；当fk(x)表示误差因子节点方程，σ’表示误差因子节点的协方差矩阵，x表示误差因子节点参数，μx表示误差因子节点的均值。
30、进一步的，所述步骤4中根据如下公式求解当前时刻系统最优状态变量
31、其中，hprior表示先验方程，zimu表示时刻k时imu的量测向量，表示k-1时刻gnss导航系统模型的量测向量，表示k-1时刻lidar导航系统模型的量测向量；∑imu表示imu的协方差矩阵，∑i表示gnss导航系统模型的协方差矩阵，∑j表示lidar导航系统模型的协方差矩阵；himu表示imu量测方程，表示时刻k-1时gnss导航系统模型的模型转移概率，表示时刻k-1时lidar导航系统模型的模型转移概率，n表示滑动窗口的长度。
32、进一步的，所述步骤5中根据计算当前时刻lidar导航系统模型的模型转移概率具体为：
33、步骤5.1：交互模型算法中模型输入交互的过程如下所示：
34、
35、
36、
37、
38、
39、其中，表示时刻时模型j的归一化常数，pij表示模型i到子模型j初始转移概率，表示时刻k-1时模型i的模型转移概率，表示模型i转移到模型j的转移概率，表示模型j自主转移概率，表示k-1时刻lidar导航系统模型的混合量测状态初值，表示k-1时刻子lidar导航系统模型的混合协方差矩阵，表示k-1时刻lidar导航系统模型的量测输入，表示k-1时刻gnss导航系统模型的输入协差矩阵，和分别表示时刻k-1时模型i的量测输入和量测协方差矩阵；
40、步骤5.2：采用如下公式计算当前时刻k时gnss导航系统模型的模型转移概率：
41、
42、其中，ck表示时刻k时归一化常数，为似然函数，的表达式如下所示：
43、
44、其中，exp(.)表示指数函数，t表示转置，表示lidar导航系统模型状态残差，其中hj表示lidar导航系统模型量测方程，表示lidar导航系统模型状态残差的协方差，表示时刻k时lidar导航系统模型的协方差矩阵，
45、有益效果：本专利技术引入了交互模型自适应滤波算法使导本文档来自技高网...
【技术保护点】
1.一种基于因子图的交互模型自适应组合导航方法，其特征在于，具体包括如下步骤：
2.根据权利要求1所述的一种基于因子图的交互模型自适应组合导航方法，其特征在于，所述步骤1中IMU的初始量测信息包括初始陀螺仪和加速度计零偏，IMU的解算方法为IMU预积分解算；卫星的初始量测信息为卫星的星历数据，卫星的解算方法为伪距差分解算；激光雷达的初始量测信息为激光雷达外参标定，激光雷达的解算方法为点云匹配迭代解算。
3.根据权利要求1所述的一种基于因子图的交互模型自适应组合导航方法，其特征在于，所述步骤2中系统状态方程表达式相同，具体表达式如下所示：
4.根据权利要求1所述的一种基于因子图的交互模型自适应组合导航方法，其特征在于，所述步骤4中根据如下公式求解当前时刻系统最优状态变量
5.根据权利要求1所述的一种基于因子图的交互模型自适应组合导航方法，其特征在于，所述步骤5中根据计算当前时刻LIDAR导航系统模型的模型转移概率具体为：
【技术特征摘要】
1.一种基于因子图的交互模型自适应组合导航方法，其特征在于，具体包括如下步骤：
2.根据权利要求1所述的一种基于因子图的交互模型自适应组合导航方法，其特征在于，所述步骤1中imu的初始量测信息包括初始陀螺仪和加速度计零偏，imu的解算方法为imu预积分解算；卫星的初始量测信息为卫星的星历数据，卫星的解算方法为伪距差分解算；激光雷达的初始量测信息为激光雷达外参标定，激光雷达的解算方法为点云匹配迭代解算。
3.根据权...
【专利技术属性】
技术研发人员：曾庆化，王守一，邵晨，李方东，许睿，孙永荣，刘建业，卓鹏，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。