CN105171188B - 一种用于大型构件空间曲线轨迹焊接的运动控制方法 - Google Patents

Abstract

一种用于大型构件空间曲线轨迹焊接的运动控制方法，属于焊接自动化领域。该发明在焊接过程中使用三维平移机构调整焊炬位置，使用双自由度旋转机构调整焊炬姿态，并根据焊炬与待焊工件的相对位姿实时调整焊接能量输入参数，在任意空间曲线轨迹焊接中实现了焊接速度、焊炬倾角、焊炬末端与待焊点距离可预设且焊接过程中保持恒定等要求，保证焊接过程的稳定性和产品质量的一致性。系统结构简单，成本低，适于任意空间曲线轨迹焊缝电弧焊、激光焊、搅拌摩擦焊等多种焊接场合。

Description

一种用于大型构件空间曲线轨迹焊接的运动控制方法

技术领域

本发明属于焊接自动化领域，特别涉及一种用于大型构件空间曲线轨迹焊接的运动控制方法。

背景技术

大型构件空间曲线轨迹焊接常现于航天航空、船舶制造、石油化工等领域的设备制造过程。为获得良好的焊接质量，往往要求满足以下若干目标：其一，焊接速度可在焊前预设，且在焊接过程中保持恒定；其二，在焊接过程中待焊点与焊炬末端点的距离保持恒定，且在焊前可预设，在电弧焊中表现为弧长恒定，在激光焊中表现为离焦量恒定，在搅拌摩擦焊中表现为搅拌头插入深度恒定；其三，在焊接过程中焊炬轴线与待焊点法向可保持恒定的预设倾角，如在电弧焊中常需保持焊炬轴线垂直于工件表面，在搅拌摩擦焊中常需保持轴肩和工件表面成一定角度以施加一定的顶锻压力；其四，考虑到设备自重导致的精确连续驱动问题，往往不允许工件在焊接过程中运动，只能允许焊炬沿待焊轨迹运动。目前，空间曲线轨迹焊接大多采用人工焊接方式，难以保证焊缝质量的稳定性和一致性。

中国专利《一种沿立面内任意曲线轨迹焊接的机器人控制方法》(专利号：201210488690.0)提出了用于平面二维曲线焊接的一种三轴联动装置和控制方法，待焊轨迹使用轨迹上的若干离散点表征，在焊接过程中对轨迹上的离散点进行圆弧插补，使得焊接过程中满足焊接速度恒定、焊炬末端与工件表面距离不变、始终保持平焊位置等若干目标。但该方法仅适用于平面二维曲线轨迹焊接，且无法适用于要求焊炬存在一定前倾角或后倾角的场合，如搅拌摩擦焊过程要求轴肩与工件表面成一定夹角以给焊缝提供一定的顶锻压力；另外，在该方法中工件需要实时变速旋转，在大型构件焊接场合对设备的精确连续驱动要求极高。

综上，目前尚未有满足焊接速度、待焊点与焊炬末端点距离、焊炬倾角等参数均可焊前预设且焊接过程中保持恒定的、用于大型构件任意空间曲线轨迹焊接的运动控制方法。

发明内容

本发明的目的是针对已有技术的不足之处，提出一种用于大型构件空间曲线轨迹焊接的运动控制方法，该方法旨在解决目前技术存在的无法满足焊接速度、弧长/离焦量/搅拌头插入深度、焊炬倾角等均可预设且焊接过程保持恒定等问题，以求满足焊接速度、待焊点与焊炬末端点距离、焊炬倾角等参数均可焊前预设且焊接过程中保持恒定等技术要求，并在焊接过程中根据焊接姿态实时调整焊接能量输入参数，并在焊接过程中根据焊炬、待焊工件和世界坐标系的相对位姿关系实时调整焊接能量输入参数，保持焊接过程的稳定性和产品质量的一致性。

本发明的技术方案如下：

一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于，该方法采用的装置包括底座、运动控制器、焊接能量源、焊炬、三维平移机构和双自由度旋转机构；所述底座与所述三维平移机构机械连接，所述双自由度旋转机构安装在所述三维平移机构的运动输出端，焊炬安装在所述双自由度旋转机构的运动输出端；或所述底座与所述双自由度旋转机构机械连接，所述三维平移机构安装在所述双自由度旋转机构的运动输出端，焊炬安装在所述三维平移机构的运动输出端；所述运动控制器分别与所述三维平移机构和所述双自由度旋转机构通过导线连接，或通过无线传输方式通讯；所述焊炬与所述焊接能量源通过导线连接，或通过光路连接；所述三维平移机构包括第一一维平移机构、第二一维平移机构和第三一维平移机构；所述第一一维平移机构、所述第二一维平移机构和所述第三一维平移机构的运动方向相互正交；所述双自由度旋转机构包括第一旋转机构和第二旋转机构；所述第一旋转机构和所述第二旋转机构的旋转轴相互正交；待焊工件安装在底座上；

所述方法包括以下步骤：

1)建立与所述待焊工件固结的世界坐标系{W}；建立与所述焊炬固结的焊炬坐标系{P}，所述焊炬坐标系{P}的原点O_p与所述焊炬末端点重合，z_p轴与所述焊炬轴线重合；初始时刻所述焊炬坐标系{P}的z_p轴方向与世界坐标系{W}的z_w轴方向相互平行；

2)在待焊轨迹上自起点至终点测量N个离散空间点的三维坐标以及各点处的单位法向量，其中N为大于或等于2的正整数，记第i个离散空间点在所述世界坐标系{W}中的三维坐标为X_i，第i个离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标为m_i，其中i是小于或等于N的正整数，X_i和m_i均为三维列向量；

3)设焊炬轴线与待焊轨迹的交点为待焊点；在焊接前，预先设定焊接速度C、焊炬末端点与待焊点之间的有向距离h以及焊炬倾角α，其中C为任意不等于零的实数，h、α为任意实数；

4)对所述离散空间点在所述世界坐标系{W}中的三维坐标X_i和所述离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标m_i进行曲线插补，包括以下步骤：

a)对X_i进行B样条曲线插补，使得插补的样条曲线X_w(u)满足X_w(u_i)＝X_i，其中u为

