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

第 2 期 2 0 1 3年 2月 机 械 设 计 与 制 造 Ma c h i ne r y De s i g n Ma nu f a c t u r e 3 9 一 种基于 K a l m a n滤波的双轮机器人姿态控制算法 任国华 唐昌进 蒋刚 西南科技大学 制造科学与工程学院 四川 绵阳6 2 1 0 1 0 摘要 采用 自制的双轮式结构机器人作为试验 平台 以 A D X L 3 4 5三轴加速度计和 I T G 3 2 0 0三轴陀螺仪微重力 M E MS 微机械电子技术的传感器作为机器人的角度传感器 通过建立离散卡尔曼滤波器数学模型对两种传感器的数据进行滤 波融合 使其得到更稳定更准确更接近真实值的角度值 使用S T M3 2 F 1 0 3 V E获取动态的角度信息 通过变参数 自整定平 衡控制算法的调节使机器人保持平衡 实验结果验 j 该方法的可行性 对特殊环境 自动无人巡航机器人和载人巡航机 器人控制有很大的研究价值 关键词 卡尔曼滤波 自平衡 机器人 AD X L 3 4 5 中图分类号 T H1 6 T P 2 4 2 文献标识码 A 文章编号 1 0 0 1 3 9 9 7 2 0 1 3 0 2 0 0 3 9 0 3 A Ki n d O f At t i t u d e Co n t r o I Me t h o d o f TwO W h e e I Ro b o t B a s e d o n K a l ma n F i lt e r i n g Al g o r i t h m R E N G u o h u a T A N G C h a n g j i n J I A N G G a n g S c h o o l o f Ma n u f a c t u r i n g S c i e n c e a n d E n g i n e e r i n g o f S o u t h w e s t U n i v e r s i t y o f S c i e n c e a n d T e c h n o l o g y S i c h u a n M i a n y a n g 6 2 1 0 1 0 C h i n a Ab s t r a c t A s e V m a d e t W O w h e e l e d s t r u c t u r e r o b o t w a s c h o s e n a n e x p e r i m e n t p l a tf o r m U s i n g A D X L 3 4 5 t h r e e a x i s a c c e l e r o me t e r a n d I T G3 2 0 0 axi s g y r o s c o p e mi c r o g r a v i t y MEMS mi c r o me c h a n i c a l e l e c t r o n i c t e c h n o l o g y s e n s o r s n s t h e棚t g e o ft h e r o b o t s e n s o r t h r o u g h t h e e s t a b l i s h m e n t o fd i s c r e t e k al ma n fi l t e r m a t h e m ati c al m o d e I oft w o k i n d s o f s e n s o r d ata fil t e r i n g f u s i o n D 阳 s t a b l e m o r e a c c u r ate a n d C l o s e r t o t h e r e al v al u e o ft h e An v a l u e W C IS g o t T M3 2 F 1 0 3 V E T 6 W a S u s e d t O g e t d y n a mi c i n f o r mat n T h r o u g h t h e v a r i abl e p ara m e t e r s e V s e t t i n g b ala n c e c o n t r o l alg o r i t h m of r e g u l ati o n r o b o t w a s m ade t o k e e p t h e b a l a n c e E x p e r i m e n t al r e s u l t s v e r ifie d t h e f e a s i b i l i t y of t h i s m e t h o d p r o v i d i n g g r e at r e s e a r c h V allt e f o r s p e c i al e n v i r o n me n t a u t o mati c u n man n e d c r u is i n g r o b o t a n d ma n n e d c r u i s i n g r o b o t c o n t r o 1 Ke y W o r d s Ka l ma n Fi l t e r i n g S e l f Ba l a n c i n g Ro b o t ADXL3 4 5 1引言 双轮机器人是一个非线性 多变量 参数不稳定的控制系 统 也是国内外研究者积极挑战的一个研究课题 该实验平台为两 轮平行布置 两个电机驱动的平衡机器人 随着两轮自平衡机器人 的深入研究和它自身简洁灵活的机构 已经在一些领域得到了应用 和推广 例如代步交通工具 巡逻机器人等 无论是在军事 工业还 是民用行业都有着重要的研究价值 目前多数研究者都是基于一种 角度传感器的控制算法的研究 所以存在着动态不稳定以及可控 角度小的问题l l l 根本原因是选用的倾角传感器在使用过程中静 态时较准确 但在动态时受加速度影响较大 有较大的噪声 选用 陀螺仪罔 贝 0 存在累计误差 利用卡尔曼滤波器把力 I1 速度计的静态优 势和陀螺仪的动态优势进行融合 得到一个比较稳定可靠的实时 角度反馈数据 从而提高了机器人的姿态平衡控制能力 2两轮 自平衡小车的总体设计 系统结构具有传统的一级倒立摆的特性 但是受到干扰因 素更多 布局采用两轮平行布置 两个电机驱动 控制平台采用 s T意法半导体的S T M3 2 F 1 0 3 V E 最高主频 7 2 MH z 满足平衡控 制2 0 0 k Hz 的调节频率的同时有更多的 C P U资源 驱动轮的里程 计采用 1 0 0 线的光电编码器 陀螺仪传感器采用 I T G 3 2 0 0 倾角 传感器采用高分辨率的 A D X L 3 4 5 t3 1 然后通过高速通信将信号发 送给主控 S T M3 2 S T M3 2 通过卡尔曼滤波器检测小车的 自身倾 角姿态 通过计算输出P WM信号到控制电机的 H桥驱动模块调 节电机的调节电压使车体保持平衡 在平衡系统控制自身平衡的 基础上 采用普通游戏手柄 通过对手柄解码 获取操作信息 通 过手柄接收模块将信息发送给 S T M3 2 S T M3 2根据操作信息以 及编码器的信息反馈 通过控制算法调节计算控制输出的电机调 节电压 使车体完成启动 停止 转弯及转弯加速等操作 2 1系统硬件设计 控制系统硬件设计框图 如图 1 所示 系统硬件主要以 S T M 3 2为核心主控 C P U来设计 S T M 3 2系列基于专为要求高性 能 低成本 低功耗的嵌入式应用专门设计的A R M C O R T E X M3 来稿 日期 2 0 1 2 0 4 0 7 基金项 目 国家 自然科学基金 中国工程物理研究院联合基金 N s lA F 1 1 1 7 6 0 2 7 作者简介 任国华 1 9 8 5 一 男 浙江长兴县 主要研究方 向 机器人控制技术 蒋刚 1 9 7 8 一 男 四川广安 教授 工学博士 硕士导师 主要研究方向 机器人技术 信号处理 机电一体化 4 0 任国华等 一种基 于K a l ma n滤波的双轮机器人姿态控制算法 第 2期 内核 S T M3 2 F 1 0 3 V E具有 5 1 2 K B fl a s h 和 6 4 K B的 S R A M 选用 S T M3 2 1 0 3 V E I T G 3 2 0 0和 A D X L 3 4 5的组合可以极大的提高控 制电路的集成度 且信号稳定性好 抗干扰能力强 满足各种复杂 环境的使用要求 图 l控制系统硬件设计框图 Fi g 1 Co n t r o l Di a g r a m o f t h e S y s t e m Ha r d wa r e De s i g n 3 3 v 加 速 度 计r E Ad x 1 3 4 5 寺 1 GND I NT 2 G R N E S D T s D N v G s N R s E D S o T 陀螺仪 CL Kl N VDD NC NC NC NC NC NC NC NC NC GND NC RE S V V DGl C C P0U T ADO RE S V REGOU T RES V REV G S CL I NT S DA A GND I I C SC K 控制器 图 2 S T M3 2 1 0 3 V E I T G 3 2 0 0和 A D X L 3 4 5的连接电路 F i g 2 S TM3 2 1 0 3 VE I T G3 2 0 0 a n d ADXL 3 4 5 C o n n e c t i o n Ci r c u i t 在图2中控制器 S T M3 2 加速度计和陀螺仪通过总线连接 到一起 I I C S C K为时钟线 I I C S D A为数据线 同时两个信号线 都加上 1 0 K上拉电阻以保证有确定的高低电平 S T M3 2通过选 择不同的读取地址 通过总线向两个传感器发送数据 传感器根 据发来的数据以及寄存器地址进行判断 如果是 自己的地址则向 总线发送相应寄存器的最新值 S T M3 2同时存贮相应的传感器 的测量数据 通过读取一次加速度计的数据再读取一次陀螺仪的 数据 完成一个运算周期里传感器原始数据的采集工作 2 2系统结构设计 图 3 自制平衡车模型及实物结构 F i g 3 Ho me ma d e B a l a n c e Ca r Mo d e l a n d P h y s i c a l S t r uc t u r e 系统结构采用的仍是国内外研究者采用的典型布局方式 两轮平行布局 两个电机驱动 由于考虑到载人的结构强度需 要 采用了带传动的结构方式 通过 P R O E软件建模 布局 仿 真 确保机构硬件平台的合理性以及稳定性 自制平衡车模型及 实物结构 如图 3所示 3算法及其实现 3 1卡尔曼滤波器设计 在众多的滤波算法中 卡尔曼滤波器具有使离散数据滤波 递归的特性 它是一种通过高效可计算的数学方法来对离散系 统的过去状态进行估计 估计均方差最小 并对估计信号的当前 以及下一刻的状态进行预测 的数学方法 可以很好的把陀螺仪 和加速度计进行综合滤波 采用的是离散卡尔曼滤波器 卡尔曼 滤波器包括离散卡尔曼滤波器和扩展卡尔曼滤波器 3 1 1滤波器的计算原型 定义 一 表示先验 表示估计 为在 已知第 步 以前 状态情况下第k步的先验状态估计 定义 为已知测量变量 Z j 时第 k 步的后验状态估计 由此定义先验估计误差和后验估 计误差 X 1 e X 2 先验估计误差的协方差为 e 3 后验估计误差的协方差为 E e e J 4 戈 一 5 式 5 中表示了先验估计 和加权的测量变量 及其预测 之差的线性组合构成了后验状态估计 测量变量及其预 测之差 一 被称为测量过程的革新或残余 残余反映了预测 值和实际值之间的不一致程度 残余为零表明二者完全吻合 是 n X m 阶矩阵 叫做残余的增益或混合因数 作用是使 4 式中 的后验估计误差协方差最小 可以通过以下步骤计算 首先将 5 式代人 e 的定义式 再将 e 代人 4 式中 求得期望后 将 4 式中的 对 求导 并使一阶导数为零从而解得K 值 的一 种表示形式为 一 一l n e H 6 由 6 式可知 观测噪声协方差 越小 残余的增益越大 越大 增益K 的另一种解释是随着测量噪声协方差R趋于0 测 量变量 的权重越来越大 而 的预测 的权重越来越小 另一方面 随着先验估计误差协方差 趋于0 测量变量 的权 重越来越小 而 的预测 肼 的权重越来越大 3 1 2卡尔曼滤波器程序实现 v o i d Kalma n F i l t e r 2 f l o a t a n g l e m fl o a t g y r o m f A n g l e g y r o m q b i a s d t 先验估计 P d o t 0 Q a n g l e P o 一P 1 O P k 一 先验估 计误差协方差的微 No 2 F e b 2 0 1 3 机械 设 计 与制 造 4 1 分 P d o t 1 一P 1 1 1 1 P d o t 2 一P 1 1 P d o t 3 Q g y r o H o D P d o t O d t P k 一 先验估计误 差协方 差微分的积分 先 验估计误差协方差 H0 1 P d o t 1 d t P 1 0 P d o t 2 d t P 1 1 P d o t 3 d t a n g l e e r r a n g l e m A u g l e z k 验估计 PC t O C O P o o J PCt 一1 C O P 1 I o E R a n g l e C O PCt O K O P C t 0 E J K k K 一 1 PC t 一1 E t O PCt 0 t l C O P O 1 Ho D卜 K O t O 后验估计误差协方差 Ho D卜 K 0 t l P 1 1 o l 一 K 一 1 Lo P l l D 一 K 一 1 t 一 1 An g l e K 0 a n g l e e r r d 验估计 q b i a s K I a n g l e e l x l n 验估计 An g l e d o t g y r o m q i a s 出值 后验估计 的微 分 角速度 将加速度计作为观测变量 陀螺仪作为先验估计变量 进行 了有机的结合 把卡尔曼滤波器应用到了双轮机器人的姿态控制 数据处理中 3 2软件设计 软件程序设计 如图4所示 图 4软 件执行设计流程 图 F i g 4 S o f t wa r e De s i g n F l o w C h a r t o f t h e Ex e c u t i o n 上电后进行 S T M3 2外设 I z C等硬件的初始化 然后启卡尔 曼滤波器并初始化加速度计和陀螺仪 对加速度计和陀螺仪的数 据进行采集 输入到卡尔曼滤波器进行滤波 然后将滤波器的输 出值输入到变参数 P I D控制器 进行非线性特性线性化处理 并 进行平衡控制调节 同时将 S T M3 2 采集的编码器信号也输入到 控制器 控制器通过比例 积分 微分环节的调节后输出控制信 号 对双轮机器人的姿态与运动进行动态调节 输出控制的状态 再度反馈到到加速度计和陀螺仪 从而实现闭环 自平衡控制 4实验仿真与数据分析 使双轮机器人从静止开始做往复晃动运动 并将加速度计 陀 螺仪的数据以及卡尔曼滤波后的角度通过 S T M3 2的串口输出 所得数据通过 M a t l a b 7 0 进行实验仿真 结果如图5 图6所示 只用加速度计测量获得的角度 图 5只用加速度计测量的角度 F i g 5 On l y Ac c e l e r o me t e r Me a s u r e me s t Po i n t o f Vi e w 加速度和陀螺仪数据进行卡尔曼滤波前后对 比图 图 6陀螺仪 和加速度计滤波前后结果对 比图 Fi g 6 G y r o s c o p e a n d Ac c e l e r o me t e r F i l t e r i n g Re s u l t s b e f o r e a n d a f t e r t h e Co mp a r i s o n Ch a r t 实验结果表明在滤波前静态状态下加速度计的数据比较准 确 而在动态的过程中加速度计因为受到非重力作用的影响出现 了较大的噪声干扰 而陀螺仪的角速度在动态过程中则相对要准 确 但是由于累计误差 如果用陀螺仪进行倾角测量 则随着时间 的推移 数据失真度愈大 通过卡尔曼滤波后得到的数据结合了 陀螺仪动态的优势和加速度计静态时的优势 输出的数据稳定 准确 滤波方法具有较强的实用性 由于卡尔曼滤波器的良好滤波效果和变参数 P I D控制算法 的集合 使得平衡小车无论是动态调节还是静态调节都取得了稳 定的调节效果 5结束语 针对一种特殊的轮式非完整性约束的两轮自平衡机器人 重点研究离散卡尔曼算法在其姿态控制过程中的应用 通过设 计 制作 实验以及数据分析验证了卡尔曼滤波算法在自平衡小 车控制中应用的可行性 结合 P I D控制算法可以实现双轮机器人 更稳定的动态调节 由于时间问题 该双轮机器人的组合参数只 能通过人为整定 所以不同的硬件系统在寻找最优参数时耗时较 多 下一步计划结合智能控制算法进行组合参数的自整定研究 以提高白平衡双轮机器人面对不同硬件系统的适应性 希望研究 工作能为更多的同类研究提供参考以推动自平衡双轮机器人的 应用发展 下转第 4 4页 机 械 设 计 与 制 造 No 2 Fe b 2 01 3 表 3优化后的叶片参数 Ta b 3 P a r a me t e r s o f Bl a d e Aft er Op t i miz a t i o n 褂 风 速 m s 图 3遗传算法优化前后 的输f 功率 Fi g 3 Ou t p u t Po we r Be f o r e a n d Af t e r Op t i mi z a t i o n o f Ge n e t i c Al g o r i t h m 从图 3 可知 采用遗传算法设计出的叶片在扭角值基本吻合 的前提下 比原有叶片在不同的风速下 功率都有所增加 5结论 对风力机叶片的气动性能进行了计算 并对气动性能进行 了优化设计 仿真结果表明 采用改进的遗传算法在对风力机叶 片在扭角值基本吻合的前提下 比原有叶片在不同的风速下 功 率增加了 2 左右 参考文献 1 肖 俊 席德科 赵旭 6 0 0 k W变桨距风力机叶轮优化设计及数值计算 机械设计与制造 2 0 0 7 1 0 4 6 X i a o J u n X i D e k e Z h a o X u O p t i m i z e d e s i g n o f t h e 6 0 0 k W w i n d t u r b i n e b l a d e s a n d n u me r i c a l s i n ml a t i o n J j Ma c h i n e D e s i g n Ma n u f a c t u r e 2 0 0 7 1 0 4 6 上接 第 4 1页 2 1 刘雄 陈严 叶枝全 水平轴风力机气动性能计算模型 J 太阳能学报 2 0 0 5 2 6 6 7 9 2 8 0 0 L i u Xi o n g Ch e n Ya h Ye Zh i q u a rt Ap p l i c a t i o n o f g e n e t i c a l g o r i t h ms t o h a w r o t o r b l a d e s o p t i miz a t i o n J J o u r n a l o f s o l a r e n e r g y 2 0 0 5 2 6 6 7 9 2 8 0 0 3 徐燕吃 席德科 田 彬 等 力机没计与数值模拟分析 J 1 机械设计与 制造 2 0 0 6 7 l 8 2 0 X u Y a n c h i X i D e k e T i a n B i n e t a 1 Wi n d t u r b i n e b l a d e d e s i g n a n d n u me ri c a l c o m p u t e J Ma c h i n e r y D e s i g n Ma n u f a c t u r e 2 0 0 6 7 1 8 2 0 4 包耳 邵晓荣 刘德庸 风力机叶片设计的新方法 l J1 机械设计 2 0 0 5 2 2 2 2 4 2 6 B a n E r S h a o Xi a o t o n g L i u De y o n g Ne w me t h o d O l t t h e v a n e d e s i g n o f w i n dt u r b i n e J J J o u r n a l o t m a c h i n e d e s i g n 2 0 0 5 2 2 2 2 4 2 6 5 张仲住 王会社 赵晓璐 水平轴风力机叶片气动性 能研究 J I J 程热 物理学报 2 0 0 7 2 8 5 7 8 1 7 8 3 Zh a n g Z h o n g Zh u Wa n g Hu i s h e Zh a o Xi a o L u Re s e a r c h o n t he a e r o d y n a n fi e p e r f o r m a a c e f b l a d e s f o r h o r i z o n t a l a x i s w i m l t u r b i n e J J o u r n a l o f E n g i n e e r i n g T h e m10 一p h y s i c s 2 0 0 7 2 8 5 7 8 1 7 8 3 6 赵峰 段巍 基于叶素一 动量理论 及有限元方法的风力机 叶片载荷分析 和强度计算 J 机械设 计与制造 2 0 1 0 8 4 2 4 4 Zh a o F e n g Du a n We t Lo a d i n g a n a l y s i s a n d s t r e n g t h c a l c u l a t i o n o f wi n d t u r b i n e b l a d e b a s e d O U l a de e l e me n t mo me fl t mn t h e o r y a n d f i n i t e e l e m e n t m e t h o d J Ma c h i n e r y D e s i g n Ma n u f a c t u r e 2 0 1 O 8 4 2 4 4 7 齐肖璐 段巍 水平轴风力机叶片气动性能计算及影 响 素分析 J J 机 械设计与制造 2 0 1 1 1 0 4 4 6 Q i Xi a o l u D u a n We t A e r o d y n a mi c p e r f o r ma n c e c Mc u l a t i o n o f h o r i z o n t a l a x i s w i n d t u r b i n e b l a d e a n d a n a l y s i s o t i n fl u e n c e f a c t o r s J j Ma c h i n e r y D e s i g n Ma n u f a c t u r e 2 0 1 1 1 0 4 4 4 6 f 8 韩中合 吴铁军 基于遗传算法的风力 L n f 片优化设汁 J l 动力 程 2 0 0 8 1 2 9 5 5 9 58 H a n Z h o n g h e Wu T i e i n n O p t i ma l d e s i g n o f w i n d t u r b i n e b l a d e s b a s e d O lq g e n e t i c a l g o r i t h m J J o u r n a l o f P o w e r E n g i n e e r i n g 2 0 0 8 1 2 9 5 5 9 5 8 参考文献 l 1 j T s a i Hu a n g a n d L i n A d a p t i v e N e u r a l Ne t w o r k C o n t r o l o f a S e l f B a l a n c i n g rr w o Wh e e l e d S c o o t e r c I E E E T r a n s a c t i o n s o n I n d u s t r i a l El e c t r o n i c s Vo 1 5 7 I s s u e4 Apr i l 2 01 0 2 j问子波 魏鸣 微机械陀螺仪 的工作原理及其应用 JI 电子设计技 术 2 0 0 9 9 6 2 6 4 Ya n Zi b o We t Mi n g Mi c r o me c h a n i c a l g y r o s c o pe wo r k p rin c i p l e a n d a p p l i c a t i o n J j T h e E l e c t r o n i c D e s i g n T e c h n o l o g y 2 0 0 9 9 6 2 6 4 3 付勇智 陈伟 P I D参数整定的一种混合优化算法 J 机械设计与制 造 2 o o 7 1 1 8 F uYo n g z h i Ch e nWe t Ah y b rid o p t i mi z a t i o n a l g o rit h m o f PI Dp a r a m e t e r s s e t t i n g J j Me c h a n i c a l D e s i g n A n dMa n u f a e t u r e 2 0 0 7 1 l 8 4 李兴法 尹冠飞 数字式加速度传感器 A D X L 3 4 5 的原理及应用 J 黑 龙江科技信息 2 0 1 0 1 2 L i X i n g f a Y i n G u a n f e i D i g i t a l a c c e l e r a t i o n s e n s o r A D XL 3 4 5 p ri n c i p l e a n d a p p l i c a t i o n H e i l o n g j i a n g S c i e n c e a n d T e c h n o l o g y I n f o r ma t i o n 2 0 1 0 5 李慧琴 韩建海 赵书尚 等 一种 自平衡机器人的制作和研究 J 1 机 床与液压 2 0 0 6 1 2 9 5 9 8 L i H u i q i n Ha