基于陀螺仪和加速度计的飞行机器人自平衡算法研究曹珂杰南京工程学院Word格式文档下载.docx

1、2013年4月24日江苏省教育厅 制二一三年三月项目名称基于陀螺仪和加速度计的飞行机器人自平衡算法的研究项目所属一级学科 08工学二级学科 项目类型（）重点项目 （）一般项目 （ ）指导项目项目实施时间起始时间： 2013 年 6 月 完成时间： 2014年 5 月项目简介（100字以内）鉴于融合机械臂与四轴飞行器于一体的飞行机器人高空作业时稳定性差的缺点，利用本套基于陀螺仪和加速度计的自平衡算法，可以消除机械臂运动对飞行器位姿产生的影响，实现系统的自平衡。主要成果第二指导教师姓名单位年龄专业技术职务一、申请理由（包括自身具备的知识条件、自己的特长、兴趣、已有的实践创新成果等）熟练应用51单片

2、机，并且开始学习arm，以及熟练掌握Protel 99SE，DXP，Multisim，Altium designer等电路制作以及电路仿真软件的使用，VC+6.0，VS2010，IAR，Keil，OPENCVCodeWarrior等多种编译环境的使用，Matlab等多种数字处理软件的使用。现正在学习LabView的使用及技巧，基于Linux环境下的软件的编写，Matlab的深入研究。本组组员已有实践创新成果：（1） 基于51单片机的智能小车（2） 基于OPENCV双目测距（3） 亚洲水下机器人亚军（4） 对四轴飞行器的研究与制作（5） 对各种算法深入研究（6） 对机械臂精细研究二、项目方案1、

3、 研究现状： 四轴飞行机器人已广泛应用到军事、消防、安检、娱乐等场所； 对机械臂的研究和制作颇具规模；利用搭载机械臂的四轴飞行器高空作业技术尚未成熟。2项目研究目标及主要内容 研究基于陀螺仪和加速度计的自平衡算法，消除机械臂运动对飞行器位姿产生的影响，实现系统的自平衡，从而完成减小机械臂对物品的操作误差的目标。3.项目主要技术方向：方案一：姿态角度测控系统主要由加速度计、陀螺仪、微控制器、滤波电路、电机调速器、无刷电机等部分组成.姿态检测系统的硬件平台如图,由微处理器对陀螺仪、滤波电路和加速度计构成的传感器组进行高速A/D采样后,通过卡尔曼滤波器对传感器数据进行补偿和信息融合,得到准确的姿态角

4、度信号,此角度信号再通过PID控制器运算,输出给电子调速器转换成PWM信号,进而对电机进行控制.加速度计用于测量物体的线性加速度,加速度计的输出值与倾角呈非线性关系,随着倾角的增加而表现为正弦函数变化.因此对加速度计的输出 进 行 反 正 弦 函 数 处 理,才 能 得 到 其 倾角值5-6.测量数据噪声与带宽的平方根成正比,即噪声会随带宽的增加而增加.VNoise=350ug Bw 1.5式中:Bw为传感器带宽（单位为Hz）.因此在设计卡尔曼滤波器时,首先要确定被测加速度的频率范围,然后再设计滤波器的参数,尽量使滤波器带宽略高于被测频率,这样不仅有助于滤除高频干扰,而且也有利于降低系统噪声干

5、扰.但是如果要得到精确的倾角值,带宽就需要设置得比较小,而这时加速度计动态响应慢,不适合跟踪动态角度运动,如果期望快速的响应,又会引入较大的噪声.再加上其测量范围的限制,使得单独应用加速度计检测飞行器倾角并不合适,需要与其他传感器共同使用.陀螺仪的作用是用来测量角速度信号,通过对角速度积分,便能得到角度值.陀螺仪本身极易受噪声干扰,微机械陀螺不能承受较大的震动,同时由于温度变化、不稳定力矩等因素,陀螺仪会产生漂移误差,并会随着时间的推移而累加变大,通过积分会使得误差变得很大.因此,也不能单独使用陀螺仪作为本系统的倾角传感器.2 卡尔曼滤波融合过程首先建立系统的状态方程和测量方程.由于倾角和倾角

6、角速度存在导数关系,系统倾斜真实角度可以用来做一个状态向量.利用机械臂角度，估计机械臂下一个位置，对机械臂产生何种影响。在该系统中,采用加速度计估计出陀螺仪常值偏差b,以此偏差作为状态向量得到相应的状态方程和观测方程:式中,gyro为包含固定偏差的陀螺仪输出角速度,acce为加速度计经处理后得到的角度值,wg为陀螺仪测量噪声,wa为加速度计测量噪声,b为陀螺仪漂移误差,wg与wa相互独立,此处假设二者为满足正态分布的白色噪声,令Ts为系统采样周期,得到离散系统的状态方程和测量方程:同时,要估算k时刻的实际角度,就必须根据k-1时刻的角度值,再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方

