CN113334385A - 一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法 - Google Patents

Abstract

本发明涉及一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法。将位姿信息和约束条件输入加减速算法和圆弧过渡模型得到过渡轨迹，与直线轨迹相结合得到完整的测量轨迹。借助逆运动学与逆雅克比矩阵求得各关节角度、速度、角速度，于速度突变处进行关节空间五次多项式插值，确保速度、加速度连续性和平滑性，防止由于速度、加速度突变对自驱动关节臂测量机造成冲击，提高了测量精度。本发明可实现自驱动关节臂测量机的高精度测量，也可应用于工业机器人的轨迹规划。

Description

一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法

技术领域

本发明属于机械臂轨迹规划领域，具体涉及一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法。

背景技术

市场上已有的关节臂测量机采用手动拖拽、离线检测的模式，测量效率低且无法应用于在线检测，因此研发了自驱动关节臂测量机来实现在线自动测量。而轨迹规划是自动测量中必不可缺的部分。自驱动关节臂测量机的轨迹包括直线与圆弧的组合，为保证高精度测量，在确保末端轨迹平滑的同时，关节空间的速度、加速度变化也有严格要求，避免速度、加速度突变对自驱动关节臂测量机造成振动，因此提出了一种直线轨迹间平滑过渡的方法。

目前自驱动关节臂测量机的轨迹规划方面的内容参考资料极少，由于自驱动关节臂测量机与工业机器人结构类似，而工作过程与要求存在差异，可参考机器人轨迹规划方面的内容并在此基础上加以改进。轨迹间的过渡常采用圆弧过渡模型，例如：中国发明专利“一种焊接机器人过渡轨迹规划方法”(申请号为201110000264.3)，针对焊接机器人的过渡焊缝采用圆弧衔接，但圆弧过渡在过渡段与轨迹衔接处存在速度的跃变，影响轨迹平滑性，严重可导致机械振动。而本方法实现末端轨迹连续、速度平滑的同时，可保证关节空间角度、速度、加速度的平滑，对提高自驱动关节臂测量机的测量精度具有重要意义。

发明内容

为了实现自驱动关节臂测量机的高精度测量，满足避障要求以及避免速度、加速度突变引起振动，本发明根据自驱动关节臂测量机的实际测量过程，提出了一种直线轨迹间过渡的规划方法，该方法能有效地保证自驱动关节臂测量机在直线轨迹间平滑过渡，且关节速度、加速度连续、平滑的变化。

本发明的基本技术方案包括以下步骤：

步骤1：输入轨迹过渡所需参数与约束

自驱动关节臂测量机的末端位姿由3×1的位置矢量P和3×3的姿态矩阵R共同描述，轨迹过渡模块所需的参数有：起始点位姿P₀，两段直线交点位姿P₁，终点位姿P₂。约束有：起始点P₁的速度，终止点P₂的速度，最大速度限制V_max，最大加速度限制A_max。

步骤2：过渡模型的建立

建立圆弧过渡模型进行直线拐角的过渡，设定过渡半径r，过渡速度v。求解拐角θ、过渡转接点P_r1、P_r2，由几何关系可得：

步骤3：过渡段速度规划

由加减速轨迹规划算法求得P₀P₁段在过渡模型衔接点P_r1的速度，P₁P₂段与圆弧衔接点 P_r2的速度，过渡段速度为匀速，即点P_r1的速度与点P_r2的速度相等。考虑机械系统动力学因素计算曲线实际能达到的最大速度v_r。再将第一段直线初始点速度v₀，第二段直线终止点速度v₁以及最大速度限制v_r导入加减速轨迹规划算法求得整段轨迹的速度变化。

步骤4：插补点位姿的求取

整段轨迹的位置插补包括两段直线插补和一段圆弧插补，第一段直线插补算法需要将加减速算法得到的位移、速度、加速度进行归一化处理，每个插值点的时间值都有一个归一化参数λ与之对应。圆弧插补需新建坐标系，将空间圆弧转化为平面圆弧，对其圆心角进行插补，同样进行归一化处理。第二段直线插补速度规划为匀速。姿态插补需要P₀、P_r1、P_r2、以及P₂的姿态矩阵，将姿态矩阵用单位四元数表示，进行四元数线性插值求得各插值点所对应的姿态，与位置矢量结合在一起可表示每个插值点的位姿。

