CN113276116B - 一种误差可控的机器人轨迹同步过渡方法 - Google Patents

Abstract

本发明提供一种误差可控的机器人轨迹同步过渡方法，采用多维轨迹点表示多种机器人轨迹，并建立多维轨迹的统一运算规则和和多维曲线，基于多维轨迹点和多维运算建立基于凸组合表示的机器人轨迹的高连续同步过渡方法，该方法可针对不同的情况扩展、客制化，实现不同类型机器人及不同连续性要求的机器人轨迹过渡，实现了机器人轨迹的高连续性同步过渡，能够提高多种工业机器人轨迹的精度和效率，减少作业时的振动。

Description

一种误差可控的机器人轨迹同步过渡方法

技术领域

本发明属于工业机器人轨迹优化领域，具体涉及一种误差可控的机器人轨迹同步过渡方法。

背景技术

六关节工业机器人或SCARA(Selective Compliance Assembly Robot Arm)机器人的运动模式主要有三种：线性运动、关节运动和圆弧运动，其中线性运动最常用于复杂且精度要求较高的机器人应用中。但是线性轨迹存在切向和曲率不连续的问题，将会导致机器人运动过程中的降速和振动，最终导致效率和精度的降低。虽然高档数控机床中提供了提高精度和降低振动的方案，但是工业机器人的稳定性比数控机床差，控制系统也不如数控机床成熟。目前工业机器人领域尚无既保证精度又降低振动的解决方案。

轨迹平顺是避免振动、提高效率和精度的有效控制技术之一。当前工业机器人领域的轨迹平顺技术可以分为局部拐角过渡法(如CNT指令)和局部样条插值法(如库卡的SPLINE指令)，但目前很多商用工业机器人的局部拐角过渡功能未提供误差控制，或者不具备位置和姿态的参数同步性，进而导致轨迹曲线的几何形状受速度或加速度影响。

申请号为CN201911300865.9的发明专利提出了一种位姿同步的六轴工业机器人轨迹平顺方法，采用圆弧曲线对位置轨迹过渡，并采用四元数B样条对姿态过渡，但该过渡方法只能满足G1连续的机器人轨迹过渡，过渡方法不具有可扩展性。因此，本发明将提供一种扩展性好、简单有效、误差可控、轨迹保型且适用于多种类型机器人轨迹的过渡方法。

发明内容

针对现有技术存在的问题，本发明拟提供一种误差可控的机器人轨迹同步过渡方法，其目的在于解决当前机器人轨迹平顺时精度和效率低的问题，并减少机器人作业时的振动问题。具体技术方案如下：

一种误差可控的机器人轨迹同步过渡方法，其特征在于，包括如下步骤：

步骤1、通过定义多维轨迹点来表示多种机器人的位置和姿态轨迹，该多维轨迹点能够同时表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹；

步骤2、基于多维轨迹点的定义，建立多维轨迹点的统一运算规则和多维曲线，多维轨迹点的多维运算包括多维距离、多维加法、多维数乘和多维减法；

步骤3、基于多维轨迹点和多维运算建立基于凸组合表示的机器人轨迹的高连续同步过渡方法，该方法可针对不同的情况扩展、客制化，实现不同类型机器人及不同连续性要求的机器人轨迹过渡，同步过渡可采用圆弧、抛物线、B样条等曲线作为过渡曲线，不同过渡类型的区别仅在于基函数的不同；

设一系列机器人线性轨迹点表示为

其中Q_i为多维轨迹点，遍历i＝1，2，...M-1，基于凸组合的Q_i点处的过渡轨迹表示为：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n

其中n＝3，4，7，式中的加法、数乘和减法均表示多维轨迹点的多维运算，F_i(u)和K_i(u)为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到；

步骤4、基于步骤3中的机器人轨迹过渡方法实现位置和姿态轨迹的过渡误差控制和过渡轨迹保型控制：首先采用过渡轨迹的参数中点C_i(u＝0.5)来描述过渡误差，设β_i＝F_i(0.5)，γ_i＝K_i(0.5)为两个过渡参数，过渡轨迹C_i(u)的参数中点R_i表示为：

