CN106826829B - 一种可控误差的工业机器人光顺运动轨迹生成方法 - Google Patents

Abstract

一种可控误差的工业机器人光顺运动轨迹生成方法，包括如下步骤：S1、生成MOVEB运动指令，描述工业机器人运动轨迹，包括轨迹点和姿态，以及用户输入的轨迹点误差阈值和弦高误差阈值；S2、机器人轨迹点的插值，利用高阶B样条插值算法，将高阶B样条曲线按照轨迹点误差和弦高误差阈值对轨迹点进行插值，分别实现机器人轨迹点的G²插值和G³插值，得到具有高连续性并且保证满足轨迹点误差和弦高误差要求的插值轨迹；S3、机器人姿态的插值，得到分段G²和G³连续的机器人姿态曲线；S4、通过轨迹点插值曲线和机器人姿态曲线共同得到机器人插值后的运动轨迹。本发明运动轨迹实时连续插值，计算简单、高效、精确，减少机器振动和磨损。

Description

一种可控误差的工业机器人光顺运动轨迹生成方法

技术领域

本发明属于工业机器人应用技术领域，具体涉及一种可控误差的工业机器人光顺运动轨迹生成方法。

背景技术

工业机器人广泛应用于焊接、装配、搬运、抛光、喷涂和切削加工等复杂作业。在作业中工业机器人通过运动指令来控制机器人的运动轨迹。现有工业机器人可识别的运动指令主要包括直线运动指令、圆弧运动指令及轴坐标运动指令，如ABB机器人中分别用MOVEL、MOVEC和MOVEJ表示以上三种指令。简单运动轨迹可通过示教器编写轴坐标运动指令，主要应用于只要求精确到点，点与点之间轨迹任意但时间最短的作业(如点焊机器人)。大多数复杂的运动轨迹需要通过离散编程软件得到，且主要包括直线运动指令和圆弧运动指令。但直线段和圆弧表示的运动轨迹在连接处仅具有G⁰连续性，在机器人作业过程中，为了精确到达给定轨迹点必须降速，因此大大降低了作业效率；另外，速度和加速度不连续可能导致机器人运动时的振动，从而加快机器磨损并影响作业质量。

为了提高机器人运动轨迹的连续性，现有运动指令中加入了逼近指令。如图1所示，ABB机器人中利用fine指令表示精确通过给定轨迹点，z10表示误差10mm的圆滑过渡，但是该逼近指令无法保证轨迹点误差和弦高误差满足要求；安川机器人用MOVES表示抛物线指令，虽然抛物线指令能够精确通过给定轨迹点，但无法保证弦高误差满足需求。

由上可知，仅仅利用现有直线和圆弧运动指令无法生成同时满足轨迹点误差和弦高误差且具有高连续的运动轨迹。于是现有研究中出现了一些在离线编程阶段插值运动轨迹和在控制***内部插值运动轨迹的方法。用光滑曲线插值轨迹点是提高连续性的最好方法，但是轨迹插值需满足轨迹点误差和弦高误差要求。误差控制具有很重要的实际意义，如焊接过程中要求焊接点在轨迹点误差范围内、喷涂过程中两点之间的喷涂轨迹要求在弦高误差范围内。两个误差阈值由用户根据加工工艺设置，任何一个误差不满足都会造成产品的质量不合格。

现有插值方法大部分只考虑了轨迹点误差控制，未考虑弦高误差控制：如现有非专利文献《基于样条插值算法的工业机器人轨迹规划研究》在离线编程过程中通过三次B样条曲线插值轨迹点，然后将插值曲线离散并后处理为直线或圆弧运动指令。该方法虽然通过插值提高了轨迹的连续性，但离散后还会存在误差不满足和连续性低的不足。非专利文献《Real-time Coplanar NURBS Curve Fitting and Interpolation for 6-DOF RobotArm》中实现了一种机器人控制***实时NURBS插补和离散的算法，但该方法未保证弦高误差，且通过矩阵求逆计算控制顶点的方法稳定性差，不能保证弦高误差满足要求。

