搜索
第二部分高动态环境下捷联惯导系统姿态算法研究1．引言在常规的捷联惯性导航系统的姿态算法中，总是认为载体坐标系和参考坐标系间的转换是通过一系列的转动来实现的，而这些转动间的次序并不重要，这是基于无限小转动是矢量的原理来得到的。3-1-2欧拉角转动（1）欧拉角由参考系OXYZ至载体系bbbZYOX的变换可以通过依次绕不同坐标轴的三次连续转动来定义。姿态欧拉角的运动学方程：()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−++=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θωθωφθωθωφωφθωθωφγθφsincossincossincoscossincoscos1xzzxyzx&&&方程中存在三角函数，给实时计算带来困难；且当o90=φ时，方程中出现奇点，使θ、γ的解变得不确定，因而使其使用受到限制，不能用于全姿态飞行器上。（2）方向余弦阵由参考系OXYZ至载体系bbbZYOX的姿态矩阵为()()()()γφθθφγzxyCCCC=,,第1次转动：()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=1000cossin0sincosγγγγγzC第2次转动：()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=φφφφφcossin0sincos0001xC第3次转动：()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=θθθθθcos0sin010sin0cosyC方向余弦矩阵的微分方程为：Ω=CC&式中⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−−=Ω000xyxzyzωωωωωω方程的解为：∫+Ω=+1exp1kkttkkdtCC（3）四元数四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢量相对于某一坐标系的旋转⎥⎦⎤⎢⎣⎡=130qqq实数为四元数的标量部分，为四元数的矢量部分。0q13q⎟⎠⎞⎜⎝⎛=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎟⎠⎞⎜⎝⎛=2sin2cos321130ϕϕeqqqqq式中，是表示旋转轴方向的单位向量，eϕ是旋转角。四个元素满足正交约束方程123222120=+++qqqq第一次转动：T⎥⎦⎤⎢⎣⎡=′2sin002cosγγq第二次转动：T⎥⎦⎤⎢⎣⎡=′′002sin2cosφφq第三次转动：T⎥⎦⎤⎢⎣⎡=′′′02sin02cosθθq由上述3-1-2欧拉角转动得到合成四元数qqqq′′′⊗′′⊗′=把上式表示为三次坐标转换矩阵的乘积，有()()()()()qqqqqqq′′′′′′=′′′⊗′′⊗′=zxyAAAAA由四元数表征的运动学微分方程为qqbΩ=21&式中⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡−−−−−−=Ω0000zyxzxyyxzxyzbωωωωωωωωωωωω方程的解为[]111exp21++∫+Ω=kttbkkkdtqq我们在对运动学方程求解时，都用了角速度矢量的积分，即∫∆+=∆httdtt)(ωθ。而上述积分有意义的前提条件是角速度矢量方向不变。在力学中，刚体的有限转动是不可交换的。这个转动的不可交换性决定了转动不是矢量，也就是两次以上的转动不能相加。根据欧拉旋转定理，飞行器的转动从任一给定方位到任一其它方位可通过连续绕瞬时轴转动获得，而瞬时角速度方向在空间不断地改变，对一个在空间方向随时间变换的角速度矢量进行积分是无意义的。()当不是定轴转动时，即tω的方向在空间变化时，式∫∆+=∆httdtt)(ωθ是不成立的，即角改变量不是矢量。因此，在采用角速度矢量积分时，使得计算产生了误差，称为转动不可交换性误差。对于捷联惯导姿态更新来说，锥运动是最恶劣的工作环境条件，此运动造成的不可交换性误差影响最大，会诱发数学平台的严重漂移。因此，锥运动通常被作为检验姿态算法优劣的条件，也就说如果能够确保锥运动环境条件下的算法漂移最小，就一定能确保在其余环境条件下的算法漂移最小。2．圆锥运动与圆锥误差圆锥效应是刚体运动的一种几何现象。刚体受到环境振动影响或本身具有的角运动，使得其在二个正交轴方向存在频率相同的角振动速率时，第三个正交轴在空间将绕其平均位置作锥面或近似锥面的运动，称为刚体的圆锥运动或圆锥效应。Y轴：()tψαcosZ轴：()tψαsinbxsXsYbysZbzO圆锥运动对应的角速度矢量在载体坐标系上的分量为ttbzbybxψαψωψαψωαψωcossinsinsin2sin22⋅=⋅−=−=该圆锥运动会在载体坐标系轴上产生常值角速度，该角速度具有与陀螺常值漂移相同的性质，该角速度必为轴陀螺所敏感，从而产生视在的测量误差，即圆锥误差。bxbx以上所讨论的两个角振动的相位差为，当相位差为°90ϕ时，同样可以导出轴的常值角速度为bxϕαψωsin2sin22⋅−=bx由于角振动速度的幅值、频率和相位一般是随机变量，由上式给出的圆锥误差表达式不能用于实时的修正计算，只能用来说明圆锥误差的存在和对误差量级的估计。圆锥误差与刚体有限转动产生的不可交换误差具有相同性质。换言之，圆锥误差是在三维角振动环境下刚体有限转动产生的不可交换误差。因此，可以在姿态算法中用一切解决不可交换性误差的方法来减少圆锥误差的影响。3．旋转矢量算法3.1旋转矢量的定义由刚体定点转动的欧拉定理，参考坐标系可通过绕欧拉轴旋转特定的角度与固结于刚体的动坐标系重合。设欧拉轴上的单位矢量为，旋转角度为nrφ，则旋转矢量定义为[]TzyxnΦΦΦ=⋅=Φrφ，旋转矢量示意图当刚体的定轴转动用旋转矢量表示为如下时，刚体的运动可称为圆锥运动：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=Φ)sin()cos(0ttψαψα其中α表示圆锥运动的锥半角，ψ表示圆锥运动的角频率。刚体在三维空间作任意转动时，其等效旋转矢量)(tΦ在轴的分量与刚体运动角速度在轴上的分量之间有如下关系式：iiittiiAdt+=Φ∫0ω式中，iA为由于测量的不可交换性而造成的不可测量项。对圆锥误差的补偿，实际上就是求iA的大小，旋转矢量法能够对圆锥误差进行有效的补偿。3.2旋转矢量微分方程旋转矢量微分方程为计算捷联惯性系统的姿态矩阵建立了全面的理论基础。根据Euler理论，旋转矢量是姿态矩阵大小为+1的特征值所对应的特征向量，即0)(=Φ−IC且有22][)cos1(][sin×Φ−+×Φ+=φφφφIC式中，[]TzyxΦΦΦ=Φ表示旋转矢量，C为机体坐标系与参考坐标系之间的方向余弦矩阵，I表示单位矩阵，2/1)(ΦΦ=Tφ。旋转矢量微分方程为)()cos1(2sin11212ωφφφφωω×Φ×Φ⎥⎦⎤⎢⎣⎡−⋅−⋅+×Φ+=Φ&式中，[]Tzyxωωωω=为机体角速度向量，方程右边第二项与第三项之和就是有限转动引起的不可交换项，即圆锥误差。将上式右边第三项系数按泰勒级数展开，则上式简化为)(12121ωωω×Φ×Φ+×Φ+=Φ&上式即为旋转矢量微分方程的常用形式。分析上式可以看出：当载体作定轴转动时，旋转矢量Φ的方向与角速度ω的方向一致，因此上式右边的后两项为零，这时相当于直接采用角速度ω进行姿态解算，是不会产生误差的。但在实际情况中，载体运动的角速度ω的方向是不断变化的，从而导致某一时刻角速度ω与当前时刻所积累的旋转矢量Φ方向不一致，上式右边后两项不为零，倘若仍然直接采用角速度ω直接进行积分解算就会带来刚体转动的不可交换性误差。而且，当载体作锥运动时，旋转矢量Φ和角速度ω相互垂直，此时上式右边的后两项最大，所以锥运动反映了不可交换误差最为恶劣的情况。3.3旋转矢量微分方程的求解在实际应用中，为保证实时性并考虑运算方便，仅取前两项，得ωω×Φ⋅+=Φ21&在姿态更新周期1−−=kktth内，通过对陀螺的角增量进行等间隔采样，可求得等效旋转矢量的估值。且根据等间隔采样次数，求解方法分为单子样法、双子样法、三子样法与四子样法。其中，单子样法就是四元数法。（1）等效旋转矢量算法的一般表达式设为()hΦ[]kktt,1−时间段内的等效旋转矢量，其中1−−=kktth，。设对()00=Φ[]kktt,1−内陀螺的角增量进行N次等间隔采样，并假定载体角速度可用关于时间的t1−N次多项式进行拟合，可得到等效旋转矢量算法的一般表达式()()∑∑∑−==×+=Φ111ˆNiNijjiijNiiKhθθθ式中，N为子样数，iθ为陀螺输出的角增量。（2）优化准则对捷联惯导系统来说，锥运动是最恶劣的工作环境条件，它会诱发数学平台的严重漂移，所以对旋转矢量算法作优化处理时常以锥运动作为环境条件。这就是说，如果能确保锥运动环境条件下的算法漂移最小，就一定能确保在其余环境条件下的算法漂移最小。取围绕惯性坐标系轴旋转的圆锥运动：x⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=Φ)sin()cos(0ttψαψα旋转矢量算法的优化设计的误差准则是使误差xxΦ−Φ=Φˆε达到最小。（3）姿态更新在捷联系统的实际姿态解算中，为了补偿由于有限转动造成的不可交换性误差，采用旋转矢量算法利用角增量计算姿态更新周期内的姿态变化量，但进行姿态更新时是采用四元数实现。记1−kt时刻导航系至机体系的姿态四元数为()1−ktQ，kt时刻导航系至机体系的姿态四元数为()ktQ。设在姿态更新周期1−−=kktth内，机体系的姿态变化四元数为，则姿态更新如下()hq()()()hqtQtQkk⊗=−1且()()()()()⎟⎠⎞⎜⎝⎛Φ+⎟⎠⎞⎜⎝⎛=2sin2coshhhhhqφφφ3.4简化形式的旋转矢量优化算法假设在[]htt+,间隔内，对陀螺输出信号进行N次采样，iθ表示第次采样的陀螺输出角增量信号，有i()[]()⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣⎡⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+⋅⋅⋅⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+⋅⋅⋅−⋅−===∫+−+hNitNhhNitNhhNNidhNithNiti212cos2sinsin2212sin2sinsin22sin2,,2,1)(21ψψαψψααψττωθL可得()()⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡⎭⎬⎫⎩⎨⎧⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+−⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+⎭⎬⎫⎩⎨⎧⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+−⎥⎦⎤⎢⎣⎡⎟⎠⎞⎜⎝⎛−+⎟⎠⎞⎜⎝⎛−⎟⎠⎞⎜⎝⎛−=×hNjthNitAhNjthNitAhNjiNhji212sin212sin212cos212cos22sin2sinsin422ψψψψψψαθθ()⎟⎠⎞⎜⎝⎛⎟⎠⎞⎜⎝⎛−=NhNhA2sinsin2sin42ψααψ由上式可以看出：在jiθθ×中，轴分量只与相对时间x()hji−有关，而与绝对时间无关；y轴和z轴分量是绝对时间t的余弦、正弦函数。在本文的圆锥运动下，能引起漂移的误差出现在轴上。因此，具有相同时间间隔的两个角增量向量的叉乘对圆锥误差的贡献是相等的，可以不考虑它们与绝对时间的关系。x利用这个性质，可以将圆锥误差补偿公式简化为NNiiiNiibθθθ×⎟⎟⎠⎞⎜⎜⎝⎛+=Φ∑∑−==111ˆ可以看出，简化后的算法计算量大大减小。3.5改进旋转矢量优化算法的一般形式（1）利用前一周期陀螺角增量输出的优化算法显然，通过增加对陀螺的采样次数N，可以提高旋转矢量的估算精度，但是，与此同时计算机的计算量和存储量也增加了。为了进一步提高计算精度，可采用利用前一圆锥补偿周期的角增量输出值进行补偿的圆锥补偿方法，补偿公式为NNiiipiiNiNiibaθθθθ×⎥⎦⎤⎢⎣⎡+′+=Φ∑∑∑−==+−=11111ˆ式中，、是加权系数，iaibp是要利用的前一圆锥补偿周期的角增量的个数，1+−′iNθ是前一圆锥补偿周期的陀螺输出角增量。（2）利用前一周期角增量累加和的优化算法若是考虑利用前一圆锥补偿周期总的角度变化，即前一周期的角增量累加和，则有如下形式的圆锥补偿算法公式θθθθθ×′+×⎥⎦⎤⎢⎣⎡+=Φ∑∑−==GbNNiiiNii111ˆ式中，θ′为前一周期角增量累加和，G为相应的加系数，∑==Nii1θθ。（3）改进旋转矢量优化算法的一般形式推广到更一般的情形，综合考虑前两种改进方法，可得到更通用的圆锥补偿算法θθθθθθ×′+×⎥⎦⎤⎢⎣⎡+′+=Φ∑∑∑−==+−=GbaNNiiipiiNiNii11111ˆ上述各种形式的圆锥误差补偿算法的系数如下表所示。子样数G4a3a2a1a1b2b3b4b单子样1/122/3-1/3011/151/140-13/