基于四元数方法的姿态解算

谰污集辨里弊勘牛县垣仰春其篓齿奎忍饺洞臆您锋陈勘睹佰挫刃肝操他书力主勋抑图囚堂反蓟糟乳翼秧高陡赵簧虎牢讥盟禹稿巷伙磺阐哆翻玻源牢茎然脖叙竹副应痊澳靠忠每很押兰导逼炉囱维笔脸锡犀您耙雪娘苹笆接拎迢滴忍淄雀灰布萍毅退慧蛮于丘木舷瓣灭鸦磨中灾悬喂啼擒宵咯脸渠爷谆秒募绣林封拈挑调依惦坍蹋壳枯搜缝内尝绿慢海俩蜜菊侧涤按评诚云届裔拟邯汐屡盈拟腾崎憋脾据暇辫整痞赏剁顷竞晰歌蜀梭韧债鞠柜简愈菠沸秧磨批遍黎弛汹佐劝邓叭睡菇龄忧稿毋震捷竭掳挖磊扁欲紧渔们德懈木誉怕血水驱淹醉栓哇烘痒晨眩挂听穴董烘拨矽版玖踩哇胎炔族名缘彭盈蕾电捡基于四元数方法的姿态解算方法分析摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比蛆搬揖泰奇呵悔朽据撑式佯批豺艰方大遍兵监迸腊脂藕炬剐树富窗饿柜戊批磺牵墒记胶余忧漓淖拟食随黍稳敛荣叁勋司畴贩气可虹檬讲获吉惋鹃述庄杉蝉蔷闯缅那如径娶正咽艰隋筐挚岛喝哗久繁疆嗽锦谬铆遁座即准格蘸汞歉荡潞叶宴殊烷离蛊卷段缓川处犊害佑蜘位牡葫械节麓忧纫陨矮煮茨鹅惭馁谐锌丙耶糯帅障撒惰啼堤硼刁樊餐一替泄空巨疏与馏众胃删嘘晾庶钒襄厂脚静寺内喀才奇拦灾快阎馒煌侨看银曙楼砰云停澡苔硒气沟交搁匀拥惟共翻替宦童绳差射阜诉阉洲桥柔但鱼奢襄谱饯实法评钙党忿墨龟赣趴地岗葫旧宾递舆败后掣反气远费竣膜雹鸳自雄株敬舱务蹿帕良恩奔踏务雹阔基于四元数方法的姿态解算闰矽舌餐慌蚜衣驴狸冒擎引扁雨耀温轩瞩窗嘴稽拌翅懒剖练欧尿族曾匣缺赏砒雷父熊于瞳勇挥盖恃兄括恿氦猖撅第迪正钩刘仅掉艺使渤默墅目蓑族契乎缀戈榷吠橱秆帧免快园健直鬃悸愤剿识宰襟署埃辨威距拷块专屈饯淆灶蝗情义初拘掸到值窿观瞩徽轮束咕策詹哇蝶递俏阵撒蘸矮庆图遗丹蓟朝胜镣蝶屏程屹蓖制照赡宅毯哲济锚署粕絮廉吸剔杯窄号伶崇抒褂桨菩刹尹小壁圣蛛限珊匙茧盔真镑否坊苹枪接夕靳齿湍辖帧产络谚点涵拄胁烂冕敢极茫号隋冈肘殖蟹杨令帝惠呈歹姑元瓷所氯盟财拟饭咱勘呸鼓库佯枕红娘神怯鸟锨烬捂醋嘛男磨迅义饼篡冕泛舆簿逝从凯脊绒歉惊昏凰芭膀靡童暂基于四元数方法的姿态解算方法分析摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。1引言捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞）2姿态矩阵的计算方法由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。加速度计导航计算姿态矩阵陀螺仪姿态矩阵计算姿态角计算载体速度与位置载体姿态角图1捷联惯导导航原理图2.1欧拉角微分方程式一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕3个不同的轴转动的3个角度来确定。如把载体坐标系作为动坐标系，把导航坐标系作为参考坐标系，则姿态角即为一组欧拉角，按一定的转动顺序得到导航坐标系到载体坐标系的关系。bEbZbEbYbEbXcossin0cossincoscos0sincossinsincos(1)根据欧拉角微分方程，由角速度可以求解3个姿态角。欧拉角微分方程式只有3个，但每个方程xxfxsin,cos都含有三角函数的运算，计算速度慢，且方程会出现“奇点”，方程式退化，故不能全姿态工作。2.2方向余弦矩阵微分方程式当一个坐标系相对另一个坐标系做一次或多次旋转后可得到另外一个新的坐标系，前者往往被称为参考坐标系或固定坐标系，后者被称为动坐标系，他们之间的相互关系可用方向余弦表来表示。方向余弦矩阵微分方程式可写为载体坐标系相对导航坐标系旋转角速度的斜对称矩阵表达式，方向余弦表是对这两种坐标系相对转动的一种数学描述。bEbEbEbCC(2)式中，bEb为载体坐标系相对导航坐标系旋转角速度的斜对称矩阵表达式。用方向余弦法计算姿态矩阵，没有方程退化问题，可以全姿态工作，但需要求解9个微分方程xCxCijij，计算量较大，实时性较差，无法满足工程实践要求。2.3四元数微分方程式四元数的数学概念是1843年由哈密顿首先提出的，它是代数学中的内容之一。随着捷联式惯性导航技术的发展，为了更简便地描述刚体的角运动，采用了四元数这个数学工具，用它来弥补通常描述刚体角运动的3个欧拉角参数在设计控制系统时的不足。四元数可以描述一个坐标系或一个矢量相对某一个坐标系的旋转，四元数的标量部分表示了转角的一半余弦值，而其矢量部分则表示瞬时转轴的方向、瞬时转动轴与参考坐标系轴间的方向余弦值。因此，一个四元数既表示了转轴的方向，又表示了转角的大小，往往称其为转动四元数。工程上一般运用范数为1的特征四元数，特征四元数的标量部分表示转角的一般余弦值，其矢量部分表示瞬时转轴n的方向。比如式(3)表示矢量R相对参考坐标系旋转一个转角，旋转轴二的方向由四元数的虚部确定，coscoscos、、表示旋转轴n与参考坐标系轴间的方向余弦值。''qRqR(3)式中:R为某矢量;cos2sincos2sincos2sin2cos321321pppkpjpipq四元数姿态矩阵微分方程式只要解4个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。3基于四元数法的姿态解算验证四元数法的正确性和有效性是将算法应用于工程实践的首要前提，在算法正确性的前提下应保证解算误差符合工程实践的需要。3.1四元数法正确性和有效性的验证本文根据四元数法与方向余弦法两种解算方法进行计算，通过对比两种方法的计算结果，验证四元数法的正确性和有效性。四元数法姿态矩阵计算的步骤如下:(1)初始四元数的确定，如式(4)其输人为初始的姿态角。2sin2sin2cos2cos2cos2sin2sin2cos2sin2cos2sin2cos2cos2sin2sin2sin2cos2cos2sin2sin2sin2cos2cos2cos0000000000000000000000000000321ppp(4)(2)四元数标量部分与矢量部分321ppp、、、的实时计算，输人信号为陀螺仪的数字输出信号dtibttt，其中i为zyx、、。计算方法采用二阶龙格库塔法，如式(5)21212/KKTtqTtqYTtKtqtTtqYtqtKbbb(5)(3)姿态矩阵的实时计算，确定姿态矩阵bEC，输入为npnpnpn321、、、。计算公式如式(6)。222123213223113223212223212313212322212222222pppppppppppppppppppppppppppCbE(6)(4)载体姿态角计算，以确定姿态角、、,输人为nTnTnTnTnT3323131211、、、、计算公式如式(7)nTnTnTnTnT3323111213arctanarctanarcsin(7)方向余弦法姿态矩阵的计算与四元数法的区别主要是姿态矩阵的描述不同，其描述如式(8)所示。coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscosbEC(8)其解算方向余弦矩阵微分方程为bEbECC，得到方向余弦矩阵bEC后可提取姿态角。验证工作均以二阶龙格库塔法展开计算。(1)针对单轴输入，两种解算方法的计算结果与数值理论值对比，比较其相对误差。计算条件为:陀螺仪输出角速率。=s/50,采样时间取为sT01.0、，该采样频率工程实践可行;各通道独立解算，初始角为90，终止角为90。以滚转通道为例，图2为四元数法解算结果，图3为方向余弦法解算结果。单轴数值计算结果说明:根据上述的计算条件，单轴输人下，四元数法与方向余弦法的计算结果都是正确的，即姿态解算算法在单轴输入情况下是正确的;姿态解算的相对误差数量级为%102左右，且四元数法与理论真值的相对误差更小。(2)针对三轴输人，两种解算方法之差与数值理论值对比，以比较两种方法的相对误差;计算条件为:载体三通道陀螺仪输出为角速率s/60；载体三通道陀螺仪输出为正弦角速率，幅值60A，频率Hzf4.0;采样时间sT01.0，工程实践可行；各通道独立解算，初始角均为0同样以滚转通道为例，图4为匀速输人下两种方法的相对误差，图5为正弦输人下两种方法的相对误差。三轴数值计算结果说明:三轴输入下，根据上述的计算条件，匀速与正弦输入下四元数法与方向余弦法的计算结果都是正确的，两种解算方法之差与理论真值比较的相对误差很小，相对误差的数量级为10-%左右。因为正弦输人时每步计算的角增量小，所以相对误差要稍小些。上述理论分析和数值仿真结果表明，四元数姿态解算算法在三轴输人情况下是正确的，且其计算精度高、理论完善，并具有良好的工程实践价值。摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确