【】本发明涉及无人机领域,尤其涉及一种无人机姿态解算方法、计算装置和计算机可读介质。
背景技术
0、
背景技术:
1、在小型无人机姿态解算中,一般是利用低成本的mems(micro-electro-mechanical system)传感器,包括加速度计、陀螺仪和磁力计等多个传感器融合计算姿态,但是由于无人机飞行环境过于复杂和低成本的mems传感器随机偏差大等问题,导致小型无人机姿态解算偏差较大。
2、因此,亟需提出一种新的技术方案来解决上述问题。
技术实现思路
0、
技术实现要素:
1、本发明的目的之一在于提供一种无人机姿态解算方法,其将无人机的姿态四元数和陀螺仪的随机偏差作为待估计参数,以消除随机偏差的影响,修正姿态角。
2、为实现上述目的,根据本发明的一个方面,本发明提供一种无人机姿态解算方法,所述无人机的航姿模块中包括陀螺仪、加速度计和磁力计,其包括:获得上一时刻的估计状态向量、估计状态均方误差矩阵和过程噪声协方差矩阵以及当前时刻的状态矩阵、量测噪声协方差矩阵、加速度计和磁力计的测量矩阵、加速度计和磁力计的量测向量;基于上一时刻的估计状态向量进行状态一步预测得到当前时刻的一步预测状态向量;基于当前时刻的状态矩阵、上一时刻的估计状态均方误差矩阵、上一时刻的过程噪声协方差矩阵进行计算得到当前时刻的一步预测均方误差矩阵;基于当前时刻的加速度计和磁力计的测量矩阵、当前时刻的测量噪声协方差矩阵、当前时刻的一步预测均方误差矩阵计算当前时刻的滤波增益;基于当前时刻的一步预测状态向量、当前时刻的滤波增益、当前时刻的加速度计和磁力计的量测向量进行状态估计得到当前时刻的估计状态向量;基于当前时刻的一步预测均方误差矩阵、当前时刻的滤波增益、当前时刻的加速度计和磁力计的测量矩阵、当前时刻的量测噪声协方差矩阵计算得到当前时刻的估计状态均方误差矩阵,其中所述估计状态向量和所述一步预测状态向量均包括无人机的姿态四元数和陀螺仪的随机偏差。
3、根据本发明的另一个方面,本发明提供一种计算装置,包括:存储器,用于存储程序;处理器,用于加载所述程序以执行上文的无人机姿态解算方法。
4、根据本发明的再一个方面,本发明提供一种计算机可读介质,其内存储有程序,该程序被执行以实现上文所述的无人机姿态解算方法。
5、与现有技术相比,本发明中具有如下优点中的一个或多个:1)可以基于低成本mems陀螺仪、加速度计和磁力计等多传感器进行姿态解算;2)将无人机的姿态四元数和陀螺仪的随机偏差作为待估计参数,以消除随机偏差的影响,修正姿态角;3)在不同的飞行条件下,基于自适应滤波的思想,基于自适应因子来不断地调节加速度计和磁力计的测量噪声方差,提高了航姿滤波在复杂条件下的鲁棒性和抗扰性。
1.一种无人机姿态解算方法,所述无人机的航姿模块中包括陀螺仪、加速度计和磁力计,其特征在于,其包括:
2.根据权利要求1所述的无人机姿态解算方法,其特征在于,
3.根据权利要求2所述的无人机姿态解算方法,其特征在于,
4.根据权利要求3所述的无人机姿态解算方法,其特征在于,
5.根据权利要求1所述的无人机姿态解算方法,其特征在于,其还包括:
6.根据权利要求1所述的无人机姿态解算方法,其特征在于,所述陀螺仪、所述加速度计和所述磁力计分别为mems陀螺仪、mems加速度计、mems磁力计。
7.一种计算装置,其特征在于,包括:
8.一种计算机可读介质,其内存储有程序,该程序被执行以实现如权利要求1-6任一所述的无人机姿态解算方法。