一种机载分布式POS的实时传递对准的方法及装置转让专利
申请号 : CN201810153926.2
文献号 : CN108303120B
文献日 : 2020-03-24
发明人 : 宫晓琳 , 刘刚 , 陈隆君 , 房建成
申请人 : 北京航空航天大学
摘要 :
权利要求 :
1.一种机载分布式POS的实时传递对准的方法,其特征在于,包括:建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型;所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型;所述数学模型包括系统状态方程和系统量测方程;其中,所述系统状态方程包括线性状态方程和非线性状态方程;
所述非线性状态方程由与姿态状态变量有关的微分方程组成,所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成;
所述建立大失准角条件下的机载分布式POS传递对准的误差模型,包括:建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型,所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程,所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型;
所述姿态误差微分方程为:
其中, 为子系统姿态失准角,φE、φN和φU分别为东向、北向、天向失准角, 为子系统真实导航坐标系相对惯性坐标系的角速度; 为 的误差角速度; 为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵;I3×3为3行、3列的单位矩阵,εb为子系统陀螺仪误差,εb=εc+wε,其中,εc为子系统陀螺仪常值漂移,wε为子系统陀螺仪随机误差, 和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移, 和 分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差; 为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵;其中,
所述速度误差微分方程为:
其中,Vn=[VE VN VU]T和δVn=[δVE δVN δVU]T分别为子系统速度和速度误差,其中VE、VN和VU分别为东向、北向和天向速度,δVE、δVN和δVU分别为东向、北向和天向速度误差;fb=[fx fy fz]T是子系统的比力,其中fx、fy和fz分别为载体坐标系x方向、y方向和z方向比力; 和分别为地球坐标系相对惯性坐标系的角速度ωie及其误差δωie在子系统真实导航坐标系下的表示; 和 分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差; 为子系统加速度计误差, 其中, 为子系统加速度计常值偏置,为系统加速度计随机误差,
和 分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置,和 分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差;
所述位置误差微分方程为:
其中,L、λ、h分别为子系统的纬度、经度、高度,δL、δλ、δh分别为纬度误差、经度误差、高度误差 ; 为纬度的一阶导 数, 为经度的一阶导数 ,为高度的一阶导数;RM和RN分别为沿子午圈和卯酉圈的主曲率半径;
所述惯性仪表误差微分方程为:
其中,εc为子系统陀螺仪常值漂移, 为子系统加速度计常值偏置;
所述建立主系统和子系统间的角误差模型,包括:建立子系统固定安装误差角ρ的微分方程:T
其中,ρ=[ρx ρy ρz]为子系统固定安装误差角,ρx、ρy和ρz分别为子系统在载体坐标系x轴、y轴和z轴的安装误差角;
建立子系统弹性变形角θ的微分方程:
其中,θj为子系统载体坐标系第j轴上的弹性变形角,j=x,y,z,θ=[θx θy θz]T为弹性变形角;βj=2.146/τj,τj为二阶马尔科夫过程相关时间;ηj为零均值白噪声,其方差 满足:其中,σj2为弹性变形角θj的方差,βj和 为描述弹性变形角θ的二阶马尔科夫过程的参数;
利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新,并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新;
所述非线性状态变量xNL和线性状态变量xL分别定义为:xNL=[x1]T
xL=[x2 x3]T
其中,
1
x=[φE φN φU],
所述非线性状态方程为:
所述线性状态方程为:
其中, 和 分别为tk时刻的非线性状态变量和线性状态变量, 和 分别为tk-1时刻的非线性状态变量和线性状态变量,非线性状态方程的系统噪声为线性状态方程的系统噪声为
其中 分别为子系统载体坐标系
x轴、y轴、z轴陀螺仪的随机误差, 分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差;非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声,wNL的方差阵QNL由陀螺仪随机漂移确定,wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数 确定;系统状态方程中各系数矩阵的表达式如下:所述建立大失准角条件下的机载分布式POS传递对准的数学模型包括:建立大失准角条件下的机载分布式POS传递对准的系统量测模型;
所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型,包括:系统量测量定义为:
z=[δψ δθ δγ δV′E δV′N δV′U δL′ δλ′ δh′]T建立系统量测方程:
其中,zk为tk时刻的量测量,δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差,δV′E、δV′N、δV′U分别为子系统与主系统东向、北向、天向速度之差,δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差;量测噪声其中vδψ、vδθ、vδγ分别为主
系统航向角、俯仰角、横滚角的量测噪声, 分别为主系统东向、北向、天向速度的量测噪声,vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声;v为量测噪声,取为零均值高斯白噪声,vk为v在tk时刻的值;
所述利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量进行时间更新,包括:
计算tk-1时刻的2nNL+1个样本点 其中,nNL为非线性状态变量的维数:其中, 10-4≤α≤1,κ=3-nNL, 和 分别为tk-1时刻非线性状态变量 的估计值和估计协方差矩阵; 表示矩阵的下三角分解平方根的第i列;
利用UKF计算tk时刻非线性状态变量的一步预测值其中, 为 的一步预测模型值, 为补偿项;Wi为权值;
所述利用KF对tk-1时刻所述线性状态方程中的线性状态变量进行时间更新,包括:其中, 为补偿项;
将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数,并利用UKF对所述非线性状态方程进行量测更新,得到tk时刻的非线性状态变量估计值 将所述作为所述线性状态方程的参数,并利用KF对所述线性状态变量方程进行量测更新,得到tk时刻的线性状态变量估计值所述将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数,并利用UKF对所述非线性状态方程进行量测更新,得到tk时刻的非线性状态变量估计值 包括:根据所述非线性状态变量一步预测值 计算非线性状态变量的一步预测样本点其中, 为 的误差, 为协方差矩阵为 的零均值高斯白噪声, 为tk时刻量测变量一步预测值的自协方差矩阵, 为tk时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵, 和 的计算过程如下:
其中,
为补偿项;
计算非线性状态变量的增益矩阵
计算滤波估计值及其协方差矩阵:
将所述UKF更新后的tk-1时刻的非线性状态作为所述线性状态方程的参数,并利用KF对所述线性状态变量方程进行量测更新,得到tk时刻的线性状态变量估计值 包括:其中 , 为线性状态变量的增益矩阵 ,Y ′为 的雅可比矩阵 ,为补偿项;I21×21为21行21列的单位矩阵;
根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正,分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值;
所述根据所述线性状态变量估计值对所述子系统的捷联解算结果进行修正,得到修正后的tk时刻的子系统的线性状态变量值,包括:其中, 和 分别为子IMU修正后的东向、北向和天向速度; 和分别为子系统捷联解算得到的东向、北向和天向速度;δVE、δVN和δVU分别为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差;
Lnew=Lold-δL
λnew=λold-δλ
hnew=hold-δh
其中,Lold、λold和hold分别为子IMU捷联解算得到的纬度、经度和高度;Lnew、λnew和hnew分别为子IMU修正后的纬度、经度和高度;δL、δλ和δh分别为tk时刻KF估计出的子IMU捷联解算纬度、经度和高度误差;
所述根据所述非线性状态变量估计值对所述子系统的捷联解算结果进行修正,得到修正后的tk时刻的子系统的非线性状态变量值,包括:计算tk时刻子系统真实导航坐标系n与计算导航坐标系n1间的方向余弦矩阵计算tk时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵其中 为tk时刻子系统捷联解算得到的方向余弦矩阵;
由被更新后的 计算tk时刻子系统的航向角ψs、俯仰角θs和横滚角γs,将 记为:其中,Tlm为矩阵 中第l行、第m列的元素,则子IMU航向角ψs、俯仰角θs和横滚角γs的主值,即ψs主、θs主和γs主分别为:θs主=arcsin(T32)
θs=θs主,
2.一种机载分布式POS的实时传递对准的装置,其特征在于,包括:建立模块,用于建立大失准角条件下的机载分布式POS传递对准的误差模型和数学模型;所述误差模型包括子系统的惯导误差模型、主系统和子系统间的角误差模型;所述数学模型包括系统状态方程和系统量测方程;其中,所述系统状态方程包括线性状态方程和非线性状态方程;所述非线性状态方程由与姿态状态变量有关的微分方程组成,所述线性状态方程为由与速度误差、位置误差、陀螺仪常值误差、加速度计常值偏置、安装误差角、弹性变形角、弹性变形角速率中的至少一个状态变量有关的微分方程组成;
所述建立模块具体用于建立大失准条件下的机载分布式POS的子系统的惯导误差模型和主子系统间的角误差模型,所述惯导误差模型包括姿态误差微分方程、速度误差微分方程、位置误差微分方程和惯性仪表误差微分方程,所述主子系统间的角误差模型包括安装角误差模型和弹性变形角模型;
其中,所述姿态误差微分方程为:
其中, 为子系统姿态失准角,φE、φN和φU分别为东向、北向、天向失准角, 为子系统真实导航坐标系相对惯性坐标系的角速度; 为 的误差角速度; 为子系统载体坐标系到其计算导航坐标系的方向余弦矩阵;I3×3为3行、3列的单位矩阵,εb为子系统陀螺仪误差,εb=εc+wε,其中,εc为子系统陀螺仪常值漂移,wε为子系统陀螺仪随机误差, 和分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常值漂移, 和 分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差; 为子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵;其中,
所述速度误差微分方程为:
n T n T
其中,V=[VE VN VU] 和δV=[δVE δVN δVU]分别为子系统速度和速度误差,其中VE、VN和VU分别为东向、北向和天向速度,δVE、δVN和δVU分别为东向、北向和天向速度误差;fb=[fx fy fz]T是子系统的比力,其中fx、fy和fz分别为载体坐标系x方向、y方向和z方向比力; 和分别为地球坐标系相对惯性坐标系的角速度ωie及其误差δωie在子系统真实导航坐标系下的表示; 和 分别为子系统真实导航坐标系相对地球坐标系的角速度及其误差; 为子系统加速度计误差, 其中, 为子系统加速度计常值偏置,为系统加速度计随机误差,
和 分别为子系统载体坐标系x轴、y轴和z轴加速度计常值偏置,和 分别为子系统载体坐标系x轴、y轴和z轴加速度计随机误差;
所述位置误差微分方程为:
其中,L、λ、h分别为子系统的纬度、经度、高度,δL、δλ、δh分别为纬度误差、经度误差、高度误差 ; 为 纬度的一阶导 数, 为经 度的 一阶导数 ,为高度的一阶导数;RM和RN分别为沿子午圈和卯酉圈的主曲率半径;
所述惯性仪表误差微分方程为:
其中,ε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为零均值白噪声,其方差 满足:其中,σj2为弹性变形角θj的方差,βj和 为描述弹性变形角θ的二阶马尔科夫过程的参数;
第一更新模块,用于利用UKF对tk-1时刻所述非线性状态方程中的非线性状态变量xNL进行时间更新,并利用KF对tk-1时刻所述线性状态方程中的线性状态变量xL进行时间更新;
所述非线性状态变量xNL和线性状态变量xL分别定义为:xNL=[x1]T
xL=[x2 x3]T
其中,
1
x=[φE φN φU],
所述非线性状态方程为:
所述线性状态方程为:
其中, 和 分别为tk时刻的非线性状态变量和线性状态变量, 和 分别为tk-1时刻的非线性状态变量和线性状态变量,非线性状态方程的系统噪声为线性状态方程的系统噪声为
其中 分别为子系统载体坐标系x
轴、y轴、z轴陀螺仪的随机误差, 分别为子系统载体坐标系x轴、y轴、z轴加速度计的随机误差;非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声,wNL的方差阵QNL由陀螺仪随机漂移确定,wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数 确定;系统状态方程中各系数矩阵的表达式如下:所述建立模块具体用于建立大失准角条件下的机载分布式POS传递对准的系统量测模型;
所述建立大失准角条件下的机载分布式POS传递对准的系统量测模型,包括:系统量测量定义为:
z=[δψ δθ δγ δV′E δV′N δV′U δL′ δλ′ δh′]T建立系统量测方程:
其中,zk为tk时刻的量测量,δψ、δθ、δγ分别为子系统与主系统的航向角、俯仰角、横滚角之差,δV′E、δV′N、δV′U分别为子系统与主系统东向、北向、天向速度之差,δL′、δλ′、δh′分别为子系统与主系统的纬度、经度、高度之差;量测噪声其中vδψ、vδθ、vδγ分别为主
系统航向角、俯仰角、横滚角的量测噪声, 分别为主系统东向、北向、天向速度的量测噪声,vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声;v为量测噪声,取为零均值高斯白噪声,vk为v在tk时刻的值;
所述第一更新模块具体用于,计算tk-1时刻的2nNL+1个样本点 其中,nNL为非线性状态变量的维数:
其中, 10-4≤α≤1,κ=3-nNL, 和 分别为tk-1时刻非线性状态变量 的估计值和估计协方差矩阵; 表示矩阵的下三角分解平方根的第i列;
利用UKF计算tk时刻非线性状态变量的一步预测值其中, 为 的一步预测模型值, 为补偿项;Wi为权值;
所述第一更新模块还具体用于根据以下公式利用KF对tk-1时刻所述线性状态方程中的线性状态变量进行时间更新:
其中, 为补偿项;
第二更新模块,用于将所述KF更新后的tk-1时刻的线性状态作为所述非线性状态方程的参数,并利用UKF对所述非线性状态方程进行量测更新,得到tk时刻的非线性状态变量估计值 以及将所述 作为所述线性状态方程的参数,并利用KF对所述线性状态变量方程进行量测更新,得到tk时刻的线性状态变量估计值所述第二更新模块具体用于,根据所述非线性状态变量一步预测值 计算非线性状态变量的一步预测样本点
其中, 为 的误差, 为协方差矩阵为 的零均值高斯白噪声, 为tk时刻量测变量一步预测值的自协方差矩阵, 为tk时刻非线性状态变量一步预测值与量测量一步预测值的互协方差矩阵, 和 的计算过程如下:
其中,
为补偿项;
计算非线性状态变量的增益矩阵
计算滤波估计值及其协方差矩阵:
以及,所述第二更新模块具体用于根据以下获得时刻的tk时刻的非线性状态变量估计值
其中 , 为线性状态变量的增益矩阵 ,Y ′为 的雅可比矩阵 ,为补偿项;I21×21为21行21列的单位矩阵;
修正模块,用于根据所述线性状态变量估计值、非线性状态变量估计值对所述子系统的捷联解算结果进行修正,分别得到修正后的tk时刻的子系统的线性状态变量值和非线性状态变量值;
所述修正模块具体用于,根据以下公式对所述子系统的捷联解算结果进行修正得到修正后tk时刻的子系统的线性状态变量值:其中, 和 分别为子IMU修正后的东向、北向和天向速度; 和分别为子系统捷联解算得到的东向、北向和天向速度;δVE、δVN和δVU分别为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差;
Lnew=Lold-δL
λnew=λold-δλ
new old
h =h -δh
其中,Lold、λold和hold分别为子IMU捷联解算得到的纬度、经度和高度;Lnew、λnew和hnew分别为子IMU修正后的纬度、经度和高度;δL、δλ和δh分别为tk时刻KF估计出的子IMU捷联解算纬度、经度和高度误差;
所述修正模块还具体用于,根据以下对所述子系统的捷联解算结果进行修正得到修正后tk时刻的子系统的非线性状态变量值:计算tk时刻子系统真实导航坐标系n与计算导航坐标系n1间的方向余弦矩阵计算tk时刻子系统载体坐标系b与真实导航坐标系n之间的方向余弦矩阵其中 为tk时刻子系统捷联解算得到的方向余弦矩阵;
由被更新后的 计算tk时刻子系统的航向角ψs、俯仰角θs和横滚角γs,将 记为:其中,Tlm为矩阵 中第l行、第m列的元素,则子IMU航向角ψs、俯仰角θs和横滚角γs的主值,即ψs主、θs主和γs主分别为:θs主=arcsin(T32)
θs=θs主,
说明书 :
一种机载分布式POS的实时传递对准的方法及装置
技术领域
背景技术
其中,p为系统模型的状态变量维数,q为观测量维数。由于实际飞行中机体
存在弹性变形,且主、子系统之间存在安装误差角,需要将弹性变形角和安装误差角均扩充为状态变量进行估计,再加上子系统的误差(包括速度误差、姿态误差、位置误差和惯性仪表误差),这样完整的传递对准状态变量维数将高达24维甚至更高,使得高精度非线性滤波方法难以满足传递对准的实时性要求。
Z.Initial Alignment of Large Azimuth Misalignment Angle in SINS Based on UKF-KF[C]中国卫星导航学术年会.2014.)仅用于SINS静基座自对准和GNSS/SINS组合初始对准中,存在以下不足:(1)假设位置精确已知,且仅考虑方位失准角为大角度,没有考虑三维大失准角的情况,简化了误差方程,但损失了精度;(2)初始对准仅采用速度误差作为量测量,因而系统量测方程为线性,不适用于传递对准系统量测方程为非线性的情况;(3)在进行UKF滤波时,没有考虑线性状态变量估计值的误差,直接将线性状态变量的估计值作为非线性状态方程的参数,引入了模型误差,导致非线性状态变量的估计结果出现误差;(4)先估计非线性状态变量再估计线性状态变量的串行方式,不但增加了计算时间,而且使得非线性状态变量进行tk时刻的量测更新时,系统量测方程的参数仍为tk-1时刻的线性状态变量估计值,而tk-1时刻的线性状态变量估计值的精度必然差于tk时刻线性状态变量估计值的精度,因此将tk-1时刻的线性状态变量估计值作为非线性状态方程的参数给非线性状态的估计值带来误差。目前,有文献(尹建君,张建秋,林青.Unscented卡尔曼滤波-卡尔曼滤波算法[J].系统工程与电子技术,2008(04):617-620.)假设非线性状态的系统状态方程与线性状态无关,考虑了问题(2)中提到的系统量测方程为非线性的情况,提出用蒙特卡洛法对线性状态进行采样,匹配非线性状态的样本点来进行UKF的量测更新,以减小线性状态的估计误差对滤波结果的影响。但是蒙特卡洛法仍然需要对状态变量进行采样,额外增加了计算量。另外,蒙特卡洛法还存在采样点少时精度低的问题。
发明内容
得到tk时刻的线性状态变量估计值
附图说明
具体实施方式
新,得到tk时刻的线性状态变量估计值
航坐标系的方向余弦矩阵;I3×3为3行、3列的单位矩阵,εb为子系统陀螺仪误差,εb=εc+wε,其中,εc为子系统陀螺仪常值漂移, wε为子系统陀螺仪随机误差,
和 分别为子系统载体坐标系x轴、y轴和z轴陀螺仪常
值漂移, 和 分别为子系统载体坐标系x轴、y轴和z轴陀螺仪随机误差; 为
子系统真实导航坐标系到其计算导航坐标系的方向余弦矩阵。
系统真实导航坐标系下的表示; 和 分别为子系统真实导航坐标系相对地球坐标系
的角速度及其误差; 为子系统加速度计误差, 其中,
为子系统加速度计常值偏置, 为系统加速度计随机误差,
和 分别为子系统载体坐标系x轴、y轴和z轴加速
度计常值偏置, 和 分别为子系统载体坐标系x轴、y轴和z轴加速度计随机
误差。
径。
线性状态方程的系统噪声为
其中 分别为自系统载体坐标系
x轴、y轴、z轴陀螺仪的随机误差, 分别为子系统载体坐标系x轴、y轴、z轴
加速度计的随机误差;非线性状态方程的系统噪声wNL和线性状态方程的wL均为零均值高斯白噪声,wNL的方差阵QNL由陀螺仪随机漂移确定,wL的方差阵QL由加速度计随机偏置和二阶马尔科夫过程参数 决定;
系统航向角、俯仰角、横滚角的量测噪声, 分别为主系统东向、北向、天
向速度的量测噪声,vδL、vδλ、vδh分别为主系统纬度、经度、高度的量测噪声;v为量测噪声,取为零均值高斯白噪声,vk为v在tk时刻的值,其方差阵R由主系统的姿态精度、速度精度和位置精度决定。
的下三角分解平方根的第i列;
线性状态变量 在tk-1时刻的估计协方差矩阵;由于非线性状态方程的系统噪声为复杂加性噪声,因此不需要对状态变量进行扩维,系统维数降低了15维,大大减少了采样数目,减小了计算量。
矩阵为 的零均值高斯白噪声并在计算 时予以补偿;
方差矩阵, 为tk时刻非线性状态变量一步预测值与量测变量一步预测值的互协方差
矩阵。由于系统量测方程的噪声为加性噪声, 和 的计算过程如下:
为tk时刻KF估计出的子系统捷联解算东向、北向和天向速度误差;
况,考虑了位置误差、非线性滤波时线性状态变量估计值作为参数以及线性滤波时非线性状态变量估计值作为参数存在的误差,并分别在计算非线性状态变量一步预测协方差矩阵和线性状态变量一步预测协方差矩阵时予以补偿,因而提高了传递对准的精度,解决了现有方法先估计非线性状态变量再估计线性状态变量的串行方式带来的非线性滤波时间更
新滞后的问题,压缩了计算时间,从而满足机载分布式POS实时传递对准的精度和实时性要求。