CN105149833A - 一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法 - Google Patents

Abstract

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

Description

一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法

技术领域

本发明属于焊接自动化领域，特别涉及一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法。

背景技术

空间曲线轨迹焊接常现于航天航空、船舶制造、石油化工等领域的设备制造过程。为获得良好的焊接质量，往往需要满足以下若干目标：其一，焊接速度可在焊前预设，且在焊接过程中保持恒定；其二，在焊接过程中待焊点与焊炬末端点的距离保持恒定，且在焊前可预设，在电弧焊中表现为弧长恒定，在激光焊中表现为激光离焦量恒定，在搅拌摩擦焊中表现为搅拌头插入深度恒定；其三，在焊接过程中焊接姿态保持恒定，焊炬轴线与待焊点法向可保持恒定的预设倾角，且熔池或搅拌区与世界坐标系保持相对稳定姿态，如在电弧焊中常需保持平焊位置，在搅拌摩擦焊中常需保持轴肩和工件表面成一定角度以施加一定的顶锻压力。目前，空间曲线轨迹焊接大多采用人工焊接方式，难以保证焊缝质量的稳定性和一致性。

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

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

发明内容

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

本发明的技术方案如下：

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

所述方法包括以下步骤：

1)建立与所述底座固结的世界坐标系{W}，所述世界坐标系{W}的x_w轴方向与所述第一一维平移机构的运动方向相互平行，y_w轴方向与所述第二一维平移机构的运动方向相互平行，z_w轴方向与所述第三一维平移机构的运动方向相互平行；建立与所述待焊工件固结的工件坐标系{P}；

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

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

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

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

$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.$

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

$s_p\left(u\right)=\frac{{\mathrm{dX}}_p\left(u\right)}{du}$

c)计算：

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

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

d)计算法向量插补函数n_p(u)＝s_p(u)×r_p(u)；

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

设t为任意非负实数；

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

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

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

$l_p\left(t\right)=-\frac{s_p\left(u\left(t\right)\right)}{\left|\right|s_p\left(u\left(t\right)\right)\left|\right|}\sin \mathrm{\α}+\frac{n_p\left(u\left(t\right)\right)}{\left|\right|n_p\left(u\left(t\right)\right)\left|\right|}\cos \mathrm{\α}$

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

${\∫}_0^{u\left(t\right)}\left|\right|s_p\left(\mathrm{\ξ}\right)\left|\right|d\mathrm{\ξ}=C\·t$

式中，ξ为积分变量；

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

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

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_p\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_p\left(u\right)}{\left|\right|n_p\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_p\left(u\left(t\right)\right)\left|\right|}\\ \frac{d}{dt}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_p\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_p\left(u\right)}{du}-\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)|{|}^3}{s}_p^T\left(u\right)\frac{{\mathrm{ds}}_p\left(u\right)}{du}\\ \frac{d}{dt}\[\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_p\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_p\left(u\right)}{du}-\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)|{|}^3}{n}_p^T\left(u\right)\frac{{\mathrm{dn}}_p\left(u\right)}{du}\end{array}\right.$

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

G(t)＝R(θ(t),γ(t))·X_p(u(t))+T(θ(t),γ(t))+h·e₃

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

$\begin{array}{c}\frac{dG\left(t\right)}{dt}=\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ +\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\\ +R\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)\·s_p\left(u\left(t\right)\right)\·\frac{du\left(t\right)}{dt}\end{array};$

一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法，其特征在于：所述焊接能量源为电弧焊焊接电源、激光焊热源或搅拌摩擦焊运动驱动器；

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

一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法，其特征在于：所述运动控制器为电机控制器或液压控制器。

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

附图说明

图1为本发明提出的一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法实施例所采用装置的轴测图。

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

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

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

图5为采用图1所示装置的一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法实施例的流程图。

图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、运动控制器2、焊接能量源3、焊炬4、三维平移机构5和双自由度旋转机构6；所述底座1分别与所述三维平移机构5、所述双自由度旋转机构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的旋转轴相互正交；所述焊炬4安装在所述三维平移机构5的运动输出端；待焊工件7安装在所述双自由度旋转机构6的运动输出端。待焊轨迹71为一相贯线轨迹，设形成相贯线焊缝的两个圆管分为为第一圆管和第二圆管。

