一种无人装备姿态稳定与自适应航向控制方法和相关设备

本发明公开了一种无人装备姿态稳定与自适应航向控制方法和相关设备，该方法包括：采集角速度、加速度和磁场数据；基于扩展卡尔曼滤波，融合角速度、加速度和磁场数据，输出估计姿态四元数；根据估计姿态四元数，获取姿态控制量以及第一航向控制量；根据估计姿态四元数，判断姿态角度是否大于设定阈值，若姿态角度大于设定阈值，则对第一航向控制量进行动态修正，得到第二航向控制量，若姿态角度小于或者等于设定阈值，则将第一航向控制量作为第二航向控制量输出；通过伪逆矩阵运算，将姿态控制量和第二航向控制量转换为驱动信号；根据驱动信号，控制无人装备运动。本发明能够提高姿态稳定性和航向跟踪精度，可以广泛应用于自主导航控制技术领域。