一种基于Kalman滤波的双轮机器人姿态控制算法

本资料包含pdf文件1个，下载需要1积分

双轮机器人是-个非线性、多变量、参数不稳定的控制系统，也是国内外研究者积极挑战的-个研究课题，该实验平台为两轮平行布置，两个电机驱动的平衡机器人。随着两轮自平衡机器人的深入研究和它自身简洁灵活的机构，已经在-些领域得到了应用和推广，例如代步交通工具、巡逻机器人等，无论是在军事、工业还是民用行业都有着重要的研究价值。目前多数研究者都是基于-种角度传感器的控制算法的研究，所以存在着动态不稳定以及可控角度小的问题ll。根本原因是选用的倾角传感器在使用过程中静态时较准确，但在动态时受加速度影响较大，有较大的噪声。选用陀螺仪罔贝0存在累计误差。利用卡尔曼滤波器把力I1速度计的静态优势和陀螺仪的动态优势进行融合，得到-个比较稳定可靠的实时角度反馈数据，从而提高了机器人的姿态平衡控制能力。

2两轮自平衡小车的总体设计系统结构具有传统的-级倒立摆的特性，但是受到干扰因素更多，布局采用两轮平行布置，两个电机驱动∝制平台采用sT意法半导体的STM32F103VE，最高主频 72MHz，满足平衡控制200kHz的调节频率的同时有更多的CPU资源。驱动轮的里程计采用 100线的光电编码器 ；陀螺仪传感器采用 ITG3200；倾角传感器采用高分辨率的 ADXL345t31。然后通过高速通信将信号发送给主控 STM32，STM32通过卡尔曼滤波器检测小车的 自身倾角姿态，通过计算输出PWM信号到控制电机的H桥驱动拈调节电机的调节电压使车体保持平衡。在平衡系统控制自身平衡的基础上，采用普通游戏手柄，通过对手柄解码，获取操作信息，通过手柄接收拈将信息发送给 STM32，STM32根据操作信息以及编码器的信息反馈，通过控制算法调节计算控制输出的电机调节电压。使车体完成启动、停止、转弯及转弯加速等操作。

2.1系统硬件设计控制系统硬件设计框图，如图 1所示。系统硬件主要以STM32为核心主控CPU来设计，STM32系列基于专为要求高性能、低成本 、低功耗的嵌入式应用专门设计的ARM CORTEX-M3来稿日期：2012-04-07基金项目：国家自然科学基金中国工程物理研究院联合基金(NslAF：11176027)作者简介：任国华，(1985-)，男，浙江长兴县，主要研究方向：机器人控制技术；蒋 刚，(1978-)，男，四川广安，教授，工学博士，硕士导师，主要研究方向：机器人技术、信号处理，机电-体化40 任国华等：-种基于Kalman滤波的双轮机器人姿态控制算法 第2期内核。STM32F103VE具有 512KB flash和 64KB的 SRAM。选用STM32103VE、ITG3200和 ADXL345的组合可以极大的提高控制电路的集成度，且信号稳定性好，抗干扰能力强，满足各种复杂环境的使用要求。

图 l控制系统硬件设计框图Fig.1 Control Diagram of the System Hardware Design3 3v加速度计 r：E]Adx1345 寺-1GND INT2GRNESDTsDNvGsN RsEDSoT陀螺仪CLKlN VDDNC NCNC NCNC NCNC NCNC GNDNC RESVV DGlC CP0UTADO RESVREGOUT RESVREV-G SCLINT SDAAGNDIIC SCK控制器图 2 STM32103VE 、ITG3200和 ADXL345的连接电路Fig.2 STM32103VE，ITG3200 and ADXL345 Connection Circuit在图2中控制器 STM32、加速度计和陀螺仪通过总线连接到-起，IIC-SCK为时钟线，IIC-SDA为数据线，同时两个信号线都加上 10K上拉电阻以保证有确定的高低电平，STM32通过选择不同的读取地址，通过总线向两个传感器发送数据，传感器根据发来的数据以及寄存器地址进行判断，如果是 自己的地址则向总线发送相应寄存器的最新值，STM32同时存贮相应的传感器的测量数据。通过读取-次加速度计的数据再读取-次陀螺仪的数据，完成-个运算周期里传感器原始数据的采集工作。

2-2系统结构设计图3自制平衡车模型及实物结构Fig.3 Homemade Balance Car Model and Physical Structure系统结构采用的仍是国内外研究者采用的典型布局方式，两轮平行布局，两个电机驱动 。由于考虑到载人的结构强度需要，采用了带传动的结构方式，通过PRO/E软件建模、布局、仿真，确保机构硬件平台的合理性以及稳定性。自制平衡车模型及实物结构，如图3所示。

3算法及其实现3.1卡尔曼滤波器设计在众多的滤波算法中，卡尔曼滤波器具有使离散数据滤波递归的特性 ，它是-种通过高效可计算的数学方法来对离散系统的过去状态进行估计，估计均方差最小，并对估计信号的当前以及下-刻的状态进行预测 的数学方法，可以很好的把陀螺仪和加速度计进行综合滤波，采用的是离散卡尔曼滤波器〃尔曼滤波器包括离散卡尔曼滤波器和扩展卡尔曼滤波器。

