基于IMU的机械臂末端执行器姿态优化_汪坤.pdf

电 子 测 量 技 术E L E C T R ON I CME A S UR EME N TT E CHNO L OG Y第4 6卷 第1期2 0 2 3年1月 D O I:1 0.1 9 6 5 1/j.c n k i.e m t.2 2 1 0 2 6 8基于I MU的机械臂末端执行器姿态优化*汪 坤1,2 张国良1,2 张自杰1,2 王艺成1,2(1.四川轻化工大学自动化与信息工程学院 自贡6 4 3 0 0 0;2.人工智能四川省重点实验室 自贡6 4 3 0 0 0)摘 要:为了补偿机器人关节扭转不足和末端执行器连接等造成的末端执行器姿态误差,提出一种基于惯性测量单元在线获取末端姿态的方法。首先将整个机械臂系统的运动过程分为静态和动态过程。静态时,由于外部加速度噪声较小,提出使用加速度计根据当地重力估计末端执行器姿态角的方法。动态时,针对系统外部加速度噪声和陀螺仪零漂、比例因子误差等影响测量精度的问题,提出一种基于噪声统计方法的自适应扩展卡尔曼滤波算法。根据加速度计的测量值,更新观测噪声方差阵的权重,从而调节卡尔曼增益,降低加速度噪声对测量精度的影响。实验结果表明:静态时,通过静态算法估算的姿态角误差平均值为0.0 7、0.0 5、0.2;动态时,本文算法可以很好补偿外部加速度的对姿态的影响,能有效提高姿态测量精度,相比于E K F算法,姿态角平均误差分别降低了2.6 9、1.0 1、0.5。关键词:机械臂;姿态估计;卡尔曼滤波;自适应中图分类号:T P 2 4 1.2 文献标识码:A 国家标准学科分类代码:5 1 0.1 0 5 0I MU-b a s e da t t i t u d eo p t i m i z a t i o no f t h er o b o t i ca r me n de f f e c t o rW a n gK u n1,2 Z h a n gG u o l i a n g1,2 Z h a n gZ i j i e1,2 W a n gY i c h e n g1,2(1.S c h o o l o fA u t o m a t i o na n dI n f o r m a t i o nE n g i n e e r i n g,S i c h u a nU n i v e r s i t yo fS c i e n c e&E n g i n e e r i n g,Z i g o n g6 4 3 0 0 0,C h i n a;2.A r t i f i c i a l I n t e l l i g e n c eK e yL a b o r a t o r yo fS i c h u a nP r o v i n c e,Z i g o n g6 4 3 0 0 0,C h i n a)A b s t r a c t:I no r d e r t oc o m p e n s a t e f o r t h ee n de f f e c t o r a t t i t u d ee r r o r c a u s e db y i n s u f f i c i e n t t o r s i o no f t h e r o b o t j o i n t a n dt h ee n de f f e c t o r c o n n e c t i o n,am e t h o db a s e do n i n e r t i a lm e a s u r e m e n tu n i t t oo b t a i nt h ee n da t t i t u d eo n l i n e i sp r o p o s e d.F i r s to f a l l,t h em o t i o np r o c e s so f t h ee n t i r e r o b o t i ca r ms y s t e mi sd i v i d e d i n t os t a t i ca n dd y n a m i cp r o c e s s e s.A t s t a t i c,d u e t ot h es m a l l e x t e r n a l a c c e l e r a t i o nn o i s e,am e t h o d f o r e s t i m a t i n g t h e a t t i t u d e a n g l eo f t h e e n de f f e c t o r b a s e do n l o c a lg r a v i t yu s i n ga na c c e l e r o m e t e r i sp r o p o s e d.I nd y n a m i c t i m e,a na d a p t i v ee x t e n d e dK a l m a n f i l t e r i n ga l g o r i t h mb a s e do nn o i s es t a t i s t i c s i sp r o p o s e df o r t h ep r o b l e m so f e x t e r n a l a c c e l e r a t i o nn o i s e,g y r o s c o p ez e r od r i f t,a n ds c a l e f a c t o re r r o rt h a t a f f e c t t h em e a s u r e m e n ta c c u r a c y.B a s e do nt h em e a s u r e m e n t so ft h ea c c e l e r o m e t e r,t h ew e i g h t so ft h eo b s e r v e dn o i s ev a r i a n c ea r r a ya r eu p d a t e dt oa d j u s tt h e K a l m a n g a i na n dr e d u c et h ee f f e c to fa c c e l e r a t i o n n o i s eo nt h em e a s u r e m e n ta c c u r a c y.E x p e r i m e n t a lr e s u l t ss h o w t h a tt h ea v e r a g ea t t i t u d ea n g l ee r r o re s t i m a t e d b yt h es t a t i ca l g o r i t h mi s0.0 7,0.0 5,0.2;I nd y n a m i ct i m e,t h ep r o p o s e da l g o r i t h mc a nc o m p e n s a t ew e l l f o rt h ei n f l u e n c eo fe x t e r n a l a c c e l e r a t i o no na t t i t u d e,a n dc a ne f f e c t i v e l yi m p r o v et h ea t t i t u d em e a s u r e m e n ta c c u r a c y,c o m p a r e dw i t ht h eE K Fa l g o r i t h m,t h ea v e r a g ee r r o ro f a t t i t u d ea n g l e i sr e d u c e db y2.6 9,1.0 1,0.5 .K e y w o r d s:r o b o t i ca r m;a t t i t u d ee s t i m a t i o n;K a l m a nf i l t e r i n g;a d a p t i v e 收稿日期:2 0 2 2-0 6-0 9*基金项目:四川省应用基础研究项目(2 0 1 9 Y J 0 0 4 1 3)资助0 引 言 机械臂运动学模型往往是在理想情况下构建的,与实际运动学模型参数存在误差。运动学参数误差包含几何误差和非几何误差1:几何参数误差主要是机器人的运动学模型和实际运动学模型之间差异导致的;非几何误差的引入是多方面的,包含末端负载、场景温度等原因。机械臂在操作过程中的位姿误差是多种因素的结果,存在难以实时估计出末端执行器准确位姿的问题。描述机器人末端执行器位姿包含位置和姿态两方面。可以通过D-H参数法计算机械臂执行器姿态,但存在结构性误差。当机械臂末端添加重物后引入的非线性误差可以通过系统辨识的方法辨识出误差系数。刘志等2通过使用基于G a u s s过程回归预测和测量平差模型修正位姿误差。27汪 坤 等:基于I MU的机械臂末端执行器姿态优化第1期唐越等3通过MD-H参数误差补偿机械臂末端定位精度。此外,也可以通过添加外部传感器获取机械臂末端姿态,如I MU、视觉传感器等。D i g o等4提出一种基于惯性测量单元(i n e r t i a lm e a s u r e m e n tu n i t,I MU)的仿人机器人上肢典型姿态实时估计方法。李旭妍等5基于扩展卡尔曼滤波(e x t e n d e dK a l m a nf i l t e r,E K F)算法设计出一套用于姿态测量的航姿参考系统。由于陀螺仪存在误差累计的缺点,因此I MU常与不存在误差累计的传感器联用,如G P S、超宽带无线通信技术(u l t r aw i d e b a n d,UWB)、里程计、相机等传感器。张文等6提出一种基于深度相机和I MU组合的自主定位方法。江朝军等7提出了一种基于I MU与UWB结合的弹跳机器人位姿检测方案。I MU的误差主要是系统结构引起的误差、陀螺仪的零漂、温度、震动等。通过对误差补偿,从而提高测量精度。B a l e k等8利用多项式近似进行温度补偿,以获得接近实验室条件的现场测量精度。S a e e d等9通过找出误差系数对温度的依赖关系对温度漂移进行补偿。U z a i r等1 0对传感器误差模型参数进行估算,并用于对加速度计生成的数据进行实时校正。程为彬等1 1根据均衡校正点设计校正矩阵,校正I MU各种系统误差。另外,也可通过优化系统融合算法提高精度。班朝等1 2提出一种基于四元素的自适应扩展卡尔曼滤波算法优化机器人末端姿态。魏韡等1 3提出一种改进自适应互补滤波算法优化三级流水线算法提高姿态解算速度和精度。马星河等1 4提出一种改进自适应鲁棒卡尔曼滤波算法改善随机误差累计的问题。P a r a g等1 5提出了一种采用非线性和线性版本的互补滤波器,分别校正陀螺仪偏置和估计系统姿态角。S i m o n e等1 6提出一种基于四元数法的两级扩展卡尔曼滤波器估计载体的姿态。以上算法在对姿态估计精度上都有一定的提高,但对系统中的有害加速度的处理较少。由于末端执行器的引入、机械臂关节的扭转不准确、环境温度等原因造成末端执行器姿态误差。针对这一问题,本文通过在机械臂末端执行器安装MEM S-I MU在线估计末端执行器姿态。依据噪声统计的思想,设计了一种基于欧拉角的自适应扩展卡尔曼滤波器估计末端执行器姿态。根据机械臂不同的运动加速度,在线调节卡尔曼滤波增益,消除机械臂系统加速度对姿态估计精度的影响,提高测量精度。1 I MU姿态表示与解算 在定位系统中,为了准确表示载体的位姿、运动速度等,在I MU的安装中心建立坐标系b。为了便于描述和解算I MU数据,选取地理坐标系“东北天”为参考坐标系n。规定旋转方式为“X Y Z”,如图1所示。坐标