[发明专利]一种机载分布式POS的实时传递对准的方法及装置

权利要求书

1.一种机载分布式POS的实时传递对准的方法，其特征在于，包括：

建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；所述非线性状态方程由与姿态状态变量有关的微分方程组成，所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成；

所述建立大失准角条件下的机载分布式POS传递对准的误差模型，包括：建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型；

所述姿态误差微分方程为：

其中，为子系统姿态失准角，φ_E、φ_N和φ_U分别为东向、北向、天向失准角，为子系统真实导航坐标系相对惯性坐标系的角速度；为的误差角速度；为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵；I_3×3为3行、3列的单位矩阵，ε^b为子系统陀螺仪误差，ε^b＝ε^c+w_ε，其中，ε^c为子系统陀螺仪常值漂移，w_ε为子系统陀螺仪随机误差，和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移，和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差；为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵；其中，

所述速度误差微分方程为：

其中，Vⁿ＝[V_E V_N V_U]^T和δVⁿ＝[δV_E δV_N δV_U]^T分别为子系统速度和速度误差，其中V_E、V_N和V_U分别为东向、北向和天向速度，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差；f^b＝[f_xf_y f_z]^T是子系统的比力，其中f_x、f_y和f_z分别为载体坐标系x方向、y方向和z方向比力；和分别为地球坐标系相对惯性坐标系的角速度ω_ie及其误差δω_ie在子系统真实导航坐标系下的表示；和分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差；为子系统加速度计误差，其中，为子系统加速度计常值偏置，为系统加速度计随机误差，和分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置，和分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差；

所述位置误差微分方程为：

其中，L、λ、h分别为子系统的纬度、经度、高度，δL、δλ、δh分别为纬度误差、经度误差、高度误差；为纬度的一阶导数，为经度的一阶导数，为高度的一阶导数；R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径；

所述惯性仪表误差微分方程为：

其中，ε^c为子系统陀螺仪常值漂移，为子系统加速度计常值偏置；

所述建立主系统和子系统间的角误差模型，包括：

建立子系统固定安装误差角ρ的微分方程：

其中，ρ＝[ρ_x ρ_y ρ_z]^T为子系统固定安装误差角，ρ_x、ρ_y和ρ_z分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角；

建立子系统弹性变形角θ的微分方程：

其中，θ_j为子系统载体坐标系第j轴上的弹性变形角，j＝x,y,z，θ＝[θ_x θ_y θ_z]^T为弹性变形角；β_j＝2.146/τ_j，τ_j为二阶马尔科夫过程相关时间；η_j为零均值白噪声，其方差满足：

其中，σ_j²为弹性变形角θ_j的方差，β_j和为描述弹性变形角θ的二阶马尔科夫过程的参数；

利用UKF对t_k-1时刻所述非线性状态方程中的非线性状态变量x^NL进行时间更新，并利用KF对t_k-1时刻所述线性状态方程中的线性状态变量x^L进行时间更新；

所述非线性状态变量x^NL和线性状态变量x^L分别定义为：

x^NL＝[x¹]^T

x^L＝[x² x³]^T

其中，

x¹＝[φ_E φ_N φ_U],

所述非线性状态方程为：

所述线性状态方程为：

其中，和分别为t_k时刻的非线性状态变量和线性状态变量，和分别为t_k-1时刻的非线性状态变量和线性状态变量，非线性状态方程的系统噪声为线性状态方程的系统噪声为其中分别为子系统载体坐标系x轴、y轴、z轴陀螺仪的随机误差，分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差；非线性状态方程的系统噪声w^NL和线性状态方程的w^L均为零均值高斯白噪声，w^NL的方差阵Q^NL由陀螺仪随机漂移确定，w^L的方差阵Q^L由加速度计随机偏置和二阶马尔科夫过程参数确定；系统状态方程中各系数矩阵的表达式如下：

所述建立大失准角条件下的机载分布式POS传递对准的数学模型包括：建立大失准角条件下的机载分布式POS传递对准的系统量测模型；

所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型，包括：

系统量测量定义为：

