电子产品世界 » 论坛首页 » DIY与开源设计 » 电子DIY » 【讨论帖】姿态解算相关知识储备

共22条 1/3 1 2 3 跳转至

【讨论帖】姿态解算相关知识储备

高工
2014-05-06 00:11:12    评分

想了一下,还是单独开个贴拿出来大家伙讨论一下更合适。

欢迎各位发表自己的意见和看法。


最近四轴没怎么动,都去看了些基础的理论

有点小心得,研究的时间不长,难免有错误的地方,欢迎各位大神来喷

------------------------------------我割------------------------------------------

在了解四轴的数据融合前,必须要了解

微型四轴里面,为什么要进行数据融合?

其实数据融合(data fusion)是一门很复杂的学问,在很多领域都是重点研究的方向。

很多时候,我们所需要的最终数据可能由若干个看上去相互独立

但是在某一个方面具有一定相干性的基础数据构成或者计算得出

每个独立基础数据的来源都各有特点

有些抗干扰能力强,但是采集的时间过长或者条件变化的情况下,会出现较大的误差

有些很容易受到外界条件的干扰,但是在某个很小的时间范围内,对外表现非常精确。

拿烈火的四轴飞行器来说,采用了MPU6050传感器,能够采集加速度和角速度

而我们最终需要获取的是飞行器的姿态角度

其实这里的姿态角度是绝对的

是指MPU6050自身的三维坐标与自然界(或者说地表,这里把地表看成一个平面,因为飞行器能够飞行的距离相对于地球表面来讲,小得几乎可以忽略,在这个区间内,地表可以看成一个平面,感觉是不是又回到了高数世界?)三维坐标之间的夹角,这个夹角实际上能够通过加速度或者角速度计算出来

譬如,角速度的一次积分,结果就是角度;加速度与重力之间的角度,则可以通过三角关系得出。

打字手抽了,抽根烟先。。。。。。。。。。。。。。。。。。。。。。。。。。。。。

但是,单纯用角速度或者加速度来计算姿态角度,实不可取的,为毛?

对于MPU6050测得的角速度而言,它噪声很小,收到运动的影响相对不大

但是由于积分运算的累积,长时间使用角速度计算姿态角度,会产生很大的误差

所以很小的一段时间内,用角速度积分得到的姿态角度相对比较精确

对于MPU6050测得的加速度而言,由于振动等运动环境影响

很容易使输出的加速度值发生比较大的变化,也就是噪声比较大,有一定的误差

一旦发生剧烈的变加速运动,加速度和重力之间的夹角的计算就不那么准确了

但是从长期来看,加速度相对较为稳定。

也就是说,如果是静止或者某种理想的运动(加速度趋近恒定),加速度计算出来的姿态角度还是不错的。

从上面的分析,就能知道。

如果需要在四轴中计算姿态角度,单纯使用加速度或者角速度来计算姿态角度是不合适的

必须处理加速度和角速度,然后通过二者来计算出一个相对精确的姿态角度。

一般的处理的原则是,以陀螺仪测得的数据为主,加速度进行校正(具体系统,具体处理)。

或者说,长期信赖加速度,而短期信赖角速度。

这个处理的过程就是一次数据融合的过程。

OK,再聊聊滤波。

这个应该是比较好理解的了

在一段时间的数据采样中,一般会出现一些在我们预值以外的数据出现

滤波的作用就是把这些误差比较大的数据给干掉。

常用的滤波算法不少,Kalman、mahony等等

还有一些自创神奇算法,前几天看到一个德国鬼子的滤波算法

很神奇,基本上没有很复杂的运算

听说效果不错,但是没看懂,能力有限。

----------------------------------割割更健康-------------------------------------

四旋翼这个东西还是蛮深的

涉及到不少领域的知识

如果能够自己动手做一次,那可牛大发了

码字手都发抖了

让射精、加分、上首页神马的来得更猛烈些吧~~~~~~~~




专家
2014-05-06 00:48:01    评分
2楼

经验之谈!!

——————————————割割————————————————————

我顶,先送上10个积分。


院士
2014-05-06 09:16:28    评分
3楼

看着有点意思,先设定成精华。

养成总结习惯非常好


工程师
2014-05-06 09:53:56    评分
4楼
看的我头大,不过这个改天做四轴是避免不了的。。。

高工
2014-05-06 12:50:16    评分
5楼

讨论贴好啊,论坛里就该多讨论讨论!

我也凑凑热闹哦,就当抛砖引玉了~

——————————————割割———————————————————

1,对加速度和角速度求得的角度用互补滤波,得到的角度会更准确,即用加速度求得的角度来纠正陀螺仪积分的偏差。当然也可以用卡尔曼滤波,但是卡尔曼滤波系数比较难搞定,没有深入研究。

2,看的飞控代码还不多,目前感觉四元数做姿态解算,一般是用一阶毕卡解法来解四元数微分方程。龙格库塔法也能解四元数微分方程,但是还不了解,没用过。

——————————————割割———————————————————

