加速度计磁力计求初始姿态角

Posted 艾攀

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了加速度计磁力计求初始姿态角相关的知识,希望对你有一定的参考价值。

1.几个基本概念。

地理坐标系:北东地。北为X轴、东为Y轴、地为Z轴。

机体坐标系:见下图。

姿态角:我的理解是,roll角是Zb在ZOY平面与Z轴的夹角。pitch角是Zb在ZOX平面与Z轴的夹角。yaw角是Xb在XOY平面与X轴的夹角。

旋转矩阵:地理坐标系到机体坐标系的转换矩阵。旋转矩阵为正交阵,逆即转置。

传感器:都是基于机体坐标系的。加速度计测三轴的加速度,磁力计测三轴的磁感应强度,陀螺仪测三轴的角速度。注意角速度的定义,方向是沿着三轴的。得到姿态角主要靠陀螺仪测得的角速度积分,但存在漂移误差,因此要用加速度计和磁力计随时校正。

很重要的:在地理坐标系下,重力加速度坐标始终为(0,0,1g),磁场坐标为(M,0,D)。在地球表面,磁场始终是沿着磁感应线指北的,某点的磁场强度为磁感应线在该点的切线,它在北方有个分量,在垂直方向有个分量,在Y轴是没有分量的,即(M,0,D)。这里还有一点,地理的北极和磁场的北极是不重合的,有一定的夹角,即磁偏角。根据经纬度是可以查到这个夹角的大小的。我们定义的地理坐标系的北当然是地理的北。但用磁力计求得实际是Xb与地磁的北的夹角,我们最后会加上磁偏角,这样就是Xb与地理北的夹角了,就是真正的航向角啦。

 

理解重力加速度和磁感应强度的概念是非常重要的。

好了,我们可以开始求初始姿态角了。

2.加速度计求roll,pitch角。

飞机静止放在放地上时,地理坐标系下重力加速度为(0,0,g),机体坐标系下加速度测量的三轴加速度为(ax,ay,az)

它们两者之间的关系是怎么表示呢,可以想象一个长方体,(ax,ay,az)是三条边,(0,0,g)是对角线。

 

这个图可以清晰的说明一切啦。画图花了我好久。求姿态角就是求两个坐标系的夹角关系。在求俯仰角和横滚角时,我们完全可以把Z轴当做参考来求。上面我说俯仰角是Zb在XOZ平面的投影与Z轴的夹角,这里我是把Z轴投影到XbOZb平面上的,仔细想想是一样的,Z轴在XbOZb平面上的投影和Zb轴的夹角依然是俯仰角啦。横滚同理。

这样roll=arctan(ay/az),pitch =-arctan(ax/az) 方向的正负号是自己定义的。

3.磁力计求航向角。

地理坐标系下该点的磁场强度为(M,0,D),磁力计测得的三轴磁场强度为(mx,my,mz)。

当我们认为飞机是完全水平放在地上的时候,即Z轴和Zb轴是平行的时候,完全不用考虑垂直方向了。我们只用考虑XOY平面了。

那么yaw=arctan(my/mx)

 很简单对不对,但实际上飞机是不可能水平放在地面上的,肯定会有一点倾斜的,也就是说肯定会有roll和pitch角啦。只要飞机倾斜,磁力计测得的磁场强度mx就不是真正的水平X轴上的分量了,my当然也不是了。我们必须把根据mx,my,mz,roll,pitch求出投影在XOY面上的两个量了。

我们是先用加速度计求出roll和pitch角,然后根据求得的roll和pitch角,磁力计的测量的(mx,my,mz)来求yaw角。对,这才是求初始姿态角正确的打开方式。

直接按思路求出XOY上投影量比较复杂,但绝对是可以的,下面有一种比较巧妙的办法。

 

 

4.好吧,我们在这里又要讲另外一个很重要的线性代数知识。才能理解上面图片里说的什么。

 

根据上图可以得到一个重要结论,那就是,从机体坐标系到地理坐标系的转换矩阵的第3行的三个元素即重力加速度在机体坐标的三个分量,即地理坐标系下[0,0,g]对应机体坐标系下[ax,ay,az]!同理,磁力计的三个分量,也就是可以认为地理坐标系下[M,0,D]对应机体坐标系下[mx,my,mz]。

为什么重力加速度对应的机体测量值可以当做旋转矩阵的第三行。

如上图,这是通过转换矩阵把地理坐标转换到机体坐标,可以看出x3,x6,x9除以g就是重力加速度在机体坐标下的三个分量。可能我没说清楚,上文说的[ax,ay,az]是单位化了的(为了让你看的更清楚,该图片的没哟,是真是测量值)![mx,my,mz]也是单位化了的!我们在谈旋转矩阵时,向量都是单位化的,因为牢记旋转矩阵是单位正交的!!!!这是地理坐标到机体坐标的矩阵,转置一下,就是从机体坐标系到地理坐标系的转换矩阵了。故从机体坐标系到地理坐标系的转换矩阵的第3行的三个元素即重力加速度在机体坐标的三个分量。

以上是关于加速度计磁力计求初始姿态角的主要内容,如果未能解决你的问题,请参考以下文章

MEMS加速度计 MEMS陀螺仪有啥区别

陀螺仪和加速度计的读数和四元数怎么建立关系

无人机学习Pix4

使用原始加速度计和磁力计数据计算磁航向

智能手机GPS定位算法研究

imu和mems的差别_MEMS加速度计,磁力计和定向角