姿态角是控制四轴的依据，那么，要控制四轴到正确的角度，就必须有一个正确的角度，这个正确包括，3个欧拉角的静态正确，还有动态准确。
要计算出水平的两个欧拉角，最简单的方法就是利用加速度机，计算重力加速度在各个轴上的投影，通过反三角函数，就能计算出姿态，但实际上，加速度不止重力加速度。还有运动加速度，还有噪音加速度。用这个计算出来的欧拉角，有噪音，特别是电机的振动噪音，有运动加速度的干扰，所以，这个欧拉角没法用来控制四轴。还有就是3个角速度，角速度的特点是噪音低，不会受运动的干扰。但缺点是我们前面说过的，想要四轴能自平衡，需要的是角度，用角速度直接积分的角度，没有平衡点参照，积分导致的偏移。目前用的方法是数据融合。而融合的方法也不止一种。最常见的是权值融合，就是互补滤波了。先写下计算方法，陀螺积分角度+=角速度*dt；加速度角度=反三角函数（重力加速度在该轴的投影）；融合角度=陀螺权值*陀螺积分角度+(1-陀螺权值)*加速度角度；还有一种叫最大梯度法的，就是融合算法有些区别：
陀螺积分角度+=角速度*dt；加速度角度=反三角函数（重力加速度在该轴的投影）；角度偏差=加速度角度-陀螺积分角度；融合角度=陀螺角度+衰减系数*角度偏差；这两张种计算方法其实本质是一样的，只是表达效果不同，但最大梯度法可以引入高一介的数据，用来矫正陀螺本身的静态漂移：角度偏差积分+=角度偏差；角速度=角速度+衰减系数*角度偏差积分；这个就可以一定程度上矫正陀螺的零漂。
接下来说说很常用的四元数，说这个之前当然要先说为什么要用这个没法看懂的算法。上面说的都是一个轴的融合。但四轴是3个轴的角度，而且，是要用角速度积分出来的，那就会出现一个问题。轴间的互相影响。当四轴不是水平状态时，一个轴的旋转，都能影响其他轴的角度的。注意这不是什么传感器误差照成的影响。比如一本书，倾斜一定角度后，把书转动起来，你会发现，书的xy欧拉角在不停的变化。而四元数的计算，就是把这些统统计算进去了，你可以认为四元数把复杂的3维姿态运动放到四维空间中，就成了简单的运动了。比如把3维物体的简单旋转运动，投影到2维图像中，就成了复杂的形态变化了。至于你能不能看懂四元数的计算，反正我只能看个大概。不过我能在3维空间中把各个轴的相互影响，都计算出来。方法是：x轴角速度=x轴角速度*（1-y轴角度/全姿态）+z轴角速度*z轴相对水平面夹角;同样其他的轴都要用另外的轴来融合；最后同样的计算出角速度积分角度和加速度角度融合。 |
楼主
标题置顶
标题高亮
