[发明专利]基于角加速度计/陀螺/加速度计的惯性导航解算方法有效
申请号: | 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)对状态中的角速度进行校正。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于南京航空航天大学,未经南京航空航天大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/201910354258.4/1.html,转载请声明来源钻瓜专利网。