建立与底座1固结的世界坐标系{W}，所述世界坐标系{W}的x_w轴方向与第一一维平移机构51的运动方向相互平行且与第一旋转机构61的旋转轴重合，y_w轴方向与第二一维平移机构52的运动方向相互平行，z_w轴方向与第三一维平移机构53的运动方向相互平行；建立与待焊工件7固结的工件坐标系{P}，所述工件坐标系{P}的y_p轴方向与第一圆管轴线方向重合，z_p轴方向与第二圆管轴线方向重合；工件坐标系{P}的原点O_p位于世界坐标系{W}的y_wO_wz_w平面内。

在待焊轨迹71上自起点至终点测量N个离散空间点的三维坐标以及各点处的单位法向量，其中N是大于或等于2的正整数，记第i个离散空间点在工件坐标系{P}中的三维坐标为X_i，第i个离散空间点处的单位法向量在工件坐标系{P}中的三维坐标为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_p\left(u\right)=\underset{\mathrm{\σ}=1}{\overset{N}{\Σ}}{D}_{\mathrm{\σ}}\·{\mathrm{\Γ}}_{\mathrm{\σ},q}\left(u\right),u\∈\[0,1\]\mathrm{---}\left(1\right)$

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

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

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

除需确定B样条曲线X_p(u)的节点值外，还需确定点X_i处B样条曲线X_p(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.\mathrm{---}\left(3\right)$

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

${\mathrm{\β}}_{\mathrm{\η}}=\frac{1}{q}\underset{k=\mathrm{\η}+1}{\overset{\mathrm{\η}+q}{\Σ}}{u}_k\mathrm{---}\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\mathrm{---}\left(5\right)$

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

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

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

n_p,i＝λ_is_p(u_i)+μ_im_i(7)

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

${\[s_p\left(u_i\right)\]}^T{n}_{p,i}={\mathrm{\λ}}_i{\[s_p\left(u_i\right)\]}^T{s}_p\left(u_i\right)+{\mathrm{\μ}}_i{\[s_p\left(u_i\right)\]}^T{m}_i=0---\left(8\right)$

即：

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

因此：

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

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

n_p(u)＝s_p(u)×r_p(u)(11)

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

$r_{p,i}=r_p\left(u_i\right)=n_{p,i}\×\frac{s_p\left(u_i\right)}{\left|\right|s_p\left(u_i\right)\left|\right|}=\frac{m_i-\frac{{\[s_p\left(u_i\right)\]}^T{m}_i}{{\[s_p\left(u_i\right)\]}^T{s}_p\left(u_i\right)}{s}_p\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_p\left(u_i\right)\]}^T{m}_i}{{\[s_p\left(u_i\right)\]}^T{s}_p\left(u_i\right)}{s}_p\left(u_i\right)\left|\right|}\×\frac{s_p\left(u_i\right)}{\left|\right|s_p\left(u_i\right)\left|\right|}---\left(12\right)$

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

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

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

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

由几何关系可知，在本实施例中，当第一旋转机构61的旋转角为θ且所述第二旋转机构62的旋转角为γ时，工件坐标系{P}和世界坐标系{W}的旋转转换矩阵R(θ,γ)和平移转换矩阵T(θ,γ)分别等于：

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

$T(\mathrm{\θ},\mathrm{\γ})=\left[\begin{array}{c}0\\ -Lsin\mathrm{\θ}\\ L\cos \mathrm{\θ}\end{array}\right]---\left(15\right)$

式中，θ和γ为任意实数，在本实施例中L＝340mm。根据式(14)和式(15)可得：

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

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

$\frac{\∂T(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}=\left[\begin{array}{c}0\\ -Lcos\mathrm{\θ}\\ -Lsin\mathrm{\θ}\end{array}\right]---\left(18\right)$

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

设在t时刻，三维平移机构5的位移量为G(t)，它定义为焊炬4末端点在世界坐标系{W}中的坐标；设在t时刻，第一一维平移机构51、第二一维平移机构52和第三一维平移机构53的位移量分别为X(t)、Y(t)和Z(t)，第一旋转机构61和第二旋转机构62的旋转角分别为θ(t)和γ(t)。由坐标变换关系可得：

$G\left(t\right)=\left[\begin{array}{c}X\left(t\right)\\ Y\left(t\right)\\ Z\left(t\right)\end{array}\right]=R\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)\·X_p\left(u\left(t\right)\right)+T\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)+h\·e_3---\left(20\right)$

式中，e₃为世界坐标系{W}的z_w轴的单位方向向量，u(t)为t时刻函数X_p(u)的自变量取值，X_p(u(t))为t时刻待焊点在工件坐标系{P}中的坐标。