样条曲线X_w(u)的自变量，且：

$u_i=\left\{\begin{array}{cc}0,& i=1\\ \frac{\underset{k=1}{\overset{i-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|}{\underset{k=1}{\overset{N-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|},& 2\≤i\≤N\end{array}\right.$

b)计算所述样条曲线X_w(u)的一阶导数s_w(u)：

$s_w\left(u\right)=\frac{{\mathrm{dX}}_w\left(u\right)}{du}$

c)计算：

$r_{w,i}=\frac{m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)\left|\right|}\×\frac{s_w\left(u_i\right)}{\left|\right|s_w\left(u_i\right)\left|\right|}$

对r_w,i进行B样条曲线插补，使得插补的样条曲线r_w(u)满足r_w(u_i)＝r_w,i；

d)计算法向量插补函数n_w(u)＝s_w(u)×r_w(u)；

5)采用焊接能量源提供焊接时的能量输入，并使所述运动控制器发出控制信号，驱动所述三维平移机构和所述双自由度旋转机构联合运动；设当所述第一旋转机构的旋转角为θ，且所述第二旋转机构的旋转角为γ，且所述三维平移机构的位移量为G时，所述焊炬坐标系{P}和所述世界坐标系{W}的旋转转换矩阵和平移转换矩阵分别为R(θ,γ)和T(θ,γ,G)，其中θ和γ为任意实数，G为任意三维列向量；

设t为任意非负实数；

在t时刻，运动控制器驱动所述第一旋转机构和所述第二旋转机构运动，使所述第一旋转机构的旋转角θ(t)和所述第二旋转机构的旋转角γ(t)满足：

R(θ(t),γ(t))·e₃＝l_w(t)

式中，e₃为所述世界坐标系{W}的z_w轴的单位方向向量，l_w(t)由下式确定：

$l_w\left(t\right)=-\frac{s_w\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}\sin \mathrm{\α}+\frac{n_w\left(u\right(t))}{\left|\right|n_w\left(u\right(t)\left)\right||}cos\mathrm{\α}$

式中，u(t)由下式确定：

${\∫}_0^{u\left(t\right)}\[\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|-\frac{hcos\mathrm{\α}}{\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|\·\left|\right|n_w\left(\mathrm{\ξ}\right)\left|\right|}{n}_w^T\left(\mathrm{\ξ}\right)\frac{{\mathrm{ds}}_w\left(\mathrm{\ξ}\right)}{d\mathrm{\ξ}}\]d\mathrm{\ξ}=C\·t$

式中，ξ为积分变量；

在t时刻，运动控制器驱动所述第一旋转机构和所述第二旋转机构运动，使所述第一旋转机构的瞬时角速度和所述第二旋转机构的瞬时角速度满足：

$\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\]\left[\begin{array}{c}\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ \frac{d\mathrm{\γ}\left(t\right)}{dt}\end{array}\right]=\frac{{\mathrm{dl}}_w\left(t\right)}{dt}$

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_w\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\cos \mathrm{\α}\right\}\frac{du\left(t\right)}{dt}\\ \frac{du\left(t\right)}{dt}=\frac{C}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|-\frac{h\cos \mathrm{\α}}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|\·\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}{n}_w^T\left(u\left(t\right)\right)\·\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}}\\ \frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_w\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_w\left(t\right)}{du}-\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)|{|}^3}{s}_w^T\left(u\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}\\ \frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_w\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(t\right)}{du}-\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)|{|}^3}{n}_w^T\left(u\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}\end{array}\right.$

在t时刻，运动控制器驱动所述三维平移机构运动，使所述三维平移机构的位移量G(t)满足：

T(θ(t),γ(t),G(t))＝X_w(u(t))+h·l_w(t)

在t时刻，运动控制器驱动所述三维平移机构运动，使所述三维平移机构的瞬时速度满足：

$\begin{array}{c}\frac{dG\left(t\right)}{dt}={\[\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\]}^{-1}\[s_w\left(u\left(t\right)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}\\ -\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}-\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\]\end{array}$

根据所述焊炬、待焊工件和世界坐标系{W}的相对位姿关系，实时调整所述焊接能量源的能量输入参数；

一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：所述焊接能量源为电弧焊焊接电源、激光焊热源或搅拌摩擦焊运动驱动器；

一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：该方法采用的装置还包括送丝机构和送丝机构控制器；所述送丝机构控制器与所述送丝机构相连；所述送丝机构末端与所述焊炬相连；使送丝机构控制器发出控制信号，控制送丝机构在焊接过程进行送丝；

一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：所述运动控制器为电机控制器或液压控制器。

与已有技术相比，本发明可以在焊接过程中实现以下目标：焊接速度、焊炬倾角、焊炬末端点与待焊点距离均可预先设定，且在焊接过程中保持恒定；系统结构简单，成本低，适于大型构件任意空间曲线轨迹焊缝电弧焊、激光焊、搅拌摩擦焊等多种焊接场合。

附图说明

图1为本发明提出的一种用于大型构件空间曲线轨迹焊接的运动控制方法实施例所采用装置的轴侧图。

图2为图1所示装置的正视图。

图3为图1所示装置的侧视图。

图4为图1所示装置的俯视图。

图5为本发明提出的一种用于大型构件空间曲线轨迹焊接的运动控制方法实施例的流程图。

图6为本发明实施例中第一旋转机构和第二旋转机构的旋转角随时间的变化规律。

图7为本发明实施例中第一旋转机构和第二旋转机构的瞬时角速度随时间的变化规律。

图8为本发明实施例中第一一维平移机构、第二一维平移机构和第三一维平移机构的位移量随时间的变化规律。

图9为本发明实施例中第一一维平移机构、第二一维平移机构和第三一维平移机构的瞬时速度随时间的变化规律。

在图1至图9中：

1—底座；2—运动控制器；3—焊接能量源；4—焊炬；5—三维平移机构；51—第一一维平移机构；52—第二一维平移机构；53—第三一维平移机构；6—双自由度旋转机构；61—第一旋转机构；62—第二旋转机构；7—待焊工件；71—待焊轨迹；

