CN111964688B - 结合无人机动力学模型和mems传感器的姿态估计方法 - Google Patents

结合无人机动力学模型和mems传感器的姿态估计方法 Download PDF

Info

Publication number
CN111964688B
CN111964688B CN202010660035.3A CN202010660035A CN111964688B CN 111964688 B CN111964688 B CN 111964688B CN 202010660035 A CN202010660035 A CN 202010660035A CN 111964688 B CN111964688 B CN 111964688B
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
axis
equation
matrix
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.)
Active
Application number
CN202010660035.3A
Other languages
English (en)
Other versions
CN111964688A (zh
Inventor
王学运
张京娟
徐一钒
张谦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202010660035.3A priority Critical patent/CN111964688B/zh
Publication of CN111964688A publication Critical patent/CN111964688A/zh
Application granted granted Critical
Publication of CN111964688B publication Critical patent/CN111964688B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • G01C23/005—Flight directors
    • 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/165—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 combined with non-inertial navigation instruments
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于无人机导航领域，特别提出一种结合无人机动力学模型和MEMS传感器的姿态估计方法，以有效提高无人机在大机动运动时的姿态估计精度。在GPS信号有效时，本发明利用GPS/MEMS组合导航信息对无人机动力学模型参数进行在线估计，给出了修正后的无人机动力学模型；在GPS失效后，利用修正后动力学模型对MEMS传感器中陀螺仪数据滤波以及估计无人机的运动加速度，并结合MEMS传感器进行卡尔曼滤波得到无人机系统的姿态估计。本发明能够补偿无人机大机动带来的运动加速度对加速度计测量重力的影响，适用于在GPS失效时对无人机姿态估计精度要求较高的应用场合。

Description

结合无人机动力学模型和MEMS传感器的姿态估计方法
技术领域
本发明属于无人机导航领域，特别涉及一种利用无人机动力学模型辅助MEMS传感器进行姿态解算的方法，能够补偿无人机大机动带来的运动加速度对加速度计测量重力的影响，适用于在GPS失效时对无人机姿态估计精度要求较高的应用场合。
背景技术
无人机导航技术是无人机系统实现自主飞行的关键技术之一，无人机路径规划、实时避障、精准着陆和飞行控制等功能的实现需要导航模块提供持续全面的导航信息。无人机常用的导航技术主要包括惯性导航、GPS导航、视觉导航、无线电导航和地形辅助导航等。
这些导航技术各有优缺点，考虑到传感器价格、重量体积以及带宽等因素限制，一般中小型无人机导航模块利用GPS/MEMS传感器进行组合导航来为无人机其它模块提供可靠的导航信息反馈。GPS受使用环境范围影响，例如在室内和桥梁等地区信号容易中断，不能完全依靠；MEMS传感器虽然价格便宜，但具有精度低、误差大以及导航结果短时间迅速发散等缺点。因此在GPS信号不可用情况下，需要其他导航信息源对MEMS传感器进行辅助，得到无人机可靠的导航信息估计。
目前，部分文献采用基于MEMS传感器的航姿参考系统来为无人机提供三维姿态估计，但由于将对重力的测量近似用加速度计输出来代替，因此航姿参考系统在无人机进行大机动运动时会出现失效情况。另外部分文献提出将飞机动力学模型、GPS和MEMS传感器组合的方式进行导航，但飞机动力学模型的建立主要依靠软件仿真计算或者大量风洞试验离线得到，存在耗时耗力和在线模型参数不准确的问题。目前对在线估计动力学模型参数以及动力学模型如何辅助MEMS传感器进行导航研究较少。
发明内容
为解决无人机动力学模型在线参数辨识和动力学模型/MEMS传感器融合的无人机姿态估计问题，本发明提供一种利用无人机动力学模型辅助MEMS传感器进行姿态估计的方法，以有效提高无人机在大机动运动时的姿态估计精度。在GPS信号有效时，本发明利用GPS/MEMS组合导航信息对无人机动力学模型参数进行在线估计，给出了修正后的无人机动力学模型；在GPS失效后，利用修正后动力学模型对陀螺仪数据滤波以及估计无人机的运动加速度，并结合MEMS传感器进行卡尔曼滤波得到无人机系统的姿态估计。
具体地，本发明提供了一种结合无人机动力学模型和MEMS传感器的姿态估计方法，包括如下步骤：
步骤一：在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，根据估计得到的电机拉力系数和三轴常值干扰力矩修正无人机动力学模型；
步骤二：在GPS信号失效时，利用磁力计数据计算并补偿磁偏角后得到磁航向角，利用雷达高度、气压高度和真实高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量；
步骤三：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，进而构造重力加速度补偿量，得到重力加速度观测的俯仰角和横滚角；
步骤四：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的转动动力学方程对MEMS传感器的测量数据滤波，以抑制无人机机体震动带来数据噪声和异常值；
步骤五：在GPS信号失效时，根据步骤二到步骤四得到的无人机的俯仰角和横滚角的余弦乘积量、重力加速度观测的俯仰角和横滚角、滤波后的MEMS传感器的测量数据构建卡尔曼滤波器进行无人机系统的姿态估计。
进一步，步骤一中，在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，具体过程如下：
下式(1)给出了估计电机拉力系数和三轴常值干扰力矩的状态量
Figure BDA0002578149890000031
和非线性状态方程：
Figure BDA0002578149890000032
式中，p，q，r分别为无人机横滚、俯仰、偏航三个方向上的角速度；Mx,My,Mz为三轴力矩；
Figure BDA0002578149890000033
为无人机三轴干扰力矩；β为电机拉力系数；ci,i∈{1,2,…9}为与无人机转动惯量有关的系数；ξp,ξq,ξr分别为三轴角速度的过程噪声；
Figure BDA0002578149890000034
ξβ为三轴干扰力矩和电机拉力系数的过程噪声；Fd(X,t)为非线性状态方程，Wd(t)为无人机系统噪声矩阵；上标·表示一阶求导；上标T表示转置矩阵；
对式(1)求雅克比矩阵得到线性化状态方程：
Figure BDA0002578149890000035
式中，Fd(t)是线性化的状态转移矩阵；Ud(t)为无人机系统输入；Wd(t)为无人机系统噪声矩阵；
估计电机拉力系数和三轴常值干扰力矩的量测量为
Figure BDA0002578149890000041
以及非线性量测方程为：
Figure BDA0002578149890000042
式中，ωx，ωy，ωz为补偿过零偏的三轴陀螺仪输出；
Figure BDA0002578149890000043
为Z轴运动加速度；φ，θ分别为GPS/MEMS组合导航得到的横滚角和俯仰角；Dj,j∈{1,2,…n}为电机收到的PWM占空比信号，n为无人机电机数量；m为无人机总质量；
Figure BDA0002578149890000044
为三轴陀螺仪输出量测噪声；
Figure BDA0002578149890000045
为Z轴运动加速度计量测噪声；Vd(t)为量测噪声矩阵；g为当地重力加速度；H(Xd,t)为非线性量测方程；
对式(3)求雅克比矩阵进行线性化：
Figure BDA0002578149890000046
式中，Hd(t)为线性化后的量测矩阵；
对式(2)和(4)进行离散化得到离散模型(5)：
Figure BDA0002578149890000047
式中，Xk为当前时刻状态量；Xk-1为上一时刻状态量；Φk,k-1为一步状态转移矩阵；Γk-1为系统噪声转移矩阵；Wk-1为系统噪声；Zk为当前时刻量测量；Hk为当前时刻量测矩阵；Vk为量测噪声阵；
利用离散模型(5)估计出电机拉力系数和三轴常值干扰力矩。
进一步，步骤二具体过程如下：
利用磁力计数据计算并补偿磁偏角后得到磁航向角ψm：
Figure BDA0002578149890000051
式中，
Figure BDA0002578149890000052
为三轴磁力计输出；ψ0为补偿的磁偏角；
雷达高度、气压高度和真实高度之间的关系表示为：
Figure BDA0002578149890000053
式中，Hlidar,Hair,PZe分别为雷达高度、气压高度和真实高度；nlidar,nair分别为雷达高度和气压高度的测量噪声；
改写式(7)构造无人机的俯仰角和横滚角的余弦乘积量：
Figure BDA0002578149890000054
进一步，步骤三具体过程如下：
利用修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，补偿加速度计输出后得到三轴重力加速度，按下式(9)计算重力加速度观测的俯仰角和横滚角：
Figure BDA0002578149890000055
式中，φa，θa分别为重力加速度观测的俯仰角和横滚角；
Figure BDA0002578149890000056
为无人机三轴加速度计输出；
Figure BDA0002578149890000057
分别为X轴、Y轴和Z轴运动加速度。
进一步，步骤五具体过程如下：
下式(10)给出了估计无人机姿态角的状态量为
Figure BDA0002578149890000058
和状态方程：
Figure BDA0002578149890000061
式中，q0,q1,q2,q3为姿态表示的四元素；
Figure BDA0002578149890000062
为无人机三轴角速度；Δgx,Δgy,Δgz为相邻时刻的陀螺仪输出的差值；Fa(t)为线性化的状态转移矩阵，Ua(t)为无人机系统输入，Wa(t)为无人机系统噪声矩阵；
估计无人机姿态角的量测方程由式(11)给出：
Figure BDA0002578149890000063
式中，Za(t)为估计无人机姿态角的量测量；pv,qv,rv为转动动力学方程计算的三轴角速度；Hair/Hlidar是气压高度和雷达高度比值；
通过式(10)和(11)并结合卡尔曼滤波方程计算无人机姿态角。
本发明的有益效果：
1)本发明能准确快速估计(估计值收敛时间在0.025s内)出无人机电机拉力系数和三轴干扰力矩，避免了软件仿真计算或者大量风洞试验；
2)本发明能有效提高无人机姿态估计精度，使估计精度在1°内。
附图说明
图1为本发明的结合无人机动力学模型和MEMS传感器的姿态估计方法流程图；
图2为本发明实施例的无人机动力学模型参数中三轴常值干扰力矩和电机拉力系数估计结果图；
图3为本发明实施例的无人机利用磁力计数据计算得到的磁航向角和利用雷达高度、气压高度和真实高度雷达高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量结果图；
图4为本发明实施例的利用平动动力学模型估计运动加速度结果图；
图5为本发明实施例的补偿运动加速度后的重力观测得到的俯仰角和横滚角以及加速度计直接计算得到俯仰角和横滚角结果图；
图6为本发明实施例的利用转动动力学方程对MEMS传感器中陀螺仪测量值滤波结果图；
图7为本发明实施例的构建的卡尔曼滤波器进行无人机系统的姿态估计结果图。
具体实施方式
下面结合附图和实施例进一步描述本发明，应该理解，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。
如图1所示，本发明的结合无人机动力学模型和MEMS传感器的姿态估计方法，包括如下步骤：
步骤一：在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，根据估计得到的电机拉力系数和三轴常值干扰力矩修正无人机动力学模型。
下式(1)给出了估计电机拉力系数和三轴常值干扰力矩的状态量
Figure BDA0002578149890000071
和非线性状态方程：
Figure BDA0002578149890000072
式中，p，q，r分别为无人机横滚、俯仰、偏航三个方向上的角速度；Mx,My,Mz为三轴力矩；
Figure BDA0002578149890000081
为无人机三轴干扰力矩；β为电机拉力系数；ci,i∈{1,2,…9}为与无人机转动惯量有关的系数；ξp,ξq,ξr分别为三轴角速度的过程噪声；
Figure BDA0002578149890000082
ξβ为三轴干扰力矩和电机拉力系数的过程噪声；Fd(X,t)为非线性状态方程，Wd(t)为无人机系统噪声矩阵；上标·表示一阶求导；上标T表示转置矩阵。
对式(1)求雅克比矩阵得到线性化状态方程：
Figure BDA0002578149890000083
式中，Fd(t)是线性化的状态转移矩阵；Ud(t)为无人机系统输入；Wd(t)为无人机系统噪声矩阵。
估计电机拉力系数和三轴常值干扰力矩的量测量为
Figure BDA0002578149890000084
以及非线性量测方程为：
Figure BDA0002578149890000085
式中，ωx，ωy，ωz为补偿过零偏的三轴陀螺仪输出；
Figure BDA0002578149890000091
为Z轴运动加速度；φ，θ分别为GPS/MEMS组合导航得到的横滚角和俯仰角；Dj,j∈{1,2,…n}为电机收到的PWM占空比信号，n为无人机电机数量；m为无人机总质量；
Figure BDA0002578149890000092
为三轴陀螺仪输出量测噪声；
Figure BDA0002578149890000093
为Z轴运动加速度计量测噪声；Vd(t)为量测噪声矩阵；g为当地重力加速度；H(Xd,t)为非线性量测方程。
对式(3)求雅克比矩阵得到线性化后量测矩阵为：
Figure BDA0002578149890000094
式中，Hd(t)为线性化后的量测矩阵。
对式(2)和(4)进行离散化得到离散模型(5)：
Figure BDA0002578149890000095
式中，Xk为当前时刻状态量；Xk-1为上一时刻状态量；Φk,k-1为一步状态转移矩阵；Γk-1为系统噪声转移矩阵；Wk-1为系统噪声；Zk为当前时刻量测量；Hk为当前时刻量测矩阵；Vk为量测噪声阵；
利用离散模型(5)估计出电机拉力系数和三轴常值干扰力矩。
图2给出了本实施例的无人机动力学模型参数中电机拉力系数和三轴常值干扰力矩估计结果，估计方法由公式(1)—(5)给出，应该理解，本实施例是一个飞行仿真过程，中途采集了很多数据，利用这些数据循环估计得到的，这些数据都是随时间变化的。从结果可以看出，本发明能够准确估计出上述模型参数，估计出的电机拉力系数在1.1×10^-5左右，无人机三轴常值干扰力矩分别在0.1Nm，-0.15Nm和0.05Nm附近。
步骤二：在GPS信号失效时，利用磁力计数据计算并补偿磁偏角后得到磁航向角，利用雷达高度、气压高度和真实高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量。
利用磁力计数据计算并补偿磁偏角后得到磁航向角ψm：
Figure BDA0002578149890000101
式中，
Figure BDA0002578149890000102
为三轴磁力计输出；ψ0为补偿的磁偏角；
雷达高度、气压高度和真实高度之间的关系表示为：
Figure BDA0002578149890000103
式中，Hlidar,Hair,PZe分别为雷达高度、气压高度和真实高度；nlidar,nair分别为雷达高度和气压高度的测量噪声；
改写式(7)构造无人机的俯仰角和横滚角的余弦乘积量：
Figure BDA0002578149890000104
图3给出了本实施例无人机利用磁力计数据计算得到的磁航向角和利用雷达高度、气压高度和真实高度雷达高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量结果，计算方法由公式(6)-(8)给出，整个飞行过程中保持无人机机头方向指向正北，磁航向角基本在0°附近；在30s和40s时无人机进行姿态机动，引起气压计和雷达高度测量不一致，导致俯仰角和横滚角的余弦乘积量发生变化。
步骤三：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，进而构造重力加速度补偿量，得到重力加速度观测的俯仰角和横滚角。
利用电机拉力系数估计得到无人机运动加速度，补偿加速度计输出后得到三轴重力加速度，按下式(9)计算重力加速度观测的俯仰角和横滚角：
Figure BDA0002578149890000105
式中，φa，θa分别为重力加速度观测的俯仰角和横滚角；
Figure BDA0002578149890000106
为无人机三轴加速度计输出；
Figure BDA0002578149890000107
分别为X轴、Y轴和Z轴运动加速度。
图4给出了本实施例中利用平动动力学模型估计运动加速度结果，在30s和40s附近无人机进行姿态机动，产生运动加速度进而改变速度和位置。X轴和Y轴运动加速度在±2.5m/s^2内。
图5给出了本实施例补偿运动加速度后的重力观测得到的俯仰角和横滚角以及加速度计直接计算得到俯仰角和横滚角，计算方法由公式(9)给出。加速度计测量是运动加速度和重力加速度之和，由于存在运动加速度成分，加速度计直接计算得到的俯仰角和横滚角会存在较大误差，而补偿图4中的运动加速度后，可见由重力加速度计算的俯仰角和横滚角更准确。
步骤四：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的转动动力学方程对MEMS传感器的测量数据滤波，以抑制无人机机体震动带来数据噪声和异常值。
图6给出了本实施例利用转动动力学方程对MEMS传感器中陀螺仪测量值滤波结果。图中可见由于机体震动或者传感器故障会导致陀螺仪输出故障，产生不正常“尖刺”情况；而滤波后数据比较平滑，没有大的“尖刺”出现。
步骤五：在GPS信号失效时，根据步骤二到步骤四得到的无人机的俯仰角和横滚角的余弦乘积量、重力加速度观测的俯仰角和横滚角、滤波后的MEMS传感器的测量数据构建卡尔曼滤波器进行无人机系统的姿态估计。
下式(10)给出了估计无人机姿态角的状态量为
Figure BDA0002578149890000111
和状态方程：
Figure BDA0002578149890000112
式中，q0,q1,q2,q3为姿态表示的四元素；
Figure BDA0002578149890000113
为无人机三轴角速度；Δgx,Δgy,Δgz为相邻时刻的陀螺仪输出的差值；Fa(t)为线性化的状态转移矩阵，Ua(t)为无人机系统输入，Wa(t)为无人机系统噪声矩阵。
估计无人机姿态角的量测方程由式(11)给出：
Figure BDA0002578149890000121
式中，Za(t)为估计无人机姿态角的量测量；pv,qv,rv为转动动力学方程计算的三轴角速度；Hair/Hlidar是气压高度和雷达高度比值；
通过式(10)和(11)并结合卡尔曼滤波方程计算无人机姿态角。
图7给出了本发明中在GPS失效情况下，利用上述得到的磁航向角、无人机的俯仰角和横滚角的余弦乘积量、重力加速度观测的俯仰角和横滚角、滤波后的MEMS传感器的测量数据，构建卡尔曼滤波器进行无人机系统的姿态估计结果，由公式(10)-(11)给出。经过本发明提出的算法估计得到姿态角和真实值比较接近，误差在1°以内，证明本发明在GPS失效情况下也能有效使用。
对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例作出若干变型和改进，这些都属于本发明的保护范围。

