2. 武汉科技大学冶金自动化与检测技术教育部工程研究中心, 湖北 武汉 430081
2. Engineering Research Center of Metallurgical automation and Measurement Technology, Ministry of Education, Wuhan University of Science and Technology, Wuhan 430081, China
1 引言
微型四旋翼飞行器 (quadrotor) 具有体积小、重量轻、易于隐蔽等优点，被广泛应用在农业、军工业维护和侦查探测上[1-2]，成为了目前无人飞行机器人 (unmanned aerial vehicle，UAV) 研究领域的热点问题.微型四旋翼飞行器的姿态解算速度和精度直接决定了四旋翼飞行器的飞行质量，研究具有快速、准确的姿态算法具有重要的理论和实践
意义.微机电系统 (micro-electro-mechanical systems，MEMS) 具有体积小、重量轻等优点成为载荷量小的微型四旋翼飞行器惯性测量的主要传感器.由于MEMS传感器中陀螺仪存在漂移积分累计误差[3]，必须采用加速度计进行误差修正，才能获得较为准确的姿态信息.但是在强机动、高速运动情况下，机体产生的运动加速度会导致加速度计测量精度降低进而引起姿态解算算法精度下降、动态性能变差.因此如何设计更加精确、快速的姿态解算[4-7]算法成为当前研究热点之一.
针对以上问题，国内外学者们研究后提出了多种姿态算法.文[8]针对陀螺仪的漂移误差提出了一种自适应互补滤波姿态估计算法，利用加速度计补偿陀螺仪偏差引起的姿态漂移误差.文[9]提出了利用卡尔曼滤波对陀螺仪漂移误差的补偿.但是以上两种算法均没有考虑到机体在强机动运动过程中产生的运动加速度造成姿态误差变大的问题.相比文[8]和文[9-10]考虑了加速度计的高频噪声，提出了一种自适应混合滤波算法，利用最优重力估计来对参数优化，消除了加速度的噪声影响，仍然没有对运动加速度进行处理.文[11]利用加速度计和陀螺仪提出了一种定步长的梯度下降姿态估计算法.文[12]在文[11]的基础上增加了磁力计，提出了一种基于梯度下降法的9轴姿态融合算法.这两种梯度下降算法中梯度步长为定值，当微型四旋翼飞行器处于高速运动时，梯度步长过小导致算法动态性能差.文[13]基于四元数的卡尔曼滤波提出了一种三子样姿态测量模型，将地磁信息融合进卡尔曼滤波姿态估计模型，提高了姿态解算精度和抗干扰性；文[14]提出了一种基于扩展卡尔曼滤波的算法实现了姿态平稳误差小的优点；文[15]利用基于无迹卡尔曼滤波的传感器融合算法，提高了姿态跟踪的鲁棒性.然而，微型四旋翼飞行器在强机动情况需要对运动加速度进行抑制，在高速运动情况下需要对定值梯度步长进行改进，上述3种基于卡尔曼的算法都需要计算系统噪声方差阵和测量噪声方差阵，使得计算量过大，不易在微控制器上实现.
本文以低成本商用MEMS传感器和ARM-CortexM3为内核的STM32F103CBT6、32位微处理器搭建微型四旋翼实验平台，提出一种运动加速度抑制的动态步长梯度下降姿态解算算法，完成了微型四旋翼飞行器的姿态精确、快速解算，实现了四旋翼飞行器的稳定飞行.
2 飞行器姿态描述 2.1 姿态坐标系描述在姿态解算中，定义两个坐标系导航坐标系n系和载体 (body) 坐标系b系来表示姿态角，其中导航坐标系采用东北天建立坐标系，导航坐标系的坐标原点O位于运载体的质心；载体坐标系采用右前上建立坐标系，载体坐标系原点O′与导航坐标系原点O重合.姿态角的定义如图 1所示，绕x轴旋转对应俯仰角 (pitch)、绕y轴旋转对应横滚角 (roll)、绕z轴旋转对应偏航角 (yaw)，分别为3轴欧拉角所示ϕ、θ、ψ.
|
| 图 1 载体坐标系和导航坐标系 Figure 1 The navigation frame and the body frame |
描述导航坐标系和载体坐标系的转换关系通常有3种方法：欧拉角法[16]、方向余弦法[17]、四元数法[18].这里选用计算量小，算法简单的四元数法描述导航坐标系和载体坐标系的转换关系.捷联惯导系统理论[19]中定义四元数q：
|
(1) |
式 (1) 中，q0为四元数的标量部分，q1、q2、q3为四元数的矢量部分，其中i2=j2=k2=-1.
四元数微分方程[20]：
|
(2) |
式 (2) 中，bnq为b系相对n系的四元数；
定义陀螺仪的测量输出值为ωb=[0 ωxb ωyb ωzb]T，可得
|
(3) |
采用先离散后迭代的方法对四元数微分方程进行求解.定义系统采样周期为Ts，离散化后四元素方程为
|
(4) |
|
(5) |
载体坐标系到导航坐标系的旋转矩阵[21]Cbn由单位化四元数表示为
|
(6) |
根据z-y-x旋转顺规，可以求解出3个姿态角：
|
(7) |
假设初始姿态四元数给定为bnq0=[1 0 0 0]T，由式 (4) 和式 (5) 可得t时刻的四元数bnqk，代入式 (7) 即可得到t时刻的3轴欧拉角ϕ、θ、ψ.
3 动态步长梯度下降法 3.1 梯度下降法梯度下降法是利用目标函数对应的负梯度方向来更新每次迭代的新的方向，使得每次迭代能使待优化的目标函数逐步减小，通常用来求解函数的最小值.因此，将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，这就是通过加速度计表征的旋转矩阵的误差函数.
重力加速度g在导航坐标系n中的值标准化后为gn=[0 0 0 1]T.假设载体坐标系中加速度计的各轴分量为ax、ay、az，那么ab=[0 ax ay az]T.重力加速度g从导航坐标系n旋转到载体坐标系b：
|
(8) |
式中，gb为向量g在b系中的坐标，nbq为n系相对b系的四元数，nbq*为n系相对b系的复数共轭四元数，gn为向量g在n系中的坐标.
将标准化gn=[0 0 0 1]T代入式 (8)，得出重力加速度g在载体坐标系下的值gb，将其与载体坐标系中的加速度计测量值相减得到误差函数fg(nbq，ab)：
|
(9) |
对误差函数fg(nbq，ab) 求导后，得出对应的雅可比矩阵：
|
(10) |
通过雅可比矩阵得到误差函数的梯度值：
|
(11) |
迭代公式为
|
(12) |
式中，nbq∇(k) 为梯度下降法所求的目标姿态四元数，nbqest(k-1) 为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，
迭代公式 (12) 表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态.
3.2 梯度下降法中的动态步长确定在梯度下降法中，步长μ与载体运动的角速度、系统的采样时间正相关：
|
(13) |
式 (13) 中，ω为载体的运动角速度，Ts为系统的采样时间，α>1.式 (13) 表明载体的运动角速度越快，采样时间越长，那么梯度的步长也就越长.
载体的运动角速度即为陀螺仪的测量角速度.假设陀螺仪的测量角速度为
|
则载体的运动角速度为
|
(14) |
为避免静态时ω=0而导致动态步长为0的结果，这里给动态步长补一个初始值μ0，该初始值μ0通过静态测试实验得出.因此梯度下降法中的动态步长为
|
(15) |
梯度下降法可以求得一组基于加速度计表征出来的姿态四元数.加速度计的动态性能差，而陀螺仪是处理高速运动下的理想传感器.因此将两种传感器进行融合，融合两种传感器的优点，即减小了陀螺仪姿态的误差并且提高了加速度计的动态性能.
基于四元数微分方程和梯度下降的姿态融合算法为
|
(16) |
式 (16) 中，q∇(k) 为梯度下降法求出的姿态四元数；qω(k) 为基于陀螺仪的四元微分方程求出的姿态四元数；χ和 (1-χ) 分别为两种姿态四元数的权重.式 (16) 取最优姿态的条件为q∇(k) 的收敛速度等于qωε(k) 的发散速度，如式 (17) 所示：
|
(17) |
其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ.由于当四旋翼飞行器高速运动时，飞行器姿态变化大，动态步长μ随之也变大，因此式 (17) 可近似为
|
(18) |
因为μ较大，对式 (12) 而言上一次姿态nbqest(k-1) 可以忽略不计，既直接由梯度的负方向迭代到达目标姿态值，则式 (12) 可以重新定义为
|
(19) |
由式 (4) 和式 (5) 得到陀螺仪微分方程迭代出的姿态：
|
(20) |
将式 (18)~(20) 代入式 (16)：
|
(21) |
将式 (21) 简单定义为迭代公式 (22)：
|
(22) |
其中，
|
(23) |
由此可得，动态步长梯度下降法的姿态融合算法为
|
(24) |
其中，
梯度下降算法中，通过加速度计表征的旋转矩阵误差为导航坐标系n中的重力加速度gn旋转到载体坐标系b中的向量在gb与加速度计测量值的差.因此载体坐标系中加速度计的测量值理论上只存在重力加速度，但是飞行器在强机动情况下会产生运动加速度，会降低姿态解算的精度.所以要把飞行器的重力加速度和运动加速度分离开来处理，如果运动加速度的值过大，那么式 (24) 通过加速度计补偿陀螺仪偏差的部分就该相应减少.所以运动加速度越大，算法对加速度计测量值的不信任程度越深.
定义δk为加速度计测量值的信任程度因子：
|
(25) |
式中，‖akb‖为运动加速度的模长，ε为信任因子的可调参数，δ0是为了避免运动加速度过大导致信任程度因子接近0而给出的初始固定值.式 (25) 表明运动加速度的值越大，信任因子越低.运动加速度向量akb为
|
(26) |
式 (26) 中，fkb为当前加速度计的测量值，gn为导航坐标系下的标准重力加速度.由于只需要求解gb，所以只需用到旋转矩阵的第3列[Cb31n Cb32n Cb33n]T.
为了消除加速度计测量值对姿态解算的影响，使得处理方法更为精确，式 (24) 中[Cb31n Cb32n Cb33n]k|k-1T为第k-1次的姿态加上第k次陀螺仪增量积分的出来的姿态的旋转矩阵的第3列，没有加上第k次加速度计梯度下降姿态的增量，如式 (27) 所示：
|
(27) |
通过迭代算法，增加了运动加速度求取的准确性并较好地抑制了运动加速度.
加速度计的测量值的信任程度和运动运动加速度的大小成反比，因此可得运动加速度抑制处理的动态步长梯度下降姿态解算公式为
|
(28) |
其中，
|
| 图 2 整体算法框图 Figure 2 Block diagram of the whole algorithm |
本文实验平台选用以ARM-CortexM3为内核的STM32-F103CBT6、32位微处理器来搭建的微型四旋翼飞行器.惯性导航传感器选用美国体感技术公司Inven Sense的MPU6050，该模块为集成三轴加速度计、三轴陀螺仪的EMES传感器.系统的结构示意图如图 3所示.
|
| 图 3 微型四旋翼飞行器实验平台系统结构示意图 Figure 3 Schematic diagram of the micro-quadrotor experimental platform |
图 3中，微型四旋翼的飞行控制单元由控制器、NRF24L01无线通讯模块、惯性导航传感器MPU6050、电机驱动模块、电源及稳压模块组成.遥控器由控制器、无线通讯模块、Oled显示模块、电源及稳压模块组成.微型四旋翼飞行器实验平台实物效果图如图 4所示. 图 4中将飞行控制单元和机架集成在一块PCB板上，既减轻了重量也减小了体积.微型四旋翼飞行器的各项参数如表 1所示.
|
| 图 4 微型四旋翼飞行器和遥控器的硬件图 Figure 4 Hardware of the micro-quadrotor and the remote controller |
| 四旋翼飞行器重量 | 35.7 g |
| 四旋翼飞行器对角线轴距 | 10.1 cm |
| 四旋翼飞行器螺旋桨直径 | 5.5 cm |
| 空心杯电机直径 | 7 mm |
| 空心杯电机高度 | 20 mm |
| 四旋翼飞行器续航时间 | 5 min |
为了验证对运动加速抑制的动态步长梯度下降姿态融合算法的准确性和快速性.做了以下实验：第1组为静态实验，测试算法的静态性能.第2组实验为水平滑动实验，测试飞行器在强机动情况下有了运动加速度后，加速度抑制处理算法对姿态解算的影响.第3组实验为绕轴旋转实验，测试飞行器在高速运动情况下动态步长对姿态解算的动态性能影响，每一组实验都是同时用4种姿态算法解算出飞行器的姿态进行对比，所以确保了4种姿态结算算法是在相同条件下进行的性能对比.
|
| 图 5 静态测试下4种姿态算法的结果 Figure 5 Static test results of the four attitude algorithms |
| 算法 | 姿态角 | 均方根误差 | 最大值 | 最小值 |
| CF | PITCH | 0.139 0 | 0 | -0.2 |
| ROLL | 0.120 2 | 0.2 | 0 | |
| GDA | PITCH | 0.077 9 | 0 | -0.1 |
| ROLL | 0.052 0 | 0.1 | 0 | |
| DGDA | PITCH | 0.077 8 | 0 | -0.1 |
| ROLL | 0.052 1 | 0.1 | 0 | |
| DGDA+MAP | PITCH | 0.077 9 | 0 | -0.1 |
| ROLL | 0.052 2 | 0.1 | 0 |
图 5所示为当实验平台静止时，同时用4种姿态算法解算出飞行器的姿态并统计俯仰角和横滚角的数据，并进行对比. 图 5中自上而下分别为互补滤波算法 (complementary filter，CF)、定步长梯度下降姿态融合算法 (gradient descent algorithm，GDA)、动态步长梯度下降姿态融合算法 (dynamic gradient descent algorithm，DGDA)、动态步长梯度下降+运动加速度抑制处理的姿态算法 (dynamic gradient descent algorithm+motion acceleration processing，DGDA+MAP).由图 5中的姿态波形对比得出CF算法得到的姿态角波动范围为-0.2~0.2，而3种梯度下降姿态融合算法得到的姿态角波动范围为-0.1~0.1. 表 2给出了4种算法姿态角的均方根误差、最大值及最小值.通过表中的数据可以得到梯度下降姿态融合算法的姿态解算精度高于CF算法.通过GDA算法和DGDA算法姿态波形对比可以得到，静态时梯度下降的动态步长对姿态无影响.通过DGDA算法和DGDA+MAP算法姿态波形对比可以得到，静态时四旋翼飞行器只有重力加速度，并不存在运动加速度.因此静态时，运动加速度抑制处理对姿态解算无影响.通过第1组静态实验得到静态时梯度下降姿态融合算法优于CF算法，并且静态时GDA算法、DGDA算法和DGDA+MAP算法得到的姿态相同.
水平滑动测试实验结果如图 6和表 3所示，其中δ0=0.04，ε=0.1.
|
| 图 6 4种算法的水平滑动测试结果 Figure 6 Horizontal slip test results of the four attitude algorithms |
| 算法 | 姿态角 | 均方根误差 | 最大值 | 最小值 |
| GDA | PITCH | 0.344 4° | 0.7° | -1.2° |
| ROLL | 0.303 1° | 1.1° | -0.8° | |
| DGDA | PITCH | 0.491 5° | 1.0° | -1.6° |
| ROLL | 0.450 9° | 2.2° | -1.2° | |
| GDA+MAP | PITCH | 0.277 9° | 0.1° | -0.6° |
| ROLL | 0.189 9° | 0.3° | -0.7° | |
| DGDA+MAP | PITCH | 0.288 7° | 0.1° | -0.6° |
| ROLL | 0.189 4° | 0.4° | -0.7° |
将四旋翼实验平台固定在滑块上，而滑块只做水平运动，沿y轴水平来回快速运动4次，然后沿x轴水平来回快速运动4次，做运动加速度为2 m/s2的加速运动，运动加速度满足微型四旋翼的飞行要求.同时用4种姿态算法解算出飞行器的姿态并统计俯仰角和横滚角的数据，进行对比，完成水平滑动实验测试. 图 6中自上而下分别为定步长梯度下降姿态融合算法 (GDA)、动态步长梯度下降姿态融合算法 (DGDA)、定步长梯度下降+运动加速度抑制处理姿态融合算法 (GDA+MAP)、动态步长梯度下降+运动加速度抑制处理姿态融合算法 (DGDA+MAP).微型飞行器水平滑动时会产生运动加速度，由图中GDA和DGDA、GDA+MAP和DGDA+MAP算法形成两组对比实验可以得到动态步长在四旋翼飞行器水平滑动时对姿态解算没有影响.图中GDA和GDA+MAP、DGDA和DGDA+MAP算法形成2组对比实验可以得到经过运动加速度抑制处理后，水平运动的四旋翼飞行器的姿态解算精度更加准确. 表 2给出水平滑动实验测试下四中算法得到的姿态角均方根误差、最大值和最小值.由表中数据得到，在水平滑动实验测试下，经过运动加速度抑制处理后得到的姿态角误差明显小与没有经过运动加速度抑制处理得到的姿态角且姿态的波动范围小.
通过第2组水平滑动测试实验得到，水平滑动测试下GDA+MAP和DGDA+MAP算法优于GDA和DGDA算法.实验结果说明了对运动加速度抑制处理会在强机动情况下提高姿态解算的精度且能够增强姿态的抗干扰性.
绕轴旋转实验测试结果如图 7和表 4所示，其中μ0=0.04.
|
| 图 7 4种算法的动态绕轴测试结果 Figure 7 Rotation test results of the four attitude algorithms |
| 算法 | 姿态跟随滞后平均时间/ms |
| GDA | 522 |
| DGDA | 118 |
| GDA+MAP | 513 |
| DGDA+MAP | 111 |
将四旋翼实验平台沿x轴固定在圆柱状的碳纤维杆上，保证四旋翼只能作绕轴运动.将四旋翼绕x轴匀速旋转旋转80°既增大PITCH角到80°静止，然后减小PITCH角到0°静止，重复以上运动3次，同时用4种姿态算法解算出飞行器的姿态并统计俯仰角和横滚角的数据，进行对比，完成动态实验测试. GDA和GDA+MAP、DGDA和DGDA+MAP算法形成2组对比实验得出GDA和GDA+MAP波形基本一样，说明在绕轴旋转中无运动加速度产生，运动加速度抑制处理算法在这种情况下对姿态无影响.在飞行器沿x轴旋转增大PITCH角的过程中，GDA算法中姿态角度跟随缓慢，而在飞行器减小PITCH角至0°的过程中，姿态角度同样下降缓慢. DGDA和DGDA+MAP中的波形也基本相似，说明在绕轴旋转中运动加速度处理算法对姿态无影响.在飞行器沿x轴旋转增大PITCH角的过程中，DGDA算法中姿态角度跟随迅速基本无超调，而在飞行器减小PITCH角至0度的过程中，姿态角度同样下降迅速. GDA和DGDA、GDA+ALP和DGDA+ALP算法形成2组对比实验得出，当飞行器作绕轴运动时，动态步长的梯度下降姿态融合算法的姿态跟随性、快速性要好过定步长的梯度下降姿态融合算法.说明了动态步长梯度下降算法的动态性能更好. 表 4给出了4种算法在绕轴旋转测试下，姿态跟随的平均滞后时间.通过表中的数据得到GDA和GDA+MAP的姿态跟随滞后时间平均为500 ms，而DGDA和DGDA+MAP的姿态跟随滞后时间平均为110 ms.
通过绕轴旋转轴实验测试说明动态步长梯度下降算法姿态跟踪性能优于定步长姿态下降算法.既动态步长的梯度下降算法动态性能比定步长梯度下降姿态算法好.
6 结论本文提出了一种对运动加速度抑制处理的动态步长梯度下降姿态算法.通过对强机动运动过程中机体产生的运动加速度进行抑制处理，提高了姿态解算的精度；在高速运动过程中，将梯度下降算法的步长与飞行器的运动合角速度关联起来构成动态步长，增强了算法的动态性能.该算法与其它算法在微型四旋翼实验平台上进行测试，分别完成静态测试、水平运动测试、绕轴运动测试.实验结果表明，对运动加速度抑制处理的动态步长梯度下降姿态算法在强机动情况下，抑制了运动加速度提高了姿态解算的精度；对运动加速度抑制处理的动态步长梯度下降姿态算法在高速运动情况下的动态步长增强了算法的动态性能，实现了对四旋翼飞行器姿态的实时跟踪；对运动加速度抑制处理的动态步长梯度下降姿态算法加入了运动加速度的抑制处理，增强了姿态解算算法的抗干扰性.后续工作会从系统误差和最优估计方面来改善算法的性能，会把传感器的噪声和系统噪声考虑在内，从而进一步提高算法的精度和性能.
| [1] | 张静, 霍建文, 刘星, 等. 微型四旋翼飞行侦察机器人控制系统设计[J]. 测控技术, 2015, 34(7): 67–69. Zhang J, Huo J W, Liu X, et al. Design of a micro-quadrotor reconnaissance robot control system[J]. Measurement and Control Technology, 2015, 34(7): 67–69. |
| [2] | Primicerio J, Gennaro S F D, Fiorillo E, et al. A flexible unmanned aerial vehicle for precision agriculture[J]. Precision Agriculture, 2012, 13(4): 517–523. DOI:10.1007/s11119-012-9257-6 |
| [3] | 杜继永, 黄国荣, 张凤鸣, 等. 基于低成本MEMS器件的捷联航姿系统设计[J]. 传感技术学报, 2010, 23(11): 1662–1666. Du J Y, Huang G R, Zhang F M, et al. Design of AHRS based on low-cost MEMS[J]. Chinese Journal of Sensors and Actuators, 2010, 23(11): 1662–1666. DOI:10.3969/j.issn.1004-1699.2010.11.029 |
| [4] | Di L, Fromm T, Chen Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1/2/3/4): 621–635. |
| [5] | Madgwick S. An efficient orientation filter for inertial and inertial/magnetic sensor arrays[R]. UK:Report x-io and University of Bristol, 2010. |
| [6] | Madgwick S O H, Harrison A J L, Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//IEEE International Conference on Rehabilitation Robotics. Piscataway, NJ, USA:IEEE, 2011:1-7. |
| [7] | Wang L, Zhang Z, Sun P. Quaternion-based Kalman filter for AHRS using an adaptive-step gradient descent algorithm[J]. International Journal of Advanced Robotic Systems, 2015(12): 131. |
| [8] | 王立, 章政, 孙平. 一种自适应互补滤波姿态估计算法[J]. 控制工程, 2015, 22(5): 881–886. Wang L, Zhang Z, Sun P. An adaptive complementary filter for attitude estimation[J]. Control Engineering of China, 2015, 22(5): 881–886. |
| [9] | 叶锃锋, 冯恩信. 基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J]. 传感技术学报, 2012, 25(4): 524–528. Ye Z F, Feng E X. Attitude stabilization based on quaternion and Kalman filter for two-wheeled vehicle[J]. Chinese Journal of Sensors and Actuators, 2012, 25(4): 524–528. |
| [10] | 傅忠云, 刘文波, 孙金秋, 等. 自适应混合滤波算法在微型飞行器姿态估计中的应用[J]. 传感技术学报, 2014, 27(5): 698–703. Fu Zhong Y, Liu W B, Sun J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014, 27(5): 698–703. |
| [11] | 冀亮, 钱正洪, 白茹. 基于四元数的四轴无人机姿态的估计和控制[J]. 现代电子技术, 2015, 38(11): 112–116. Ji L, Qian Z H, Bai R. Attitude estimation and control of quarternion based quad-axis UAV[J]. Modern Electronics Technique, 2015, 38(11): 112–116. |
| [12] | 彭孝东, 张铁民, 李继宇, 等. 基于传感器校正与融合的农用小型无人机姿态估计算法[J]. 自动化学报, 2015, 41(4): 854–860. Peng X D, Zhang T M, Li J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4): 854–860. |
| [13] | 朱云峰, 孙永荣, 黄斌. 融合地磁信息的三子样姿态确定算法设计[J]. 信息与控制, 2014, 43(6): 664–668. Zhu Y F, Sun Y R, Huang B. Algorithm design of tri-sample attitude determination with fusion of geomagnetic information[J]. Information and Control, 2014, 43(6): 664–668. |
| [14] | 曹美会, 鲜斌, 张旭, 等. 基于视觉的四旋翼无人机自主定位与控制系统[J]. 信息与控制, 2015, 44(2): 190–196. Cao M H, Xian B, Zhang X, et al. An autonomous vision-based localization and control system for quadrotor UAV[J]. Information and Control, 2015, 44(2): 190–196. |
| [15] | Enayati N, De Momi E, Ferrigno G. A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer assisted surgery[J]. IEEE Transactions on Instrumentation & Measurement, 2015, 64(8): 2291–2301. |
| [16] | 张力军, 张士峰, 杨华波, 等. 基于欧拉角观测模型的航天器姿态确定方法[J]. 国防科技大学学报, 2012, 34(6): 84–88. Zhang L J, Zhang S F, Yang H B, et al. Spacecraft attitude determination based on Euler angle measurement model[J]. Journal of National University of Defense Technology, 2012, 34(6): 84–88. |
| [17] | 周绍磊, 丛源材, 李娟, 等. 方向余弦矩阵中四元数提取算法比较[J]. 中国惯性技术学报, 2008, 16(4): 415–418. Zhou S L, Cong Y C, Li J, et al. Comparison of algorithm for extracting quaternion from DCM[J]. Journal of Chinese Inertial Technology, 2008, 16(4): 415–418. |
| [18] | 蒋钰, 谌海云, 岑汝平. 基于四元数的四旋翼飞行器姿态解算算法[J]. 制造业自动化, 2015, 37(23): 77–80. Jiang Y, Chen H Y, Cen R P. Attitude solution algorithm for four rotor aircraft based on four element number[J]. Manufacturing Automation, 2015, 37(23): 77–80. DOI:10.3969/j.issn.1009-0134.2015.23.22 |
| [19] | 张荣辉, 贾宏光, 陈涛, 等. 基于四元数法的捷联式惯性导航系统的姿态解算[J]. 光学精密工程, 2008, 16(10): 1963–1970. Zhang R H, Jia H G, Chen T, et al. Attitude estimation based on Quaternion in strapdown inertial navigation[J]. Optics and Precision Engineering, 2008, 16(10): 1963–1970. DOI:10.3321/j.issn:1004-924X.2008.10.029 |
| [20] | 秦永元. 惯性导航[M]. 北京: 科学出版社, 2014: 288-299. Qin Y Y. Inertial navigation[M]. Beijing: Science Press, 2014: 288-299. |
| [21] | Diebel J. Representing attitude:Euler angles, unit quaternions, and rotation vectors[J]. Matrix, 2006, 58(15/16): 1–35. |