[发明专利]飞行器惯性/气动模型组合导航方法

权利要求书

1.一种飞行器惯性/气动模型组合导航方法，其特征在于：利用飞行器已知的气动参数、外形参数、控制量以及运动参数信息对飞行器的速度、姿态进行求解，利用惯导系统误差模型与飞行器气动模型，构建卡尔曼滤波器，将惯性导航系统得到的位置、速度、姿态信息与气动模型得到的速度、姿态信息进行数据融合，同时对惯性器件误差进行实时估计与补偿，具体步骤如下：

(1)以周期                                                读取飞行器的阻力系数，侧力系数，升力系数，并将上述3个系数、、分别定义于机体系轴、轴和轴；以周期读取飞行器的滚转力矩系数，俯仰力矩系数，偏航力矩系数，并将上述3个系数、、分别定义于机体系轴、轴和轴；以周期读取飞行器的发动机推力，其在机体坐标系下轴、轴和轴方向的分量分别为、、；以周期读取飞行器的空速和飞行器的总质量，飞行器的总质量包括飞行器机体质量、机载设备质量、乘员质量、武器质量以及剩余燃油质量；

(2)以周期读取惯导系统输出的飞行器位置、速度、姿态信息；

(3)根据飞行器动力学方程计算飞行器所受的力与力矩；

根据步骤(1)得到的飞行器的总质量，得飞行器重力为，为重力加速度，其在机体坐标系下轴、轴和轴方向的分量分别为、、；

根据飞行器的动力学方程，以及步骤(1)中得到的飞行器气动参数，即飞行器的阻力系数、侧力系数、升力系数；滚转力矩系数、俯仰力矩系数、偏航力矩系数；飞行器的发动机推力在机体坐标系下轴、轴和轴方向的分量、、；飞行器重力在机体坐标系下轴、轴和轴方向的分量、、，得飞行器所受到合外力为：，

其中、、为飞行器所受合外力在机体系轴、轴和轴方向的分量，为当地大气密度，为空速，为机翼面积，所受力矩为：，

其中、、为飞行器所受合外力在机体系轴、轴和轴方向的分量，被称为滚转力矩、俯仰力矩以及偏航力矩，为机翼展长，为机翼平均气动弦长，为机翼面积；为滚转力矩系数，为俯仰力矩系数，为偏航力矩系数；

(4)根据飞行器运动学方程计算导航参数；

以、、分别表示飞行器机体系下轴、轴和轴方向的速度，以、、分别表示飞行器机体系下轴、轴和轴方向的角速度，以分别表示飞机绕机体轴的转动惯量，为飞行器对轴、轴的惯性积，由于飞机具有机体坐标系的对称面，所以飞行器对轴、轴的惯性和对轴、轴的惯性为零，由飞行器的运动学方程组，为飞行器机体系下轴的角加速度，为飞行器机体系下轴的角加速度，为飞行器机体系下轴的角加速度以及步骤(3)中求得的飞行器在机体系轴、轴和轴方向的滚转力矩、俯仰力矩、偏航力矩，对机体系下的角速度、、进行求解，其中，，，，，，，，，；

以、、分别表示飞行器的横滚角、俯仰角、航向角三个姿态角，根据姿态角的变化率与角速率的关系，为横滚角速率， 为俯仰角速率，为航向角速率，以及步骤(4)中求得的机体系下的角速度、、，求解三个姿态角的数值；

根据飞行器的运动学方程组，为飞行器机体系下轴的加速度，为飞行器机体系下轴的加速度，为飞行器机体系下轴的加速度，以及步骤(3)中求得的飞行器所受到的合外力、、，步骤(1)中获得的飞行器的总质量，步骤(4)中求得的机体系下的角速度、、，对飞行器机体系下轴、轴和轴方向的速度、、进行求解；

(5)根据飞行器的气动模型与惯导系统误差模型，选取气动模型解算出的速度误差、姿态误差、角速度误差与惯导系统的位置误差、姿态误差、速度误差、陀螺一阶马尔科夫误差、陀螺零偏误差、加速度计一阶马尔科夫误差为状态量，建立状态方程；选取飞行器速度、姿态为量测量，建立观测方程；根据步骤(2)得到惯导系统输出导航参数与步骤(4)气动模型解算的导航参数，根据卡尔曼滤波方程得到时刻状态量的最优估计值，其具体步骤为：

（a）卡尔曼滤波器状态方程的建立

根据飞行器的气动模型与惯导系统误差模型，选取气动模型解算出的速度误差、姿态误差、角速度误差与惯导系统的位置误差、姿态误差、速度误差、陀螺一阶马尔科夫误差、陀螺零偏误差、加速度计一阶马尔科夫误差为状态量，状态量共27维，表达式为，其中与惯导系统相关的状态量

，其中下标代表与惯导系统相关的参数，下同，分别为惯导系统解算的东北天坐标系下轴、轴和轴三个方向的平台误差角，该坐标系中轴、轴和轴分别于当地东向、北向、天向重合，、、分别为惯导系统解算的东北天坐标系下轴、轴和轴三个方向的速度误差，分别为惯导系统解算的经度误差、纬度误差以及高度误差，为机体系下三个陀螺的零偏误差，为机体系下三个陀螺的一阶马尔科夫过程误差，为机体系下三个加速度计的一阶马尔科夫过程误差，其中与气动模型相关的状态量，其中下标代表与气动模型相关的参数，下同，、、分别表示气动模型解算的飞行器机体系下轴、轴和轴方向的速度误差，、、分别表示气动模型解算的飞行器机体系下轴、轴和轴方向的角速度误差，、、分别表示气动模型解算的飞行器的横滚角误差、俯仰角误差、航向角误差；

