您的位置 首页 IOT

一种常见的四轴飞行器姿势解算办法剖析

全国各地已经陆续开放低空管制,北京也将在2015年全面开放低空领域,这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。实际上,近年四轴飞行器发展相当迅速,国内的航拍水平越

  全国各地现已连续敞开低空操控,北京也将在2015年全面敞开低空范畴,这对低空飞翔器将是一个非常严重的好消息!低空飞翔器也将迎来一个新的开展春天。实际上,近年四轴飞翔器开展适当敏捷,国内的航拍水平越来越高,顺丰及亚马逊已在测验将无人机用于快递职业。越来越多的人开端重视并研讨四轴飞翔器。

  本文将剖析一种常见的四轴飞翔器姿势解算办法,Mahony的互补滤波法。此法简略有用,期望能给学习四轴飞翔器的朋友们带来协助。关于姿势解算和滤波的理论知识,引荐秦永元的两本书,一是《惯性导航》,现在已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论根底,可在书中寻觅。

  一起欢迎到论坛发帖沟通:http://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的互补滤波函数,的确很奇妙,使用叉乘差错来批改四轴的姿势,姿势解算速度越快,则解算的精度越高。在许多国内开源程序中,也是用到了这种办法。在解四元数微分方程时,该程序用到了一阶毕卡解法。相同可用于解四元数微分方程的还有龙格-库塔法,因为篇幅有限,此处就不介绍龙格-库塔法了,有爱好的网友请自行查阅相关材料。

声明:本文内容来自网络转载或用户投稿,文章版权归原作者和原出处所有。文中观点,不代表本站立场。若有侵权请联系本站删除(kf@86ic.com)https://www.86ic.net/yingyong/iot/118085.html

为您推荐

联系我们

联系我们

在线咨询: QQ交谈

邮箱: kf@86ic.com

关注微信
微信扫一扫关注我们

微信扫一扫关注我们

返回顶部