CN103676787A - 一种用于运动控制系统的圆心模式空间圆弧插补方法 - Google Patents

Abstract

本发明涉及一种圆弧插补方法，一种用于运动控制系统的圆心模式空间圆弧插补方法，包括以下步骤：根据圆心模式给定的圆心坐标，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵；计算整个空间圆弧弧长；在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式；计算O′X′Y′平面每个插补周期平面圆弧插补点坐标；计算OXYZ坐标系下空间圆弧插补点坐标；空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制。本发明提供的一种用于运动控制系统的圆心模式空间圆弧插补方法减少了空间圆弧参数计算量，提高了计算效率和插补精度。

Description

一种用于运动控制系统的圆心模式空间圆弧插补方法

技术领域

本发明涉及工业控制领域中的空间圆弧插补方法，特别是数控机床或机器人的一种用于运动控制系统的圆心模式空间圆弧插补方法。

背景技术

在现代运动控制与机械加工制造中，复杂空间曲线加工一般采用小线段的空间直线来逼近，此方法编程复杂，加工精度与效率低，为了实现高速高精加工与控制，要求运动控制系统具有空间圆弧插补功能，通过采用空间圆弧与空间直线来拟合空间曲线。

目前普遍采用边界点模式空间圆弧插补方法，即根据空间圆弧上一点、起点和终点的三点坐标来计算空间圆弧圆心坐标和半径，再通过每个插补周期插补出的空间圆弧圆心角，实时计算插补点在直角坐标系OXYZ中的坐标。

边界点模式空间圆弧插补方法能够很好地实现空间任意位置空间圆弧加工与控制，但此方法需要先计算空间圆弧中心坐标，计算工作量大，计算过程复杂，且插补过程在空间三维坐标系下进行，降低了空间圆弧插补点计算效率和精度。

发明内容

为了克服已有技术存在的不足，本发明目的是提供一种用于运动控制系统的圆心模式空间圆弧插补方法。该方法直接给出空间圆弧圆心坐标，并采用坐标变换将空间圆弧转换为平面圆弧，通过采用时间分割法对平面圆弧进行实时插补后，再根据坐标变换矩阵计算插补点在直角坐标系OXYZ中的坐标，此方法可显著减少插补过程计算量，提高了插补点计算效率和精度。

为了实现上述发明目的，解决已有技术中所存在的问题，本发明采取的技术方案是：一种用于运动控制系统的圆心模式空间圆弧插补方法，根据圆心模式给定的圆心坐标，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵，计算整个空间圆弧弧长，在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，计算O′X′Y′平面每个插补周期平面圆弧插补点坐标，计算OXYZ坐标系下空间圆弧插补点坐标，空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制；插补方法具体步骤如下：

（A）根据圆心模式给定的圆心坐标O′(x_o′,y_o′,z_o′)，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵M，包括以下步骤：

第一步、在O′X′Y′Z′坐标系，以向量计算X′轴正向单位向量

则 $\stackrel{\→}{i^{\′}}=\frac{\stackrel{\→}{O^{\′}{P}_1}}{\left|\stackrel{\→}{O^{\′}{P}_1}\right|}---\left(1\right)$

式中，P₁(x₁,y₁,z₁)为空间圆弧起点坐标；

第二步、计算空间圆弧所在的O′X′Y′平面单位法向向量