Claims (4)

1.一种结合无人机动力学模型和MEMS传感器的姿态估计方法，其特征在于，包括如下步骤：
步骤一：在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，根据估计得到的电机拉力系数和三轴常值干扰力矩修正无人机动力学模型；
步骤二：在GPS信号失效时，利用磁力计数据计算并补偿磁偏角后得到磁航向角，利用雷达高度、气压高度和真实高度的冗余关系构造无人机的俯仰角和横滚角的余弦乘积量；
步骤三：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，进而构造重力加速度补偿量，得到重力加速度观测的俯仰角和横滚角；
步骤四：在GPS信号失效时，利用步骤一中修正后的无人机动力学模型的转动动力学方程对MEMS传感器的测量数据滤波，以抑制无人机机体震动带来数据噪声和异常值；
步骤五：在GPS信号失效时，根据步骤二到步骤四得到的无人机的俯仰角和横滚角的余弦乘积量、重力加速度观测的俯仰角和横滚角、滤波后的MEMS传感器的测量数据构建卡尔曼滤波器进行无人机系统的姿态估计；
步骤一中，在GPS信号有效时，利用GPS/MEMS组合导航结果对无人机动力学模型的电机拉力系数和三轴常值干扰力矩进行估计，具体过程如下：
下式(1)给出了估计电机拉力系数和三轴常值干扰力矩的状态量
Figure FDA0003457478450000011
和非线性状态方程：
Figure FDA0003457478450000012
式中，p，q，r分别为无人机横滚、俯仰、偏航三个方向上的角速度；Mx,My,Mz为三轴力矩；
Figure FDA0003457478450000021
为无人机三轴干扰力矩；β为电机拉力系数；ci,i∈{1,2,…9}为与无人机转动惯量有关的系数；ξp,ξq,ξr分别为三轴角速度的过程噪声；
Figure FDA0003457478450000022
为三轴干扰力矩和电机拉力系数的过程噪声；Fd(X,t)为非线性状态方程，Wd(t)为无人机系统噪声矩阵；上标·表示一阶求导；上标T表示转置矩阵；
对式(1)求雅克比矩阵得到线性化状态方程：
Figure FDA0003457478450000023
式中，Fd(t)是线性化的状态转移矩阵；Ud(t)为无人机系统输入；Wd(t)为无人机系统噪声矩阵；
估计电机拉力系数和三轴常值干扰力矩的量测量为
Figure FDA0003457478450000024
以及非线性量测方程为：
Figure FDA0003457478450000025
式中，ωx，ωy，ωz为补偿过零偏的三轴陀螺仪输出；
Figure FDA0003457478450000031
为Z轴运动加速度；φ，θ分别为GPS/MEMS组合导航得到的横滚角和俯仰角；Dj,j∈{1,2,…n}为电机收到的PWM占空比信号，n为无人机电机数量；m为无人机总质量；
Figure FDA0003457478450000032
为三轴陀螺仪输出量测噪声；
Figure FDA0003457478450000033
为Z轴运动加速度计量测噪声；Vd(t)为量测噪声矩阵；g为当地重力加速度；H(Xd,t)为非线性量测方程；
对式(3)求雅克比矩阵进行线性化：
Figure FDA0003457478450000034
式中，Hd(t)为线性化后的量测矩阵；
对式(2)和(4)进行离散化得到离散模型(5)：
Figure FDA0003457478450000035
式中，Xk为当前时刻状态量；Xk-1为上一时刻状态量；Φk,k-1为一步状态转移矩阵；Γk-1为系统噪声转移矩阵；Wk-1为系统噪声；Zk为当前时刻量测量；Hk为当前时刻量测矩阵；Vk为量测噪声阵；
利用离散模型(5)估计出电机拉力系数和三轴常值干扰力矩。
2.根据权利要求1所述的方法，其特征在于，步骤二具体过程如下：
利用磁力计数据计算并补偿磁偏角后得到磁航向角ψm：
Figure FDA0003457478450000036
式中，
Figure FDA0003457478450000037
为三轴磁力计输出；ψ0为补偿的磁偏角；
雷达高度、气压高度和真实高度之间的关系表示为：
Figure FDA0003457478450000041
式中，Hlidar,Hair,PZe分别为雷达高度、气压高度和真实高度；nlidar,nair分别为雷达高度和气压高度的测量噪声；
改写式(7)构造无人机的俯仰角和横滚角的余弦乘积量：
Figure FDA0003457478450000042
3.根据权利要求2所述的方法，其特征在于，步骤三具体过程如下：
利用修正后的无人机动力学模型的平动动力学方程估计无人机运动加速度，补偿加速度计输出后得到三轴重力加速度，按下式(9)计算重力加速度观测的俯仰角和横滚角：
Figure FDA0003457478450000043
式中，φa，θa分别为重力加速度观测的俯仰角和横滚角；
Figure FDA0003457478450000044
为无人机三轴加速度计输出；
Figure FDA0003457478450000045
分别为X轴、Y轴和Z轴运动加速度。
4.根据权利要求3所述的方法，其特征在于，步骤五具体过程如下：
下式(10)给出了估计无人机姿态角的状态量为
Figure FDA0003457478450000046
和状态方程：
Figure FDA0003457478450000047
式中，q0,q1,q2,q3为姿态表示的四元数；
Figure FDA0003457478450000048
为无人机三轴角速度；Δgx,Δgy,Δgz为相邻时刻的陀螺仪输出的差值；Fa(t)为线性化的状态转移矩阵，Ua(t)为无人机系统输入，Wa(t)为无人机系统噪声矩阵；
估计无人机姿态角的量测方程由式(11)给出：
Figure FDA0003457478450000051
式中，Za(t)为估计无人机姿态角的量测量；pv,qv,rv为转动动力学方程计算的三轴角速度；Hair/Hlidar是气压高度和雷达高度比值；
通过式(10)和(11)并结合卡尔曼滤波方程计算无人机姿态角。
CN202010660035.3A 2020-07-10 2020-07-10 结合无人机动力学模型和mems传感器的姿态估计方法 Active CN111964688B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010660035.3A CN111964688B (zh) 2020-07-10 2020-07-10 结合无人机动力学模型和mems传感器的姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010660035.3A CN111964688B (zh) 2020-07-10 2020-07-10 结合无人机动力学模型和mems传感器的姿态估计方法

