3D物体是如何旋转滴2

  1. 1. 旋转的微分
  2. 2. 四元数、旋转矩阵、欧拉角的互相转换

在上一篇文章中我们已经知道了如何表示旋转,但是现实世界中直接得到四元数或欧拉角的方法少之又少,那么新的问题就是如何通过角速度、加速度甚至地磁计这些我们能够容易测量的量得到旋转呢?
本期我们来讨论通过姿态解算的问题!

旋转的微分

我们都知道,使用陀螺仪,可以很轻松地得到物体的角速度,而角速度又是角度的微分,通过角速度我们计算出“旋转的微分”,再解这个微分方程,不就可以得到角度了吗!但是说的轻巧做起来难,到底该使用哪种角度(旋转)表示法才能够计算简便并且容易理解呢?欧拉角在微分时会出现万向节死锁的现象,如图:
eularerr.PNG
此时分母上产生了0,在计算时就会出问题,因此我们不能采用这种方法。
而轴角法没有办法表示旋转的叠加,就不能进行积分,因此也不可取。
最终答案就是四元数!我们能够很方便的通过角速度计算出四元数的微分方程(具体推导省略),就像下面这样:
4diff.PNG
(四元数乘法,转换为矩阵乘法可表示为下式)
4diff2.PNG
其中wb代表机体坐标系下的角速度向量,也就是直接从陀螺仪读取的值!
接下来初始化一个q(根据需求),然后解微分方程,就能够随时得到世界->机体的四元数啦!
(注:得到的四元数表示的是坐标系的旋转,即从世界坐标系到机体坐标系的旋转,即第一种理解)

四元数、旋转矩阵、欧拉角的互相转换

四元数与旋转矩阵的转换很容易完成:
rocol24.jpg
42rocol.PNG
突破q0后再依照关系一一突破即可

而旋转矩阵化为欧拉角则比较麻烦,需要先约定好欧拉角的种类(24种之一)
详情就不贴了,论坛上一抓一大把的(>人<;)

至此,我们已经通过角速度计算出欧拉角!

但是我们解微分方程的方法是积分,由于传感器零偏,积分就难免会产生漂移,我们需要纠正这个偏移,就需要这种积分传感器以外的绝对传感器,就像SLAM中的里程计+激光雷达的组合一样,我们也常用陀螺仪与加速度计、地磁计的组合进行角度测量!
下面介绍一种较为简便的AHRS(航姿参考系)算法,MahonyAHRS
Mahony的中心思想是陀螺仪更新角度,加速度计与地磁计以较缓慢的方式纠正更新的角度。
就拿加速度计来说,静止状态下,加速度计测量为g,方向指向世界坐标z轴负方向,利用上一次计算得到的四元数,我们把世界坐标系中z轴负方向在机体坐标系下的坐标解出来,再将其与加速度计做叉乘来计算误差(产生误差正是由于你从世界转换到机体的这个旋转是不准的),这时我们得到了一个误差向量,要注意它在机体坐标系下的方向可能是任意的,但在世界坐标系下一定是在xOy平面上的(这时由于先叉乘再旋转与先旋转再叉乘得到的结果是一样的),这也是加速度计仅能矫正pitch与roll轴漂移而不能矫正yaw轴的原因,误差向量在世界坐标系下根本没有z轴的分量啊!!
误差向量经过PI控制器后可以得到一个矫正向量,这个向量是一个角速度量,把它和陀螺仪所测得的角速度相加,即可得到最终的角速度,具体系统请自行分析一哈,这样一来我们就能够通过加速度计的修正角度漂移!
与此同理,若我们知道了地磁计的数据,拿它和机体坐标系下的地磁方向作比较,即可得到存在z轴分量的误差向量,这样一来我们就可以修正yaw轴的零偏啦!
(这里存在一个问题,即地磁方向的确定,大家可以想一想如何解决)
如此迭代,MahonyAHRS就完成啦!

与此相类似的算法还有MadgWickAHRS、卡尔曼滤波等,
MadgWick是通过加速度计、地磁计计算(使用了梯度下降最优化的思想)的四元数与陀螺仪计算的四元数进行加权得到;
Kalman则是使用陀螺仪作为状态方程,加速度计、地磁计作为测量方程进行滤波得到。
这俩就不详细讲了(其实是因为我也不懂( ̄▽ ̄)”)