则 $\left\{\begin{array}{cc}\stackrel{\→}{k^{\′}}=\frac{\stackrel{\→}{O^{\′}{P}_3}\×\stackrel{\→}{O^{\′}{P}_1}}{|\stackrel{\→}{O^{\′}{P}_3}\×\stackrel{\→}{O^{\′}{P}_1}|}& (\stackrel{\→}{O^{\′}{P}_3}\×\stackrel{\→}{O^{\′}{P}_1}>\stackrel{\→}{0})\\ \stackrel{\→}{k^{\′}}=\frac{\stackrel{\→}{O^{\′}{P}_2}\×\stackrel{\→}{O^{\′}{P}_1}}{|\stackrel{\→}{O^{\′}{P}_2}\×\stackrel{\→}{O^{\′}{P}_1}|}& (\stackrel{\→}{O^{\′}{P}_3}\×\stackrel{\→}{O^{\′}{P}_1}=\stackrel{\→}{0})\end{array}\right.---\left(2\right)$

式中，P₃(x₃,y₃,z₃)为空间圆弧终点坐标，P₂(x₂,y₂,z₂)为空间圆弧圆心、起点与终点三点共线时，空间圆弧上一点坐标；

第三步、计算在空间圆弧插补方向约束下的Z′正向单位向量

则 $\left\{\begin{array}{c}\stackrel{\→}{k^{\′}}\·\stackrel{\→}{i}\·\mathrm{sign}\≤0\\ \stackrel{\→}{k^{\′}}\·\stackrel{\→}{j}\·\mathrm{sign}\≤0\\ \stackrel{\→}{k^{\′}}\·\stackrel{\→}{k}\·\mathrm{sign}\≤0\end{array}\right.---\left(3\right)$

式中，

为直角坐标系X轴正向单位向量，

为直角坐标系Y轴正向单位向量，

为直角坐标系Z轴正向单位向量，sign为空间圆弧插补方向符号，顺时针插补时sign为-1，逆时针插补时sign为1；

第四步、根据右手定则计算Y′正向单位向量

则 $\stackrel{\→}{j^{\′}}=\stackrel{\→}{k^{\′}}\×\stackrel{\→}{i^{\′}}---\left(4\right)$

第五步、由公式（1）、（2）、（3）和（4）得坐标变换矩阵为，

$M=\left[\begin{array}{ccc}{i}_x^{\′}& i_y^{\′}& i_z^{\′}\\ j_x^{\′}& j_y^{\′}& j_z^{\′}\\ k_x^{\′}& k_y^{\′}& k_z^{\′}\end{array}\right]---\left(5\right)$

（B）计算整个空间圆弧弧长S_s，包括以下步骤：

第一步、计算P₃在O′X′Y′Z′坐标系中的坐标P′₃(x′₃,y′₃,z′₃)，

$\left[\begin{array}{c}{x}_3^{\′}\\ y_3^{\′}\\ z_3^{\′}\end{array}\right]=M\·\left[\begin{array}{c}{x}_3-x_{o\′}\\ y_3-y_{o\′}\\ z_3-z_{o\′}\end{array}\right]---\left(6\right)$

第二步、由公式（6）得P′₃在O′X′Y′Z′坐标系末角度

正弦值和余弦值计算公式为：

第三步、由公式（7）得空间圆弧弧长计算公式为：

（C）在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，包括以下步骤：

第一步、由公式（8）得基于梯形曲线加减速控制的圆弧插补的加减速结束时间节点计算公式为：

$\left\{\begin{array}{c}{t}_1=\frac{v_f-v_0}{a_1}\\ t_2=t_1+\frac{1}{v_f}[S_s-(v_0{t}_1+\frac{1}{2}{a}_1{t}_1^2)-\frac{1}{2}\frac{v_f^2}{a_2}]\\ t_3=t_1+t_2+\frac{v_f}{a_2}\end{array}\right.---\left(9\right)$

式中，t₁为平面圆弧插补的加速结束时刻，t₂为平面圆弧插补的恒速结束时刻，t₃为平面圆弧插补的减速结束时刻，v₀为平面圆弧插补初始线速度，v_f为平面圆弧插补进给线速度，a₁为平面圆弧插补加速度，a₂为平面圆弧插补减速度；

第二步、由公式（9）得O′X′Y′平面每个插补周期T加速度计算公式为：

$a\left(\mathrm{iT}\right)=\left\{\begin{array}{cc}{a}_1& 0\&le;i\&le;N_1\\ 0& N_1<i\&le;N_2\\ a_2& N_2<i\&le;N_3\end{array}\right.---\left(10\right)$

式中，N₁为t₁/T取整后需要的插补周期总个数，N₂为t₂/T取整后需要的插补周期总个数，N₃为t₃/T取整后需要的插补周期总个数，i=0,1,2…N₃；

第三步、由公式（10）得O′X′Y′平面每个插补周期线速度计算公式为：

$v\left(\mathrm{iT}\right)=v_0+\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}a\left(\mathrm{iT}\right)\&CenterDot;T---\left(11\right)$

第四步、由公式（11）得O′X′Y′平面每个插补周期弧长计算公式为：

$s\left(\mathrm{iT}\right)=\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}v\left(\mathrm{iT}\right)\&CenterDot;T---\left(12\right)$

（D）计算O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)，

由公式（12）得O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)计算公式为：

