自驱动关节臂坐标测量机轨迹优化

杨洪涛，刘月琪，程晶晶，沈 梅，胡 毅

（1.安徽理工大学 机械工程学院，安徽 淮南 232001；2.合肥工业大学 仪器科学与光电工程学院，安徽 合肥 230009）

1 引 言

现有手动式关节臂测量机（Arm Articulated Coordinate Measuring Machine，AACMM）采 用人力拖拽的测量方式，虽然避免了路径规划问题，但存在测量力不可控制、测量位姿与测量路径不可复现等问题［1-2］。因此提出一种自驱动关节臂坐标测量机［3-6］（后简称为：自驱动关节臂测量机），通过在旋转关节部分引入一体化关节模组，实现自驱动关节臂测量机自驱动控制以及对工件几何特征的自动测量。轨迹规划是自动测量中关键环节，且自驱动关节臂测量机结构与常见工业机械臂结构相似，因此自驱动关节臂测量机也需要进行轨迹规划及优化。

合理的轨迹规划有利于提高机械臂测量机的测量精度和测量效率，决定了机械臂的运动方式和工作性能［7］。机械臂的轨迹规划［8-9］一般是在关节空间内和笛卡尔空间内，前者主要有多项式、S形速度曲线等规划方法，后者则大多采用插值点坐标逆解的规划方式。随着对测量效率的要求越来越高，出现了以时间、能量、冲击为目标或多目标的轨迹优化算法。马宇科等［10］针对两栖机器人的水下轨迹优化问题，通过对比基于GCOP与SQP的轨迹算法，证明了使用SQP算法规划的路径更合理、更有效。Amruta Rout等［11］为了降低机器人的位置误差，采用一种改进的多目标蚁狮算法进行轨迹优化，显著降低了扭矩率和加加速度，使运行时间大大减少。韩江桂等［12］针对机械臂运动效率和冲击问题采用灰狼算法进行优化，降低了运动过程中的振动。袁锦涛等［13］针对工业机器人多目标综合最优轨迹规划问题，采用粒子群算法对五次多项式构造的关节空间轨迹进行优化，有效降低了工业机器人的冲击振动，实现了时间-能量-冲击的性能最优。王延年等［14］针对六自由度机器人多目标综合最优轨迹规划问题，采用改进粒子群算法对B样条轨迹规划进行优化，有效降低运行过程中冲击振动，实现了时间性能最优。

本文基于自驱动关节臂测量机的机械结构及上述轨迹规划、轨迹优化的现有研究，提出自驱动关节臂测量机的混合轨迹规划以及基于粒子群算法的多目标测量轨迹优化方法，并通过以标准球为测量对象的仿真实验和实际测量，验证该方法的有效性。

2 自驱动关节臂测量机的结构原理与运动轨迹规划

2.1 自驱动关节臂测量机结构原理

自驱动关节臂测量机的结构主要由6个转动关节、3个连杆以及基座，测座，恒力触发式测头串联组成，如图1（a）所示。为实现在线自动测量，自驱动关节臂测量机采用一体化关节模组，组成部分分别为：驱动电路、伺服电机、谐波减速器、圆光栅等。

为了直观反映自驱动关节臂测量机各个关节之间的运动传递关系，本文基于改进的Dena⁃vit-Hartenberg［15］（MD-H）参数法建立自驱动关节臂测量机的连杆坐标系以及测量模型，坐标系建立情况如图1（b）所示。

根据MD-H模型所得自驱动关节臂测量机参数以及坐标系变换的链式法则，得到相邻坐标系之间的齐次变换矩阵，如式（1）所示。

其 中：αi-1为zi-1沿xi-1轴移动到zi轴的距离，ai-1为zi-1轴绕xi-1轴旋转到zi轴的角度，di为xi-1轴沿zi轴移动到xi轴的距离，θi为xi-1轴绕zi轴旋转到xi轴的角度。

图1 自驱动关节臂测量机结构及MD-H模型Fig.1 Structure and MD-H model of self-driven AAC⁃MM

其测量模型为测头到末端坐标系之间所有变换矩阵相乘得到的自驱动关节臂的正运动学方程，如式（2）所示。

其中：θ1，θ2，…，θ6为关节角度变量；07T表示测头末端的位姿。

2.2 自驱动关节臂测量机的轨迹规划

根据自驱动关节臂测量机的测量过程提出一种混合轨迹规划方法，且将自驱动关节臂测量机的测量过程分为定位、测量、回退三个阶段。在定位、回退中选择关节空间内的轨迹规划，测量阶段选用笛卡尔空间内的轨迹规划。

测量阶段无需复杂路径且速度过高会造成测量结果不准确和测杆折断，因此在笛卡尔空间内选择匀速直线轨迹规划，其表达式如式（3）所示。直线插补示意图如图2（a）所示。