卡尔曼滤波的状态方程为，其中为状态向量，为状态向量一阶导数，为状态转换矩阵，为系统噪声系数矩阵；

为系统噪声，其值为，与惯导系统相关的系统噪声，其中、、代表惯导系统机体系下三个陀螺的白噪声，、、代表惯导系统机体系下三个陀螺一阶马尔科夫误差的驱动白噪声，、、代表惯导系统机体系下三个加速度计一阶马尔科夫误差的驱动白噪声，与气动模型相关的系统噪声，其中、、代表飞行器受到的力的误差，、、代表飞行器所受到的力矩的误差；

所述状态转换矩阵为，，

与惯性导航相关的部分，其中，表达式如下：

，

和为地球子午圈和卯酉圈主曲率半径，，，为地球椭圆度，为地球赤道半径，为地球自转角速度，、、分别代表惯性导航系统解算出的飞行器在地理系下轴、轴和轴方向的速度，为惯性导航系统解算出的飞行器纬度，为惯性导航系统解算出的飞行器高度，

，，

，

，，

，；

，其中为惯性导航解算出的机体系到地理系的转换矩阵，，其中、、为惯导系统机体系下三个陀螺一阶马尔科夫误差的相关时间，、、为惯导系统机体系下三个加速度计一阶马尔科夫误差的相关时间，

与气动模型相关的部分，表达式如下：，，，，，，，，，

其中、、分别表示气动模型解算的飞行器机体系下轴、轴和轴方向的速度，、、分别表示气动模型解算的飞行器机体系下轴、轴和轴方向的角速度，、分别表示气动模型解算的飞行器的横滚角、俯仰角两个姿态角；

所述系统噪声系数矩阵为，，其中

，为惯性导航解算出的机体系到地理系的转换矩阵：；

（b）卡尔曼滤波器量测方程的建立

根据飞行器的气动模型特点，选取姿态与速度为观测量，卡尔曼滤波器的量测方程为，量测量为

，其中为气动模型求解的飞行器在地理系下的速度，为惯导系统求解的飞行器在地理系下的速度，为气动模型求解的飞行器姿态角，为惯导系统求解的飞行器姿态角；

在滤波方程中，状态量为惯导系统的平台误差角，为了与气动模型的状态量相统一，需将其转换为惯导系统的姿态误差角，其转换关系式为

，其中、、为惯导系统解算的姿态误差角，而状态量为惯导系统求解的飞行器在地理系下的速度误差，状态量为气动模型求解的行器在机体系下的速度误差，因此为使二者一致，将转换到地理系下，综上，得到，其中为气动模型解算的机体系到地理系的转换矩阵，

，其中为量测噪声；

（c）连续型线性方程离散化

取采样周期对步骤(a)与步骤(b)中得到的连续型线性方程进行离散化得到离散型线性方程：

式中，为时刻的状态最优估计值，为时刻的状态最优估计值，为状态变量从时刻到时刻的状态转移矩阵，为时刻的系统噪声，为时刻的系统噪声对时刻状态影响的噪声系数矩阵，为时刻的观测值，为时刻的观测噪声，，为步骤(a)中的状态转换矩阵在时刻的值，，为步骤(a)中的系统噪声系数矩阵在时刻的值，，为步骤(b)中的量测矩阵在时刻的值；

（d）卡尔曼滤波方程

利用时刻的状态最优估计值，根据求取的一步预测值，其中根据步骤(c)得到，中的系数由步骤(2)、步骤(3)、步骤(4)得到；

通过式求解时刻状态量的一步预测值的方差阵；通过式求解时刻滤波增益矩阵；

根据步骤(2)获取惯导系统输出的飞行器速度与姿态信息，以及步骤(4)获取气动模型输出的飞行器速度与姿态信息，组成时刻总的量测量，结合求解得到的时刻状态量的一步预测值和滤波增益矩阵，利用公式对时刻状态最优估计值进行求解；

通过式对时刻状态最优估计值的误差方差阵进行求解；

(6)惯导系统误差修正，

利用步骤(5)得到的时刻状态最优估计值对惯导系统输出的姿态、位置、速度进行修正；

惯导系统修正速度值为，其中为修正后的惯导系统速度，为步骤(2)中得到的惯性导航系统解算出的飞行器在地理系下速度，为步骤(5)中解算出的的部分状态量；

惯导系统修正位置为，其中为修正后的惯导系统速度，为步骤(2)中得到的惯性导航系统解算出的飞行器的经度、纬度、高度信息，为步骤(5)中解算出的的部分状态量；

以、、表示步骤(2)中得到的惯性导航系统解算出的飞行器的横滚角、俯仰角、航向角信息，则惯导系统解算的机体系到地理系的转换矩阵为

，为步骤(5)中解算出的的部分状态量，则补偿矩阵为，则修正后的机体系到地理系的转换矩阵为，则补偿后的横滚角，俯仰角，航向角。