$\left\{\begin{array}{c}{x}_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\cos \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\\ y_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\sin \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\end{array}\right.---\left(13\right)$

（E）计算OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)，

由公式（5）和（13）经过坐标变换，得OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)计算公式为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=M^T\left[\begin{array}{c}{x}_i^{\&prime;}\\ y_i^{\&prime;}\\ 0\end{array}\right]+\left[\begin{array}{c}{x}_{o\&prime;}\\ y_{o\&prime;}\\ z_{o\&prime;}\end{array}\right]---\left(14\right)$

其中M^T为M的转换矩阵；

（F）空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制。

本发明有益效果是：一种用于运动控制系统的圆心模式空间圆弧插补方法，是根据圆心模式给定的圆心坐标，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵，计算整个空间圆弧弧长，在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，计算O′X′Y′平面每个插补周期平面圆弧插补点坐标，计算OXYZ坐标系下空间圆弧插补点坐标，空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制。与已有技术相比，一是，本发明直接给定空间圆弧圆心坐标，减少了空间圆弧参数计算量。二是，本发明计算圆弧插补点坐标通过坐标变换将空间圆弧经过平移与旋转变换为平面圆弧，通过对平面圆弧的弧长插补和坐标变换矩阵得到实际空间圆弧插补点坐标，因此提高了计算效率和插补精度。

附图说明

图1是本发明采用的运动控制系统组成框图。

图2是本发明流程图。

图3是本发明计算下一个空间圆弧插补点坐标的示意图。

图中：1、运动控制器，2、驱动器，3、执行器，4、被控对象，5、传感器。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，运动控制系统组成框图，包括运动控制器1、驱动器2、执行器3、被控对象4和传感器5，所述运动控制器1包括空间圆弧插补模块、位置闭环控制模块、执行器接口和传感器接口；所述空间圆弧插补模块与位置闭环控制模块连接，主要用于产生空间圆弧插补点坐标，所述位置闭环控制模块分别与执行器接口和传感器接口连接，主要用于空间圆弧插补模块产生的空间圆弧补点坐标进行位置闭环控制；所述驱动器2包括X轴驱动器、Y轴驱动器和Z轴驱动器，选自步进驱动器或伺服驱动器；所述执行器3包括X轴电机、Y轴电机和Z轴电机，选自步进电机或伺服电机；所述传感器5选自旋转编码器或光栅传感器。所述运动控制器1中的执行器接口与驱动器2连接，所述驱动器2与执行器3连接，主要用来产生脉冲量，输出给驱动器2用于驱动执行器3运动；所述执行器3分别与传感器5和被控对象4连接，所述被控对象4与传感器5连接。所述传感器5分别与驱动器2及运动控制器1中的传感器接口连接，主要用于通过传感器5来获取被控对象4实际圆弧插补点坐标。

如图2所示，本发明的一种用于运动控制系统的圆心模式空间圆弧插补方法，包括如下步骤：

