电子产品世界 » 论坛首页 » 电子DIY » QuadCopter DIY » 捷联惯导算法心得(转载)


共9条 1/1 1 跳转至

捷联惯导算法心得(转载)

高工
2014-08-20 13:11:07    评分
1、四个概念:“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。

地理坐标系:东、北、天,以下简称地理。在这个坐标系里有重力永远是(0,0,1g),地磁永远是(0,1,x)(地磁的垂直不关心)两个三维向量。
机体坐标系:以下简称机体,上面有陀螺、加计、电子罗盘传感器,三个三维向量。
换算公式:以下简称公式,公式就是描述机体姿态的表达方法,一般都是用以地理为基准,从地理换算到机体的公式,有四元数、欧拉角、方向余弦矩阵。
换算公式的系数:以下简称系数,四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。

姿态,其实就是公式+系数的组合,一般经常用人容易理解的公式“欧拉角”表示,系数就是横滚xx度俯仰xx度航向xx度。

2、五个数据源:重力、地磁、陀螺、加计、电子罗盘,前两个来自地理,后三个来自机体

3、陀螺向量:基于机体,也在机体上积分,因为地理上无参考数据源,所以很独立,直接在公式的老系数上积分,得到新系数。
狭义上的捷联惯导算法,就是指这个陀螺积分公式,也分为欧拉角、方向余弦矩阵、四元数,他们的积分算法有增量法、数值积分法(X阶龙格-库塔)等等

4、加计向量、重力向量:加计基于机体,重力基于地理,重力向量(0,0,1g)用公式换算到机体,与机体的加计向量算出误差。理论上应该没有误差,这误差逆向思维一下,其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数(横滚、俯仰),也就是姿态。

5、电子罗盘向量、地磁向量:同上,只不过要砍掉地理上的垂直向量,因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数(航向)。

6、就这样,系数不停地被陀螺积分更新,也不停地被误差修正,它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法(因为运算量较少、无奇点误差),最后用欧拉角输出控制PID(因为角度比较直观),那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式,它们之间都有转换算法。

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



关键词: 捷联     惯导    

高工
2014-08-20 13:11:41    评分
2楼
------------------------------------------------------
来点干货,注意以下的欧拉角都是这样的顺序:先航向-再俯仰-然后横滚
公式截图来自:袁信、郑锷的《捷联式惯性导航原理》,邓正隆的《惯性技术》。
--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要,因为加计和电子罗盘给出欧拉角的描述方式比较方便。
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);

 

----------------------------------------------------


高工
2014-08-20 13:12:49    评分
3楼
以下代码摘自网上,很巧妙,附上注释,有四元数微分,有加计耦合。
没电子罗盘,其实耦合原理也一样。
//=====================================================================================================
// 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 

//----------------------------------------------------------------------------------------------------
// 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
//====================================================================================================

 

 

 

高工
2014-08-20 13:14:15    评分
4楼

陀螺一般有零点漂移、非线性、加速度影响等等多种误差。
其中零点漂移影响最大,但在初始化(保持静止读取一段时间平均值作为零点)后,漂移还可以忍受。不过温度变化大,漂移也比较厉害。
没有磁力计,是无法对航向角做纠正融合的。

以下代码是上面那段代码的磁力计+加计+陀螺版,没仔细研究过,粗看看像是把磁阻mxyz的向量转到地理坐标系,然后用地理坐标系的正北向标准磁场向量取代变成bxyz?又转回机体坐标系变成wxyz,最后和原始磁阻测量值mxyz做向量叉积来修正陀螺。
想学习的就研究研究吧

我自己做的磁阻耦合很简单很粗暴。把磁阻方向角解出来,和四元数解除的欧拉角航向角做个差,再用k滤波器把差值融合到欧拉角航向角上去,然后直接把欧拉角转成四元数。

//=====================================================================================================
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// 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 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//
//=====================================================================================================

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

