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

于 飞，姜 燕，郑阶财

（1.山东体育学院体育传媒与信息技术学院，山东济南 250000；2.齐鲁工业大学计算机科学与技术学院，山东济南 250000）

1 引言

工业制造业飞速发展以及生产需求的不断上升，促进了机器人技术的普及应用。机器人现已成为军事、医学、农业等领域中不可或缺的辅助设备，并逐步朝着智能化、高级化方向发展，技术与功能均有大幅度提升［1］。随着企业数字化、智能化转型战略目标的提出，用机器人代替人工逐渐演变为工业产业革新的必然趋势，因此高精度、高效率作业的智能机器人成为了企业加强自身竞争力的迫切需求。

机器人标定技术［2］作为工业作业过程中的重要一环，直接影响任务完成精度与效率，故众多相关领域人员对此展开了广泛而深入的探索。

比如文献［3］与文献［4］采用位姿约束与指数积公式，设计出工业机器人快速标定系统与视觉测量的机器人标定方法，这两项研究成果均在一定程度上推动了机器人标定技术的提升。为了进一步提升智能机器人的标定水平，这里以粒子滤波为基本算法，构建出位置自标定算法。

粒子滤波不仅有助于解决真实工作场景中的非线性系统、非高斯噪声问题，能为机器人位置的准确标定提供可靠依据，而利用世界坐标系描述运动学模型，既有助于优化自标定精度，还能加快自标定速度。

2 基于粒子滤波的智能机器人位置自标定

通过粒子初始化、粒子更新、粒子权值更新、粒子重组、模糊位置标定以及构建智能机器人位置自标定实现智能机器人位置自标定算法的判断执行。将智能机器人数据分为测试样本与实验样本，通过不断迭代对该算法进行训练，以此实现该算法的自学习。

假设智能机器人的自由度是n，且全部关节均可旋转，而以相邻连杆位置关系为基础的变换矩阵，采用下列表达式描述该机器人的正运动学：

式中：ε—位置补偿系数；

—机器人基坐标系中的末端工具中心点位置；

i—任意自由度，i= 1，2，…，n。

当机器人末端存在执行器时，将当前位置的工具坐标系定为该执行器的位置，故其位置表达式：

式中：T0φ—末端执行器位置；

Ω—连杆参数；

βi—附加旋转角度；

AI—基于法兰盘中心坐标系［5］的工具坐标系位置变换矩阵。

下列矩阵式为位置变换矩阵AI：

式中：di—连杆偏移距离；

—以法兰盘中心坐标系为基准的工具坐标系姿态；

—以法兰盘中心坐标系为基准的工具坐标系位置。

针对智能机器人在实际应用中的精度需求，使用粒子滤波算法用于测量、标定机器人的模糊位置，为机器人位置的准确自标定提供可靠依据。

粒子滤波算法共分为五个流程，具体描述如下：

式中：k—粒子下一个位置序数；

k−1—粒子上一个位置序数；

—下一个位置的粒子水平方向坐标；

—下一个位置的粒子垂直方向坐标；

m—粒子排序，m= 1，2，…，M。

（3）粒子权值更新：粒子包含测量权值与地图权值。假设第m个粒子的测量权值是，地图权值是，当观测粒子与更新粒子存在重合部分，且该部分所含粒子个数P大于阈值th时，选取重合部分粒子作为父代粒子群，赋权1P，以完成下一步更新；当两类粒子不存在重合部分，或者该部分所含粒子个数P小于阈值th时，选取所有更新粒子作为父代粒子群，赋权1/D，实现下一步更新。

两种粒子间欧几里得距离D的计算公式如下所示：

式中：x′k—观测粒子的水平方向坐标；

y′k—观测粒子的垂直方向坐标。

根据下列条件方程组，确定第m个粒子的地图权值：

该式表明，若第m个粒子已穿墙，则令地图权值取值是0；若该粒子不穿墙，则取值为1。

由此得到第m个粒子的总权值计算公式，如下所示：

