资源预览内容
第1页 / 共50页
第2页 / 共50页
第3页 / 共50页
第4页 / 共50页
第5页 / 共50页
第6页 / 共50页
第7页 / 共50页
第8页 / 共50页
第9页 / 共50页
第10页 / 共50页
亲,该文档总共50页,到这儿已超出免费预览范围,如果喜欢就下载吧!
资源描述
捷联惯导基本算法与误差捷联惯导基本算法与误差捷联惯导系统算法概述捷联惯导系统算法概述算法:从惯性仪表输出到导航与控制信息算法:从惯性仪表输出到导航与控制信息捷联惯导算法的基本内容:捷联惯导算法的基本内容:一、系统初始化:一、系统初始化:1、给定飞行器初始位置、速度等、给定飞行器初始位置、速度等2、数学平台的初始对准、数学平台的初始对准3、惯性仪表的校准、惯性仪表的校准二、惯性仪表的误差补偿二、惯性仪表的误差补偿三、姿态矩阵的计算三、姿态矩阵的计算四、导航计算四、导航计算五、导航控制信息的提取五、导航控制信息的提取惯性器件误差惯性器件误差 陀螺陀螺惯性器件误差补偿惯性器件误差补偿惯性器件误差补偿对捷联惯导的重要性惯性器件误差补偿对捷联惯导的重要性一、陀螺仪误差模型一、陀螺仪误差模型静态误差模型静态误差模型动态误差模型动态误差模型惯性器件误差惯性器件误差 加速度计加速度计二、加速度计误差模型二、加速度计误差模型静态误差模型静态误差模型动态误差模型动态误差模型三、惯性器件的误差补偿三、惯性器件的误差补偿姿态计算姿态计算 欧拉角微分方程欧拉角微分方程1姿态矩阵的计算姿态矩阵的计算假设数学坐标系模拟地理坐标系假设数学坐标系模拟地理坐标系飞行器姿态的描述:飞行器姿态的描述: 航向角航向角、俯仰角、俯仰角、滚动角、滚动角一、欧拉微分方程一、欧拉微分方程从地理坐标系到载体坐标系从地理坐标系到载体坐标系的旋转顺序:的旋转顺序: 方向余弦矩阵:方向余弦矩阵:姿态计算姿态计算 欧拉角微分方程欧拉角微分方程2飞行器相对地理坐标系的角速度:飞行器相对地理坐标系的角速度:姿态计算姿态计算 欧拉角微分方程欧拉角微分方程3求解欧拉角速率得求解欧拉角速率得注意事项:当注意事项:当 = 90 度时,方程出现奇点度时,方程出现奇点姿态计算姿态计算 矩阵方程精确解矩阵方程精确解1二、方向余弦矩阵微分方程及其解二、方向余弦矩阵微分方程及其解其中其中由于陀螺仪直接测得的是载体由于陀螺仪直接测得的是载体相对惯性空间的角速度,所以:相对惯性空间的角速度,所以:导航计算可以得到导航计算可以得到有有因此因此得得姿态计算姿态计算 矩阵方程精确解矩阵方程精确解2的精确解(毕卡逼近):的精确解(毕卡逼近):其中其中方向不变时的精确解方向不变时的精确解九个微分方程求解,计算量大九个微分方程求解,计算量大姿态计算姿态计算 四元数精确解四元数精确解1三、四元数微分方程式及其解三、四元数微分方程式及其解由第一章,四元数微分方程式:由第一章,四元数微分方程式:对对 的处理类似上一节的处理类似上一节精确解:精确解:其中:其中:姿态计算姿态计算 四元数精确解四元数精确解2其中:其中:姿态计算姿态计算 姿态航向角计算姿态航向角计算1四、姿态和航向角的计算四、姿态和航向角的计算根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角姿态、航向角姿态、航向角真值的判断真值的判断姿态计算姿态计算 姿态航向角计算姿态航向角计算2如利用四元数微分方程求解,如利用四元数微分方程求解,则先利用四元数求解结果计算则先利用四元数求解结果计算方向余弦矩阵的元素方向余弦矩阵的元素(1-58):姿态实时计算姿态实时计算 概述概述姿态矩阵的实时计算姿态矩阵的实时计算因假定因假定“数学平台数学平台”跟踪地理坐标系,因跟踪地理坐标系,因此此所以可得相应的姿态矩阵微分方程(所以可得相应的姿态矩阵微分方程(6-12):):或四元数微分方程:或四元数微分方程:注意事项:注意事项:1、上述两个方程中的角速度表达式不一样、上述两个方程中的角速度表达式不一样2、方程第二项较小,计算时速度可以低一些、方程第二项较小,计算时速度可以低一些增量算法增量算法 矩阵方程精确解矩阵方程精确解一、角增量算法一、角增量算法角增量:陀螺仪数字脉冲输出,每个脉冲代表一个角增量角增量:陀螺仪数字脉冲输出,每个脉冲代表一个角增量一个采样周期内,陀螺输出脉冲数对应的角增量为:一个采样周期内,陀螺输出脉冲数对应的角增量为:1、矩阵微分方程计算、矩阵微分方程计算根据矩阵微分方程的精确解(根据矩阵微分方程的精确解(6-20),有:),有:(解(解的第一项)的第一项)增量算法增量算法 矩阵方程矩阵方程CS参数参数展开合并上式,得展开合并上式,得其中其中增量算法增量算法 矩阵方程矩阵方程1阶阶将前式简写为:将前式简写为:或离散形式:或离散形式:C按按 Cn、Sn 取不同的近似值,形成相应的一阶取不同的近似值,形成相应的一阶 四阶算法四阶算法一阶算法:一阶算法:令令可将上述算法解写成可将上述算法解写成矩阵元素的形式:矩阵元素的形式:增量算法增量算法 矩阵方程矩阵方程1阶阶 一阶增量算法一阶增量算法增量算法增量算法 矩阵方程矩阵方程2-4阶阶当当 Cn、Sn 取取 n = 2, 3, 4 时:时:二阶增量算法:二阶增量算法:三阶增量算法:三阶增量算法:四阶增量算法:四阶增量算法:增量算法增量算法 四元数四元数2、四元数微分方程的计算:、四元数微分方程的计算:其中,其中,I 为单位四元数,为单位四元数, 如如 (6-24)所示:)所示:写成迭代形式:写成迭代形式:增量算法增量算法 四元数四元数设设一阶算法:一阶算法:增量算法增量算法 四元数四元数或展开为元素形式:或展开为元素形式:增量算法增量算法 四元数四元数同理,可得二阶算法:同理,可得二阶算法:三阶算法:三阶算法:四阶算法:四阶算法:数值积分数值积分 1阶阶用一阶用一阶 四阶龙格四阶龙格-库塔积分矩阵和四元数微分方程库塔积分矩阵和四元数微分方程1、一阶龙格、一阶龙格-库塔法库塔法一个矩阵微分方程一个矩阵微分方程当初始条件已知,其一阶龙格当初始条件已知,其一阶龙格-库塔的解为库塔的解为:方程的解为初始值加上以初方程的解为初始值加上以初始点斜率为斜率的一个增量始点斜率为斜率的一个增量斜率斜率K的准确度不同,解的的准确度不同,解的精确度也不同精确度也不同数值积分数值积分 1阶阶 矩阵矩阵(1)姿态矩阵微分方程)姿态矩阵微分方程简化为简化为其一阶龙格其一阶龙格-库塔解:库塔解:展开为展开为元素形元素形式:式:与一阶与一阶增量算增量算法一致法一致数值积分数值积分 1阶阶 四元数四元数(2)四元数微分方程)四元数微分方程或或一阶龙格一阶龙格-库塔解库塔解数值积分数值积分 2阶阶 矩阵矩阵 2、二阶龙格、二阶龙格-库塔法库塔法对一阶算法适当改进,使平均斜率更准确一些对一阶算法适当改进,使平均斜率更准确一些二阶龙格二阶龙格-库塔算法的解:库塔算法的解:(1)矩阵微分方程)矩阵微分方程数值积分数值积分 2阶阶 矩阵矩阵二阶龙格二阶龙格-库塔解:库塔解:设设则则数值积分数值积分 2阶阶 矩阵矩阵数值积分数值积分 2阶阶 四元数四元数(2)四元数微分方程)四元数微分方程数值积分数值积分 2阶阶 四元数四元数数值积分数值积分 4阶阶 矩阵矩阵3、四阶龙格、四阶龙格-库塔法库塔法则解则解(1)矩阵微分方程)矩阵微分方程则解则解数值积分数值积分 4阶阶 四元数四元数(2)四元数微分方程)四元数微分方程则解则解数值积分数值积分 4阶阶 四元数四元数数值积分数值积分 4阶阶 四元数四元数角速度提取角速度提取龙格龙格-库塔积分需要用到角速度信息库塔积分需要用到角速度信息而陀螺仪的数字脉冲输出为角增量而陀螺仪的数字脉冲输出为角增量如果采用周期如果采用周期 T 很小,可以近似把角速度看成常值或线性变化很小,可以近似把角速度看成常值或线性变化1、把角速度看成常值,则周期、把角速度看成常值,则周期 T 内内 一阶角速率提取一阶角速率提取2、把角速度看成线性变化、把角速度看成线性变化则角增量则角增量角速度提取角速度提取从从 ti 到到 ti + T/2 的角增量:的角增量:从从 ti 到到 ti + T 的角增量:的角增量:求解,得求解,得因此因此ti 时刻陀螺输出置零时刻陀螺输出置零ti + T/2 时刻陀螺不置零时刻陀螺不置零ti + T 时刻陀螺置零时刻陀螺置零角速度提取角速度提取如果如果 ti + T/2 时刻陀螺输出置零时刻陀螺输出置零从从 ti 到到 ti + T/2 的角增量:的角增量:从从 ti + T/2 到到 ti + T 的角增量:的角增量:代入代入得得捷联惯导误差传播特性捷联惯导误差传播特性基本特性与平台式惯导系统一致基本特性与平台式惯导系统一致误差方程的建立方法误差方程的建立方法系统误差方程的建立(角度、速度、位置)系统误差方程的建立(角度、速度、位置)1、数学平台的误差方程(四元数法)、数学平台的误差方程(四元数法)四元数微分方程:四元数微分方程:假设数学平台模拟地理坐标系,因此假设数学平台模拟地理坐标系,因此则则 理想情况理想情况实际系统中实际系统中实际计算的四元数微分方程实际计算的四元数微分方程四元数误差四元数误差四元数计算误差表达式四元数计算误差表达式设一矢量设一矢量 R 相对地理坐标系静止,在地理坐标系内表示为相对地理坐标系静止,在地理坐标系内表示为 RE,在,在飞行器坐标系内表示为飞行器坐标系内表示为 Rb,用,用 q 表示载体坐标系相对地理坐标系表示载体坐标系相对地理坐标系的转动。则的转动。则或或设设 Rb 是准确值,考虑四元数的计算误差,有是准确值,考虑四元数的计算误差,有则则定义定义 RE、RE 之间的转动四元数为之间的转动四元数为q四元数误差四元数误差得得四元数误差四元数误差考虑到考虑到可得可得另有另有代入上式,并忽略二阶小量代入上式,并忽略二阶小量 数学平台误差角的矢量表达数学平台误差角的矢量表达第一项:第一项:第三项:第三项:第二项:第二项:平台误差角:平台误差角:四元数误差四元数误差四元数误差、速度误差四元数误差、速度误差展开得到展开得到数学平台误差角的分量表达式数学平台误差角的分量表达式2、速度误差方程、速度误差方程因数学平台模拟地理坐标系,由第二章(因数学平台模拟地理坐标系,由第二章(2-36)可得)可得速度误差速度误差因加速度计固联于载体,将上式写在载体坐标系内:因加速度计固联于载体,将上式写在载体坐标系内:定义速度误差定义速度误差则则考虑考虑设设则则速度误差速度误差转换到地理坐标系:转换到地理坐标系:由于由于则则又有又有因此因此将将代入上式,展开并忽略二阶小量:代入上式,展开并忽略二阶小量:速度误差速度误差第一项:第一项:速度误差方程的矢量表达式速度误差方程的矢量表达式第二项:第二项:第三项:第三项:速度误差速度误差第四项:第四项:速度误差速度误差展开得展开得 速度误差方程的分量展开形式速度误差方程的分量展开形式位置误差、系统误差位置误差、系统误差根据经度、纬度的定义根据经度、纬度的定义取小偏差,得取小偏差,得4、系统误差方程、系统误差方程由数学平台误差角方程、速度误差方程、位置误差方程组成由数学平台误差角方程、速度误差方程、位置误差方程组成经度误差方程仍可单独考虑经度误差方程仍可单独考虑静基座条件下,捷联惯导系统误差传播特性与平台式一致静基座条件下,捷联惯导系统误差传播特性与平台式一致End
网站客服QQ:2055934822
金锄头文库版权所有
经营许可证:蜀ICP备13022795号 | 川公网安备 51140202000112号