The invention discloses an attitude estimation method, which can fuse the readings of gyroscopes, accelerometers and magnetometers, estimate the quaternion of the measured object in real time, and then obtain its attitude angle. The invention mainly includes the following steps: initialization, reading gyroscope data and calculating quaternion prediction and estimation, reading accelerometer data and calculating dynamic function to obtain fusion weight, calculating quaternion measurement value from accelerometer, calculating quaternion measurement value from magnetometer, calculating quaternion observation vector and calculating final estimation of quaternion. The invention has a small amount of calculation and a certain robustness to the motion interference of the tested object.
【技术实现步骤摘要】
一种姿态估计方法
本专利技术涉及测量
,特别是涉及一种姿态估计方法。
技术介绍
四旋翼飞行器又称为四轴飞行器,是一种能够垂直升降的飞行器。因为其具有较强的机动性能、体积小、隐蔽性好等优势而被广泛应用在工业和军事上。在最近几年的研究领域中也成为了新的研究热点。四旋翼飞行器的设计主要包括姿态解算和飞行控制两部分,其中,姿态解算是飞行器设计的核心和重点。文献“周克旻,周蓉,滕婧,陈亦奇.多标记室内小型无人机定位与姿态估计方法[J/OL].计算机工程与应用:1-11”针对目前流行的基于虚拟现实(AR)技术的ArUco标记算法和颜色空间域标记算法设计了一种多标记的无人机跟踪系统。这种方法将Kinect深度摄像机固定在实验环境顶部,直接对有ArUco标记的无人机进行定位跟踪与姿态估计。这种方法免除了机载摄像机,减轻无人机重量,同时也简化了跟踪系统。但是这种方法无法在摄像机视野外工作。文献“付雷,章政,余义.基于改进型显性互补滤波的MENS姿态解算[J].自动化与仪表,2018,33(11):7-12”针对单一的传感器无法准确进行姿态估计以及低成本MENS器件易发散的问题,提出了一种改进显性互补滤波算法。该算法利用加速度计校正陀螺仪漂移引起的姿态误差,并在数据融合之前对加速度原始数据进行巴特沃斯低通滤波,同时设置缓存区保存更新后的四元数向量,减小加速度通过低通滤波器之后产生的延时对姿态估计的影响,提高姿态解算的实时性。但是,文中涉及的Kp、Ki参数的设置具有一定的盲目性,缺乏一定的理论依据。如何根据传感器噪声统计特性变化来自动调整滤波增益一直是一个难解问题。为此, ...
【技术保护点】
1.一种姿态估计方法,其特征在于,包括以下步骤:步骤一:令物体处于静止状态,并检测其姿态角,然后根据姿态角换算出其四元数
【技术特征摘要】
1.一种姿态估计方法,其特征在于,包括以下步骤:步骤一:令物体处于静止状态,并检测其姿态角,然后根据姿态角换算出其四元数初始化一个长度为L的队列A,并令该队列为空;初始化融合权重φ,令其为0.5;步骤二:采集陀螺仪数据w=[wx,wy,xz]′,计算四元数向量的预测估计如下:其中,T表示采样时间间隔,I4表示4×4单位向量,步骤三:读取加速度计数据a=[ax,ay,az]′,并加入到队列A中,然后计算检测函数f,如下其中,表示加速度计本体白噪声方差,g为重力加速度,l(·)表示取二范数;计算检测函数的理论方差如下:接着,判断是否有如...
【专利技术属性】
技术研发人员:刘阳,刘珂,佀昶,赵强,
申请(专利权)人:合肥优控科技有限公司,
类型:发明
国别省市:安徽,34
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。