R_i＝((Q_i+(β_i(Q_i-1-Q_i)_n)_n)_n+((γ_i(Q_i+1-Q_i)_n)_n)_n

过渡参数β_i，γ_i根据R_i和Q_i之间的过渡误差阈值反算得到，这两个过渡参数将唯一确定过渡曲线的几何形状，基于位置和姿态误差控制的拐角过渡表示为以下目标函数：

maxβ_i，s.t.D₃(R_i，Q_i)≤ε_max

D_θ(R_i，Q_i)≤o_max

其中D₃(R_i，Q_i)表示两个多维点R_i和Q_i之间的位置距离，D_θ(R_i，Q_i)表示两个多维点R_i和Q_i之间的姿态夹角距离，ε_max表示位置距离阈值，o_max表示姿态夹角距离阈值，s.t.为“subject to”的简写，意思是满足以下条件；

过渡轨迹的保型控制是指相邻两个拐角的过渡曲线不存在交叉，通过对β_i进行上界约束实现过渡轨迹的保型控制，过渡参数β_i越大对应的曲率极值越小，轨迹越平顺，该目标函数为二次规划问题，首先根据位置对称性条件计算满足位置误差的β_i，1，再根据姿态对称性条件计算满足姿态误差的β_i，2，同时考虑保型约束的上界β_i，3，取三者之中的最小值作为β_i的取值，参数γ_i根据对称性条件对应求解，计算出过渡参数β_i和γ_i后，根据过渡参数β_i，γ_i和r_i，1，r_i，2之间的固定参数关系，对应计算r_i，1和r_i，2，进而构造出机器人轨迹的过渡曲线。

所述步骤1中多维轨迹点表示为P_i∈Rⁿ，n＝3，4，7，当n＝3时，P_i(x_i，y_i，z_i)表示三维位置点，当n＝4时，P_i(x_i，y_i，z_i，θ_i)表示SCARA机器人的位置和姿态点，θ_i表示SCARA机器人的姿态采用轴角法表示时的旋转角，当n＝7时，P_i(x_i，y_i，z_i，q_i)表示六关节机器人的位置和姿态点，其中q_i＝(ｑ_s，i，ｑ_x，i，q_ｙ，i，q_z，i)是六关节机器人姿态的四元数表示。

所述步骤2中多维距离是指两个多维轨迹点之间的抽象距离，当多维轨迹点为3D点时，该距离指两个位置点之间的三维距离；当多维轨迹点为4D点或7D点时，多维距离为两点之间的三维位置距离和姿态夹角距离组成的二维向量；

设两个多维轨迹点P_i-1，P_i∈Rⁿ，n＝3，4，7，定义两点之间的多维距离运算D_n(P_i-1，P_i)，其中三维距离D₃(P_i-1，P_i)表示为：

SCARA机器人和六关节机器人的多维距离表示为：

D_n(P_i-1，P_i)＝(D₃(P_i-1，P_i)，D_θ(P_i-1，P_i))，

其中

|θ_i-θ_i-1|表示两个旋转角的差的绝对值，angle(q_i-1，q_i)表示从四元数q_i-1表示的姿态旋转到q_i表示的姿态所旋转的角度。

多维轨迹点的多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，设两个多维轨迹点P_i-1，P_i∈Rⁿ，n＝3，4，7，定义多维加法运算(P_i-1+P_i)_n，其中3D点和4D点的加法与三维线性空间的向量加法相同，即：(P_i-1+P_i)₃＝(x_i+x_i-1，y_i+y_i-1，z_i+z_i-1)；(P_i-1+P_i)₄＝((P_i-1+P_i)₃，θ_i+θ_i-1)；7D向量的加法运算是3D点的数乘和四元数的指数运算的组合，即(P_i-1+P_i)₇＝((P_i-1+P_i)₃，q_i-1q_i)，其中q_i-1q_i表示四元数q_i-1和q_i的乘法：设q_i-1＝[s，v]，q_i＝[s′，v′]，四元数的乘法表示为：q_i-1q_i＝[ss′-v·v′，v×v′+sv′+s′v]，其中s为一个一维变量，v为一个三维向量。

一个多维向量和一个常数的数乘运算可表示为位置数乘和姿态数乘的组合：其中3D点和4D点的数乘与三维线性空间的向量数乘相同，即：m(P_i)₃＝(mx_i，my_i，mz_i)，m(P_i)₄＝(mx_i，my_i，mz_i，mθ_i)，m为用于数乘的常数；

7D向量的数乘运算是3D点的数乘和四元数的指数运算的组合，即：

为四元数的指数运算。

类似三维线性空间的减法，多维减法可采用多维加法和多维数乘表示，即：(P_i-P_i-1)_n＝((-P_i-1)_n+P_i)_n。

基于以上多维运算建立位置和姿态几何同步的多维线段和多维B样条曲线。

所述多维线段表示两个多维轨迹点之间的线性插值，其中3D点和4D点采用线性空间的线性插值，7D点采用三维线性空间的线性插值和旋转空间(姿态)的球面线性插值(SLERP)组合得到，设P_i-1，P_i∈Rⁿ，n＝3，4，7为两个多维轨迹点，两点之间的多维线段Lerp_n(P_i-1，P_i，u)采用公式表示为：

Lerp_n(P_i-1，P_i，u)＝(P_i-1+(u(P_i-P_i-1)_n)_n)_n，u∈[0，1]

上式中的加法、数乘和减法均表示多维轨迹点的多维运算。

所述多维B样条曲线与多维线段的定义类似，其中四维B样条曲线与三维B样条曲线的定义一致，七维B样条曲线由位置的三维B样条曲线和姿态的四元数B样条曲线组成，多维B样条曲线的具体定义如下：

给定控制顶点

节点向量U，次数k，多维B样条曲线Bsp_n(P，U，k，u)可表示为：

其中：ΔP_i＝(P_i-P_i-1)_n，n＝3，4，7，i＝1，2，..N，

为累积B样条基函数累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数

表示为：

累积B样条基函数表示如下：

所述步骤3中以多维机器人轨迹的G2连续过渡为例，采用5个控制点的三次B样条对线性轨迹

进行拐角过渡，设三次多维B样条曲线C_i(u)具有5个多维控制点

E_i，j为多维轨迹点，节点向量为U＝[0，0，0，0，0.5，1，1，1，1]，当E_i，0，E_i，1位于多维线段Q_i-1Q_i上，E_i，3，E_i，4位于多维线段Q_iQ_i+1上，且E_i，2＝Q_i时，该多维B样条可实现G2连续过渡轨迹的表达，其中控制点

的分布和基函数分别表示为：

其中｛B_0，3(u)～B_4，3(u)}为具有5个控制点的三次B样条的基函数，r_i，1，r_i，2为两个比例参数；

