共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法
【专利摘要】本发明涉及共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，首先采集传感器信息获取四旋翼飞行状态，然后利用共轭梯度方法进行观测模型建模，进而进行过程模型建模，将共轭梯度方法得到的四元数作为扩展卡尔曼滤波的测量值；最后利用扩展卡尔曼滤波法得到最佳的四元数，解算出四旋翼的三个姿态角。相比单纯的梯度方法和互补滤波方法，本发明的扩展卡尔曼滤波法考虑到了系统误差和传感器的测量噪声，因此估计的四元数有更好的准确性。利用共轭梯度方法得到观测四元数运用到扩展卡尔曼滤波中，可以避免计算复杂的线性化观测模型。
【专利说明】
共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法
技术领域
[0001] 本发明涉及一种四旋翼无人机姿态航向数据测量方法，特别是涉及一种共辄梯度 与扩展卡尔曼结合的对四旋翼无人机（以下简称四旋翼）姿态解算方法，主要用于军事侦 察，农业植保，电力巡检，测绘，航拍等。
【背景技术】
[0002] 随着机器人和航天航空技术的不断发展，四旋翼逐渐成为了国内外学者和航拍发 烧友们的研究热点。四旋翼是一种具有垂直起降与空中悬停等特殊飞行能力的多旋翼无人 飞行器。在飞行过程中，通过改变四个旋翼的转速，可以改变各种飞行姿态。正因为四旋翼 无人机具有垂直起降、空中定点悬停、可以实现六自由度的飞行等独特的飞行能力，使其在 军事领域和民用领域得到了广泛的应用。
[0003] 姿态解算是四旋翼的核心部分，其担负着为四旋翼实时提供可靠的飞行状态数据 的重任。目前运用比较广泛的姿态解算方法主要是互补滤波、卡尔曼滤波法和梯度下降法 的方法。互补滤波方法和梯度下降方法计算简单，利用了加速度传感器和地磁传感器去补 偿由于陀螺仪传感器随时间产生漂移造成的误差，但是都没有考虑系统本身的噪声和传感 器的测量误差。互补滤波方法需要选取动态的权值去适应不同的飞行状态，大部分运用者 选择给一个比较大的权值去适应快速飞行，这将会导致飞行器静态性能欠佳。卡尔曼滤波 方法中考虑了系统的测量误差和传感器的测量噪声，相比前两种方法有更好的准确性，且 是运用最广泛的姿态解算方法。一般的卡尔曼方法中需要线性化观测模型，而线性化观测 模型需要面临求解雅各比矩阵的问题，这将会带来很大的计算量。
【发明内容】
[0004] 本发明所要解决的技术问题是，针对四旋翼姿态解算的问题，提供一种能够实时 获取姿态且精度较高的姿态解算方法，从而更好的控制四旋翼的飞行状态。
[0005] 为解决上述技术问题，本发明采用的技术方案是：
[0006] -种共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，所述四旋翼包含单 片机以及与单片机相连的陀螺仪、加速度传感器、地磁传感器，单片机与地面系统上位机通 讯连接;其特征在于包括如下步骤：
[0007] 步骤SI:采集传感器信息:在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感 器采集四旋翼飞行状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋 翼加速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度;所述机体 坐标系是固连于四旋翼的参考坐标系；
[0008] 步骤S2:分别对陀螺仪、加速度计做去零飘处理：
[0009] 步骤S3:采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影 的影响;对三轴磁感应强度做归零归一处理；
[0010] 步骤S4:利用共辄梯度方法建立对扩展卡尔曼滤波观测模型；
[0011] 步骤S5:建立扩展卡尔曼滤波器的过程模型:将共辄梯度法求出的使误差函数e(q) 最小的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四 元数；
[0012] 步骤S6:联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态 角方程;将步骤S5估算出的最优四元数带入三个姿态角方程解算出四旋翼的三个姿态角： 航向角γ，俯仰角Θ，横滚角％,
[0013] 上述技术方案中，步骤S2对陀螺仪、加速度计做去零飘处理的具体做法是将四旋 翼保持水平，取200次值取平均值，这个平均值就为零飘值，以后取得的值都减去该零飘值。
[0014] 上述技术方案中，步骤S3对地磁传感器测得的三轴磁感应强度做归零归一处理的 具体过程如下：
[0015] 首先，将安装有地磁传感器的四旋翼水平放置，然后不断转动四旋翼让地磁传感 器旋转出一个圆球，同时旋转过程中下位机不断的将数据发送给上位机;进而，上位机对接 收到的数据进行处理，分别找出X轴、y轴和Z轴各单个轴的最大值和最小值，最后进行归零 归一计算：归零，就是要使单个轴的最大值和最小值的绝对值相等，或者说在坐标轴上对 称;归一，就是使两个轴的数据范围趋于一致;计算公式如下：
[0016] ⑴
[0017] (2)
[0018] (3.)
[0019] 式中，X轴为长轴，y轴为短轴;Hxi、Hyi、Hzi为X轴、y轴和z轴各单个轴经过归零归一 处理后的标定值；Hsxi、Hsyi、Hszi分别表示在X轴、y轴、z轴采集到的三轴磁感应强度； HsXmax、HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分别表示在X轴、y轴、Z轴采集到的三轴磁感应强 度的最大值和最小值。
[0020] 上述技术方案中，步骤S4用共辄梯度方法建立对扩展卡尔曼滤波观测模型过程如 下:将理想状态均匀重力场和地磁场作为参考方向，通过导航坐标系下四元数转换到机体 坐标系下得到三轴加速度[a'b x a'by a'bz]T和磁感应强度[m'bx m'by m'bz]T;将转换得到的 三轴加速度减去机体坐标系下测得的重力加速度在三轴上的投影，且将转换得到的磁感应 强度减去机体坐标系下测得的三轴磁感应强度，得到测量误差函数e( q)，利用共辄梯度法求 出使测量误差函数e(q)最小的测量四元数，即利用共辄梯度方法建立扩展卡尔曼滤波观测 模型。
[0021] 上述技术方案中，步骤S5建立对扩展卡尔曼滤波的过程模型的具体过程如下：
[0022] 首先由四元数的微分值彳与角速度w的关系可以得到扩展卡尔曼滤波的状态等式：
[0023] (17)
[0024] (18)
[0025] 〇[￥]为状态转移矩阵，1、^、￥2分别表示陀螺仪传感器测量的三轴角速度;#表示 四元数的微分。将连续时间模型转化为离散时间模型，令系统采样时间为Ts，离散时间模型 为：
[0026] (19)
[0027]式中q(k)表示第k次采样，对exp( QkTs)进行泰勒级数展开保留一阶和二阶项，可得 公式(20)·
[0028]
(20)
[0029] 式中：
[0030]
(21)
[0031] 13*3表示三维单位向量，wx、w y、Wz分别表示陀螺仪传感器测量的三轴角速度。
[0032] 上述技术方案中，步骤S5利用扩展卡尔曼滤波器估算出最优四元数包括如下步 骤:首先利用步骤S5的过程模型建模估算出下一个时刻的四元数，然后进行误差协方差更 新，第三步利用更新的协方差计算出卡尔曼增益，第四步利用步骤S4中观测模型求解出的 测量四元数减去第一步估计出来的下一个时刻的四元数，乘上第三步求出的卡尔曼增益， 加上上一个时刻的四元数就是扩展卡尔曼滤波器求出的最优四元数;之后更新下一时刻协 方差为下一次计算做准备。
[0033] 本发明的扩展卡尔曼滤波法考虑到了系统误差和传感器的测量噪声，因此估计的 四元数有更好的准确性。利用共辄梯度方法得到观测四元数运用到扩展卡尔曼滤波中，可 以避免计算复杂的线性化观测模型。
[0034] 本发明选择的共辄梯度与扩展卡尔曼结合的姿态解算方法，利用共辄梯度方法求 出观测四元数运用于扩展卡尔曼滤波中，不需要考虑线性化观测模型的问题。共辄梯度法 相比梯度下降方法，下降的步长为实时修正的动态值，精准度更高，更容易跟踪各种飞行状 ??τ O
[0035] 除此之外，本发明运用了目前最流行的四元数的方法去表示机体的转动，四元数 法运用在R4空间中，相比欧拉角表示机体转动的方法，四元数的方法更容易表示机体任何 的转动，且不会出现奇异性问题。
[0036] 综上所述，本发明的共辄梯度法与扩展卡尔曼结合的四旋翼姿态解算方法，主要 优点有三个:其一利用多传感器数据融合，对低成本传感器飘移问题进行修正，提高测量精 度。其二利用了共辄梯度方法建立扩展卡尔曼滤波的观测模型，共辄梯度方法是一阶迭代 方法相比其它需要线性化观测模型，求解复杂的雅各比矩阵的方法计算量大大减小。让方 法的实时性有了很好的保证，为更好的控制四旋翼飞行器的姿态做好准备。其三运用扩展 卡尔曼方法考虑了系统本身噪声和传感器的测量误差，提高测量精度，利用了共辄梯度的 方法能够动态的调整下降方向和步长可以很好的适应不同的飞行状态。
【附图说明】
[0037] 图1为本发明涉及的导航坐标系与机体坐标系关系图；
[0038]图2为本发明涉及的导航坐标系经过三次转动到机体坐标系图；
[0039] 图3为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法方框图；
[0040] 图4为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法测量流程 图；
[0041] 图5为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法测量结果图 (滚转角随时间变化曲线）。
[0042] 图6为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法测量结果图 (俯仰角随时间变化曲线）。
【具体实施方式】
[0043] 下面对照附图1-6和实施例详细说明本发明，本发明并不仅仅限于所给附图和实 施例。
[0044] 根据本发明实施的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法总体 流程如图3和4。其特征在于包括如下步骤：
[0045] 步骤Sl:采集传感器信息:在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感 器采集四旋翼状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋翼加 速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度;机体坐标系是 固连于四旋翼的参考坐标系；
[0046] 步骤S2:分别对陀螺仪、加速度计做去零飘处理：
[0047]步骤S3:采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影 的影响;对三轴磁感应强度做归零归一处理；
[0048]加速度传感器又叫重力加速度计，它可以测量飞行器重力在三轴上的投影;但是 实际测量的重力加速度中包含有机体加速度，由于机体加速度是高频噪声，所以采用巴特 沃斯低通滤波器滤除机体加速度的影响;经过实验选择截止频率为5HZ的巴特沃斯低通滤 波器；由于地磁传感器本身的特性不是完全对称的，所以需要采用归零归一的方法对地磁 传感器测得的三轴磁感应强度进行处理；
[0049]步骤S4:将理想状态均匀重力场和地磁场作为参考方向，通过导航坐标系下四元 数转换到机体坐标系下得到三轴加速度[a'bx a'by a'bz]T和磁感应强度[m'bx m'by m'bz]T; 将转换得到的三轴加速度减去机体坐标系下测得的重力加速度在三轴上的投影，且将转换 得到的磁感应强度减去机体坐标系下测得的三轴磁感应强度，得到测量误差函数e( q)，利用 共辄梯度法求出使测量误差函数最小的四元数，即利用共辄梯度方法建立扩展卡尔曼 滤波观测模型；
[0050] 步骤S5:建立扩展卡尔曼滤波器的过程模型:将共辄梯度法求出的使误差函数e(q) 最小的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四 元数；
[0051] 步骤S6:联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态 角方程;将步骤S5估算出的最优四元数带入三个姿态角方程求出三个姿态角:航向角γ，俯 仰角Θ，横滚角
[0052]上述技术方案中，步骤S2对陀螺仪、加速度计做去零飘处理的具体做法是将四旋 翼无人机保持水平，取200次值取平均值，这个平均值就为零飘值，以后取得的值都需要减 去这个零飘值。
[0053]上述技术方案中，步骤S3对地磁传感器测得的三轴磁感应强度做归零归一处理的 具体过程如下：
[0054] 首先，将安装有地磁传感器的四旋翼水平放置，然后不断转动四旋翼让地磁传感 器旋转出一个圆球，同时旋转过程中下位机(下位机是四旋翼上的单片机，三大传感器集成 在单片机中，下位机与电脑上位机通讯连接)不断的将数据发送给上位机;进而，上位机对 接收到的数据进行处理，分别找出X轴、y轴和Z轴各单个轴的最大值和最小值，最后进行归 零归一计算:归零，就是要使单个轴的最大值和最小值的绝对值相等，或者说在坐标轴上对 称；归一，就是伸两个轴的数据范闱趋干一敔：计筧公式如下：
[0055] (1)
[0056] (2>
[0057] (3.)
[0058] 式中，X轴为长轴，y轴为短轴;Hxi、Hyi、Hzi为X轴、y轴和z轴各单个轴经过归零归一 处理后的标定值；Hsxi、Hsyi、Hszi分别表示在X轴、y轴、z轴采集到的三轴磁感应强度； HsXmax、HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分别表示在X轴、y轴、Z轴采集到的三轴磁感应强 度的最大值和最小值。
[0059] 上述技术方案中，步骤S4利用共辄梯度方法建立对扩展卡尔曼滤波观测模型的具 体步骤如下：
[0060] 首先在理想情况下，认为重力的参考量为fg=[0 0 g]T，地磁场的参考量为H= [Hn 0 Hd]τ;其中g表不重力加速度，Hn和Hd分别是地磁场在北东地坐标系中北向分量和垂直分 量;单位化可：
[0061] 如图1所示定义导航坐标系(η系)和机体坐标系(b系）。导航系统中，机体坐标系相 对导航坐标系的转动可以用转动四元数表示，假设起始状态Ve = XEi+yE j+ZEk，式中Xe、yE、Ze 分别表示待转动的量在三轴上的投影;经过一次四旋翼的转动q到达终止状态Vb = x'i+y'j +z'k，（这里是用四元数的方式来表示四旋翼的一次旋转q，然后利用四元数微分方程更新 新的四元数，下面提到的耶^:^^阳都是经过四元数微分方程更新后的四元数：由于四旋 翼无人机的是处于一个不断运动的状态，所以每个周期q都是需要通过四元数的微分方程 来更新的，因而q也表示四元数微分方程更新后的四元数。启动前给q-个初始值[1 0 0 0]，后面每个周期更新一次通过微分方程来更新。）其中：q = qQ+qii+q2j+q3k，qo、qi、q2、q3表 示四元数的分量，i、j、k表示三个矢量；，其中X'、y'、z'表示转动后的量分别在三轴上的投 影，根据四元数转动的映像方式为：
[0062]
c4)
[0063] 式中cf为q的共辄四元数即标量相同矢量相反，表示为q* = q〇-qii-q2j-q3k。将上式 展开，可得如式(5):
[0064]
(5)
[0066]
[0065] 根据公式(5)，将理想情况下，重力的参考量和地磁场的参考量转换到机体坐标系 中，如式(6)和(7):
[0067]
[0068] 式(6)和式（7)中么、#分别表示单位化后的理想情况下重力场和磁感应强度在 三轴上的分量，4、氧^分别表示单位化后北东地坐标系中北向分量和垂直分量，q表示四 元数微分方程更新后的四元数，其中則、9 1、屯43表示四元数的分量彳为9的共辄四元数。将 式(6)和式(7)所得数据与加速度传感器和地磁传感器在机体坐标系测得的加速度和磁感 应强度相减，可得测量误差函数e( q):
[0069] Θ(q) - [a bx-Sbx ? by-Sby ? bz-Sbz ΠΙ bx-IIlbx ΠΙ by-IIlby ΠΙ bz-IIlbz ] ( 8 )
[0070] 式(8)中，36\1丫、31)2、11?^、11?^、11?)2表示加速度传感器和地磁传感器在机体坐标系下 测得的加速度和磁感应强度^'^^^^'^^'^^^^'^是式⑷~⑴计算出的理想情况 下机体的加速度和磁感应强度。则对四元数的优化问题可转换为求目标函数.Z w=Aa9,最 小值的问题。
[0071 ]根据共辄梯度法的计算公式：
[0072] Z(k+i) = Z(k)+X(k+i)d(k+i) (9)
[0073]式中，A(k+1)为最优步长，d(k+i)为下降的方向，Ζ〇〇表示上一时刻四元数，Z(k+i)表示 下一时刻估算出来的四元数，k表示第k次迭代。
[0074]根据FR共辄梯度法：
[0075]
IO^
[0076] 式中W为一中间变量，JTSe(q)的雅格比矩阵，具体展开后的雅格比矩阵如式 (11)：
[0077]
[0078] 式中Hn和//y分别表示单位化后北东地坐标系中北方向的北向分量和垂直方向的 垂直分量，η表示北方向，d表示垂直方向；其中9〇、91、92、93表示四元数的分量。
[0079] 根据FR共辄梯度法，利用下面公式选择步长，以及下降方向：
[0080] di = -gi (12)
[0081] dk+i = _gk+i+&+idk (13)
[0082] Ek^l=Vfkn (14)
[0083] (15)
[0084] 16)
[0085] 上面5个公式为共辄梯度法选择合适步长以及最快下降方向的核心公式，式中 gk+1，&+1为两个中间变量，Wa+1为公式（10)经过k+Ι次运算出的结果，d k+1表示第k+Ι次下降 的步长，Ak+1为第k+Ι次下降的方向，Cl1为第一次下降的方向， gl为中间变量gk+i的初值；
[0086] 至此为步骤S4利用共辄梯度法求解误差函数的最小值，也就是对扩展卡尔曼滤波 的观测模型建模，从而得到的一组姿态四元数，这组四元数是基于加速度传感器和地磁传 感器表征出来的四元数，作为扩展卡尔曼滤波的测量值。
[0087] 步骤S5建立对扩展卡尔曼滤波的过程模型的具体过程如下：
[0088] 首先由四元数的微分值4与角速度w的关系可以得到卡尔曼滤波的状态等式：
[0089] (17)
[0090] (18；
[0091] 〇[￥]为状态转移矩阵，1、％、￥2分别表示陀螺仪传感器测量的三轴角速度;纟表示 四元数的微分。将连续时间模型转化为离散时间模型，令系统采样时间为T s，离散时间模型
为：
[0092]
[0093]式中q(k)表示第k次采样，对exp( QkTs)进行泰勒级数展开保留一阶和二阶项，可得 公式(20):
[0094] (20)
[0095]
[0096]
[0097] 13*3表示三维单位向量，wx、w y、Wz分别表示陀螺仪传感器测量的三轴角速度，至此 完成对扩展卡尔曼滤波的过程模型建模，对过程模型的建模主要用于扩展卡尔曼滤波的第 一步，也就是估计下一个时刻的四元数。之后就是需要利用扩展卡尔曼滤波器估算出最优 四元数:首先利用步骤S5的过程模型建模估算出下一个时刻的四元数也就是公式(22)，然 后进行第二步利用公式(23)进行误差协方差更新，第三步利用公式(24)更新的协方差计算 出卡尔曼增益，第四步利用步骤S4的观测模型求解出的测量四元数减去第一步估计出来的 下一个时刻的四元数，乘上第三步求出的卡尔曼增益，加上上一个时刻的四元数就是扩展 卡尔曼滤波器求出的最优四元数，第五步利用公式(26)更新下一时刻协方差为下一次计算 做准备。扩展卡尔曼滤波器就是这样不断的把协方差递归，从而能求出最优四元数。下面给 出利用扩展卡尔曼滤波器估算出最优四元数的五个具体步骤：
[0098] 第一步:根据陀螺仪测量的k时刻角速度更新四旋翼k+Ι时刻的状态四元数，如下：
[0099] ?_(?+η =-4(?,?(?-> (22)
[0100] 式(22)对应公式(20)，％:)表示四元数，Ak表示exp (QkTs)泰勒级数展开后的一阶 和二阶项义+υ表示预测的k+Ι时刻四元数。
[0101] 第二步：四旋翼状态向前推算误差协方差更新，如下：
[0102] P-(k+1)=A(k)P(k)AT(k)+Q(k+l) (23)
[0103] 式中，P(k)表不k时刻的协方差矩阵，Q(k+i)表不四维过程噪声，<表不六〇〇的转置矩 阵，表示预测的k+Ι时刻的协方差矩阵。
[0104] 第三步:滤波器增益系数的求解，如下：
[0105；
124)
[0106] 式中，Kk+i表示卡尔曼增益，R(k+i)表示四维的测量噪声，H(k+i)表示单位矩阵， 表示预测的k+Ι时刻的协方差矩阵。
[0107] 笛四步：枏据共轭梯庠法得剞观测更新方程，如下：
[0108]
(25)
[0109] 式中，4+1表示卡尔曼滤波器估计的最优四元数，Zk+1为步骤S4中共辄梯度方法求 解出的四元数，％η表示预测的k+Ι时刻四元数，Η (1?+υ表示单位矩阵；
[0110]第五步:协方差更新，如下：
[0111]
(26)
[0112] 式中，I为四维单位矩阵，表示预测的k+i时刻的协方差矩阵，H(k+1)表示单位矩 阵。
[0113] 至此，利用扩展卡尔曼滤波器估算出最优四元数过程结束，接下来就是利用估算 出最优四元数求解出欧拉角的过程。
[0114] 上述技术方案中，步骤S6利用估算出最优四元数解算姿态角方程的具体过程如 下：
[0115] 定义四旋翼绕ζ轴转过的角度P为航向角;绕y轴转过的角度γ为横滚角;绕X轴转 过的角度Θ为俯仰角;假设四旋翼依次经过下面三次转动从导航坐标系运动到机体坐标系： 先绕ζ轴转过0角度;再绕y轴转过Θ角度;最后绕X轴转过γ角度；
[0116] 1)绕ζ轴转动^ (如图2)
[0117]
[0118]
[0119]
[0120]
[0121]
[0122]这里把四旋翼的一次转动用欧拉角的方式分解为三次转动，式中x，y，z表示转动 前机体坐标系的X，y，Z轴，经过第一次绕Z轴转动机体坐标系变为Xi，yi，Zi，经过第二次绕yi 转动机体坐标系变为X2，y2，Z2，最后经过第三次绕X2转动机体坐标系变为 X3，y3，Z3。
[0123] S#立H术=汝鮮动可得，
[0124]
(27)
[0125] 则可得欧枋方向会弦矩阵如下：
[0126]
(2.8)
[0127] 根据式(5)和(28)可得：
[0128] ?29)
[0129]
[0130] (3丨）
[0131] 图5为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法测量结果图 (横滚角随时间变化曲线）。
[0132] 图6为本发明共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法测量结果图 (俯仰角随时间变化曲线）。
[0133] 图5和6中，纵轴表示相应的角度变化，横轴是采样次数（随时间变化每隔4ms采样 一次）。其中细实线部分为没有进行融合算法计算出来的姿态随时间变化曲线，粗实线部分 为本发明的姿态解算方法，从图形中可以明显看出本发明的姿态解算方法静态性能和动态 性能良好，不会随时间变化产生漂移误差，曲线光滑没有延时，能够很好的跟踪姿态的变 化。
【主权项】
1. 一种共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，所述四旋翼包含单片 机以及与单片机相连的陀螺仪、加速度传感器、地磁传感器，单片机与地面系统上位机通讯 连接;其特征在于包括如下步骤： 步骤SI:采集传感器信息:在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感器采 集四旋翼飞行状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋翼加 速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度;所述机体坐标 系是固连于四旋翼的参考坐标系； 步骤S2:分别对陀螺仪、加速度计做去零飘处理： 步骤S3:采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影的影 响;对三轴磁感应强度做归零归一处理； 步骤S4:利用共辄梯度方法建立对扩展卡尔曼滤波观测模型； 步骤S5:建立扩展卡尔曼滤波器的过程模型:将共辄梯度法求出的使误差函数e(q)最小 的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四元数； 步骤S6:联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态角方 程;将步骤S5估算出的最优四元数带入三个姿态角方程解算出四旋翼的三个姿态角:航向 角γ，俯仰角Θ，横滚角0。2. 根据权利要求1所述的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其 特征在于：步骤S2对陀螺仪、加速度计做去零飘处理的具体做法是将四旋翼保持水平，取 200次值取平均值，这个平均值就为零飘值，以后取得的值都减去该零飘值。3. 根据权利要求2所述的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其 特征在于:步骤S3对地磁传感器测得的三轴磁感应强度做归零归一处理的具体过程如下： 首先，将安装有地磁传感器的四旋翼水平放置，然后不断转动四旋翼让地磁传感器旋 转出一个圆球，同时旋转过程中下位机不断的将数据发送给上位机;进而，上位机对接收到 的数据进行处理，分别找出X轴、y轴和ζ轴各单个轴的最大值和最小值，最后进行归零归一 计算：归零，就是要使单个轴的最大值和最小值的绝对值相等，或者说在坐标轴上对称；归 一，就是使两个轴的数据范围趋于一致;计算公式如下：(1) ⑵： (3) 式中，X轴为长轴，y轴为短轴;Hxi、Hyi、Hzi为X轴、y轴和z轴各单个轴经过归零归一处理 后的标定值;Hsxi、Hsyi、Hszi分别表示在X轴、y轴、z轴采集到的三轴磁感应强度;Hsxmax、 HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分别表示在X轴、y轴、Z轴采集到的三轴磁感应强度的最 大值和最小值。4. 根据权利要求3所述的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其 特征在于:步骤S4用共辄梯度方法建立对扩展卡尔曼滤波观测模型过程如下:将理想状态 均匀重力场和地磁场作为参考方向，通过导航坐标系下四元数转换到机体坐标系下得到三 轴加速度[a'bx a'by a'bz]T和磁感应强度[m'bx m'by m'bz]Tj#转换得到的三轴加速度减去 机体坐标系下测得的重力加速度在三轴上的投影，且将转换得到的磁感应强度减去机体坐 标系下测得的三轴磁感应强度，得到测量误差函数e( q)，利用共辄梯度法求出使测量误差函 数e(q)最小的测量四元数，即利用共辄梯度方法建立扩展卡尔曼滤波观测模型。5. 根据权利要求4所述的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其 特征在于:步骤S5建立对扩展卡尔曼滤波的过程模型的具体过程如下： 首先由四元数的微分值纟与角速度w的关系可以得到扩展卡尔曼滤波的状态等式：(17) (18) Ω[?]为状态转移矩阵，Wx、Wy、Wz分别表示陀螺仪传感器测量的三轴角速度表示四元 数的微分;将连续时间模型转化为离散时间模型，令系统采样时间为Ts，离散时间模型为： q(k+i) = exp( Ω kTs)*qk (19)式中q〇〇表示第k次采样，对exp( QkTs)进行泰勒级数展开保留一阶和二阶项，可得公式 (20)： (20) 式中： Δ Θ2= (wxTs)2+(wyTs)2+(wzTs)2 (21) 13*3表示三维单位向量，Wx、Wy、Wz分别表示陀螺仪传感器测量的三轴角速度。6. 根据权利要求5所述的共辄梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其 特征在于:步骤S5利用扩展卡尔曼滤波器估算出最优四元数包括如下步骤:首先利用步骤 S5的过程模型建模估算出下一个时刻的四元数，然后进行误差协方差更新，第三步利用更 新的协方差计算出卡尔曼增益，第四步利用步骤S4中观测模型求解出的测量四元数减去第 一步估计出来的下一个时刻的四元数，乘上第三步求出的卡尔曼增益，加上上一个时刻的 四元数就是扩展卡尔曼滤波器求出的最优四元数;之后更新下一时刻协方差为下一次计算 做准备。
【文档编号】G01C21/20GK105890598SQ201610216667
【公开日】2016年8月24日
【申请日】2016年4月8日
【发明人】陈洋, 龙文, 吴怀宇, 程磊, 王正熙
【申请人】武汉科技大学