#include "AHRS.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 AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
        float norm;
        float hx, hy, hz, bx, bz;
        float vx, vy, vz, wx, wy, wz;
        float ex, ey, ez;

        // auxiliary variables to reduce number of repeated operations
        float q0q0 = q0*q0;
        float q0q1 = q0*q1;
        float q0q2 = q0*q2;
        float q0q3 = q0*q3;
        float q1q1 = q1*q1;
        float q1q2 = q1*q2;
        float q1q3 = q1*q3;
        float q2q2 = q2*q2;   
        float q2q3 = q2*q3;
        float q3q3 = q3*q3;          
        
        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);       
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;
        norm = sqrt(mx*mx + my*my + mz*mz);          
        mx = mx / norm;
        my = my / norm;
        mz = mz / norm;         
        
        // compute reference direction of flux
        hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
        hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
        hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
        bx = sqrt((hx*hx) + (hy*hy));
        bz = hz;        
        
        // estimated direction of gravity and flux (v and w)
        vx = 2*(q1q3 - q0q2);
        vy = 2*(q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3;
        wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
        wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
        wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
        
        // error is sum of cross product between reference direction of fields and direction measured by sensors
        ex = (ay*vz - az*vy) + (my*wz - mz*wy);
        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
        
        // 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;
        
        // 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
//====================================================================================================

 

 


高工
2014-08-20 13:16:13    评分
5楼
补偿,得有观测者才能对测量者做纠正。
加计本来是观测者,对陀螺补偿。
对外力加速度来说,加计属于测量者,没有外力的观测者,无法补偿加计。
如果是震动,可以调小PI参数,靠长时间的数值平均,减小震动引起的加速度误差。
而机体转弯的离心力可以用 GPS地速×陀螺三维角速度,根据离心力公式算出三维离心加速度,去减掉转弯带来的加速度影响。
但是机体的纵向加速度就没法补偿了,不过一般情况下长时间单方向加速的情况较少,误差可以容忍。

如果要高精度的姿态测算,那除非再引入观测者,比如GPS的坐标定位信息。也就是所谓的GPS/INS组合导航,一般是卡尔曼算法。
举个例子,比如惯导中当前估算速度为匀速xx米/秒,移动方向为北向,所以估算1秒后坐标会向北移动XX米。
1秒后GPS给出信息,和惯导估算的相比,坐标向北多移动了YY米。,变成了XX+YY米。
那么应该是有外力加速度,导致机体北向速度变快,本周期平均值为YY-XX米/秒^2(只是举个例子,数值不做准)系统就会这外力加速度补偿到加计的测量值里去。

GPS/INS组合导航,系统就会运算出姿态和坐标定位信息,是融合过的,都非常准。卡尔曼算法很难,我不懂,只知道个大概原理。

一言蔽之,这些算法都是以测量者的数值为基础,用观测者观察到的其他方面的数值,用一个转换算法去推断测量者的数值误差,然后纠正之。而这就是卡尔曼算法的流程。

高工
2014-08-20 13:16:44    评分
6楼
这里误差没说清楚,不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与重力向量重合,需要绕什么轴,转多少角度
逆向推理一下,这个叉积在机体三轴上的投影,就是加计和重力之间的角度误差。
也就是说,如果陀螺按这个叉积误差的轴,转动叉积误差的角度(也就是转动三轴投影的角度)那就能把加计和重力向量的误差消除掉。(具体可看向量叉积的定义)
如果完全按叉积误差转过去,那就是完全信任加计。如果一点也不转,那就是完全信任陀螺。
那么把这个叉积的三轴乘以X%,加到陀螺的积分角度上去,就是这个x%互补系数的互补算法了。

ps:实际上叉积的length是两向量夹角的正弦,而且必须在±90度以内,并不完全与误差角度成线性正比。
如果转成三轴夹角,按欧拉角的转动顺序分解到三轴上去,会很麻烦。
这里的叉积算法把sin误差当成角度误差,并无视欧拉角的转动顺序,在误差较小的时候,并不会有影响。
因为这个修正对误差来说,是收敛的,正常情况下误差只会越来越小。
为了解释方便,所以上面就把叉积等同于角度误差了。

高工
2014-08-20 13:17:09    评分
7楼
仔细看了一会,上面的磁力计修正流程已经想通了,看看大家想没想通吧。

陀螺的零点漂移可以靠加计和磁力计来补偿,不过加计和磁力计的零点漂移、椭圆修正就没法动态补偿了。因为没有观测者。
只能在初始化的时候修正,但是温度的影响就不太好办。
gps/ins是王道,可惜高阶卡尔曼实在不好弄,谁有心得分享一下吧。

不过一般玩玩,这些算法够了。

菜鸟
2014-12-19 15:28:04    评分
8楼
谢谢分享,很好的资料

专家
2018-04-10 09:11:00    评分
9楼

向大神学习了。


共9条 1/1 1 跳转至

回复

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