所述步骤4中的轨迹过渡曲线的构造过程为根据r_i，1和r_i，2计算出5个控制点

然后根据控制点

和基函数F_i(u)，K_i(u)，采用凸组合过渡轨迹公式：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n构造过渡曲线。

本发明具有如下优点：

1、本发明提出的机器人轨迹过渡方法具有多轨迹适用性：能够同时适用于三维位置轨迹、SCARA机器人位姿轨迹、及六关节机器人位姿轨迹的几何平顺；

2、提出的多维轨迹点的定义、多维运算、以及多维轨迹过渡方法将位置与姿态统一处理，建立统一的计算框架，能够涵盖任意过渡曲线格式和任意连续性要求，可以很容易针对不同的情况扩展、客制化。

3、本发明所述方法生成的机器人平顺轨迹，其位置和姿态都具有高连续性和过渡误差可控，适用于多种类型工业机器人的高精度轨迹平顺。

附图说明

图1为四元数姿态线性插值示意图；

图2为五个控制点描述的三次四元数B样条轨迹示意图；

图3为六关节机器人位置轨迹过渡示意图；

图4为六关节机器人姿态轨迹过渡示意图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

一种针对多种机器人轨迹的误差可控的同步过渡方法，包括：

(1)本发明定义多维轨迹点来统一表示多种机器人的位置和姿态轨迹，该多维轨迹点能够同时表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹。

多维轨迹点可表示为P_i∈Rⁿ，n＝3，4，7，当n＝3时，P_i(x_i，y_i，z_i)表示三维位置点，当n＝4时，P_i(x_i，y_i，z_i，θ_i)表示SCARA机器人的位置和姿态点，由于SCARA机器人的姿态旋转轴固定，只有旋转角度改变，因此采用θ_i表示SCARA机器人姿态的旋转角，当n＝7时，P_i(x_i，y_i，z_i，q_i)表示六关节机器人的位置和姿态点，其中q_i为姿态的四元数表示：q_i＝(q_s，i，q_x，i，q_y，i，q_z，i)。

(2)本发明建立了多维轨迹点的统一运算规则和多维曲线的定义，多维运算包括多维距离、多维加法、多维数乘和多维减法，多维运算将三维欧式空间的基本运算和旋转空间的基本运算统一集合为多维轨迹点的运算。

