mpu6050姿态解算与卡尔曼滤波(1)数学

定义地理坐标系n系:x轴指向东,y轴指向北,z轴指向天。在mpu6050芯片上定义载体坐标系b系。那么b系的姿态就是指n系与b系相对的旋转关系,即如何由n系旋转到b系。

描述这种旋转关系通常使用欧拉角 (ψ,θ,γ)T ,姿态矩阵T(3x3),四元数 Q=(q0q1q2q3)T

欧拉角指的是将n系按照z轴->x’轴->y”轴的顺序依次转动 ψ,θ,γ 后就是b系,事实上欧拉角并没有规定转动顺序,以上转动顺序z轴->x’轴->y”轴为惯性导航中常使用的规定, (ψ,θ,γ)T 依次命名为偏航、俯仰、滚转。

姿态矩阵T描述的是三维空间中的两个标准正交基之间的向量转换关系,T是一个3x3的正交阵。若一个向量在n系下的坐标为三维列向量 xn ,则该向量在b系下的坐标为 xb=Tbnxn ,反之 xn=Tnbxb 。T阵是一个正交矩阵,故 Tbn Tnb 互为转置矩阵,互为逆矩阵。

四元数Q的本质涉及到更高等的数学,若仅考虑用四元数描述姿态,只需记住:描述姿态的四元数是归一化的,即:

|Q|=q20+q21+q22+q23=1

每个归一化的四元数都描述了一个姿态,或者说描述了一个可由n系旋转得到的b坐标系。

以上三种描述方式是相互等价的,只要确定了两个坐标系的旋转关系,那么就唯一地确定了一组欧拉角,也唯一地确定了一个姿态矩阵和归一化四元数。三种描述方式可以互相推导,由欧拉角可以推导出姿态矩阵,再由姿态矩阵可以推导出四元数。同样,由四元数可以得到姿态矩阵,再得到欧拉角。
改正:以上三种描述方式是等价的,但是要注意对于两个确定的旋转关系的坐标系,表示两者旋转关系的三种方式中,姿态矩阵一定是唯一的,但是四元数却可以有两个,而欧拉角可以有很多。在做彼此的转换,尤其是涉及到四元数表示的时候,一定要注意。

转换关系如下:

由欧拉角 ψ,θ,γ 得到姿态矩阵 Tbn
绕z轴旋转矩阵 Tz

Tz=cosψsinψ0sinψcosψ0001

绕x轴旋转矩阵 Tx :
Tx=1000cosθsinθ0sinθcosθ

绕y轴旋转矩阵 Ty :
Ty=cosγ0sinγ010sinγ0cosγ

则姿态矩阵 Tbn=TyTxTz
所以:
Tbn=cosγcosψ+sinγsinθsinψcosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψcosγsinθcosψsinγcosθsinθcosγcosθ

由姿态矩阵得到欧拉角 ψ,θ,γ
mpu6050姿态解算与卡尔曼滤波(1)数学_第1张图片

由四元数 Q 转换到姿态矩阵T:

