粒子滤波的智能机器人位置自标定算法.pdf

第8 期2023年8 月机械设计与制造Machinery Design&Manufacture219粒子滤波的智能机器人位置自标定算法于飞,姜燕,郑阶财1（1.山东体育学院体育传媒与信息技术学院，山东济南2 50 0 0 0;2.齐鲁工业大学计算机科学与技术学院，山东济南2 50 0 0 0）摘要：为了提升智能机器人位置自标定的精准度、稳定性，降低用时，设计基于粒子滤波的智能机器人位置自标定算法。根据智能机器人相邻连杆位置关系，描述机器人正运动学，通过粒子初始化、位置更新、权值更新、重组的粒子滤波方法标定机器人的模糊位置。根据三球座设备在机器人外部架构世界坐标系，将末端工具坐标系转换到世界坐标系中，令正运动学的起始点从基座变为三球座设备，根据机器人与设备间装配关系建立世界坐标系与基坐标系的变换矩阵，推导出位置自标定的线性模型，完成智能机器人位置自标定。仿真结果表明，该算法的自标定精准度较高、稳定性较高，标定用时较短，修正效果好，能满足快速标定需求。关键词：智能机器人；正运动学；粒子滤波；位置自标定；坐标系中图分类号：TH16；T P2 4 2.6 文献标识码：A文章编号：10 0 1-39 9 7(2 0 2 3)0 8-0 2 19-0 5Position Self Calibration Algorithm of Intelligent Robot Based on Particle FilterYU Fei,JIANG Yan,ZHENG Jie-cai(1.School of Sport Communication and Information Technology,Shandong Sport University,Shandong Jinan 250000,China;2.School of Computer Science and Technology,Qilu University of Technology,Shandong Jinan 250000,China)Abstract:In order to improve the accuracy,stability and reduce the time of intelligent robot position self calibration,a positionself calibration algorithm of intelligent robot based on particle filter is designed.According to the position relationship between ad-jacent links of intelligent robot,the forward kinematics of robot is described,and the fuzzy position of robot is calibrated by parti-cle filter method of particle initialization,position update,weight update and reorganization.According to the world coordinatesystem of the three ball sochet equipment outside the robot,the coordinate system of the end tool is transformed into the world coor-dinate system,so that the starting point of the forward kinematics is changed from the base to the three ball socket equipment.Ac-cording to the assembly relationship between the robot and the equipment,the transformation matrix between the world coordi-nate system and the base coordinate system is established,and the linear model of position self calibration is derived,completethe position self calibration of intelligent robot.The simulation results show that the algorithm has high self calibration accuracy,high stability,short calibration time and good correction effect,and can meet the needs of rapid calibration.Key Words:Intelligent Robot;Forward Kinematics;Particle Filter;Position Self Calibration;Coordinate System1引言工业制造业飞速发展以及生产需求的不断上升，促进了机器人技术的普及应用。机器人现已成为军事、医学、农业等领域中不可或缺的辅助设备，并逐步朝着智能化、高级化方向发展，技术与功能均有大幅度提升。随着企业数字化、智能化转型战略目标的提出，用机器人代替人工逐渐演变为工业产业革新的必然趋势，因此高精度、高效率作业的智能机器人成为了企业加强自身竞争力的迫切需求。来稿日期：2 0 2 2-0 6-12基金项目：国家级自然科学基金一基于异构信息网络的社会化推荐系统研究（7 2 0 0 4 12 7)作者简介：于飞，（198 0-）,女，山东潍坊人，硕士研究生，讲师，主要研究方向：人工智能，机器学习,运动信息处理与分析；姜燕，（198 0-），女，山东泰安人，硕士研究生，讲师，主要研究方向：软件工程，计算机网络机器人标定技术2 作为工业作业过程中的重要一环，直接影响任务完成精度与效率，故众多相关领域人员对此展开了广泛而深入的探索。比如文献(3 与文献4 采用位姿约束与指数积公式,设计出工业机器人快速标定系统与视觉测量的机器人标定方法这两项研究成果均在一定程度上推动了机器人标定技术的提升。为了进一步提升智能机器人的标定水平,这里以粒子滤波为基本算法，构建出位置自标定算法。220粒子滤波不仅有助于解决真实工作场景中的非线性系统、非高斯噪声问题，能为机器人位置的准确标定提供可靠依据，而利用世界坐标系描述运动学模型，既有助于优化自标定精度，还能加快自标定速度。2基于粒子滤波的智能机器人位置自标定通过粒子初始化、粒子更新、粒子权值更新、粒子重组、模糊位置标定以及构建智能机器人位置自标定实现智能机器人位置自标定算法的判断执行。将智能机器人数据分为测试样本与实验样本，通过不断迭代对该算法进行训练，以此实现该算法的自学习。假设智能机器人的自由度是n，且全部关节均可旋转，而以相邻连杆位置关系为基础的变换矩阵，采用下列表达式描述该机器人的正运动学：T=*II Ti-=*TT,.T1式中：8 一位置补偿系数；T一机器人基坐标系(0)中的末端工具中心点位置；i一任意自由度,i=1,2,n。当机器人末端存在执行器时，将当前位置的工具坐标系定为该执行器的位置，故其位置表达式：T(2)=T0*,*A,式中：T一末端执行器位置；Q一连杆参数；一附加旋转角度；A一基于法兰盘中心坐标系5 的工具坐标系位置变换矩阵。下列矩阵式为位置变换矩阵A：d,Rd.PrA,=L01式中：d一连杆偏移距离；R一以法兰盘中心坐标系为基准的工具坐标系姿态；P一以法兰盘中心坐标系为基准的工具坐标系位置。针对智能机器人在实际应用中的精度需求，使用粒子滤波算法用于测量、标定机器人的模糊位置，为机器人位置的准确自标定提供可靠依据。粒子滤波算法共分为五个流程，具体描述如下：(1)粒子初始化：已知机器人待标定末端的初始位置是（o,yo),移动步长是入,运动方向是。，据此生成一个圆形，令其圆心是初始位置（o，%),半径是移动步长入。在该圆中均匀采样M个粒子后,结合运动方向在(。-,0。+)范围中均匀采样L个角度。其中,为方向更新的角度偏差；(2)粒子更新：根据粒子的上一个位置(-1,y-）移动步长入-1及运动方向6%-1,利用下列坐标方程组，更新下个位置的粒子，令其均匀采样数量为M*L：xm=xm-I+k-1*cos(0k-1)(ym=yl-I+Ak-I*sin(ok-1)于飞等：粒子滤波的智能机器人位置自标定算法式中：h一粒子下一个位置序数；k一1一粒子上一个位置序数；x一下一个位置的粒子水平方向坐标；y一下一个位置的粒子垂直方向坐标；m一粒子排序,m=1,2,M。(3)粒子权值更新：粒子包含测量权值与地图权值。假设第m个粒子的测量权值是，地图权值是，当观测粒子与更新粒子存在重合部分，且该部分所含粒子个数P大于阈值t，时，选取重合部分粒子作为父代粒子群，赋权1/P,以完成下一步更新；当两类粒子不存在重合部分，或者该部分所含粒子个数P小于阈值t，时，选取所有更新粒子作为父代粒子群，赋权1/D，实现下一步更新。两种粒子间欧几里得距离D的计算公式如下所示：(1)D=/(x-xi)+(yn-yi)式中：x一观测粒子的水平方向坐标；y一观测粒子的垂直方向坐标。根据下列条件方程组，确定第m个粒子的地图权值：1,穿墙o=(0,未穿墙(2)该式表明，若第m个粒子已穿墙，则令地图权值取值是0；若该粒子不穿墙，则取值为1。由此得到第m个粒子的总权值计算公式，如下所示：利用下列公式归一化处理总权值，完成粒子权值更新操作：(8)(3)Mm=1(4)粒子重组：按照粒子更新后的权值大小降序排列,选取高权值粒子重组粒子群；(5)模糊位置标定：采用下列方程组加权平均处理各个粒子，获取机器人的对应方向坐标：Mx=2x1*Tm=Mm=1经粒子滤波模糊定位后，机器人的工具坐标与实际坐标间存在一个固定偏差8。故采用世界坐标系6 ,以提升位置自标定的绝对精准度。以目标机器人为中心，利用三球座设备在其外部建立世界坐标系(w)7,将末端工具坐标系转换到世界坐标系(w)下，正运动学的起始点将从基座变为三球座设备，根据机器人与设备间的关系,得到世界坐标系(w)与基坐标系(0)的变换矩阵8 ,如下所示：(4)第8 期(5)(6)(7)(9).1(10)No.8Aug.2023式中：P一世界坐标系(w)的工具坐标系位置；3,一关节运动旋量；q,一关节角；+一关节位姿旋量；t一基坐标系(0)对应的李代数元素；e一映射指数。若机器人末端与设备有接触,则三个球心坐标即是末端工具坐标系在世界坐标系(w)中的实际坐标9-10 。由此得出位置自标定模型的线性表达式，如下所示：*P,=tu,t,t2,ot,*R.,RPl,RP,RoP 式中：t一运动学位姿旋量；t一初始位姿对应的李代数元素；出一转置因子；R一以世界坐标系(w)为基准的基坐标系(0)姿态；R一以基坐标系(0)为基准的工具坐标系姿态;P以世界坐标系(w)为基准的基坐标系(0)位置;P一以基坐标系(0)为基准的工具坐标系位置。3实验验证3.1实验对象概况实验所用机器人的机械机构为多关节型，所有关节均可转动，用于控制末端手腕的位置与姿态。智能机器人的Denavit-Hartenberg参数,如表1所示。表1智能机器人Denavit-Hartenberg参数Tab.1 Denavit-Hartenberg Parameters of Intelligent Robot物理指标长度/m关节偏移/m偏转角度/关节角度/利用机器人工具箱编写相应程序，通过teach命令可控制机器人关节运动，并显示该模型在基坐标系下坐标值，获取机器人自标定结果。实验场景，如图1所示。图1实验场景Fig.1 Experimental Scene机械设计与制造机器人位置参数，如表2 所示。表2 机器人位置参数Tab.2 Robot Position Parameters自标定试验组别X/mm1232.63278.253121.664257.445257.046258.66(11)789103.2位置自标定的精准度检测针对目标机器人手腕末端多个不同的空间位置，利用Simu-link仿真软件与激光跟踪测量设备，分别得到粒子滤波算法的模拟坐标值与测量的实际坐标值，根据机器人基坐标系统一坐标值，利用绝对误差指标，验证算法对不同空间位置的自标定精准度。各方向坐标的指标结果，如图2 所示。0.80.60.41号连杆2号连杆20.50.20.41809010050221Y/mmZ/mm623.85-452.39623.85-454.87697.45-446.17573.26-618.53709.23-614.52276.26-618.53214.35563.74120.31257.44147.23741.36121.44563.853号连杆0.50.49050-647.85-596.37-674.21-647.240.050.040.030.020.0112345678910十种空间位置x坐标值-y坐标值一之坐标值图2 不同空间位置的自标定坐标值示意图Fig.2 Schematic Diagram of Self-Calibration CoordinateValues at Different Spatial Positions由图2 中的结果可以看出：该算法的仿真结果较为趋近于机器人手腕末端三个方向坐标的实际值；各坐标的最大绝对误差值分别是0.8 7 1mm、0.0 4 8 m m 0.92 1m m，由于xz方向的位置坐标范围分别在(2 0 0 30 0)mm、（30 0 4 0 0)mm，数值较大，而y方向的位置坐标范围是（0 10)mm，数值较小,故xz方向远超出y方向的绝对误差值，但根据绝对误差与实际坐标值的比例可知，粒子滤波算法的标定精准度完全可以满足各领域的精度需求。从绝对误差的波动情况可以看出：对于不同空间位置，粒子滤波算法显示出了不同程度的标定能力，尽管波动幅度较大，频率较快，但x、z 方向的绝对误差值始终不超过1mm，y 方向则一直未大于0.0 5mm，标定精准度较为理想。综上所述，该算法利用基于机器人的正运动学模型，改进粒子滤波算法，凭借高权值粒子群，为算法位置自标定精准度提供了可靠的理论基础，可满足不同领域对智能机器人的实际应用精度需求。No.8222机械设计与制造3.3位置自标定的稳定性检测消除了机器人工具坐标系坐标与实际坐标间存在的固定偏差，为进一步验证该算法位置自标定稳定性的优越性，选用基因此，该算法不仅位置标定精准度更高，而且具有明显的稳定于位姿约束与基于指数积公式的两种标定方法及粒子滤波算性优势。法，对目标机器人手腕末端的同一空间位置，各展开多组自标3.4位置自标定的用时检测定试验。结合激光测量设备测得的实际坐标值，利用绝对误差现代化工业时代对智能机器人的工作效率有较高要求，标指标，评估各方法的标定效果。不同方法位置标定的绝对误差定用时是影响机器人工作效率的因素之一。故该实验环节以结果，如图3所示。位置标定速度为探讨方向，针对不同空间位置与同一空间位置两个角度，对比三种方法完成自标定的所用时长，以验证粒子20滤波算法是否具备良好的实时性。各方法在不同实验条件下的16F八8412345678910十组自标定试验-指数积公式标定法一一位姿约束标定法(a)X坐标0.60.48Fwu/0.36F10.24F0.1212345678910十组自标定试验指数积公式标定法一一位姿约束标定法(b)Y坐标181612804012345678910十组自标定试验指数积公式标定法一一位姿约束标定法(c)Z坐标图3同一空间位置的自标定绝对误差示意图Fig.3 Schematic Diagram of Self-CalibrationAbsolute Error at Same Spatial Position从三种方法的绝对误差指标变化情况可以看出：位姿约束标定法因讲求速度，故标定稳定性较差，不仅绝对误差值最高，而且波动幅度最大，频率最快；指数积公式标定法的指标值较低，且具有一定的稳定性；而粒子滤波算法的指标值则始终维持在较低的误差水平上，且波动幅度与频率均较小。综上所述，相较于对比方法，根据粒子滤波算法测量、标定的末端模糊位置，利用三球座设备建立的世界坐标系，有效Aug.2023位置自标定用时，如图4 所示。10080S/6040粒子滤波算法200121指数积公式标定法一一位姿约束标定法一粒子滤波算法(a）不同空间位置5040230E20一粒子滤波算法1012345.678910实验次数指数积公式标定法一一位姿约束标定法/(b)同一空间位置/1/34十组自标定试验图4 不同方法的自标定用时示意图Fig.4 Schematic Diagram of Self-CalibrationTime of Different Methods分析不同空间位置下的自标定用时可知，三种方法的平均标定用时分别是6 2.4 4 s、2 0.34 s、2 2.54 s，指数积公式标定法用时最长且最不稳定，虽然粒子滤波算法的标定速度仅次于以效率为目一这里算法标需求的位姿约束标定法，但却更加平稳。通过同一空间位置的自标定用时发现，标定相同位置时，三种方法都表现出了较为平稳的标定速度；粒子滤波算法与位姿约束标定法的平均用时2 0.6 s、14.35s，将标定速度作为今后工作的主要攻克目标，确保在实现精准标定的前提下，更高效地完成标定任务。3.5修正效果比较利用不同方法对智能机器人某一位置进行自标定后，对机器人位置进行多次修正，如图5所示。为了更为直观比较不同算法的修正效果，结合图5获取位置修正的相关数据，如表3所示。5678910一粒子滤波算法No.8Aug.202332%/基0-1位-2-3-4L0Tab.3PositionCorrectionData绝对值的平绝对值的标距离误差最大值/mm最小值/mm修正前2.967指数积标定法2.139位姿约束标定法1.452粒子滤波算法0.713分析图4 与表3中的数据可知，与指数积标定法、位姿约束标定法相比，将粒子滤波算法应用至智能机器人自标定后，可以有效修正机器人标定误差，实际应用效果更好。综上所述，粒子滤波算法将末端工具坐标系转换至世界坐标系下，令正运动学的起始点从基座变为三球座设备，简化了坐标系处理的复杂度，加快了自标定速度，该算法效率能基本满足大部分工业现场的快速标定需求。4结论机器人技术的诞生与发展，为社会生产与日常生活带来了极为深刻的影响，推动各行各业朝着自动化、智能化方向发展。科技水平不断进步，使各类智能机器人越来越普及，逐渐成为工业、服务业等领域中不可或缺的重要设备。随着机器人应用范围与工作内容不断升级,要求也与日俱增,其中以位置自标定技术的讨论最为热烈。因此，提出智能机器人位置自标定算法，希望能为机器人持续创新与发展前景拓展奠定坚实的基础。由于研究过程中未利用运动学参数的实际值来补偿误差，故今后将从以下几点不断完善算法性能，以实现真正的推广使用，即针对机器人基座与工件关系，探讨工业现场中工件操作阶段对位置标定结果的影响；需就受力、温度等外界因素，分析机器人末端实际位置与标定位置的误差，并尝试采用新型的算法与技术优化精度，使其满足行业的精密性需求。参考文献1杨静宜，崔建弘，庞雅静.基于特征深度学习的机器人协调操作感知控制J.计算机仿真,2 0 2 1,38（1)：30 7-311.(Yang Jing-yi,Cui Jian-hong,Pang Ya-jing.Perceptual control simula-tion of robot coordinated operation based on feature deep learning J.机械设计与制造Computer Simulation,2021,38(1):307-311.)2程麒，潘丰，袁瑜键.基于3D视觉传感器的龙门架机器人手眼标定方法J.光电工程,2 0 2 1,4 8(4)：30-38.(Cheng Qi,Pan Feng,Yuan Yu-jian.Hand-eye calibration method ofgantry robot based on 3D vision sensor J.Opto-Electronic Engineer-ing,2021,48(4):30-38.)一修正前3张新星，王辉，张元祥.基于位姿约束的工业机器人快速标定系统研究指数积公式标定法J.机械设计与制造，2 0 2 1（8)：2 91-2 94.位姿约束标定法一粒子滤波算法510实验次数图5位置修正结果Fig.5Position Correction Results表3位置修正数据均值/mm-3.5856.552-2.8621.0247-0.8530.6617-0.1330.1514223(Zhang Xin-xing,Wang Hui,Zhang Yuan-xiang.Research on the rapid一calibration system of industrial robot based on pose constraint J.Ma-152025准差/mm0.96240.66400.39500.1260chinery Design&Manufacture,2021(8):291-294.)4石怀涛,张执锦，祁若龙，等.一种基于POE公式的最小模型与视觉测量的机器人标定方法J.沈阳建筑大学学报：自然科学版，2 0 2 1,37(2):354-361.(Shi Huai-tao,Zhang Zhi-jin,Qi Ruo-long,et al.A minimal model andvisual measurement method based on poe formula for robot calibrationJ.Journal of Shenyang Jianzhu University Natural Science,2021,37(2):354361.)5李广云，罗豪龙，王力.机器人工具坐标系的快速标定方法J.光学精密工程,2 0 2 1,2 9(6)：137 5-138 6.(Li Guang-yun,Luo Hao-long,Wang Li.Fast calibration method of ro-bot tool coordinate J.Optics and Precision Engineering,2021,29(6):1375-1386.)6吴孟桦，胡晓兵，赵清祥，等.基于双结构光视觉的机器人运动学参数标定方法J1.组合机床与自动化加工技术，2 0 2 0(6)：37-4 0+4 6.(Wu Meng-hua,Hu Xiao-bing,Zhao Qing-xiang,et al.Calibration met-hod of robot kinematics parameters base on structured light and visionJ.Modular Machine Tool&Automatic Manufacturing Technique,2020(6):37-40+46.)7郭冰菁，韩建海，李向攀，等.步态康复机器人动力学李群李代数建模及仿真J.系统仿真学报，2 0 2 0,32(6)：112 6-1135.(Guo Bing-jing,Han Jian-hai,Li Xiang-pan,et al.Dynamics modelingand simulation of gait rehabilitation robot based on lie groups and lie al-gebras theoryJ.Journal of System Simulation,2020,32(6):1126-1135.)8刘东来，崔亚飞，罗辉，等.基于改进型LeNet-5的工业机器人工件自动识别研究J.制造技术与机床,2 0 2 1(8)：10 3-10 7.(Liu Dong-lai,Cui Ya-fei,Luo Hui,et al.Research on automaticrecog-nition of industrial robot workpiece based on improved LeNet-5 J.Manufacturing Technology&Machine Tool,2021(8):103-107.)9陈相君，古力那尔祖农，等.基于激光跟踪仪的协作机器人标定算法与实验研究J.计量学报，2 0 2 1,4 2(5：552-557.(Chen Xiang-jun,Guli Naer Zu Nong,et al.Calibration algorithm andexperimental research of cooperative robot based on laser tracker J.Acta Metrologica Sinica,2021,42(5):552-557.)10于凌涛，于晓砚，汤泽旭，等.手术机器人的运动学标定方法与实验J.哈尔滨工程大学学报，2 0 2 0,4 1(7)：10 8 0-10 8 6.(Yu Ling-tao,Yu Xiao-yan,Tang Ze-xu,et al.Kinematic calibrationmethod and experiments for surgical robots J.Journal of Harbin Engi-neering University,2020,41(7):1080-1086.)