[发明专利]一种可控误差的工业机器人光顺运动轨迹生成方法

权利要求书

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，转步骤二。