[发明专利]一种位置姿态系统的滤波方法

权利要求书

1.一种位置姿态系统的滤波方法，其特征在于：包括下列步骤：

(1)建立位置姿态系统的高阶误差模型；

位置姿态系统的高阶误差模型包括系统状态方程和量测方程：

a)系统状态方程

X·=F·X+G·W]]>

其中，X=δLδλδhδVEδVNδVUφEφNφU▿bx▿by▿bz▿rx▿ry▿rz]]>▿mx▿my▿mzδKAxδKAyδKAzδAyzδAzyδAxzδAzxδAxyδAyxϵbxϵbyϵbzϵrxϵryϵrzϵmxϵmyϵmz]]>δKGxδKGyδKGzδGyzδGzyδGxzδGzxδGxyδGyx1×45T]]>为系统状态向量；δL、δλ、δh为纬度误差、经度误差、高度误差；δV_E、δV_N、δV_U为东向速度误差、北向速度误差、天向速度误差；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角；为加速度计三个测量轴上的随机常值偏置；为加速度计三个测量轴上的随机游走偏置；为加速度计三个测量轴上的一阶马尔科夫过程偏置；δK_Ax、δK_Ay、δK_Az为加速度计三个测量轴上的刻度因子误差；δA_zx、δA_yx，δA_zy、δA_xy，δA_yz、δA_xz为加速度计三个测量轴上的安装误差，每个加速度计测量轴的安装误差用两个参数表示，因此加速度计三个测量轴的安装误差共六个；ε_bx、ε_by、ε_bz为陀螺三个测量轴上的随机常值漂移；ε_rx、ε_ry、ε_rz为陀螺三个测量轴上的随机游走漂移；ε_mx、ε_my、ε_mz为陀螺三个测量轴上的一阶马尔科夫过程漂移；δK_Gx、δK_Gy、δK_Gz为陀螺三个测量轴上的刻度因子误差；δG_zx、δG_yx，δG_zy、δG_xy，δG_yz、δG_xz为陀螺三个测量轴上的安装误差，每个陀螺测量轴的安装误差用两个参数表示，因此陀螺三个测量轴的安装误差共六个；为系统状态向量的导数，F为系统状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量，具体表达式如下：

W=ωaxωayωazωgxωgyωgzωarxωaryωarzωamxωamyωamzωgrxωgryωgrzωgmxωgmyωgmz1×18T]]>

G=03×303×303×303×303×303×3Cbn03×303×303×303×303×303×3Cbn03×303×303×303×303×303×303×303×303×303×303×303×3I3×303×303×303×303×303×303×3I3×303×303×3012×3012×3012×3012×3012×3012×303×303×303×303×3I3×303×303×303×303×303×303×3I3×309×309×309×309×309×309×3,45×18]]>F=F13×903×303×303×903×303×303×9F23×9CbnCbnF33×903×303×303×9F43×903×303×303×9CbnCbnF53×903×903×303×303×903×303×303×903×903×3F63×303×903×303×303×9012×9012×3012×3012×9012×3012×3012×903×903×303×303×903×3F73×303×909×909×309×309×909×309×309×945×45,]]>

F1=00-VN(Rm+h)201Rm+h0000VEsecLtanLRn+h0-VEsecL(Rn+h)2secLRn+h000000000010003×9,]]>

F3=Cbn(1,1)fxCbn(1,2)fyCbn(1,3)fzCbn(1,1)fyCbn(1,2)fxCbn(1,1)fzCbn(1,3)fxCbn(1,2)fzCbn(1,3)fyCbn(2,1)fxCbn(2,2)fyCbn(2,3)fzCbn(2,1)fyCbn(2,2)fxCbn(2,1)fzCbn(2,3)fxCbn(2,2)fzCbn(2,3)fyCbn(3,1)fxCbn(3,2)fyCbn(3,3)fzCbn(3,1)fyCbn(3,2)fxCbn(3,1)fzCbn(3,3)fxCbn(3,2)fzCbn(3,3)fy3×9,]]>

