[发明专利]一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法

权利要求书

1.一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，其特征在于：该方法具体步骤如下：

步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；

步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；

步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；取游动自由方位系为子惯导的导航坐标系，所采用子惯导误差模型为：

δv·x=2Ωzδvy-(ρy+2Ωy)δvz-fzψy+fyψz+▿xδv·y=-2Ωzδvx+(ρx+2Ωx)δvz+fzψx-fxψz+▿yδv·z=(ωy+Ωy)δvx-(ωx+Ωx)δvy-fyψx+fxψy+▿zψ·x=Ωzψy-ωyψz-ϵxψ·y=-Ωzψy+ωxψz-ϵyψ·z=ωyψx-ωxψy-ϵzη·x=-1τηxη·y=-1τηyη·z=-1τηzκ·=-1τκ]]>

其中δv_x、δv_y、δv_z为速度误差，ρ_x、ρ_y、ρ_z为载体运动角速率矢量分量，Ω_x、Ω_y、Ω_z为地球自转角速率矢量分量,ω表示ρ+Ω；▽_x、▽_y、▽_z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；ψ_x、ψ_y、ψ_z为姿态误差，ε_x、ε_y、ε_z为陀螺漂移；η_x、η_y、η_z为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τ_ηx、τ_ηy、τ_ηz分别为三轴向相对安装角误差的相关时间；κ为主惯导相对子惯导的延迟时间误差，τ_κ为延迟时间误差的相关时间；

子惯导器件误差模型：

陀螺的误差模型为

ϵxϵyϵzn=Cbnϵxϵyϵzb=C11C12C13C21C22C23C31C32C33(BxByBz+Sgx000Sgy000Sgzωibxωibyωibz)]]>

B_x、B_y、B_z为陀螺的零位误差项；S_gx、S_gy、S_gz为陀螺的标度误差项，C_ij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素；

加速度计的误差模型为

▿x▿y▿zn=Cbn▿x▿y▿zb=C11C12C13C21C22C23C31C32C33(AxAyAz+Sax000Sax000Saxfbxfbyfbz)]]>

其中A_x、A_y、A_z为加速度计的零位误差项；S_ax、S_ay、S_az为加速度计的标度误差项；

所选系统状态变量为：

X(t)＝[δv_x δv_y δv_z ψ_x ψ_y ψ_z η_x η_y η_z κ B_x B_y B_z S_gx S_gy S_gz A_x A_y A_z S_ax S_ay S_az]^T

对应的系统状态方程为：

X·=FX+GW]]>

其中

F=F11F1203×303×103×303×3F13F1403×3F2103×303×1F22F2303×303×3016×22]]>

G=I3×303×303×303×103×1203×3I3×303×303×103×1203×303×3I3×303×103×1201×301×301×3I1×101×12012×22]]>

W=waxwaywazwϵxwϵywϵz-1τηx-1τηx-1τηx-1τκ01×12T]]>

w_ax,w_ay,w_az为加速度计零偏随机白噪声，w_εx,w_εy,w_εz为陀螺仪漂移随机白噪声；

式中

F11=02Ωz-(ρy+2Ωy)-2Ωz0ρx+2Ωxρy+2Ωy-(ρx+2Ωx)0F12=0-fzfyfz0-fx-fyfx0]]>

F13=Cbn=C11C12C13C21C22C23C31C32C33F14=fx000fy000fz]]>

F23=ωibxn000ωibyn000ωibzn]]>

系统观测量：

Z＝[δv_x δv_y δv_z M₂₃ -M₁₃ M₁₂]^T

其中子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差

δV=Vst-Vmt-κ≈δv+κ·f]]>

子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积

M=(C‾bn)st·(Cb1n)m′t-κ≈[I-((ψ+Cb1nη+ωbnn·κ)×)]]]>

式中(ψ×)=0-ψzψyψz0-ψx-ψyψx0,]]>其他类推；

观测方程为

Z＝HX+V

其中

H=I3×303×303×3f03×1203×3I3×3Cb1nωbnn03×12]]>

时间延迟量测值计算方法

考虑到闭环卡尔曼滤波能减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进，设子惯导t时刻时间延迟估计值为相对安装角误差估计值为

1)速度量测值

2)姿态量测值

步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。