多维距离是指两个多维轨迹点之间的抽象距离，设两个多维轨迹点P_i-1，P_i∈Rⁿ，n＝3，4，7，定义两点之间的多维距离运算D_n(P_i-1，P_i)，其中三维距离D₃(P_i-1，P_i)表示为：

SCARA机器人和六关节机器人的多维距离表示为：

D_n(P_i-1，P_i)＝(D₃(P_i-1，P_i)，D_θ(P_i-1，P_i))，

其中

|θ_i-θ_i-1|表示两个旋转角的差的绝对值，angle(q_i-1，q_i)表示从四元数q_i-1表示的姿态旋转到q_i表示的姿态所旋转的角度。

多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，设P_i-1，P_i∈Rⁿ，n＝3，4，7。定义多维加法运算(P_i-1+P_i)_n，其中3D点和4D点的加法与三维线性空间的向量加法相同，即：(P_i-1+P_i)₃＝(x_i+x_i-1，y_i+y_i-1，z_i+z_i-1)；(P_i-1+P_i)₄＝((P_i-1+P_i)₃，θ_i+θ_i-1)。

7D向量的加法运算是3D点的数乘和四元数的指数运算的组合，即(P_i-1+P_i)₇＝((P_i-1+P_i)₃，q_i-1q_i)，其中q_i-1q_i表示四元数q_i-1和q_i的乘法：设q_i-1＝[s，v]，q_i＝[s′，v′]，四元数的乘法表示为：q_i-1q_i＝[ss′-v·v′，v×v′+sv′+s′v]，其中s为一个一维变量，v为一个三维向量。

一个多维向量和一个常数的数乘运算可表示为位置数乘和姿态数乘的组合：其中3D点和4D点的数乘与三维线性空间的向量数乘相同，即：m(P_i)₃＝(mx_i，my_i，mz_i)，m(P_i)₄＝(mx_i，my_i，mz_i，mθ_i)，m为用于数乘的常数；

7D向量的数乘运算是3D点的数乘和四元数的指数运算的组合，即：

为四元数的指数运算。

类似三维线性空间的减法，多维减法可采用多维加法和多维数乘表示，即(P_i-P_i-1)_n＝((-P_i-1)_n+P_i)_n。

基于以上多维运算可建立位置和姿态几何同步的多维线段和多维B样条曲线。

其中多维线段表示两个多维轨迹点之间的线性插值，其中3D点和4D点采用线性空间的线性插值，7D点采用三维线性空间的线性插值和旋转空间(姿态)的球面线性插值(SLERP)组合得到，采用公式表示为：设P_i-1，P_i∈Rⁿ，n＝3，4，7为两个多维轨迹点，两点之间的多维线段Lerp_n(P_i-1，P_i，u)可表示为：

Lerp_n(P_i-1，P_i，u)＝(P_i-1+(u(P_i-P_i-1)_n)_n)_n，u∈[0，1]

图1为旋转空间的线性插值示意图，其中q_i-1，q_i，q_i+1表示多维轨迹点P_i-1，P_i，P_i+1对应的四元数姿态，两个四元数姿态的线性插值为单位球上的一条曲线。

多维B样条曲线与多维线段的定义类似，其中四维B样条曲线与三维B样条曲线的定义一致，七维B样条曲线由位置的三维B样条曲线和姿态的四元数B样条曲线组成。多维B样条曲线的具体定义如下：

给定控制顶点

n＝3，4，7，节点向量U，次数k，多维B样条曲线Bsp_n(P，U，k，u)可表示为：

其中：ΔP_i＝(P_i-P_i-1)_n，n＝3，4，7，i＝1，2，..N，

为累积B样条基函数，累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数

表示为：

累积B样条基函数可表示如下：

图2为旋转空间的四元数B样条曲线示意图，其中q_i-2～q_i+2表示5个四元数控制点，虚线轨迹为每两个控制点之间的球面线性插值轨迹，实线轨迹为四元数B样条曲线。

(3)基于多维轨迹点和多维运算的定义，本发明提出了基于凸组合表示的机器人轨迹的高连续同步过渡方法，该方法可很容易针对不同的情况扩展、客制化，如采用圆弧、抛物线、B样条等作为过渡曲线，不同过渡曲线的区别仅在于基函数的不同。

设一系列机器人线性轨迹点表示为

