共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法技术

本发明专利技术涉及共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，首先采集传感器信息获取四旋翼飞行状态，然后利用共轭梯度方法进行观测模型建模，进而进行过程模型建模，将共轭梯度方法得到的四元数作为扩展卡尔曼滤波的测量值；最后利用扩展卡尔曼滤波法得到最佳的四元数，解算出四旋翼的三个姿态角。相比单纯的梯度方法和互补滤波方法，本发明专利技术的扩展卡尔曼滤波法考虑到了系统误差和传感器的测量噪声，因此估计的四元数有更好的准确性。利用共轭梯度方法得到观测四元数运用到扩展卡尔曼滤波中，可以避免计算复杂的线性化观测模型。

【技术实现步骤摘要】

本专利技术涉及一种四旋翼无人机姿态航向数据测量方法，特别是涉及一种共轭梯度与扩展卡尔曼结合的对四旋翼无人机(以下简称四旋翼)姿态解算方法，主要用于军事侦察，农业植保，电力巡检，测绘，航拍等。
技术介绍
随着机器人和航天航空技术的不断发展，四旋翼逐渐成为了国内外学者和航拍发烧友们的研究热点。四旋翼是一种具有垂直起降与空中悬停等特殊飞行能力的多旋翼无人飞行器。在飞行过程中，通过改变四个旋翼的转速，可以改变各种飞行姿态。正因为四旋翼无人机具有垂直起降、空中定点悬停、可以实现六自由度的飞行等独特的飞行能力，使其在军事领域和民用领域得到了广泛的应用。姿态解算是四旋翼的核心部分，其担负着为四旋翼实时提供可靠的飞行状态数据的重任。目前运用比较广泛的姿态解算方法主要是互补滤波、卡尔曼滤波法和梯度下降法的方法。互补滤波方法和梯度下降方法计算简单，利用了加速度传感器和地磁传感器去补偿由于陀螺仪传感器随时间产生漂移造成的误差，但是都没有考虑系统本身的噪声和传感器的测量误差。互补滤波方法需要选取动态的权值去适应不同的飞行状态，大部分运用者选择给一个比较大的权值去适应快速飞行，这将会导致飞行器静态性能欠佳。卡尔曼滤波方法中考虑了系统的测量误差和传感器的测量噪声，相比前两种方法有更好的准确性，且是运用最广泛的姿态解算方法。一般的卡尔曼方法中需要线性化观测模型，而线性化观测模型需要面临求解雅各比矩阵的问题，这将会带来很大的计算量。
技术实现思路
本专利技术所要解决的技术问题是，针对四旋翼姿态解算的问题，提供一种能够实时获取姿态且精度较高的姿态解算方法，从而更好的控制四旋翼的飞行状态。为解决上述技术问题，本专利技术采用的技术方案是：一种共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度传感器、地磁传感器，单片机与地面系统上位机通讯连接；其特征在于包括如下步骤：步骤S1：采集传感器信息：在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感器采集四旋翼飞行状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋翼加速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度；所述机体坐标系是固连于四旋翼的参考坐标系；步骤S2：分别对陀螺仪、加速度计做去零飘处理：步骤S3：采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影的影响；对三轴磁感应强度做归零归一处理；步骤S4：利用共轭梯度方法建立对扩展卡尔曼滤波观测模型；步骤S5：建立扩展卡尔曼滤波器的过程模型：将共轭梯度法求出的使误差函数e(q)最小的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四元数；步骤S6：联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态角方程；将步骤S5估算出的最优四元数带入三个姿态角方程解算出四旋翼的三个姿态角：航向角γ，俯仰角θ，横滚角上述技术方案中，步骤S2对陀螺仪、加速度计做去零飘处理的具体做法是将四旋翼保持水平，取200次值取平均值，这个平均值就为零飘值，以后取得的值都减去该零飘值。上述技术方案中，步骤S3对地磁传感器测得的三轴磁感应强度做归零归一处理的具体过程如下：首先，将安装有地磁传感器的四旋翼水平放置，然后不断转动四旋翼让地磁传感器旋转出一个圆球，同时旋转过程中下位机不断的将数据发送给上位机；进而，上位机对接收到的数据进行处理，分别找出x轴、y轴和z轴各单个轴的最大值和最小值，最后进行归零归一计算：归零，就是要使单个轴的最大值和最小值的绝对值相等，或者说在坐标轴上对称；归一，就是使两个轴的数据范围趋于一致；计算公式如下：Hxi=Hsxi-Hsxmax+Hsxmin2---(1)]]>Hyi=Hsxmax-HsxminHsymax-Hsymin*(Hsyi-Hsymax+Hsymin2)---(2)]]>Hzi=Hsxmax-HsxminHszmax-Hszmin*(Hszi-Hszmax+Hszmin2)---(3)]]>式中，x轴为长轴，y轴为短轴；Hxi、Hyi、Hzi为x轴、y轴和z轴各单个轴经过归零归一处理后的标定值；Hsxi、Hsyi、Hszi分别表示在x轴、y轴、z轴采集到的三轴磁感应强度；Hsxmax、Hsxmin、Hsymax、Hsymin、Hszmax、Hszmin分别表示在x轴、y轴、z轴采集到的三轴磁感应强度的最大值和最小值。上述技术方案中，步骤S4用共轭梯度方法建立对扩展卡尔曼滤波观测模型过程如下：将理想状态均匀重力场和地磁场作为参考方向，通过导航坐标系下四元数转换到机体坐标系下得到三轴加速度[a'bx a'by a'bz]T和磁感应强度[m'bx m'by m'bz]T；将转换得到的三轴加速度减去机体坐标系下测得的重力加速度在三轴上的投影，且将转换得到的磁感应强度减去机体坐标系下测得的三轴磁感应强度，得到测量误差函数e(q)，利用共轭梯度法求出使测量误差函数e(q)最小的测量四元数，即利用共轭梯度方法建立扩展卡尔曼滤波观测模型。上述技术方案中，步骤S5建立对扩展卡尔曼滤波的过程模型的具体过程如下：首先由四元数的微分值与角速度w的关系可以得到扩展卡尔曼滤波的状态等式：q·=Ω[w]*q---(17)]]>Ω[w]=120-wx-wy-wzwx0wz-wywy-wz0wxwzwy-wx0---(18)]]>Ω[w]为状态转移矩阵，wx、wy、wz分别表示陀螺仪传感器测量的三轴角速度；表示四元数的微分。将连续时间模型转化为离散时间模型，令系统采样时间为Ts，离散时间模型为：q(k+1)＝exp(ΩkTs)*qk (19)式中q(k)表示第k次采样，对exp(ΩkTs)进行泰勒级数展开保留一阶和二阶项，可得公式(20)：q(k+1)=[I3*3(1-Δθ28)+12ΩkTs]qk---(20)]]>式中：Δθ2＝(wxTs)2+(wyTs)2+(wzTs)2 (21)I3*3表示三维单位向量，wx、wy、wz分别表示陀螺仪传感器测量的三轴角速度。上述技术方案中，步骤S5利用扩展卡尔曼滤波器估算出最优四元数包括如下步骤：首先利用步骤S5的过程模型建模估算出下一个时刻的四元数，然后进行误差协方差更新，第三步利用更新的协方差计算出卡尔曼增益，第四步利用步骤S4中观测模型求解出的测量四元数减去第一步估计出来的下一个时刻的四元数，乘上第三步求出的卡尔曼增益，加上上一个时刻的四元数就是扩展卡尔曼滤波器求出的最优四元数；之后更新下一时刻协方差为下一次计算做准备。本专利技术的扩展卡尔曼滤波法考虑到了系统误差和传感器的测量噪声，因此估计的四元数有更好的准确性。利用共轭梯度方法得到观测四元数运用到扩展卡尔曼滤波中，可以避免计算复杂的线性化观测模型。本专利技术选择的共轭梯度与扩展卡尔曼结合的姿态解算方法，利用共轭梯度方法求出观测四元数运用于扩展卡尔曼滤波中，不需要考虑线性化观测模型的问题。共轭梯度法相比梯度下降方法，下降的步长为实时修正的动态值，精准度更高，更容易跟踪各种飞行状态。除此之外，本专利技术运用了目前最流行的本文档来自技高网...

