[发明专利]基于螺旋矢量的捷联姿态位置一体化更新算法

权利要求书

1.一种基于螺旋矢量的捷联姿态位置一体化更新算法，其特征在于：该算法是采用如下步骤实现的：

1)假设刚体相对惯性参考坐标系进行螺旋运动，刚体的螺旋矢量更新周期为H，陀螺仪的采样周期和加速度计的采样周期均为h，陀螺仪的采样子样数和加速度计的采样子样数均为N，则H＝Nh；设定N＝3，则H＝3h；通过陀螺仪实时测出刚体相对惯性参考坐标系的角速度；通过加速度计实时测出刚体相对惯性参考坐标系的比力；根据刚体相对惯性参考坐标系的角速度和刚体相对惯性参考坐标系的比力，求解出在时间[t_k,t_k+H]内刚体相对惯性参考坐标系的螺旋矢量增量；求解公式如下：

ΔΦ^=α^1+α^2+α^3+920(α^1×α^3)+2740α^2×(α^3-α^1)---(1);]]>

ΔΦ^=|ΔΦ^|---(2);]]>

α^i=αi+ϵαi′---(3);]]>

αi=∫tk+(i-1)htk+ihw(τ)dτ---(4);]]>

αi′=∫tk+(i-1)htk+ihf(τ)dτ---(5);]]>

式(1)-(5)中：i＝1,2,3；为在时间[t_k,t_k+H]内刚体相对惯性参考坐标系的螺旋矢量增量；为在时间[t_k,t_k+H]内刚体相对惯性参考坐标系的螺旋矢量增量的模；为在时间[t_k,t_k+h]内刚体相对惯性参考坐标系的旋量增量；为在时间[t_k+h,t_k+2h]内刚体相对惯性参考坐标系的旋量增量；为在时间[t_k+2h,t_k+H]内刚体相对惯性参考坐标系的旋量增量；α₁为在时间[t_k,t_k+h]内刚体相对惯性参考坐标系的角速度增量；α₂为在时间[t_k+h,t_k+2h]内刚体相对惯性参考坐标系的角速度增量；α₃为在时间[t_k+2h,t_k+H]内刚体相对惯性参考坐标系的角速度增量；α₁′为在时间[t_k,t_k+h]内刚体相对惯性参考坐标系的比力增量；α₂′为在时间[t_k+h,t_k+2h]内刚体相对惯性参考坐标系的比力增量；α₃′为在时间[t_k+2h,t_k+H]内刚体相对惯性参考坐标系的比力增量；w(τ)为刚体相对惯性参考坐标系的角速度；f(τ)为刚体相对惯性参考坐标系的比力；ε为对偶标记；

2)根据刚体相对惯性参考坐标系的螺旋矢量增量，求解出在时间[t_k,t_k+H]内刚体的更新对偶四元数；求解公式如下：

q^(H)=cosΔΦ^2ΔΦ^ΔΦ^sinΔΦ^2---(6);]]>

式(6)中：为在时间[t_k,t_k+H]内刚体的更新对偶四元数；为在时间[t_k,t_k+H]内刚体相对惯性参考坐标系的螺旋矢量增量；为在时间[t_k,t_k+H]内刚体相对惯性参考坐标系的螺旋矢量增量的模；

3)根据刚体的更新对偶四元数，实时更新刚体的姿态位置对偶四元数；更新公式如下：

式(7)中：为在t_k+H时刻刚体的姿态位置对偶四元数；为在t_k时刻刚体的姿态位置对偶四元数；为在时间[t_k,t_k+H]内刚体的更新对偶四元数；

4)根据实时更新的刚体的姿态位置对偶四元数，求解出在t_k+H时刻刚体相对惯性参考坐标系的平移矢量；求解公式如下：

tⁱ(t_k+H)＝2Q′(t_k+H)οQ^*(t_k+H)    (8)；

Q^(tk+H)=Q(tk+H)+ϵQ′(tk+H)---(9);]]>

式(8)-(9)中：tⁱ(t_k+H)为在t_k+H时刻刚体相对惯性参考坐标系的平移矢量；为在t_k+H时刻刚体的姿态位置对偶四元数；Q(t_k+H)为在t_k+H时刻刚体的姿态位置对偶四元数的实部；Q′(t_k+H)为在t_k+H时刻刚体的姿态位置对偶四元数的对偶部；Q^*(t_k+H)为在t_k+H时刻刚体的姿态位置对偶四元数实部的共轭；ε为对偶标记。