基于卡尔曼滤波和粒子群算法的飞行参数辨识

北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品1第二十四届“冯如杯”学生创意大赛基于卡尔曼滤波和粒子群算法的飞行参数辨识北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品2基于卡尔曼滤波和粒子群算法的飞行参数辨识摘要针对低精度捷联航姿系统与北斗系统组合导航在某些情况下适应性差，以及系统模型准确性和噪声统计特性随实际工作情况变化的问题，提出了利用大气数据系统输出的速度信息辅助航姿解算的方法。基于模糊自适应卡尔曼滤波原理，对观测模型的参数和系统特性进行估计、修正，从而实现水平航姿最优估计。关键词：粒子群算法，卡尔曼滤波，飞行参数辨识前言中低精度的航姿系统，价格低廉且性能优越，但是漂移过大，导致航姿角精度不能长时间保持。通常需要采用中低精度的捷联航姿系统与北斗卫星定位系统组合。北斗导航定位具有精度的长期稳定性，但在高动态和强干扰环境下的适应性自主性差，输出具有间断性。而大气数据系统提供信息，不需依赖外部条件，具有较强的自主性，空速可以在北斗信号间断时，近似转换为对地速度，有效辅助惯导进行导航。国内现有大气数据与惯性数据信息融合方面成熟的研究主要集中在利用大气数据系统的气压高度信息阻尼惯导系统高度通道的发散，未深入挖掘大气提供的空速、气流角等数据对惯导的辅助能力，所以本文的研究点很有开拓意义。本文基于大气数据辅助中低精度航姿系统，弥补了北斗信号同步性差、输出间断的缺陷，利用一种有效的模糊自适应卡尔曼滤波融合算法，对过程噪声和测量噪声权值进行自适应模糊调节，达到多传感器数据有效融合的目的，获得较好的计算精度和可靠性。正文1基于粒子群算法的VTOL飞行器参数辨识ＶＴＯＬ飞行器即垂直起降飞行器，一般指战斗机或轰炸机。该飞行器北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品3可实现飞行器自由起落，从而突破跑道的限制，具有重要的军用价值。如图所示为Ｘ－Ｙ平面上的ＶＴＯＬ受力图。由于只考虑起飞过程，因此只考虑垂直方向Ｙ轴和横向Ｘ轴，忽略了前后运动（即ｚ方向）。Ｘ－Ｙ为惯性坐标系，bX和bY为飞行器的机体坐标系。图1飞行器图2VTOL示意图根据图2，可建立ＶＴＯＬ动力学平衡方程为（1）其中，T和l为控制输入，即飞行器底部推力力矩和滚动力矩，g为重力加速度，是描述T和l之间耦合关系的系数。由式（1）可见，该模型为两个控制输入控制三个状态，为典型的前驱动系统。模型中包括三个物理参数，即m、和xI。令[X，Ẋ，Y，𝑌̇，θ，𝜃̇]＝［x1，x2，x3，x4，x5，x6，］，则式（1）可表示为北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品4（2）令，，T=𝑢1，l=𝑢2，则（3）上式可表示为21512524115225326sincos0cossin0xaxuaxuxauxauxgaux由于151252551111522555223232sincossincos0cossincossin0001axuaxuxxauauxauxxxauauau北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品5551155223sincos00cossin000001xxauxxaua则得255114552236sincos000cossin0000001xxxauxxxaguax即1255115542236sincos000cossin0000001xxxauxxxgauax上式可写成下面的形式Y=Aτ其中，12555546sincos00cossin00001xxxxxxgxY，123000aaaA，12uuτ。由11(1)auY、22(2)auY及32(3)auY可知，参数1a、2a及3a之间线性无关，因此，可采用智能搜索算法进行参数辨识。采用实数编码，辨识误差指标取11ˆˆ()()2NTiiiiiJyyyy其中，N为测试数据的数量，()iyiY北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品6仿真中，取真实参数为0[][68.60.5123.1]xmIP，辨识参数集为0ˆˆˆˆ[]xmIP。运行模型测试程序，对象的输入信号取正弦和余弦信号，从而得到用于辨识的模型测试数据，并将数据保存。在离子群算法仿真程序中，将待辨识的参数向量记为ˆP.取粒子群个数为Size=80，最大迭代次数G=100，采用实数编码，待辨识参数m、和xI分别分布在、。粒子运动最大速度为maxV=1.0，即速度范围为。学习因子取1c=1.3，2c=1.7.采用线性递减的惯性权重，惯性权重采用从0.90线性递减到0.10的策略。将辨识误差直接作为粒子的目标函数，越小越好。更新粒子的速度和位置，产生新种群，辨识误差函数J的优化过程如图所示。图4辨识误差函数J的优化过程北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品72捷联姿态算法2.1坐标系和捷联矩阵2.1.1坐标系设定取导航坐标系（n系）为东北天坐标系，即gX轴指向东，gY轴指向北，gZ轴垂直地平面指向天。机体坐标系的原点与机体的质心重合，bX轴沿机体横轴指向右，bY轴沿机体纵轴指向前方，bZ轴与bX轴、bY轴按右手法则构成直角坐标系。姿态角是根据机体坐标系相对导航坐标系的转角来确定的。2.1.2捷联矩阵bnC表示向量由n系向b系投影的变换矩阵，nbC表示向量由b系向n系投影的变换矩阵。从载体坐标系到导航坐标系的变换矩阵nbC用欧拉角（横滚角，俯仰角，航向角）表示如下：nbccssscssccssCcssscccsscscscscc（4）2.2姿态更新算法2.2.1基于四元数的姿态更新四元数方法不仅计算简单，而且避免了欧拉角的奇异性问题，广泛应用在飞行器姿态控制和导航解算当中。根据陀螺测量值时，惯性导航系统可对姿态进行更新。采用四元数得到即时修正的微分方程如式（5），每个采样周期内可根据机体三轴绝对角速度[,,]bbbbTibxyz对姿态进行更新：12bqq（5）北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品8其中，0000bbbxyzbbbxzybbbbyzxbbbzyx，可由固连在机体上的三轴角速率陀螺测量得到。系统通过陀螺仪所采集到的角速率在单位步长内对四元数进行实时更新，数值积分得到中间结果，进一步归一化，得到了下一个采样周期的新四元数。通过四元数与姿态矩阵的转换关系，可以得到新时刻的姿态矩阵，之后根据姿态矩阵解算出俯仰角、滚转角和航向角，对于给定的nbC，其元素为ijC，解算公式如下：3231332122arcsin()arctan()arctan()CCCCC（6）综上，利用陀螺数据就可以得到未修正的飞行器的姿态信息。3模糊自适应卡尔曼滤波器设计3.1大气数据辅助惯性姿态解算系统模型3.1.1状态方程忽略惯性垂直通道对水平通道的影响，略去天向速度，建立水平通道状态方程。忽略地球表观运动，以低精度惯导系统的姿态误差角[,,]nxyz、速度误差[,,]niieiniuVVVV、陀螺零偏误差0[,,]xyzbbbb、经度误差L以及纬度误差，组成扩展卡尔曼滤波器的状态向量。建立状态方程如下：()()()()XtAtXtwt（7）北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品933333333333333333300()00(())0000()000000000000000nbnbbCCfAt（8）()VbL其中，[,,]bxyzffff表示捷联在机体轴上的加速度计输出的三轴比力，()wt是系统噪声变量，假设其是零均值的白噪声，且其协方差矩阵为：[()()]()()TEwtwtQtt（9）3.1.2量测方程量测信息除了利用北斗和INS输出的水平位置和速度之差作为量测值，还可采用惯导提供的水平面内速度和大气数据提供的空速经过投影分解的速度之差。nnnieaeiezaneannnninaninzaeVVVVvZvVVVV（10）式中，nieV、ninV表示惯导速度量测信息在地理系下的东向和北向真值，naeV、nanV表示大气速度量测信息在地理系中的东向和北向投影量值，ev、nv表示东向、北向速度测量误差，可视为白噪声，且与系统噪声互不相关。则惯导与北斗和大气组合的系统量测方程()()()()ZtHtXtvt（11）可表示如下：北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品10nnieaenninannnieBDenninBDniBDiBDVVVVVVZVVLL1313'13'13130130010000000010000000100000000010000000000010000000001xyneanznnaeieeinniuLvVvVVvVvVvbvL（12）式中，nBDeV、nBDnV、BDL、BD表示北斗测量的速度和经纬度信息，''()[,,,,,]TenenLvtvvvvvv。量测噪声的协方差矩阵：[()()]()()TEvtvtRtt（13）3.1.3离散化通过离散化，得到离散时间状态转移矩阵,1()kkITAt（14）定义残差为,1ˆkkkkkrZHX（15）3.2模糊自适应卡尔曼滤波设计3.2.1设计模糊规则模糊逻辑系统通过在每个滤波周期得到的残差，进而得到残差方差，实时调整参数，对新息序列的协方差和均值进行模糊化，可以得到时域上的模糊子集相应的隶属度函数。当系统噪声不稳定时，通过调整参数，使新息尽量为白噪声。模糊控制器的输入为残差协方差与均值，输出为加权系数。北京航空航天大学第二十四届“冯如杯”学生创意大赛参赛作品113.2.2模糊加权卡尔曼滤波算法设系统过程噪声和量测噪声的方差矩阵为指数函数，采用指数加权的方法来减少误差较大的数据在计算中所占的比例。(21)(21),kkkkQQRR（16）加权估计的协方差2kkkPP（17）量测中新息ˆkkkkrZHx（18）模糊自适应卡尔曼滤波方程为,112,11,11ˆˆ()(1)ˆˆˆ()kkkkTkkkkkkkTTkkkkkkkkkkkkkkkkXXPPQKPHHPHPPKHPXXKZHX（19）系统噪声增大时，反映在新息上就是其协方差增大。若新息的协方差很大，表明系统噪声不稳定，量测数据可信程度低，滤波器性能将会受到很大影响，故应取较小的值，从而降低滤波器对量测数据的依赖，以改善滤波器性能。反之，