由于S形加减速法存在匀速段，并能够提高电机利用率，因此在定位、回退阶段中采用末速度不为0的S形加减速法，其运动参数曲线如图2（b）所示。对图2（b）中速度函数进行积分求解，得到运行轨迹的时间函数S（t），如式（4）所示。

图2 轨迹规划原理图Fig.2 Trajectory planning schematic diagram

其中，Si（t）（i=1，2，…，7）表示不同阶段运行的轨迹长度。

笛卡尔空间与关节空间之间的速度映射关系如式（5）所示。其中，v为速度矢量，J为雅可比矩阵，θ̇为关节速度向量。

3 基于PSO的运动轨迹优化与仿真

粒子群算法［16］（Particle Swarm Optimiza⁃tion，PSO）是一种进化计算的技术，起源于鸟群捕食的行为研究，简单易实现，收敛快且鲁棒性高，仅有两个属性：速度和位置，分别代表移动的快慢和方向。初始值是一个随机粒子，粒子在每一次迭代中，通过跟踪两个极值来更新自己，分别是粒子本身寻找到的最优解称为个体极值和整个种群目前寻找到的最优解，用Pbest，Gbest表示，其流程图如图3所示。

图3 粒子群算法流程图Fig.3 Flow chart of PSO

3.1 目标函数和约束条件

为了保证自驱动关节臂测量机的测量精度与测量效率，将时间和运动平稳性确定为优化目标，并用脉冲来评价运动平稳性，将多目标优化转为单目标优化，因此本文的优化目标函数如式（6）所示。

其中：H1表示各关节的运动总时间，H2表示关节的脉冲指标，Ti和ji分别表示第i个关节的运行时间和加加速度，a1、a2为归一化系数，b1、b2为权重系数。

由于测量段采用匀速直线插补法，对测量时间和运动平稳性影响较小，因此只优化定位段和回退段中的运动学参数即可满足优化要求。即对定位段和回退段内采取的S形加减速参数进行优化。结合自驱动关节臂测量机的关节角度、速度、加速度、加加速度等运动学参数限制，七段S形加减法轨迹规划的优化约束条件如式（7）所示。

其中：v0，v1分别代表轨迹规划起点和终点所对应的速度；vimax，aimax，jimax分别表示第i个关节通过轨迹规划可以达到的速度，加速度，加加速度峰值；Tv>0表示整段轨迹规划中匀速段存在。

3.2 标准球测量轨迹仿真

为了验证轨迹优化的可行性，开展测量标准球的仿真分析，利用MATLAB中Robotics Tool⁃box工具箱创建自驱动关节臂测量机简化模型及一个标准球，其球心坐标为（14.634 9 mm，482.808 6 mm，221.431 2 mm），半径为19.056 4 mm。MATLAB仿真系统如图4（a）所示。为使仿真结果更加贴合测量机实际工作情况，在AD⁃AMS中搭建测量机的虚拟样机与标准球的碰撞环境，分析存在重力、惯性力、触测力时测量机的定位精度与运动平稳性。该环境中标准球参数与MATLAB仿真中一致，ADAMS虚拟样机的仿真系统如图4（b）所示。

需要在标准球表面选取6个测量点拟合完整球体，因此在测量过程中，自驱动关节臂测量机对该标准球体进行6次测量，每次测量后将会得到一个测量点的实际三维坐标，选取的测量点坐标如表1所示。

图4 自驱动关节臂测量机仿真系统Fig.4 Simulation system of self-driven AACMM

表1 选取的测量点坐标Tab.1 Coordinates of selected measuring points

测量段的匀速速度设为5 mm/s，将测点按照测量顺序1~6，经过基于粒子群算法的轨迹规划后得到整个标准球测量仿真轨迹如图5所示。

由于测量过程分为定位段、测量段、回退段，因此再引入5个中间点，其坐标分别为（76.56，447.23，234.84），（78.68，526.97，241.99），（-3.98，544.40，231.56），（-64.65，501.35，271.57），（-1.48，515.17，285.03），单位为mm。将测点过程分为6段，第1段是起始点到测量点1，其余五段是中间点到测量点。当测量过程处于定位阶段时，测量机以先加速、后匀速、再减速的变速方式运动至测量点附近3~5 mm处。之后进入测量阶段，测头以5 mm/s的速度匀速缓慢触测空间中的测量点。测头触测完毕后，测量机测头回退至中间点。

图5 标准球测量仿真轨迹Fig.5 Standard ball measurement simulation trajectory

当自驱动关节臂测量机产生振动时，因其串联式结构，误差会逐级放大，传至测头的振动会更明显，影响最终的测量精度。因此选择以测头速度变化来评估自驱动关节臂测量机在标准球测量过程中的平稳性。使用MATLAB对基于粒子群算法的标准球测量优化轨迹进行仿真试验，所得测头速度曲线如图6所示。

图6 优化前后测头速度曲线Fig.6 Probe velocity curve before and after optimize