再搞个直白一点的例子:
机体好似一条船,地理就是那地图,姿态就是航向(船头在地图上的方位),重力和地磁是地图上的灯塔,陀螺/积分公式是舵手,加计和电子罗盘是瞭望手。
舵手负责估计和把稳航向,他相信自己,本来船向北开的,就一定会一直往北开,觉得转了90度弯,那就会往东开。
当然如果舵手很牛逼,也许能估计很准确,维持很长时间。不过只信任舵手,肯定会迷路,所以一般都有地图和瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向,算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位,那肯定船的当前航向有偏差了,偏差就是ERR=X-Y。
舵手收到瞭望手给的ERR报告,觉得可靠,那就听个90%*ERR,觉得天气不好、地图误差大,那就听个10%*ERR,根据这个来纠正估算航向。。

上次看到一个帖子,说的也是德国人的算法,说效果很不错,也很神奇,只可惜没有相关代码。有机会还是找找来看看,肯定有收获。

 


高工
2014-05-06 13:54:57    评分
6楼
堂主V5 一起来玩玩呀。

高工
2014-05-06 13:56:18    评分
7楼

多谢老王 我还更想看到更多参与者的见解和看法 

 毕竟一个人看着玩意儿还是比较孤单的


高工
2014-05-06 13:57:28    评分
8楼
不用改天了 就今天了 快申请加入哇

高工
2014-05-06 14:00:17    评分
9楼

蛮生动的比喻 

 浏览了一些开源的代码 

 感觉融合离不开滤波,滤波是必须的 

 版主拿点干货出来学习一下呀。


高工
2014-05-06 14:43:58    评分
10楼

以下是从阿莫论坛转来的,很干的干货啊,呵呵

这就是mahony滤波算法吧!有互补滤波的意思,效果还不错!

虽然对阿莫这个论坛有自己的看法,但是看看一些精华帖子还是好的

 

------------------------------------------------------
来点干货,注意以下的欧拉角都是这样的顺序:先航向-再俯仰-然后横滚
公式截图来自:袁信、郑锷的《捷联式惯性导航原理》,邓正隆的《惯性技术》。
--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要,因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 为9.81米/秒^2,accel.xyz的都为这个单位,算出来的euler.xyz单位是弧度
航向imu.euler.z可以用电子罗盘计算
--------------------------------------------------
欧拉角微分方程
如果用欧拉角算法,那么这个公式就够了,不需要来回转换。

矩阵上到下三个角度(希腊字母)是roll pitch和yaw,公式最左边的上面带点的三个是本次更新后的角度,不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度,单位为弧度,计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.


--------------------------------------------------
以下是四元数
--------------------------------------------------
四元数初始化
q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化

--------------------------------------------------
四元数微分方程
四元数更新算法,一阶龙库法,同样4个量(入、P1-3)也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度,比如欧拉角微分方程中的0.01弧度/秒,T为更新周期,比如上面的0.02秒。

再来一张,另外一本书上的,仔细看和上面是一样的delta角度,就是上面的角速度*周期,单位为弧度

--------------------------------------------------
四元数微分方程更新后的规范化
每个周期更新完四元数,需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。
求q0-3的平方和,再开根号算出的向量长度length。然后每个q0-3除这个length。

--------------------------------------------------
四元数转欧拉角公式
把四元数转成了方向余弦矩阵中的几个元素,再用这几个元素转成了欧拉角
先从四元数q0-3转成方向余弦矩阵:

再从方向余弦矩阵转成欧拉角


代码:
        //更新方向余弦矩阵
        t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
        t12=2.0*(q.q1*q.q2+q.q0*q.q3);
        t13=2.0*(q.q1*q.q3-q.q0*q.q2);
        t21=2.0*(q.q1*q.q2-q.q0*q.q3);
        t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
        t23=2.0*(q.q2*q.q3+q.q0*q.q1);
        t31=2.0*(q.q1*q.q3+q.q0*q.q2);
        t32=2.0*(q.q2*q.q3-q.q0*q.q1);
        t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
        //求出欧拉角
        imu.euler.roll = atan2(t23,t33);
        imu.euler.pitch = -asin(t13);
        imu.euler.yaw = atan2(t12,t11);
        if (imu.euler.yaw < 0){
                imu.euler.yaw += ToRad(360);
        }
----------------------------------------------------
以下代码摘自网上,很巧妙,附上注释,有四元数微分,有加计耦合。
没电子罗盘,其实耦合原理也一样。

 

 

//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "IMU.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f                // half the sample period

//---------------------------------------------------------------------------------------------------
// Variable definitions

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

//====================================================================================================
// Function
//====================================================================================================

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;        
      
        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);     
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;     
把加计的三维向量转成单位向量。
      

        // estimated direction of gravity
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到机体坐标系,正好是这三个元素。
所以这里的vx\y\z,其实就是当前的欧拉角(即四元数)的机体坐标参照系上,换算出来的重力单位向量。


        // error is sum of cross product between reference direction of field and direction measured by sensor
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

axyz是机体坐标参照系上,加速度计测出来的重力向量,也就是实际测出来的重力向量。
axyz是测量得到的重力向量,vxyz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示,exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

 

        // integral error scaled integral gain
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;

        // adjusted gyroscope measurements
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;

用叉积误差来做PI修正陀螺零偏

 


        // integrate quaternion rate and normalise
        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; 
四元数微分方程

      

        // normalise quaternion
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
四元数规范化
}

//====================================================================================================
// END OF CODE
//====================================================================================================

 


共22条 1/3 1 2 3 跳转至

回复

匿名不能发帖!请先 [ 登陆 注册 ]