步骤5：关节空间角度、速度、加速度求解

已知插补点的位姿，通过自驱动关节臂测量机的逆运动学模型求得六个关节的角度变化曲线，由关节空间速度与操作空间速度之间的关系，借助逆雅可比矩阵，可求得六个关节的速度变化曲线，利用差分法求得加速度变化曲线。

步骤6：关节空间轨迹过渡模型

操作空间末端速度与加速度曲线平滑过渡，映射到关节空间，直线与圆弧过渡段衔接处速度和加速度仍存在突变，因此选择速度和加速度连续的五次多项式插值进行关节空间轨迹规划。在速度突变点附近的插值点中选取适当的点数，假定选取两点，分别用A、B表示。从A到B点的运动时间用t_f表示。五次多项式表达式为：

θ(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

约束条件：

其中θ_A表示A点的角度，θ_B表示B点的速度；

解得六个系数为：

将关节空间轨迹规划得到的角度值带入自驱动关节臂测量机正运动学模型得到末端在操作空间中的位置，与操作空间轨迹规划的插值点相结合，得到完整的测量轨迹。

本发明具有的有益效果是：

1)目前市场上关节臂测量机采用手动拖拽的方式，测量效率低。本发明所得轨迹可实现自驱动关节臂测量机的自动在线测量，也能应用于工业机器人的轨迹规划。

2)采用圆弧过渡衔接两段直线，保证操作空间轨迹和末端速度连续、平滑。

3)关节空间速度突变处采用五次多项式重新插值规划，确保各关节角速度、角加速度平滑性，避免速度、加速度波动对自驱动关节臂测量机造成冲击，提高测量精度。

附图说明

图1是自驱动关节臂测量机直线轨迹间平滑过渡的流程图

图2是自驱动关节臂测量机圆弧过渡轨迹示意图

图3是自驱动关节臂测量机末端点的空间位置图

图4是自驱动关节臂测量机关节角度曲线图

图5是自驱动关节臂测量机关节速度曲线图

图6是自驱动关节臂测量机关节加速度曲线图

具体实施方式

下面结合附图对本发明技术方案作进一步说明。

本发明提供一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法，流程图如图1所示。包括以下步骤：

步骤1：输入轨迹过渡所需参数与约束

自驱动关节臂测量机的末端位姿由3×1的位置矢量P和3×3的姿态矩阵R共同描述，起始点，中间点和终止点的位姿由操作者任意指定，将位置坐标设定为P₀(461.9,0,127.9)，P₁(5 40,200,150)，P₂(542,203,150)。约束设定为：末端最大速度V_max＝100mm/s、最大加速度A_max＝100mm/s²。

步骤2：过渡模型的建立

建立圆弧过渡模型进行直线拐角的过渡，如图2所示。设定过渡半径r＝3mm，过渡速度 v＝5mm/s，圆弧过渡几何关系如下：

由以上公式可求得拐角θ，过渡转接点P_r1、P_r2。

步骤3：过渡段速度规划

P₀P₁段速度规划利用加减速算法求得在过渡模型衔接点P_r1的速度，圆弧过渡段为匀速过程，整段速度与P_r1点速度相等，由此可得P_r1与P_r2点处速度相等。考虑到机械系统动力学因素求解曲线实际能达到的最大速度v_r。再将第一段直线初始点速度v₀，第二段直线终止点速度v₁以及最大速度限制v_r导入加减速算法求得整段轨迹的速度变化。

步骤4：插补点位姿的求取

整段轨迹的位置插补包括两段直线插补和一段圆弧插补，第一段直线插补算法需要将加减速算法得到的位移、速度、加速度进行归一化处理，每个插值点的时间值都有一个归一化参数λ与之对应。圆弧插补需新建坐标系，将空间圆弧转化为平面圆弧，对其圆心角进行插补，同样进行归一化处理。第二段直线插补为匀速直线插补，姿态插补需要P₀、P_r1、P_r2、以及P₂的姿态矩阵，将姿态矩阵用单位四元数表示，进行四元数线性插值求得各插值点所对应的姿态，与位置矢量结合在一起可表示每个插值点的位姿。

步骤5：关节空间角度、速度、加速度求解