z＝[δψ δθ δγ δV′_E δV′_N δV′_U δL′ δλ′ δh′]^T

建立系统量测方程：

其中，z_k为t_k时刻的量测量，δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差，δV′_E、δV′_N、δV′_U分别为子系统与主系统东向、北向、天向速度之差，δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差；量测噪声其中v_δψ、v_δθ、v_δγ分别为主系统航向角、俯仰角、横滚角的量测噪声，分别为主系统东向、北向、天向速度的量测噪声，v_δL、v_δλ、v_δh分别为主系统纬度、经度、高度的量测噪声；v为量测噪声，取为零均值高斯白噪声，v_k为v在t_k时刻的值；

所述利用UKF对t_k-1时刻所述非线性状态方程中的非线性状态变量进行时间更新，包括：

计算t_k-1时刻的2n_NL+1个样本点其中，n_NL为非线性状态变量的维数：

其中，10^-4≤α≤1，κ＝3-n_NL，和分别为t_k-1时刻非线性状态变量的估计值和估计协方差矩阵；表示矩阵的下三角分解平方根的第i列；

利用UKF计算t_k时刻非线性状态变量的一步预测值

其中，为的一步预测模型值，为补偿项；W_i为权值；

所述利用KF对t_k-1时刻所述线性状态方程中的线性状态变量进行时间更新，包括：

其中，为补偿项；

将所述KF更新后的t_k-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到t_k时刻的非线性状态变量估计值将所述作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到t_k时刻的线性状态变量估计值

所述将所述KF更新后的t_k-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到t_k时刻的非线性状态变量估计值包括：

根据所述非线性状态变量一步预测值计算非线性状态变量的一步预测样本点

其中，为的误差，为协方差矩阵为的零均值高斯白噪声，为t_k时刻量测变量一步预测值的自协方差矩阵，为t_k时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵，和的计算过程如下：

其中，为补偿项；

计算非线性状态变量的增益矩阵

计算滤波估计值及其协方差矩阵：

将所述UKF更新后的t_k-1时刻的非线性状态作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到t_k时刻的线性状态变量估计值包括：

其中，为线性状态变量的增益矩阵，Y′为的雅可比矩阵，为补偿项；I_21×21为21行21列的单位矩阵；

根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的t_k时刻的子系统的线性状态变量值和非线性状态变量值；

所述根据所述线性状态变量估计值对所述子系统的捷联解算结果进行修正，得到修正后的t_k时刻的子系统的线性状态变量值，包括：

其中，和分别为子IMU修正后的东向、北向和天向速度；和分别为子系统捷联解算得到的东向、北向和天向速度；δV_E、δV_N和δV_U分别为t_k时刻KF估计出的子系统捷联解算东向、北向和天向速度误差；

L^new＝L^old-δL

λ^new＝λ^old-δλ

h^new＝h^old-δh

其中，L^old、λ^old和h^old分别为子IMU捷联解算得到的纬度、经度和高度；L^new、λ^new和h^new分别为子IMU修正后的纬度、经度和高度；δL、δλ和δh分别为t_k时刻KF估计出的子IMU捷联解算纬度、经度和高度误差；

所述根据所述非线性状态变量估计值对所述子系统的捷联解算结果进行修正，得到修正后的t_k时刻的子系统的非线性状态变量值，包括：

计算t_k时刻子系统真实导航坐标系n与计算导航坐标系n₁间的方向余弦矩阵

计算t_k时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵

其中为t_k时刻子系统捷联解算得到的方向余弦矩阵；

由被更新后的计算t_k时刻子系统的航向角ψ_s、俯仰角θ_s和横滚角γ_s，将记为：

其中，T_lm为矩阵中第l行、第m列的元素，则子IMU航向角ψ_s、俯仰角θ_s和横滚角γ_s的主值，即ψ_s主、θ_s主和γ_s主分别为：

θ_s主＝arcsin(T₃₂)

θ_s＝θ_s主，

2.一种机载分布式POS的实时传递对准的装置，其特征在于，包括：

