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

由浅至深理解6轴陀螺仪姿态解算算法 <中篇>:

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

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

先从简单的二维平面坐标分析.设初始状态平面坐标系0xy和坐标系Ox`y`完全重合. 坐标系内有一个矢量P,其在Oxy坐标系内分量为Pxi和Pyj,在Ox`y`坐标系内分量为Px`i`和Py`j`, 其中i和j为x轴和y轴上同向单位向量,坐标为i=(1,0), j=(0,1), 故有i*i = j*j = 1, i*j = 0. 这里*为两个向量的点积,因为都是单位向量,所有i*i = |i||i|cosα, 当α=0时,i*i = 1; 而i*j = |i||j|cosα, 当α=90时,i*j = 0. 同理: i`和j`也有类似性质.

当坐标系Ox`y`相对于坐标系Oxy旋转一个角度α时, 矢量P在两个坐标系的变换关系如下(向量的另一种表示法):

(1)

(2)

(3)

对上式(3)三边分别同乘i`或j`,得:

(4)

(5)

把上面公式(5)和(6)的线性方程组,用矩阵形式表示就是:

(6)

对于上面公式(6),我们可以这样理解:

矢量P相对于固定坐标系Oxy没有转动,而动坐标系Ox’y’相对固定坐标系Oxy旋转了角度α后,矢量P在两个坐标系上的分量关系.

上述公式(6)中的矩阵R称为方向余弦矩阵,其元素是两组坐标系坐标轴单位矢量之间的夹角的余弦值. 因为i,j,I’,j’均为单位向量,所以根据向量的点积定义有:

i’*i = |i||i|cosα = cosα

i’*j = cos(90֯ - α) = sina

j’*i = cos(90֯ + α) = -sinα

j’*j = cosa

所以得: (7)

表从地理坐标系转换到载体坐标系的方向余弦矩阵;

上述只是二维平面坐标系Oxy下方向余弦矩阵表示旋转变换的情况,采用同样方法我们可以推广到三维立体坐标系Oxyz,或我们使用的三维地理坐标系OENξ. 如下图:

设地理坐标系OENξ为定坐标系,其三个坐标轴单位矢量为:i,j,k; 任一矢量P在该坐标系的分量分别是:x,y,z; Oxyz表示动坐标系(载体坐标系),其三轴单位矢量为: i',j',k', 同一个矢量P在该坐标系下各轴分量为: x',y',z', 根据上面二维坐标系的同样方法可得,两个坐标系下各分量满足下面关系:

(8)

因为类似: i' * i = |i| |i| cosα1 = cosα1 所以得:

(9)

(10)

这里表从地理坐标系转换到载体坐标系的方向余弦矩阵;

因为方向余弦矩阵是正交矩阵,故其逆矩阵就是其转置矩阵(此性质请参考线性代数中正交矩阵相关章节),即: .

所以上述公式(10)中的从地理坐标系转换到载体坐标系的方向余弦矩阵 ,反过来,就是从载体坐标系转换到地理坐标系的方向余弦矩阵 : 得:

(11)

对于上面公式(11),我们可以这样理解:

对应到我们正在讨论的载体(四轮车)而言, 当载体任意旋转变换后的瞬间时刻, 我们可以理解地理坐标系(固定坐标系)OENξ没有转动, 而载体坐标系(动坐标系)Oxyz相对地理坐标系(固定坐标系)做了转动 .

