这是”四轮车驱动控制”系列,分多个小节来介绍:

1. 八.四轮车驱动开发之一:正/逆向运动学分析

2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机

3. 十.四轮车驱动开发之三: 巧用编码器获取电机转速信息

4. 十一.四轮车驱动开发之四: 理解直流电机PID控制器

5. 十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上)
    十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中)
    十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(下)
 

==================================================================

        花了近一个星期的每个晚上,试图将6轴陀螺仪姿态解算算法的每一步前因后果讲清楚,有概念引用,也有公式推导,约2万字,实属不易,转载请注明出处,看了感觉对您有帮助的,劳烦点个赞,谢谢!

        对于我们工作实践中的很多算法,能用不等于会用, 从扎实的理论基础开始,完全掌握它,并能做到根据实际的问题需求去修改调整它,才能把问题解决的更好.

        好了, Enjoy it.

==================================================================

        多年前,工作中就曾用到Mahony的6轴陀螺仪姿态解算算法(以下简称算法),用我当时初看这个算法的的感悟讲就是:"这算法一行一行都能看明白,但两行或多行放一起就蒙圈了."  记得后来把算法背后的知识点弄明白,才敢说,这个算法我看懂了.

        这个算法想讲明白其实并不容易,因为背后的知识点太多,每个细节都讲明白了就够写几个章节了. 特别是自下而上,尝试把各个知识点堆积起来,来讲解这个算法,那就得长篇连载.

        我试图通过另一种方式来讲解这个算法: 即采用自上而下为主,自下而上为辅 相结合的方式,来讲解这个算法. 大致过程就是: 先讲解算法依赖的一些最基本的概念,然后在此基础上从整体上理解算法的基本逻辑,最后针对算法中关键的几个知识点,自上而下挖掘其理论基础;

本文基本目录结构如下:

<上篇>:

一. 算法依赖的最基本概念 

        1. 载体坐标系

        2. 地理坐标系

        3. 欧拉角

        4. 四元数

        5. 常见旋转表示法

二. 算法基本思想

三. 算法源码及居高临下看算法流程框图

<中篇>:

四. 方向余弦矩阵, 欧拉旋转,四元数旋转 等理论基础  

<下篇>:

五.对算法中关键知识点进行重点讲解   

        1. 关键知识点1: 当前姿态四元数q(参考坐标n系)算出重力在三个轴上的分量(载体坐标b系)

        2. 关键知识点2: 用两个向量叉积(也叫向量积,外积),来表示载体坐标系角速度分量误差error

        3. 关键知识点3: 四元数微分方程,一阶毕卡解法,融合修正后陀螺仪姿态到当前四元数q中

        4. 关键知识点4: 把用四元数q表示的姿态转化为用欧拉角表示的姿态

----------------------------------------正文开始--------------------------------------

一. 算法依赖的最基本概念

1. 载体坐标系

        载体坐标系是依附在物体本身上虽物体一起运动的本地坐标系,类似人体理解的上下左右前后.一般陆地车辆的载体坐标系如下图所示,原点固定在物体的重心,沿载体横向指向载体的右侧,沿载体纵向指向载体的前方,沿载体的竖轴指向载体的上方. 三轴的构成的坐标系符合右手直角坐标系法则.(飞行器的载体坐标系一般为前向,这里仅讨论四轮车,具体以实际陀螺仪芯片安装方位为准,算法也要相应做调整)

        车载6轴陀螺仪的角速度输出和加速度计加速度输出,都是针对载体坐标系的.

2. 地理坐标系OENξ

        如下图所示,地理坐标系OENξ的原点O取在载体的重心,E轴指向当地地理方向:东方,N轴指向当地地理方向: 北方,ξ轴沿当地地垂线指向:上方. OENξ三轴的构成的坐标系符合右手直角坐标系法则. 地理坐标系跟随载体运动,但三轴指向不会变化. 类似人体理解的相对自身位置的前后左右上下为载体坐标系,但描述方位的东西南北天地为地理坐标系,你在地球上(仅限地球上)走到哪里地理坐标原点都跟到哪里,但方位始终不变.

        一般情况下,对陆地四轮车而言,地理坐标系OENξ就是导航坐标系. 对于没有加装磁力计的载体,无法准确识别出真实的地理方位,其地理坐标系一般以初始状态为准.

        算法的目的就是: 将针对载体坐标系的角速度和加速度数据,按照一定的算法变换到地理坐标系OENξ, 最终解算出载体在地理坐标系下的用欧拉角表示的姿态: 偏航角(yaw),俯仰角(pitch),翻滚角(roll).

        NOTE: 俯仰角(pitch)和翻滚角(roll),具体哪个是相对于x轴旋转,哪个是相对于y轴旋转,请以实际陀螺仪芯片安装方位为准,算法也要相应做调整.

