一种基于扩展卡尔曼滤波的手持云台姿态估计方法技术

一种基于扩展卡尔曼滤波的手持云台姿态估计方法，包括以下步骤：步骤1，选取姿态估计系统的状态向量，构建状态向量的预测方程；步骤2，根据量测预测方程计算量测预测值的雅克比矩阵，此矩阵为量测矩阵，完成姿态估计系统模型的建立；步骤3，将扩展卡尔曼滤波器编写为序贯滤波的形式，完成手持云台姿态估计。本发明专利技术提出的一种基于扩展卡尔曼滤波的手持云台姿态估计方法首先建立姿态估计状态模型，然后建立量测模型，最后根据选取的传感器设计滤波参数，最后进行序贯滤波形式的扩展卡尔曼滤波并输出手持云台高精度的姿态信息与速度信息。

【技术实现步骤摘要】
一种基于扩展卡尔曼滤波的手持云台姿态估计方法
本专利技术属于自动控制
，特别涉及一种基于扩展卡尔曼滤波的手持云台姿态估计方法。
技术介绍
目前，云台在航拍上应用最广泛，然而在日场生活中，人们应用最多的摄影设备是手机，在手机摄影时面临着画面抖动的问题。虽然目前摄影设备的光学防抖技术得到了长足发展，但相机内的镜头组只能对微小的运动位移进行补偿，为了拍摄到稳定的画面，手必须要长期保持稳定的状态，所以引入手持云台来进行更大范围的运动补偿是十分必要的。手持云台是近两年才兴起的，其原理与机载云台的原理是相通的，均是通过姿态传感器估计云台的姿态，向电机反馈当前的姿态信息，电机输出转矩进行高效率的运动补偿，从而使姿态始终保持稳定，最终获得稳定的拍摄画面，在手持云台系统中，姿态估计是最重要的环节。在姿态估计技术上，经典的一阶互补滤波姿态估计算法由于算法实现简单，计算量小，被广泛地应用于各工程项目中，但是，一阶互补滤波算法的性能有限，不能很好地适应有高频扰动输入的环境；基于欧拉角的姿态估计，其算法直观，易于理解，因此被广泛地应用于各种运动控制系统当中本文档来自技高网...

【技术保护点】
1.一种基于扩展卡尔曼滤波的手持云台姿态估计方法，其特征在于，包括以下步骤：/n步骤1，选取姿态估计系统的状态向量，构建状态向量的预测方程，根据状态向量的预测方程计算状态预测的雅克比矩阵，此矩阵为状态转移矩阵；/n步骤2，根据姿态估计系统中的传感器选取量测并结合状态向量构造量测预测方程，然后根据量测预测方程计算量测预测值的雅克比矩阵，此矩阵为量测矩阵，完成姿态估计系统模型的建立；/n步骤3，将扩展卡尔曼滤波器编写为序贯滤波的形式，完成手持云台姿态估计。/n

【技术特征摘要】
1.一种基于扩展卡尔曼滤波的手持云台姿态估计方法，其特征在于，包括以下步骤：
步骤1，选取姿态估计系统的状态向量，构建状态向量的预测方程，根据状态向量的预测方程计算状态预测的雅克比矩阵，此矩阵为状态转移矩阵；
步骤2，根据姿态估计系统中的传感器选取量测并结合状态向量构造量测预测方程，然后根据量测预测方程计算量测预测值的雅克比矩阵，此矩阵为量测矩阵，完成姿态估计系统模型的建立；
步骤3，将扩展卡尔曼滤波器编写为序贯滤波的形式，完成手持云台姿态估计。


2.根据权利要求1所述的一种基于扩展卡尔曼滤波的手持云台姿态估计方法，其特征在于，步骤1中，状态向量包括手持云台的姿态四元数、速度、陀螺零偏、加速度计零偏、北东地方向地磁场分量和磁力计零偏，状态向量表示为：



其中q0为手持云台姿态四元数实部，q1、q2、q3为手持云台姿态四元数虚部，vn为手持云台北向速度，ve为手持云台东向速度，vd为手持云台地向速度，εx为x轴陀螺零偏，εy为y轴陀螺零偏，εz为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏，mn为北向地磁场分量，me为东向地磁场分量，md为地向地磁场分量，mbx为x轴磁力计零偏，mby为y轴磁力计零偏，mbz为z轴磁力计零偏；
状态方程为：



其中：Xk-1——k-1时刻状态向量；Φk/k-1——状态转移矩阵；——k时刻状态向量的预测。


3.根据权利要求2所述的一种基于扩展卡尔曼滤波的手持云台姿态估计方法，其特征在于，手持云台姿态四元数的状态预测方程



其中：

——k时刻手持云台姿态四元数预测；qk-1——k-1时刻手持云台姿态四元数；Δq——k-1至k时刻手持云台姿态四元数变化量；——四元数相乘；
k-1至k时刻手持云台姿态四元数变化量计算公式如下：



其中：Δθx——k-1至k时刻手持云台x轴角增量；Δθy——k-1至k时刻手持云台y轴角增量；Δθz——k-1至k时刻手持云台z轴角增量；
k-1至k时刻手持云台各轴角增量计算公式如下：
Δθx＝(ωx-εx)·Δt
Δθy＝(ωy-εy)·Δt
Δθz＝(ωz-εz)·Δt
其中：ωx——手持云台x轴陀螺输出的角速度；εx——手持云台x轴陀螺零偏；ωy——手持云台y轴陀螺输出的角速度；εy——手持云台y轴陀螺零偏；ωz——手持云台z轴陀螺输出的角速度；εz——手持云台z轴陀螺零偏；Δt——k-1至k时刻时间间隔。


4.根据权利要求2所述的一种基于扩展卡尔曼滤波的手持云台姿态估计方法，其特征在于，手持云台速度预测方程



其中：——手持云台k时刻速度预测；vk-1——手持云台k-1时刻速度；g0——手持云台当地重力加速度；——载体坐标系至导航坐标系的坐标转换矩阵；Δv——k-1至k时刻速度增量；
载体坐标系至导航坐标系的坐标转换矩阵的计算公式如下：



k-1至k时刻速度增量的计算公式如下：









其中：ax——手持云台x轴加速度计输出的加速度；——手持云台x轴加速度计零偏；ay——手持云台y轴加速度计输出的加速度；——手持云台y轴...

【专利技术属性】
技术研发人员：陈朋印，李定涌，闫永驰，陈柯柯，马一鸣，曾幼涵，
申请(专利权)人：西安因诺航空科技有限公司，
类型：发明
国别省市：陕西;61