由图6可知，经基于粒子群优化算法轨迹优化后，定位阶段与测量阶段的测头速度变化曲线变化更为平稳，且无明显突变，满足测量机平稳测量的要求。其中，图6速度曲线存在的宏观折线性质，验证了测量机在定位阶段的加速—匀速—减速运动特性和测量阶段的匀速测量特性。但由于建立的刚柔耦合测量机虚拟样机在仿真过程中，关节臂杆件因不同的空间测量姿态而发生不同的柔性变形，且相邻两个测量点之间的空间距离不同，因此导致测量机测量6个点的速度变化曲线存在差异，且第2~6段优化后的定位阶段速度微观变化与第1段的加速—匀速—减速曲线变化特征存在差异。

为了进一步验证基于粒子群算法的轨迹优化方法对提高测量机平稳运动测量的有效性，根据图6仿真结果，求解并绘制出优化前后的测量机测头加速度变化曲线，如图7所示。

根据图7可知，经基于粒子群优化算法轨迹优化后，定位阶段与测量阶段衔接处的测头加速度突变情况得到明显改善，有效降低了测量机从定位转向测量过程中的测头振动，提高测量机的在线自动测量精度。

在ADAMS仿真环境中，分别对基于粒子群算法的标准球测量优化轨迹与未优化的测量轨迹进行仿真实验，得到了轨迹优化前后所对应实际测量点坐标以及整个测量过程所用的时间，如表2所示。

图7 优化前后测头加速度曲线Fig.7 Probe accelerate curve before and after optimize

利用最小二乘法［17］将仿真得到的6个实际测量点拟合为球面，拟合得到标准球半径与误差如表3所示。

从仿真结果可知，优化前测量标准球所需的总时间为63.534 7 s，优化后总时间为57.886 2 s，效率提高了8.890 5%。同时标准球体半径误差由0.165 5 mm减少到0.056 88 mm，测量精度提高了65.631 4%，该优化算法的有效性得到了验证。

表2 仿真测量点坐标与所用时间Tab.2 Simulation coordinates and measurement time

表3 拟合的球体半径与误差Tab.3 Fitting the sphere radius and error

4 标准球测量轨迹实验验证

4.1 实验平台搭建

为了验证自驱动关节臂测量机的实际测量效果，搭建了自驱动关节臂测量机实验平台，如图8所示，其组成部件有：测头、测量机本体、电气箱、上位机。

其中红宝石测头采用Renishaw公司的恒力触发测头，一体化关节模组则采用INNFOS公司的关节模组。电气箱是集成了关节模组驱动与接口电路、测头电路和NI MYDAQ。上位机软件由LabVIEW平台编写搭建，可以通过导入规划好的路径实现自动测量，并实时显示自驱动关节臂测量机的状态，同时输出测头空间坐标等信息。

图8 自驱动关节臂测量机实验平台Fig.8 Experimental platform of self-drive AACMM

4.2 标准球测量实验及结果

将自驱动关节臂坐标测量机样机固定在工作台上，标准球通过磁性表座固定在剪式升降台上，剪式升降台可调节标准球在空间中的位置。保持实验室温度在20˚左右，并保持标准球的空间位置不变，利用自驱动关节臂测量机测量标准球表面6个点的实际坐标，标准球参数、6个点的理论坐标与仿真实验相同。

将采用传统算法规划的轨迹和利用粒子群算法的优化算法规划的混合轨迹分别导入上位机，测量得到6个测量点的实际坐标，再利用最小二乘法把实验测得的6个点坐标拟合成球体。重复测量标准球实验5次，经数据处理后得标准球测量半径。优化前后所得测量半径和误差如表4所示。上位机采样时间为10 ms，根据采集的角度数据个数计算得到优化前测量机测量标准球耗时62.91 s，优化后测量机测量标准球耗时57.35 s。

实验表明：轨迹优化后，标准球半径的平均测量误差由0.057 1 mm降至0.042 3 mm，降幅为25.919 4%，测量时间由62.91 s降至57.35 s，效率提升了8.838%，验证了该轨迹优化算法的有效性，能够提高自驱动关节臂坐标测量机的自动测量精度。

表4 优化前后的测量半径和误差Tab.4 Measurement radius and error before and after op⁃timization

5 结 论

本文提出基于粒子群算法的自驱动关节臂测量机多目标轨迹优化方法，根据时间和运动平稳性综合确定了轨迹优化的目标函数，并通过基于粒子群算法的S形加减速和匀速直线插补法进行混合轨迹规划，实现了运行时间和运动平稳的性能完善。通过实验验证，使用基于粒子群算法的多目标测量轨迹优化方法后，自驱动关节臂测量机测得的标准球半径误差值从0.057 1 mm下降为0.042 3 mm，降低了25.919 4%，测量时间由62.91 s降至57.35 s，提升了8.838%，表明该方法优化效果明显。所提出的基于粒子群算法的轨迹优化方法保证了自驱动关节臂测量机平稳运行，提高了自驱动关节臂测量机的在线测量精度和效率。