（A）根据圆心模式给定的圆心坐标O′(x_o′,y_o′,z_o′)，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵M，包括以下步骤：

第一步、在O′X′Y′Z′坐标系，以向量

计算X′轴正向单位向量

则 $\stackrel{\&RightArrow;}{i^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}---\left(1\right)$

式中，P₁(x₁,y₁,z₁)为空间圆弧起点坐标；

第二步、计算空间圆弧所在的O′X′Y′平面单位法向向量

则 $\left\{\begin{array}{cc}\stackrel{\&RightArrow;}{k^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}|}& (\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}>\stackrel{\&RightArrow;}{0})\\ \stackrel{\&RightArrow;}{k^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_2}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_2}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}|}& (\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}=\stackrel{\&RightArrow;}{0})\end{array}\right.---\left(2\right)$

式中，P₃(x₃,y₃,z₃)为空间圆弧终点坐标，P₂(x₂,y₂,z₂)为空间圆弧圆心、起点与终点三点共线时，空间圆弧上一点坐标；

第三步、计算在空间圆弧插补方向约束下的Z′正向单位向量

则 $\left\{\begin{array}{c}\stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{i}\&CenterDot;\mathrm{sign}\&le;0\\ \stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{j}\&CenterDot;\mathrm{sign}\&le;0\\ \stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{k}\&CenterDot;\mathrm{sign}\&le;0\end{array}\right.---\left(3\right)$

式中，

为直角坐标系X轴正向单位向量，为直角坐标系Y轴正向单位向量，

为直角坐标系Z轴正向单位向量，sign为空间圆弧插补方向符号，顺时针插补时sign为-1，逆时针插补时sign为1；

第四步、根据右手定则计算Y′正向单位向量

则 $\stackrel{\&RightArrow;}{j^{\&prime;}}=\stackrel{\&RightArrow;}{k^{\&prime;}}\&times;\stackrel{\&RightArrow;}{i^{\&prime;}}---\left(4\right)$

第五步、由公式（1）、（2）、（3）和（4）得坐标变换矩阵为，

$M=\left[\begin{array}{ccc}{i}_x^{\&prime;}& i_y^{\&prime;}& i_z^{\&prime;}\\ j_x^{\&prime;}& j_y^{\&prime;}& j_z^{\&prime;}\\ k_x^{\&prime;}& k_y^{\&prime;}& k_z^{\&prime;}\end{array}\right]---\left(5\right)$

（B）计算整个空间圆弧弧长S_s，包括以下步骤：

第一步、计算P₃在O′X′Y′Z′坐标系中的坐标P′₃(x′₃,y′₃,z′₃)，

$\left[\begin{array}{c}{x}_3^{\&prime;}\\ y_3^{\&prime;}\\ z_3^{\&prime;}\end{array}\right]=M\&CenterDot;\left[\begin{array}{c}{x}_3-x_{o\&prime;}\\ y_3-y_{o\&prime;}\\ z_3-z_{o\&prime;}\end{array}\right]---\left(6\right)$

第二步、由公式（6）得P′₃在O′X′Y′Z′坐标系末角度正弦值和余弦值计算公式为：

第三步、由公式（7）得空间圆弧弧长计算公式为：

（C）在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，包括以下步骤：

第一步、由公式（8）得基于梯形曲线加减速控制的圆弧插补的加减速结束时间节点计算公式为：

$\left\{\begin{array}{c}{t}_1=\frac{v_f-v_0}{a_1}\\ t_2=t_1+\frac{1}{v_f}[S_s-(v_0{t}_1+\frac{1}{2}{a}_1{t}_1^2)-\frac{1}{2}\frac{v_f^2}{a_2}]\\ t_3=t_1+t_2+\frac{v_f}{a_2}\end{array}\right.---\left(9\right)$

式中，t₁为平面圆弧插补的加速结束时刻，t₂为平面圆弧插补的恒速结束时刻，t₃为平面圆弧插补的减速结束时刻，v₀为平面圆弧插补初始线速度，v_f为平面圆弧插补进给线速度，a₁为平面圆弧插补加速度，a₂为平面圆弧插补减速度；

第二步、由公式（9）得O′X′Y′平面每个插补周期T加速度计算公式为：

$a\left(\mathrm{iT}\right)=\left\{\begin{array}{cc}{a}_1& 0\&le;i\&le;N_1\\ 0& N_1<i\&le;N_2\\ a_2& N_2<i\&le;N_3\end{array}\right.---\left(10\right)$

式中，N₁为t₁/T取整后需要的插补周期总个数，N₂为t₂/T取整后需要的插补周期总个数，N₃为t₃/T取整后需要的插补周期总个数，i=0,1,2…N₃；

第三步、由公式（10）得O′X′Y′平面每个插补周期线速度计算公式为：

$v\left(\mathrm{iT}\right)=v_0+\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}a\left(\mathrm{iT}\right)\&CenterDot;T---\left(11\right)$

第四步、由公式（11）得O′X′Y′平面每个插补周期弧长计算公式为：

$s\left(\mathrm{iT}\right)=\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}v\left(\mathrm{iT}\right)\&CenterDot;T---\left(12\right)$

（D）计算O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)，