其中Q_i为多维轨迹点，遍历i＝1，2，...M-1，基于凸组合的Q_i点处的过渡轨迹可表示为：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n

其中n＝3，4，7，式中的加法、数乘和减法均表示多维轨迹点的多维运算，F_i(u)和K_i(u)称为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到。

以机器人轨迹的G2连续过渡为例，采用5个控制点的三次B样条对Q_i点进行过渡，设三次多维B样条曲线C_i(u)具有5个多维控制点

E_i，j为多维轨迹点，节点向量为U＝[0，0，0，0，0.5，1，1，1，1]，当E_i，1，E_i，2位于多维线段Q_i-1Q_i上，E_i，4，E_i，5位于多维线段Q_iQ_i+1上，且E_i，3＝Q_i时，该多维B样条可实现G2连续过渡轨迹的表达。其中控制点

的分布和基函数分别表示为：

其中{B_0，3(u)～B_4，3(u)}为具有5个控制点的三次B样条的基函数，r_i，1，r_i，2为两个比例参数。

图3和图4分别为六关节机器人位置和姿态的轨迹过渡示意图，设六关节机器人轨迹点表示为Q_i(p_i，q_i)，其中p_i表示三维位置点，q_i表示四元数姿态，在图3中，线性轨迹在p_i点处的拐角过渡轨迹由5个控制点{e_i，1～e_i，5}组成的三次B样条曲线描述，在图4中，姿态q_i点处的拐角过渡轨迹由5个控制点{f_i，1～f_i，5}组成的三次四元数B样条曲线描述。

(4)本发明提出的机器人轨迹过渡方法可实现位置和姿态轨迹的误差控制，采用过渡轨迹的参数中点R_i＝C_i(u＝0.5)来描述过渡误差，设β_i＝F_i(0.5)，γ_i＝K_i(0.5)为两个过渡参数，过渡轨迹C_i(u)的参数中点R_i表示为：

