买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：华航导控(天津)科技有限公司

摘要：本发明提供一种极区因子图构建方法和极区组合导航方法，步骤一，依据定位系统初始状态构建因子图模型的先验因子；步骤二，生成新因子图模型：添加GNSS变量因子和IMU变量因子，其中k为正整数，表示第k个GNSS变量因子或IMU变量因子，相邻IMU变量因子通过IMU因子节点关联，GNSS变量因子通过GNSS因子节点与同时刻的IMU变量因子关联，以生成新因子图模型；步骤三，依据变量节点的变量周期添加对应因子节点；步骤四，优化新因子图模型：ISAM2求解器依据新因子图模型和添加的变量节点重置因子节点和变量节点，跳转至步骤二。本发明能够适应极区各种复杂环境，并进行精准定位。

主权项：1.一种极区因子图构建方法，其特征在于，包括，步骤一，依据定位系统初始状态构建因子图模型的先验因子；步骤二，生成新因子图模型：添加GNSS变量因子和IMU变量因子，其中k为正整数，表示第k个GNSS变量因子或IMU变量因子，相邻IMU变量因子通过IMU因子节点关联，GNSS变量因子通过GNSS因子节点与同时刻的IMU变量因子关联，以生成新因子图模型；步骤三，依据变量节点的变量周期添加对应因子节点；步骤四，优化新因子图模型：ISAM2求解器依据新因子图模型和添加的变量节点重置因子节点和变量节点，跳转至步骤二；所述变量节点包括GNSS变量和IMU变量，其中k为正整数，表示第k个GNSS变量或IMU变量，所述因子节点包括IMU因子节点和GNSS因子节点；两相邻IMU变量之间包括IMU因子节点，生成所述IMU因子节点包括：两相邻IMU因子节点之间间隔一变量周期，计算变量周期预积分的同时进行设备扰动误差补偿、坐标系下的牵连角速度补偿和地球自转角速度补偿，以生成该变量周期的极区IMU预积分，依据该变量周期的IMU实际积分和极区IMU预积分计算出极区IMU预积分残差，定义成IMU因子节点；所述极区IMU预积分包括速度预积分、位置预积分和姿态预积分，横地理坐标系下，k-1时刻到k时刻之间的速度预积分、位置预积分和姿态预积分，其公式为： ， ，其中：速度预积分初值，位置预积分初值，姿态预积分初值，为时刻下的加速度测量值，为k-1时刻的加速度计偏差，为为k-1时刻的陀螺仪偏差，为到的姿态四元数的增量；姿态四元数增量的具体公式为： ，其中，为m时刻的角速度测量，为k-1时刻地球自转角速度在横地理坐标系下的投影，为k-1时刻横地理坐标系相对于横地球系的转动角速度在横坐标系下的投影，用以补充极区的姿态误差；所述先验因子节点为，IMU因子节点为，GNSS因子节点为，所述因子图模型的数字表达式为： ，其中，为目标函数最小时的状态量。

全文数据：

权利要求：

百度查询： 华航导控(天津)科技有限公司 一种极区因子图构建方法和极区组合导航方法