由公式（12）得O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)计算公式为：

$\left\{\begin{array}{c}{x}_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\cos \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\\ y_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\sin \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\end{array}\right.---\left(13\right)$

（E）计算OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)，

由公式（5）和（13）经过坐标变换，得OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)计算公式为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=M^T\left[\begin{array}{c}{x}_i^{\&prime;}\\ y_i^{\&prime;}\\ 0\end{array}\right]+\left[\begin{array}{c}{x}_{o\&prime;}\\ y_{o\&prime;}\\ z_{o\&prime;}\end{array}\right]---\left(14\right)$

其中M^T为M的转换矩阵；

（F）空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制，重复步骤（D）、（E）和（F）直到空间圆弧插补结束。

本发明的一种用于运动控制系统的圆心模式空间圆弧插补方法进行空间圆弧加工与控制过程为：根据圆心模式给定的圆心坐标和起点、终点坐标计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵M，然后由步骤（B）至（E）计算每个插补周期的插补点坐标，并将空间圆弧插补点坐标输出给位置闭环控制模块；传感器接口获取由传感器检测的被控对象实际圆弧插补点坐标，并输出给位置闭环控制模块；位置闭环控制模块根据实际空间圆弧插补点坐标对空间圆弧插补器产生的空间圆弧插补点坐标进行位置闭环控制，并将控制后的圆弧插补点坐标输出给执行器接口；执行器接口根据位置闭环控制模块位置闭环控制后的圆弧插补点坐标产生脉冲量，输出给驱动器驱动执行器运动；执行器带动被控对象完成任意空间圆弧加工与控制。圆心模式空间圆弧插补方法应用于数控系统的G代码格式可定义为：

G07Xx Yy Zz Ii Jj Kk Ff

G08Xx Yy Zz Ii Jj Kk Ff

其中G07为顺时针空间圆弧插补G代码指令；G08为逆时针空间圆弧插补G代码指令；x、y、z为空间圆弧终点坐标；i、j、k为空间圆弧起点坐标到圆心坐标增量值；f为空间圆弧插补进给速度。数控系统通过对G07、G08代码编译、解释与执行，即可完成空间圆弧轨迹加工。

本发明优点在于：一是，本发明直接给定空间圆弧圆心坐标，减少了空间圆弧参数计算量。二是，本发明计算圆弧插补点坐标通过坐标变换将空间圆弧经过平移与旋转变换为平面圆弧，通过对平面圆弧的弧长插补和坐标变换矩阵得到实际空间圆弧插补点坐标，因此提高了计算效率和插补精度。

