遥控器:发送数据
MPU6050:加速度做一维卡尔曼滤波,角速度做一介低通滤波。
然后四元数姿态解算。
接着串级PID控制平衡,外环角度环,内环角速度环。
高度控制:高度和速度进行互补滤波(气压计得到的是高度,速度通过Z轴加速度积分获得,而加速度通过陀螺仪的Z轴得到。)
最后电机输出。
四元数姿态解算公式:
第一行为载体坐标下X方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。
第一行为载体坐标下Y方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。
第一行为载体坐标下Z方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。
龙格库塔求解四元数。。未完待续。。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)