四轴思路(持续更新)

来源:互联网 发布:mac格式化ntfs格式 编辑:程序博客网 时间:2024/06/05 01:05

遥控器:发送数据
MPU6050:加速度做一维卡尔曼滤波,角速度做一介低通滤波。
然后四元数姿态解算
接着串级PID控制平衡,外环角度环,内环角速度环。
高度控制:高度和速度进行互补滤波(气压计得到的是高度,速度通过Z轴加速度积分获得,而加速度通过陀螺仪的Z轴得到。)
最后电机输出。

四元数姿态解算公式
四元数
第一行为载体坐标下X方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。
第一行为载体坐标下Y方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。
第一行为载体坐标下Z方向单位化向量,第一列为x向量,第二列为y向量,第三列为z向量。

龙格库塔求解四元数。。未完待续。。

原创粉丝点击