7、差,在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值.在此,须知道系统过程噪声协方差阵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|

8、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 0K为卡尔曼增益（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为单位阵,对于

9、本系统则有当系统进入k+1状态时,P（k|k）就是式子（2）的P（k-1|k-1）.（3）、（4）、（5）式为卡尔曼滤波器状态更新方程.计算完时间更新方程和测量更新方程后,再次重复上一次计算得到的后验估计,作为下一次计算的先验估计,这样,周而复始、循环反复地运算下去直至找到最优的结果.方案二：在姿态控制算法里，只用标准的PI和加权移动平均这2种算法，没有采用卡尔曼滤波或别的“高深”算法。具体流程如下：首先利用处理器的AD功能，将陀螺仪、加速度计的数值读取近来。由于加速度计对振动非常敏感，所以我这里对加速度计做了一个移动期为10的加权移动平均，用来过滤掉部分振动。由于我的四轴对这些数据的采集是工

10、作在300Hz的频率下，所以移动期为10对我的四轴来说只相当于延迟33ms，再加上适当的加权，基本可以消除由于加权移动平均带来的响应滞后问题。对于陀螺仪，由于这个比较重要，所以我没有做多余的处理，就把AD的数据直接拿来用了。以前也测试过移动期为2的情况下，也没有问题。下面对每个轴的陀螺仪做PI，就可以得到马达修正量。将这个量加上油门后输出给马达，就完成姿态调整任务了。不过这里就有一个“隐患”。由于陀螺仪有漂移，也就是以恒定速度右转90度，然后再左转90度后，陀螺仪积分不会为0。如下图所示：可以看到在四轴最后回到平衡位置时，陀螺仪积分并不为0。这个偏差在所有陀螺仪里都存在，在ENC03上更加厉害

11、一些，所以需要对陀螺仪的积分数据进行适当处理后才能使用，否则积分会越积越大。这就是下面要讨论的。如何将遥控器的控制信号融合进来：在姿态控制算法里，我只用到了标准的PI和加权移动平均这2种算法，没有采用卡尔曼滤波或别的“高深”算法。对于陀螺仪，由于这个比较重要，所以我没有做多余的处理，就把AD的数据直接拿来用了。如何调整PI参数：实时融合： 1.将陀螺仪积分和加表滤波后的值做差； 2.按照情况对差值进行衰减，并作限幅处理； 3.将衰减值加入到角度中。 长期融合： 长期融合主要包括两个部分，一是对机械臂下一个位置的估计，二是对角速度的漂移进行估计（估计值是要在每一个控制周期都耦合到角度中的），三是

12、对陀螺仪的常值误差（也就是陀螺仪中立点）进行实时的修正。 1.将陀螺仪积分的积分和加速度积分做差（PS：为什么这里要使用加表积分和陀螺仪积分的积分，因为在256个检测周期内，有一些加速度计的值含有有害的加速度分量，如果只使用一个时刻的加表值对陀螺仪漂移进行估计，显然估计值不会准确，使用多个周期的值进行叠加后做座平均处理，可以减小随机的有害加速度对估计陀螺仪漂移过程中所锁产生的影响） 2.将上面两个值进行衰减，得到估计的陀螺仪漂移 3.对使考虑了陀螺仪漂移和不考虑陀螺仪漂移得到的角度做差，如果这两个值较大，说明陀螺仪在前段时间内测到的角速率不够准确，需要对差值误差（也就是陀螺仪中立点）进行修正，

13、修正幅度和差值有关 长期融合十分关键，如果不能对陀螺仪漂移做修正，则系统运行一段时间后，速率环的稳定性会降低。德国人的控制算法的核心是对角速度做PI计算，P的作用是使四轴能够产生对于外界干扰的抵抗力矩，I的作用是让四轴产生一个与角度成正比的抵抗力。 如果只有P的作用，将四轴拿在手上就会发现，四轴能够抵抗外界的干扰力矩的作用，而且这个抵抗力非常快速，只要手妄图改变四轴的转速，四轴就会产生一个抵抗力矩，但是，如果用手将四轴扳过一个角度，则四轴无法自己回到水平的角度位置，这就需要I的调节作用。 对角速度做I（积分）预算实际得到的就是角度，德国人四轴里面用的也是角度值，如果四轴有一个倾斜角度，那么四轴

14、就会自己进行调整，直到四轴的倾角为零，它所产生的抵抗力是与角度成正比的，但是，如果只有I的作用，会使四轴迅速产生振荡，因此，必须将P和I结合起来一起使用，这时候基本上就会得到德国四轴的效果了。如何将加速度计的数据融合进陀螺仪积分里：根据上面的说明，已经可以大概了解为什么要对陀螺仪积分进行处理了。但这里的陀螺仪积分还有另一个重要的物理意义，那就是这个积分记录了四轴偏转的绝对角度（虽然不是直接的角度，但是只要乘一个系数就可以变角度了），而不再是角速度。为了能做到四轴的稳定悬停，这个角度的重要性不可小看。通常的做法，是利用加速度计一个轴的偏转和陀螺仪积分建立对照关系，通过试验测出一个系数。然后程序不