Publications (2)

Publication Number Publication Date
CN111964688A CN111964688A (zh) 2020-11-20
CN111964688B true CN111964688B (zh) 2022-04-12

Family

ID=73361632

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010660035.3A Active CN111964688B (zh) 2020-07-10 2020-07-10 结合无人机动力学模型和mems传感器的姿态估计方法

Country Status (1)

Country Link
CN (1) CN111964688B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113607167B (zh) * 2021-03-15 2024-07-05 南京航空航天大学 一种用于航姿系统的自适应姿态估计方法及其平滑切换算法
CN113778120B (zh) * 2021-10-27 2024-05-17 北京航空航天大学 一种多传感器融合的无人机复杂天候飞行控制方法
CN114509071B (zh) * 2022-04-20 2022-07-08 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法
CN116301009B (zh) * 2023-05-25 2023-08-22 北京航空航天大学杭州创新研究院 一种变构型无人机的抗干扰安全控制方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106568449B (zh) * 2016-09-06 2019-04-30 北京理工大学 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法
CN107063248A (zh) * 2017-02-10 2017-08-18 南京航空航天大学 基于旋翼转速的动力学模型辅助惯导的导航方法
US10969228B2 (en) * 2018-06-05 2021-04-06 Novatel Inc. Relative position navigation system for multiple moving vehicles
CN109916395B (zh) * 2019-04-04 2023-06-23 山东智翼航空科技有限公司 一种姿态自主冗余组合导航算法
CN111152795B (zh) * 2020-01-08 2022-12-13 东南大学 一种基于模型和参数动态调整的自适应车辆状态预测系统及预测方法