机器人运动轨迹的光顺插值包括轨迹点插值和姿态插值，轨迹点的光顺插值和数控铣削加工中小线段的光顺插值类似：非专利文献《A real-time look-aheadinterpolation methodology with curvature-continuous B-spline transitionscheme for CNC machining of short line segments》提出了一种数控加工中三轴刀具轨迹的G²连续桥接算法来解决上述问题，可得到轨迹点光滑过渡且曲率连续的实时保形插值轨迹。但该方法得到的桥接轨迹无法实现高精度的插值，轨迹点误差越小桥接曲线越短，过短的桥接轨迹无法实现速度的平滑。非专利文献《A real-time curvature-smoothinterpolation scheme and motion planning for CNC machining of short linesegments》对上述方法进行改进，提出了刀具轨迹的G³连续插值算法，可得到满足轨迹点误差和弦高误差且曲率光顺的实时保形插值轨迹，能够有效地提高加工效率和加工质量，并减少机床振动。该方法在三轴数控加工中取得了较好的应用，但不适用于五轴加工和工业机器人应用中。

运动轨迹的姿态不连续也会影响加工的精度和效率，现有姿态插值方法有四元数法，基于矩阵指数法和直接插值欧拉角的方法。现有非专利文献《Interpolation methodfor robot trajectory planning》中直接对欧拉角插值来实现机器人姿态的插值，但直接对欧拉角插值不具有坐标不变性，欧拉角的连续不能对应轨迹点姿态的连续。

鉴于现有技术中机器人运动轨迹不能同时满足高连续性和高精度的要求，亟需一种工业机器人光顺运动轨迹的生成方法。

发明内容

本发明要解决的技术问题是，针对现有技术中机器人存在连续性差和误差不满足导致工作效率低、质量不合格和机器振动的问题，提供一种可控误差的工业机器人光顺运动轨迹生成方法，计算简单、高效、精确，机器人运动轨迹能同时满足高连续性和高精度的要求，工业机器人误差(轨迹点误差和弦高误差)满足实时实现要求光顺的保形运动轨迹。

本发明为解决上述技术问题所采取的技术方案是：

一种可控误差的工业机器人光顺运动轨迹生成方法，包括如下步骤：

S1、生成MOVEB运动指令：MOVEB运动指令用来描述工业机器人运动轨迹，包括轨迹点和姿态，以及用户输入的轨迹点误差阈值和弦高误差阈值；

S2、机器人轨迹点的插值：将MOVEB运动指令表示的工业机器人运动轨迹导入机器人控制***，利用高阶B样条插值算法，将高阶B样条曲线按照轨迹点误差和弦高误差阈值对轨迹点进行插值，分别实现机器人轨迹点的G²插值(具有G²连续的三次B样条插值)和G³插值(具有G³连续的四次B样条插值)，得到具有高连续性并且保证满足轨迹点误差和弦高误差要求的插值轨迹；

S3、机器人姿态的插值：通过B样条插值算法分别实现机器人姿态的分段G²和G³插值，并对插值后的机器人轨迹的姿态进行单位正交化处理，得到分段G²和G³连续的机器人姿态曲线；

S4、生成机器人插值后的运动轨迹：通过上述步骤S2获取的轨迹点插值曲线和步骤S3获取的机器人姿态曲线共同得到机器人插值后的运动轨迹。

按上述方案，所述步骤S1中MOVEB运动指令的数据内容包括X,Y,Z,I,J,K,U,V,W,D,E共十一个变量，其中(X,Y,Z)表示工具坐标系下的轨迹点的坐标，(I,J,K)表示上述轨迹点处建立的工具坐标系的Z轴单位矢量，(U,V,W)表示工具坐标系的X轴单位矢量，D,E分别表示弦高误差阈值和轨迹点误差阈值。

按上述方案，所述步骤S2及步骤S3中，机器人控制***解析MOVEB运动指令时，将轨迹点误差阈值和弦高误差阈值相同的一段连续轨迹组成一组进行插值。

按上述方案，所述步骤S2中，机器人轨迹点的插值具体包括如下步骤：

步骤一：初始迭代参数决定，从MOVEB运动指令中获取一组机器人原始轨迹点弦高误差阈值D和轨迹点误差阈值E；迭代次数记为k＝0，计算初始迭代轨迹点和初始迭代弦高误差阈值

步骤二：具体包括：

①建立每个内部轨迹点(不包括首尾端点的迭代轨迹点)处的桥接B样条曲线，在整个步骤二中，省略迭代次数k，即等同于P_i，等同于d_i(方便描述)：

桥接B样条曲线的次数为p,共有2p+1个控制点，分别为A_1,i，A_2,i…A_p,i,Q_i,B_p,i,…B_2,i,B_1,i,节点矢量为{0,0.5,1},三个节点的重复度分别为p+1,p,p+1；该桥接B样条曲线等价于两条对称的p次Bezier曲线；