根据焊炬倾角为α可计算t时刻焊炬4轴线的单位方向向量在工件坐标系{P}中的坐标l_p(t)：

$l_p\left(t\right)=-\frac{s_p\left(u\right(t))}{\left|\right|s_p\left(u\right(t)\left)\right||}sin\mathrm{\α}+\frac{n_p\left(u\right(t))}{\left|\right|n_p\left(u\right(t)\left)\right||}cos\mathrm{\α}---\left(21\right)$

由于焊炬4轴线的单位方向向量在世界坐标系{W}中的坐标恒为e₃，因此：

R(θ(t),γ(t))·l_p(t)＝e₃(22)

式(22)对时间t求导可计算第一旋转机构61和第二旋转机构62的瞬时角速度和 $\frac{d\mathrm{\γ}\left(t\right)}{dt}:$

$\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·l_p\left(t\right)\·\frac{d\mathrm{\θ}\left(t\right)}{dt}+\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·l_p\left(t\right)\·\frac{d\mathrm{\γ}\left(t\right)}{dt}+R(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right))\·\frac{{\mathrm{dl}}_p\left(t\right)}{dt}=0---\left(23\right)$

即：

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

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_p\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_p\left(u\right)}{\left|\right|n_p\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_p\left(u\left(t\right)\right)\left|\right|}\\ \frac{d}{dt}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_p\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_p\left(u\right)}{du}-\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)|{|}^3}{s}_p^T\left(u\right)\frac{{\mathrm{ds}}_p\left(u\right)}{du}\\ \frac{d}{dt}\[\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_p\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_p\left(u\right)}{du}-\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)|{|}^3}{n}_p^T\left(u\right)\frac{{\mathrm{dn}}_p\left(u\right)}{du}\end{array}\right.---\left(25\right)$

式(20)对时间t求导可计算三维平移机构5的瞬时速度

$\begin{array}{c}\frac{dG\left(t\right)}{dt}=\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ +\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\\ +R\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)\·s_p\left(u\left(t\right)\right)\·\frac{du\left(t\right)}{dt}\end{array}---\left(26\right)$

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

t时刻待焊点在世界坐标系{W}中的速度v(t)等于：

$v\left(t\right)=\frac{dR\left(\mathrm{\θ}\right(t),\mathrm{\γ}\left(t\right))}{dt}{X}_p\left(u\right(t\left)\right)+\frac{dT\left(\mathrm{\θ}\right(t),\mathrm{\γ}\left(t\right))}{dt}---\left(27\right)$

式中，

$\frac{dR\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)}{dt}=\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}=\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}---\left(28\right)$

$\frac{dT\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)}{dt}=\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·\frac{d\mathrm{\θ}\left(t\right)}{dt}=\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·\frac{d\mathrm{\γ}\left(t\right)}{dt}---\left(29\right)$

为保证焊接速度恒定为C，必须使三维平移机构5的瞬时速度在待焊点处切向的投影与待焊点的瞬时速度在待焊点处切向的投影之差等于C，即：

$\frac{s_w^T\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}\frac{dG\left(t\right)}{dt}-\frac{s_w^T\left(u\right(t))}{\left|\right|s_w\left(u\right(t)\left)\right||}v\left(t\right)=C---\left(30\right)$

式中，

s_w(u(t))＝R(θ(t),γ(t))·s_p(u(t))(31)

为待焊点处的切向量在世界坐标系{W}中的坐标。

结合式(26)至式(31)，可化简式(30)的焊接速度恒定方程：

$\left|\right|s_p\left(u\left(t\right)\right)\left|\right|\frac{du\left(t\right)}{dt}=C---\left(32\right)$

即：

$\frac{du\left(t\right)}{dt}=\frac{C}{\left|\right|s_p\left(u\right(t)\left)\right||}---\left(33\right)$

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

${\∫}_0^{u\left(t\right)}\left|\right|s_p\left(\mathrm{\ξ}\right)\left|\right|d\mathrm{\ξ}=C\·t---\left(34\right)$

式中，ξ为积分变量。

综合以上分析结果，本发明提出的一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法实施例的流程图如图5所示，其包括以下步骤：