{W}—世界坐标系；O_w，x_w，y_w，z_w—世界坐标系{W}的原点、横轴、纵轴和竖轴；

{P}—焊炬坐标系；O_p，x_p，y_p，z_p—焊炬坐标系{P}的原点、横轴、纵轴和竖轴；

t—时间；

X(t)—t时刻第一一维平移机构的位移量；

时刻第一一维平移机构的瞬时速度；

Y(t)—t时刻第二一维平移机构的位移量；

时刻第二一维平移机构的瞬时速度；

Z(t)—t时刻第三一维平移机构的位移量；

时刻第三一维平移机构的瞬时速度；

θ(t)—t时刻第一旋转机构的旋转角；

时刻第一旋转机构的瞬时角速度；

γ(t)—t时刻第二旋转机构的旋转角；

时刻第二旋转机构的瞬时角速度；

C—焊接速度；

α—焊炬倾角；

h—焊炬末端点与待焊点之间的有向距离。

具体实施方式

下面结合附图对本发明提出的一种用于大型构件空间曲线轨迹焊接的运动控制方法原理作进一步说明。

图1为本发明提出的一种用于大型构件空间曲线轨迹焊接的运动控制方法实施例所采用装置的轴侧图，图2、图3和图4分别为图1所示装置的正视图、侧视图和俯视图，该装置包括底座1、运动控制器2、焊接能量源3、焊炬4、三维平移机构5和双自由度旋转机构6；所述底座1与所述三维平移机构5机械连接，所述双自由度旋转机构6安装在所述三维平移机构5的运动输出端，焊炬4安装在所述双自由度旋转机构6的运动输出端；所述运动控制器2为电机控制器，分别与所述三维平移机构5和所述双自由度旋转机构6通过导线连接；所述运动控制器2驱动所述三维平移机构5和所述双自由度旋转机构6运动；所述焊接能量源3为钨极氩弧焊焊接电源，提供焊接过程的能量输入；所述焊炬4与所述焊接能量源3通过导线连接；所述三维平移机构5包括第一一维平移机构51、第二一维平移机构52和第三一维平移机构53；所述第一一维平移机构51、所述第二一维平移机构52和所述第三一维平移机构53均采用滚珠丝杆机构，所述滚珠丝杆机构由电机驱动；所述第一一维平移机构51、所述第二一维平移机构52和所述第三一维平移机构53的运动方向相互正交；所述双自由度旋转机构6包括第一旋转机构61和第二旋转机构62；所述第一旋转机构61和所述第二旋转机构62由电机和减速器组成；所述第一旋转机构61和所述第二旋转机构62的旋转轴相互正交；待焊工件7安装在底座1上。待焊轨迹71为一相贯线轨迹，设形成相贯线焊缝的两个圆管分为为第一圆管和第二圆管。

建立与待焊工件7固结的世界坐标系{W}，所述世界坐标系{W}的y_w轴方向与第一圆管轴线方向重合，z_w轴方向与第二圆管轴线方向重合；建立与焊炬4固结的焊炬坐标系{P}，所述焊炬坐标系{P}的原点O_p与焊炬4末端点重合，z_p轴与焊炬4轴线重合；初始时刻所述焊炬坐标系{P}的z_p轴方向与世界坐标系{W}的z_w轴方向相互平行，y_p轴方向与第二一维平移机构52的运动方向相互平行。

在待焊轨迹71上自起点至终点测量N个离散空间点的三维坐标以及各点处的单位法向量，其中N为大于或等于2的正整数，记第i个离散空间点在世界坐标系{W}中的三维坐标为X_i，第i个离散空间点处的单位法向量在世界坐标系{W}中的三维坐标为m_i，其中i是小于或等于N的正整数，X_i和m_i均为三维列向量。X_i和m_i可使用三坐标测量仪获得，也可利用工件CAD模型导入。

设焊炬4轴线与待焊轨迹71的交点为待焊点；在焊接前，预先设定焊接速度C、焊炬4末端点与待焊点之间的有向距离h以及焊炬倾角α，其中C为任意不等于零的实数，h、α为任意实数。

为实现连续轨迹焊接，首先必须对离散空间点进行曲线插补。在本发明中，选取B样条曲线对X_i进行插补，设插补的B样条曲线方程为：

$X_w\left(u\right)=\underset{\mathrm{\σ}=1}{\overset{N}{\Σ}}{D}_{\mathrm{\σ}}\·{\mathrm{\Γ}}_{\mathrm{\σ},q}\left(u\right),u\∈\[0,1\]---\left(1\right)$

式中，u为B样条曲线X_w(u)的自变量，D_σ为B样条曲线X_p(u)的控制点坐标，D_σ是三维列向量，σ为任意不大于N的正整数，Γ_σ,q(u)为q-1阶B样条曲线的基函数，q是任意正整数。

在本实施例中采用具有重节点的B样条曲线进行插值，B样条曲线X_w(u)的节点值分别为：

其中，β₁,β₂,...,β_N-1-q为待求的节点值。

除需确定B样条曲线X_w(u)的节点值外，还需确定点X_i处B样条曲线X_w(u)的自变量取值u_i。在本实施例中，采用累积弦长法确定u_i的值：