②建立在Q_i对称的两条p次Bezier曲线，两条Bezier曲线的控制点分别为A_1,i,A_2,i…A_p,i,Q_i和Q_i,B_p,i,…B_2,i,B_1,i，其中A_1,i，A_2,i…A_p,i与P_i-1P_i共线，B_p,i,…B_2,i,B_1,i与P_iP_i+1共线，Q_i为A_p,iB_p,i的中点；

③分G²和G³插值两种情形给出保证桥接B样条曲线处处G²或G³连续的控制点：

设h_i＝|A_p,iP_i|,θ_i＝<T_0,i,T_1,i>

其中T_0,i为P_i-1P_i的单位向量；T_1,i为的A_p,iB_p,i单位向量；h_i为线段A_p,iP_i的长度；θ_i为向量T_0,i和T_1,i的夹角；

i)在G²插值时，G²桥接曲线的控制顶点满足以下条件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_3,i＝Q_i-h_icosθ_iT_1,i

A_2,i＝A_3,i-h_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

ii)在G³插值时，G³桥接曲线的控制顶点满足以下条件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_4,i＝Q_i-h_icosθ_iT_1,i

A_3,i＝A_4,i-α_ih_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

α_i≥0.7,0<β_i≤0.5

在G²插值时，h_i,β_i为两个桥接曲线参数；在G³插值时,h_i,α_i,β_i为三个桥接曲线参数。桥接曲线参数决定了桥接曲线控制点的位置，在以下步骤⑤中通过目标函数最优化求解得到，B_1,i,B_2,i,…,B_p,i由桥接B样条曲线的对称性得到；

④以桥接曲线更平缓为目标建立优化目标函数，并给出保形和弦高误差控制的约束条件：

两条Bezier曲线构造的G²桥接曲线和G³桥接曲线在交点Q_i处的曲率最大，且最大曲率分别为和为得到更平缓的桥接曲线，要求k_g2max和k_g3max越小，即要求步骤③中桥接曲线参数h_i越大，因此将目标函数定义为求解桥接曲线参数h_i,i＝1,2,…N-1的和最大问题，即如下式(1)所示：

为实现保形约束，要求两个轨迹点之间的桥接曲线不存在重合部分，其中G²连续和G³连续的桥接保形约束为：

|P_i-1B_1,i-1|+|A_1,iP_i|≤|P_i-1P_i|,i＝2,…N-1 (2)

|A_1,1P₁|≤|P₀P₁| (3)

|P_N-1B_1,N-1|≤|P_N-1P_N| (4)

为实现弦高误差约束，只需控制P_iQ_i的距离不大于迭代弦高误差阈值d_i即可，即：

|P_iQ_i|≤d_i (5)

⑤求解最优化问题即式(1)同时要求桥接曲线参数满足约束条件式(2)、(3)、(4)、(5)，求解出桥接曲线参数，即得到迭代轨迹点的桥接轨迹；

步骤三：计算桥接轨迹与原始轨迹点的轨迹点误差最大值Δ_k，并判断Δ_k是否小于轨迹点误差阈值E，轨迹点误差的计算方法为计算轨迹点与两条Bezier曲线交点的距离。本步骤三中加k上标参数与步骤二中未加k上标参数等同，为描述第k次迭代的误差检测，未省略k：

其中位于的角平分线上，根据插值曲线的对称性，通过计算的大小和角平分线向量得到，计算方法为：

若Δ_k>E，说明桥接曲线未满足轨迹点误差要求，进入步骤四更新迭代轨迹点和迭代弦高误差阈值；否则，插值成功，退出迭代，导出插值轨迹；

步骤四：更新迭代参数，令迭代轨迹点的调整向量为：调整后的迭代轨迹点为：迭代弦高误差阈值为：更新迭代次数：k＝k+1，转步骤二。

按上述方案，所述步骤S3中，机器人姿态的插值具体包括如下步骤：

步骤一：计算偏移点，记处的工具坐标系的X轴单位矢量为Z轴单位矢量为将沿X轴偏移一段固定距离m，得到X轴偏移点记为将沿Z轴偏移一段固定距离n，得到Z轴偏移点记为

步骤二：对X轴偏移点和Z轴偏移点插值，采用轨迹点插值的方法分别对X轴偏移点、Z轴偏移点进行插值，得到分段插值曲线O(t),R(t)；

