本发明属于飞行器导航方法，特别涉及了一种基于动力学模型辅助惯导的导航方法。
背景技术：
多旋翼飞行器具有结构简单、体积小、操控性好、垂直起降等特性，对不同环境具有良好的适应性。目前，其在农业播种、航空拍摄、气象监测、城市反恐、灾区救援、军事侦察等方面均已有广泛的应用，展现出广阔的前景。
目前多数多旋翼飞行器导航系统所采取的方案是微机电惯导系统(micro-electromechanicalsystemsinertialnavigationsystem，memsins)/全球定位系统(globalpositionsystem，gps)的组合导航系统。但是在gps信号受干扰、丢失的情况下，采用该导航系统方案的多旋翼只能依靠单独的memsins进行导航，而memsins的误差会急剧发散，导致多旋翼失控，甚至坠毁。因此，为增强该组合导航系统的可靠性，亟需一种新型的导航方式在无gps的情况下对惯导进行辅助。
动力学模型辅助惯导是一种新型的自主导航方法，其最早应用于大型飞机。近年来，为提高多旋翼模型的准确性，国外参考直升机旋翼模型，引入气动阻力相关理论进行精细化建模研究。如法国系统控制中心验证了动力学模型辅助可以提高姿态控制精度，德国卡尔斯鲁厄理工学院系统优化研究所提出了一种统一模型的滤波结构来融合气动模型和惯导。但是目前大多数模型由于过于简化，依然存在缺陷，在机动飞行过程中导致较大误差。
旋翼转速是多旋翼飞行器动力学模型的主要参数之一。但是目前大多数旋翼飞行器缺乏旋翼转速测量方案，不能获取旋翼转速信息，导致大多数动力学模型辅助惯导的方法将旋翼转速假设为定值或耦合到气动参数中。该方法对模型进行简化的同时也降低了导航精度。本专利基于多旋翼飞行器动力学模型特性，发明了基于旋翼转速的动力学模型辅助惯导改进算法，具有模型更准确、动态导航精度更高、且易于实现的优点。
技术实现要素：
为了解决上述背景技术提出的技术问题，本发明旨在提供基于旋翼转速的动力学模型辅助惯导的导航方法，解决了传统的动力学模型由于缺乏多旋翼飞行器的旋翼转速测量方案而影响导航精度的问题。
为了实现上述技术目的，本发明的技术方案为：
基于旋翼转速的动力学模型辅助惯导的导航方法，包括以下步骤：
(1)采集机载传感器信息，包括三轴加速度计的输出、三轴陀螺仪的输出和各旋翼转速信息；
(2)根据三轴加速度计的输出和各旋翼转速信息，建立飞行器的气动阻力特性方程；
(3)根据三轴加速度计的输出、三轴陀螺仪的输出和各旋翼转速信息，建立飞行器的动力学模型方程；
(4)根据步骤(3)建立的动力学模型方程，建立导航系统的卡尔曼滤波器状态方程；根据步骤(2)建立的气动阻力特性方程，建立导航系统的卡尔曼滤波器量测方程；
(5)采用卡尔曼滤波器对导航系统的状态进行更新并实时输出姿态、速度信息。
进一步地，在步骤(2)中，所建立的飞行器的气动阻力特性方程：
上式中，fx、fy为三轴加速度计x轴、y轴的输出，k1、k2为机体系x轴、y轴上的气动参数，ωi为飞行器第i个旋翼的转速，i＝1,2,…,l，l为旋翼总数，u、v为飞行器在机体系x轴、y轴上的线速度。
进一步地，在步骤(3)中，首先根据三轴陀螺仪的输出计算飞行器在机体系x、y、z轴上的角速率p、q、r：
上式中，θ、
然后，根据角速率p、q、r建立飞行器的动力学模型方程：
上式中，ω＝ω1+ω2+...+ωl，g为当地重力加速度。
进一步地，在步骤(4)中，所建立的导航系统的状态方程：
上式中，状态量
所建立的导航系统的量测方程：
z＝hx+v
上式中，量测量z＝[fxfy]t，v为量测噪声阵，是均值为0的高斯白噪声，其中：
进一步地，步骤(5)的具体过程：
采用卡尔曼滤波器对导航系统状态进行更新：
xk+1＝φk+1,kxk+bkuk+wk
zk+1＝hk+1xk+1+vk+1
上式中，xk+1为k+1时刻的状态向量，uk为k时刻的控制向量，φk+1,k为k到k+1时刻的系统一步转移矩阵，bk为uk的系数矩阵；wk为k时刻的系统噪声阵，zk+1为k+1时刻的状态向量，hk+1为k+1时刻的量测矩阵，vk+1为k+1时刻的量测噪声；
首先进行状态一步预测：
其次求解一步预测的均方差误差：
上式中，
然后求解滤波增益：
上式中，rk+1为vk+1的方差矩阵；
最后对状态估值，得到滤波结果：
同时更新估计均方差误差：
pk+1＝[i-kk+1hk+1]pk+1|k
按照上述步骤进行循环更新，卡尔曼滤波器输出飞行器的俯仰角
采用上述技术方案带来的有益效果：
(1)本发明将多旋翼飞行器的旋翼转速信息引入动力学模型，从而使旋翼飞行器动力学模型更为准确，从而有效提升多旋翼飞行器导航解算的精度；
(2)本发明采用卡尔曼滤波器对动力学模型和惯导进行融合滤波，从而实现对导航信息最优估计。
附图说明
图1是本发明的流程框架示意图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
基于旋翼转速的动力学模型辅助惯导的导航方法，如图1所示，步骤如下：
步骤1：
采集机载传感器信息，包括三轴加速度计的输出[fxfyfz]、三轴陀螺仪的输出
步骤2：
飞行器受到的阻力fh与水平速度
上式中，k为阻力系数，为常值；vah为空速的在机体系下的水平面分量；m是飞行器的质量。
在本发明中，机体系：原点与载体质心重合，x轴沿载体纵轴向前，y轴沿载体横轴向右，z轴沿载体竖轴向下，即“前右下”坐标系。
由此得到机体系xoy平面内的气动阻力：
fhx＝mk1ωu
fhy＝mk2ωv
上式中，ω＝ω1+ω2+...+ωl；u、v是飞行器在机体系x轴和y轴上的线速度；k1和k2是x轴和y轴上的气动参数，为常值，可由相应参数计算或参数辨识得到。
由上述内容，建立飞行器的气动阻力特性方程：
步骤3：
首先根据三轴陀螺仪的输出计算飞行器在机体系x、y、z轴上的角速率p、q、r：
上式中，θ、
然后，根据角速率p、q、r建立飞行器的动力学模型方程：
上式中，ω＝ω1+ω2+...+ωl，g为当地重力加速度。
步骤4：
根据动力学模型方程建立导航系统的状态方程：
其中，状态量
根据气动阻力特性方程，建立导航系统的量测方程：
z＝hx+v
其中，量测量z＝[fxfy]t，v为量测噪声阵，是均值为0的高斯白噪声。其中：
步骤5：
采用卡尔曼滤波器对导航系统状态进行更新：
xk+1＝φk+1,kxk+bkuk+wk
zk+1＝hk+1xk+1+vk+1
上式中，xk+1为k+1时刻的状态向量，uk为k时刻的控制向量，φk+1,k为k到k+1时刻的系统一步转移矩阵，bk为uk的系数矩阵；wk为k时刻的系统噪声阵，zk+1为k+1时刻的状态向量，hk+1为k+1时刻的量测矩阵，vk+1为k+1时刻的量测噪声；
首先进行状态一步预测：
其次求解一步预测的均方差误差：
上式中，
然后求解滤波增益：
上式中，rk+1为vk+1的方差矩阵；
最后对状态估值，得到滤波结果：
同时更新估计均方差误差：
pk+1＝[i-kk+1hk+1]pk+1|k
按照上述步骤进行循环更新，卡尔曼滤波器输出飞行器的俯仰角
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。