建立模块，用于建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型；所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型；所述数学模型包括系统状态方程和系统量测方程；其中，所述系统状态方程包括线性状态方程和非线性状态方程；所述非线性状态方程由与姿态状态变量有关的微分方程组成，所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成；

所述建立模块具体用于建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型，所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程，所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型；

其中，所述姿态误差微分方程为：

其中，为子系统姿态失准角，φ_E、φ_N和φ_U分别为东向、北向、天向失准角，为子系统真实导航坐标系相对惯性坐标系的角速度；为的误差角速度；为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵；I_3×3为3行、3列的单位矩阵，ε^b为子系统陀螺仪误差，ε^b＝ε^c+w_ε，其中，ε^c为子系统陀螺仪常值漂移，w_ε为子系统陀螺仪随机误差，和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移，和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差；为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵；其中，

所述速度误差微分方程为：

其中，Vⁿ＝[V_E V_N V_U]^T和δVⁿ＝[δV_E δV_N δV_U]^T分别为子系统速度和速度误差，其中V_E、V_N和V_U分别为东向、北向和天向速度，δV_E、δV_N和δV_U分别为东向、北向和天向速度误差；f^b＝[f_xf_y f_z]^T是子系统的比力，其中f_x、f_y和f_z分别为载体坐标系x方向、y方向和z方向比力；和分别为地球坐标系相对惯性坐标系的角速度ω_ie及其误差δω_ie在子系统真实导航坐标系下的表示；和分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差；为子系统加速度计误差，其中，为子系统加速度计常值偏置，为系统加速度计随机误差，和分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置，和分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差；

所述位置误差微分方程为：

其中，L、λ、h分别为子系统的纬度、经度、高度，δL、δλ、δh分别为纬度误差、经度误差、高度误差；为纬度的一阶导数，为经度的一阶导数，为高度的一阶导数；R_M和R_N分别为沿子午圈和卯酉圈的主曲率半径；

所述惯性仪表误差微分方程为：

其中，ε^c为子系统陀螺仪常值漂移，为子系统加速度计常值偏置；

所述建立主系统和子系统间的角误差模型，包括：

建立子系统固定安装误差角ρ的微分方程：

其中，ρ＝[ρ_x ρ_y ρ_z]^T为子系统固定安装误差角，ρ_x、ρ_y和ρ_z分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角；

建立子系统弹性变形角θ的微分方程：

其中，θ_j为子系统载体坐标系第j轴上的弹性变形角，j＝x,y,z，θ＝[θ_x θ_y θ_z]^T为弹性变形角；β_j＝2.146/τ_j，τ_j为二阶马尔科夫过程相关时间；η_j为零均值白噪声，其方差满足：

其中，σ_j²为弹性变形角θ_j的方差，β_j和为描述弹性变形角θ的二阶马尔科夫过程的参数；

第一更新模块，用于利用UKF对t_k-1时刻所述非线性状态方程中的非线性状态变量x^NL进行时间更新，并利用KF对t_k-1时刻所述线性状态方程中的线性状态变量x^L进行时间更新；

所述非线性状态变量x^NL和线性状态变量x^L分别定义为：

x^NL＝[x¹]^T

x^L＝[x² x³]^T

其中，

x¹＝[φ_E φ_N φ_U],

所述非线性状态方程为：

所述线性状态方程为：

其中，和分别为t_k时刻的非线性状态变量和线性状态变量，和分别为t_k-1时刻的非线性状态变量和线性状态变量，非线性状态方程的系统噪声为线性状态方程的系统噪声为其中分别为子系统载体坐标系x轴、y轴、z轴陀螺仪的随机误差，分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差；非线性状态方程的系统噪声w^NL和线性状态方程的w^L均为零均值高斯白噪声，w^NL的方差阵Q^NL由陀螺仪随机漂移确定，w^L的方差阵Q^L由加速度计随机偏置和二阶马尔科夫过程参数确定；系统状态方程中各系数矩阵的表达式如下：

所述建立模块具体用于建立大失准角条件下的机载分布式POS传递对准的系统量测模型；

所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型，包括：

系统量测量定义为：

