[发明专利]基于角加速度计/陀螺/加速度计的惯性导航解算方法有效

专利信息
申请号: 201910354258.4 申请日: 2019-04-29
公开(公告)号: CN110207697B 公开(公告)日: 2023-03-21
发明(设计)人: 吕品;白师宇;赖际舟;李志敏;刘士超 申请(专利权)人: 南京航空航天大学
主分类号: G01C21/18 分类号: G01C21/18;G01C25/00
代理公司: 南京经纬专利商标代理有限公司 32200 代理人: 姜慧勤
地址: 210016 江*** 国省代码: 江苏;32
权利要求书: 查看更多 说明书: 查看更多
摘要:
搜索关键词: 基于 角加速度 陀螺 加速度计 惯性 导航 方法
【权利要求书】:

1.基于角加速度计/陀螺/加速度计的惯性导航解算方法,其特征在于,包括如下步骤:

步骤1,周期读取k时刻载体的传感器信息,包括三轴角加速度计输出三轴陀螺输出三轴加速度计输出

步骤2,根据k时刻的传感器信息,预测k时刻载体的角速度、姿态、速度、位置信息;具体过程如下:

1)采用如下公式预测载体角速度:

其中,为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的分量,为k-1时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的分量,为k时刻机体系相对于惯性系的角加速度在机体系X、Y、Z轴上的分量,ΔT为离散采样周期;

2)采用如下公式预测载体姿态:

其中,a=x,y,z,q0(k)、q1(k)、q2(k)、q3(k)为k时刻的姿态四元数,q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻的姿态四元数,为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量,为k-1时刻的姿态转移矩阵,为k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量;

3)采用如下公式预测载体速度:

其中,为k时刻载体速度在导航系X、Y、Z轴上的分量,为k-1时刻载体速度在导航系X、Y、Z轴上的分量,L(k-1)、h(k-1)为k-1时刻载体的纬度、高度,RM、RN为地球的子午圈、卯酉圈半径,为k时刻三轴加速度计在X、Y、Z轴上的输出分量,ωie为地球自转角速度,g为重力加速度;

4)采用如下公式预测载体位置:

其中,λ(k)、L(k)、h(k)为k时刻载体的经度、纬度与高度,λ(k-1)为k-1时刻载体的经度;

步骤3,通过卡尔曼滤波器,对k时刻载体的角速度进行校正。

2.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法,其特征在于,所述k-1时刻的姿态转移矩阵为:

其中,q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻的姿态四元数。

3.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法,其特征在于,所述k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量为:

其中,为k-1时刻载体速度在导航系X、Y轴上的分量,L(k-1)、h(k-1)为k-1时刻载体的纬度、高度,RM、RN为地球的子午圈、卯酉圈半径,ωie为地球自转角速度,上标T表示转置。

4.根据权利要求1所述基于角加速度计/陀螺/加速度计的惯性导航解算方法,其特征在于,所述步骤3的具体过程如下:

1)计算一步预测均方误差P(k|k-1):

P(k|k-1)=A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T

其中,A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵,P(k-1|k-1)为k-1时刻的状态估计均方差,P(k|k-1)为k-1时刻到k时刻的一步预测均方差,G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵,W(k-1)为k-1时刻状态噪声,上标T表示转置;

2)计算k时刻扩展卡尔曼滤波器滤波增益K(k):

K(k)=P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1

其中,K(k)为k时刻的滤波增益,H(k)为k时刻量测矩阵,R(k)为k时刻的量测噪声,上标-1表示求逆;

3)计算k时刻扩展卡尔曼滤波器状态估计值

其中,为k时刻状态量的估计值,为k-1到k时刻的状态变量一步预测值,Y(k)为k时刻的量测值,为k时刻三轴陀螺在X、Y、Z轴上的输出分量;

4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k):

P(k|k)=[I-K(k)H(k)]P(k|k-1)

其中,P(k|k)为k时刻估计均方误差,I为单位矩阵;

5)基于卡尔曼滤波器,通过三轴陀螺的输出Y(k)对状态中的角速度进行校正。

下载完整专利技术内容需要扣除积分,VIP会员可以免费下载。

该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于南京航空航天大学,未经南京航空航天大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服

本文链接:http://www.vipzhuanli.com/pat/books/201910354258.4/1.html,转载请声明来源钻瓜专利网。

×

专利文献下载

说明:

1、专利原文基于中国国家知识产权局专利说明书;

2、支持发明专利 、实用新型专利、外观设计专利(升级中);

3、专利数据每周两次同步更新,支持Adobe PDF格式;

4、内容包括专利技术的结构示意图流程工艺图技术构造图

5、已全新升级为极速版,下载速度显著提升!欢迎使用!

请您登陆后,进行下载,点击【登陆】 【注册】

关于我们 寻求报道 投稿须知 广告合作 版权声明 网站地图 友情链接 企业标识 联系我们

钻瓜专利网在线咨询

周一至周五 9:00-18:00

咨询在线客服咨询在线客服
tel code back_top