[发明专利]一种无人机飞行控制器姿态解算和控制方法

摘要

本发明公开了一种无人机飞行控制器姿态解算和控制方法，姿态解算方法使用由加速度计与磁力计融合得到的第一姿态矩阵，陀螺仪和上个控制周期的第三姿态矩阵融合得到的第二姿态矩阵，第一姿态矩阵和第二姿态矩阵融合得到包含高精度姿态信息的第三姿态矩阵。姿态控制方法以姿态空间矢量为控制对象，根据第三姿态矩阵得到各个电机的控制量，实现对无人机飞行器的姿态控制。本发明能适用于无人机全姿态的控制，实时性高，姿态解算和控制稳定。

主权项

1.一种无人机飞行控制器姿态解算方法，其特征在于，通过无人机飞行控制器内部的IMU模块采集原始三轴加速度计数据、原始三轴磁力计数据和原始三轴陀螺仪数据；通过以下步骤进行姿态解算：1)对原始三轴加速度计数据进行误差修正和规范化，得到修正后的三轴加速度计数据记为：aXN，aYN，aZN；2)对原始三轴磁力计数据进行误差修正和规范化，得到修正后的三轴磁力计数据记为：mXN，mYN，mZN；3)对步骤1)和2)中的两组修正后的数据进行融合，得到第一姿态矩阵；4)对原始三轴陀螺仪数据进行误差修正，得到修正后的三轴陀螺仪数据记为gX、gY、gZ；将修正后的三轴陀螺仪数据与上一个控制周期得到的第三姿态矩阵进行融合，得到第二姿态矩阵；第三姿态矩阵的初始值等于第一个控制周期得到的第一姿态矩阵的值；5)将第一姿态矩阵和第二姿态矩阵进行融合，得到该控制周期的第三姿态矩阵，即融合后的方向余弦矩阵。