基于Simulink与M语言的捷联惯导系统仿真方法研究

第１０卷第１６期２０１０年６月　科学技术与工程　Ｖｏ１．１０　Ｎｏ．１６　Ｊｕｎｅ　２０１０　１６７１－１８１５（２ＯＬＯ）１６—４０３２—０５　Ｓｃｉｅｎｃｅ　Ｔｅｃｈｎｏｌｏｇｙ　ａｎｄ　Ｅｎｇｉｎｅｅｒｉｎｇ　＠２０１０　Ｓｃｉ．Ｔｅｃｈ．Ｅｎｇｎｇ．　基于Ｓｉｍｕｌｉｎｋ与Ｍ语言的捷联惯导系统　仿真方法研究　胡　鑫韩崇伟　王玮　马捷　（西北机电工程研究所，咸阳７１２０９９）　摘要在研究捷联惯导系统的导航解算算法时，经常需要对算法进行仿真以测试算法的性能。比较研究了在Ｍａｔｌａｂ中实　现捷联惯导系统仿真的两种方法：一种是采用Ｍ语言编写各功能函数，通过函数调用的方式实现捷联惯导系统的仿真；另一　种是采用Ｓｉｍｕｌｉｎｋ模型与Ｍ语言结合的方式实现捷联惯导系统的仿真。分别用这两种方法进行了１　ｈ的导航解算仿真，仿真　结果表明，这两种仿真方法均具有很高的仿真速度和仿真精度，都能满足工程应用的需要，与第一种方法相比第二种仿真方　法可以方便地设定仿真时间、惯性测量组件的采样频率，也可以在仿真中暂停或停止仿真，具有更好的可操作性。　关键词Ｓｉｍｕｌｉｎｋ　Ｍ语言　捷联惯导系统　仿真　中图法分类号ＴＰ３９１．９；　文献标志码Ａ　在研究捷联惯导系统的导航解算算法时，经常需　解算并且控制导航时间的长短。在主程序中首先　设定仿真参数，如地球参数、惯性测量组件的采样　周期以及导航时间等，然后依次调用轨迹产生程序　和导航解算程序进行导航解算。本文中设定惯性　测量组件的采样周期为０．０１　Ｓ，导航时间为１　ｈ。导　航主程序的流程图如图１所示。　要对算法进行仿真以验证算法的正确性。现提出在　Ｍａｔｌａｂ环境下实现捷联惯导系统仿真的两种方法：一　种是采用Ｍ语言编写各功能函数，通过函数调用的　方式实现捷联惯导系统的仿真（简称方法１）；另一种　是采用Ｓｉｍｕｌｉｎｋ模型与Ｍ语言结合的方式实现捷联　惯导系统的仿真（简称方法２），并且研究了这两种方　法在仿真结果的精度和仿真速度方面的异同点，对捷　联质导系统的研究具有一定的参考价值。　塑　！　设定仿真参数　１采用Ｍ语言的捷联惯导系统仿真方法　采用Ｍ语言的捷联惯导系统仿真方法，就是在　Ｍａｔｌａｂ环境下用Ｍ语言编写捷联惯导系统的导航　二二］二　设定初始姿态、速　度和位置　调用轨迹产生程序　调用导航解算程序　存储导航结果　否　主程序和子程序，通过主程序调用子程序的方式实　现对捷联惯导系统的仿真。由于研究的重点是在　Ｍａｔｌａｂ环境下实现捷联惯导系统仿真的两种方法以　及它们之间的异同点，而不是捷联惯导系统的算　法，所以在主程序中直接设定初始姿态角然后进行　导航解算，可省略初始对准的过程。　１．１导航主程序设计　＜　墼　二＝：　＋是　绘制导航过程曲线　结束　导航主程序的功能就是调用子程序进行导航　２０１０年３月９日收到　１导航主程序流程图　１６期　胡鑫，等：基于Ｓｉｍｕｌｉｎｋ与Ｍ语言的捷联惯导系统仿真方法４０３３　１．２子程序设计　１．２．１轨迹产生程序　航解算程序的流程图如图３所示。　轨迹产生程序的作用就是根据设定的姿态、方　位、位置变化规律产生陀螺仪和加速度计的输出数　据。轨迹产生程序的流程图如图２所示。具体的轨　姿态更新　迹生成算法参见文献［１］。　橄据拔体的姿态变化规律计算　我体当前的姿态伯和方位霸　悯　、　、　、　－　楸撅拔体的他　变化规件计锋　载体当漪灼经度、纬艘和　嫂　，　汁筇地球血角　怀系，。ｐ载体位　霞的坐标值．并求赢角坐标的　阶导数和　阶导数　◆　利ｊＨ地球麻角　标系ｔｌＩ钱体位　标的阶导数和ｃ　ｎ计ｔ算　利用　、蝶、　、　产　ｉ陀螺仪的输ｆ＝ｌｔ＝　；ｂ　利删ｃ　、Ｏ）ｉ”ｅ、　、ｃ　以及藏体　鬣　坐标的　阶导数，　生加速艘计的输Ｌｌ　图２轨迹产生程序的流程图　设定在导航时间内，载体姿态、方位、位置的变　化规律为：　载体姿态变化：俯仰角　＝５￣ｃｏｓ（，ｒｒｔ／５）；横滚角　ｙ＝５￣ｃｏｓ（ｅｆｔ／５）。载体方位变化：方位角ｌｆ，＝　５￣ＣＯＳ（￣ｒｔ／５）。载体位置变化：纬度　＝３１。＋　３￣ｃｏｓ（ｚｒｔ／２０ｏｏｏ）；经度Ａ＝１０３。十５。ＣＯＳ（　／２５　ｏｏｏ）；　高度ｈ＝３９０＋１０ｃｏｓ（￣ｒｔ／５　ｏｏｏ）。　１．２．２导航解算程序　导航解算程序主要进行姿态更新、速度更新、　位置更新以及地球参数更新。本文中设定姿态、速　度、位置以及地球参数的更新周期均为０．０２　Ｓ。导　速度更新　　Ｉ位置更新　【地球参数更新　（　结束　）　图３导航解算的程序流程图　姿态更新算法采用优化二子样旋转矢量算法。　令ｔ　时刻的姿态四元数为Ｑ（ｔ　），ｔ　时刻的姿态　四元数为Ｑ（　），【ｔ　，ｔ川】时间段内的姿态变化四　元数为ｇ（　），则有ｌ２ｊ　Ｑ（ｔ　）＝Ｑ（ｔｋ）　霉（　）　其中：Ｑ（ｔｋ）与垡（　）之间的乘法为四元数乘法；　口（　）＝ｃ。ｓ詈＋　ｓｉｎ芋，　为载体坐标系从　时　刻至ｔ　时刻的等效旋转矢量，　＝Ｉ　Ｊ，根据优　化二子样旋转矢量算法有：　：ＡＯ，＋△　＋　。△　。起始时刻的姿态四元数Ｑ（０）根据初　始姿态角与四元数之间的转换关系获得ｌ３］，求得　Ｑ（ｔ…）后根据姿态角和姿态四元数之间的关系可　以求出ｔ　时刻的姿态角，从而完成姿态更新。　速度更新算法利用简化算法　］：　＝ｌ，　＋（ｃ　＋［０　０　一　】　）ｔ　。　其中：，，　为ｔ　时刻载体速度在导航系中的投影；　ｌ，　一。为ｔ　一。时刻载体速度在导航系中的投影；　为　姿态矩阵ｃ：的转置矩阵；　为加速度计输出的比　力；ｔ　为速度更新周期。　位置更新计算利用位置更新迭代计算式实现。　位置更新迭代式为　］：　＝　＋　ｉ！关　。　科学技术与工程　１０卷　ｚ，　（ｍ一１）＋　（ｍ）　２（ｎ』ｖ（　一Ｉ）＋ｈ　一１）　ｈ　＝ｈ　＋Ｓ　－（　ｍ＿１）＋　））ｔ　。　其中：　，Ａ　，ｈ　分别为ｔ　时刻的纬度、经度和高　度；Ｌ　，Ａ　，ｈ　分别为ｔ　时刻的纬度、经度和　高度；　（　），　Ⅳｎ（　）分别为ｔ　时刻和ｔ　时刻的北向　速度；　（　），　㈤分别为ｔ　时刻和ｔ　时刻的东向　速度；　ｎｕ（　），　（　）分别为ｔ　时刻和ｔ　时刻的天向　速度；尺　（　，Ｒ　（　分别为ｔ　时刻的子午圈曲　率半径和卯酉圈曲率半径；ｔ　为位置更新周期。　在位置更新之后，某些地球参数也随之发生了　变化，因此需要对地球参数进行更新，需要更新的　地球参数包括：载体所在地的子午圈曲率半径Ｒ　和卯酉圈曲率半径Ｒ　、当地重力加速度ｇ等。具体　的更新计算公式为　Ｒｆｌｆ　Ｒ　（Ｉ一２ｅ＋３ｅｓｉｎ　Ｌ）；　Ｒ，ｖ　Ｒ　（１＋ｅｓｉｎ　Ｌ）；　ｇ＝ｇ０（１＋０．００５　２７０　，　Ａ　９ｓｉｎ　Ｌ＋　’０．０００　０２３　ｖ　，　２７１　８ｓｉｏ　．－　ｍ　ｎ４　）一０．１３１３０　００３　０８６　ｈ。　２　ＳｉｍｕＵｎｋ模型与Ｍ语言结合的捷联惯导　系统仿真方法　利用Ｓｉｍｕｌｉｎｋ模型与Ｍ语言结合实现捷联惯　导系统的仿真时，首先利用Ｓｉ一一ｍｕｌ一ｉｎｋ工具箱中的工　一一一一÷一具搭建捷联惯导系统的仿真模型框图，然后用Ｍ语　言编写相关的函数供模型中的Ｍａｔｌａｂ函数调用＿５］。　现设计的捷联惯导系统的模型框图如图４所示。　在图４中Ｄｉｇｉｔａｌ　Ｃｌｏｃｋ模块以０．０１　Ｓ为采样周　期输出时间，ｄａｔａ～ｇｅｎｅｒａｔｏｒ模块根据Ｄｉｇｉｔａｌ　Ｃｌｏｃｋ　模块输出的时间产生陀螺仪和加速度计的输出数　据，数据生成算法与１．２．１节中的轨迹生成算法相　同。ＳＩＮＳ—ｃｏｍｐｕｔｅ模块根据陀螺和加速度计的输　出数据进行导航解算并将解算出的姿态、速度和位　置数据送入Ｍａｔｌａｂ工作空间的相关变量中存储，以　便最后输出导航过程曲线，导航解算程序与１．２．２　节中的导航解算程序完全相同。ＳＩＮＳ—ｃｏｍｐｕｔｅ模　块中程序的流程图如图５所示。　Ｖ　ｐ—ｓ　Ｖ　Ｓ　ｐｏｓｉｔｉｏｎ　ｖｅｌｏｃｉｔｙ　图４捷联惯导系统的仿真模型框图　接收导航时间以及陀螺仪　和加速度计的输出数据　ｌ调用导航解算程序ｌ　Ｊ输出导航结果至工作空间　ａ　◆　（　结束　）　图５　ＳＩＮＳ—ｃｏｍｐｕｔｅ模块中程序的流程图　３仿真分析　在Ｍａｔｌａｂ中分别采用方法１和方法２对捷联　惯导系统的导航解算算法进行了１　ｈ的仿真，得到　仿真结果如图６和图７所示，仿真所用时间如表１　所示。　表１　两种方法所用的仿真时间表　仿真方法　所用仿真时间／ｓ　方法１　７ｌ　方法２　８３　　一。　０　０　ｌ６期　胡鑫，等：基于Ｓｉｍｕｌｉｎｋ与Ｍ语言的捷联惯导系统仿真方法　４０３５　　：鞔｛　＃　ｌ　ｏｏ０　２　００ｏ　３　Ｏｏ０　３　０００　Ｉ　脯　瓤　荆　弩　端　＿　辩　＾。　１　０００　２　０００　３’０００　３　Ｏｏｏ　０．２　０　…～……，　卜ｓ．吕　田　．ｇ　．　。０。２０　ｌ　ｏ００　２　０００　３　０００　‘ｏ　ｇ・３　００００　ｌ　Ｏ０ｏ　２　０ｏｏ　３　ｏｏｏ　ｔ／ｓ　ｔ／ｓ　（ａ）姿态和方位误差　（ｂ）速度误差估计曲线　ｌ。＾Ｚ　ｌ。　＾　咖　ｌ　００Ｏ　２　Ｏｏｏ　３　０００　０．５　－０．０　０　１　０００　２　０００　～　３　０００　。　：　１。。。　２　０００　３　０００　ｔ／ｓ　（ｃ）位置误差估计曲线　一。　。一／＾　一＿ｓ．吕　图６６　０　方法１的仿真结果　０　１．０．１圳　・－　３　０００　ｏ　～　＊　∞～一　“　…　‘。－０．１０　ｌ　ｏｏ０　２ｏｏｏ　３　ｏｏ０　３　０００　１　０００　：，０…………～……一一…一　Ｓ一。・　ｏ　１　０００　２　０００　３　ｏ００　７．１　Ｏｏｏ　－　一ｔ６目一３　ｏｏｏ０　ｌ　０００　２　００　３　ｏｏ　ｔ／ｓ　（ｂ）速度误差估计曲线　２　０００　３　０００　§　０　２　ｏｏＯ　３　０００　１７　｜　．Ｉ　Ｉ｜　｜｜　．（ｃ）位置误差估计曲线　图７方法２的仿真结果　根据图６和图７得到方法１与方法２之间的导　航解算误差如图８所示。　从图８可以看出，两种仿真方法的姿态解算误　差小于２×１０　度，速度解算误差小于１　ｘ　１０～ｍ／ｓ，　纬度解算误差小于５×１０　度，经度解算误差小于　ｌ　０ｏ０　２Ｏｏｏ　３　０００　～　《琏ｌ穗鞣　１　０００　０　２　０００　∞　３　Ｏｏ０　卜ｓ．ｇ　－ｓ．日　Ｏ　ｌ　０００　２　０００　３　０Ｏ０　一∞　Ｄｔ　／ｓ　一∞　０　（ａ）姿态解算误差　２　２　—Ｏ　，０．　∞　∞　：　ｓ　Ｏ　ＯＯｌ　０００　２　０００　３　０００　ｌ　０００　２　０００　３　０００　０　＿ｓ．舀Ｒ＿０＿【　一。　＿【＼７９一　＿０ｌｔＣｏ）速度解算误差　　／ｓ　　●．ｎ－Ｕ，ｌ　０　Ｏ　６　Ｏ　０　ｌ　２　ｏｏＯ　Ｉ　３　０００　咖　２　０ｏｏ　３　０００　１　６　＋　。　０　１　０００　２　ｏｏｏ　３　ｏ０ｏ　ｔ｝ｓ　（ｃ）位置解算误差　图８方法１与方法２解算的差别　１×１０　度，高度解算误差小于１×１０～ｍ，都能满足　工程应用的需要，但是方法２可以方便地设定仿真　时间、惯性测量组件的采样频率，也可以在仿真中　暂停或停止仿真，具有更好的可操作性。　４结论　提出了在Ｍａｔｌａｂ中实现捷联惯导系统仿真的　两种方法，分别采用这两种方法对捷联惯导系统的　导航解算进行了１　ｈ的仿真，仿真结果表明，采用　Ｓｉｍｕｌｉｎｋ模型与Ｍ语言结合的仿真方法与完全采用　Ｍ语言的仿真方法在仿真速度和仿真结果的精度　方面的性能相当，但是前者可以方便的设定仿真时　．—Ｕｌ４０３６　科学技术与工程　１０卷　间、惯性测量组件的采样频率，也可以在仿真中暂　停或停止仿真，具有更好的可操作性。　参１赵鸿，赵ｉｃｓ，１９９８；２１（１）：１９－－２８　秦永元．惯性导航．北京：科学出版社．２００６　４　Ｓａｖａｇｅ　Ｐ　Ｇ．Ｓｔｒａｐｄｏｗｎ　ｉｎｅ￣ｉｌ　ｎａｖｉａｇａｔｉｏｎ　ｉｎｔｅｇｒａｔｉｏｎ　ｌｇｏｒａｉｔｈｍ　ｄｅｓｉｇｎ　考文献　ｐａｒｔ　２：ｖｅｌｏｃｉｔｙ　ａｎｄ　ｐｏｓｉｔｉｏｎ　ａｌｇｏｒｉｔｈｍｓ．Ｊｏｕｒｎａｌ　ｏｆ　Ｇｕｉｄａｎｃｅ，Ｃｏｎｔｒｏｌ　ａｎｄ　Ｄｙｎａｍｉｃｓ，１９９８；２１（２）：２０８－－２２９　忠，龙国庆．捷联惯性导航系统飞行轨迹数据生成　齐鑫，秦永元，朱新颖，等．基于ＭＡＴＬＡＢ／Ｓｉｍｕｌｉｎｋ的捷联惯性　与惯性器件建模．系统仿真学报，２００５；１７（５）：１０２６－－１０２８　２　Ｓａｖａｇｅ　Ｐ　Ｇ．Ｓｔｒａｐｄｏｗｎ　ｉｎｅａｉａｌ　ｎａｖｉｇａｔｉｏｎ　ｉｎｔｅｇｒａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ　ｄｅｓｉｇｎ　ｐａｒｔ　１：ａｔｔｉｔｕｄｅ　ａｌｇｏｒｉｔｈｍｓ．Ｊｏｕｒｎａｌ　ｏｆ　Ｇｕｉｄａｎｃｅ，Ｃｏｎｔｒｏｌ　ａｎｄ　Ｄｙｎａｍ－　导航系统仿真．计算机测量与控制，２００８；１６（８）：１１６ｌ一１１６３　Ｒｅｓｅａｒｃｈ　ｏｎ　Ｓｉｍｕｌａｔｉｏｎ　Ｍｅｔｈｏｄ　ｏｆ　ＳＩＮＳ　Ｂａｓｅｄ　ｏｎ　Ｓｉｍｕｌｉｎｋ　ａｎｄ　ｎ　Ｌａｎｇｕａｇｅ　ＨＵ　Ｘｉｎ，ＨＡＮ　Ｃｈｏｎｇ—ｗｅｔ，ＷＡＮＧ　Ｗｅｔ，ＭＡ　Ｊｉｅ　（Ｎｏｒｔｈｗｅｓｔ　Ｉｎｓｔｉｔｕｔｅ　ｏｆ　Ｅｌｅｃｔｉｒｃａｌ　ａｎｄ　Ｍｅｃｈａｎｉｃａｌ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｘｉａｎｙａｎｇ　７１２０９９，Ｐ．Ｒ．Ｃｈｉｎａ）　ｌ　Ａｂｓｔｒａｃｔ］　Ｉｔ　ｉｓ　ｎｅｃｅｓｓａｒｙ　ｔｏ　ｔｅｓｔ　ｔｈｅ　ｐｅｒｆｏｒｍａｎｃｅ　ｏｆ　ｔｈｅ　ｎａｖｉｇａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ　ｔｈｒｏｕｇｈ　ｓｉｍｕｌａｔｉｏｎ　ｗｈｅｎ　Ｉｅ．ｓｅａＩｃｌ．　ｈｉｎｇ　ＳＩＮＳ．Ｔｈｅ　ｔｗｏ　ｍｅｔｈｏｄｓ　ｗｉｔｈ　ｗｈｉｃｈ　ＳＩＮＳ　ｓｉｍｕｌａｔｉｏｎ　ｉｓ　ｉｍｐｌｅｍｅｎｔｅｄ　ｕｓｉｎｇ　Ｍａｔｌａｂ　ｓｏｆｔｗａｒｅ　ｈａｖｅ　ｂｅｅｎ　ｓｔｕｄｉｅｄ．．　Ｔｈｅ　ｆｉｒｓｔ　ｍｅｔｈｏｄ　ｉｍｐｌｅｍｅｎｔ　ＳＩＮＳ　ｓｉｍｕｌａｔｉｏｎ　ｔｈｒｏｕｇｈ　ｆｕｎｃｔｉｏｎ　ｃａｌｌｉｎｇａｎｄ　ｔｈｅ　ｆｕｎｃｔｉｏｎ　ｉｓ　ｐｒｏｇｒａｍｅｄ　ｗｉｔｈ　Ｍ　１ａｎ—　ｇｕａｇｅ．Ｔｈｅ　ｓｅｃｏｎｄ　ｍｅｔｈｏｄ　ｕＳｅ　Ｓｉｍｕｌｉｎｋ　ｍｏｄｅｌ　ｗｉｔｈ　Ｍ　ｌａｎｇｕａｇｅ　ａｓ　ｔｈｅ　ｗａｙ　ｔｏ　ａｃｈｉｅｖｅ　ｔｈｅ　ｓｉｍｕｌａｔｉｏｎ　０ｆ　ＳＩＮＳ１　ｈ　．ｎａｖｉｇａｔｉｏｎ　ｓｉｍｕｌａｔｉｏｎ　ｈａｓ　ｂｅｅｎ　ｉｍｐｌｅｍｅｎｔｅｄ　ｗｉｔｈ　ｔｈｅ　ｔｗｏ　ｍｅｔｈｏｄｓ　ｒｅｓｐｅｃｔｉｖｅｌｙＴｈｅ　ｓｉｍｕｌａｔｉｏｎ　ｒｅｓｕｌｔｓ　ｓｈｏｗ　ｔｈａｔ　ｔｈｅ　．ｔｗｏ　ｓｉｍｕｌａｔｉｏｎ　ｍｅｔｈｏｄｓ　ａｒｅ　ｏｆ　ｈｉｇｈ　ｓｉｍｕｌａｔｉｏｎ　ｓｐｅｅｄ　ａｎｄ　ｓｉｍｕｌａｔｉｏｎ　ａｃｃｕｒａｃｙａｎｄ　ｃｏｕｌｄ　ｍｅｅｔ　ｔｈｅ　ｎｅｅｄｓ　ｏｆ　ｅｎｇｉ—　，ｎｅｅｒｉｎｇ　ａｐｐｌｉｃａｔｉｏｎｓ．Ｂｕｔ　ｃｏｍｐａｒｅｄ　ｗｉｔｈ　ｔｈｅ　ｆｉｒｓｔ　ｍｅｔｈｏｄ，ｆｈｅ　ｓｅｃｏｎｄ　ｏｎｅ　ｃｏｕｌｄ　ｂｅ　ｅａｓｉｌｙ　ｔｏ　ｓｅｔ　ｕｐ　ｔｈｅ　ｓｉｍｕｌａｔｉｏｎ　ｔｉｍｅ　ａｎｄ　ｔｈｅ　ｓａｍｐｌｉｎｇ　ｆｒｅｑｕｅｎｃｙ　ｏｆ　ｉｎｅｒｔｉａｌ　ｍｅａｓｕｒｅｍｅｎｔ　ｕｎｉｔ．Ａｎｄ　ｙｏｕ　ｃｏｕｌｄ　ａｌｓｏ　ｐａｕｓｅ　ｏｒ　ｓｔｏｐ　ｔｈｅ　ｓｉｍｕｌａｔｉ０ｎ　ｗｈｅｎ　ｓｉｍｕｌａｔｉｎｇ．Ｓｏ　ｔｈｅ　ｓｅｃｏｎｄ　ｍｅｔｈｏｄ　ｉｓ　ｏｆ　ａ　ｂｅｔｔｅｒ　ｏｐｅｒａｂｉｌｉｔｙ．　［Ｋｅｙ　ｗｏｒｄｓ］　ｓｉｍｕｌｉｎｋ　Ｍ　ｌａｎｇｕａｇｅ　ＳＩＮＳ　ｓｉｍｕｌａｔｉｏｎ