F5=Cbn(1,1)ωxCbn(1,2)ωyCbn(1,3)ωzCbn(1,1)ωyCbn(1,2)ωxCbn(1,1)ωzCbn(1,3)ωxCbn(1,2)ωzCbn(1,3)ωyCbn(2,1)ωxCbn(2,2)ωyCbn(2,3)ωzCbn(2,1)ωyCbn(2,2)ωxCbn(2,1)ωzCbn(2,3)ωxCbn(2,2)ωzCbn(2,3)ωyCbn(3,1)ωxCbn(3,2)ωyCbn(3,3)ωzCbn(3,1)ωyCbn(3,2)ωxCbn(3,1)ωzCbn(3,3)ωxCbn(3,2)ωzCbn(3,3)ωy3×9,]]>

F6=diag(-1α,-1α,-1α),]]>F7=diag(-1β,-1β,-1β);]]>ω_ax、ω_ay、ω_az为加速度计三个测量轴上的白噪声偏置；ω_gx、ω_gy、ω_gz为陀螺三个测量轴上的白噪声漂移；ω_arx、ω_ary、ω_arz为加速度计三个测量轴上的随机游走偏置驱动白噪声；ω_amx、ω_amy、ω_amz为加速度计三个测量轴上的一阶马尔科夫过程偏置驱动白噪声；ω_grx、ω_gry、ω_grz为陀螺三个测量轴上的随机游走漂移驱动白噪声；ω_gmx、ω_gmy、ω_gmz为陀螺三个测量轴上的一阶马尔科夫过程漂移驱动白噪声；为载体坐标系到地理系的转换矩阵，I为单位阵，V_E、V_N、V_U为运动载体速度在地理系下的分量，R_n与R_m分别为卯酉圈与子午圈的主曲率半径，h为运动载体的高度，L为运动载体的纬度，ω_ie为地球自转角速度，f_E、f_N、f_U为运动载体加速度在地理坐标系下的分量，f_x、f_y、f_z为运动载体加速度在载体坐标系下的分量，ω_x、ω_y、ω_z为运动载体角速度在载体坐标系下的分量，α与β为陀螺和加速度计一阶马尔科夫过程的相关时间；

b)量测方程

Z＝H·X+v

其中，Z为量测向量，H为量测矩阵，X为系统状态向量，v为测量噪声向量，具体表达式如下：

Z=LSINS-LGNSSλSINS-λGNSShSINS-hGNSSVESINS-VEGNSSVNSINS-VNGNSSVUSINS-VUGNSS6×1,]]>v=vLvλvhvvevvnvvu6×1,]]>H=Rm0000001×390RncosL000001×3900100001×3900010001×3900001001×3900000101×396×45;]]>

L_SINS-L_GNSS，λ_SINS-λ_GNSS，h_SINS-h_GNSS，V_ESINS-V_EGNSS，V_NSINS-V_NGNSS，V_USINS-V_UGNSS，为SINS与GNSS输出的纬度、经度、高度、东向速度、北向速度、天向速度之差；v_L，v_λ，v_h，v_ve，v_vn，v_vu为GNSS的纬度、经度、高度、东向速度、北向速度、天向速度的测量噪声，均为零均值随机白噪声，其方差阵为R；R_n与R_m分别为卯酉圈与子午圈的主曲率半径，L为当地纬度；

(2)设计基于三参数递推的卡尔曼滤波器，其设计步骤为：

a)将系统状态方程离散化，为：

Xk=Fk/k-1Xk-1+Wk-1Fk/k-1=I+TF(tk-1)+T22!F2(tk-1)+T33!F3(tk-1)+···]]>