R_i＝((Q_i+(β_i(Q_i-1-Q_i)_n)_n)_n+((γ_i(Q_i+1-Q_i)_n)_n)_n

过渡参数β_i，γ_i和r_i，1，r_i，2之间存在固定参数关系，在第三部分所述案例中，

特殊的，我们可以通过构造对称的过渡位置轨迹来简化过渡参数，位置轨迹对称的含义是|e_i，2-e_i，3|＝|e_i，4-e_i，3|，通过该约束可建立β_i和γ_i之间的线性关系。

过渡参数β_i，γ_i根据R_i和Q_i之间的过渡误差阈值反算得到，这两个过渡参数将唯一确定过渡曲线的几何形状，基于位置和姿态误差控制的拐角过渡表示为以下目标函数：

maxβ_i，s.t.D₃(R_i，Q_i)≤ε_max

D_θ(R_i，Q_i)≤o_max

其中D₃(R_i，Q_i)表示两个多维点R_i和Q_i之间的位置距离，D_θ(R_i，Q_i)表示两个多维点R_i和Q_i之间的姿态夹角距离，ε_max表示位置距离阈值，o_max表示姿态夹角距离阈值，s.t.为“subject to”的简写，意思是满足以下条件；另外，以上目标函数描述的是一个拐角处的过渡轨迹构造，为了实现整条线性轨迹多个拐角处的过渡，需考虑相邻拐角处的保型约束，即保证相邻两条过渡曲线不会出现交叉，特殊的，该保型约束只需保证r_i，1，r_i，2∈(0，0.5]即可。

过渡参数β_i越大对应的曲率极值越小，轨迹越平顺，该目标函数为二次规划问题，可首先根据位置对称性条件计算满足位置误差的β_i，1，再根据姿态对称性条件计算满足姿态误差的β_i，2，最后根据保型约束计算出满足保型需求的β_ｉ，3，取三者之中的最小值作为β_i的取值。参数γ_i可根据对称性条件对应求解。

计算出过渡参数β_i和γ_i后，可对应计算r_i，1和r_i，2，根据r_i，1和r_i，2可以计算出5个控制点

然后根据控制点

和基函数F_i(u)，K_i(u)，采用上文凸组合的公式：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n构造出机器人轨迹的过渡曲线。

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (9)

1.一种误差可控的机器人轨迹同步过渡方法，其特征在于，包括如下步骤：

步骤1、通过定义多维轨迹点来表示多种机器人的位置和姿态轨迹，该多维轨迹点能够同时表示三维位置轨迹、SCARA机器人的位姿轨迹和六关节机器人的位姿轨迹；

步骤2、基于多维轨迹点的定义，建立多维轨迹点的统一运算规则和多维曲线，多维轨迹点的多维运算包括多维距离、多维加法、多维数乘和多维减法，基于多维运算建立位置和姿态几何同步的多维线段和多维B样条曲线；

步骤3、基于多维轨迹点和多维运算建立基于凸组合表示的机器人轨迹的高连续同步过渡方法，同步过渡采用圆弧、抛物线或B样条曲线作为过渡曲线，不同过渡类型的区别仅在于基函数的不同；

设一系列机器人线性轨迹点表示为

其中Q_i为多维轨迹点，遍历i＝1，2，...M-1，基于凸组合的Q_i点处的过渡轨迹表示为：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n

其中u为曲线的参数，u∈[0，1]，n＝3，4，7，式中的加法、数乘和减法均表示多维轨迹点的多维运算，F_i(u)和K_i(u)为过渡曲线的基函数，基函数的具体表示根据采用的过渡曲线类型和连续性条件推导得到；

步骤4、基于步骤3中的机器人轨迹过渡方法实现位置和姿态轨迹的过渡误差控制和过渡轨迹保型控制：首先采用过渡轨迹的参数中点C_i(u＝0.5)来描述过渡误差，设β_i＝F_i(0.5)，γ_i＝K_i(0.5)为两个过渡参数，过渡轨迹C_i(u)的参数中点R_i表示为：

R_i＝((Q_i+(β_i(Q_i-1-Q_i)_n)_n)_n+((γ_i(Q_i+1-Q_i)_n)_n)_n

过渡参数β_i，γ_i根据R_i和Q_i之间的过渡误差阈值反算得到，这两个过渡参数将唯一确定过渡曲线的几何形状，基于位置和姿态误差控制的拐角过渡表示为以下目标函数：

maxβ_i，s.t.D₃(R_i，Q_i)≤ε_max

D_θ(R_i，Q_i)≤o_max

其中D₃(R_i，Q_i)表示两个多维点R_i和Q_i之间的位置距离，D_θ(R_i，Q_i)表示两个多维点R_i和Q_i之间的姿态夹角距离，ε_max表示位置距离阈值，o_max表示姿态夹角距离阈值；

过渡轨迹的保型控制是指相邻两个拐角的过渡曲线不存在交叉，通过对β_i进行上界约束实现过渡轨迹的保型控制，过渡参数β_i越大对应的曲率极值越小，轨迹越平顺，首先根据位置对称性条件计算满足位置误差的β_i，1，再根据姿态对称性条件计算满足姿态误差的β_i，2，同时考虑保型约束的上界β_i，3，取三者之中的最小值作为β_i的取值，参数γ_i根据对称性条件对应求解，计算出过渡参数β_i和γ_i后，根据过渡参数β_i，γ_i和r_i，1，r_i，2之间的固定参数关系，对应计算r_i，1和r_i，2，进而构造出机器人轨迹的过渡曲线，r_i，1和r_i，2分别表示：第i段过渡轨迹的控制点的两个比例参数。

2.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤1中多维轨迹点表示为P_i∈Rⁿ，n＝3，4，7，当n＝3时，P_i(x_i，y_i，z_i)表示三维位置点，当n＝4时，P_i(x_i，y_i，z_i，θ_i)表示SCARA机器人的位置和姿态点，θ_i表示SCARA机器人的姿态采用轴角法表示时的旋转角，当n＝7时，P_i(x_i，y_i，z_i，q_i)表示六关节机器人的位置和姿态点，其中q_i＝(q_s，i，q_x，i，q_y，i，q_z，i)是六关节机器人姿态的四元数表示。

3.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维距离是指两个多维轨迹点之间的抽象距离，当多维轨迹点为3D点时，该距离指两个位置点之间的三维距离；当多维轨迹点为4D点或7D点时，多维距离为两点之间的三维位置距离和姿态夹角距离组成的二维向量；

设两个多维轨迹点P_i-1，P_i∈Rⁿ，n＝3，4，7，定义两点之间的多维距离运算D_n(P_i-1，P_i)，其中三维距离D₃(P_i-1，P_i)表示为：

SCARA机器人和六关节机器人的多维距离表示为：

D_n(P_i-1，P_i)＝(D₃(P_i-1，P_i)，D_θ(P_i-1，P_i))，其中

|θ_i-θ_i-1|表示两个旋转角的差的绝对值，angle(q_i-1，q_i)表示从四元数q_i-1表示的姿态旋转到q_i表示的姿态所旋转的角度。

4.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维加法是三维线性空间位置加法和三维旋转空间姿态加法的组合，设两个多维轨迹点P_i-1，P_i∈Rⁿ，n＝3，4，7，定义多维加法运算(P_i-1+P_i)_n，其中3D点和4D点的加法与三维线性空间的向量加法相同，即：(P_i-1+P_i)₃＝(x_i+x_i-1，y_i+y_i-1，z_i+z_i-1)；(P_i-1+P_i)₄＝((P_i-1+P_i)₃，θ_i+θ_i-1)；7D向量的加法运算是3D点的数乘和四元数的指数运算的组合，即(P_i-1+P_i)₇＝((P_i-1+P_i)₃，q_i-1q_i)，其中q_i-1q_i表示四元数q_i-1和q_i的乘法：设q_i-1＝[s，v]，q_i＝[s′，v′]，四元数的乘法表示为：q_i-1q_i＝[ss′-v·v′，v×v′+sv′+s′v]，其中s为一个一维变量，v为一个三维向量。

5.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维数乘是一个多维向量和一个常数的数乘运算，为位置数乘和姿态数乘的组合，其中3D点和4D点的数乘与三维线性空间的向量数乘相同，即：m(P_i)₃＝(mx_i，my_i，mz_i)，m(P_i)₄＝(mx_i，my_i，mz_i，mθ_i)，m为用于数乘的常数；7D向量的数乘运算是3D点的数乘和四元数的指数运算的组合，即：

为四元数的指数运算。

6.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维减法采用多维加法和多维数乘表示，即：(P_i-P_i-1)_n＝((-P_i-1)_n+P_i)_n。

7.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维线段表示两个多维轨迹点之间的线性插值，其中3D点和4D点采用线性空间的线性插值，7D点采用三维线性空间的线性插值和旋转空间的球面线性插值组合得到，设P_i-1，P_i∈Rⁿ，n＝3，4，7为两个多维轨迹点，两点之间的多维线段Lerp_n(P_i-1，P_i，u)采用公式表示为：Lerp_n(P_i-1，P_i，u)＝(P_i-1+(u(P_i-P_i-1)_n)_n)_n，u∈[0，1]，式中的加法、数乘和减法均表示多维轨迹点的多维运算。

8.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤2中多维B样条曲线，其中四维B样条曲线与三维B样条曲线的定义一致，七维B样条曲线由位置的三维B样条曲线和姿态的四元数B样条曲线组成，多维B样条曲线的具体定义如下：给定控制顶点

n＝3，4，7，节点向量U，次数k，多维B样条曲线Bsp_n(P，U，k，u)表示为：

其中：

为累积B样条基函数，累积B样条基函数由普通B样条基函数变换得到，其中普通B样条基函数

表示为：

累积B样条基函数表示如下：

9.如权利要求1所述的一种误差可控的机器人轨迹同步过渡方法，其特征在于：所述步骤3中以多维机器人轨迹的G2连续过渡为例，采用5个控制点的三次B样条对线性轨迹

进行拐角过渡，设三次多维B样条曲线C_i(u)具有5个多维控制点

E_i，j为多维轨迹点，节点向量为U＝[0，0，0，0，0.5，1，1，1，1]，当E_i，0，E_i，1位于多维线段Q_i-1Q_i上，E_i，3，E_i，4位于多维线段Q_iQ_i+1上，且E_i，2＝Q_i时，该多维B样条实现对G2连续过渡轨迹的表达，其中控制点

的分布和基函数分别表示为：

其中{B_0，3(u)～B_4，3(u)}为具有5个控制点的三次B样条的基函数，r_i，1，r_i，2为两个比例参数；

所述步骤4中的轨迹过渡曲线的构造过程为根据r_i，1和r_i，2计算出5个控制点

然后根据控制点

和基函数F_i(u)，K_i(u)，采用凸组合过渡轨迹公式：

C_i(u)＝((Q_i+(F_i(u)(Q_i-1-Q_i)_n)_n)_n+(K_i(u)(Q_i+1-Q_i)_n)_n)_n构造过渡曲线。

Images