步骤三：计算插值后的轨迹的姿态，记机器人轨迹点的分段插值曲线为P(t)，插值后的X轴矢量：X(t)＝O(t)-P(t)，Z轴矢量：Z(t)＝R(t)-P(t)，对X轴矢量、Z轴矢量单位正交化处理，进一步计算得到插值后单位正交的X轴单位矢量、Z轴单位矢量：

插值后的X轴单位矢量X^new(t)为：

插值后的Z轴单位矢量Z^new(t)为：

Z^new(t)＝F(t)×X^new(t)×F(t)

其中

根据以上得到的X^new(t)，Z^new(t)描述插值后的姿态曲线，得到每个轨迹插值曲线上轨迹点对应的姿态信息。

按上述方案，所述步骤S3的步骤一中，偏移距离m,n的取值设置为轨迹点平均间距的5～10倍。

按上述方案，所述步骤S3的步骤三中，单位正交化之后的姿态分段曲线具有分段G²或G³连续性，即在分段点之间的曲线G²或G³连续，在分段点处至少具有G⁰连续性。

通过本发明所构思的以上技术方案，与现有技术相比，本发明具有的优势：

1、本发明提出的工业机器人连续运动轨迹生成方法能够保证数据点误差和弦高误差满足用户要求：用户可以根据加工工艺需求通过MOVEB指令指定数据点误差和弦高误差阈值，本发明的插值算法能够保证满足用户设置的数据点误差和弦高误差阈值，并且保证插值曲线的平缓和保形，从而能够保证实际作业的高精度性；可用于工业机器人在焊接、喷涂、激光切割等复杂作业中的运动轨迹实时连续插值；

2、本发明提出的工业机器人连续运动轨迹生成方法能够保证轨迹点高连续性(G²连续和G³连续)，G²连续的轨迹具有连续的速度和加速度，而G³连续的轨迹不仅具有连续的速度和加速度，还具有连续的加加速度，在机器人实际作业中，高连续性的轨迹能够有效的提高作业效率和作业质量精度，加加速度连续能够减少机器振动，从而减少机器磨损；

3、本发明提出的工业机器人连续运动轨迹生成方法能够保证姿态的分段高连续性(G²连续和G³连续)，分段姿态插值曲线能保证每两个分段点之间姿态(G²或G³)连续变化，机器人姿态的连续变化不仅可以提高机器人的作业精度，还能够减少机器人手臂大幅度摆动，从而有效的提高效率。

附图说明

图1为现有技术中ABB机器人运动指令；

图2为本发明实施例设计的MOVEB运动指令案例；

图3为本发明轨迹点高连续性插值流程图；

图4为本发明实施例G²桥接示意图；

图5为本发明实施例G³桥接示意图；

图6为本发明实施例轨迹点G²(G³)插值的保形约束；

图7为本发明实施例轨迹点G²(G³)插值的迭代示意图；

图8为本发明实施例Z轴偏移点插值示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施案例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施案例仅仅用以解释本发明，并不用于限定本发明。

本发明可控误差的工业机器人光顺运动轨迹生成方法，细分为如下四个内容：

<1>MOVEB运动指令的生成

首先发明了一种新的MOVEB运动指令，用来描述离线编程技术得到的工业机器人运动轨迹，并包含轨迹点误差阈值和弦高误差阈值信息，用户可以根据工艺要求设置轨迹点误差和弦高误差。MOVEB运动指令后面的数据内容包括X,Y,Z,I,J,K,U,V,W,D,E共十一个变量，其中前九个数据用于描述工业机器人的线性运动轨迹(轨迹点和姿态)，后两个变量分别为弦高误差阈值和轨迹点误差阈值。如图2所示，描述了五个轨迹点和姿态，其中用户指定的弦高误差阈值D＝0.001mm，轨迹点误差阈值E＝0.0001mm。

用户可以通过D,E设置机器人运动轨迹插值所需要的误差阈值，每个轨迹点可以设置相同的误差(轨迹点误差和弦高误差)阈值，也可以设置不同阈值。误差阈值由用户根据工艺决定，例如在机器人切削加工时弦高误差阈值可设置为加工允差，如在数控加工中，零部件的加工要求一般在±0.05mm～±0.1mm，可将轨迹点弦高误差设置为0.005mm～0.03mm；轨迹点误差阈值可设置为数值计算精度，一般为10^-4～10^-8mm。

MOVEB运动指令用来描述原始运动轨迹和插值误差，机器人控制***可识别MOVEB运动指令并按照误差阈值对原始运动轨迹进行高阶B样条插值。本发明实施例支持具有G²连续的三次B样条插值和具有G³连续的四次B样条插值，分别得到曲率连续和曲率光顺的满足误差要求的光顺插值曲线。

