基于陀螺仪和加速度计的飞行机器人自平衡算法研究-曹珂杰-南京工程学院.doc

江苏省高等学校 大学生实践创新训练计划项目申报表 推荐学校： 南京工程学院 项目名称： 基于陀螺仪和加速度计的飞行器自平衡算法的研究 项目类型： √重点项目 □一般项目 □ 指导项目 所属一级学科名称： 电子信息类 项目负责人： 曹珂杰 联系电话： 指导教师： 联系电话： 申报日期： 2013年4月24日 江苏省教育厅 制 二○一三年三月 项目名称 基于陀螺仪和加速度计的飞行机器人自平衡算法的研究 项目所属 一级学科 08工学 项目所属 二级学科 电子信息类 项目类型 （√）重点项目 （）一般项目 （ ）指导项目 项目实施时间 起始时间： 2013 年 6 月 完成时间： 2014年 5 月 项 目 简 介 (100字以内） 鉴于融合机械臂与四轴飞行器于一体的飞行机器人高空作业时稳定性差的缺点，利用本套基于陀螺仪和加速度计的自平衡算法，可以消除机械臂运动对飞行器位姿产生的影响，实现系统的自平衡。 主要成果 第二指导教师 姓名 单位 年龄 专业技术职务 主要成果 一、申请理由（包括自身具备的知识条件自己的特长、兴趣利用搭载机械臂的四轴飞行器尚未成熟。VNoise=350ug Bw ×1.5式中:Bw为传感器带宽(单位为Hz).因此在设计卡尔曼滤波器时,首先要确定被测加速度的频率范围,然后再设计滤波器的参数,尽量使滤波器带宽略高于被测频率,这样不仅有助于滤除高频干扰,而且也有利于降低系统噪声干扰.但是如果要得到精确的倾角值,带宽就需要设置得比较小,而这时加速度计动态响应慢,不适合跟踪动态角度运动,如果期望快速的响应,又会引入较大的噪声.再加上其测量范围的限制,使得单独应用加速度计检测飞行器倾角并不合适,需要与其他传感器共同使用.陀螺仪的作用是用来测量角速度信号,通过对角速度积分,便能得到角度值.陀螺仪本身极易受噪声干扰,微机械陀螺不能承受较大的震动,同时由于温度变化、不稳定力矩等因素,陀螺仪会产生漂移误差,并会随着时间的推移而累加变大,通过积分会使得误差变得很大.因此,也不能单独使用陀螺仪作为本系统的倾角传感器. 2 卡尔曼滤波融合过程 首先建立系统的状态方程和测量方程.由于倾角和倾角角速度存在导数关系,系统倾斜真实角度φ可以用来做一个状态向量.利用机械臂角度，估计机械臂下一个位置，对机械臂产生何种影响。在该系统中,采用加速度计估计出陀螺仪常值偏差b,以此偏差作为状态向量得到相应的状态方程和观测方程: ? 式中,ωgyro为包含固定偏差的陀螺仪输出角速度,φacce为加速度计经处理后得到的角度值,wg为陀螺仪测量噪声,wa为加速度计测量噪声,b为陀螺仪漂移误差,wg与wa相互独立,此处假设二者为满足正态分布的白色噪声,令Ts为系统采样周期,得到离散系统的状态方程和测量方程: 同时,要估算k时刻的实际角度,就必须根据k-1时刻的角度值,再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差,在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值.在此,须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正.Q与R矩阵的形式如下: 式中,q_acce和q_gyro分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据的信任程度,值越小,表明信任程度越高.在该系统中陀螺仪的值更为接近准确值,因此取q_gyro的值小于q_acce的值.当前状态: 式中 X(k|k-1)是利用k预测的结果,X(k-1|k-1)是k-1时刻的最优结果. 则有对应于X(k|k-1)的协方差为: P(k|k-1)=AP(k-1|k-1)AT+Q (2) 式中,P(k-1|k-1)是X(k-1|k-1)对应的协方差,AT表示A的转置矩阵,Q是系统过程的协方差.式子(1)、(2)即对系统的状态更新.则状态k的最优化估算值 X(k|k):X(k|k)=X(k|k-1)+K(k)(Z(k)-HX(k|k-1)) (3) 其中H =[1 0] K为卡尔曼增益(KalmanGain):K(k)=P(k|k-1)HT/(HP(k|k-1)HT+R) (4) 此时,我们已经得到了k状态下最优的估算值X(k|k).但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值,我们还要更新k状态下X(k|k)的协方差: P(k|k)=(I-Kg(k)H)P(k|k-1) (5) 其中,I为单位阵,对于本系统则有 当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1). (3)、(4)、(5)式为卡尔曼滤波器状态更新方程.计算完时间更新方程和测量更新方程后,再次重复上一次计