3.1.1滤波器的计算原型定义 ，(-”表示先验， 表示估计)为在已知第 步以前状态情况下第k步的先验状态估计。定义 ，为已知测量变量Z(j时第 k步的后验状态估计。由此定义先验估计误差和后验估计误差： -X (1)e -X (2)先验估计误差的协方差为： [e ] (3)后验估计误差的协方差为：Ee e：J (4)戈 ( - ) (5)式(5)中表示了先验估计 和加权的测量变量 及其预测. 之差的线性组合构成了后验状态估计 。测量变量及其预测之差 - 被称为测量过程的革新或残余。残余反映了预测值和实际值之间的不-致程度。残余为零表明二者完全吻合。

是(nXm)阶矩阵，叫做残余的增益或混合因数，作用是使(4)式中的后验估计误差协方差最猩以通过以下步骤计算 ：首先将(5)式代人e 的定义式，再将e 代人(4)式中，求得期望后，将(4)式中的 对 求导。并使-阶导数为零从而解得K 值。 的-种表示形式为：， - - l∥(ne；H ) (6)由(6)式可知，观测噪声协方差 越小，残余的增益越大越大。增益K 的另-种解释是随着测量噪声协方差R趋于0，测量变量 的权重越来越大，而 的预测 的权重越来越校另-方面，随着先验估计误差协方差 趋于0，测量变量 的权重越来越小，而 的预测 肼 的权重越来越大。

3.1.2卡尔曼滤波器程序实现void Kalman-Filter2(float anglem，float gyrom)f Angle(gyrom-qbias) dt.//先验估计 。

Pdot[0]Qangle-Po] -P[1][O]'，//Pk- 先验估，/计误差协方差的微No.2Feb.2013 机械设计与制造 41分Pdot[1]-P11[11；Pdot[2]-P1Ⅱ1；Pdot[3]Q.gyro；HoD]Pdot[O] dt；/Pk-先验估计误。差协方，/差微分的积分先验估计误差协方差H0][1Pdot[1 dt；P[1Ⅱ0]Pdot[2] dt；P[1Ⅱ1]：Pdot[3 dt；angleerr：anglem-Augle'，/zk-验估计PCt OC O Po]oJ；PCt- 1C O P[1]Io；ERangleCO PCt-O；K OPCt 0/EJ/KkK-1PCt-1/E；tOPCt0；tlC O P[OⅡ1；HoDKO tO；/后验估计误差协方差HoDK0 tl；P11ol-K-1 Lo：PllD]-K-1 t-1；AngleK0 angle err'd/)验估计qbiasKI angle elx'，/ln验估计Angledotgyro m-q ias； 出值(后验估计)的微，/分 角速度)将加速度计作为观测变量，陀螺仪作为先验估计变量，进行了有机的结合 ，把卡尔曼滤波器应用到了双轮机器人的姿态控制数据处理中。

图4软件执行设计流程图Fig.4 Software Design Flow Chart of the Execution上电后进行 STM32外设 IzC等硬件的初始化，然后启卡尔曼滤波器并初始化加速度计和陀螺仪，对加速度计和陀螺仪的数据进行采集，输入到卡尔曼滤波器进行滤波。然后将滤波器的输出值输入到变参数 PID控制器 进行非线性特性线性化处理，并进行平衡控制调节。同时将 STM32采集的编码器信号也输入到控制器，控制器通过比例 、积分 、微分环节的调节后输出控制信号，对双轮机器人的姿态与运动进行动态调节。输出控制的状态再度反馈到到加速度计和陀螺仪，从而实现闭环 自平衡控制 。

4实验仿真与数据分析使双轮机器人从静止开始做往复晃动运动，并将加速度计、陀螺仪的数据以及卡尔曼滤波后的角度通过STM32的串口输出，所得数据通过 Matlab7.0进行实验仿真，结果如图5、图6所示。

只用加速度计测量获得的角度图5只用加速度计测量的角度Fig5 Only Accelerometer Measuremest Point of View加速度和陀螺仪数据进行卡尔曼滤波前后对比图图6陀螺仪和加速度计滤波前后结果对比图Fig.6 Gyroscope and Accelerometer Filtering Resultsbefore and after the Comparison Chart实验结果表明在滤波前静态状态下加速度计的数据比较准确，而在动态的过程中加速度计因为受到非重力作用的影响出现了较大的噪声干扰，而陀螺仪的角速度在动态过程中则相对要准确，但是由于累计误差，如果用陀螺仪进行倾角测量，则随着时间的推移，数据失真度愈大。通过卡尔曼滤波后得到的数据结合了陀螺仪动态的优势和加速度计静态时的优势，输出的数据稳定、准确。滤波方法具有较强的实用性。

由于卡尔曼滤波器的良好滤波效果和变参数 PID控制算法的集合，使得平衡小车无论是动态调节还是静态调节都取得了稳定的调节效果。

5结束语针对-种特殊的轮式非完整性约束的两轮自平衡机器人，重点研究离散卡尔曼算法在其姿态控制过程中的应用。通过设计、制作 、实验以及数据分析验证了卡尔曼滤波算法在自平衡小车控制中应用的可行性，结合 PID控制算法可以实现双轮机器人更稳定的动态调节。由于时间问题，该双轮机器人的组合参数只能通过人为整定 ，所以不同的硬件系统在寻找最优参数时耗时较多，下-步计划结合智能控制算法进行组合参数的自整定研究，以提高白平衡双轮机器人面对不同硬件系统的适应性。希望研究工作能为更多的同类研究提供参考以推动自平衡双轮机器人的应用发展。 (下转第 44页)机械 设 计 与制造No.2Feb.201 3表3优化后的叶片参数Tab.3 Parameters of Blade After Optimization褂风速m/s)图3遗传算法优化前后的输fⅡ功率Fig.3 Output Power Before and After Optimizationof Genetic Algorithm从图 3可知，采用遗传算法设计出的叶片在扭角值基本吻合的前提下，比原有叶片在不同的风速下，功率都有所增加。

5结论对风力机叶片的气动性能进行了计算，并对气动性能进行了优化设计。仿真结果表明，采用改进的遗传算法在对风力机叶片在扭角值基本吻合的前提下，比原有叶片在不同的风速下，功率增加了 2%左右。