$u_i=\left\{\begin{array}{cc}0,& i=1\\ \frac{\underset{k=1}{\overset{i-1}{\mathrm{\Σ}}}\left|\right|X_{k+1}-X_k\left|\right|}{\underset{k=1}{\overset{N-1}{\mathrm{\Σ}}}\left|\right|X_{k+1}-X_k\left|\right|},& 2\≤i\≤N\end{array}\right.---\left(3\right)$

B样条曲线X_w(u)的节点值β₁,β₂,...,β_N-1-q用下式确定：

${\mathrm{\β}}_{\mathrm{\η}}=\frac{1}{q}\underset{k=\mathrm{\η}+1}{\overset{\mathrm{\η}+q}{\Σ}}{u}_k---\left(4\right)$

式中，η为任意不大于N-1-q的正整数。

结合式(1)至式(4)可获得控制点D_σ满足的方程：

$\underset{\mathrm{\σ}=1}{\overset{N}{\Σ}}{\mathrm{\Γ}}_{\mathrm{\σ},q}\left(u_i\right)\·D_{\mathrm{\σ}}=X_i---\left(5\right)$

根据式(3)和式(4)确定B样条曲线的节点值，并根据式(5)计算控制点坐标，完成对X_i的B样条曲线插补，获得插补曲线方程X_w(u)。根据曲线方程X_w(u)可计算待焊轨迹71上任意一点处的切向量在世界坐标系{W}中的坐标s_w(u)：

$s_w\left(u\right)=\frac{{\mathrm{dX}}_w\left(u\right)}{du}---\left(6\right)$

然而，根据式(6)计算获得的离散空间点处的切向量在世界坐标系{W}中的坐标s_w(u_i)不一定与离散空间点处的单位法向量在世界坐标系{W}中的三维坐标m_i垂直。为保证以下理论推导的正确性，需对m_i进行修正，计算B样条曲线X_w(u)上任意一点处的法向量。假设B样条曲线X_w(u)在u＝u_i处的单位法向量在世界坐标系{W}中的坐标n_w,i与向量s_w(u_i)、m_i共面，则可令：

n_w,i＝λ_is_w(u_i)+μ_im_i (7)

式中，λ_i和μ_i为待定系数。根据n_w,i与s_w(u_i)垂直，可得：

[s_w(u_i)]^Tn_w,i＝λ_i[s_w(u_i)]^Ts_w(u_i)+μ_i[s_w(u_i)]^Tm_i＝0 (8)

即：

$\frac{{\mathrm{\λ}}_i}{{\mathrm{\μ}}_i}=-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}---\left(9\right)$

因此：

$n_{w,i}=\frac{m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)\left|\right|}---\left(10\right)$

修正后的法向量n_w,i一般与m_i之间的偏差较小，该偏差对实际焊炬倾角控制造成的影响可忽略不计。除了计算出u＝u_i处的单位法向量在世界坐标系{W}中的坐标n_w,i，还需对B样条曲线X_w(u)上任意一点的法向量进行曲线插补。此时，必须要求法向量的插补方程n_w(u)处处垂直于s_w(u)，因此可令：

n_w(u)＝s_w(u)×r_w(u) (11)

式中，r_w(u)为待求的插补函数。根据向量外积的性质，n_w(u)处处垂直于s_w(u)。在u＝u_i处，r_w(u)的取值为：

$r_{w,i}=r_w\left(u_r\right)=n_{w,i}\×\frac{s_w\left(u_i\right)}{\left|\right|s_w\left(u_i\right)\left|\right|}=\frac{m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)\left|\right|}\×\frac{s_w\left(u_i\right)}{\left|\right|s_w\left(u_i\right)\left|\right|}---\left(12\right)$

函数r_w(u)可由B样条曲线插补获得，其控制点在世界坐标系{W}中的坐标F_σ满足：

$\underset{\mathrm{\σ}=1}{\overset{N}{\Σ}}{\mathrm{\Γ}}_{\mathrm{\σ},q}\left(u_i\right)\·F_{\mathrm{\σ}}=r_{w,i}---\left(13\right)$

根据式(10)至式(13)即可完成对法向量的插补，获得n_w(u)。

在完成焊接参数预设和B样条曲线插补后，焊接能量源3提供焊接时的能量输入，且运动控制器2发出控制信号，驱动三维平移机构5和双自由度旋转机构6联合运动。此时，必须计算第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的位移量和瞬时速度、以及第一旋转机构61和第二旋转机构62的旋转角和瞬时角速度等参数随时间的变化规律，使得在整个焊接过程中满足焊接速度、焊炬4末端点与待焊点之间的有向距离、焊炬倾角等参数保持恒定等要求。

由几何关系可知，当第一旋转机构61的旋转角为θ，且第二旋转机构62的旋转角为γ，且三维平移机构5的位移量为G时，其中三维平移机构5的位移量定义为第一旋转机构61和第二旋转机构62的旋转轴交点相对于世界坐标系{W}的坐标，焊炬坐标系{P}和世界坐标系{W}的旋转转换矩阵R(θ,γ)和平移转换矩阵T(θ,γ,G)分别等于：

$R(\mathrm{\θ},\mathrm{\γ})=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& \sin \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\sin \mathrm{\θ}\cos \mathrm{\γ}& \sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(14\right)$

$T(\mathrm{\θ},\mathrm{\γ},G)=R(\mathrm{\θ},\mathrm{\γ})\left[\begin{array}{c}{a}_1\\ 0\\ -a_2\end{array}\right]+G---\left(15\right)$

其中，θ和γ为任意实数，G为任意三维列向量，在本实施例中a₁和a₂均为常数，a₁＝35mm，a₂＝265mm。

根据式(14)和式(15)可得：

$\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}=\left[\begin{array}{ccc}-sin\mathrm{\θ}& 0& cos\mathrm{\θ}\\ cos\mathrm{\θ}\sin \mathrm{\γ}& 0& sin\mathrm{\θ}\sin \mathrm{\γ}\\ -cos\mathrm{\θ}\cos \mathrm{\γ}& 0& -sin\mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(16\right)$

$\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}=\left[\begin{array}{ccc}0& 0& 0\\ sin\mathrm{\θ}cos\mathrm{\γ}& -\sin \mathrm{\γ}& -cos\mathrm{\θ}cos\mathrm{\γ}\\ sin\mathrm{\θ}sin\mathrm{\γ}& \cos \mathrm{\γ}& -cos\mathrm{\θ}sin\mathrm{\γ}\end{array}\right]---\left(17\right)$

$\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}=\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}\left[\begin{array}{c}{a}_1\\ 0\\ -a_2\end{array}\right]---\left(18\right)$

$\frac{\∂T(\mathrm{\θ},\mathrm{\γ},X)}{\∂\mathrm{\γ}}=\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}\left[\begin{array}{c}{a}_1\\ 0\\ -a_2\end{array}\right]---\left(19\right)$

$\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(20\right)$

设在t时刻，三维平移机构5的位移量为G(t)，第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的位移量分别为X(t)、Y(t)和Z(t)，第一旋转机构61和第二旋转机构62的旋转角分别为θ(t)和γ(t)。考虑到焊炬4末端点为焊炬坐标系{P}的原点，因此由坐标变换关系可得：