利用下列公式归一化处理总权值，完成粒子权值更新操作：

（4）粒子重组：按照粒子更新后的权值大小降序排列，选取高权值粒子重组粒子群；

（5）模糊位置标定：采用下列方程组加权平均处理各个粒子，获取机器人的对应方向坐标：

经粒子滤波模糊定位后，机器人的工具坐标与实际坐标间存在一个固定偏差δ。故采用世界坐标系［6］，以提升位置自标定的绝对精准度。

以目标机器人为中心，利用三球座设备在其外部建立世界坐标系，将末端工具坐标系转换到世界坐标系{w} 下，正运动学的起始点将从基座变为三球座设备，根据机器人与设备间的关系，得到世界坐标系与基坐标系的变换矩阵［8］，如下所示：

—关节运动旋量；

qn—关节角；

—关节位姿旋量；

—基坐标系{ 0 }对应的李代数元素；

e—映射指数。

若机器人末端与设备有接触，则三个球心坐标即是末端工具坐标系在世界坐标系{w} 中的实际坐标［9−10］。由此得出位置自标定模型的线性表达式，如下所示：

式中：tn—运动学位姿旋量；

tw—初始位姿对应的李代数元素；

ψ—转置因子；

—以世界坐标系{w} 为基准的基坐标系{ 0 }姿态；—以基坐标系{ 0} 为基准的工具坐标系姿态；

—以世界坐标系{w} 为基准的基坐标系{ 0 }位置；—以基坐标系{ 0} 为基准的工具坐标系位置。

3 实验验证

3.1 实验对象概况

实验所用机器人的机械机构为多关节型，所有关节均可转动，用于控制末端手腕的位置与姿态。智能机器人的Denavit−Hartenberg参数，如表1所示。

表1 智能机器人Denavit-Hartenberg参数Tab.1 Denavit-Hartenberg Parameters of Intelligent Robot

利用机器人工具箱编写相应程序，通过teach命令可控制机器人关节运动，并显示该模型在基坐标系下坐标值，获取机器人自标定结果。实验场景，如图1所示。机器人位置参数，如表2所示。

图1 实验场景Fig.1 Experimental Scene

表2 机器人位置参数Tab.2 Robot Position Parameters

3.2 位置自标定的精准度检测

针对目标机器人手腕末端多个不同的空间位置，利用Simu⁃link仿真软件与激光跟踪测量设备，分别得到粒子滤波算法的模拟坐标值与测量的实际坐标值，根据机器人基坐标系统一坐标值，利用绝对误差指标，验证算法对不同空间位置的自标定精准度。各方向坐标的指标结果，如图2所示。

图2 不同空间位置的自标定坐标值示意图Fig.2 Schematic Diagram of Self−Calibration Coordinate Values at Different Spatial Positions

由图2中的结果可以看出：该算法的仿真结果较为趋近于机器人手腕末端三个方向坐标的实际值；各坐标的最大绝对误差值分别是0.871mm、0.048mm、0.921mm，由于x、z方向的位置坐标范围分别在（200~300）mm、（300~400）mm，数值较大，而y方向的位置坐标范围是（0~10）mm，数值较小，故x、z方向远超出y方向的绝对误差值，但根据绝对误差与实际坐标值的比例可知，粒子滤波算法的标定精准度完全可以满足各领域的精度需求。

从绝对误差的波动情况可以看出：对于不同空间位置，粒子滤波算法显示出了不同程度的标定能力，尽管波动幅度较大，频率较快，但x、z方向的绝对误差值始终不超过1mm，y方向则一直未大于0.05mm，标定精准度较为理想。

综上所述，该算法利用基于机器人的正运动学模型，改进粒子滤波算法，凭借高权值粒子群，为算法位置自标定精准度提供了可靠的理论基础，可满足不同领域对智能机器人的实际应用精度需求。

3.3 位置自标定的稳定性检测

