新型自适应卡尔曼无人机姿态解算方法与流程

本发明涉及无人机控制技术领域，具体而言，涉及一种新型自适应卡尔曼无人机姿态解算方法。

背景技术：

姿态是无人机的重要飞行参数，对无人机的正常运行具有十分重要的意义。目前常用的姿态解算方法有互补滤波姿态解算算法，基于梯度下降法的姿态解算算法，基于卡尔曼滤波器的姿态解算算法，以及粒子滤波姿态解算算法。其中，互补滤波姿态算法主要集中于滤波器系数的自适应调节，以提高算法对于噪声的补偿，精度并不高，kalman滤波是目前较为适合多传感器姿态融合的算法，但常规kalman滤波算法需要系统噪声和量测噪声的统计特性已知，这会使常规kalman滤波估计精度降低，失去算法最优性，粒子滤波姿态解算算法虽然精度较高，但在系统干扰下，易导致算法发散而失败。上述的姿态解算系统大都具有逻辑结构复杂，运行效率低下，精度差的缺陷，并不适用于现下低成本、技术集成度高的无人机飞行器的姿态解算需求。

技术实现要素：

本发明正是基于上述技术问题至少之一，提出了一种新的新型自适应卡尔曼无人机姿态解算方法，将改进型自适应算法与信息卡尔曼滤波算法相结合，可以在线更新状态方程的量测误差，提高对无人机航姿态解算的精确度。

有鉴于此，本发明提出了一种新的新型自适应卡尔曼无人机姿态解算方法，包括：选用北东地右手坐标系，创建动态无人机姿态解算系统的离散时间空间模型：

其中，动态无人机姿态解算系统包括用于测量无人机加速度的加速度计、用于接收无人机位置信息的gps接收模块、用于测量无人机飞行磁偏角的磁强计模块、用于测量无人机飞行角速度的陀螺仪以及用于测量无人机对地飞行高度的无线电测高仪模块，xk＝[φkθkψk]^t为状态矩阵，q为噪声矩阵，zk＝[zφzθzψ]^t为量测矩阵，观测方程的量测向量z由加速度计以及磁强度计计算，r为系统量测噪声，hk为系统量测状态转移矩阵，φ为横滚角，θ为俯仰角，ψ为偏航角，模型矩阵微分方程w＝[wxwywz]^t为无人机角速度，观测方程由公式确定，；根据状态变量的维数，确定sigma点集：其中，表示矩阵方根的第i列；根据以下公式计算sigma点集的权值：

其中，下标m表示均值，c表示协方差，上标表示第几个采样点，参数λ＝a²(n+κ)-n是缩放比例参数，用于降低总的预测误差，a的选取控制了采样点的分布状态，κ为另外一个缩放比例参数，用于保证矩阵(n+λ)p为半正定矩阵，参数β≥0是一个非负的权系数，用来合并方程中高阶项的动差；根据公式计算sigma点集的一步预测；根据sigma点集的预测值加权求和得到系统状态量的一步预测及协方差矩阵：

根据一步预测值，由ut变换产生新的sigma点集：

将新的sigma点集代入离散时间空间模型中的观测方程，以得到预测的观

测量：

将所述预测的观测量通过加权求和得到系统预测的均值及协方差：

更新卡尔曼增益矩阵以及系统的状态和协方差

在上述技术方案中，还包括：根据以下公式对βk进行更新：βk＝βk-1(βk-1+b)^-1,β1＝i，其中，b＝diag([bφbθbψ])为渐消记忆因子，0＜bi＜1，i＝φ，θ，ψ，b取值为0.9至0.999。

在上述任一项技术方案中，优选地，俯仰角θ由以下公式进行限定：

根据坐标变换矩阵，得到由加速度描述的横滚角及俯仰角其中，ab＝[axayaz]^t表示加速度计的输出值，an＝[00g]^t为重力加速度在参考坐标系下的向量表示；根据公式计算偏航角ψ，其中，mb＝[mxmymz]^t为磁强计模块在x轴、y轴和z轴的输出值，δψ为真北与磁北的夹角，且每次姿态解算后，根据公式对mx和my进行修正。

通过以上技术方案，可以有效的补偿低成本传感器存在的姿态漂移以及噪声影响，可以有效提高姿态解算精度，将改进型自适应算法与信息卡尔曼滤波算法相结合，可以在线更新状态方程的量测误差，同时避免了使用扩展卡尔曼滤波算法带来的高阶截断误差，然后使用加速度计以及磁强计作为量测信息校准陀螺仪解算欧拉角的误差，提高了基于惯性测量单元解算的准确性与可靠性。

附图说明

图1示出了根据本发明的实施例的新型自适应卡尔曼无人机姿态解算方法的示意流程图；