R(θ(t),γ(t))·0+T(θ(t),γ(t),G(t))＝X_w(u(t))+h·l_w(t) (21)

即：

T(θ(t),γ(t),G(t))＝X_w(u(t))+h·l_w(t) (22)

式中，l_w(t)为t时刻焊炬4轴线的单位方向向量在世界坐标系{W}中的坐标，u(t)为t时刻函数X_w(u)的自变量取值，X_w(u(t))为t时刻待焊点在世界坐标系{W}中的坐标，G(t)满足：

$G\left(t\right)=\left[\begin{array}{c}X\left(t\right)\\ Y\left(t\right)\\ Z\left(t\right)\end{array}\right]---\left(23\right)$

利用坐标变换关系，还可计算l_w(t)满足的关系式：

R(θ(t),γ(t))·e₃＝l_w(t) (24)

式中，e₃为世界坐标系{W}的z_w轴的单位方向向量。

根据焊炬倾角为α可计算t时刻焊炬4轴线的单位方向向量与待焊点处的切向量和法向量的关系：

$l_w\left(t\right)=-\frac{s_w\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}sin\mathrm{\α}+\frac{n_w\left(u\right(t))}{\left|\right|n_w\left(u\right(t)\left)\right||}cos\mathrm{\α}---\left(25\right)$

式(22)、式(24)和式(25)分别对时间t求导可计算三维平移机构5的瞬时速度第一旋转机构61的瞬时角速度和第二旋转机构62的瞬时角速度满足的关系：

$\begin{array}{c}\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}={|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{dG\left(t\right)}{dt}+\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ +\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}=s_w\left(u\left(t\right)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}\end{array}---\left(26\right)$

$\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\·\frac{d\mathrm{\θ}\left(t\right)}{dt}+\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\·\frac{d\mathrm{\γ}\left(t\right)}{dt}=\frac{{\mathrm{dl}}_w\left(t\right)}{dt}---\left(27\right)$

即：

$\begin{array}{c}\frac{dG\left(t\right)}{dt}={\[\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\]}^{-1}\[s_w\left(u\left(t\right)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}\\ -\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}-\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\]\end{array}---\left(28\right)$

$\[\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\]\left[\begin{array}{c}\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ \frac{d\mathrm{\γ}\left(t\right)}{dt}\end{array}\right]=\frac{{\mathrm{dl}}_w\left(t\right)}{dt}---\left(29\right)$

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_w\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{dt}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\cos \mathrm{\α}\right\}\frac{du\left(t\right)}{dt}\\ \frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_w\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_w\left(u\right)}{du}-\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)|{|}^3}{s}_w^T\left(u\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}\\ \frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_w\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(u\right)}{du}-\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)|{|}^3}{n}_w^T\left(u\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}\end{array}\right.---\left(30\right)$

只要获取u(t)的表达式，即可根据式(14)至式(30)计算第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的位移量和瞬时速度、以及第一旋转机构61和第二旋转机构62的旋转角和瞬时角速度等参数随时间的变化规律。u(t)可由焊接速度恒定条件计算获得。

焊炬4末端点的瞬时速度在待焊点处切向量的投影等于焊接速度，即：

$\frac{s_w^T\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}\frac{dT\left(\mathrm{\θ}\right(t),\mathrm{\γ}\left(t\right),G\left(t\right))}{dt}=C---\left(31\right)$

式中，可由式(22)计算获得：

$\frac{dT\left(\mathrm{\θ}\right(t),\mathrm{\γ}\left(t\right),G\left(t\right))}{dt}=s_w\left(u\left(t\right)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}---\left(32\right)$

将式(30)和式(32)代入式(31)：

$\left|\right|s_w\left(u\left(t\right)\right)\left|\right|\frac{du\left(t\right)}{dt}+h\·\frac{1}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|}\left[\begin{array}{cc}-\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{s}_w^T\left(u\left(t\right)\right)\frac{d}{du}\left(\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\right){|}_{u=u\left(t\right)}\\ s_w^T\left(u\left(t\right)\right)\frac{d}{du}\left(\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\right){|}_{u=u\left(t\right)}\end{array}\right]=C---\left(33\right)$

式中，

$\begin{array}{c}{s}_w^T\left(u\left(t\right)\right)\frac{d}{du}\left(\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\right){|}_{u=u\left(t\right)}\\ =s_w^T\left(u\left(t\right)\right)\[\frac{1}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|}\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}-\frac{s_w\left(u\left(t\right)\right)}{\left|\right|s_w\left(u\left(t\right)\right)|{|}^3}{s}_w^T\left(u\left(t\right)\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}\]\\ =\frac{1}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|}{s}_w^T\left(u\left(t\right)\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}-\frac{1}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|}{s}_w^T\left(u\left(t\right)\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}\\ =0\end{array}---\left(34\right)$

$\begin{array}{c}{s}_w^T\left(u\left(t\right)\right)\frac{d}{du}\left(\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\right){|}_{u=u\left(t\right)}\\ =s_w^T\left(u\left(t\right)\right)\[\frac{1}{\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}-\frac{n_w\left(u\left(t\right)\right)}{\left|\right|n_w\left(u\left(t\right)\right)|{|}^3}{n}_w^T\left(u\left(t\right)\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}\]\\ =\frac{s_w^T\left(u\left(t\right)\right)}{\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}\end{array}---\left(35\right)$

考虑到因此：

$s_w^T\left(u\left(t\right)\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}+n_w^T\left(u\left(t\right)\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}=0---\left(36\right)$

将式(34)至式(36)代入式(33)可得最终的焊接速度恒定方程：

$\[\left|\right|s_w\left(u\left(t\right)\right)\left|\right|-\frac{h\cos \mathrm{\α}}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|\·\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}{n}_w^T\left(u\left(t\right)\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}\]\frac{du\left(t\right)}{dt}=C---\left(37\right)$

即：