Tbn=q20+q21q22q232(q1q2q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q20q21+q22q232(q2q3q0q1)2(q1q3q0q2)2(q2q3+q0q1)q20q21q22+q23

由姿态矩阵转换到四元数:
由矩阵 T 的对角线元素可知:

q20+q21q22q23=T11q20q21+q22q23=T22q20q21q22+q23=T33q20+q21+q22+q23=1

故而有
|q1|=121+T11T22T33|q2|=121T11+T22T33|q3|=121T11T22+T33|q0|=1q21q22q23

其中,正负号如下:
sign(q0)=+sign(q1)=sign(T32T23)sign(q2)=sign(T13T31)sign(q3)=sign(T21T12)

——————————————————————————————————————————————————————————
考虑这样一个问题:若b系相对n系的欧拉角为 (ψ,θ,γ) ,以角速度 ωnb 转动了 Δt ,注意角速度 ωnb 是一个三维向量,那么欧拉角的变化量 (Δψ,Δθ,Δγ) 应该是多少呢?这个问题旨在找出欧拉角的变化速率与转动角速度的关系,以下给出这个关系。
欧拉角微分方程

ψ˙θ˙γ˙=1cosθsinγcosγcosθsinθsinγ00cosθcosγsinγcosθsinθcosγωbnbxωbnbyωbnbz

上式中 ωbnbx 表示转动角速度 ωnb 在b坐标系x轴上的投影,也就是固连在转动物体上的三轴陀螺仪测量的角速度。若已知 t0 时刻的欧拉角 (ψ,θ,γ) ,以及从 t0 t 时刻的角速度曲线,那么 t 时刻的欧拉角自然是可以计算出的,这是一个求解一阶微分方程组的问题。
然而欧拉角微分方程是一个非线性微分方程组,它的解析解并不能以简单函数的形式表达出来。若把目光投向姿态矩阵和四元数,亦有微分方程如下:

姿态矩阵微分方程

T˙11T˙21T˙31T˙12T˙22T˙32T˙13T˙23T˙33=T11T21T31T12T22T32T13T23T330ωbnbzωbnbyωbnbz0ωbnbxωbnbyωbnbx0

四元微分方程:

q˙0q˙1q˙2q˙3=120ωbnbxωbnbyωbnbzωbnbx0ωbnbzωbnbyωbnbyωbnbz0ωbnbxωbnbzωbnbyωbnbx0q0q1q2q3

到这里,对于载体的姿态问题就有了一个解决办法:在载体上安装相互正交的三个陀螺仪,测量载体相对导航坐标系的转动,选取欧拉角/姿态矩阵/四元数描述姿态,按照微分方程积分,就可以得到姿态了。
若以四元数描述姿态,则一个典型公式为:

q0q1q2q3k+1=q0q1q2q3k+q˙0q˙1q˙2q˙3Δt

若记
[ω×]=0ωbnbxωbnbyωbnbzωbnbx0ωbnbzωbnbyωbnbyωbnbz0ωbnbxωbnbzωbnbyωbnbx0

Qk+1={I4×4+12[ω×]}Qk

由于这是一个积分的做法,在实际应用中不可避免的会出现误差发散,长时间之后这个办法给出的姿态就不具有太高的可信度了。此外实际应用中还需注意一个问题,由于角速度测量的误差,四元数/姿态矩阵在一段时间后可能会不满足归一性/正交性,需要对四元数/姿态矩阵做归一化/正交化处理。
——————————————————————————————————————————————————————————
关于mpu6050加速度计,很多人可能无法理解为什么在相对n系静止的情况下加速度计会测量到一个与重力加速度相反的加速度,建议阅读这篇知乎专栏文章中关于加速度计的部分: https://zhuanlan.zhihu.com/p/20082486这里对加速度计的测量解释的较为形象。
理想的载体相对n系无加速度状态下,加速度计的测量为:
axayaz=Tbn00g=cosγcosψ+sinγsinθsinψcosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψcosγsinθcosψsinγcosθsinθcosγcosθ00g=gsinγcosθgsinθgcosγcosθ

可以看到,在载体本身无加速度的情况下,加速度计的测量输出是与 ψ 无关的,也就是说加速度计只能反映姿态的一部分信息,仅凭加速度计的测量并不能唯一确定一个姿态,这是显而易见的。
要测量姿态,我们至少需要在b系下测量两个n系下已知的向量,很容易想到,引入磁场传感器测量地磁向量,这样就能唯一地计算出姿态矩阵,从而确定载体的姿态。
若考虑载体本身的加速度,如果不依靠其他手段对载体本身的加速度做出估计,那么加速度计的测量就既与载体姿态变化有关,亦与载体加速度有关。

你可能感兴趣的:(姿态解算,mpu6050,姿态解算)