前言如果对理论不感兴趣可直接跳转到第三章的代码实践部分并且作者也把四元数姿态解算的代码放到最后用者自取。1.为什么不用欧拉角1.1 旋转矩阵1.1.1 矩阵的性质一般条件下不符合交换律除了一些特殊矩阵外1.1.2如何用旋转矩阵的方式来表示点的空间位置先给结论旋转的过程就是在原本坐标用列矩阵表示然后依次左乘旋转矩阵。如图所示二维下面是三维的旋转矩阵表示方式 以绕z轴方向为例 我们假设看过去的视角是沿着z轴看过去的垂直于xoy平面然后z轴与o(零点)此时重合在一块。1.2 万向节死锁1.2.1 现象从别的帖子上找的动图可以参考一下 https://i-blog.csdnimg.cn/direct/0be496eed36a4950af70924b7c8e0898.gif#pic_center就是当一个角转到-90°时剩下两个轴重合了使得物体少了一个轴的自由度1.2.2 用旋转矩阵来解释这一现象现在我们假设一点H(x0,y0,z0)我们绕x,y,z轴这样的先后顺序依次旋转10°90°10°和20°90°0°的区别结果如下。没有区别。1.2.3 用动画的方式来理解下面这个b站up主做的动画很不错可以看看。无伤理解欧拉角中的“万向死锁”现象_哔哩哔哩_bilibilihttps://www.bilibili.com/video/BV1Nr4y1j7kn/?spm_id_from333.337.search-card.all.clickhttps://www.bilibili.com/video/BV1Nr4y1j7kn/?spm_id_from333.337.search-card.all.click2.理论上四元数推导2.1四元数的构成及运算规则四元数是个复数由一个实数加三个虚部组成qabicjdk除了和复数一样的i*i-1这样之外还有他特殊的运算规则ijk-1这是人为构造的。由此可以推导出一些别的式子比如ijkji-k......;2.1.1 为啥是四元数而不是三元数我们前面可以知道在二维世界里复数乘复数代表着向量的旋转和放缩然后是一个实部加一个虚部abi那推理一下三维是不是就是abicj呢我们举个例子看看q0abicj;q1efigj;我们在运算q0×q1时会出现含ij的项ij等于什么呢先不管我直接假设ijABiCj因为他总是在现有的维度之下找到吧。但此时悖论出现了我们已知i²-1; --- i²j-j;i²jAi-BC*(ABiCj)AC-B(AB)iC²jC是实数所以C²≠-1这应该就是为什么要多一元的原因了。2.2 四元数的形式及规则2.2.1 四元数的几种形式和运算规则一般形式下面是基本运算规则最重要的是有θ角的那个表示旋转的形式和最下面行列式形式2.2.2 四元数的共轭范数逆与乘法的标准形式以下2.2.2以下的内容概念都可以与向量和复数的一些概念产生很强的联系。qabicjdk;q*a-bi-cj-dk;其实就是复数的共轭的表示形式四元数的范数可以想成是一个四维空间向量的长度。||q||²qq*a²b²c²d²根据上面的运算规则易得纯虚数实部为零的四元数0VV代表一个三维的向量。四元数的逆参考矩阵的逆及q^-1*q1;且当范数为1时q^-1q*。证明已知 q^-1*q1---q^-1 * q * q*q*---q^-1*||q||²q*---q^-1q*/||q||²当且仅当q的范数为1时q的逆等于q的共轭。四元数乘法的标准形式最后用点乘和叉乘来表示2.3四元数的几何意义2.3.1 如何用四元数代数表示三维上向量的旋转上一节我们已经知道了四元数的乘法的标准形式 现在我们将使用一个四元数去乘一个三维向量一个纯四元数来表示旋转qq0q;(q表示向量)p0v(这是一个纯四元数三维向量)可以看到红圈中是向量点乘是个标量有实部的可见这样相乘并不能像二维复数相乘一样可以表示旋转实部的出现代表他不是一个纯四元数不能代表三维向量的旋转于是为了构造出纯四元数的结果我们再引入一个四元数r。此部分不关心向量部分只关心实部是否被去掉要把点乘部分去掉那就把前面的系数去掉易得r0q0,r-q。实际上这不就是四元数的共轭q*且当范数为1时q*q^-1。这就是四元数旋转。2.3.2 罗德里格旋转公式的推导此公式是为了解决一个三维向量绕着另一个单位向量旋转的问题。我们先从简答的例子讲起。好我们知道了旋转轴和被旋转的向量在垂直关系下的旋转公式我们继续一般化其他条件不变但v向量和k向量夹角不是90°。接着罗德里格旋转公式的矩阵形式。实际上最标准的罗德里格矩阵形式是原因不过都是对的只是形式上不一样而已。这里sinθ*KK是RxI3×3代表3×3的单位矩阵2.3.3四元数轴角形式怎么来的我们已经知道了罗德里格旋转公式还有两个四元数“夹逼”旋转而这俩就可以推导出一个轴角形式。我们先假设其中k是单位向量其实是旋转轴V是一个纯四元数0v,θ是旋转的角度Vrot是旋转后的向量我们根据2.3.1四元数代数来得到Vrotcosθk*sinθ*v*(cosθ-k*sinθ)一下是推导过程即结果三重积公式证明很简单但比较麻烦就是直接用代数的方法再算一遍即可。半角也是为了去补偿左乘q右乘q^-1,因为乘了两次角度应该减半个人见解 所以说四元数的轴角公式的半角的原因也就出来了。qcos(θ/2)k*sin(θ/2)当然也可以写成这个形式qcos(θ/2)limjnk*sin(θ/2)。对应一般四元数形式q0cos(θ/2)q1l*sin(θ/2),q2m*sin(θ/2),q3n*sin(θ/2)。2.4四元数矩阵和罗德里格公式的联系2.4.1 根据罗德里格公式的矩阵形式反解欧拉角之前我们已经知道了罗德里格旋转矩阵。再带入一下轴角公式qcos(θ/2)limjnk*sin(θ/2)。得到合并起来的旋转矩阵R。还记得我们第一章讲过的方向余弦矩阵吗罗德里格本质也是一个方向余弦矩阵只是把三角的变量换成了四元数。假设我们三个轴都旋转指定角度可以是0°得到的矩阵便是把三个单轴旋转的矩阵乘起来得到先替代一下方便书写如果说我们先用这个矩阵去反解欧拉角然后再对应去带入我们的四元数矩阵反解欧拉角得到2.5四元数姿态更新2.5.1四元数关于时间的导数我们要研究四元数的更新我们就得构建四元数关于时间的微分方程到这就终于可以跟我们的陀螺仪测出的角速度建立直接联系了然后用一阶龙格库塔求解考虑到四元数与角度直接相关所以我们用三角形式的四元数来构建微分方程。既然是微分方程那我们先求四元数关于时间的导数。这是导数的定义。然后我们还得先知道四元数乘法公式其实证明也就是把四元数搞成一般形式然后乘起来利用四元数乘法规则把实部虚部分开在写成一个实数加一个方向向量的形式。很简单但繁琐我就让AI帮我证明了一下接着我们就正式开始找四元数关于时间的导数以及其对应的微分方程。这样就求出了当前时刻q对时间的导数的结果。2.5.2 一阶龙格库塔法解微分方程上一小结我们已经知道了q对时间的导数那其实直接这个结果看成变化率如果我们想求得下一时刻q那我们可以把这一时刻的q再加上这一时刻q的变化率乘上一个极短时间t。这就是一阶龙格库塔的基本思想。到这我们就知道了如何更新四元数以及反解欧拉角现在开始实践。Wx,Wy,Wz是已知的陀螺仪可以测出来角速度。3.在mpu6050上演示四元数姿态解算代码部分3.1先用纯陀螺仪四元数解算Step1:先去零漂并初始化最开始的四元数1000死循环需要一段时间静置不要动他不要动它不要动它Step2:四元数解算这里的步骤总结起来就是先找微分方程需要的变量陀螺仪然后解出四元数最后反解四元数得到欧拉角。但在用这些变量时需要一些简单的处理3.2Mahony加速度计修正这里引入了一个PI控制器误差靠的是用四元数算出的理论重力向量和加速度计测出来的实际重力向量然后分别得到二者的夹角即为误差。然后角速度数据Kp*erroKi*integret。3.2.1 估计重力方向怎么来的正常情况下如果陀螺仪水平静止放置重力向量归一化应该是001z轴是竖直方向。四元数表示就是0001。那如果旋转了之后呢我们可以先前用陀螺仪得到的四元数来计算出旋转后的重力方向怎么来呢还记得我们之前旋转公式吗q*v*q^-1(单位四元数共轭等于四元数的逆)。q(q0,q1,q2,q3)(此刻的四元数)q^-1(q0,-q1,-q2,-q3)v(0,0,0,1)最后得到这就是估计重力。而实际重力方向直接就用加速度计得到三轴数据然后归一化即可。3.2.2夹角误差怎么来的这里我们用的是叉积公式因为我们设二者叉积得到的向量为p估计重力向量为v,实际重力向量为g。那么差积|p||v|*|g|*sinθ因为v和g都是归一化后的单位向量所以|p|sinθ有因为时间很短产生的夹角很小所以θ≈sinθ所以|p|θ。3.3.3 PI参数记得调Kp调反应Ki调漂移。怎么调Kp陀螺仪随便放个角度最开始初始化完后然后一小段时间角度会增加的很快然后到一个值基本不变速度快慢反应Kp。怎么调Ki:静置看yaw如果飘的很厉害比如1s飘了0.1°就慢慢加Ki。3.3.5效果及硬件展示我们用了一块oled4脚一块mpu6050,一块stm32f103c8t6作为主控P-pitch,R-roll,Y-yaw。随机摆放了几个位置3.3.4程序源码10-1 软件I2C读写MPU6050.zip · 坎为水/stm32单片机 - 码云 - 开源中国https://gitee.com/kan-is-water/stm32-microcontroller/blob/master/10-1%20%E8%BD%AF%E4%BB%B6I2C%E8%AF%BB%E5%86%99MPU6050.zip结语感谢b站的两位佬的课程视频挺好的。https://www.bilibili.com/video/BV1gS4y1771h?vd_source02f62b9850a0e16ffc200afce1f565d7这个我拿来看大体步骤的。https://www.bilibili.com/video/BV1yW41177Y8?vd_source02f62b9850a0e16ffc200afce1f565d7这个我拿来看罗德里格的。很棒剩下的理论知识旋转矩阵之类的我是自己AI一起推的。然后如果有疑问可以在评论区或者私聊虽然我不一定会但我们可以一起讨论。