1)建立与所述底座1固结的世界坐标系{W}，所述世界坐标系{W}的x_w轴方向与所述第一一维平移机构51的运动方向相互平行，y_w轴方向与所述第二一维平移机构52的运动方向相互平行，z_w轴方向与所述第三一维平移机构53的运动方向相互平行；建立与所述待焊工件7固结的工件坐标系{P}；

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

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

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

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

$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(35\right)$

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

$s_p\left(u\right)=\frac{{\mathrm{dX}}_p\left(u\right)}{du}---\left(36\right)$

c)计算：

$r_{p,i}=\frac{m_i-\frac{{\[s_p\left(u_i\right)\]}^T{m}_i}{{\[s_p\left(u_i\right)\]}^T{s}_p\left(u_i\right)}{s}_p\left(u_i\right)}{\left|\right|m_i-\frac{{\[s_p\left(u_i\right)\]}^T{m}_i}{{\[s_p\left(u_i\right)\]}^T{s}_p\left(u_i\right)}{s}_p\left(u_i\right)\left|\right|}\×\frac{s_p\left(u_i\right)}{\left|\right|s_p\left(u_i\right)\left|\right|}---\left(37\right)$

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

d)计算法向量插补函数n_p(u)＝s_p(u)×r_p(u)；

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

设t为任意非负实数；

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

R(θ(t),γ(t))·l_p(t)＝e₃(38)

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

$l_p\left(t\right)=-\frac{s_p\left(u\left(t\right)\right)}{\left|\right|s_p\left(u\left(t\right)\right)\left|\right|}\sin \mathrm{\α}+\frac{n_p\left(u\left(t\right)\right)}{\left|\right|n_p\left(u\left(t\right)\right)\left|\right|}\cos \mathrm{\α}---\left(39\right)$

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

${\∫}_0^{u\left(t\right)}\left|\right|s_p\left(\mathrm{\ξ}\right)\left|\right|d\mathrm{\ξ}=C\·t---\left(40\right)$

式中，ξ为积分变量；

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

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

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_p\left(t\right)}{dt}=\left\{-\frac{d}{du}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_p\left(u\right)}{\left|\right|n_p\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_p\left(u\left(t\right)\right)\left|\right|}\\ \frac{d}{dt}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_p\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_p\left(u\right)}{du}-\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)|{|}^3}{s}_p^T\left(u\right)\frac{{\mathrm{ds}}_p\left(u\right)}{du}\\ \frac{d}{dt}\[\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_p\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_p\left(u\right)}{du}-\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)|{|}^3}{n}_p^T\left(u\right)\frac{{\mathrm{dn}}_p\left(u\right)}{du}\end{array}\right.---\left(42\right)$

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

G(t)＝R(θ(t),γ(t))·X_p(u(t))+T(θ(t),γ(t))+h·e₃(43)

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

$\begin{array}{c}\frac{dG\left(t\right)}{dt}=\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ +\[\frac{\∂R\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\left(t\right)\right)+\frac{\∂T\left(\mathrm{\θ},\mathrm{\γ}\right)}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\\ +R\left(\mathrm{\θ}\left(t\right),\mathrm{\γ}\left(t\right)\right)\·s_p\left(u\left(t\right)\right)\·\frac{du\left(t\right)}{dt}\end{array}---\left(44\right)$

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

$\left\{\begin{array}{c}{x}^2+y^2={100}^2{\mathrm{mm}}^2\\ z^2+x^2={150}^2{\mathrm{mm}}^2\end{array}\right.---\left(45\right)$

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

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

式中，

$y_i=\left\{\begin{array}{cc}-100+2\left(i-1\right)& ,1\≤i\≤101\\ -98+2\left(200-i\right)& ,102\≤i\≤200\end{array}\right.---\left(47\right)$

$x_i=\left\{\begin{array}{cc}\sqrt{{100}^2-y_i^2}& ,1\≤i\≤101\\ -\sqrt{{100}^2-y_i^2}& ,102\≤i\≤200\end{array}\right.---\left(48\right)$

$z_i=\sqrt{{150}^2-x_i^2}---\left(49\right)$

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

在本发明的实施例中，焊接速度C＝6mm/s，焊炬4末端点与待焊点之间的有向距离h＝8mm，焊炬倾角α＝10°，采用的B样条曲线阶次q＝3。根据式(10)计算的修正法向量n_p,i与真实法向量m_i之间的角度偏差不超过0.002°。图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)机械连接；所述运动控制器(2)分别与所述三维平移机构(5)和所述双自由度旋转机构(6)通过导线连接，或通过无线传输方式通讯；所述焊炬(4)与所述焊接能量源(3)通过导线连接，或通过光路连接；所述三维平移机构(5)包括第一一维平移机构(51)、第二一维平移机构(52)和第三一维平移机构(53)；所述第一一维平移机构(51)、所述第二一维平移机构(52)和所述第三一维平移机构(53)的运动方向相互正交；所述双自由度旋转机构(6)包括第一旋转机构(61)和第二旋转机构(62)；所述第一旋转机构(61)和所述第二旋转机构(62)的旋转轴相互正交；所述焊炬(4)安装在所述三维平移机构(5)的运动输出端；待焊工件(7)安装在所述双自由度旋转机构(6)的运动输出端；

所述方法包括以下步骤：

1)建立与所述底座固结的世界坐标系{W}，所述世界坐标系{W}的x_w轴方向与所述第一一维平移机构的运动方向相互平行，y_w轴方向与所述第二一维平移机构的运动方向相互平行，z_w轴方向与所述第三一维平移机构的运动方向相互平行；建立与所述待焊工件固结的工件坐标系{P}；

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

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

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

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

$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.$

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

$s_p\left(u\right)=\frac{{\mathrm{dX}}_p\left(u\right)}{du}$

c)计算：

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

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

d)计算法向量插补函数n_p(u)＝s_p(u)×r_p(u)；

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