图2示出了根据本发明的实施例的无人机姿态解算系统的结构示意框图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

以下结合图1和图2对本发明的技术方案做进一步说明：

如图1所示，新型自适应卡尔曼无人机姿态解算方法包括以下步骤：

步骤102，选用北东地(ned)右手坐标系，创建动态无人机姿态解算系统的离散时间空间模型。其中，动态无人机姿态解算系统具体如图2所示，包括用于测量无人机加速度的加速度计、用于接收无人机位置信息的gps接收模块、用于测量无人机飞行磁偏角的磁强计模块、用于测量无人机飞行角速度的陀螺仪以及用于测量无人机对地飞行高度的无线电测高仪模块，其中，加速度计可选用mems三轴加速度计，陀螺仪可选用mems三轴角速度计；无人机姿态解算系统的离散时间空间模型为其中，公式(1)中xk＝[φkθkψk]^t为状态矩阵，q为噪声矩阵，zk＝[zφzθzψ]^t为量测矩阵，r为系统量测噪声，hk为系统量测状态转移矩阵，φ为横滚角，θ为俯仰角，ψ为偏航角，使用欧拉角法来预测三个姿态角，无人机姿态解算系统离散时间状态空间模型矩阵微分方程为防止方程退化，俯仰角θ由式进行限定，式(1)中观测方程的量测向量z可以由加速度计(如mems三轴加速度计)和磁强计直接计算，式(1)中的观测方程可以由式计算，利用坐标变换矩阵可得到由加速度描述的横滚角和俯仰角，其中，ab＝[axayaz]^t表示加速度计的输出值，an＝[00g]^t为重力加速度在参考坐标系下的向量表示，借助磁强计的输出值，利用公式解算出偏航角，其中，mb＝[mxmymz]^t为磁强计模块在x轴、y轴和z轴的输出值，δψ为真北与磁北的夹角，且每次姿态解算后，根据公式对mx和my进行修正。

步骤104，根据状态变量的维数，确定sigma点集。

在本实施例中，由于状态变量为三维，所以取7个sigma点：其中，表示矩阵方根的第i列。

步骤106，计算sigma点集的权值。具体地，根据公式计算sigma点集的权值，其中，下标m表示均值，c表示协方差，上标表示第几个采样点，参数λ＝a²(n+κ)-n是缩放比例参数，用于降低总的预测误差，a的选取控制了采样点的分布状态，κ为另外一个缩放比例参数，用于保证矩阵(n+λ)p为半正定矩阵，参数β≥0是一个非负的权系数，用来合并方程中高阶项的动差。

步骤108，计算sigma点集的一步预测。具体地，可利用公式计算sigma点集的一步预测。

步骤110，根据sigma点集的预测值加权求和得到系统状态量的一步预测及协方差矩阵：其中，权值由公式计算。

步骤112，根据一步预测值，由ut变换产生新的sigma点集：

步骤114，将新的sigma点集代入离散时间空间模型中的观测方程，以得到预测的观测量：

步骤116，将预测的观测量通过加权求和得到系统预测的均值及协方差。具体地，系统预测的均值及协方差公式如下：

步骤118，更新卡尔曼增益矩阵及系统的状态和协方差并返回步骤110。

进一步地，在上述计算过程中，可利用指数渐消记忆法对βk的更新，可有效消除在无人机飞行过程中，量测噪声的弱非平稳性带来的误差，具体更新公式如βk＝βk-1(βk-1+b)^-1,β1＝i，其中，b＝diag([bφbθbψ])为渐消记忆因子，0＜bi＜1，i＝φ，θ，ψ，b取值为0.9至0.999。

针对无人机加速度计容易受到噪声和震动等的干扰，采用了低通fir滤波器对加速度信号进行了处理，基于汉明窗的线性相位fir滤波器的传递函数如式其中，n是滤波器阶数，hk是滤波器系数由截止频率决定。

为了有效的获取加速度计的截止频率，使用离散傅里叶变换来计算谱振幅，如式其中，dft表示离散傅里叶变换，m是频率(f)的谱振幅。

以上结合附图详细说明了本发明的技术方案，本发明的技术方案提出了一种新的新型自适应卡尔曼无人机姿态解算方法，可以有效的补偿低成本传感器存在的姿态漂移以及噪声影响，可以有效提高姿态解算精度，将改进型自适应算法与信息卡尔曼滤波算法相结合，可以在线更新状态方程的量测误差，同时避免了使用扩展卡尔曼滤波算法带来的高阶截断误差，然后使用加速度计以及磁强计作为量测信息校准陀螺仪解算欧拉角的误差，提高了基于惯性测量单元解算的准确性与可靠性。

上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。