智能车制作

标题: 一种常见的四轴飞行器姿态解算方法分析 [打印本页]

作者: forele    时间: 2014-6-11 11:34
标题: 一种常见的四轴飞行器姿态解算方法分析
本帖最后由 forele 于 2014-6-11 11:35 编辑

 全国各地已经陆续开放低空管制,北京也将在2015年全面开放低空领域,这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。实际上,近年四轴飞行器发展相当迅速,国内的航拍水平越来越高,顺丰及亚马逊已在尝试将无人机用于快递行业。越来越多的人开始关注并研究四轴飞行器。

本文引用地址:eepw.com.cn/article/247809.htm

  本文将分析一种常见的四轴飞行器姿态解算方法,Mahony的互补滤波法。此法简单有效,希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识,推荐秦永元的两本书,一是《惯性导航》,目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础,可在书中寻找。
  同时欢迎到论坛发帖交流:forum.eepw.com.cn/forum/368/1
  下面开始进入正题:
  先定义Kp,Ki,以及halfT 。
  Kp,Ki,控制加速度计修正陀螺仪积分姿态的速度
  halfT ,姿态解算时间的一半。此处解算姿态速度为500HZ,因此halfT 为0.001
  #define Kp 2.0f
  #define Ki 0.002f
  #define halfT 0.001f
  初始化四元数
  float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
  定义姿态解算误差的积分
  float exInt = 0, eyInt = 0, ezInt = 0;
  以下为姿态解算函数。
  参数gx,gy,gz分别对应三个轴的角速度,单位是弧度/秒;
  参数ax,ay,az分别对应三个轴的加速度原始数据
  由于加速度的噪声较大,此处应采用滤波后的数据
  void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
  {
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;
  将加速度的原始数据,归一化,得到单位加速度
  norm = sqrt(ax*ax + ay*ay + az*az);
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;
  把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到机体坐标系,正好是这三个元素。所以这里的vx、vy、vz,其实就是当前的机体坐标参照系上,换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)
  vx = 2*(q1*q3 - q0*q2);
  vy = 2*(q0*q1 + q2*q3);
  vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
  这里说明一点,加速度计由于噪声比较大,而且在飞行过程中,受机体振动影响比陀螺仪明显,短时间内的可靠性不高。陀螺仪噪声小,但是由于积分是离散的,长时间的积分会出现漂移的情况,因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
  在机体坐标参照系上,加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量,就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
  向量间的误差,可以用向量积(也叫外积、叉乘)来表示,ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
  叉乘是数学基础,百度百科里有详细解释。
  ex = (ay*vz - az*vy);
  ey = (az*vx - ax*vz);
  ez = (ax*vy - ay*vx);
  将叉乘误差进行积分
  exInt = exInt + ex*Ki;
  eyInt = eyInt + ey*Ki;
  ezInt = ezInt + ez*Ki;
  用叉乘误差来做PI修正陀螺零偏,通过调节Kp,Ki两个参数,可以控制加速度计修正陀螺仪积分姿态的速度
  gx = gx + Kp*ex + exInt;
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;
  四元数微分方程,没啥好说的了,看上面推荐的书吧,都是理论的东西,自个琢磨琢磨
  实在琢磨不明白,那就把指定的参数传进这个函数,再得到相应的四元数,最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
  四元数单位化
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;
  }
  姿态解算后,就得到了表示姿态的四元数。但四元数不够直观,一般将其转化为欧拉角。转化时根据旋转轴的次序不同,公式也不同。以下给出其中一种公式:
  读完程序,深刻的意识到了理论基础的重要性。Mahony的互补滤波函数,确实很巧妙,利用叉乘误差来修正四轴的姿态,姿态解算速度越快,则解算的精度越高。在许多国内开源程序中,也是用到了这种方法。在解四元数微分方程时,该程序用到了一阶毕卡解法。同样可用于解四元数微分方程的还有龙格-库塔法,由于篇幅有限,此处就不介绍龙格-库塔法了,有兴趣的网友请自行查阅相关资料。


作者: 小胡杨    时间: 2014-6-11 12:56
不错
作者: 暗剑    时间: 2014-6-12 09:55
收藏了~
作者: super_mice    时间: 2014-7-24 14:01
不错
作者: jiushibuzhidao    时间: 2014-7-29 10:29
请问楼主传的参数是直接读到得数据吗?
还有就是这个测试过没啊?我使用这个还想并没有能够解算出姿态角啊!得到四元数后就用的三角函数到欧拉角!但是没有成功啊
作者: 东东东    时间: 2014-8-23 20:55
楼主 你好 请问那个姿态速度hz 是怎么算的? 还有那个ki kp 怎么算的
作者: 六步上篮    时间: 2015-12-9 20:49
不错不错
作者: wijop    时间: 2015-12-18 10:23
慢慢来
作者: wijop    时间: 2015-12-18 10:23
慢慢来
作者: CSU虚心向前    时间: 2016-2-24 15:27
好贴怎么没人顶




欢迎光临 智能车制作 (http://111.231.132.190/) Powered by Discuz! X3.2