其中，下标k-1、k分别表示t_k-1和t_k时刻，X_k、X_k-1为t_k、t_k-1时刻的系统状态向量，W_k-1为t_k-1时刻系统噪声向量阵，F_k/k-1为t_k-1时刻到t_k时刻的系统状态一步转移阵，I为单位阵，T＝t_k-t_k-1为滤波周期，F(t_k-1)为常数矩阵；

b)在离散化后的系统状态方程和量测方程引入基于三参数递推的滤波模型：

Xk=C1Fk/k-1C2Fk/k-2C3Fk/k-3Xk-1Xk-2Xk-3+Wk-1,]]>ZkZk-1Zk-2=HXkXk-1Xk-2+vkvk-1vk-2]]>

其中，下标k、k-1、k-2分别表示t_k、t_k-1、t_k-2时刻，X_k、X_k-1、X_k-2、X_k-3为t_k、t_k-1、t_k-2、t_k-3时刻的系统状态向量，F_k/k-1为k-1时刻到k时刻的系统状态一步转移阵，F_k/k-2为k-2时刻到k时刻的系统状态转移阵，F_k/k-3为k-3时刻到k时刻的系统状态转移阵，C₁、C₂、C₃为状态转移阵F_k/k-1、F_k/k-2、F_k/k-3的加权系数，W_k-1为t_k-1时刻系统噪声向量阵，Z_k、Z_k-1、Z_k-2为t_k、t_k-1、t_k-2时刻的量测向量，H为量测矩阵，v_k、v_k-1、v_k-2为t_k、t_k-1、t_k-2时刻的测量噪声向量；

(3)基于步骤(1)中建立的位置姿态系统的高阶误差模型，取SINS与GNSS的定位结果之差作为观测量，采用SINS/GNSS分布式组合形式，通过步骤(2)设计的基于三参数递推的卡尔曼滤波器对各误差状态进行最优滤波估计；

(4)将步骤(3)中计算得到的位置误差、速度误差、姿态误差和惯性器件误差的最优滤波估计值反馈回SINS，并对陀螺漂移、加速度计偏置以及载体坐标系相对于计算坐标系的转换矩阵进行校正，计算出更加精确的位置、速度和姿态信息；

修正SINS的位置、速度、姿态的公式为：

a)位置修正

Lat′=Lat-δ^L]]>

Lon′=Lon-δ^λ]]>

Height′=Height-δ^h]]>

其中，Lat′、Lon′、Height′为修正后的纬度、经度、高度信息；Lat、Lon、Height为SINS输出的纬度、经度、高度信息；为纬度误差、经度误差、高度误差的最优滤波估计值；

b)速度修正

Ve′=Ve-δ^Ve]]>

Vn′=Vn-δ^Vn]]>

Vu′=Vu-δ^Vu]]>

其中，Ve′、Vn′、Vu′为修正后的东向速度、北向速度、天向速度信息；Ve、Vn、Vu为SINS输出的东向速度、北向速度、天向速度信息；为东向速度误差、北向速度误差、天向速度误差的最优滤波估计值；

c)姿态修正

Cnc=cosφU×cosφN-sinφU×sinφE×sinφNsinφU×cosφN+cosφU×sinφE×sinφN-cosφE×sinφN-sinφU×cosφEcosφU×cosφEsinφEcosφU×sinφN+sinφU×sinφE×cosφNsinφU×sinφN-cosφU×sinφE×cosφNcosφE×cosφN]]>

其中，为地理系n到计算系c的方向余弦矩阵，即姿态误差矩阵；φ_E、φ_N、φ_U为东向水平姿态失准角、北向水平姿态失准角、航向失准角的最优滤波估计值：

Cnb=Ccb×Cnc]]>

其中，为地理系n到载体系b的方向余弦矩阵，即修正后的姿态矩阵；为计算系c到载体系b的方向余弦矩阵，即SINS输出的姿态矩阵；根据即可计算出载体的姿态角，包括航向角、俯仰角、横滚角。