已知插补点的位姿，通过自驱动关节臂测量机的逆运动学模型求得六个关节的角度变化曲线，由关节空间速度与操作空间速度之间的关系，借助逆雅可比矩阵，可求得六个关节的速度变化曲线，利用差分法求得加速度变化曲线。

步骤6：关节空间轨迹过渡模型

操作空间末端速度与加速度曲线平滑过渡，映射到关节空间，直线与圆弧过渡段衔接处速度和加速度仍存在突变，因此选择速度和加速度连续的五次多项式插值进行关节空间轨迹规划。以一个关节为例，在速度突变点附近的插值点中选取适当的点数，假定选取两点，分别用A、B表示。A点所对应的角度为0.45rad，速度为0.039rad/s，加速度为-0.15rad/s²，B 点所对应的角度为0.47rad，速度为0.02rad/s，加速度为0.07rad/s²。从A到B点的运动时间用t_f表示，整个过程中t_f＝0.3s，五次多项式表达式为：

θ(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

约束条件：

根据下式，可解得六个系数，带入五次多项式表达式中得到详细的表达式。

重新规划后的各关节角度、速度、加速度变化如图4、图5、图6所示。将关节空间轨迹规划得到的角度值带入自驱动关节臂测量机正运动学模型得到末端在操作空间中的位置，与操作空间轨迹规划的插值点相结合，得到完整的测量轨迹，见图3。

从附图中可看出本发明规划方法结果可靠。

Claims (1)

1.一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法，其特征在于，该方法的步骤是：

步骤1：输入轨迹过渡所需参数与约束

自驱动关节臂测量机的末端位姿由3×1的位置矢量P和3×3的姿态矩阵R共同描述，轨迹过渡模块所需的参数有：起始点位姿P₀，两段直线交点位姿P₁，终点位姿P₂。约束有：起始点P₁的速度，终止点P₂的速度，最大速度限制V_max，最大加速度限制A_max。

步骤2：过渡模型的建立

建立圆弧过渡模型进行直线拐角的过渡，设定过渡半径r，过渡速度v。求解拐角θ、过渡转接点P_r1、P_r2，由几何关系可得：

步骤3：过渡段速度规划

由加减速轨迹规划算法求得P₀P₁段在过渡模型衔接点P_r1的速度，P₁P₂段与圆弧衔接点P_r2的速度，过渡段速度为匀速，即点P_r1的速度与点P_r2的速度相等。考虑机械系统动力学因素计算曲线实际能达到的最大速度v_r。再将第一段直线初始点速度v₀，第二段直线终止点速度v₁以及最大速度限制v_r导入加减速轨迹规划算法求得整段轨迹的速度变化。

步骤4：插补点位姿的求取

整段轨迹的位置插补包括两段直线插补和一段圆弧插补，第一段直线插补算法需要将加减速算法得到的位移、速度、加速度进行归一化处理，每个插值点的时间值都有一个归一化参数λ与之对应。圆弧插补需新建坐标系，将空间圆弧转化为平面圆弧，对其圆心角进行插补，同样进行归一化处理。第二段直线插补为匀速直线插补。姿态插补需要P₀、P_r1、P_r2、以及P₂的姿态矩阵，将姿态矩阵用单位四元数表示，进行四元数线性插值求得各插值点所对应的姿态，与位置矢量结合在一起可表示每个插值点的位姿。

步骤5：关节空间角度、速度、加速度求解

已知插补点的位姿，通过自驱动关节臂测量机的逆运动学模型求得六个关节的角度变化曲线，由关节空间速度与操作空间速度之间的关系，借助逆雅可比矩阵，可求得六个关节的速度变化曲线，利用差分法求得加速度变化曲线。

步骤6：关节空间轨迹过渡模型

操作空间末端速度与加速度曲线平滑过渡，映射到关节空间，直线与圆弧过渡段衔接处速度和加速度仍存在突变，因此选择速度和加速度连续的五次多项式插值进行关节空间轨迹规划。在速度突变点附近的插值点中选取适当的点数，假定选取两点，分别用A、B表示。从A到B点的运动时间用t_f表示。五次多项式表达式为：

θ(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

约束条件：

其中θ_A表示A点的角度，θ_B表示B点的角度；

解得六个系数为：

将关节空间轨迹规划得到的角度值带入自驱动关节臂测量机正运动学模型得到末端在操作空间中的位置，与操作空间轨迹规划的插值点相结合，得到完整的测量轨迹。

Images