[发明专利]共轭梯度与扩展卡尔曼滤波结合的四旋翼姿态解算方法

摘要

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

主权项

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