<2>机器人轨迹点的插值

在机器人控制***中导入MOVEB运动指令，获得工业机器人运动轨迹和误差阈值信息，由于用户可以对连续的轨迹点设置不同的误差阈值，因此在机器人控制***解析MOVEB运动指令时，将轨迹点误差阈值和弦高误差阈值相同的一段连续轨迹组成一组进行插值。

如图3所示，轨迹点的插值是指对MOVEB运动指令描述的运动轨迹点按照轨迹点误差和弦高误差阈值进行插值，该插值方法可实现具有G²连续的三次B样条插值和具有G³连续的四次B样条插值。轨迹点的插值的主要技术方案为导入一组MOVEB运动指令表示的运动轨迹和误差阈值(弦高误差阈值和轨迹点误差阈值)，然后计算迭代轨迹点的桥接轨迹，接着判断桥接轨迹与原始轨迹点之间的误差是否满足轨迹点误差阈值，若满足阈值，插值结束，导出桥接后的轨迹；否则根据轨迹点误差更新迭代轨迹点和迭代弦高误差阈值，重新桥接。

步骤S2中，通过构造桥接B样条曲线，并不断迭代更新桥接曲线使轨迹点误差和弦高误差满足，记最终桥接B样条曲线为插值曲线：首先设置迭代轨迹点和迭代弦高误差阈值，然后在每个轨迹点处用一条B样条曲线进行桥接，其次计算桥接曲线与原始轨迹的轨迹点误差是否满足用户要求，若满足要求，插值结束，导出插值曲线；若不满足要求，根据轨迹点误差更新迭代轨迹点和迭代弦高误差，重新桥接，不断迭代，直到轨迹点误差满足要求，插值结束；机器人轨迹点的插值具体包括如下步骤：

步骤一：初始迭代参数决定，从MOVEB运动指令中获取一组机器人原始轨迹点弦高误差阈值D和轨迹点误差阈值E；迭代次数记为k＝0，计算初始迭代轨迹点和初始迭代弦高误差阈值

步骤二：首先，建立每个内部轨迹点(不包括首尾端点)P_i,i＝1,…N-1处的p次桥接B样条曲线，在整个步骤二中，为方便描述，省略迭代次数k，即等同于P_i，等同于d_i：

该桥接B样条曲线共有2p+1个控制点，分别为A_1,i，A_2,i…A_p,i,Q_i,B_p,i,…B_2,i,B_1,i，节点矢量为{0,0.5,1}，三个节点的重复度分别为p+1,p,p+1；

然后，为计算方便，使桥接B样条曲线处处G²或G³连续，建立在Q_i对称的B样条曲线，将该桥接B样条曲线转化为由两条对称的p次Bezier曲线组成，两条Bezier曲线的控制点分别为A_1,i,A_2,i…A_p,i,Q_i和Q_i,B_p,i,…B_2,i,B_1,i，其中前p个控制点共线，后p个控制点共线，Q_i为A_p,iB_p,i的中点，控制点共线保证了桥接B样条曲线在首尾拼接处的G²(G³)连续性，但仍需构造Q_i点处的G²(G³)连续性。

如图4～图5所示，圆点为轨迹点，五角星表示的点为桥接曲线的控制点，在图4中，三次B样条桥接曲线由七个控制点组成，等价为两条对称的三次Bezier曲线。在图5中，三次B样条桥接曲线由九个控制点组成，等价为两条对称的四次Bezier曲线。

然后，建立目标函数和约束条件：两条Bezier曲线组成的G²和G³桥接曲线在交点Q_i处曲率最大，且最大曲率分别为和推导出最大曲率与h_i成反比，为得到更平缓的桥接曲线，要求k_g2max和k_g3max越小，因此将目标函数建立为使所有的h_i和最大，从而得到更平缓的桥接曲线。在G²插值中，可取β_i＝0.5；在G³插值中，可知α_i与曲率极值成正比，α_i的取值范围是α_i≥0.7,因此在实际计算中，可取α_i＝0.7，已知0<β_i≤0.5，因此可取β_i＝0.5。

