1、捷联惯性解算捷联惯性解算 主讲：主讲： 申申 强强 副教授副教授机电工程与控制国家级重点实验室机电工程与控制国家级重点实验室010-689185001 1 姿态微分方程计算姿态微分方程计算1.1 1.1 方向余旋矩阵微分方程计算方向余旋矩阵微分方程计算)exp()()(00dttttbnbnbnbCC方程的解析解为方程的解析解为: :bnbnbnbCC则从则从 到到 经过一次循环可写成如下关系式：经过一次循环可写成如下关系式：kt1kt)exp(11dtkkttkkCC（1）bnbCnbC如果旋转角速度矢量如果旋转角速度矢量 的方向在整个更新间隔内，在空间保持固定的方向在整个更新间隔内，在空间
2、保持固定不变，则有：不变，则有：1dtkktt式（式（1 1）可写成）可写成kkkkACCC)exp(11kkttTzyxTzyxdt000 xyxzyz（2）! 4! 3! 2432IAk)()()(2222222yxzyzxzyzxyxzxyxzy222222422223)()(zxzxyy22124242! 6! 4! 21! 5! 31 IIAaak22cos1)sin(IAk或：或：0, 121aa为为1 1阶算法；阶算法；5 . 0, 121aa为为2 2阶算法；阶算法；5 . 0,61221aa为为3 3阶算法；阶算法；1.2 1.2 四元数微分方程解法四元数微分方程解法QMQd
3、tdQbnbbnb)(2121WMxyzxzyyzxzyxbnb0000)(kttkQdtQkk)21exp(11WkkQQ)21exp(1jjiPPMdtzyxxyzxzyyzxzyxttkk0)(00001WI222222zyx令23I4445I66)2sin2sin2sin2cos（)2sin2cos（2sin2cos21!5)2(!3)2(22!6)2(!4)2(!2)2(1!2)21(21)21exp()21exp(53642211zyxkkkkkkttkkjiQPQQIIQIQQdtQkkW!4)5.0(! 2)5.0(1)2cos(42ca)!5)5.0(!3)5.0(1( 5.
4、0)2/sin(42sa)(2sin2sin2sin2coskjiaakjirzyxsczyxk令：501.,scaa5.0,2)5.0(12scaa)6)5.0(1( 5.0,2)5.0(122scaa1 1阶阶2 2阶阶3 3阶阶四元数初值确定和规范化处理3322110332211333221123322111121121121121TTTqcccqcccqcccq)()()()()()()()()(122103311302233201ccsignqsignqsignccsignqsignqsignccsignqsignqsign2 2 速度算法速度算法l 利用方向余旋矩阵进行速度求解利用
5、方向余旋矩阵进行速度求解n1nnnienn2engvfvdttdttdtCdtkkkkkkkkttkttktttt1111bbbnbnn)()(fACfACffu!4!3!2)（432IAtdtttkb bdtkkttk5.021b bb bb bn nfffCu忽略二阶及以上阶次项：忽略二阶及以上阶次项：11kkkkttttkdtdtb bb bn nffCudtttkbfv)(21211111dtkkttkkkkvfvvCub bb bn n旋转修正动态修正划摇误差l 利用四元数行速度求解利用四元数行速度求解kttkkkkdtkkqvfvvqu)(21211111b bb bn nl 导航
6、算法导航算法tttdtdtdt0n10nnnie0nn2engvfvtgkknnn1uvvnnnn12eniellllttvIvdttkkttk1b)(fAC)(2121bb1111dtkkttkkkkvfvvCkkkACC111kkdtkkttk1211ttkdt11kkl 导航算法导航算法 在现代的捷联系统中，研究重点集中在为求解姿态和导航微分方程更确切的积分解析解而使用的更精确的算法上。计算精度的改善使捷联算法更容易验证，因为在给定的运动条件下，试验结果能够很好地符合解析解。通过现代计算机技术的应用，更精确的算法会得到更广泛应用；计算机的快速处理速度加上长的浮点字长使得这样的算法能够执行。3 3 捷联惯导误差源捷联惯导误差源捷联惯导主要固误差源为：捷联惯导主要固误差源为：（1 1）惯性传感器误差；）惯性传感器误差；（2 2）初始对准误差；）初始对准误差；（3 3）计算误差。）计算误差。 导航结果误差取决于上述误差大小和误差的传播方式。计导航结果误差取决于上述误差大小和误差的传播方式。计算误差产生的本质是对微分方程进行数值求解时计算频率不算误差产生的本质是对微分方程进行数值求解时计算频率不足以适应载体的动态。足以适应载体的动态。4.4.误差传播方程误差传播方程ibnbininCbnbnfCfvvp000
