三轴陀螺仪结算
在结算三轴陀螺仪数据之前需要先了解坐标系的概念,关于齐次坐标变换和变换矩阵详情可以参考笔者学习的书本,熊有伦教授主编的《机器人学》。
1、建系
首先建立两个坐标系,导航坐标系也被称为地理坐标系,载体坐标系也被称为物体坐标系。
导航坐标系:东-北-天(地理坐标系X、Y、Z轴的正方向)
载体坐标系:右-前-上(物体坐标系X、Y、Z轴的正方向)
注意:全文按这两个坐标系进行行文,不同坐标系的建立也会有不同的公式,读者切忌照搬。
2、姿态描述方法的选取
在很多博客中都存在一个问题:对姿态描述方法的混用与模糊概念。在描述刚体方位方面,多数博客中用到了RPY角方法和欧拉角方法。但未解释两者之间的区别。欧拉角的定义在不同书中也有不同之处,本文主要介绍两种不同绕系旋转方式的区别。
RPY角方法
RPY角方法:刚体绕固定坐标系三轴依次旋转一定的角度。旋转矩阵的求取采用的是矩阵“左乘”的方法若。RPY角最早开始是描述船舶(后来逐渐用于手爪上)在海中航行时姿态的一种方法。将船行驶方向取为Z轴方向,则绕Z轴旋转()称为回转(roll),使用右手螺旋定则,用Z轴确定另外两轴,绕Y轴旋转()称为俯仰(pitch),绕X轴旋转()称为偏转(yaw),各书中叫法不同,读者能理解三个角的实际意义即可。同时,机械臂手爪姿态的规定方法与之类似。
刚体绕固定坐标系三轴依次旋转一定的角度。采用的是矩阵“左乘”的方法。例如,坐标系{B}与参考系{A},初始位置重合。{B}先绕旋转 角,绕旋转角,绕 旋转角,得到相应的旋转矩阵
欧拉角方法
欧拉角方法:这种描述法中的各次转动都是相对运动坐标系的某轴进行的而不是相对固定坐标系{A}进行的。 旋转矩阵的求取采用矩阵“右乘”的方法计算旋转矩阵。
例如,{B}与参考系{A}的初始方位相同,首先使{B}绕轴旋转角,然后绕轴旋转角,最后绕轴旋转。
将上述两式乘开,即可得到很多博客中提到的余弦矩阵。但是,当绕着不同顺序的旋转轴旋转时,余弦矩阵也不相同,所以掌握余弦矩阵的推导更为重要。
3、三轴陀螺仪角度计算
由于笔者能力有限,本文为大家提供求解姿态的两种方法:欧拉角法和四元数法 。在计算前先要明确的是,陀螺仪测得的数据是载体坐标系绕三轴旋转的速度。
欧拉角解算姿态
一般情况下,根据欧拉角解算姿态时,直接套用微分方程即可。笔者在此被困扰良久,笔者觉得绕着不同顺序轴进行旋转,欧拉角微分方程也会不同,所以直接套用不可取,下面为各位读者简单地推导其中一种欧拉角的微分方程。
例如,{B}与参考系{A}的初始方位相同,首先使{B}绕轴旋转角,然后绕轴旋转角,最后绕轴旋转。求取此旋转的欧拉角微分方程。假设 P 是在坐标系{A}下的坐标,P‘ 是在坐标系{B}旋转变换后的坐标描述。则,
假设坐标系{B}已经完成了三轴转动,我可以直接看到最后一轴的转动,那么在下一瞬间坐标系{B}下 P‘ 绕{B}中的X轴(最后旋转轴)的角速度,这种理解方式很抽象,也可纯理论分析推导,比较麻烦
假设已经完成了两轴旋转,则需要将 P‘ 进行依次逆旋转变换,这样我才能看到,P’ 绕Y轴(第二个旋转轴)的旋转,其角速度为
假设完成了一轴的旋转,则需要进行剩下两轴的旋转逆变换,然后我才能看到绕Z轴(第一个旋转轴)的旋转,其角速度为
则,
在计算时需要注意,绕着每个轴旋转的都是什么角!!!
则有,
可以通过一阶差分的方法对相邻两个扫描周期的欧拉角进行计算。
Ts为扫描周期,上式表示根据k-1个周期时的三轴角度去计算k个周期时的角度。
以上即为欧拉角姿态解算方法。
参考文献:
熊有伦,《机器人学》