找回密码
 立即注册

QQ登录

只需一步,快速开始

点击进入授权页面

只需一步,快速开始

  • QQ空间
  • 回复
  • 收藏

参加活动: 1

组织活动: 0

  • TA的每日心情
    奋斗
    2016-5-17 22:16
  • 签到天数: 275 天

    [LV.8]以坛为家I

    本帖最后由 对折之内 于 2016-2-27 01:09 编辑

           在一些对角度要求不高的场合,要得到角度,只用加速度计就可以了,比如做一个自平衡小车。可是要得到一个更加精确的角度,就需要用到陀螺仪了,比如四旋翼飞行器。陀螺仪测量的是角度的变化率,对这个变化率积分,就可以得到角度值。
        我们一般用的是比较常见的卡尔曼滤波(当然还有其他融合方式)。我们先来说说用加速度计和陀螺仪测量倾角的原理,等会儿再说卡尔曼滤波的原理。(以下测量倾角的说明摘抄自文档《MPU6050数据轻松分析》,原理意思懂了就行。)
        首先我们知道重力加速度可以分解成x,y,z三个方向的分加速度。而加速度计可以测量某一时刻x,y,z三个方向的加速度值。利用各个方向的分量与重力加速度的比值来计算出小车大致的倾角。其实在自平衡小车上非静止的时候,加速度计测出的结果并不是非常精确。因为大家在高中物理的时候都学过,物体时刻都会受到地球的万有引力作用产生一个向下的重力加速度,而小车在动态时,受电机的作用肯定有一个前进或者后退方向的作用力,而加速度计测出的结果是,重力加速度与小车运动加速度合成得到一个总的加速度在三个方向上的分量。这是加速度计测不准的一个原因。我们这里先理想化加速度计不受外界干扰。
        下边我们就开始分析从加速度得到角度的方法。如下图,把加速度计平放,分别画出xyz轴的方向。这三个轴就是我们后边分析所要用到的坐标系。
    图像 1.png
    把mpu6050安装在自平衡车上时也是这样的水平安装在小车底盘上的,假设两个车轮安装时车轴和y轴在一条直线上。那么小车摆动时,参考水平面就是桌面,并且车轴(y轴)与桌面始终是平行的,小车摆动和移动过程中y轴与桌面的夹角是不会发生变化的,一直是0度。发生变化的是x轴与桌面的夹角以及z轴与桌面的夹角,而且桌面与x轴z轴夹角变化度数是一样的。所以我们只需要计算出x轴和z轴中任意一个轴的夹角就可以反映出小车的倾斜的情况了。
    图像 1.png
    为了方便分析,由于y轴与桌面夹角始终不变,我们从y轴的方向俯看下去,那么这个问题就会简化成只有x轴和z轴的二维关系。假设某一时刻小车上加速度计(mpu6050)处于如下状态,下图是我们看到简化后的模型。
    图像 1.png
    在这个图中,y轴已经简化和坐标系的原点o重合在了一起。我们来看看如何计算出小车的倾斜角,也就是与桌面的夹角a。上图g是重力加速度,gx、gz分别是g在x轴和z轴的分量。
    由于重力加速度是垂直于水平面的,得到:
    角a+角b=90度
    X轴与y轴是垂直关系,得到:
    角c+角b=90度
    于是轻松的就可以得出:
    角a=角c
    根据力的分解,g、gx、gz三者构成一个长方形,根据平行四边形的原理可以得出:
    角c=角d

    所以计算出角度d就等效于计算出了x轴与桌面的夹角a。前边已经说过gx是g在x轴的分量,那么根据正弦定理就可以得出:
    Sind=gx/g
    得到这个公式可是还是得不到想要的角度,因为需要计算反正弦,而反正弦在单片机里不是很好计算。

    为了得到角度,于是又查了相关资料,原来在角度较小的情况下,角度的正弦与角度对应的弧度成线性关系。先看看下边的图:
    图像 1.png
    这个图x轴是角度,取值范围是0~90度,有三个函数曲线,分别是:
    Y=sinx  正弦曲线
    Y=x*3.14/180  弧度
    Y=0.92*x*3.14/180   乘以一个0.92系数的弧度
    从图上可以看出,当角度范围是0~29度时:
    sinx=x*3.14/180
    对于自平衡车来说,小车的摆动范围在-29~29度之内,如果超过这个范围,小车姿态也无法调整,所以对于自平衡小车sinx=x*3.14/180基本上是成立的。当然有时候也会担心-29~29度的摇摆范围还是无法满足需求。那可以给上边的公式乘一个系数。得到如下公式:
    Sinx=k*x*3.14/180
    从上边的函数对比图可以看出,当系数取0.92时,角度范围可以扩大到-45~45度。
             经过这一系列的分析,终于得到角度换算方法:
             
    Sind=gx/g
    Sind=k*d*3.14/180
    得到:
    gx/g=k*d*3.14/180
    那么角度就可以通过如下公式计算出:
    d=180*gx/(k*g*3.14)

    而gx可以从加速度计里读出来,所以这下角度就可以轻松得到了。而之前也说过这个角度不是很精确,但是至少可以反映出角度变化的趋势。不过可以通过卡尔曼滤波等算法把加速度计读出的角度和陀螺仪读出的角度结合起来,使小车的角度更加准确。
    通过陀螺仪来测量角度就很简单了,因为陀螺仪读出的是角速度,大家都知道,角速度乘以时间,就是转过的角度。把每次计算出的角度做累加就会等到当前所在位置的角度。先看下图:
    图像 1.png

    假设最初陀螺仪是与桌面平行,单片机每tms读一次陀螺仪的角速度,当读了三次角速度以后z轴转到上图的位置,则在这段时间中转过的角度为x:
    角x=角1+角2+角3
    假设从陀螺仪读出的角速度为w,那总角度为:
    X=(w1*t1+w2*t2+w3*t3)/1000
    假设经过n次,那么总的角度如下:
    X=(w1*t1+w2*t2+w3*t3+…+wn*tn)/1000
    实际上这就是一个积分过程。

    其实这种计算出来的角度也存在一定的误差,而且总的角度是经过多次相加得到的,这样误差就会越积累越大,最终导致计算出的角度与实际角度相差很大。于是也可以使用卡尔曼滤波把加速度计读出的角度结合在一起,使计算出的角度更准确。
    接着我们讲一下卡尔曼滤波的原理:
    新手平衡小车的卡尔曼滤波算法总结.rar (30.84 KB, 下载次数: 646)
    好贴,学习了,谢谢谢谢
    楼主,我想问一个问题,就是MPU6050最终可以计算输出3种数据,分别是3轴加速度,3轴角速度,3轴的角度,比如我要让四旋翼保持平衡,找只需要知道每个方向偏转的角度,然后来调整电机转速就行了,加速度和角速度这两个参数有什么用?
    helloworld123 发表于 2016-3-15 17:48
    楼主,我想问一个问题,就是MPU6050最终可以计算输出3种数据,分别是3轴加速度,3轴角速度,3轴的角度,比 ...

    mpu6050只能得到3轴加速度和3轴角速度,用在四旋翼上的话,就通过前面6个数据计算欧拉角,即:Roll、Pitch、Yaw来控制四旋翼。这就叫姿态解算。3轴的角度也是通过前面6个数据得到的。
    对折之内 发表于 2016-3-16 08:30
    mpu6050只能得到3轴加速度和3轴角速度,用在四旋翼上的话,就通过前面6个数据计算欧拉角,即:Roll、Pitc ...

    明白了,谢谢,X宝上有一种串口MPU6050,内部集成了姿态解算,能直接输出角度数据,可以看一下,不知道怎么样
    对折之内 发表于 2016-3-16 08:30
    mpu6050只能得到3轴加速度和3轴角速度,用在四旋翼上的话,就通过前面6个数据计算欧拉角,即:Roll、Pitc ...

    还有一个问题想请教,就是我要根据三轴方向的倾角来用PID来控制四个电机转速,问题是如果只是X和Y轴有偏差,我也要同时改变4个电机的转速,还是只需要改变2个电机转速就可以了
    helloworld123 发表于 2016-3-16 17:51
    明白了,谢谢,X宝上有一种串口MPU6050,内部集成了姿态解算,能直接输出角度数据,可以看一下,不知道怎 ...

    我买过一个挺好用的,做了一个蓝牙水平仪

    http://www.lab-z.com/diyspy/
    打赏作者鼓励一下!
    Zoologist 发表于 2016-3-16 19:48
    我买过一个挺好用的,做了一个蓝牙水平仪

    http://www.lab-z.com/diyspy/

    100Hz的速率,四轴的话是不是太低了
    helloworld123 发表于 2016-3-16 19:54
    100Hz的速率,四轴的话是不是太低了

    有可能吧

    没搞过四轴
    打赏作者鼓励一下!
    楼组,“接下来,我们开始修改这个程序,使得mpu6050也能使用这个算法。 待续。”这个有吗
    12下一页
    发新帖
    发表评论
    高级模式  
    您需要登录后才可以回帖 登录 | 立即注册  
    关闭

    推荐主题 上一条 /2 下一条