最后说明的是本发明的一种用于运动控制系统的圆心模式空间圆弧插补方法不局限于上述实施例，还可以做出各种修改、变换和变形。因此，说明书和附图应被认为是说明性的而非限制性的。凡是依据本发明的技术方案进行修改、修饰或等同变化，而不脱离本发明技术方案的思想和范围，其均应涵盖在本发明的权利要求保护范围之内。

Claims (1)

1.一种用于运动控制系统的圆心模式空间圆弧插补方法，其特征在于：根据圆心模式给定的圆心坐标，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵，计算整个空间圆弧弧长，在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，计算O′X′Y′平面每个插补周期平面圆弧插补点坐标，计算OXYZ坐标系下空间圆弧插补点坐标，空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制；插补方法具体步骤如下：

（A）根据圆心模式给定的圆心坐标O′(x_o′,y_o′,z_o′)，计算直角坐标系OXYZ与空间圆弧所在平面建立的O′X′Y′Z′坐标系的坐标变换矩阵M，包括以下步骤：

第一步、在O′X′Y′Z′坐标系，以向量计算X′轴正向单位向量

则 $\stackrel{\&RightArrow;}{i^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}---\left(1\right)$

式中，P₁(x₁,y₁,z₁)为空间圆弧起点坐标；

第二步、计算空间圆弧所在的O′X′Y′平面单位法向向量

则 $\left\{\begin{array}{cc}\stackrel{\&RightArrow;}{k^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}|}& (\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}>\stackrel{\&RightArrow;}{0})\\ \stackrel{\&RightArrow;}{k^{\&prime;}}=\frac{\stackrel{\&RightArrow;}{O^{\&prime;}{P}_2}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}}{|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_2}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}|}& (\stackrel{\&RightArrow;}{O^{\&prime;}{P}_3}\&times;\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}=\stackrel{\&RightArrow;}{0})\end{array}\right.---\left(2\right)$

式中，P₃(x₃,y₃,z₃)为空间圆弧终点坐标，P₂(x₂,y₂,z₂)为空间圆弧圆心、起点与终点三点共线时，空间圆弧上一点坐标；

第三步、计算在空间圆弧插补方向约束下的Z′正向单位向量