本发明将目标函数定义为求解桥接参数h_i,i＝1,2,…N-1的和最大问题，即如公式(1)所示。为实现保形约束，如图6所示，要求两个轨迹点之间的桥接曲线不存在重合部分，通过建立以下三个不等式(2)、(3)、(4)使桥接曲线保形；为实现弦高误差约束，只需控制P_iQ_i的距离满足不大于迭代弦高误差阈值d_i即可，建立不等式(5)；以上四个式子均可转化为对h_i约束的不等式。

最后，求解最优化问题(1)使桥接曲线参数满足约束条件式(2)、(3)、(4)、(5)约束条件，求解出桥接曲线参数，即得到迭代轨迹点的桥接轨迹。该优化问题为线性规划问题，在求解过程中可采用一维搜索方法。h₁的初值由弦高误差约束(5)和保形约束(3)决定；h_N的初值由弦高误差约束(5)和保形约束(4)决定；对于i＝2,…N-1，首先根据式(5)中弦高误差阈值约束，取初值然后判断初值是否满足式(2)的保形条件，若满足，桥接成功；若不满足，缩小h_i直到式(2)满足；

步骤三：计算桥接轨迹与原始轨迹点的轨迹点误差，判断是否满足轨迹点误差阈值需求，轨迹点误差的计算方法为计算轨迹点与两条Bezier曲线交点的距离：

Δ_k是所有轨迹点误差的最大值，已知本发明构造的桥接曲线由两条对称的Bezier曲线得到，如图4所示，位于的角平分线上，因此可以通过计算的大小和角平分线向量得到。

若Δ_k>轨迹点误差阈值E，说明桥接曲线未满足轨迹点误差要求，进入步骤四更新迭代轨迹点和迭代弦高误差阈值；否则，插值成功，导出插值轨迹；

步骤四：更新迭代参数，图7演示了迭代轨迹点的更新过程，令 调整后的迭代轨迹点为：迭代弦高误差阈值为：更新迭代次数：k＝k+1，转步骤二。

通过上述方法得到的机器人轨迹点插值曲线(插值轨迹)满足数据点误差和弦高误差、保形且曲线平缓，具有高连续性，并且由于迭代求解简单稳定收敛，能够实时的运用到机器人控制***的插值过程中。利用三次B样条曲线插值保证处处G²连续，即速度和加速度连续；利用四次B样条曲线插值保证处处G³连续，即速度、加速度连续和加加速度连续。

<3>机器人姿态的插值

本发明机器人运动轨迹的插值不仅包括轨迹点的插值，还包括姿态的插值。本发明采用与轨迹点插值类似的方法对姿态插值，姿态的插值是指对每一个轨迹点的工具坐标系的插值，得到分段G²(G³)连续的姿态插值曲线。已知工具坐标系可由Z轴和X轴决定，Y轴可以根据Z轴和X轴叉乘算出。

本发明将姿态的插值转化为每个轨迹点的工具坐标系的Z轴和X轴的插值，即插值Z轴和X轴上距离轨迹点固定距离的偏移点的插值。首先分别计算Z轴和X轴上距离轨迹点固定长度的两组偏移点，然后通过与轨迹点类似的插值方法分别对两组偏移点插值，最后经过单位化和正交化处理得到分段G²(G³)连续的插值姿态曲线。为了得到插值后的坐标系，需要对轨迹点和偏移点组成的向量单位化。同时插值后的Z轴和X轴不一定正交，因此插值后需要做正交化处理。

机器人姿态的插值具体包括如下步骤：

步骤一：计算偏移点，将轨迹点分别沿X轴和Z轴偏移固定距离m,n得到一系列点，分别记为X轴偏移点和Z轴偏移点，如图8所示，黑色圆点为六个轨迹点； 为六个Z轴偏移点，同理可计算X轴偏移点；该步骤中偏移距离m,n的取值不应过小或过大，可设置为轨迹点平均间距的5～10倍；

步骤二：对X轴偏移点和Z轴偏移点插值，采用轨迹点插值的方法分别对X轴偏移点和Z轴偏移点进行插值，得到分段插值曲线O(t),R(t)，如图8所示，插值后的分段曲线为R(t)＝{R_i(t)},i＝1…13，其中黑色五角星表示分段点，每个R_i(t),i＝1…13都满足G²(G³)连续；

步骤三：计算插值后的轨迹的姿态。为了计算插值轨迹上每点的姿态，需要计算一组单位正交的X,Z轴矢量，上述步骤可得到插值后的X,Z轴矢量：X(t)＝O(t)-P(t),Z(t)＝R(t)-P(t)，但轨迹的姿态需为单位正交的坐标系，因此对X、Z轴矢量单位正交化处理，得到插值轨迹的姿态曲线。由于弦高误差的存在，单位正交化之后的姿态曲线在分段点内部姿态G²(G³)连续，在分段点处不一定G²(G³)连续。但至少具有G⁰连续。

