CN107607112A - 飞行器用低成本位姿测量装置及测量方法 - Google Patents
飞行器用低成本位姿测量装置及测量方法 Download PDFInfo
- Publication number
- CN107607112A CN107607112A CN201710820684.3A CN201710820684A CN107607112A CN 107607112 A CN107607112 A CN 107607112A CN 201710820684 A CN201710820684 A CN 201710820684A CN 107607112 A CN107607112 A CN 107607112A
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- mtd
- mtr
- msubsup
- 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
Landscapes
- Gyroscopes (AREA)
Abstract
本发明提供了一种飞行器用低成本位姿测量装置及测量方法，属于飞行器用位姿测量装置与方法技术领域。测量装置包括安装面板、加速度计、陀螺和磁力计，所述加速度计固定在安装面板上的一侧，磁力计固定在安装面板上的另一侧，陀螺安装在加速度计和磁力计之间的安装面板上。方法：将三轴加速度计、三轴磁力计和三轴陀螺安装到面板上；在装置工作前，首先要完成姿态角的初对准；根据得到的初始姿态角得到初始姿态矩阵T，然后根据初始姿态矩阵得到姿态四元数初始值；获取加速度计，磁力计，陀螺测量得到9轴数据，采用改进互补滤波器进行数据融合；限幅环节幅值的设定；四元数更新采用龙格—库塔算法，根据更新得到的四元数，从而得到捷联矩阵和姿态角。
Description
技术领域
本发明涉及一种飞行器用低成本位姿测量装置及测量方法，属于飞行器用位姿测量装置与方法技术领域。
背景技术
现有的导航系统存在以下问题：
(1)配置精度较高的激光陀螺的惯性导航系统，虽然测得姿态角精度很高，但是成本过高，不适用于低成本飞行器中，且对于只需要测量姿态角，而不需要其他导航信息的情况下，使用传统惯性导航系统是一种资源的浪费。
(2)航姿参考系统(AHRS)成本低、体积小、功耗低，常常用于无人机飞控系统中，用于提供实时的航向姿态。AHRS中使用加速度计和磁力计测量的数据校正MEMS陀螺仪更新的姿态角，在9轴数据融合时，最常用的滤波方法就是互补滤波和kalman滤波，由于互补滤波运算量小，一般采用较为简单的互补滤波器进行数据融合。但是互补滤波器不能够根据飞行器实际的机动情况来自动调整滤波系数，因此滤波效果并不好，尤其在飞行器做高动态的运动时，加速度计测量数据会很不稳定，严重影响姿态角的估计结果，而且收敛很慢，甚至会出现超调。
(3)由于利用天文导航确定姿态用不到陀螺，因此不会因为陀螺的漂移问题而产生误差，但是要应用到星敏感器，成本大大提高，数据刷新率低，由于其成本、数据刷新频率以及气象要求使其应用场合受到限制。
发明内容
本发明的目的是为了解决上述现有技术存在的问题，进而提供一种飞行器用低成本位姿测量装置及测量方法。
本发明的目的是通过以下技术方案实现的：
一种飞行器用低成本位姿测量装置，包括：安装面板、加速度计、陀螺和磁力计，所述加速度计固定在安装面板上的一侧，磁力计固定在安装面板上的另一侧，陀螺安装在加速度计和磁力计之间的安装面板上。
一种飞行器用低成本位姿测量方法，
步骤一、将三轴加速度计、三轴磁力计和三轴陀螺安装到面板上；
步骤二、在装置工作前，首先要完成姿态角的初对准，这时飞行器一般处于静态，根据加速度计和磁力计传感器的输出进行初始化，完成初对准过程，公式如下：
步骤三、根据得到的初始姿态角得到初始姿态矩阵T，然后根据初始姿态矩阵得到姿态四元数初始值，结果如下：
符号确定：
{sign(q1)＝sign(q0)[sign(4q1q0)]＝sign(q0)[sign(T32-T23)]
{sign(q2)＝sign(q0)[sign(4q2q0)]＝sign(q0)[sign(T13-T31)]
{sign(q3)＝sign(q0)[sign(4q3q0)]＝sign(q0)[sign(T21-T12)]
其中q0符号任选；
步骤四、获取加速度计，磁力计，陀螺测量得到9轴数据，采用改进互补滤波器进行数据融合；
根据改进卡尔曼滤波器结构可得到融合后的姿态角的微分方程为：
其中，ωg为陀螺仪测量角速度值，θa为加速度计(磁力计)测量角度值。
步骤五、限幅环节幅值的设定：在选择限幅环节时，采用实验的方法：
对飞行器采取直线加速，转弯的机动，并且不断提高加速度，当姿态估计出现明显超调时，以此时PI控制器的输出作为限幅环节的幅值；
步骤六、四元数更新采用龙格—库塔算法，根据更新得到的四元数，从而得到捷联矩阵和姿态角；
四元数微分方程：
其中wx，wy，wz是经过改进互补滤波器校正后的，即是步骤3中的三个分量；
根据四元数得到捷联矩阵T：
根据捷联矩阵更新得到姿态角为：
本发明的有益效果：
本发明对简单互补滤波器在结构上进行改进，提出一种新的基于PI反馈校正的互补滤波器，不仅提高了系统的型别，使低通阻带的衰减速度更快，另外，针对不同的运动状态下动态调整补偿系数。同时在滤波器结构中加入一个限幅环节，增强滤波稳定性，特别在高动态的情况下，使估计结果快速收敛，没有超调，估计精度也显著提高。
附图说明
图1为本发明飞行器用低成本位姿测量装置的结构示意图。
图2为改进的互补滤波器算法框图。
图中的附图标记，1为安装面板，2为加速度计，3为陀螺，4为磁力计。
具体实施方式
下面将结合附图对本发明做进一步的详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，但本发明的保护范围不限于下述实施例。
如图1所示，本实施例所涉及的一种飞行器用低成本位姿测量装置，包括：安装面板1、加速度计2、陀螺3和磁力计4，所述加速度计2固定在安装面板1上的一侧，磁力计4固定在安装面板1上的另一侧，陀螺3安装在加速度计2和磁力计4之间的安装面板1上。
一种飞行器用低成本位姿测量方法，步骤如下：
步骤一、将三轴加速度计、三轴磁力计和三轴陀螺安装到面板上，同时保证三者的X、Y和Z轴方向保持一致，且三者的距离应该不应过远。
步骤二、在装置工作前，首先要完成姿态角的初对准，这时飞行器一般处于静态，根据加速度计和磁力计传感器的输出进行初始化，完成初对准过程，公式如下：
步骤三、根据得到的初始姿态角得到初始姿态矩阵T，然后根据初始姿态矩阵得到姿态四元数初始值，结果如下：
符号确定：
{sign(q1)＝sign(q0)[sign(4q1q0)]＝sign(q0)[sign(T32-T23)]
{sign(q2)＝sign(q0)[sign(4q2q0)]＝sign(q0)[sign(T13-T31)]
{sign(q3)＝sign(q0)[sign(4q3q0)]＝sign(q0)[sign(T21-T12)]
其中q0符号任选。
步骤四、获取加速度计，磁力计，陀螺测量得到9轴数据，采用改进互补滤波器进行数据融合(如图2所示)。
根据改进卡尔曼滤波器结构可得到融合后的姿态角的微分方程为：
其中，ωg为陀螺仪测量角速度值，θa为加速度计(磁力计)测量角度值。
步骤五、限幅环节幅值的设定。在选择限幅环节时，可以采用实验的方法：
对飞行器采取直线加速，转弯的机动，并且不断提高加速度，当姿态估计出现明显超调时，应该以此时PI控制器的输出作为限幅环节的幅值。
步骤六、四元数更新采用龙格—库塔算法，根据更新得到的四元数，从而得到捷联矩阵和姿态角；
四元数微分方程：
其中wx，wy，wz是经过改进互补滤波器校正后的，即是步骤3中的三个分量。
根据四元数得到捷联矩阵T：
根据捷联矩阵更新得到姿态角为：
加速度计和磁力计静态特性好，能够长期稳定的输出测量数据，但动态特性较差，即在低频段动态响应特性好，在高频段动态响应特性差；微机械陀螺仪虽然动态特性较好，在短期内可提供高动态的姿态数据，但是温度漂移和零点漂移很大，所以在低频段动态响应特性差而高频段动态响应特性相对较好。于是，本实施例采用互补滤波器，对加速度计和磁力计的数据采用低通滤波器滤波，从而抑制了加速度计和磁力计的高频噪声，对陀螺仪测量数据采用高通滤波器，从而很好的抑制了陀螺仪的低频漂移。同时针对简单互补滤波器进行结构改进，增加PI反馈环节，以使滤波器根据飞行器机动情况自动调整补偿系数，低速状态下适当加大加速度计的数值权重，高动态情况时适当降低加速度计的权重，从而优化滤波结果。最后在PI控制器后面加一个限幅装置，防止在高动态时，滤波器出现不稳定和滤波结果出现超调的情况。
以上所述，仅为本发明较佳的具体实施方式，这些具体实施方式都是基于本发明整体构思下的不同实现方式，而且本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。
Claims (2)
1.一种飞行器用低成本位姿测量装置，其特征在于，包括：安装面板(1)、加速度计(2)、陀螺(3)和磁力计(4)，所述加速度计(2)固定在安装面板(1)上的一侧，磁力计(4)固定在安装面板(1)上的另一侧，陀螺(3)安装在加速度计(2)和磁力计(4)之间的安装面板(1)上。
2.一种利用权利要求1所述的飞行器用低成本位姿测量装置的测量方法，其特征在于，
步骤一、将三轴加速度计、三轴磁力计和三轴陀螺安装到面板上；
步骤二、在装置工作前，首先要完成姿态角的初对准，这时飞行器一般处于静态，根据加速度计和磁力计传感器的输出进行初始化，完成初对准过程，公式如下：
<mrow>
<mi>&theta;</mi>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msubsup>
<mi>g</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
<mo>/</mo>
<mi>g</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>&gamma;</mi>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>g</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
<mo>/</mo>
<mi>g</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>&psi;</mi>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mo>-</mo>
<mfrac>
<mrow>
<msubsup>
<mi>M</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&gamma;</mi>
<mo>-</mo>
<msubsup>
<mi>M</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
</mrow>
<mrow>
<msubsup>
<mi>M</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&theta;</mi>
<mo>+</mo>
<msubsup>
<mi>M</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&theta;</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
<mo>+</mo>
<msubsup>
<mi>M</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&theta;</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&gamma;</mi>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
步骤三、根据得到的初始姿态角得到初始姿态矩阵T，然后根据初始姿态矩阵得到姿态四元数初始值，结果如下：
<mrow>
<mi>T</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>sin</mi>
<mi>&gamma;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>cos</mi>
<mi>&gamma;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>sin</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&psi;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&psi;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mo>-</mo>
<mi>cos</mi>
<mi>&gamma;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi>&gamma;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mo>-</mo>
<mi>cos</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&gamma;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mo>|</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>|</mo>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msqrt>
<mrow>
<mn>1</mn>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>11</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>22</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>33</mn>
</msub>
</mrow>
</msqrt>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>|</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>|</mo>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msqrt>
<mrow>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>11</mn>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>22</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>33</mn>
</msub>
</mrow>
</msqrt>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>|</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>|</mo>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msqrt>
<mrow>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>11</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mn>22</mn>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>33</mn>
</msub>
</mrow>
</msqrt>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>|</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<mo>|</mo>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msqrt>
<mrow>
<mn>1</mn>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>11</mn>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>22</mn>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mn>33</mn>
</msub>
</mrow>
</msqrt>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
符号确定：
{sign(q1)＝sign(q0)[sign(4q1q0)]＝sign(q0)[sign(T32-T23)]
{sign(q2)＝sign(q0)[sign(4q2q0)]＝sign(q0)[sign(T13-T31)]
{sign(q3)＝sign(q0)[sign(4q3q0)]＝sign(q0)[sign(T21-T12)]
其中q0符号任选；
步骤四、获取加速度计，磁力计，陀螺测量得到9轴数据，采用改进互补滤波器进行数据融合；
根据改进卡尔曼滤波器结构可得到融合后的姿态角的微分方程为：
<mrow>
<mover>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<msub>
<mi>k</mi>
<mi>p</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>a</mi>
</msub>
<mo>-</mo>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>k</mi>
<mi>i</mi>
</msub>
<mo>&Integral;</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>a</mi>
</msub>
<mo>-</mo>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>g</mi>
</msub>
</mrow>
其中，ωg为陀螺仪测量角速度值，θa为加速度计(磁力计)测量角度值；
步骤五、限幅环节幅值的设定：在选择限幅环节时，采用实验的方法：对飞行器采取直线加速，转弯的机动，并且不断提高加速度，当姿态估计出现明显超调时，以此时PI控制器的输出作为限幅环节的幅值；
步骤六、四元数更新采用龙格—库塔算法，根据更新得到的四元数，从而得到捷联矩阵和姿态角；
四元数微分方程：
<mrow>
<msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>t</mi>
</msub>
<mo>+</mo>
<mfrac>
<mrow>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>x</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>y</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>z</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>x</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>y</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>z</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>x</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>y</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>z</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>w</mi>
<mi>x</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>y</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>z</mi>
</msub>
<mo>.</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中wx，wy，wz是经过改进互补滤波器校正后的，即是步骤3中的三个分量；
根据四元数得到捷联矩阵T：
<mrow>
<mi>T</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
根据捷联矩阵更新得到姿态角为：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mi>&theta;</mi>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>32</mn>
</msub>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&gamma;</mi>
<mo>=</mo>
<mi>arctan</mi>
<mo>(</mo>
<mo>-</mo>
<mfrac>
<msub>
<mi>T</mi>
<mn>31</mn>
</msub>
<msub>
<mi>T</mi>
<mn>33</mn>
</msub>
</mfrac>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&psi;</mi>
<mo>=</mo>
<mi>arctan</mi>
<mo>(</mo>
<mfrac>
<msub>
<mi>T</mi>
<mn>12</mn>
</msub>
<msub>
<mi>T</mi>
<mn>22</mn>
</msub>
</mfrac>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>.</mo>
</mrow>
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710820684.3A CN107607112A (zh) | 2017-09-13 | 2017-09-13 | 飞行器用低成本位姿测量装置及测量方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710820684.3A CN107607112A (zh) | 2017-09-13 | 2017-09-13 | 飞行器用低成本位姿测量装置及测量方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107607112A true CN107607112A (zh) | 2018-01-19 |
Family
ID=61063675
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710820684.3A Pending CN107607112A (zh) | 2017-09-13 | 2017-09-13 | 飞行器用低成本位姿测量装置及测量方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107607112A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109631939A (zh) * | 2018-11-08 | 2019-04-16 | 湖北三江航天红峰控制有限公司 | 一种基于磁强计和加速度计的快速对准方法 |
| CN111141283A (zh) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | 一种通过地磁数据判断行进方向的方法 |
| CN112327908A (zh) * | 2020-10-26 | 2021-02-05 | 上海航天控制技术研究所 | 一种适用于低舵效分离状态的稳定控制方法 |
| CN115683112A (zh) * | 2022-10-24 | 2023-02-03 | 中国航空工业集团公司洛阳电光设备研究所 | 一种基于互补滤波器的光电跟踪系统量化误差抑制方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101694389A (zh) * | 2009-10-20 | 2010-04-14 | 哈尔滨工程大学 | 无陀螺捷联惯导系统的初始姿态快速测量方法 |
| CN104235618A (zh) * | 2014-09-04 | 2014-12-24 | 哈尔滨工程大学 | 一种基于mems惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法 |
| CN104990533A (zh) * | 2015-06-22 | 2015-10-21 | 哈尔滨工业大学 | 卫星地面物理仿真系统超高精度姿态测量方法及装置 |
| CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航系统 |
| CN105607106A (zh) * | 2015-12-18 | 2016-05-25 | 重庆邮电大学 | 一种低成本高精度bd/mems融合姿态测量方法 |
| CN103900608B (zh) * | 2014-03-21 | 2016-08-17 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
| CN106094860A (zh) * | 2016-08-29 | 2016-11-09 | 广西师范大学 | 四旋翼飞行器及其控制方法 |
-
2017
- 2017-09-13 CN CN201710820684.3A patent/CN107607112A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101694389A (zh) * | 2009-10-20 | 2010-04-14 | 哈尔滨工程大学 | 无陀螺捷联惯导系统的初始姿态快速测量方法 |
| CN103900608B (zh) * | 2014-03-21 | 2016-08-17 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
| CN104235618A (zh) * | 2014-09-04 | 2014-12-24 | 哈尔滨工程大学 | 一种基于mems惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法 |
| CN104990533A (zh) * | 2015-06-22 | 2015-10-21 | 哈尔滨工业大学 | 卫星地面物理仿真系统超高精度姿态测量方法及装置 |
| CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航系统 |
| CN105607106A (zh) * | 2015-12-18 | 2016-05-25 | 重庆邮电大学 | 一种低成本高精度bd/mems融合姿态测量方法 |
| CN106094860A (zh) * | 2016-08-29 | 2016-11-09 | 广西师范大学 | 四旋翼飞行器及其控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| 李宪鹏: "面向自主巡航的多旋翼飞行器飞控系统设计", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109631939A (zh) * | 2018-11-08 | 2019-04-16 | 湖北三江航天红峰控制有限公司 | 一种基于磁强计和加速度计的快速对准方法 |
| CN111141283A (zh) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | 一种通过地磁数据判断行进方向的方法 |
| CN112327908A (zh) * | 2020-10-26 | 2021-02-05 | 上海航天控制技术研究所 | 一种适用于低舵效分离状态的稳定控制方法 |
| CN115683112A (zh) * | 2022-10-24 | 2023-02-03 | 中国航空工业集团公司洛阳电光设备研究所 | 一种基于互补滤波器的光电跟踪系统量化误差抑制方法 |
| CN115683112B (zh) * | 2022-10-24 | 2024-04-09 | 中国航空工业集团公司洛阳电光设备研究所 | 一种基于互补滤波器的光电跟踪系统量化误差抑制方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN1330935C (zh) | 安装误差角与标度因数解耦的微惯性测量单元精确标定方法 | |
| CN107607112A (zh) | 飞行器用低成本位姿测量装置及测量方法 | |
| CN102980577B (zh) | 一种微型捷联航姿系统及其工作方法 | |
| CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
| CN106482734A (zh) | 一种用于imu多传感器数据融合的滤波方法 | |
| CN104596546B (zh) | 一种单轴旋转惯导系统的姿态输出补偿方法 | |
| CN100541132C (zh) | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 | |
| CN103940425B (zh) | 一种磁‑惯性组合捷联测量方法 | |
| CN103900571B (zh) | 一种基于惯性坐标系旋转型捷联惯导系统的载体姿态测量方法 | |
| CN100547352C (zh) | 适合于光纤陀螺捷联惯性导航系统的地速检测方法 | |
| CN101629826A (zh) | 基于单轴旋转的光纤陀螺捷联惯性导航系统粗对准方法 | |
| CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
| CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
| CN103712622B (zh) | 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置 | |
| CN105628025B (zh) | 一种恒速偏频/机抖激光陀螺惯导系统导航方法 | |
| CN103712598B (zh) | 一种小型无人机姿态确定方法 | |
| CN201955092U (zh) | 一种基于地磁辅助的平台式惯性导航装置 | |
| CN103439727B (zh) | 一种地面坐标的测量方法 | |
| CN103471613A (zh) | 一种飞行器惯性导航系统参数仿真方法 | |
| CN105300381A (zh) | 一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法 | |
| CN103925930B (zh) | 一种重力仪双轴陀螺稳定平台航向误差效应的补偿方法 | |
| CN104864874B (zh) | 一种低成本单陀螺航位推算导航方法及系统 | |
| CN102680000A (zh) | 应用零速/航向修正的光纤捷联惯组在线标定方法 | |
| CN103743413A (zh) | 倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法 | |
| CN101183004A (zh) | 一种在线实时消除光纤陀螺捷联惯导系统振荡误差的方法 |
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: 20180119 |
|
| RJ01 | Rejection of invention patent application after publication |