1、 捷联惯导系统原理框图捷联惯导系统原理框图捷联惯导系统捷联惯导系统捷联惯导系统捷联惯导系统 姿态更新算法姿态更新算法 速度更新算法速度更新算法 位置更新算法位置更新算法 系统误差方程系统误差方程捷联惯导系统捷联惯导系统2.姿态更新算法（姿态更新算法（核心核心）基本思想：刚体的定点转动基本思想：刚体的定点转动2.1 2.1 欧拉角法（三参数法）欧拉角法（三参数法）一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为参个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系
2、为参考系则考系则 、和和 即为一组欧拉角。即为一组欧拉角。当当 时，方程退化，故不能全姿态工作。时，方程退化，故不能全姿态工作。bbbnbibin（-）nbC1sincoscos0sin01coscossin0bnbxbnbybnbz90sincos0coscoscos0sinsintan1costanxyzbnbbnbbnb捷联惯导系统捷联惯导系统2.2 2.2 方向余弦法（九参数法）方向余弦法（九参数法）矢量的方向余弦表示姿态矩阵的方法；可全姿态工作，但需要解含有九个未知量的线性方程组，计算量大，工程上不实用。bknbnbnbCC捷联惯导系统捷联惯导系统2.3 2.3 四元数法（四参数法）
3、四元数法（四参数法）2.3.1 2.3.1 四元数基本概念四元数基本概念 四元数是由一个实数单位1和一个虚数单位i、j、k组成的含有四个元的数。（超复数）四元数的大小范数四元数表达方式 三角式基本运算01230123,q q q qqqqqQijk23222120qqqqQ2sin2cosuQ捷联惯导系统捷联惯导系统动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转轴转动一个角度（，u）四元数描述转动：四元数是刚体转动的一种描述形式。结论：四元数可以描述刚体的定点转动，Q包含了等 效旋转的全部信息；四元数与姿态矩阵的关系；描述刚体转动的四元数是规范化四元数；捷联惯导中的姿态更新实质上
4、是如何计算四元数。捷联惯导中的姿态更新实质上是如何计算四元数。cossin22Qu2222012312031302222212030123230122221302230101232()2()2()2()2()2()Rbqqqqq qq qq qq qq qq qqqqqq qq qq qq qq qq qqqqqC捷联惯导系统捷联惯导系统2.3.2 2.3.2 四元数微分方程四元数微分方程 毕卡求解法（角增量）1）定时采样增量法：采样时间间隔相同；2）定量采样增量法：角增量达到一固定值时才更新；12nnbbbnbqq()()()(1)(1)(1)nn mnb mb mn mb mb mqqqq
5、1()()()2kkttQIQ捷联惯导系统捷联惯导系统2.3.3 2.3.3 四元数初值的确定与归一化四元数初值的确定与归一化 表征旋转的四元数应该是规范四元数；计算误差，失去规范性，需归一化处理；1Q23222120qqqqqqii3322110332211333221123322111121121121121TTTqTTTqTTTqTTTq122103311302233201444TTqqTTqqTTqq)()()()()()()()()(122103311302233201TTsignqsignqsignTTsignqsignqsignTTsignqsignqsign22cossinco
6、s()sin()cossin222222 Quuu-捷联惯导系统捷联惯导系统2.3.4 2.3.4 从姿态矩阵中提取姿态角从姿态矩阵中提取姿态角-90，90度-180，180度-180，180度 或 0，360度 真值表判断112131122232132333nbTTTTTTTTTC1321313311222sin()tan()tan()TTTTT主主coscossinsinsinsincossincoscos sinsincos sinsincossincoscossinsincoscossinsincossincoscosnb C捷联惯导系统捷联惯导系统2.4 2.4 等效旋转矢量法等效旋
7、转矢量法 四元数法求解中用到了角速度矢量的积分。当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生误差，称为转动不可交换性误差。为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是采用等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的概念将四元数微分方程转化为等效旋转矢量微分方程(即Bortz方程)：表征旋转的另一种形式：()ttttttdtdt11()()()212bbbnbnbnbttt ucossin22q捷联惯导系统捷联惯导系统 泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）常数拟合：直线拟合：抛物线拟合：三次抛物线：()bnbkta
8、h0()hba2)(kbnbt12122()3h2()23bnbktabc123132313357()()8080h23()234bnbktabcd1234123413241423736()()945334526654()945945945h捷联惯导系统捷联惯导系统四元数法与等效旋转矢量法的区别：原理相同：计算姿态四元数完成姿态更新；四元数算法 等效旋转矢量的单子样单子样算法；算法思路不同；等效旋转矢量法思路：ninbib()()()(1)(1)(1)nn mnb mb mn mb mb mqqqq12()h12122()3h捷联惯导系统捷联惯导系统2.4 2.4 几种姿态算法的比较几种姿态算
9、法的比较欧拉角法：概念直观；只适应水平姿态角变化不大的情况，不能全姿态 解算。方向余弦法：可全姿态工作；但计算量大，不实用。四元数法：算法简单，计算量小；存在不可交换误差，适应于低动态运 载体。（等效旋转矢量的单子样）等效旋转矢量法：可对不可交换性误差进行补偿，算法简单，适应于高 动态环境。捷联惯导系统捷联惯导系统2.2.速度更新算法速度更新算法 基础：比力方程 数字递推形式：旋转效应：rotation 载体线运动在空间的旋转，角速度与线速度不共线；划桨效应：scull 绕一轴做线振动同时绕另一轴做同频角振动；（根本原因：更新周期内姿态角的变化引起）有害加速度：g/Coriolis 2nnbn
10、nnnbieenVC fVg11/11/()nnbnmmmsfmg cormnnmmmrotmsculmg cormVVCVVVCVVVVrotmVsculmV/ng cormV捷联惯导系统捷联惯导系统2.2.位置更新算法位置更新算法 数字递推形式：()()(1)(1)n ln ln len leCCC()(1)()()nnnnn lenlenn lF t VtdtF RCsincos0sincossinsincoscoscoscossinsinneLLLLLL C33arcsin PL 3231ParctgP主捷联惯导系统捷联惯导系统4.4.捷联惯导系统误差方差捷联惯导系统误差方差捷联惯导系
11、统误差源捷联惯导系统误差源惯性仪表的安装误差和刻度因子误差陀螺漂移 和加速度计零位初始条件误差计算误差 000zyzxyxGGGGGGG 000000 xyzKKKKbb捷联惯导系统捷联惯导系统捷联惯导系统误差方程捷联惯导系统误差方程姿态误差方程：姿态误差方程：()nnnbnininbGibCKGEENNUNinU EinU bbbnnnbibnninC C捷联惯导系统捷联惯导系统捷联惯导系统误差方程捷联惯导系统误差方程速度误差方程：速度误差方程：位置误差方程：位置误差方程：()(2)(2)nnnnbnnnbAieennnnnieenKA VfCfVV 2)(hRVhhRVLMNMN2)(se
12、csectansechRLVhLLhRVLLhRVNENENEUVhMATLAB仿真 1、轨迹生成仿真、轨迹生成仿真 2、惯导器件输出信息的仿真、惯导器件输出信息的仿真 3、捷联惯导解算仿真、捷联惯导解算仿真 4、基本函数、基本函数 MATLAB仿真1 1、轨迹生成仿真、轨迹生成仿真 目的目的：航迹仿真的目的是生成惯性器件信息源（比力和角速度）航迹仿真的目的是生成惯性器件信息源（比力和角速度），并给出，并给出相应航迹点的航行参数（姿态、速度和位置）相应航迹点的航行参数（姿态、速度和位置）1）航行轨迹微分方程 姿态角微分方程：()tcos0sincos()01sin()sin0coscosbnb
13、tt 2）生成惯性器件增量信息 角增量 通过控制姿态角速度 和轨迹加速度 ，设置理想轨迹。，则易知轨迹微分方程组是关于向量 的一组微分方程，即 ，求解此微分方程组即可获得载体的轨迹，一般采用四阶龙格库塔解法求解。()bbnnbbnbibninbnbninnbC C C()tXMATLAB仿真()t()ttaMATLAB仿真2 2、惯导器件输出信息仿真、惯导器件输出信息仿真 01bbrabrrraww 01bbrgbrrrgww MATLAB仿真2 2、惯导器件输出信息仿真、惯导器件输出信息仿真 01bbrabrrraww 01bbrgbrrrgww MATLAB仿真3 3、捷联惯导解算仿真、捷
14、联惯导解算仿真 )(,)vatt v pos（，MATLAB仿真4 4、仿真示例、仿真示例 024681012141618x 104-505时 间(秒)俯仰角(角分)024681012141618x 104-505时 间(秒)滚转角(角分)024681012141618x 104-10010时 间(秒)偏航角(角分)MATLAB仿真4 4、仿真示例、仿真示例 024681012141618x 104-10-50510时 间(秒)东向速度（米/秒）024681012141618x 104-10-50510时 间(秒)北向速度（米/秒）MATLAB仿真4 4、仿真示例、仿真示例 024681012141618x 104-1001020时 间(秒)纬度（角分）024681012141618x 10405101520时 间(秒)经度（角分）MATLAB仿真4 4、仿真示例、仿真示例 MATLAB仿真4 4、仿真示例、仿真示例 轨迹生成仿真轨迹生成仿真静态仿真：静态仿真：2种周期种周期动态仿真：轨迹相似且发散动态仿真：轨迹相似且发散严恭敏硕士论文、严恭敏仿真原程序严恭敏硕士论文、严恭敏仿真原程序MATLAB仿真