$\frac{du\left(t\right)}{dt}=\frac{C}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|-\frac{hcos\mathrm{\α}}{\left|\right|s_w\left(u\right(t)\left)\right||\·|\left|n_w\left(u\right(t)\right)\left|\right|}{n}_w^T\left(u\left(t\right)\right)\·\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}}---\left(38\right)$

式(38)微分方程的解为：

${\∫}_0^{u\left(t\right)}\[\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|-\frac{hcos\mathrm{\α}}{\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|\·\left|\right|n_w\left(\mathrm{\ξ}\right)\left|\right|}{n}_w^T\left(\mathrm{\ξ}\right)\frac{{\mathrm{ds}}_w\left(\mathrm{\ξ}\right)}{d\mathrm{\ξ}}\]d\mathrm{\ξ}=C\·t---\left(39\right)$

式中，ξ为积分变量。

综合以上分析结果，本发明提出的一种用于大型构件空间曲线轨迹焊接的运动控制方法实施例的流程图如图5所示，其包括以下步骤：

1)建立与所述待焊工件7固结的世界坐标系{W}；建立与所述焊炬4固结的焊炬坐标系{P}，所述焊炬坐标系{P}的原点O_p与所述焊炬4末端点重合，z_p轴与所述焊炬4轴线重合；初始时刻所述焊炬坐标系{P}的z_p轴方向与世界坐标系{W}的z_w轴方向相互平行；

2)在待焊轨迹71上自起点至终点测量N个离散空间点的三维坐标以及各点处的单位法向量，其中N为大于或等于2的正整数，记第i个离散空间点在所述世界坐标系{W}中的三维坐标为X_i，第i个离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标为m_i，其中i是小于或等于N的正整数，X_i和m_i均为三维列向量；

3)设焊炬4轴线与待焊轨迹71的交点为待焊点；在焊接前，预先设定焊接速度C、焊炬4末端点与待焊点之间的有向距离h以及焊炬倾角α，其中C为任意不等于零的实数，h、α为任意实数；

4)对所述离散空间点在所述世界坐标系{W}中的三维坐标X_i和所述离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标m_i进行曲线插补，包括以下步骤：

a)对X_i进行B样条曲线插补，使得插补的样条曲线X_w(u)满足X_w(u_i)＝X_i，其中u为

样条曲线X_w(u)的自变量，且：

$u_i=\left\{\begin{array}{cc}0,& i=1\\ \frac{\underset{k=1}{\overset{i-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|}{\underset{k=1}{\overset{N-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|},& 2\≤i\≤N\end{array}\right.---\left(40\right)$

b)计算所述样条曲线X_w(u)的一阶导数s_w(u)：

$s_w\left(u\right)=\frac{{\mathrm{dX}}_w\left(u\right)}{du}---\left(41\right)$

c)计算：

$r_{w,i}=\frac{m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)\left|\right|}\×\frac{s_w\left(u_i\right)}{\left|\right|s_w\left(u_i\right)\left|\right|}---\left(42\right)$

对r_w,i进行B样条曲线插补，使得插补的样条曲线r_w(u)满足r_w(u_i)＝r_w,i；

d)计算法向量插补函数n_w(u)＝s_w(u)×r_w(u)；

5)采用焊接能量3源提供焊接时的能量输入，并使所述运动控制器2发出控制信号，驱动所述三维平移机构5和所述双自由度旋转机构6联合运动；设当所述第一旋转机构61的旋转角为θ，且所述第二旋转机构62的旋转角为γ，且所述三维平移机构5的位移量为G时，所述焊炬坐标系{P}和所述世界坐标系{W}的旋转转换矩阵和平移转换矩阵分别为R(θ,γ)和T(θ,γ,G)，其中θ和γ为任意实数，G为任意三维列向量；

设t为任意非负实数；

在t时刻，运动控制器2驱动所述第一旋转机构61和所述第二旋转机构62运动，使所述第一旋转机构61的旋转角θ(t)和所述第二旋转机构62的旋转角γ(t)满足：

R(θ(t),γ(t))·e₃＝l_w(t) (43)

式中，e₃为所述世界坐标系{W}的z_w轴的单位方向向量，l_w(t)由下式确定：

$l_w\left(t\right)=-\frac{s_w\left(u\left(t\right)\right)}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|}\sin \mathrm{\α}+\frac{n_w\left(u\left(t\right)\right)}{\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}\cos \mathrm{\α}---\left(44\right)$

式中，u(t)由下式确定：

${\∫}_0^{u\left(t\right)}\[\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|-\frac{hcos\mathrm{\α}}{\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|\·\left|\right|n_w\left(\mathrm{\ξ}\right)\left|\right|}{n}_w^T\left(\mathrm{\ξ}\right)\frac{{\mathrm{ds}}_w\left(\mathrm{\ξ}\right)}{d\mathrm{\ξ}}\]d\mathrm{\ξ}=C\·t---\left(45\right)$

式中，ξ为积分变量；

在t时刻，运动控制器2驱动所述第一旋转机构61和所述第二旋转机构62运动，使所述第一旋转机构61的瞬时角速度和所述第二旋转机构62的瞬时角速度满足：

$\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\]\left[\begin{array}{c}\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ \frac{d\mathrm{\γ}\left(t\right)}{dt}\end{array}\right]=\frac{{\mathrm{dl}}_w\left(t\right)}{dt}---\left(46\right)$

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_w\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\cos \mathrm{\α}\right\}\frac{du\left(t\right)}{dt}\\ \frac{du\left(t\right)}{dt}=\frac{C}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|-\frac{h\cos \mathrm{\α}}{\left|\right|s_w\left(u\left(t\right)\right)\left|\right|\·\left|\right|n_w\left(u\left(t\right)\right)\left|\right|}{n}_w^T\left(u\left(t\right)\right)\·\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}}\\ \frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_w\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_w\left(t\right)}{du}-\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)|{|}^3}{s}_w^T\left(u\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}\\ \frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_w\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(t\right)}{du}-\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)|{|}^3}{n}_w^T\left(u\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}\end{array}\right.---\left(47\right)$

在t时刻，运动控制器2驱动所述三维平移机构5运动，使所述三维平移机构5的位移量G(t)满足：

T(θ(t),γ(t),G(t))＝X_w(u(t))+h·l_w(t) (48)