15、断通过加速度计的偏转计算得出陀螺仪应该的积分大小，根据这个数值对陀螺仪积分进行修正。当然这个修正也有讲究，决不能直接替换掉陀螺仪的积分。通常都是将通过加速度计得出的积分“猜测值”缩小，限幅后加入陀螺仪的积分里。过大或过小都可能造成问题，须要反复试验才能得出合理值。在我的四轴里，我测出的陀螺仪和加速度计的对照关系是：当加速度计偏差=94 时，陀螺仪积分=696。这是 100Hz 积分频率，加速度计数据放大 6 倍，3.3V 参考电压下得出的。这个系数决定了加速度计积分猜测值的“斜率”大小。然后我把这个计算值缩小 22 倍，然后限制幅度为正负 5，最后才将数据加入陀螺仪积分里。在加入了积分修正代码

16、后，陀螺仪积分就可以正确收敛了。更高级的做法是利用卡尔曼滤波将加速度计和陀螺仪数据进行融合，得出绝对角度，以次来进行姿态纠正。不过卡尔曼滤波比较复杂，数据延迟的问题我一直没能很好地解决，所以我的四轴里没有使用这种方法。摄像头部分：由于摄像头本身存在误差，增加透镜中心之间的距离，可以提高深度信息的精度，但减小了 2 个摄像头共同影响的区域。此外，为保证精度，还要对透镜进行标定。 机械手臂的运动学分析对机械手臂进行控制，首先要进行数学建模，并分析其运动学规律，机械手臂可以简化为连杆和关节组成的模型，如图 O 为基座原点，d1、d2分别是连杆 1、2 的长度，d3为连杆 3 和 4 的总长，1、2和

17、 3分别为关节 1、2 和 3 的关节变量，关节 4 相当人的腕关节，方便抓物。关节 1 的旋转轴平行于连杆 1，关节 2 和 3的旋转轴垂直于平面。对于平面上一点（x,y），要使机械手臂末端达到此坐标点，须满足：通过双目视觉得到物体坐标，首先旋转关节 1，使机械手臂和物体在同一个平面上，然后控制关节 2 和 3，使机械手臂末端达到指定位置。四轴飞行器是一个具有 6 个自由度和 4 个输入的欠驱动系统, 具有不稳定和强耦合等特点, 除了受自身机械结构和旋翼空气动力学影响外, 也很容易受到外界的干扰。飞行着的高空作业机器人的姿态最终通过调节 4个电机的转速进行调整, 飞行控制系统通过各，传感器获

18、得飞行机器人的姿态信息, 经过一定的控制算法解算出 4个电机的转速, 通过 I2C 接口发送给电机调速器调整 4 个电机的转速, 以实现对其姿态的控制。预计成果图：5.进度安排：具体的时间进度和任务节点安排如下：2013.052013.6 项目申报与审批，资料收集和技术调研2013.072013.9 技术的总体方案设计2013.92013.11 各个子模块的搭建和完善2013.1220141 各模块的调试2014.220143 整体编程调试2014.420145 产品完善，资料整理，项目验收项目分工： 建模：曹珂杰、潘维豪、张晓康、郝旭阳 算法：曹珂杰、潘维豪、郝旭阳、张晓康、许瑞臣 嵌入式系

19、统实现：三、学校提供条件（包括项目开展所需的实验实训情况、配套经费、相关扶持政策等）目前学校已建成两个机器人创新实验室，本项目开展主要涉及到的课程包括电路、模电、数电、微机原理及接口技术、电子线路CAD、电力电子技术等、单片机技术、C语言等。因为学生好学，学校大力支持的缘故，故已经具备了这方面的相关知识，并且也经历过前期项目的设计和制作，可以熟练使用通用的电子仪器设备和电装工具，而且学校科技创新实验室提供各种测量（如：稳压电源、电子万用表、信号发生器、示波器等）和制作工具器械（如：线路板雕刻机、烙铁、热熔胶枪等），为本项目研究工作提供了保障。本项目学校将按1:1的比例配套经费，保证及时到位，并制订相应的规章制度予以监督和管理。学校实验室的仪器设备可供本项目开发无偿使用。项目组学生按计划完成验收后，将获得相应的选修学分，所取得的科研成果还可以参加学校组织的大学生课外科技作品竞赛。四、预期成果1.发明专利 一项 ；2.发表论文 两篇 ；3.带有机械臂的四轴飞行器 一个4.基于陀螺仪和加速度计的自平衡系统 六、导师推荐意见方案可行，推荐 签名： 2013年 4 月 25 日七、院系推荐意见院系负责人签名： 学院盖章： 2013年 4月 28 日八、学校推荐意见： 学校负责人签名： 学校公章 2013 年 4 月 30 日注：表格栏高不够可增加。