<4>生成机器人插值后的运动轨迹

通过上述步骤<2>获取的轨迹点插值曲线和步骤<3>获取的机器人姿态曲线共同得到机器人插值后的运动轨迹，插值后的运动轨迹光顺、满足数据点误差和弦高误差要求、保形并能够实时插值。既实现了轨迹点的光顺插值，又实现了姿态的分段光顺插值，使得工人机器人沿光顺插值轨迹稳定高质高效的作业。

本发明并不仅仅限于说明书和实施方式中所列运用，对于本领域的技术人员来说，可根据本发明作出各种相应的更改和变型，而所有这些相应的更改和变型都属于本发明权利要求的保护范围。

Claims (7)

1.一种可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，包括如下步骤：

S1、生成MOVEB运动指令：MOVEB运动指令用来描述工业机器人运动轨迹，包括轨迹点和姿态，以及用户输入的轨迹点误差阈值和弦高误差阈值；

S2、机器人轨迹点的插值：将MOVEB运动指令表示的工业机器人运动轨迹导入机器人控制***，利用高阶B样条插值算法，将高阶B样条曲线按照轨迹点误差和弦高误差阈值对轨迹点进行插值，分别实现机器人轨迹点的G²插值和G³插值，得到具有高连续性并且保证满足轨迹点误差和弦高误差要求的插值轨迹；

S3、机器人姿态的插值：通过B样条插值算法分别实现机器人姿态的分段G²和G³插值，并对插值后的机器人轨迹的姿态进行单位正交化处理，得到分段G²和G³连续的机器人姿态曲线；

S4、生成机器人插值后的运动轨迹：通过上述步骤S2获取的轨迹点插值曲线和步骤S3获取的机器人姿态曲线共同得到机器人插值后的运动轨迹。

2.根据权利要求1所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S1中MOVEB运动指令的数据内容包括X,Y,Z,I,J,K,U,V,W,D,E共十一个变量，其中(X,Y,Z)表示工具坐标系下的轨迹点的坐标，(I,J,K)表示上述轨迹点处建立的工具坐标系的Z轴单位矢量，(U,V,W)表示工具坐标系的X轴单位矢量，D,E分别表示弦高误差阈值和轨迹点误差阈值。

3.根据权利要求1所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S2及步骤S3中，机器人控制***解析MOVEB运动指令时，将轨迹点误差阈值和弦高误差阈值相同的一段连续轨迹组成一组进行插值。

4.根据权利要求2所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S2中，机器人轨迹点的插值具体包括如下步骤：

步骤一：初始迭代参数决定，从MOVEB运动指令中获取一组机器人原始轨迹点N≥2、弦高误差阈值D和轨迹点误差阈值E；迭代次数记为k＝0，计算初始迭代轨迹点和初始迭代弦高误差阈值

步骤二：具体包括：

①建立每个内部轨迹点处的桥接B样条曲线，在整个步骤二中，省略迭代次数k，即等同于P_i，等同于d_i：

桥接B样条曲线的次数为p,共有2p+1个控制点，分别为 A_1,i，A_2,i…A_p,i,Q_i,B_p,i,…B_2,i,B_1,i,节点矢量为{0,0.5,1},三个节点的重复度分别为p+1,p,p+1；该桥接B样条曲线等价于两条对称的p次Bezier曲线；

②建立在Q_i对称的两条p次Bezier曲线，两条Bezier曲线的控制点分别为A_1,i,A_2,i…A_p,i,Q_i和Q_i,B_p,i,…B_2,i,B_1,i，其中A_1,i，A_2,i…A_p,i与P_i-1P_i共线，B_p,i,…B_2,i,B_1,i与P_iP_i+1共线，Q_i为A_p,iB_p,i的中点；

③分G²和G³插值两种情形给出保证桥接B样条曲线处处G²或G³连续的控制点：

设

其中T_0,i为P_i-1P_i的单位向量；T_1,i为的A_p,iB_p,i单位向量；h_i为线段A_p,iP_i的长度；θ_i为向量T_0,i和T_1,i的夹角；

i)在G²插值时，G²桥接曲线的控制顶点满足以下条件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_3,i＝Q_i-h_icosθ_iT_1,i

A_2,i＝A_3,i-h_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