设t为任意非负实数；

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

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

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

$l_p\left(t\right)=-\frac{s_p\left(u\right(t))}{\left|\right|s_p\left(u\right(t)\left)\right||}sin\mathrm{\α}+\frac{n_p\left(u\right(t))}{\left|\right|n_p\left(u\right(t)\left)\right||}cos\mathrm{\α}$

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

${\∫}_0^{u\left(t\right)}\left|\right|s_p\left(\mathrm{\ξ}\right)\left|\right|d\mathrm{\ξ}=C\·t$

式中，ξ为积分变量；

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

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

式中，

$\left\{\begin{array}{c}\frac{{\mathrm{dl}}_p\left(t\right)}{dt}=\{-\frac{d}{du}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]{|}_{u=u\left(t\right)}\·\sin \mathrm{\α}+\frac{d}{du}\[\frac{n_p\left(u\right)}{\left|\right|n_p\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_p\left(u\right(t)\left)\right||}\\ \frac{d}{dt}\[\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|s_p\left(u\right)\left|\right|}\frac{{\mathrm{ds}}_p\left(u\right)}{du}-\frac{s_p\left(u\right)}{\left|\right|s_p\left(u\right)|{|}^3}{s}_p^T\left(u\right)\frac{{\mathrm{ds}}_p\left(u\right)}{du}\\ \frac{d}{dt}\[\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)\left|\right|}\]=\frac{1}{\left|\right|n_p\left(u\right)\left|\right|}\frac{{\mathrm{dn}}_p\left(u\right)}{du}-\frac{n_p\left(u\right)}{\left|\right|n_p\left(u\right)|{|}^3}{n}_p^T\left(u\right)\frac{{\mathrm{dn}}_p\left(u\right)}{du}\end{array}\right.$

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

G(t)＝R(θ(t),γ(t))·X_p(u(t))+T(θ(t),γ(t))+h·e₃

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

$\begin{array}{c}\frac{dG\left(t\right)}{dt}=\[\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\right(t\left)\right)+\frac{\∂T(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\θ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\θ}\left(t\right)}{dt}\\ +\[\frac{\∂R(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\·X_p\left(u\right(t\left)\right)+\frac{\∂T(\mathrm{\θ},\mathrm{\γ})}{\∂\mathrm{\γ}}{|}_{\mathrm{\θ}=\mathrm{\θ}\left(t\right),\mathrm{\γ}=\mathrm{\γ}\left(t\right)}\]\·\frac{d\mathrm{\γ}\left(t\right)}{dt}\\ +R\left(\mathrm{\θ}\right(t),\mathrm{\γ}(t\left)\right)\·s_p\left(u\right(t\left)\right)\·\frac{du\left(t\right)}{dt}\end{array}.$

2.如权利要求1所述的一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法，其特征在于：所述焊接能量源为电弧焊焊接电源、激光焊热源或搅拌摩擦焊运动驱动器。

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

4.如权利要求1所述的一种用于空间曲线轨迹稳定位姿恒速焊接的运动规划方法，其特征在于：所述运动控制器为电机控制器或液压控制器。