【技术保护点】
一种共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度传感器、地磁传感器，单片机与地面系统上位机通讯连接；其特征在于包括如下步骤：步骤S1：采集传感器信息：在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感器采集四旋翼飞行状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋翼加速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度；所述机体坐标系是固连于四旋翼的参考坐标系；步骤S2：分别对陀螺仪、加速度计做去零飘处理：步骤S3：采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影的影响；对三轴磁感应强度做归零归一处理；步骤S4：利用共轭梯度方法建立对扩展卡尔曼滤波观测模型；步骤S5：建立扩展卡尔曼滤波器的过程模型：将共轭梯度法求出的使误差函数e(q)最小的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四元数；步骤S6：联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态角方程；将步骤S5估算出的最优四元数带入三个姿态角方程解算出四旋翼的三个姿态角：航向角γ，俯仰角θ，横滚角...

【技术特征摘要】
1.一种共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，所述四旋翼包含单片机以及与单片机相连的陀螺仪、加速度传感器、地磁传感器，单片机与地面系统上位机通讯连接；其特征在于包括如下步骤：步骤S1：采集传感器信息：在机体坐标系下，通过陀螺仪、加速度传感器、地磁传感器采集四旋翼飞行状态信息，其中，陀螺仪测量四旋翼三轴角速度，加速度传感器测量四旋翼加速度在三轴上的投影，地磁传感器测量四旋翼所在位置的三轴磁感应强度；所述机体坐标系是固连于四旋翼的参考坐标系；步骤S2：分别对陀螺仪、加速度计做去零飘处理：步骤S3：采用巴特沃斯低通滤波器滤除机体加速度对重力加速度在三轴上的投影的影响；对三轴磁感应强度做归零归一处理；步骤S4：利用共轭梯度方法建立对扩展卡尔曼滤波观测模型；步骤S5：建立扩展卡尔曼滤波器的过程模型：将共轭梯度法求出的使误差函数e(q)最小的四元数作为测量值，设计扩展卡尔曼滤波器，利用扩展卡尔曼滤波器估算出最优四元数；步骤S6：联立欧拉方向余弦矩阵和四元数矩阵，得到三个利用四元数形成的姿态角方程；将步骤S5估算出的最优四元数带入三个姿态角方程解算出四旋翼的三个姿态角：航向角γ，俯仰角θ，横滚角2.根据权利要求1所述的共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其特征在于：步骤S2对陀螺仪、加速度计做去零飘处理的具体做法是将四旋翼保持水平，取200次值取平均值，这个平均值就为零飘值，以后取得的值都减去该零飘值。3.根据权利要求2所述的共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法，其特征在于：步骤S3对地磁传感器测得的三轴磁感应强度做归零归一处理的具体过程如下：首先，将安装有地磁传感器的四旋翼水平放置，然后不断转动四旋翼让地磁传感器旋转出一个圆球，同时旋转过程中下位机不断的将数据发送给上位机；进而，上位机对接收到的数据进行处理，分别找出x轴、y轴和z轴各单个轴的最大值和最小值，最后进行归零归一计算：归零，就是要使单个轴的最大值和最小值的绝对值相等，或者说在坐标轴上对称；归一，就是使两个轴的数据范围趋于一致；计算公式如下：Hxi=Hsxi-Hsxmax+Hsxmin2---(1)]]>Hyi=Hsxmax-HsxminHsymax-Hsymin*(Hsyi-Hsymax+Hsymin2)---(2)]]>Hzi=Hsxmax-HsxminHszmax-Hszmin*(Hszi-Hszmax+Hszmin2)---(3)]]>式中，x轴为长轴，y轴为短轴；Hxi、Hyi、Hzi为x轴、y轴和z轴各单个轴经过归零归一处理后的标定值；Hsxi、Hsyi、Hszi分别表示在x轴、y轴、z轴采集到的三轴磁感应强度；...

【专利技术属性】
技术研发人员：陈洋，龙文，吴怀宇，程磊，王正熙，
申请(专利权)人：武汉科技大学，
类型：发明
国别省市：湖北;42