则 $\left\{\begin{array}{c}\stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{i}\&CenterDot;\mathrm{sign}\&le;0\\ \stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{j}\&CenterDot;\mathrm{sign}\&le;0\\ \stackrel{\&RightArrow;}{k^{\&prime;}}\&CenterDot;\stackrel{\&RightArrow;}{k}\&CenterDot;\mathrm{sign}\&le;0\end{array}\right.---\left(3\right)$

式中，

为直角坐标系X轴正向单位向量，

为直角坐标系Y轴正向单位向量，

为直角坐标系Z轴正向单位向量，sign为空间圆弧插补方向符号，顺时针插补时sign为-1，逆时针插补时sign为1；

第四步、根据右手定则计算Y′正向单位向量

则 $\stackrel{\&RightArrow;}{j^{\&prime;}}=\stackrel{\&RightArrow;}{k^{\&prime;}}\&times;\stackrel{\&RightArrow;}{i^{\&prime;}}---\left(4\right)$

第五步、由公式（1）、（2）、（3）和（4）得坐标变换矩阵为，

$M=\left[\begin{array}{ccc}{i}_x^{\&prime;}& i_y^{\&prime;}& i_z^{\&prime;}\\ j_x^{\&prime;}& j_y^{\&prime;}& j_z^{\&prime;}\\ k_x^{\&prime;}& k_y^{\&prime;}& k_z^{\&prime;}\end{array}\right]---\left(5\right)$

（B）计算整个空间圆弧弧长S_s，包括以下步骤：

第一步、计算P₃在O′X′Y′Z′坐标系中的坐标P_′3(x′₃,y′₃,z_′3)，

$\left[\begin{array}{c}{x}_3^{\&prime;}\\ y_3^{\&prime;}\\ z_3^{\&prime;}\end{array}\right]=M\&CenterDot;\left[\begin{array}{c}{x}_3-x_{o\&prime;}\\ y_3-y_{o\&prime;}\\ z_3-z_{o\&prime;}\end{array}\right]---\left(6\right)$

第二步、由公式（6）得P′₃在O′X′Y′Z′坐标系末角度

正弦值和余弦值计算公式为：

第三步、由公式（7）得空间圆弧弧长计算公式为：

（C）在O′X′Y′平面确定满足基于梯形曲线加减速控制的平面圆弧插补过程中的弧长计算公式，包括以下步骤：

第一步、由公式（8）得基于梯形曲线加减速控制的圆弧插补的加减速结束时间节点计算公式为：

$\left\{\begin{array}{c}{t}_1=\frac{v_f-v_0}{a_1}\\ t_2=t_1+\frac{1}{v_f}[S_s-(v_0{t}_1+\frac{1}{2}{a}_1{t}_1^2)-\frac{1}{2}\frac{v_f^2}{a_2}]\\ t_3=t_1+t_2+\frac{v_f}{a_2}\end{array}\right.---\left(9\right)$

式中，t₁为平面圆弧插补的加速结束时刻，t₂为平面圆弧插补的恒速结束时刻，t₃为平面圆弧插补的减速结束时刻，v₀为平面圆弧插补初始线速度，v_f为平面圆弧插补进给线速度，a₁为平面圆弧插补加速度，a₂为平面圆弧插补减速度；

第二步、由公式（9）得O′X′Y′平面每个插补周期T加速度计算公式为：

$a\left(\mathrm{iT}\right)=\left\{\begin{array}{cc}{a}_1& 0\&le;i\&le;N_1\\ 0& N_1<i\&le;N_2\\ a_2& N_2<i\&le;N_3\end{array}\right.---\left(10\right)$

式中，N₁为t₁/T取整后需要的插补周期总个数，N₂为t₂/T取整后需要的插补周期总个数，N₃为t₃/T取整后需要的插补周期总个数，i=0,1,2…N₃；

第三步、由公式（10）得O′X′Y′平面每个插补周期线速度计算公式为：

$v\left(\mathrm{iT}\right)=v_0+\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}a\left(\mathrm{iT}\right)\&CenterDot;T---\left(11\right)$

第四步、由公式（11）得O′X′Y′平面每个插补周期弧长计算公式为：

$s\left(\mathrm{iT}\right)=\underset{i=0}{\overset{N_3}{\mathrm{\&Sigma;}}}v\left(\mathrm{iT}\right)\&CenterDot;T---\left(12\right)$

（D）计算O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)，

由公式（12）得O′X′Y′平面每个插补周期平面圆弧插补点坐标(x′_i,y′_i)计算公式为：

$\left\{\begin{array}{c}{x}_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\cos \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\\ y_i^{\&prime;}=\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|\&CenterDot;\sin \left(\frac{s\left(\mathrm{iT}\right)}{\left|\stackrel{\&RightArrow;}{O^{\&prime;}{P}_1}\right|}\right)\end{array}\right.---\left(13\right)$

（E）计算OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)，

由公式（5）和（13）经过坐标变换，得OXYZ坐标系下空间圆弧插补点坐标(x_i,y_i,z_i)计算公式为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=M^T\left[\begin{array}{c}{x}_i^{\&prime;}\\ y_i^{\&prime;}\\ 0\end{array}\right]+\left[\begin{array}{c}{x}_{o\&prime;}\\ y_{o\&prime;}\\ z_{o\&prime;}\end{array}\right]---\left(14\right)$

其中M^T为M的转换矩阵；

（F）空间圆弧插补模块将空间圆弧插补点坐标输出给位置闭环控制模块，进行位置闭环控制。

Images