CN107063248A - 基于旋翼转速的动力学模型辅助惯导的导航方法 - Google Patents
基于旋翼转速的动力学模型辅助惯导的导航方法 Download PDFInfo
- Publication number
- CN107063248A CN107063248A CN201710073287.4A CN201710073287A CN107063248A CN 107063248 A CN107063248 A CN 107063248A CN 201710073287 A CN201710073287 A CN 201710073287A CN 107063248 A CN107063248 A CN 107063248A
- Authority
- CN
- China
- Prior art keywords
- mtd
- mrow
- msub
- mtr
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Abstract
本发明公开了基于旋翼转速的动力学模型辅助惯导的导航方法。首先采集惯导数据和旋翼转速；其次建立多旋翼飞行器气动阻力特性方程和动力学模型方程；然后构建导航系统的状态方程和量测方程；最后采用卡尔曼滤波器对导航系统的状态进行更新并实时输出姿态、速度信息。本发明解决了传统的动力学模型由于缺乏多旋翼飞行器的旋翼转速测量方案而影响导航精度的问题。
Description
技术领域
本发明属于飞行器导航方法，特别涉及了一种基于动力学模型辅助惯导的导航方法。
背景技术
多旋翼飞行器具有结构简单、体积小、操控性好、垂直起降等特性，对不同环境具有良好的适应性。目前，其在农业播种、航空拍摄、气象监测、城市反恐、灾区救援、军事侦察等方面均已有广泛的应用，展现出广阔的前景。
目前多数多旋翼飞行器导航系统所采取的方案是微机电惯导系统(Micro-electromechanical Systems Inertial Navigation System，MEMS INS)/全球定位系统(Global Position System，GPS)的组合导航系统。但是在GPS信号受干扰、丢失的情况下，采用该导航系统方案的多旋翼只能依靠单独的MEMS INS进行导航，而MEMS INS的误差会急剧发散，导致多旋翼失控，甚至坠毁。因此，为增强该组合导航系统的可靠性，亟需一种新型的导航方式在无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：
上式中，θ、为飞行器的俯仰角和横滚角，为三轴陀螺仪x轴、y轴、z轴的输出；
然后，根据角速率p、q、r建立飞行器的动力学模型方程：
上式中，ω＝ω1+ω2+...+ωl，g为当地重力加速度。
进一步地，在步骤(4)中，所建立的导航系统的状态方程：
上式中，状态量控制量U＝[p q r]T，W为系统噪声阵，是均值为0的高斯白噪声，其中：
所建立的导航系统的量测方程：
Z＝HX+V
上式中，量测量Z＝[fx fy]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时刻的量测噪声；
首先进行状态一步预测：
其次求解一步预测的均方差误差：
上式中，为k时刻状态向量Xk的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量Xk+1的预测值，Pk为k时刻估计均方差误差，Qk为Wk的方差矩阵，Pk+1|k为的均方差误差；
然后求解滤波增益：
上式中，Rk+1为Vk+1的方差矩阵；
最后对状态估值，得到滤波结果：
同时更新估计均方差误差：
Pk+1＝[I-Kk+1Hk+1]Pk+1|k
按照上述步骤进行循环更新，卡尔曼滤波器输出飞行器的俯仰角横滚角θ、机体系x轴上的线速度u和y轴上的线速度ν的估计。
采用上述技术方案带来的有益效果：
(1)本发明将多旋翼飞行器的旋翼转速信息引入动力学模型，从而使旋翼飞行器动力学模型更为准确，从而有效提升多旋翼飞行器导航解算的精度；
(2)本发明采用卡尔曼滤波器对动力学模型和惯导进行融合滤波，从而实现对导航信息最优估计。
附图说明
图1是本发明的流程框架示意图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
基于旋翼转速的动力学模型辅助惯导的导航方法，如图1所示，步骤如下：
步骤1：
采集机载传感器信息，包括三轴加速度计的输出[fx fy fz]、三轴陀螺仪的输出和多旋翼飞行器各旋翼的转速ωi，i＝1,2,…,l，l为旋翼总数。
步骤2：
飞行器受到的阻力FH与水平速度的关系可简化为：
上式中，k为阻力系数，为常值；VA H为空速的在机体系下的水平面分量；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：
上式中，θ、为飞行器的俯仰角和横滚角，为三轴陀螺仪x轴、y轴、z轴的输出。
然后，根据角速率p、q、r建立飞行器的动力学模型方程：
上式中，ω＝ω1+ω2+...+ωl，g为当地重力加速度。
步骤4：
根据动力学模型方程建立导航系统的状态方程：
其中，状态量控制量U＝[p q r]T，W为系统噪声阵，是均值为0的高斯白噪声。其中：
根据气动阻力特性方程，建立导航系统的量测方程：
Z＝HX+V
其中，量测量Z＝[fx fy]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时刻的量测噪声；
首先进行状态一步预测：
其次求解一步预测的均方差误差：
上式中，为k时刻状态向量Xk的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量Xk+1的预测值，Pk为k时刻估计均方差误差，Qk为Wk的方差矩阵，Pk+1|k为的均方差误差；
然后求解滤波增益：
上式中，Rk+1为Vk+1的方差矩阵；
最后对状态估值，得到滤波结果：
同时更新估计均方差误差：
Pk+1＝[I-Kk+1Hk+1]Pk+1|k
按照上述步骤进行循环更新，卡尔曼滤波器输出飞行器的俯仰角横滚角θ、机体系x轴上的线速度u和y轴上的线速度ν的估计。
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。
Claims (5)
1.基于旋翼转速的动力学模型辅助惯导的导航方法，其特征在于，包括以下步骤：
(1)采集机载传感器信息，包括三轴加速度计的输出、三轴陀螺仪的输出和各旋翼转速信息；
(2)根据三轴加速度计的输出和各旋翼转速信息，建立飞行器的气动阻力特性方程；
(3)根据三轴加速度计的输出、三轴陀螺仪的输出和各旋翼转速信息，建立飞行器的动力学模型方程；
(4)根据步骤(3)建立的动力学模型方程，建立导航系统的状态方程；根据步骤(2)建立的气动阻力特性方程，建立导航系统的量测方程；
(5)采用卡尔曼滤波器对导航系统的状态进行更新并实时输出姿态、速度信息。
2.根据权利要求1所述基于旋翼转速的动力学模型辅助惯导的导航方法，其特征在于：在步骤(2)中，所建立的飞行器的气动阻力特性方程：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<msub>
<mi>k</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
<mo>=</mo>
<msub>
<mi>k</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
<mi>v</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
上式中，fx、fy为三轴加速度计x轴、y轴的输出，k1、k2为机体系x轴、y轴上的气动参数，ωi为飞行器第i个旋翼的转速，i＝1,2,…,l，l为旋翼总数，u、v为飞行器在机体系x轴、y轴上的线速度。
3.根据权利要求3所述基于旋翼转速的动力学模型辅助惯导的导航方法，其特征在于：在步骤(3)中，首先根据三轴陀螺仪的输出计算飞行器在机体系x、y、z轴上的角速率p、q、r：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>p</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>q</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>r</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msup>
<mi>cos</mi>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mi>&theta;</mi>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&phi;</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&phi;</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&theta;</mi>
<mi>cos</mi>
<mi>&phi;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&phi;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>cos</mi>
<mi>&phi;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>n</mi>
<mi>b</mi>
<mi>x</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>n</mi>
<mi>b</mi>
<mi>y</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>n</mi>
<mi>b</mi>
<mi>z</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
上式中，θ、为飞行器的俯仰角和横滚角，为三轴陀螺仪x轴、y轴、z轴的输出；
然后，根据角速率p、q、r建立飞行器的动力学模型方程：
上式中，ω＝ω1+ω2+…+ωl，g为当地重力加速度。
4.根据权利要求3所述基于旋翼转速的动力学模型辅助惯导的导航方法，其特征在于：在步骤(4)中，所建立的导航系统的卡尔曼滤波器状态方程：
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>A</mi>
<mi>X</mi>
<mo>+</mo>
<mi>B</mi>
<mi>U</mi>
<mo>+</mo>
<mi>W</mi>
</mrow>
上式中，状态量控制量U＝[p q r]T，W为系统噪声阵，是均值为0的高斯白噪声，其中：
<mrow>
<mi>A</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>g</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>1</mn>
</msub>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>2</mn>
</msub>
<mi>&omega;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>B</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
所建立的导航系统的卡尔曼滤波器量测方程：
Z＝HX+V
上式中，量测量Z＝[fx fy]T，V为量测噪声阵，是均值为0的高斯白噪声，
其中：
<mrow>
<mi>H</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>1</mn>
</msub>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>2</mn>
</msub>
<mi>&omega;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>.</mo>
</mrow>
5.根据权利要求4所述基于旋翼转速的动力学模型辅助惯导的导航方法，其特征在于：步骤(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时刻的量测噪声；
首先进行状态一步预测：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>k</mi>
</msub>
<msub>
<mi>U</mi>
<mi>k</mi>
</msub>
</mrow>
其次求解一步预测的均方差误差：
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mi>k</mi>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>k</mi>
</msub>
</mrow>
上式中，为k时刻状态向量Xk的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量Xk+1的预测值，Pk为k时刻估计均方差误差，Qk为Wk的方差矩阵，Pk+1|k为的均方差误差；
然后求解滤波增益：
<mrow>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
上式中，Rk+1为Vk+1的方差矩阵；
最后对状态估值，得到滤波结果：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&lsqb;</mo>
<msub>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
同时更新估计均方差误差：
Pk+1＝[I-Kk+1Hk+1]Pk+1|k
按照上述步骤进行循环更新，卡尔曼滤波器输出飞行器的俯仰角横滚角θ、机体系x轴上的线速度u和y轴上的线速度ν的估计。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710073287.4A CN107063248A (zh) | 2017-02-10 | 2017-02-10 | 基于旋翼转速的动力学模型辅助惯导的导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710073287.4A CN107063248A (zh) | 2017-02-10 | 2017-02-10 | 基于旋翼转速的动力学模型辅助惯导的导航方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107063248A true CN107063248A (zh) | 2017-08-18 |
Family
ID=59599216
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710073287.4A Pending CN107063248A (zh) | 2017-02-10 | 2017-02-10 | 基于旋翼转速的动力学模型辅助惯导的导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107063248A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108536163A (zh) * | 2018-03-15 | 2018-09-14 | 南京航空航天大学 | 一种单面结构环境下的动力学模型/激光雷达组合导航方法 |
| CN108592911A (zh) * | 2018-03-23 | 2018-09-28 | 南京航空航天大学 | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 |
| CN108827302A (zh) * | 2018-04-24 | 2018-11-16 | 大连理工大学 | 基于旋翼转速测量的多旋翼飞行器导航方法 |
| CN111964688A (zh) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | 结合无人机动力学模型和mems传感器的姿态估计方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
| CN103837151A (zh) * | 2014-03-05 | 2014-06-04 | 南京航空航天大学 | 一种四旋翼飞行器的气动模型辅助导航方法 |
| CN104335128A (zh) * | 2012-03-30 | 2015-02-04 | 鹦鹉股份有限公司 | 用于用侧风和加速度计偏差估计和补偿来控制多旋翼的旋翼无人机的方法 |
| CN105005099A (zh) * | 2015-07-03 | 2015-10-28 | 南京航空航天大学 | 一种基于捷联惯导与飞行控制系统的大气参数解算方法 |
| CN106043695A (zh) * | 2016-06-28 | 2016-10-26 | 辽宁壮龙无人机科技有限公司 | 一种油动多旋翼无人机定桨距变转速系统及控制技术 |
-
2017
- 2017-02-10 CN CN201710073287.4A patent/CN107063248A/zh active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104335128A (zh) * | 2012-03-30 | 2015-02-04 | 鹦鹉股份有限公司 | 用于用侧风和加速度计偏差估计和补偿来控制多旋翼的旋翼无人机的方法 |
| US20150057844A1 (en) * | 2012-03-30 | 2015-02-26 | Parrot | Method for controlling a multi-rotor rotary-wing drone, with cross wind and accelerometer bias estimation and compensation |
| CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
| CN103837151A (zh) * | 2014-03-05 | 2014-06-04 | 南京航空航天大学 | 一种四旋翼飞行器的气动模型辅助导航方法 |
| CN105005099A (zh) * | 2015-07-03 | 2015-10-28 | 南京航空航天大学 | 一种基于捷联惯导与飞行控制系统的大气参数解算方法 |
| CN106043695A (zh) * | 2016-06-28 | 2016-10-26 | 辽宁壮龙无人机科技有限公司 | 一种油动多旋翼无人机定桨距变转速系统及控制技术 |
Non-Patent Citations (4)
| Title |
|---|
| JOHN MACDONALD 等: "Analysis of an Improved IMU-Based Observer for Multirotor Helicopters", 《JOURNAL OF INTELLIGENT AND ROBOTIC SYSTEMS》 * |
| ROBERT C. LEISHMAN 等: "Quadrotors and Accelerometers: State Estimation with an Improved Dynamic Model", 《IEEE CONTROL SYSTEMS MAGAZINE》 * |
| WENJING CHEN 等: "An improved model aided navigation method with online dynamics parameters estimation for multirotor UAV", 《2016 IEEE CHINESE GUIDANCE, NAVIGATION AND CONTROL CONFERENCE》 * |
| 徐国祥: "《统计预测和决策 第5版》", 31 January 2016, 上海财经大学出版社 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108536163A (zh) * | 2018-03-15 | 2018-09-14 | 南京航空航天大学 | 一种单面结构环境下的动力学模型/激光雷达组合导航方法 |
| CN108536163B (zh) * | 2018-03-15 | 2021-04-16 | 南京航空航天大学 | 一种单面结构环境下的动力学模型/激光雷达组合导航方法 |
| CN108592911A (zh) * | 2018-03-23 | 2018-09-28 | 南京航空航天大学 | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 |
| CN108827302A (zh) * | 2018-04-24 | 2018-11-16 | 大连理工大学 | 基于旋翼转速测量的多旋翼飞行器导航方法 |
| CN111964688A (zh) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | 结合无人机动力学模型和mems传感器的姿态估计方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Bryson et al. | Vehicle model aided inertial navigation for a UAV using low-cost sensors | |
| Johansen et al. | On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors | |
| CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
| CN106643737B (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
| CN111351482B (zh) | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 | |
| CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
| CN104503466B (zh) | 一种微小型无人机导航装置 | |
| WO2020220729A1 (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
| CN107101636B (zh) | 一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法 | |
| De Marina et al. | Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows | |
| Mahony et al. | A non-linear observer for attitude estimation of a fixed-wing unmanned aerial vehicle without GPS measurements | |
| Rodić et al. | Modeling and simulation of quad-rotor dynamics and spatial navigation | |
| CN105094138A (zh) | 一种用于旋翼无人机的低空自主导航系统 | |
| Wenz et al. | Moving horizon estimation of air data parameters for UAVs | |
| CN106885918B (zh) | 一种面向多旋翼飞行器的多信息融合实时风速估计方法 | |
| CN107063248A (zh) | 基于旋翼转速的动力学模型辅助惯导的导航方法 | |
| CN108592911B (zh) | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 | |
| CN111964688B (zh) | 结合无人机动力学模型和mems传感器的姿态估计方法 | |
| CN108759814B (zh) | 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 | |
| CN108562289A (zh) | 连续多边几何环境中四旋翼飞行器激光雷达导航方法 | |
| CN107111321A (zh) | 控制方法、控制装置、飞行控制系统与多旋翼无人机 | |
| US10876920B1 (en) | Auxiliary aerial vehicles for flow characterization | |
| Hua | Contributions to the automatic control of aerial vehicles | |
| CN108693372A (zh) | 一种四旋翼飞行器的航向轴角速度估计方法 | |
| Tian et al. | UAV flight test evaluation of fusion algorithms for estimation of angle of attack and sideslip angle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20170818 |
|
| RJ01 | Rejection of invention patent application after publication |