笔者最近在做四轴,涉及到地磁计的融合算法,网上大多数是x-IMU的融合代码,但是这段代码对于地磁计的融合说明没有做过多的解释,网上没有相关讨论,仅在阿莫论坛看到一篇相关的代码解释,里面有关于地磁计融合部分的解说,个人觉得说的不是很清楚,虽然是正确的,我这里再补充啰嗦一下。
首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码:
相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理,这部分代码在x-IMU官网上已经给出,大家可以自行下载(http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/),有能力的可以自行查看Sebastian O.H. Madgwick在2010年4月发表的一篇论文(An efficient orientation filter for inertial and inertial/magneticsensor arrays),可以发现x-IMU官网上的融合代码就是基于此篇论文。
花了一天时间研究地磁融合代码,总算弄明白了其地磁融合的原理。为了让大家理解Madgwick对地磁量的处理方式,我先从加速度计补偿开始说起。
首先,东北天坐标系我们称之为n系(地理坐标系,参考坐标系),载体坐标系我们称之为b系,就是我们飞行器的坐标系。对于四元数法的姿态解算,我们求的就是四元数的值;方向余弦矩阵(用于表示n系和b系的相对关系)中的元素本来应该是三角函数,这里由于我们四元数法,所以矩阵中的元素就成了四元数。所以我们的任务就是求解由四元数构成的方向余弦矩阵nCb(nCb表示从b系到n的转换矩阵,同理,bCn表示从n系到b的转换矩阵,他们的关系是转置)。
显然,上述矩阵是有误差存在的。对于一个确定的向量n,用不同的坐标系表示时,他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的转换矩阵存在误差,那么当一个向量经过这么一个有误差存在的旋转矩阵变换后,在另一个坐标系中肯定和理论值是有偏差的,我们通过这个偏差来修正这个旋转矩阵。我们刚才说了,这个旋转矩阵的元素是四元数,这就是说我们修正的就是四元数,于是乎我们的姿态就这样被修正了,这才是姿态解算的原理。
我这里再重复一遍,因为这是原理部分。我们的姿态解算求的是四元数,我们是通过修正旋转矩阵中的四元数来达到姿态解算的目的,而不要以为通过加速度计和地磁计来修正姿态,加速度计和地磁计只是测量工具和载体,通过这两个器件表征旋转矩阵的误差存在,然后通过算法修正误差,修正四元数,修正姿态。
在n系中,加速度计输出为,经过bCn转换之后到b中的值为
;在b系中,加速度计的测量值为
,现在
和
均表示在b系中的竖直向下的向量,由此,我们来做向量积,得到误差
,利用这个误差来修正bCn矩阵,于是乎,我们的四元数就在这样一个过程中被修正了。(实际上这种修正方法只把b系和n系的XOY平面重合起来,对于z轴旋转的偏航,加速度计无可奈何,稍后给详细讲解)但是,由于加速度计无法感知z轴上的旋转运动,所以还需要用地磁计来进一步补偿。
我们知道加速度计在静止时测量的是重力加速度,是有大小和方向的;同理,地磁计同样测量的是地球磁场的大小和方向,只不过这个方向不再是竖直向下,而是与x轴(或者y轴)呈一个角度,与z轴呈一个角度。记作,这里我们让x轴对准北边,所以by=0,即
。倘若我们知道bx和bz的精确值,那么我们就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中,我们在n系中的参考向量是
,变成了地磁计的
。不难发现,地磁计无法感知关于y轴上的旋转运动(运动过程中始终有bx2+bz2=常量),所以如果我们知道bx和bz的精确值,那么我们就可以摆脱掉加速度计的补偿,直接用地磁计和陀螺仪进行姿态解算,但是此时就变成了无法感知y轴上旋转运动了(而没有地磁的加速度计是无法感知z轴旋转运动的)。
但是你看过谁只有陀螺仪和地磁计进行姿态解算吗?没有,因为没人会去测量当地的地磁场相对于东北天坐标的夹角,也就是bx和bz。那么现在怎么办?
-----
前面已经讲了,我们的姿态解算就是求解旋转矩阵,这个矩阵的作用就是将b系和n正确的转化直到重合。
现在我们假设nCb*旋转矩阵是经过加速度计校正后的矩阵,当某个确定的向量(b系中)经过这个矩阵旋转之后(到n系),这两个坐标系在XOY平面上重合,只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb*旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上,这时,只剩下一个偏航角误差。这也是为什么加速度计误差修正偏航的原因。
到这里,就好说了。现在我们反过来从b系推往n系:设地磁计在b系中的输出为,经过nCb*旋转之后得到
(n系)。在这个XOY平面上(n系),
的投影为bx2,
的投影为hx2+hy2。显然,地磁计在XOY平面上(n系)的向量的大小必定相同,所以有bx2= hx2+hy2。而对于bz的处理,我们不做变动,令bz=hz即可。经过这样处理之后的
,经过bCn*旋转回转到b系中,得到
,这个值再和b系中的地磁计输出
做向量积求误差,再次修正bCn*(或者nCb*),得到bCn**(或者nCb**)。这样就完成了一次地磁计的补偿。
将加速度计没能做到的z轴上的旋转修正,通过地磁计在XOY平面上的地磁力相同原理,得到了修正。于是乎,pitch和roll通过加速度计修正,然后在这个基础之上(该地磁计补偿方法必须依靠加速度计修正提供一致的XOY平面,才会有bx2= hx2+hy2等式成立),yaw通过地磁计来补偿,最终得到了没有偏差的实时姿态(也就是由四元数组成的旋转矩阵)。
如果你真的明白了我所讲的地磁计融合原理,那么现在我去掉加速度计的对于XOY平面的修正,只用陀螺仪和地磁计进行姿态融合,会出现什么情况?
(答案:只有当飞机在水平面内才能正确得到偏航角,一旦将飞机拉离平衡位置,偏航角就错的一塌糊涂,更别说pitch和roll了。)