基于卡尔曼滤波的三维空间姿态仿真系统设计

基于卡尔曼滤波的三维空间姿态仿真系统设计江汉大学喻顺志谌雅洁石银标刘静斯指导老师章红任先平（江汉大学物理与信息工程学院，湖北武汉430056）摘要：本文设计的基于卡尔曼滤波的三维空间姿态仿真系统设计，本系统的开发以ＡＲＭ内核的STM32微控制器为处理核心，采集加速度计、陀螺仪和电子罗盘信号，通过Kalman滤波算法对传感器信号进行滤波，然后通过四元数算法求解出真实的空间角，然后通过nrf24l01射频模块将数据传送到上位机接收模块。通过自主开发的一款基于C#.net的三位姿态仿真平台读取并解析数据，实时仿真显示，从而在计算机或手机终端上对被测物体的三维空间姿态进行监测。关键词：STM32；Kalman滤波；四元数；nrf24l01；姿态仿真；一、引言三维仿真系统可真实模拟工况运行状态；真实模拟设备运行状态；在海洋工程领域可通过三维仿真动画来真实模拟海洋工程装备的制造过程，不仅能够有效地缩短造船周期,还能降低投资成本。在航空领域可进行各种模拟驾驶飞行培训等。在机械设计与运动领域可通过建立系统的数值模型,利用计算机仿真使大量的产品设计缺陷在产品成形之前就得到处理。本系统的可开发前景较广，可广泛应用于军事、工业、以及汽车行业等领域，例如：改正或消除船载仪器由于船体的垂荡、横摇和纵摇引起的测量误差、飞行器姿态的测量与矫正、仿人机器人肢体姿态的检测、汽车事故现场还原等等。二、系统方案设计系统由STM32位控制器、MPU6050陀螺仪加速度计模块、HMC5883指南针模块、NRF24L01射频模块、三位姿态仿真平台组成。系统工作流程1、系统初始化：包括对单片机、MPU6050模块、HMC5883指南针模块、NRF24L01射频模块的初始化2、单片机读取MPU6050模块、HMC5883指南针模块数据3、通过Kalman滤波算法对读取出的信号进行滤波4、通过四元数算法求解空间姿态角5、将姿态角通过NRF24L01射频模块发送至平台2.1kalman滤波Kalman滤波是一种实时递推算法，它所处理的是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测作为滤波器的输入，以所要估计值（状态或参数）作为滤波器的输出，滤波器输入与输出是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计出所需要处理的信号实质是一种最优估计方法。随机信号:不具有确定的频谱特性满足:MPU6050电子罗盘STM32单片机NRF24L01射频模块0EttqtTE三维姿态仿真平台白噪声序列特点:1、零均值且具有独立性2、与时间无关，与时间间隔有关，具有平稳性随机线性离散系统的方程:一步状态预测:11,1,ˆkkkkkXX状态估计:1,1,ˆˆXˆkkkkkkkkXHZKX滤波增益矩阵:11,1,kTkkkkTkkPHPHHPKkk估计误差方差阵:TkkkTkkkkkkkKRKHKIPHKIP1,2.2四元数算法概念：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考坐标系中的矢量μ的单次转动来实现。欧拉角的缺点，必须绕着不同的转动轴旋转。方向余弦法计算比较复杂。四元数是将旋转描述为一次旋转,且四元数微分方程计算方便。单位四元数：Tqqqqq32101q23222120qqqq四元数可以看做绕一个轴的一个旋转:2.3三维姿态仿真平台本系统是基于C#.net开发环境以OPENGL为插件开发的。OpenGL是行业领域中最为广泛接纳的2D/3D图形API,其自诞生至今已催生了各种计算机平台及设备上的数千优秀应用程序。OpenGL是独立于视窗操作系统或其它操作系统的，亦是网络透明的。应用在CAD、能源、娱乐、游戏开发、制造业及虚拟现实等行业领域中。OpenGL帮助程序员在PC、工作站、超级计算机等硬件设备上实现高性能、极具冲击力的高视觉表现力。三、结论0,1Pk1kQkR1,1,1[]TTkkkkkkkkkKPHHPHP,1,1,11,1,11kkTTkkkkkkkkkkPPQ,1[]kkkkkPIKHP1kk0ˆ,1Xk,1,11ˆˆkkkkkXX,1,1ˆˆˆ[]kkkkkkkkXXKZHXkZ11,11,kkkkkkkWXXkkkVHZkXzyxqqqqcos2/sincos2/sincos2/sin2/cos3210本系统中MPU6050器件件存在许多随机噪声信号，普通的滤波算法无法实现对这类随机噪声的滤除，维纳滤波又不适合在轻量级的ARM单片机上运行。卡尔曼滤波算法可以很好的克服这两个问题，从而还原出真实的信号。本系统采用的互补滤波型四元数姿态角解算算法适合在运算量小，角度融合效果较好，能更真实的用加速度计信号去补偿陀螺仪信号。OpenGL是行业领域中应用最为广泛的2D/3D图形API,使用方便资源较多适合于初学者入门。参考文献：[1]秦永元,张洪钺,王叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社,2012[2]秦永元.惯性导航[M].北京:科学出版社,2014