在t时刻，运动控制器2驱动所述三维平移机构5运动，使所述三维平移机构5的瞬时速度满足：

$\begin{array}{c}\frac{dG\left(t\right)}{dt}={\[\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\]}^{-1}\[s_w\left(u\left(t\right)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}\\ -\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}-\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\]\end{array}---\left(49\right)$

根据所述焊炬4、待焊工件7和世界坐标系{W}的相对位姿关系，实时调整所述焊接能量源3的能量输入参数，如焊接电流、焊接电压等。

在本发明的实施例中，待焊轨迹71在世界坐标系{W}中的方程为：

$\left\{\begin{array}{c}{x}^2+y^2={400}^2{\mathrm{mm}}^2\\ z^2+x^2={500}^2{\mathrm{mm}}^2\end{array}\right.---\left(50\right)$

第一圆管和第二圆管的厚度均为10mm。在本发明的实施例中，选取的离散空间点坐标为：

$X_i=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]---\left(51\right)$

式中，

$y_i=\left\{\begin{array}{c}-400+2(i-1),1\≤i\≤401\\ -398+2(800-i),402\≤i\≤800\end{array}\right.---\left(52\right)$

$x_i=\left\{\begin{array}{cc}\sqrt{{400}^2-y_i^2},& 1\≤i\≤401\\ -\sqrt{{400}^2-y_i^2},& 402\≤i\≤800\end{array}\right.---\left(53\right)$

$z_i=\sqrt{{500}^2-x_i^2}---\left(54\right)$

共选取N＝800个离散空间点。待焊轨迹71任意一点处的法线选取为垂直于待焊轨迹71的切向量、且与第一圆管和第二圆管的内相贯线相交的直线，离散空间点处的单位法向量在世界坐标系{W}中的三维坐标m_i等于为该离散空间点处的法线的单位方向向量。

在本发明的实施例中，焊接速度C＝6mm/s，焊炬4末端点与待焊点之间的有向距离h＝8mm，焊炬倾角α＝10°，采用的B样条曲线阶次q＝3。根据式(10)计算的修正法向量n_p,i与真实法向量m_i之间的角度偏差不超过0.004°。图6为本发明实施例中第一旋转机构61和第二旋转机构62的旋转角随时间的变化规律；图7为本发明实施例中第一旋转机构61和第二旋转机构62的瞬时角速度随时间的变化规律；图8为本发明实施例中第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的位移量随时间的变化规律；图9为本发明实施例中第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的瞬时速度随时间的变化规律。

应当说明的是，以上实施例仅用于说明本发明而并非限制本发明描述的方案；因此，尽管本说明书参照以上的实施例对本发明进行了详细的说明，但是本领域的普通技术人员应该理解，仍然可以对本发明进行修改或等同替换，如本发明的控制方法可适用于激光焊、搅拌摩擦焊等多种焊接方法、采用的装置还可包括送丝机构和送丝机构控制器、运动控制器可采用液压控制器、三维平移机构可采用悬臂式机构等；而一切不脱离本发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。

本发明在焊接过程中使用三维平移机构调整焊炬位置，使用双自由度旋转机构调整焊炬姿态，并根据焊炬、待焊工件和世界坐标系的相对位姿关系实时调整焊接能量输入参数，在任意空间曲线轨迹焊接中实现了焊接速度、焊炬倾角、焊炬末端与待焊点距离可预设且焊接过程中保持恒定等要求，保证焊接过程的稳定性和产品质量的一致性。系统结构简单，成本低，适于任意空间曲线轨迹焊缝电弧焊、激光焊、搅拌摩擦焊等多种焊接场合。

Claims (4)

1.一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于，该方法采用的装置包括底座(1)、运动控制器(2)、焊接能量源(3)、焊炬(4)、三维平移机构(5)和双自由度旋转机构(6)；所述底座(1)与所述三维平移机构(5)机械连接，所述双自由度旋转机构(6)安装在所述三维平移机构(5)的运动输出端，焊炬(4)安装在所述双自由度旋转机构(6)的运动输出端；或所述底座(1)与所述双自由度旋转机构(6)机械连接，所述三维平移机构(5)安装在所述双自由度旋转机构(6)的运动输出端，焊炬(4)安装在所述三维平移机构(5)的运动输出端；所述运动控制器(2)分别与所述三维平移机构(5)和所述双自由度旋转机构(6)通过导线连接，或通过无线传输方式通讯；所述焊炬(4)与所述焊接能量源(3)通过导线连接，或通过光路连接；所述三维平移机构(5)包括第一一维平移机构(51)、第二一维平移机构(52)和第三一维平移机构(53)；所述第一一维平移机构(51)、所述第二一维平移机构(52)和所述第三一维平移机构(53)的运动方向相互正交；所述双自由度旋转机构(6)包括第一旋转机构(61)和第二旋转机构(62)；所述第一旋转机构(61)和所述第二旋转机构(62)的旋转轴相互正交；待焊工件(7)安装在底座(1)上；

所述方法包括以下步骤：

1)建立与所述待焊工件固结的世界坐标系{W}；建立与所述焊炬固结的焊炬坐标系{P}，所述焊炬坐标系{P}的原点O_p与所述焊炬末端点重合，z_p轴与所述焊炬轴线重合；初始时刻所述焊炬坐标系{P}的z_p轴方向与世界坐标系{W}的z_w轴方向相互平行；

2)在待焊轨迹上自起点至终点测量N个离散空间点的三维坐标以及各点处的单位法向量，其中N为大于或等于2的正整数，记第i个离散空间点在所述世界坐标系{W}中的三维坐标为X_i，第i个离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标为m_i，其中i是小于或等于N的正整数，X_i和m_i均为三维列向量；

3)设焊炬轴线与待焊轨迹的交点为待焊点；在焊接前，预先设定焊接速度C、焊炬末端点与待焊点之间的有向距离h以及焊炬倾角α，其中C为任意不等于零的实数，h、α为任意实数；

4)对所述离散空间点在所述世界坐标系{W}中的三维坐标X_i和所述离散空间点处的单位法向量在所述世界坐标系{W}中的三维坐标m_i进行曲线插补，包括以下步骤：