z＝[δψ δθ δγ δV′_E δV′_N δV′_U δL′ δλ′ δh′]^T

建立系统量测方程：

其中，z_k为t_k时刻的量测量，δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差，δV′_E、δV′_N、δV′_U分别为子系统与主系统东向、北向、天向速度之差，δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差；量测噪声其中v_δψ、v_δθ、v_δγ分别为主系统航向角、俯仰角、横滚角的量测噪声，分别为主系统东向、北向、天向速度的量测噪声，v_δL、v_δλ、v_δh分别为主系统纬度、经度、高度的量测噪声；v为量测噪声，取为零均值高斯白噪声，v_k为v在t_k时刻的值；

所述第一更新模块具体用于，计算t_k-1时刻的2n_NL+1个样本点其中，n_NL为非线性状态变量的维数：

其中，10^-4≤α≤1，κ＝3-n_NL，和分别为t_k-1时刻非线性状态变量的估计值和估计协方差矩阵；表示矩阵的下三角分解平方根的第i列；

利用UKF计算t_k时刻非线性状态变量的一步预测值

其中，为的一步预测模型值，为补偿项；W_i为权值；

所述第一更新模块还具体用于根据以下公式利用KF对t_k-1时刻所述线性状态方程中的线性状态变量进行时间更新：

其中，为补偿项；

第二更新模块，用于将所述KF更新后的t_k-1时刻的线性状态作为所述非线性状态方程的参数，并利用UKF对所述非线性状态方程进行量测更新，得到t_k时刻的非线性状态变量估计值以及将所述作为所述线性状态方程的参数，并利用KF对所述线性状态变量方程进行量测更新，得到t_k时刻的线性状态变量估计值

所述第二更新模块具体用于，根据所述非线性状态变量一步预测值计算非线性状态变量的一步预测样本点

其中，为的误差，为协方差矩阵为的零均值高斯白噪声，为t_k时刻量测变量一步预测值的自协方差矩阵，为t_k时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵，和的计算过程如下：

其中，为补偿项；

计算非线性状态变量的增益矩阵

计算滤波估计值及其协方差矩阵：

以及，所述第二更新模块具体用于根据以下获得时刻的t_k时刻的非线性状态变量估计值

其中，为线性状态变量的增益矩阵，Y′为的雅可比矩阵，为补偿项；I_21×21为21行21列的单位矩阵；

修正模块，用于根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正，分别得到修正后的t_k时刻的子系统的线性状态变量值和非线性状态变量值；

所述修正模块具体用于，根据以下公式对所述子系统的捷联解算结果进行修正得到修正后t_k时刻的子系统的线性状态变量值：

其中，和分别为子IMU修正后的东向、北向和天向速度；和分别为子系统捷联解算得到的东向、北向和天向速度；δV_E、δV_N和δV_U分别为t_k时刻KF估计出的子系统捷联解算东向、北向和天向速度误差；

L^new＝L^old-δL

λ^new＝λ^old-δλ

h^new＝h^old-δh

其中，L^old、λ^old和h^old分别为子IMU捷联解算得到的纬度、经度和高度；L^new、λ^new和h^new分别为子IMU修正后的纬度、经度和高度；δL、δλ和δh分别为t_k时刻KF估计出的子IMU捷联解算纬度、经度和高度误差；

所述修正模块还具体用于，根据以下对所述子系统的捷联解算结果进行修正得到修正后t_k时刻的子系统的非线性状态变量值：

计算t_k时刻子系统真实导航坐标系n与计算导航坐标系n₁间的方向余弦矩阵

计算t_k时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵

其中为t_k时刻子系统捷联解算得到的方向余弦矩阵；

由被更新后的计算t_k时刻子系统的航向角ψ_s、俯仰角θ_s和横滚角γ_s，将记为：

其中，T_lm为矩阵中第l行、第m列的元素，则子IMU航向角ψ_s、俯仰角θ_s和横滚角γ_s的主值，即ψ_s主、θ_s主和γ_s主分别为：

θ_s主＝arcsin(T₃₂)

θ_s＝θ_s主，