为进一步验证该算法位置自标定稳定性的优越性，选用基于位姿约束与基于指数积公式的两种标定方法及粒子滤波算法，对目标机器人手腕末端的同一空间位置，各展开多组自标定试验。结合激光测量设备测得的实际坐标值，利用绝对误差指标，评估各方法的标定效果。不同方法位置标定的绝对误差结果，如图3所示。

图3 同一空间位置的自标定绝对误差示意图Fig.3 Schematic Diagram of Self−Calibration Absolute Error at Same Spatial Position

从三种方法的绝对误差指标变化情况可以看出：位姿约束标定法因讲求速度，故标定稳定性较差，不仅绝对误差值最高，而且波动幅度最大，频率最快；指数积公式标定法的指标值较低，且具有一定的稳定性；而粒子滤波算法的指标值则始终维持在较低的误差水平上，且波动幅度与频率均较小。

综上所述，相较于对比方法，根据粒子滤波算法测量、标定的末端模糊位置，利用三球座设备建立的世界坐标系，有效消除了机器人工具坐标系坐标与实际坐标间存在的固定偏差，因此，该算法不仅位置标定精准度更高，而且具有明显的稳定性优势。

3.4 位置自标定的用时检测

现代化工业时代对智能机器人的工作效率有较高要求，标定用时是影响机器人工作效率的因素之一。故该实验环节以位置标定速度为探讨方向，针对不同空间位置与同一空间位置两个角度，对比三种方法完成自标定的所用时长，以验证粒子滤波算法是否具备良好的实时性。各方法在不同实验条件下的位置自标定用时，如图4所示。

图4 不同方法的自标定用时示意图Fig.4 Schematic Diagram of Self−Calibration Time of Different Methods

分析不同空间位置下的自标定用时可知，三种方法的平均标定用时分别是62.44s、20.34s、22.54s，指数积公式标定法用时最长且最不稳定，虽然粒子滤波算法的标定速度仅次于以效率为目标需求的位姿约束标定法，但却更加平稳。

通过同一空间位置的自标定用时发现，标定相同位置时，三种方法都表现出了较为平稳的标定速度；粒子滤波算法与位姿约束标定法的平均用时20.6s、14.35s，将标定速度作为今后工作的主要攻克目标，确保在实现精准标定的前提下，更高效地完成标定任务。

3.5 修正效果比较

利用不同方法对智能机器人某一位置进行自标定后，对机器人位置进行多次修正，如图5所示。为了更为直观比较不同算法的修正效果，结合图5获取位置修正的相关数据，如表3所示。

图5 位置修正结果Fig.5 Position Correction Results

表3 位置修正数据Tab.3 Position Correction Data

分析图4与表3中的数据可知，与指数积标定法、位姿约束标定法相比，将粒子滤波算法应用至智能机器人自标定后，可以有效修正机器人标定误差，实际应用效果更好。

综上所述，粒子滤波算法将末端工具坐标系转换至世界坐标系下，令正运动学的起始点从基座变为三球座设备，简化了坐标系处理的复杂度，加快了自标定速度，该算法效率能基本满足大部分工业现场的快速标定需求。

4 结论

机器人技术的诞生与发展，为社会生产与日常生活带来了极为深刻的影响，推动各行各业朝着自动化、智能化方向发展。科技水平不断进步，使各类智能机器人越来越普及，逐渐成为工业、服务业等领域中不可或缺的重要设备。随着机器人应用范围与工作内容不断升级，要求也与日俱增，其中以位置自标定技术的讨论最为热烈。因此，提出智能机器人位置自标定算法，希望能为机器人持续创新与发展前景拓展奠定坚实的基础。由于研究过程中未利用运动学参数的实际值来补偿误差，故今后将从以下几点不断完善算法性能，以实现真正的推广使用，即针对机器人基座与工件关系，探讨工业现场中工件操作阶段对位置标定结果的影响；需就受力、温度等外界因素，分析机器人末端实际位置与标定位置的误差，并尝试采用新型的算法与技术优化精度，使其满足行业的精密性需求。