Also Published As

Publication number Publication date
CN111964688A (zh) 2020-11-20

Similar Documents

Publication Publication Date Title
CN111964688B (zh) 结合无人机动力学模型和mems传感器的姿态估计方法
CN111351482B (zh) 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN103837151B (zh) 一种四旋翼飞行器的气动模型辅助导航方法
CN112630813B (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN102809377A (zh) 飞行器惯性/气动模型组合导航方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN104898681A (zh) 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
Leutenegger et al. Robust state estimation for small unmanned airplanes
CN112945225A (zh) 基于扩展卡尔曼滤波的姿态解算系统及解算方法
CN108592911B (zh) 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN109689499A (zh) 用于稳健干扰抑制的分布式加速度感测
CN111207745B (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN111880410B (zh) 一种针对电机故障的四旋翼无人机容错控制方法
Borup et al. A nonlinear model-based wind velocity observer for unmanned aerial vehicles
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN104215262A (zh) 一种惯性导航系统惯性传感器误差在线动态辨识方法
CN105929836A (zh) 用于四旋翼飞行器的控制方法
CN113295162A (zh) 基于无人机状态信息的广义因子图融合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN108759814A (zh) 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法
CN107063248A (zh) 基于旋翼转速的动力学模型辅助惯导的导航方法
Tian et al. UAV flight test evaluation of fusion algorithms for estimation of angle of attack and sideslip angle
RU2373562C2 (ru) Способ и устройство контроля горизонтальной ориентации аппарата
Nakanishi et al. Measurement model of barometer in ground effect of unmanned helicopter and its application to estimate terrain clearance

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
GR01 Patent grant
GR01 Patent grant