3. 欧拉角(这里先仅引入概念)

        以四轮车而言, 欧拉旋转就是物体以一定顺序分别绕载体坐标系三个坐标轴(x,y,z轴）旋转一定的角度。这个分别相对于三轴的旋转角度就是欧拉角. 一般沿载体前后纵向轴旋转称翻滚(roll),沿载体横向轴旋转称俯仰(pitch),沿载体上下垂线旋转称偏航(yaw), 旋转完成后载体相对于导航坐标系(地理坐标系)的姿态用欧拉角表示为翻滚角(roll),俯仰角(pitch),偏航角(yaw). 欧拉角是一系列坐标轴旋转顺序的组合.  即它与x,y,z三轴旋转顺序密切相关. 交换顺序就是不同的旋转.

        欧拉旋转可以是以载体坐标系表示,也可以以导航坐标系表示. 如下图:  做图中绕载体坐标系的三次绕轴旋转后,求载体相对于地理坐标系的姿态的欧拉角表示(这里仅引出概念,并不求解).

Note:  注意欧拉旋转和欧拉角的不同和关系: 欧拉旋转是给定三个角度(三轴旋转角度),然后按一定的顺序先后绕三轴旋转指定的角度(这里的轴一般是载体坐标系三轴),其是一个过程. 欧拉角一般指载体静态状态,或一个欧拉旋转完成后,载体坐标系相对于导航坐标系的翻滚角(roll),俯仰角(pitch),偏航角(yaw),其是一个状态值.

         在惯性导航系统中,我们更关心目标载体在任意时刻,载体坐标系和导航坐标系(地利坐标系OENξ)三轴之间的角度关系,即坐标旋转变换.   Note:这里的两个坐标系三轴之间的角度关系不同于欧拉角(roll,pitch,yaw).  但通过两个坐标系三轴之间的关系,可以求出载体在导航坐标下的姿态,用欧拉角(roll,pitch,yaw)表示.

4. 四元数(这里先仅引入概念)

        我们中学学过复数, 形如 z=a+bi（a、b均为实数）的数称为复数。其中，a 称为实部，b 称为虚部，i 称为虚数单位。四元数是复数基础上的扩展, 是由实数加上三个虚数单位 i、j和k 组成，四元数一般可表示为z=a + bi+ cj + dk，其中a、b、c 、d是实数, i² = j² = k² = -1， iº = jº = kº = 1 。对于i、j和k本身的几何意义可以理解为一种旋转，其中i旋转代表Z轴与Y轴相交平面中Z轴正向向Y轴正向的旋转，j旋转代表X轴与Z轴相交平面中X轴正向向Z轴正向的旋转，k旋转代表Y轴与X轴相交平面中Y轴正向向X轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。

        三维坐标系下空间的一个点P(x,y,z),用四元数表示就是p = (0,x,y,z),也有矢量表示法p = (P(x,y,z), 0)或p = (0,P(x,y,z)).

 5. 常见旋转表示法

* 旋转矩阵(更多细节,自行学习,比如3D数学基础)

        旋转矩阵是在用旋转矩阵乘以(不同平台有左乘和右乘之分)一个向量,其结果改变向量的方向但不改变大小的效果.矩阵旋转使用了一个4*4大小的矩阵来表示绕任意轴旋转的变换矩阵.

在计算时，我们将当前向量右乘R.  即:  p’ = pR

*欧拉旋转(更多细节,自行学习)

        一般是先绕X轴旋转α角度,再绕Y轴旋转β角度,最后绕Z轴旋转γ角度. 即 一次只旋转一个分量,按顺序累积.   其旋转矩阵为:

R = 

= 

 在计算时，我们将当前坐标,当做列向量右乘R.  即:  p’ = pR

*四元数旋转(更多细节,自行学习)

         假定某矢量绕通过O点的某轴逆时针转动一个角度θ，则与该矢量固连的动坐标系和参考坐标系间的变换四元数为：q=cos(θ/2)+sin(θ/2)cosα i+ sin(θ/2)cosβj+ sin(θ/2)cosγ k,通常称其为四元数的三角形式，也称特征四元数，其范数为1，在导航应用中一般所应用的四元数均为特征四元数。其标量部分cos(θ/2)表示了转角一半的余弦值，矢量部分则体现了转动轴的方向，α、β、γ是转动轴与参考坐标系各轴间的夹角。

        旋转矢量坐标变换的四元数描述为：p′=qpq−1   根据四元数相关特性有: p’=qpq*, 反过来有: p=q*p’q

*这么多描述旋转的方法,为什么算法使用四元数?

旋转描述法	优点	缺点
矩阵旋转	旋转轴可以是任意向量	旋转其实只需要知道一个向量+一个角度，一共4个值的信息，但矩阵法却使用了16个元素； 矩阵乘法操作时计算量大，造成了空间和时间上的一些浪费；
欧拉旋转	很容易理解，形象直观；	要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同的结果； 会造成万向节锁（Gimbal Lock）的现象; 由于万向节锁的存在，欧拉旋转无法实现球面平滑插值.
四元数旋转	可以避免万向节锁现象； 只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高； 可以提供平滑插值；	比欧拉旋转稍微复杂了一点点，因为多了一个维度； 理解更困难，不直观.

        熟悉Unity3D开发的朋友应该熟悉,Unity3D为了更直观的展示旋转,在界面上展示的是欧拉角旋转,而起内部实现的却是四元数旋转(避免万向节锁).

        对于很多嵌入式设备而言, CPU资源很宝贵,又因为6轴陀螺仪姿态解算算法调用周期一般都是毫秒级,相当频繁. 显然使用四元数表示旋转更合适. 

        本算法也一样,  为了计算高效同时避免万向节锁问题, 坐标旋转变换使用四元数, 姿态融合使用欧拉角, 最终姿态输出也使用欧拉角.

        当然现在很多陀螺仪芯片(比如: MPU-6050),都已经芯片内置(SoC)姿态解算算法,直接通过IIC使用命令获取即可.

二. 算法基本思路

        理论上,加速度计和陀螺仪的输出都可以单独用来计算姿态, 为什么还要进行姿态融合呢? 正如前几篇我发表的关于卡尔曼滤波器的文章,任何测量值都有噪音存在误差,同时预测也有误差,需要使用多方数据进行融合,获得最优估计.

        这里加速度计的测量值对系统震动相当敏感,数据波动较大,短期可信度低,但经滤波后数据长期来看可信度高; 而陀螺仪对系统震动不敏感短期可信度高,但陀螺仪存在漂移会累加,所以长期来看可信度低.  所以可以利用算法短期相信陀螺仪,长期相信加速度计,对两者进行姿态融合. (记得真正的算法开始之前,对陀螺仪和加速度计的原始数据进行单位换算, 静态校准,数据滤波等初始化工作).

如下图,载体水平静止时, 加速度计的三轴和陀螺仪的三轴分别在载体坐标系下的方向和分量大小(向量长度仅是演示).

        但当载体处于任意其他状态时, 比如运动或倾斜时, 加速度计的三轴和陀螺仪的三轴分别在载体坐标系下的方向不变,但分量大小发生改变(向量长度仅是演示).如下图:

        但, 这里的各分量表示的姿态都是基于载体坐标系的,我们需要的是基于导航坐标系(地理坐标系OENξ)的姿态.

 本算法的基本思路如下:

1. 用一个全局的四元数q(q0 , q1,  q2 , q3 )表示载体在地理坐标系OENξ下的姿态,其中初始值q0 = 1, q1 = 0, q2 = 0, q3 = 0,每个计算周期都将计算出的地理坐标系OENξ下的姿态结果归一化后保存在该四元数q中;

2.根据余弦矩阵,欧拉角和四元数旋转表示法(这是关键点(1)放最后讲),把当前地理坐标系OENξ下的四元数q姿态转换为载体坐标系下的加速度计的三个分量ax`,ay`,az`(为单位向量);

3.对当前加速度计测量值ax,ay,az归一化为单位向量, 利用两个单位向量的叉乘,衡量两个向量的偏差(这是关键点(2)放最后讲), 计算出三轴分量的偏差: error_x,error_y,error_z;

4.对各个计算周期的三分量偏差做累加积分: xErrorIntegral,yErrorIntegral ,yErrorIntegral ;

5.因为加速度计数据长期累积可信度大,而陀螺仪短期测量值可信度大,所以把加速度计的本次偏差和累积积分对陀螺仪的测量值gx,gy,gz做修正,  补偿陀螺仪的零点漂移.使本次测量值gx,gy,gz更准确;

6.利用四元数微分方程. 采用一阶毕卡解法(这是关键点(3)放最后讲)，融合修正过的陀螺仪测量值gx,gy,gz到位于地理坐标系OENξ下的当前姿态四元数q.

7.对更新后的地理坐标系OENξ下的姿态四元数q进行归一化(单位化),单位化四元数在空间旋转时不会拉伸，仅有旋转角度;

8.利用余弦矩阵,四元数旋转到欧拉角的转换(这是关键点(4)放最后讲)，求出地理坐标系OENξ下四元数q,对应的更形象更直观的欧拉角姿态表示: 翻滚角(roll),俯仰角(pitch),偏航角(yaw).

9.最新的地理坐标系OENξ下载体的姿态扔保存在四元数q中,供下个周期计算使用;

Note:  发现这个基本思路写太过了,过于详细,回头再改改.

三. 算法源码及居高临下看算法流程框图

        为了调试时翻转方便,我是在android手机上跑的Mahony的6轴陀螺仪姿态解算算法源码,android手机上陀螺仪坐标轴都是手机从左侧到右侧的水平方向为x轴正向，从手机下部到上部为y轴正向，垂直于手机屏幕向上为z轴正向,这与陆地四轮车载体坐标系表示法一致. 此源码来自网上,我只是针对手机的载体坐标系做了调整,加速度计(m/s)和陀螺仪(rad/s)数据都是用的android系统API获取的原始数据,没做任何加工.源码如下:

#define sampleFreq	100.f			// sample frequency in Hz
/*采样周期的一半，用于求解四元数微分方程时计算角增量
请确定自己的姿态调用周期: 10ms,即上面的sampleFreq: 100Hz*/
#define halfT 0.005f

//这里的Kp,Ki是用于控制加速度计修正陀螺仪积分姿态的速度
#define Kp 10.0f  //2.0f
#define Ki 0.008f  //0.002f


//初始姿态四元数(地理坐标系)，q(q0,q1,q2,q3)
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //最优估计四元数
//定义姿态解算误差的积分.     
//当前加计测得的重力加速度在三轴(x,y,z)上的分量,与当前姿态计算得来的重力在三轴上的分量的误差的积分
static float xErrorInt = 0, yErrorInt = 0, zErrorInt = 0;

/*
 * 6轴陀螺仪姿态融合算法:  Mahony的互补滤波算法 6轴版
 * 单位: m/s^2   rad/s
 * 由于加速度的噪音很大, 此处建议使用滤波后的数据
 * */
void MahonyImuUpdate(float gx, float gy, float gz, float ax, float ay, float az, IMU_Angle* angle)//g表陀螺仪，a表加计
{
    float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解"微分方程"时要用
    float norm; //矢量的模或四元数的范数
    float posture_x, posture_y, posture_z;//当前姿态计算得来的重力在三轴上的分量
    //当前加计测得的重力加速度在三轴上的分量,与用当前姿态计算得来的重力在三轴上的分量的误差
    float error_x, error_y, error_z;

    // 先把这些用得到的值算好
    float q0q0 = q0*q0, q0q1 = q0*q1, q0q2 = q0*q2, q0q3 = q0*q3, q1q1 = q1*q1, q1q2 = q1*q2;
    float q1q3 = q1*q3, q2q2 = q2*q2, q2q3 = q2*q3, q3q3 = q3*q3;

    //加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
    if( (ax == 0.0f) && (ay == 0.0f) && (az == 0.0f) )
       return;

    //将加速度的原始数据进行归一化,得到单位加速度
    norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    //用当前姿态(参考坐标n系)计算出重力在三个轴上的分量(载体坐标b系)，
    /*把四元数换算成"方向余弦矩阵"中的第三列的三个元素.根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到载体坐标系,正好是这三个元素.*/
    posture_x = 2*(q1q3 - q0q2);
    posture_y = 2*(q0q1 + q2q3);
    posture_z = q0q0 - q1q1 - q2q2 + q3q3;

    //计算: 传感器测得的重力与当前姿态计算的重力间的误差，向量外积可以表示这一误差
    // Error is sum of cross product between estimated and measured direction of gravity
    error_x = (ay*posture_z - az*posture_y) ;
    error_y = (az*posture_x - ax*posture_z) ;
    error_z = (ax*posture_y - ay*posture_x) ;

    //对两种重力分量的叉积误差进行积分
    xErrorInt = xErrorInt + error_x * Ki;// * (1.0f / sampleFreq);
    yErrorInt = yErrorInt + error_y * Ki;// * (1.0f / sampleFreq);
    zErrorInt = zErrorInt + error_z * Ki;// * (1.0f / sampleFreq);

    //对两种重力分量的叉积误差的积分做p,i修正陀螺零偏,通过调节Kp,Ki两个参数,可以控制加速度计修正陀螺仪积分姿态的速度
    gx = gx + Kp*error_x + xErrorInt;  //将误差PI后补偿到陀螺仪，即补偿零点漂移
    gy = gy + Kp*error_y + yErrorInt;
    gz = gz + Kp*error_z + zErrorInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

    // Integrate rate of change of quaternion
    //gx *= (1.0f / sampleFreq);		// pre-multiply common factors
    //gy *= (1.0f / sampleFreq);
    //gz *= (1.0f / sampleFreq);

    //下面进行姿态的更新，也就是四元数微分方程的求解
    q0temp=q0;//暂存当前值用于计算
    q1temp=q1;
    q2temp=q2;
    q3temp=q3;

    //四元数微分方程. 采用一阶毕卡解法，融合当前位姿和陀螺仪测量值,并转换到世界参考坐标系N. 
    q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
    q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
    q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
    q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;

    //对当前姿态四元数进行单位化,单位化四元数在空间旋转时不会拉伸，仅有旋转角度
    norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    //再次把这些用得到的值算好
    q0q1 = q0*q1; q0q2 = q0*q2; q0q3 = q0*q3; q1q1 = q1*q1; q1q2 = q1*q2;
    q1q3 = q1*q3; q2q2 = q2*q2; q2q3 = q2*q3; q3q3 = q3*q3;

    //四元数到欧拉角的转换,这里输出的是弧度,想要角度值,可以直接乘以57.3,即一弧度对应角度值
    angle->roll  = atan2f(2.f * (q0q1 + q2q3),1 - 2.f * ( q1q1 - q2q2) ); // roll: X轴
    angle->pitch  = asinf(2.f * (q0q2 - q1q3) ); // pitch: Y轴
    //其中YAW航向角由于加速度计对其没有修正作用，因此实际结果会逐渐偏移,想要准确,需要使用磁力计
    angle->yaw   = atan2f(2.f * (q0q3 + q1q2),1 - 2.f * (q2q2 + q3q3) ); // yaw: Z轴

/*
    Note: 上面转欧拉角的代码,其实有个问题,就是该代码是针对飞行器导航设计的,其前后纵向轴为
载体坐标系Oxyz的x轴,而不是陆地四轮车那样前后纵向轴为载体坐标系Oxyz的y轴.但不要担心, 
学习了本文后,你也可以动手修改它.
*/
}

        Note: 上面的代码,其实有个问题,就是该代码是针对飞行器导航设计的,其前后纵向轴为载体坐标系Oxyz的x轴,而不是陆地四轮车那样前后纵向轴为载体坐标系Oxyz的y轴.但不要担心, 学习了本文后,你也可以动手修改它.

下面站一定高度来观察算法流程框图,然后在对算法中4个关键知识点,逐一攻克:

四.方向余弦矩阵, 欧拉旋转,四元数旋转 理论基础 (见中编)

     十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中)