姿态更新算法优化方案

第2章分析了姿态解算的各种方法,并给出了其微分方程,本章将对其微分方程进行计算,从而完成其姿态更新算法的推导,本节的参考坐标系(R系)为当地地理坐标系(n系)。

6.3.1.1　方向余弦法

式(2.25)推导了方向余弦矩阵的微分方程,即

方程的解析解为

令Cnb＝C,＝Ω,从时间tk到tk＋1,经过一次计算循环,方程的解可以写成如下形式：

式中,Ck——第k次计算循环载体坐标系相对参考轴系的方向余弦矩阵。

如果旋转角速度矢量ω的方向在整个更新时间间隔内,且空间方位保持不变,则有

式中,σ——一个具有方向与量值的角矢量。它表示载体坐标系绕旋转量值等于σ的角度后,把载体坐标系的方向由计算循环k处转到计算循环k＋1处的位置。σ 由分量σx、σy、σz表示,它的量值为

σ×——σ的反对称矩阵,

因此,式(6.34)可写为

式中,Ak——把一个矢量从第k＋1次计算循环的载体坐标转换到第k次计算循环载体坐标的方向余弦矩阵。

因此,式(6.38)中的指数项展开为

由式(6.37),可知

因此,

式(6.41)还可以写为

由式(6.41),Ak可以用下式来计算：

式中,

对姿态算法进行精度评估：当使用一阶算法时,a1＝1,a2＝0；当使用二阶算法时,a1＝

1,a2＝0.5；当使用三阶算法时

6.3.1.2　四元数法

式(2.41)推导了姿态变换四元数的微分方程,即

其中,

式(6.44)的解为

令

式中,Δθx,Δθy,Δθz——x,y,z陀螺仪在[tk,tk＋1]采样时间间隔内的角增量。(www.xing528.com)

令(Δθ)2＝(Δθx)2＋(Δθy)2＋(Δθz)2,P＝0＋Δθxi＋Δθyj＋Δθzk,则有

即

令

对姿态算法进行精度评估：当使用一阶算法时,ac＝1,as＝0.5；当使用二阶算法时,当使用三阶算法时

接下来,进行四元数初值确定与规范化处理。

由方向余弦与四元数关系,可知

所以,q0、q1、q2、q3的符号可按下式确定：

6.3.1.3　等效旋转矢量法

式(2.104)推导了等效旋转矢量微分方程,即

从式(6.56)中可以看出,旋转矢量的导数除了角增量外还包括两个修正项,修正项反映了不可交换性误差的影响,一般可省略第3项,并通过采样角增量来求解等效旋转矢量。

假设在一个角速率周期T内,认为ω是随时间输出呈线性变化,即

式中,T——采样周期。

系统在一个周期内采样两次,得到两个角增量,即

由上式可以推导出等效旋转矢量双子样算法为

同理,假设在一个角速率周期内,认为ω是随时间变化的输出为二次曲线,即

则可推导出等效旋转矢量三子样算法为

因此可知,

单子样算法：φ(T)＝Δθ；

双子样算法：

三子样算法：

6.3.1.4　四阶龙格-库塔法

对于四元数微分方程可采用标准四阶龙格-库塔法进行求解,结果为

式中,Q(t0)由初始姿态角确定；角速率为陀螺仪数据；T为采样步长。

对采用以上几种算法进行的姿态更新进行比较：方向余弦法在解算微分方程时,需解算9个联立微分方程,计算量较大；四元数法只需解算4个联立微分方程,虽计算量小,但会引入不可交换性误差,特别是在载体作高动态运动时,这种误差不可忽视；而旋转矢量算法正好能弥补这一缺点,其在高动态环境下能够有效减小不可交换性误差,且在同样采样频率的条件下,其比龙格-库塔法更能适应高动态的恶劣环境。