十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜上＞

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

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ξ三轴的构成的坐标系符合右手直角坐标系法则. 地理坐标系跟随载体运动,但三轴指向不会变化. 类似人体理解的相对自身位置的前后左右上下为载体坐标系,但描述方位的东西南北天地为地理坐标系,你在地球上(仅限地球上)走到哪里地理坐标原点都跟到哪里,但方位始终不变.