ii)在G³插值时，G³桥接曲线的控制顶点满足以下条件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_4,i＝Q_i-h_icosθ_iT_1,i

A_3,i＝A_4,i-α_ih_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

α_i≥0.7,0<β_i≤0.5

在G²插值时，h_i,β_i为两个桥接曲线参数；在G³插值时,h_i,α_i,β_i为三个桥接曲线参数，桥接曲线参数决定了桥接曲线控制点的位置，在以下步骤⑤中通过目标函数最优化求解得到，B_1,i,B_2,i,…,B_p,i由桥接B样条曲线的对称性得到；

④以桥接曲线更平缓为目标建立优化目标函数，并给出保形和弦高误差控制的约束条件：

两条Bezier曲线构造的G²桥接曲线和G³桥接曲线在交点Q_i处的曲率最大，且最大曲率分别为和为得到更平缓的桥接曲线，要求k_g2max和k_g3max越小，即要求步骤③中桥接曲线参数h_i越大，因此将目标函数定义为求解桥接曲线参数h_i,i＝1,2,…N-1的和最大问题，即如下式(1)所示：

为实现保形约束，要求两个轨迹点之间的桥接曲线不存在重合部分，其中G²连续和G³连续的桥接保形约束为：

|P_i-1B_1,i-1|+|A_1,iP_i|≤|P_i-1P_i|,i＝2,…N-1 (2)

|A_1,1P₁|≤|P₀P₁| (3)

|P_N-1B_1,N-1|≤|P_N-1P_N| (4)

为实现弦高误差约束，只需控制P_iQ_i的距离不大于迭代弦高误差阈值d_i即可，即：

|P_iQ_i|≤d_i (5)

⑤求解最优化问题即式(1)，同时要求桥接曲线参数满足约束条件式(2)、(3)、(4)、(5)，求解出桥接曲线参数，即得到迭代轨迹点的桥接轨迹；

步骤三：计算桥接轨迹与原始轨迹点的轨迹点误差最大值Δ_k，并判断Δ_k是否大于轨迹点误差阈值E，轨迹点误差的计算方法为计算轨迹点与两条Bezier曲线交点的距离，本步骤三中加k上标参数与步骤二中未加k上标参数等同，为描述第k次迭代的误差检测，未省略k：

其中位于的角平分线上，根据插值曲线的对称性，通过计算的大小和角平分线向量得到，计算方法为：

若Δ_k>E，说明桥接曲线未满足轨迹点误差要求，进入步骤四更新迭代轨迹点和迭代弦高误差阈值；否则，插值成功，退出迭代，导出插值轨迹；

步骤四：更新迭代参数，令迭代轨迹点的调整向量为：调整后的迭代轨迹点为：迭代弦高误差阈值为：更新迭代次数：k＝k+1，转步骤二。

5.根据权利要求1所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S3中，机器人姿态的插值具体包括如下步骤：

步骤一：计算偏移点，记处的工具坐标系的X轴单位矢量为Z轴单位矢量为将沿X轴偏移一段固定距离m，得到X轴偏移点记为将沿Z轴偏移一段固定距离n，得到Z轴偏移点记为

步骤二：对X轴偏移点和Z轴偏移点插值，采用轨迹点插值的方法分别对X轴偏移点、Z轴偏移点进行插值，得到分段插值曲线O (t),R(t)；

步骤三：计算插值后的轨迹的姿态，记机器人轨迹点的分段插值曲线为P(t)，插值后的X轴矢量：X(t)＝O(t)-P(t)，Z轴矢量：Z(t)＝R(t)-P(t)，对X轴矢量、Z轴矢量单位正交化处理，进一步计算得到插值后单位正交的X轴单位矢量、Z轴单位矢量：

插值后的X轴单位矢量X^new(t)为：

插值后的Z轴单位矢量Z^new(t)为：

Z^new(t)＝F(t)×X^new(t)×F(t)

其中

根据以上得到的X^new(t)，Z^new(t)描述插值后的姿态曲线，得到每个轨迹插值曲线上轨迹点对应的姿态信息。

6.根据权利要求5所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S3的步骤一中，偏移距离m，n的取值设置为轨迹点平均间距的5～10倍。

7.根据权利要求5所述的可控误差的工业机器人光顺运动轨迹生成方法，其特征在于，所述步骤S3的步骤三中，单位正交化之后的姿态分段曲线具有分段G²或G³连续性，即在分段点之间的曲线G²或G³连续，在分段点处至少具有G⁰连续性。