另外需要注意的一点是: 的最后一列[cosγ1 cosγ2 cosγ3].T (这里[ ].T表列向量,懒得画图了), 就是[k'*i k'*j k'*k].T, 其实就是载体坐标系Oxyz的z轴与地理坐标系OENξ的三轴的夹角的cos值.

这也就解释了前面算法流程图中"关键知识点1",以及算法代码中把当前地理坐标系姿态四元数q转化到载体坐标系中三轴的重力分量时,为何使用如下代码(当然我们还在用方向余弦矩阵,后面会用四元数表示这个旋转变换矩阵):

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

OK, 到这里我们解释了算法流程图中标注的"关键知识点1"理论依据.

但

这还不是结束,上面描述的方向余弦矩阵和,仅仅是动坐标系旋转完毕后三轴相对于固定坐标系三轴的夹角余弦值的一种静态描述. 如果载体坐标系,先绕自身x轴旋转α角度,再绕自身y轴旋转β角度,最后在绕自身z轴旋转γ角度,这个过程结束后,方向余弦矩阵如何表达呢? 因为我们根本不知道整个旋转过程完毕后,动坐标系三轴相对固定坐标系三轴的夹角(或余弦值)是多少. 那么继续...

因为方向余弦矩阵具有传递性(详细请自己查阅), 我们可以利用这个特性实现多个步骤的具有相同原点的不同轴的坐标旋转.

经过n次不同轴的转动后,方向余弦矩阵可以表示为:

(12)

有了上面公式(12)这个性质,下面我们来看用欧拉角表示的坐标系旋转,并把这些旋转步骤根据上面公式(12)性质用一个方向余弦矩阵表示.

设地理坐标系OENξ为定坐标系,载体坐标系Oxyz为动坐标系,在初始时刻,两个坐标系完全重合; 经过绕载体坐标系Oxyz的三轴经过三次旋转(角度依次是: ψ, θ, γ)后,到达最后的位置0x3y3z3, 称这三次转动角ψ, θ, γ为欧拉角.其过程如下:

第一次转动: 坐标轴0xyz绕Oz轴旋转ψ角度,得到新的坐标系Ox1y1z1, 相应的方向余弦矩阵变换可以写成:

(13)

第二次转动: 坐标轴0x1y1z1绕Ox1轴旋转θ角度,得到新的坐标系Ox2y2z2, 相应的方向余弦矩阵变换可以写成:

(14)

第三次转动: 坐标轴0x2y2z2绕Oy2轴旋转γ角度,得到新的坐标系Ox3y3z3, 相应的方向余弦矩阵变换可以写成:

(15)

把公式(13)代入公式(14), 再把结果代入公式(15),得:

最终,得:

(16)

根据前面已知的到正交的方向余弦矩阵性质 ,进一步得:

(17)

上面公式(16)和公式(17)就是按照欧拉角依次旋转后得到的方向余弦矩阵. 我按照两个坐标系各轴单位矢量(i,j,k和i´, j´, k´),标注了各矩阵元素代表的哪两个轴相交的点积( 也就是余弦值.

例如: i´·i = | i´| | i | cosα = cosα ). 注: 这里i´, j´, k´指Ox3y3z3的三轴的单位矢量.

下面考虑载体(四轮车)姿态的确定

载体的姿态,实际上就是载体及其载体坐标系Oxyz和地理坐标系OENξ(也可认为是导航坐标系)之间的方位关系.

不同领域对载体姿态角定义不同,对于飞行器一般以载体坐标系Oxyz的Ox轴为前后纵坐标轴,左右横向以Oy为坐标轴; 对于陆地机器一般以载体坐标系Oxyz的Oy轴为前后纵坐标轴,左右横向以Ox为坐标轴;这个大家使用时要注意.

对于陆地四轮车来说:

航向角(yaw): 为载体坐标系前后纵轴Oy在水平面上投影与地理坐标系ON轴之间的夹角,用ψ表示,且以从地理坐标系ON轴开始顺时针方向为正,范围[0°,360°];

俯仰角(pitch): 为载体坐标系前后纵轴Oy轴与地理坐标系水平面在载体纵向方向夹角,用θ表示,且从纵向水平面算起,向上为正,向下为负,范围[-90°,90°];

翻滚角(roll): 为载体坐标系正向纵轴Oy和正向垂轴0z组成的平面,与正向纵轴Oy和地理坐标系Oξ组成的铅锤平面之间的夹角,用γ表示,且左高右低时为正,右高左低为负,范围[-90°,90°].

继续前面的三次欧拉旋转使用的图如下:

从图中当前姿态我们可知:

ON,Oy1,OE,Ox2(x1)共平面,且处于地理水平面上;

Oy1,Oy2(y3),Oξ(z1),Oz2共平面,且此平面垂直于地理水平面;

Oz2,Oz3,Ox2(x1),Ox3共平面.

观察此图,并根据上面陆地四轮车姿态的定义:

航向角(yaw): 就是ON轴和Oy1轴之间的夹角ψ.

俯仰角(pitch): 就是Oy3轴和Oy1轴之间的夹角θ.

翻滚角(roll): 就是以Oy3轴为平面交线, Oy3和Oz2组成的平面与Oy3和Oz3组成的平面之间的夹角.因为Oy3垂直Oz2,Oy3垂直Oz3,Oy3为两平面交线, 所以0z2和Oz3之间的夹角γ,就是翻滚角.

结合,上面公式(17):

(17)

令:

最终得(考虑到反正切函数的多值问题,我们使用 arctan2( x ) 函数,而不是arctan( x )):

(18)

举例证明(其他姿态角,自行推导):

就以俯航向角(偏航角)ψ举例: 直接正向推倒有点麻烦,我们就以现有公式ψ=arctan(T12/T22)进行反推吧.

根据公式(17), T12代表 j´·i = | j´| | i | cosα , 即Oy3分量在OE轴上投影OE';同理T22代表j´·j,表Oy3分量在ON轴上投影ON';

根据上图中几何关系,可以推导出: tanψ = OE' / ON' = j´·i / j´·j = T12/T22

到这里,也就是解释了算法流程图中"关键知识点4"的理论依据,以及为什么算法源码中使用以下代码(不同的时,我们还没有使用四元数q来表示):

//四元数到欧拉角的转换,这里输出的是弧度,想要角度值,可以直接乘以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轴.但不要担心, 学习了本文后,
你也可以动手修改它.
*/

现在,我们可以根据方向余弦矩阵求四轮车的姿态了. 但还不够,我们需要的是四元数q(q0,q1,q2,q3)表示法.

用四元数旋转来表示方向余弦矩阵

如果算法使用上面方向余弦矩阵来计算,那计算量会比较大,所以算法的发明者使用了四元数旋转来表示.

四元数这里只简单列几个用到的概念,定理,更详细的内容不在表述.

四元数,类似于复数的概念. 四元数是由实数加上三个虚数单位 i、j和k 组成，而且它们有如下的关系： i² = j² = k² = -1， iº = jº = kº = 1 , 每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bi+ cj + dk，其中a、b、c 、d是实数。

我们也可以这样表达四元数: q( q0, q1, q2, q3) 或 q = q0 + q1 i + q2 j + q3 k

这样,三维坐标系下一个向量p(x,y,z),映射到四元数空间就是p = 0 + xi + yj + zk

为了区分标量乘,向量点积,向量叉积,我们使用☉表示四元数乘法. 设四元数q = q0 + q1 i + q2 j + q3 k, r = r0 + r1 i + r2 j + r3 k,得:

q☉r = ( q0 + q1 i + q2 j + q3 k ) ☉ ( r0 + r1 i + r2 j + r3 k )

= ( q0r0-q1r1-q2r2-q3r3 ) + ( q0r1+q1r0+q2r3-q3r2) i

+ ( q0r2+q2r0+q3r1-q1r3) j + ( q0r3+q3r0+q1r2-q2r1) k

= r’0 + r’1i + r’2j + r’3k = r’ (19)

用矩阵形式表示( M(q)表四元数q的矩阵形式 ):

(20)

或写成:

(21)

也就是说M(q)r和M’(r)q展开都是q☉r的展开式(19),所以得:

q☉r的四元数和四元数乘法 = M(q)r的矩阵和向量乘法 = M’(r)q的矩阵和向量乘法

即: q☉r = M(q)r = M’(r)q (22)

变换四元数

四元数可以描述一个坐标系或一个矢量相对于另一个坐标系的旋转,假定一个矢量R绕过0点的某一旋转轴(不一定是坐标轴)转动了一个角度θ,则与该矢量R固联的载体坐标系和地理坐标系之间的变换四元数q为:

q = cos(θ/2) + sin(θ/2) n

= cos(θ/2) + sin(θ/2)cosαi + sin(θ/2) cosβj + sin(θ/2) cosγk (21)

公式(21)中, θ为角度实数,n为旋转轴方向的单位矢量. 称公式(21)为四元数的三角式,或特征四元数,其范数||q||=1. 在导航应用中,变换四元数q的标量为cos(θ/2),矢量部分表示旋转轴n的方向, cosα, cosβ, cosγ是旋转轴n与地理坐标系三轴间的角度余弦值. 这里四元数q既表示了旋转轴的方向,有表示了转角的大小, 称q为旋转四元数.

假设在地理坐标系OENξ下一个矢量R, 其在地理坐标系三轴的分量分别为: r1,r2,r3,

有: R = r1i + r2j + r3k

把3维矢量R映射到四元数空间,就是: r = r0*0 + r1i + r2j + r3k = 0 + R

称r 为3维矢量R在地理坐标系上的四元数映像.

假如, 一个矢量R在地理坐标系OENξ中绕旋转轴n旋转一定的角度θ,到R’. 用其四元数映射来描述这个过程为:

(22)

或

(23)

该四元数旋转变换描述和前面用方向余弦矩阵旋转变换描述,之间有如下对应关系(P^b为载体坐标系下表示的向量,P^n为P^b对应的地理坐标系下表示):

(24)

(25)

我们重点分析公式(25),即有载体坐标系到地理坐标系的旋转变换过程, 并全部用四元数矩阵表示,得:

(26)

依据前面公式(22): q☉r = M(q)r = M’(r)q

公式(26)可转化为:

(27)

这里: = [0, xn, yn, zn]^T = [0, xb, yb, zb]^T (行向量,转置成列向量)

全部带入公式(27),得:

进一步得:

去除第一行和第一列得如下公式(28):

(28)

然后, 向前面欧拉旋转表示方向余弦矩阵一样,我们把中的3x3矩阵视为方向余弦矩阵:

同样可以得到用四元数旋转表示的载体相对于导航坐标系的姿态为:

(29)

五.对算法中关键知识点进行重点讲解 ( 请看下篇)

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

十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜中＞相关推荐

十四. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜下＞
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜上＞
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...
十.四轮车驱动开发之三: 巧用编码器获取电机转速信息
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...
十一.四轮车驱动开发之四:理解直流电机PID控制器
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...
ESP-12F驱动MPU6050使用DMP库姿态解算
一.准备工作主芯片采用是安信可科技的ESF-12F模组(内置ESP8266芯片),使用GPIO模拟I2C驱动MPU6050,i2c驱动部分可直接参考官方例程中的i2c_master.c文件,(附件提 ...
驱动LSM6DS3TR-C实现高效运动检测与数据采集(5)----姿态解算
概述 lsm6ds3trc包含三轴陀螺仪与三轴加速度计. 姿态有多种数学表示方式,常见的是四元数,欧拉角,矩阵和轴角.他们各自有其自身的优点,在不同的领域使用不同的表示方式.在四轴飞行器中使用到了四元 ...
由浅到深理解ROS（9）- 几个基本概念的理解坐标系包
1.坐标系最常用的就是map,odom,base_link,base_laser坐标系,这也是开始接触gmapping的一些坐标系. map:地图坐标系,顾名思义,一般设该坐标系为固定坐标系(fix ...
MTK 驱动开发（18）---LCD 参数理解
转载自http://blog.csdn.net/longxiaowu/article/details/24319933 Linux内核的amba lcd控制器使用clcd_panel结构体表示一个LC ...
由浅到深理解ROS（7）-URDF
转自相信许多爱好者止步于昂贵的机器人价格.虽然有了Arduino,但一个载重能力很弱的机器人小车成本也得一两百块钱.搭建自己的机器人更是费时费力. 所以如果你只是普通机器人爱好者,或者还没想好要拿机 ...

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

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

但

用四元数旋转来表示方向余弦矩阵

五.对算法中关键知识点进行重点讲解 ( 请看下篇)

十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜中＞相关推荐

最新文章

热门文章

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

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

但

用四元数旋转来表示方向余弦矩阵

五.对算法中关键知识点进行重点讲解 ( 请看下篇)

十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜中＞相关推荐

最新文章

热门文章

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