1、 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 第三章第三章 惯性导航系统惯性导航系统教授：魏二虎2022-10-281 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.1、概述、概述3.2、平台式惯导系统的基本原理、平台式惯导系统的基本原理3.3、捷联式惯导系统的工作原理、捷联式惯导系统的工作原理3.4、惯性导航系统的误差分析、惯性导航系统的误差分析3.5、惯性导航系统的初始对准、惯性导航系统的初始对准2022-10-282 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 惯性导航惯性导航是一种自主式的导航方式。它完全依靠机载设备自主地完成导航任务，和外界不发生任何光、电联系，因此隐蔽性好，工作不
2、受气象条件的限制。是航天、航空和航海领域中广泛使用的主要导航方法，在导航技术中占有突出的地位。2022-10-283 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 惯性导航的基本工作原理是以牛顿力学定律为基础，利用一组加速度计连续的进行测量，而后从中提取运动载体相对某一选定的导航坐标系的加速度信息；通过一次积分运算（载体的初始速度已知）便得到载体相对导航坐标系的即时速度信息；再通过一次积分运算（载体初始位置已知）便又得到载体相对导航坐标系的即时位置信息。AyAxyx0yv0 xv0yxaya0 xxyAyAxyx 0yv0 xv0yxaya0 xxy图图3.1 惯性导航原理图惯性导航原理图202
3、2-10-284 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 在实际的惯导系统中，载体的位置一般都用地理经纬度L和 来表示。如果x轴指北，y轴指东，R表示地球半径，则用经纬度表示的载体位置为：0000txxxtyyyvva dtvva dt0000txtyxxv dtyyv dt0000costxtyvLLdtRvdtRL2022-10-285 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 惯性导航系统通常由以下几个部分组成：加速度计 用来测量载体运动的加速度。惯导平台 模拟一个导航坐标系。导航计算机 完成导航计算和平台跟踪回路中指令角速度信号的计算。控制显示器 给定初始参数及系统需要的其他参数
4、，显示各种导航信息。2022-10-286 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 从结构上来说，可把惯导分成两大类：一类是平台式惯导；在平台式惯导中，以实体的陀螺稳定平台确定的平台坐标系来精确地模拟某一选定的导航坐标系，从而获得所需的导航数据。平台惯导系统原理方块图平台惯导系统原理方块图2022-10-287 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 另一类是捷联式惯导；在捷联式惯导中则通过计算机实现的数学平台来替代实体平台，由此带来的好处是可靠性高、体积小和价格便宜。捷联惯导系统原理方块图捷联惯导系统原理方块图2022-10-288 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.1
5、、概述、概述3.2、平台式惯导系统的基本原理、平台式惯导系统的基本原理3.3、捷联式惯导系统的工作原理、捷联式惯导系统的工作原理3.4、惯性导航系统的误差分析、惯性导航系统的误差分析3.5、惯性导航系统的初始对准、惯性导航系统的初始对准2022-10-289 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.2.1 平台式惯导系统的基本组成 平台式惯导系统的核心是惯性级的陀螺稳定平台。三个加速度计的敏感轴分别沿三个坐标轴的正向安装，测得载体的加速度信息就体现为比力在平台坐标系中的三个分量。通过必要的计算和补偿，可从提取出载体相对导航坐标系的加速度矢量的三个分量。再通过两次积分，可得到载体相对导航
6、坐标系的速度和位置。2022-10-2810 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 平台式惯导系统按所选定的导航坐标系的不同又可分为：1当地水平面惯性导航系统这种系统的导航坐标系是一种当地水平坐标系，也是前面所说的地理坐标系，即平台系的两个轴x轴及y轴保持在水平面内，z轴与地垂线相重合。由于两个水平轴可指向不同的方位，故这种系统又可分为：(1)指北方位惯导系统。这种系统在工作时x轴指向地理东向（E），y轴指向地理北向（N），即平台系模拟当地地理坐标系(用g来标识)。(2)自由方位惯导系统。在系统工作中，平台y轴不跟踪地理北向而是与北向夹某个角度，称自由方位角。2022-10-2811 主
7、主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2、空间稳定惯导系统 这种系统的导航坐标系为惯性坐标系(用i来标识)，一般采用原点定在地心的惯性坐标系。z轴与地轴重合指向北极，x、y轴处于地球赤道平面内，但不随地球转动。与当地水平面惯导系统相比，平台所取的空间方位不能把运动加速度和重力加速度分离开，而要依靠计算机进行补偿。2022-10-2812 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 平台式惯导系统平台式惯导系统原理框图原理框图加速度计加速度计陀陀 螺螺 仪仪惯惯 导导 平平 台台导航计算机导航计算机加速度信息加速度信息陀螺施矩信息陀螺施矩信息稳定回路稳定回路控制平台信息控制平台信息位置信息位
8、置信息速度信息速度信息控制显示控制显示加速度计加速度计陀陀 螺螺 仪仪惯惯 导导 平平 台台导航计算机导航计算机加速度信息加速度信息陀螺施矩信息陀螺施矩信息稳定回路稳定回路控制平台信息控制平台信息位置信息位置信息速度信息速度信息控制显示控制显示由下图可见，一组加速度计安装在惯性平台上，为导航计算机的计算提供加速度信息。导航计算机根据加速度信息和由控制台给定的初始条件进行导航计算，得出载体的运动参数及导航参数，一方面送去显示器显示，一方面形成对平台的指令角速率信息，施加给平台上的一组陀螺仪，再通过平台的稳定回路控制平台精确跟踪选定的导航坐标系。此外，从平台框架轴上的角传感器可以获得载体的姿态信息
9、并送往显示器显示。2022-10-2813 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 左图给出了平台式惯导系统的组成结构图。图中所示的惯性平台是由三个单自由度陀螺组成的三环平台。沿三个平台轴线分别安装三个加速度计，分别测得各轴分量，此信号送给导航计算机，经过计算和补偿，最后可求得载体的即时地速、即时位置等导航参数；同时各轴陀螺相对惯性空间有转动角速度，计算机算出的三轴分量并变为电信号后加给平台上相应的三个陀螺控制轴的力矩器，使惯导平台与当地水平面一致，另外计算机还向显示器输出当前的姿态。2022-10-2814 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 不同算法方案的平台式惯导系统，其组成
10、结构是相似的，区别主要是选用的导航坐标系不同，因而导航参数与指令角速率的计算过程不同，即力学编排方程不同。当然，对元部件的要求也可能有所不同。2022-10-2815 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 惯导系统的机械编排是指系统的实体布局、采用的坐标系及解析计算方法的总和。它体现了从加速度计的输出到计算出即时速度和位置以及对平台陀螺进行施矩控制的整个过程。进行机械编排就是确定和提出反映系统中各力学量之间联系的方程组，又称之为惯导系统的机械编排方程。2022-10-2816 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 下面列出指北方位惯导系统的机械编排方程。1平台指令角速度由于当地地理
11、坐标系随着地球自转和载体运动而不断改变。为使平台跟踪地理坐标系，就要给平台上的陀螺加施矩指令信号使平台做相应的转动。地理系相对惯性系的转动角速率在系上的分量可表示为：式中：为地球自转角速度在n系上的分量，为地理系相对地球系的角速度在n系上的分量，是由于运载体在地球曲面运动而造成的相对角速率。nnnipieepnienep2022-10-2817 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 由于p系和n系是重合的，故角速度在两个坐标系上的投影完全相同，即有将按 的三个分量计算形成的电信号分别送给平台上相应的陀螺力矩器，就能实现p系对n系的跟踪。0coscossintgnnenyenymmpnnn
12、ipxinxiexenxnppnnnenxenipipyinyieyenyieienpnnnipzinziezenzienenxnvvRRvvLLRLvLRsintgnnxinnnenxienRvLLRpip2022-10-2818 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2.地理速度计算(2)nnnnnnenieenen vfvg(2sintg)(2cos)(2sintg)(2cos)nnnnnnxxxxieyieznnnnynnnnxyyiexznmnnynnnnxzziexynmvvvfLL vLvRRvvvfLL vvRRvvvfLvvgRR对该微分方程式进行求解，即可得到运载体在
13、地理坐标系的速度。2022-10-2819 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.纬度、经度和高度计算cossinnynenxmnnxenznnzvLRvRLLhv 000000secntymntxntnzvLdtLRvLdtRhv dth式中：、和 为初始的经纬高。0L00h2022-10-2820 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 4.姿态角的获取由于平台坐标系模拟了当地地理坐标系，故从平台框架上角度传感器（同步器）就可以直接取得载体的航向角、俯仰角和横滚（倾斜）角信号。2022-10-2821 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 5.系统原理框图导航计算（力学
14、编排）导航计算（力学编排）俯仰（俯仰（pitch）横滚（横滚（roll）方位（方位（heading）延迟延迟延迟平台系统平台系统显示输出显示输出来自于平台框架传感器来自于平台框架传感器平台指令平台指令角速率输出角速率输出比力输入比力输入(0)nV()L k(1)nfk(1)nink(1),(),(),nnF fkL k Vk T()nVknnieen(1)nVk(1)L k(1)nVk(1),(),(),(),nnP VkL kk h k T(0)(0)(0)Lh()()()L kkh k(1)(1)(1)L kkh k导航计算（力学编排）导航计算（力学编排）俯仰（俯仰（pitch）横滚（横滚
15、（roll）方位（方位（heading）延迟延迟延迟平台系统平台系统显示输出显示输出来自于平台框架传感器来自于平台框架传感器平台指令平台指令角速率输出角速率输出比力输入比力输入(0)nV()L k(1)nfk(1)nink(1),(),(),nnF fkL k Vk T()nVknnieennnieen(1)nVk(1)L k(1)nVk(1),(),(),(),nnP VkL kk h k T(1),(),(),(),nnP VkL kk h k T(0)(0)(0)Lh()()()L kkh k(1)(1)(1)L kkh k指北方位惯导系统原理方框图指北方位惯导系统原理方框图2022-1
16、0-2822 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 由上式可以看出 包含重力加速度g。因此作为精确导航应当考虑到，g不是常值而是高度h的函数，其数值随高度的增加而减少。可以证明，当 时(2cos)nnynnnnnxzziexyzBenmvvvfLvvgfagRRnzv hR02(1)hggR式中：R为地球半径，g0为地球表面重力加速度。2022-10-2823 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 纯惯导高度通道纯惯导高度通道nzf1sh02 gR-1s0h0zvBea0gnzf1s1sh02 gR02 gR-1s1s0h0zvBea0g该系统的特征方程式为：2000222()()
17、0gggsssRRR特征方程式有一个正根，说明系统是不稳定的，计算高度误差是扩散的。2022-10-2824 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 因此前述系统不能直接采用这种纯惯性的高度通道，而必须引人外部高度信息对高度通道构成阻尼回路，使两种高度信息起互补作用，这样两方面可以取长补短，得到动态品质好而误差不随时间发散的组合高度系统。通常采用有二阶阻尼或者三阶阻尼。1）二阶阻尼回路zf1sh02 gR-1s0h0zv0gbh1k-2kzf1s1sh02 gR02 gR-1s1s0h0zv0gbh1k-2k垂直通道的二阶垂直通道的二阶阻尼回路阻尼回路2022-10-2825 主主主主讲讲
18、讲讲：魏魏魏魏二二二二虎虎虎虎 2）三阶阻尼回路为了得到更好的系统特性，可采用三阶阻尼回路。zf1sh02 gR-1s0h0zv0gbh1k-2k4k-3k1szf1s1sh02 gR02 gR-1s1s0h0zv0gbh1k1k-2k2k4k4k-3k3k1s1s垂直通道的三阶阻尼回路垂直通道的三阶阻尼回路有关二阶和三阶阻尼回路中控制系数的计算通常根据控制原理的关系来最优处理。在现代惯性导航系统中，也可以采用现代控制算法来实现对高度通道的控制。2022-10-2826 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 对于自由方位平台，有由于p系和n系的z轴重合，又因为故平台绕z轴偏离地理坐标系的
19、角速率为考虑到 也是 ，故自由方位角为0pipznnnipzinznpznnnpzinz 为水平轴偏离角速度，所形成的夹角称为自由方位角af。nnpz(sintg)nnxnpzienvLLR nnpzfa 00(sintg)gtxffiexgvaaLL dtR2022-10-2827 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 载体的航向角与自由方位角的关系如图所示。()ez zexnyey()npzznxeoLpxpyxyfafao()ez zexnyey()npzznxeoLpxpyxyfafaofynybyfaf bfynybyfaf b 自由方位平台坐标系自由方位平台坐标系 航向角航向
20、角 与自由方位角与自由方位角 的关系的关系 faf bfa2022-10-2828 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 游动自由方位系统平台坐标系仍为当地水平。与自由方位系统的区别在于，对方位陀螺力矩器上要施加有限的指令角速度，即平台绕z轴只跟踪地球本身的转动，而不跟踪由载体运动速度而引起的当地地理坐标系相对惯性系的转动角速率。因而有 这样y轴与地理系y轴之间仍存在夹角，设为，称游动方位角。sinppipzieiezL0pppepzipziez00tggtxxgvLdtRsin(sintg)tgggpxxipzieiexgxgvvLLLLRR 2022-10-2829 主主主主讲讲讲讲
21、：魏魏魏魏二二二二虎虎虎虎 在极区航行时有一个问题，计算机在计算位置和指令角速率时要进行 的计算，当纬度接近90时，计算机数值计算溢出，以至于三角函数有关的表达式失效。解决这个办法一般采用横向经纬度坐标法。该方法是将极区的经纬度重新人为规定，如将经纬度坐标系相对赤道平面转过一个角度。tgLNNLLOSSNNN NLLL LO OS SSSNSSNexnyLOQezLnxnynz()nznxNNS SSSN NexexnynyL L O OQ QezezLLnxnxnynynznz()nz()nznxnx自转动后自转动后的极点的极点 横向横向“经经纬度纬度”坐坐标系标系 2022-10-2830
22、 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.1、概述、概述3.2、平台式惯导系统的基本原理、平台式惯导系统的基本原理3.3、捷联式惯导系统的工作原理、捷联式惯导系统的工作原理3.4、惯性导航系统的误差分析、惯性导航系统的误差分析3.5、惯性导航系统的初始对准、惯性导航系统的初始对准2022-10-2831 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 捷联式惯导系统捷联式惯导系统(SINS)是把惯性仪表直接固联在载体上，用计算机来完成导航平台功能的惯性导航系统。由于省去了机电式的导航平台，从而给系统带来了许多优点。1.整个系统的体积、重量和成本大大降低；2.惯性仪表便于安装维护，也便于更换
23、；3.惯性仪表可以给出载体轴向的线加速度和角速度；和平台式系统相比，捷联式系统可以提供更多的导航和制导信息；4.惯性仪表便于采用余度配置，提高系统的性能和可靠性。2022-10-2832 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 惯性仪表直接固联在载体上也带来一些新的问题。1.惯性仪表固联在载体上，直接承受载体的振动和冲击，工作环境恶劣；2.高性能歼击机角速度可达400()/s，这样，陀螺的测量范围为0.01()/h-400()/s，量程高达108，这就要求捷联陀螺有大的施矩速度和高性能的再平衡回路；3.平台式系统的陀螺仪安装在平台上，可以相对重力加速度和地球自转角速度任意定向来进行测试，便
24、于误差标定。而捷联陀螺则不具备这个条件，因而装机标定比较困难，从而要求捷联陀螺有更高的参数稳定性。2022-10-2833 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 捷联惯导系统-方块图2022-10-2834 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 捷联惯导基本算法与误差捷联惯导系统算法概述捷联惯导系统算法概述算法：从惯性仪表输出到导航与控制信息算法：从惯性仪表输出到导航与控制信息捷联惯导算法的基本内容：捷联惯导算法的基本内容：一、系统初始化：一、系统初始化：1、给定飞行器初始位置、速度等、给定飞行器初始位置、速度等2、数学平台的初始对准、数学平台的初始对准3、惯性仪表的校准、惯性仪表
25、的校准二、惯性仪表的误差补偿二、惯性仪表的误差补偿三、姿态矩阵的计算三、姿态矩阵的计算四、导航计算四、导航计算五、导航控制信息的提取五、导航控制信息的提取2022-10-2835 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 捷联式惯导计算流程框图(地理坐标系L系）2022-10-2836 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 载体的姿态和航向反映了载体坐标系和地理坐标系之间的转换关系，捷联惯性导航系统数学平台中，最重要的问题是实时获得载体坐标系相对地理坐标系的关系（姿态转换矩阵）。描述动坐标系相对参考坐标系方位关系的方法有多种，简单地把它们分作三类，即三参数法、四参数法和九参数法。三参数
26、法也叫欧拉角法；九参数法是方向余弦法；四参数法通常指四元数法，考虑到转动的不可交换性，有时用等效转动矢量加以辅助。2022-10-2837 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 1.欧拉角法欧拉角法 如把载体坐标系 作为动坐标系，把地理坐标系 作为参考坐标系，则姿态角 、和航向角 即为一组欧拉角。bbbOx y znnnOx y zny()nbzznxo()bbyybzbx()bbx xbxbybzbybzny()nbzznxo()bbyybzbx()bbx xbxbybzbybz由航姿角构成的一组欧拉角由航姿角构成的一组欧拉角 、和 表示从地理系到载体系的一种转动nnnbbbbbbbb
27、bnbbx y zx y zxy zx y zzxy 2022-10-2838 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 1.欧拉角法欧拉角法 姿态转换矩阵为：用 表示载体坐标系相对地理坐标系的角速度在载体坐标系轴向的分量构成的列矢量，可通过捷联陀螺仪的实际输出 求得，关系式如下：cos0sin100cossin00100cossinsincos0sin0cos0sincos001cos cossinsinsincos sinsinsinsinsincoscos sincoscossinsincoscos sinsinbnCC C Csinsincos sincoscos cosbnbbib
28、()bbbbnbibieen2022-10-2839 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 1.欧拉角法欧拉角法 从转动过程可以看出，和姿态航向角速度 的关系为：展开合并得bnb、000000bnbxbnbybnbz C CCcos0sin cos01-sinsin0-cos cosbnbxbnbybnbz或coscos0sincos1sincoscoscos coscossin0cosbnbxbnbybnbz 欧拉角微分方程2022-10-2840 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 1.欧拉角法欧拉角法 根据角速度 可以直接求解 、和 三个角度。微分方程只有三个，因此称为三参
29、数法。当 时，方程出现“奇点”，方程式退化，故不能全姿态工作。cos0sin cos01-sinsin0-cos cosbnbxbnbybnbzbnb902022-10-2841 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2.方向余弦法方向余弦法 方法余弦法是直接求解机体坐标系与地理坐标系的姿态转换矩阵。设一个与载体坐标系固联的任意矢量r，在地理系和机体系上的投影为：当机体系b相对地理系n有一个转动角速度 存在，如果认为n系不动，b系动（或反之也可），即 ，则 又由哥氏求导定理得nnbbrC rnb0b r0n rnnnnbbb bbbrC rC rC rnbnbnbdddtdtrrrr投影
30、在n系 nknknnnb nnbb b r r C r2022-10-2842 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2.方向余弦法方向余弦法 该微分方程有如下四种形式，但可以证明是等价的。方向余弦矩阵的微分方程是一个变系数的齐次微分方程，可用毕卡(Peano-Baker)逼近法求之，其一般解析形式为：在求得方余弦矩阵后，即可利用矩阵元素与姿态角对应的关系，求解出姿态角。nnknbbkbbnbbnnbnnnbkbbnkbbnbnnnbC CCC CCC C 0()()(0)tnknbt dtbbnnteCCnnnnbbb bbbrC rC rC rnnknnnknbbnbb bbnbbC
31、 r C rC Cnknknnnb nnbb b r r C r2022-10-2843 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2.方向余弦法方向余弦法该方法具有如下特点：)可以全姿态工作，不受限制；)微分方程组的维数太高，达到了九维；)运算过程中姿态转换矩阵会有非正交化误差产生，必须每次进行正交化处理。2022-10-2844 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.四元数法四元数法 四元数理论是数学中的一个古老的分支，是一种数学间接的方法，但在空间技术和捷联式惯导系统中得到了实际应用。用四元数的微分方程计算来代替方向余弦矩阵微分方程的计算，可大大减小计算量，并且具有更好的计算
32、性能，是一种间接处理的姿态矩阵解算方法。2022-10-2845 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数 表示 四元数：描述刚体角运动的数学工具四元数：描述刚体角运动的数学工具针对捷联惯性导航系统，弥补欧拉参数在设计现代控制系针对捷联惯性导航系统，弥补欧拉参数在设计现代控制系统时的不足。统时的不足。四元数的表示四元数的表示由一个实单位和三个虚数单位由一个实单位和三个虚数单位 i,j,k 组成的数组成的数 kPjPiPq3211或者省略或者省略 1，写成，写成kPjPiPq321i,j,k 服从如下运算公式：服从如下运算公式：2022-10-2846 主主主主讲讲讲讲：魏魏魏魏二二二
33、二虎虎虎虎 四元数 组成部分i,j,k 服从如下运算公式服从如下运算公式 1kkjjiikijjiijkkjjkiikkPjPiPq321 称作标量部分，称作标量部分，kPjPiP321称作矢量部分称作矢量部分 四元数的另一种表示法四元数的另一种表示法 Pq,P 泛指矢量部分泛指矢量部分2022-10-2847 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数基本性质 加减法kPjPiPq321kjivM3211四元数加减法四元数加减法 MqkPjPiPv)()()()(332211或简单表示为或简单表示为 PvMq,2022-10-2848 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元
34、数基本性质 乘法2四元数乘法四元数乘法)(321321kjivkPjPiPMq)(332211PPPviPPvP)(233211jPPvP)(311322kPPvP)(122133或简单表示为或简单表示为 PvPPvMq2022-10-2849 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2022-10-2850 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数基本性质 共轭 范数3共轭四元数共轭四元数 仅向量部分符号相反的两个四元数仅向量部分符号相反的两个四元数),(Pq和和),(*Pq互为共轭互为共轭 可证明：可证明：*)*(qhqh4四元数的范数四元数的范数 q定义定义 2322212
35、*PPPqqq1q则称为规范化四元数则称为规范化四元数 2022-10-2851 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数基本性质 逆 除法5逆四元数逆四元数 qq11qq*当当 1q时时*1qq6四元数的除法四元数的除法 若若 Mqh 则则 1 Mhq若若 Mhq 则则 Mhq1不能表示为不能表示为 hMq（含义不确切含义不确切）2022-10-2852 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 约定一个坐标系或矢量相对参考坐标系旋转，一个坐标系或矢量相对参考坐标系旋转，转角为转角为，转轴转轴 n n 相对参考坐标系各轴之间的方向余弦值为相对参考坐标系各轴之间的方
36、向余弦值为cos、cos、cos。则表示该旋转的四元数可以写为则表示该旋转的四元数可以写为 kjiqcos2sincos2sincos2sin2cos为特征四元数为特征四元数（范数为范数为 1）四元数既表示了转轴方向，又表示了转角大小（转动四元数）四元数既表示了转轴方向，又表示了转角大小（转动四元数）2022-10-2853 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 矢量旋转如果矢量如果矢量 R 相对固定坐标系旋转，旋转四元数为相对固定坐标系旋转，旋转四元数为 q，转动后，转动后的矢量为的矢量为 R，则这种转动关系可通过四元数旋转运算来实现则这种转动关系可通过四元数旋转运算来
37、实现1 qRqR含义：矢量含义：矢量 R 相对固定坐标系产生旋转，相对固定坐标系产生旋转，转角和转轴由转角和转轴由 q 决定决定设固定坐标系单位矢量设固定坐标系单位矢量 i,j,k，统一用符号统一用符号 Ee 表示表示 单位矢量单位矢量 Ee 经过经过 q 旋转后，得到一组新的单位矢量旋转后，得到一组新的单位矢量 Ee（以及对应的新坐标系），（以及对应的新坐标系），两个坐标系的单位坐标矢量之间存在：两个坐标系的单位坐标矢量之间存在：1qqEEee2022-10-2854 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2022-10-2855 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 2022-
38、10-2856 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 坐标系旋转如果如果坐标系坐标系 OXYZ 发生发生 q 旋转，得到新坐标系旋转，得到新坐标系 OXYZ 一个相对原始坐标系一个相对原始坐标系 OXYZ 不发生旋转变换的矢量不发生旋转变换的矢量 V zkyjxiV矢量矢量 V 在新坐标系上在新坐标系上 OXYZ 的投影为的投影为 kzjyixV则不变矢量则不变矢量 V 在两个坐标系上的投影之间存在如下关系：在两个坐标系上的投影之间存在如下关系：qVqVee1式中式中 zkyjxiVekzjyixVe分别称为矢量分别称为矢量 V 在坐标系在坐标系 OXYZ 和和 OXYZ
39、 上的映像上的映像2022-10-2857 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数 映象图解zkyjxiV kzjyixVkzjyixVezkyjxiVe2022-10-2858 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 方向余弦qVqVee1将该投影变换式展开，也就是把将该投影变换式展开，也就是把kzjyixVezkyjxiVekPjPiPq321kPjPiPq3211代入上述投影变换式代入上述投影变换式kzjyix)(321kPjPiP)(zkyjxi)(321kPjPiP进行四元数乘法运算，整理运算结果可得进行四元数乘法运算，整理运算结果可得2022-10-
40、2859 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 方向余弦zyxCzyx其中方向余弦矩阵其中方向余弦矩阵 C222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2PPPPPPPPPPPPPPPPPPPPPPPPPPP2022-10-2860 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数表示转动 旋转合成多次旋转多次旋转的合成的合成对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间的关系等效于一个一次转动的效果，的关系等效于一个一次转动的效果，相应地有合
41、成转动四元数相应地有合成转动四元数 假定假定 q1、q2 分别是第一次转动、第二次转动的四元数分别是第一次转动、第二次转动的四元数 q 是合成转动的四元数，是合成转动的四元数，那么有如下关系成立：那么有如下关系成立：21qqq上式中上式中 q1 和和 q2 的转轴方向必须以映象的形式给出。的转轴方向必须以映象的形式给出。如果如果 q1 和和 q2 的转轴方向都以原始坐标系的分量表示，则有的转轴方向都以原始坐标系的分量表示，则有 12qqq2022-10-2861 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 非映象方式1用四元数旋转变换的方法求取两个坐标系之间的方向余弦表。用四元数旋
42、转变换的方法求取两个坐标系之间的方向余弦表。坐标系坐标系 OXYZ 相对相对OXYZ 三次旋转，三次旋转，以欧拉角以欧拉角 、的形式给出。的形式给出。第一转，绕第一转，绕 Z 轴转轴转角，瞬时转轴角，瞬时转轴 n 和和 k 轴重合，则转动四元轴重合，则转动四元数为数为 kq2sin2cos12022-10-2862 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 第二转，绕第二转，绕 OX1 轴转轴转角，角，瞬时转轴瞬时转轴 n 的方向表示式为的方向表示式为)sin(cosji其转动四元数为其转动四元数为 nq2sin2cos2)sin(cos2sin2cosji求方向余弦 非映象方式22022-
43、10-2863 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 非映象方式合成由于由于 q1 和和 q2 的瞬时转轴的瞬时转轴都是以同一个坐标系的方向余弦来都是以同一个坐标系的方向余弦来表示，则合成转动四元数表示，则合成转动四元数 q 的计算采用：的计算采用：12qqqkji2sin2cos)sin(cos2sin2coskji2sin2cos2sin2sin2cos2sin2cos2cos2022-10-2864 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 映象方式1以瞬时转轴以瞬时转轴映象映象形式给出形式给出转动四元数的表达式并求转动四元数的表达式并求出合成转动四元数出
44、合成转动四元数 第一次转时，映象形式的第一次转时，映象形式的 q1 和非映象形式的和非映象形式的 q1 是是一致的：一致的：kq2sin2cos12022-10-2865 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 映象方式2第二转绕第二转绕 OX1 轴转轴转 角角瞬时转轴瞬时转轴 n 是由是由 OX 经过经过第一转转换来的第一转转换来的OX 轴对应单位矢量轴对应单位矢量 i，所，所以定义以定义 n 的映象为的映象为 i则则 q2 的映象表示式为的映象表示式为 iq2sin2cos22022-10-2866 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 映象方式3第三转，
45、绕第三转，绕 OZ 轴转动轴转动 角角瞬时转轴瞬时转轴 n 是由是由 OZ 经经过第一转和第二转转换过第一转和第二转转换来的来的OZ 轴对应单位矢量轴对应单位矢量 k，所以定义所以定义 n 的映象为的映象为 k则则 q3 的映象表示式为的映象表示式为kq2sin2cos32022-10-2867 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 求方向余弦 映象合成由于由于 q1、q2 和和 q3 都是映象形式都是映象形式，所以三次转动的合成转动所以三次转动的合成转动四元数四元数 q 为为321qqqqkik2sin2cos2sin2cos2sin2cosji2sin2sin2cos2sin2cos
46、2cosk2sin2cos据此可算出对应的方向余弦表据此可算出对应的方向余弦表 2022-10-2868 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数补充 两种转动公式 坐标系旋转时，不变矢量坐标系旋转时，不变矢量 V 在两个坐标系上的投影之间存在在两个坐标系上的投影之间存在如下关系：如下关系：qVqVee1在一些资料中，四元数的转动公式也经常写成如下的形式在一些资料中，四元数的转动公式也经常写成如下的形式 1qqVVEE这个公式的意义是说，在一个超复数空间中，或者在一个固这个公式的意义是说，在一个超复数空间中，或者在一个固定坐标系中，矢量定坐标系中，矢量 VE 按着四元数按着四元数 q
47、 所表示的方向和大小转所表示的方向和大小转动了一个角度，得到一个新的矢量动了一个角度，得到一个新的矢量 VE2022-10-2869 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数补充 计算上的优点四元数法能得到迅速发展，是由于飞行器控制与导航的发展，四元数法能得到迅速发展，是由于飞行器控制与导航的发展，要求更合理地描述刚体空间运动，以及便于计算机的应用。要求更合理地描述刚体空间运动，以及便于计算机的应用。采用方向余弦矩阵描述飞行器运动时，要积分矩阵微分方程式：采用方向余弦矩阵描述飞行器运动时，要积分矩阵微分方程式：CC式中式中C为动坐标系转置到定坐标系的方向余弦矩阵，为动坐标系转置到定坐
48、标系的方向余弦矩阵，为动坐为动坐标系相对定坐标系旋转角速度标系相对定坐标系旋转角速度的反对称矩阵：的反对称矩阵：000 xyxzyz包含包含 9 个一阶微个一阶微分方程式，计算分方程式，计算量比较大量比较大 2022-10-2870 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 四元数补充 计算上的优点如果采用四元数法，则是要求解四元数方程式如果采用四元数法，则是要求解四元数方程式 qq21q 为动坐标系的转动四元数，为动坐标系的转动四元数，为动坐标系相对定坐标系为动坐标系相对定坐标系的旋转角速度，也表示为四元数的旋转角速度，也表示为四元数 kjizyx 0按四元数乘积展开按四元数乘积展开 xy
49、zyzyyzxzyxPPPPPPPPPPPP2131323213212222只要解四个一阶微只要解四个一阶微分方程式组分方程式组即可即可2022-10-2871 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.四元数法四元数法 四元数运动学微分方程四元数运动学微分方程 设 矢量为四元数形式，表示载体坐标系相对地理坐标系的角速度在载体坐标系上的投影，其与 对应的四元数 具有如下微分方程关系 用矩阵表示为 其等效的矩阵矢量形式为：bnbnbC0.5bnb 00112233000.500bbbnbxnbynbzbbbnbxnbznbybbbnbynbznbxbbbnbznbynbx()0.5()()
50、0.5()()bbnbnb Q M Q MQ 2022-10-2872 主主主主讲讲讲讲：魏魏魏魏二二二二虎虎虎虎 3.四元数法四元数法 四元数微分方程的求解，类似矩阵微分方程，可用毕卡逼近法求解，得解析表达式如下 实际上只有在短时间内方向不变时，以上指数积分方能成立，否则，将引入不可交换性误差，严格地说，应采用等效转动矢量算法等效转动矢量算法。近似取00.5()0()()tbnbdtteMQ Q 000()00 xyztxzybnbyzxzyxdtM0000()cos(/2)sin(/2)/()tIQ Q 其中22220()xyz 2022-10-2873 主主主主讲讲讲讲：魏魏魏魏二二二二