a)对X_i进行B样条曲线插补，使得插补的样条曲线X_w(u)满足X_w(u_i)＝X_i，其中u为样条曲线X_w(u)的自变量，且：

$u_i=\left\{\begin{array}{cc}0,& i=1\\ \frac{\underset{k=1}{\overset{i-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|}{\underset{k=1}{\overset{N-1}{\Σ}}\left|\right|X_{k+1}-X_k\left|\right|},& 2\≤i\≤N\end{array}\right.$

b)计算所述样条曲线X_w(u)的一阶导数s_w(u)：

$s_w\left(u\right)=\frac{{\mathrm{dX}}_w\left(u\right)}{du}$

c)计算：

$r_{w,i}=\frac{m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_w\left(u_i\right)\]}^T{m}_i}{{\[s_w\left(u_i\right)\]}^T{s}_w\left(u_i\right)}{s}_w\left(u_i\right)\left|\right|}\×\frac{s_w\left(u_i\right)}{\left|\right|s_w\left(u_i\right)\left|\right|}$

对r_w,i进行B样条曲线插补，使得插补的样条曲线r_w(u)满足r_w(u_i)＝r_w,i；

d)计算法向量插补函数n_w(u)＝s_w(u)×r_w(u)；

5)采用焊接能量源提供焊接时的能量输入，并使所述运动控制器发出控制信号，驱动所述三维平移机构和所述双自由度旋转机构联合运动；设当所述第一旋转机构的旋转角为θ，且所述第二旋转机构的旋转角为γ，且所述三维平移机构的位移量为G时，所述焊炬坐标系{P}和所述世界坐标系{W}的旋转转换矩阵和平移转换矩阵分别为R(θ,γ)和T(θ,γ,G)，其中θ和γ为任意实数，G为任意三维列向量；

设t为任意非负实数；

在t时刻，运动控制器驱动所述第一旋转机构和所述第二旋转机构运动，使所述第一旋转机构的旋转角θ(t)和所述第二旋转机构的旋转角γ(t)满足：

R(θ(t),γ(t))·e₃＝l_w(t)

式中，e₃为所述世界坐标系{W}的z_w轴的单位方向向量，l_w(t)由下式确定：

$l_w\left(t\right)=-\frac{s_w\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}sin\mathrm{\α}+\frac{n_w\left(u\right(t))}{\left|\right|n_w\left(u\right(t)\left)\right||}cos\mathrm{\α}$

式中，u(t)由下式确定：

${\∫}_0^{u\left(t\right)}\[\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|-\frac{h\cos \mathrm{\α}}{\left|\right|s_w\left(\mathrm{\ξ}\right)\left|\right|\·\left|\right|n_w\left(\mathrm{\ξ}\right)\left|\right|}{n}_w^T\left(\mathrm{\ξ}\right)\frac{{\mathrm{ds}}_w\left(\mathrm{\ξ}\right)}{d\mathrm{\ξ}}\]d\mathrm{\ξ}=C\·t$

式中，ξ为积分变量；

在t时刻，运动控制器驱动所述第一旋转机构和所述第二旋转机构运动，使所述第一旋转机构的瞬时角速度和所述第二旋转机构的瞬时角速度满足：

$\[\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·e_3\]\left[\begin{array}{c}\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ \frac{d\mathrm{\γ}\left(t\right)}{dt}\end{array}\right]=\frac{{\mathrm{dl}}_w\left(t\right)}{dt}$

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_w\left(t\right)}{dt}=\{-\frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\cos \mathrm{\α}\}\frac{du\left(t\right)}{dt}\\ \frac{du\left(t\right)}{dt}=\frac{C}{\left|\right|s_w\left(u\right(t)\left)\right||-\frac{h\cos \mathrm{\α}}{\left|\right|s_w\left(u\right(t)\left)\right||\·|\left|n_w\left(u\right(t\left)\right)\right||}{n}_w^T\left(u\right(t\left)\right)\·\frac{{\mathrm{ds}}_w\left(u\right)}{du}{|}_{u=u\left(t\right)}}\\ \frac{d}{du}\[\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_w\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_w\left(u\right)}{du}-\frac{s_w\left(u\right)}{\left|\right|s_w\left(u\right)|{|}^2}{s}_w^T\left(u\right)\frac{{\mathrm{ds}}_w\left(u\right)}{du}\\ \frac{d}{du}\[\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_w\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_w\left(u\right)}{du}-\frac{n_w\left(u\right)}{\left|\right|n_w\left(u\right)|{|}^3}{n}_w^T\left(u\right)\frac{{\mathrm{dn}}_w\left(u\right)}{du}\end{array}\right.$

在t时刻，运动控制器驱动所述三维平移机构运动，使所述三维平移机构的位移量G(t)满足：

T(θ(t),γ(t),G(t))＝X_w(u(t))+h·l_w(t)

在t时刻，运动控制器驱动所述三维平移机构运动，使所述三维平移机构的瞬时速度满足：

$\begin{array}{c}\frac{dG}{dt}={\[\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂G}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\]}^{-1}\[s\left(u\right(t\left)\right)\frac{du\left(t\right)}{dt}+h\·\frac{{\mathrm{dl}}_w\left(t\right)}{dt}\\ -\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}-\frac{\∂T(\mathrm{\θ},\mathrm{\γ},G)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right),G=G\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\]\end{array}$

根据所述焊炬、待焊工件和世界坐标系{W}的相对位姿关系，实时调整所述焊接能量源的能量输入参数。

2.如权利要求1所述的一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：所述焊接能量源为电弧焊焊接电源、激光焊热源或搅拌摩擦焊运动驱动器。

3.如权利要求1所述的一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：该方法采用的装置还包括送丝机构和送丝机构控制器；所述送丝机构控制器与所述送丝机构相连；所述送丝机构末端与所述焊炬相连；使送丝机构控制器发出控制信号，控制送丝机构在焊接过程进行送丝。

4.如权利要求1所述的一种用于大型构件空间曲线轨迹焊接的运动控制方法，其特征在于：所述运动控制器为电机控制器或液压控制器。