一种高精度空间椭圆曲线实时插补方法与流程

本发明涉及数控机床和工业机器人领域，特别涉及一种高精度空间椭圆曲线实时插补方法。
背景技术：
：随着《中国制造2025》以及“工业4.0”的提出，工业机器人以及高档数控技术的发展上升到国家战略层面。无论对于工业机器人还是数控机床而言，其轨迹规划的优劣直接决定了控制效果，而插补算法作为轨迹规划的核心，其目的在于获得的插补点精确地着落在指定的运动轨迹上，并使得机器在运行过程中的振动较小，以快速精准的动作指令进行操作。在工业机器人和数控机床实际操作过程中，常常需要以椭圆曲线轨迹进行拾放物体和加工零部件。通常机器人和机床在进行椭圆轨迹插补时，首先确定其轨迹长度，然后根据设置的柔性加减速规划，将连续的椭圆曲线轨迹离散为一系列的插补点，在每一个脉冲指令下执行插补点的运动，直到达到总的执行时间就终止椭圆曲线的插补。那么椭圆曲线插补的核心问题就是提高其插补精度。提高插补精度就是建立椭圆曲线弧长以及插补点计算的精确数学模型。由于椭圆曲线弧长属于第二类椭圆积分，使得其没有解析解，现有的算法中一是采用三点高斯数值积分法求取得椭圆弧长，二是采用弧微分近似算法求得椭圆曲线对应的离心角，两种方法法均没有对插补精度进行控制，因此不能保证椭圆轨迹插补终点达到工程实际要求；进一步的，在零部件加工轨迹为椭圆曲线时，三点高斯数值积分法无法求取椭圆曲线中间插补点信息，弧微分法精度较差，不能确保所获得的插补点精确地落在理想椭圆轨迹上。技术实现要素：本发明的目的在于克服现有技术存在的上述不足，提供一种高精度空间椭圆曲线插补方法，通过将空间椭圆曲线转换到平面椭圆曲线，然后给定计算精度，通过加速romberg数值积分法精确地计算出椭圆曲线长度，再对其进行速度规划，通过加速romberg数值积分法结合牛顿迭代法的插补算法得到下一插补时刻对应插补点的坐标值，然后经过齐次坐标反变换，得到空间椭圆曲线对应插补点的坐标值。本发明可以保证空间椭圆曲线插补点的精度，并且整个方法流程可以实现程序模块化，对于机器人进行拾放操作的轨迹规划以及数控技术等领域具有重要意义。为了实现上述发明目的，本发明提供了以下技术方案：一种高精度空间椭圆曲线实时插补方法，包括以下步骤：步骤一：将空间椭圆曲线经过齐次坐标变换转换为平面椭圆曲线；步骤二：根据转换后的平面椭圆曲线用加速romberg数值积分法计算出空间椭圆曲线弧长，并控制所述空间椭圆曲线弧长的计算精度满足实际工程要求；步骤三：根据设置的柔性加减速曲线，进行所述平面椭圆曲线的速度运动规划，得出总插补时间t，获得所有插补时刻对应平面椭圆曲线起始点的平面椭圆曲线段弧长；步骤四：从平面椭圆曲线起始点的插补时刻开始，对所述平面椭圆曲线当前插补时刻的下一插补时刻对应插补点的离心角进行初始化，根据初始化的离心角通过加速romberg数值积分法计算起始点对应插补时刻与当前插补时刻对应的下一插补时刻之间的平面椭圆曲线段弧长，计算精度根据步骤三所获得的所述下一插补时刻相对应的插补时刻的平面椭圆曲线段弧长通过牛顿迭代法进行控制，在满足计算精度的条件下得到下一插补时刻的平面椭圆曲线段弧长所对应的离心角；步骤五：根据平面椭圆曲线离心角与插补点坐标之间的对应关系，计算出所述平面椭圆曲线所在的参数坐标系的下一插补时刻插补点的坐标，然后经过齐次反变换转换到对应空间椭圆曲线的下一插补时刻插补点的坐标；步骤六：将下一插补时刻赋值为当前插补时刻，判断当前插补时刻是否为总插补时间t，若是则终止插补，否则返回步骤四。进一步的，所述的步骤一中，所述齐次坐标转换过程如下：令xyz直角坐标系上的空间椭圆曲线papb上点的坐标为：p＝(xyz)t，其中pa为曲线上的起始点，pb为曲线上的终点，经过齐次坐标变换将所述空间椭圆曲线转换为xz平面上的平面椭圆曲线，且令xz平面上的平面椭圆曲线上点的坐标为：p′＝(x′0z′)，那么将平面椭圆曲线上的点转换到空间椭圆曲线上的点的公式为：p＝t*p′+p0其中矩阵t为3×3矩阵，表示平面椭圆曲线转换到空间椭圆曲线的旋转矩阵，p0表示平面椭圆曲线转换到空间椭圆曲线的平移量，并且p0为空间椭圆曲线的中心点坐标值；将空间椭圆曲线上的点经过齐次坐标转换到平面椭圆曲线上的点的公式为：p′＝t-1*(p-p0)其中矩阵t-1是矩阵t的逆矩阵；设空间椭圆曲线的起始点pa坐标为：(xayaza)t，终点pb坐标为：(xbybzb)t，且所述空间椭圆曲线上中点pc的坐标为：(xcyczc)t，将空间椭圆曲线经过齐次坐标变换到平面椭圆曲线，令平面椭圆曲线表达式为：a>0且b>0，其中参数a和b的求法如下：那么平面椭圆曲线上的点在参数坐标系下表示为：其中θ为平面椭圆曲线对应的离心角。进一步的，所述的步骤二中，所述平面椭圆曲线弧长计算过程如下：平面椭圆曲线弧长表达式如下:根据平面椭圆曲线用加速romberg数值积分法计算出空间椭圆曲线弧长，具体步骤如下：步骤1：确定积分上下限θmin和θmax、被积函数f(x)、积分次数k以及精度要求em，所述被积函数f(x)的表达式为：步骤2：求出梯形公式所得的积分值t1(0)，其表达式如下：步骤3：对积分区间进行k等分，并通过外推，得到复化梯形公式t1(l)，其表达式如下：其中：m＝1,2,…l，k＝1,2…,l-m+1；步骤4：采用richardson外推法，构造新序列其表达式如下：步骤5：判断是否满足积分精度，其判别式为：若满足，则返回弧长值否则返回到步骤4，计算出t1(l+1)，直到判别式成立。进一步的，所有插补时刻ti(i＝1,…,t)对应插补点的离心角θi(i＝1,…,t)的计算过程如下：步骤1：对平面椭圆曲线起始点对应的离心角进行初始化：θ1＝0，并令romberg积分上限为：θup＝θ1+δ，其中δ为离心角迭代增量；步骤2：利用加速romberg数值积分法求出g，其中g代表θ1到θup之间的平面椭圆曲线段弧长，被积函数f(x)的表达式为：确定积分次数k以及精度要求em；步骤3：构造函数fx＝g-s2，其中s2为t2插补时刻对应的位移；步骤4：由于u＝acosθ，v＝bsinθ，平面椭圆曲线弧微分表达式为：且令根据牛顿迭代法有：temp_θ＝θup-fx/fxd；步骤5：判断精度条件：|temp_θ-θup|<em是否成立，若成立，则下一插补时刻插补点对应的离心角为：θ2＝temp_θ，否则，令θup＝temp_θ，返回到步骤2中，直到步骤5中的判据条件成立；由上计算出t2插补时刻对应插补点的坐标，ti(i＝3,…,t)插补时刻所对应的离心角θi(i＝3,…,t)以此类推进行计算，最终求出所有插补时刻对应插补点的离心角。进一步的，空间椭圆曲线上插补点的坐标计算过程如下：根据所有插补时刻ti(i＝1,…,t)对应插补点的离心角θi(i＝1,…,t)，离心角θi与插补点坐标的关系所示可得：如此得到平面椭圆曲线所有插补时刻对应的插补点坐标，然后经过齐次反变换得到对应空间椭圆曲线的插补点坐标。与现有技术相比，本发明的有益效果：1、本发明建立了三维空间椭圆曲线与二维平面椭圆曲线齐次坐标变换的精确数学模型，适用于空间椭圆曲线的插补；2、本发明采用加速romberg数值积分法精确的计算出椭圆弧长，并将弧长计算精度控制在工程实际要求范围内，例如可达到0.0000005mm之内，满足工程实际需求；3、本发明将椭圆弧长段的求法进行模块化，并运用加速romberg数值积分法和牛顿迭代法算出当前插补时刻对应的下一插补时刻的插补点的坐标点信息，且每次计算迭代次数可不超过4次，从而能实现实时插补；4、本发明的方法可以精确地计算出复杂曲线的插补点，例如超椭圆曲线，因此实用性广。附图说明图1是本发明方法的整体流程图；图2是本发明空间椭圆曲线的齐次坐标变换示意图；图3是本发明计算平面椭圆弧长的方法流程图；图4是本发明由当前插补时刻离心角计算下一插补时刻离心角的方法流程框图；图5是本发明平面椭圆曲线所在的参数坐标系中插补点坐标示意图；图6是本发明实验仿真验证图。具体实施方式下面结合试验例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本
发明内容所实现的技术均属于本发明的范围。结合附图对本发明的空间椭圆曲线插补方法进行详细说明。如图1所示，其为本发明高精度空间椭圆曲线实时插补方法的整体流程图，具体步骤如下：步骤一：将空间椭圆曲线经过齐次坐标变换转换为平面椭圆曲线。参见图2所示，令xyz直角坐标系下的空间椭圆曲线papb(其中pa为曲线上的起点，pb为曲线上的终点)上点的坐标为：p＝(xyz)t，经过齐次坐标变换可将此空间椭圆曲线转换为xz平面上的椭圆曲线，且令xz平面上的椭圆曲线上点的坐标为：p′＝(x′0z′)；那么将平面椭圆曲线上的点转换到空间椭圆曲线上的点为：p＝t*p′+p0(1)其中矩阵t为3×3矩阵，表示平面椭圆曲线转换到空间椭圆曲线的旋转矩阵，p0表示平面椭圆曲线转换到空间椭圆曲线的平移向量，并且p0为空间椭圆曲线的中心点坐标。将空间椭圆曲线上的点转换到平面椭圆曲线上的点为：p′＝t-1*(p-p0)(2)其中矩阵t-1是矩阵t的逆矩阵。令α，β，χ分别为平面椭圆曲线绕其自身坐标系的z，y，x轴所转的角度，那么t-1的表达式如下：其中：r11＝c(α)c(β)r12＝c(α)s(β)s(χ)-c(χ)s(α)r13＝s(α)s(χ)+c(α)c(χ)s(β)r21＝c(β)s(α)r22＝c(α)c(χ)+s(α)s(β)s(χ)r23＝c(χ)s(α)s(β)-c(α)s(χ)r31＝-s(β)r32＝c(β)s(χ)r33＝c(β)c(χ)c(*)表示cos(*),s(*)表示sin(*)。因此通过对矩阵t-1求逆得到矩阵t，再根据矩阵t和空间椭圆曲线的中心点坐标p0将空间椭圆曲线转换到平面椭圆曲线。设空间椭圆曲线的起始点pa坐标为：(xayaza)t，终点pb坐标为：(xbybzb)t，且所述空间椭圆曲线上中点pc的坐标为：(xcyczc)t，将空间椭圆曲线经过齐次坐标变换到平面椭圆曲线，令平面椭圆曲线表达式为：a>0且b>0，其中参数a和b的求法如下：那么平面椭圆曲线上的点在其坐标系下表示为：其中θ为平面椭圆曲线对应的离心角。步骤二：根据转换后的平面椭圆曲线用加速romberg数值积分法计算出空间椭圆曲线弧长，并控制所述空间椭圆曲线弧长的计算精度满足实际工程要求。弧长表达式如下:由于此积分属于第二类椭圆积分，因此不能直接对其积分，需要用到数值积分对其进行求解。本文根据转换后的平面椭圆曲线用加速romberg数值积分法计算出空间椭圆曲线弧长，并且为了在满足工程实际精度要求的同时，获得高效的计算结果，令积分误差为em，其具体算法如下：步骤1：确定积分上下限θmin和θmax、被积函数f(x)、积分次数k以及精度要求em；步骤2：求出梯形公式所得的积分值t1(0)，其表达式如下：步骤3：对积分区间进行k等分，并通过外推，得到复化梯形公式t1(l)，其表达式如下：其中：m＝1,2,…l，k＝1,2…,l-m+1步骤4：采用richardson外推法，构造新序列其表达式如下：步骤5：判断是否满足积分精度，其判别式为：若满足，则返回弧长值否则返回到步骤4，计算出t1(l+1)，直到判别式成立。其中被积函数f(x)的表达式为：为了更好地表达计算弧长的过程，给出其算法流程框图如图3所示。步骤三：根据设置的柔性加减速曲线，进行所述平面椭圆曲线的速度运动规划，获得所有插补时刻对应的弧长。步骤四：对所述平面椭圆曲线所有插补时刻对应插补点的离心角进行初始化，通过加速romberg数值积分法获得当前插补时刻与下一插补时刻的弧长，通过牛顿迭代法控制该段弧长的计算精度，依次迭代计算直至得出所有插补时刻对应插补点的离心角。在机器人或者数控系统中，针对特定的椭圆轨迹，首先确定出轨迹长度，然后运用柔性加减速运动规律，从而可以获得任意时刻ti(i＝1,…,t)对应的弧长si(i＝1,…,t)，为了求得椭圆轨迹上插补点的位置信息，本发明通过加速romberg数值积分法获得当前插补时刻与下一插补时刻的弧长，并运用牛顿迭代法控制该段弧长的计算精度，依次迭代计算直至得出所有插补时刻对应插补点的离心角，其具体流程如下所示：步骤1：对离心角进行初始化：θ1＝0，并令romberg积分上限为：θup＝θ1+0.001；步骤2：利用上述加速romberg数值积分算法求出g，其中g代表θ1到θup之间的弧长，被积函数f(x)的表达式为：积分次数k＝100以及精度要求em＝0.0000005mm；步骤3：构造函数fx＝g-s2，其中s2为t2时刻对应的位移；步骤4：由于u＝acosθ，v＝bsinθ，那么椭圆弧微分表达式为：且令根据牛顿迭代算法有：temp_θ＝θup-fx/fxd(12)步骤5：判断精度条件：|temp_θ-θup|<em是否成立，若成立，那么下一时刻插补点对应的离心角为：θ2＝temp_θ，否则，令θup＝temp_θ，返回到步骤2中，直到步骤五中的判据条件成立；由上述算法可以快速算出t2时刻插补点的坐标，而ti(i＝3,…,t)所对应的离心角θi(i＝3,…,t)的求法与上述流程相似，只不过将步骤一中θ1替换成θi-1，将步骤三中的s2替换成si，那么由当前时刻对应的θi计算下一时刻θi+1算法流程框图如图4所示。步骤五：根据离心角与插补点坐标之间的对应关系，从而计算出所述平面椭圆曲线所在的参数坐标系下的插补点的坐标，然后经过齐次反变换转换到对应空间椭圆曲线的插补点坐标。对于任意时刻ti(i＝1,…,t)对应的θi(i＝1,…,t)已经求得，那么根据θi与插补点的关系如图5所示可得：然后经过齐次变换转换到对应空间椭圆曲线对应的插补点坐标为：(xiyizi)，因此，各轴的进给量为：本发明提出的一种高精度的空间椭圆曲线实时插补算法，其插补终止的判断条件如下：在给定空间椭圆曲线后，通过步骤一将其转换为平面椭圆曲线，然后根据设置的柔性加减速规划(如s型加减速规划)获得总插补时间t，以1ms作为一个脉冲，直到插补结束为止。为了验证本发明所提算法的高精度，先给出仿真实验验证过程。以半椭圆逆时针插补为例，令经过齐次坐标变换后的椭圆曲线为：其中插补起点坐标为(152.50)，中间点坐标为(050)，终点坐标为(-152.50)，因此可以计算出a＝152.5，b＝50。那么首先经过步骤二的算法求出弧长，然后采用s型加减速运动规划得出任意时刻ti(i＝1,…,t)对应的椭圆弧长段si(i＝1,…,t)，再通过步骤四的算法得到任意时刻ti(i＝1,…,t)对应插补点坐标，最后运用步骤五得到对应空间椭圆曲线插补点。实验结果如图6所示，上图为空间椭圆曲线，下图为平面椭圆曲线，其表明该方法获得的椭圆插补曲线高度吻合理想的椭圆曲线，并通过对终点误差进行分析，由于半椭圆曲线对应的离心角为π，结果表明终点对应的离心角：θ＝3.141592653589794，而π＝3.141592653589793，其相对误差为3.183×10-16，可见此插补算法具有极高的精度。为了验证本发明所提出的椭圆曲线插补方法的精确性，将其与三点高斯积分法和弧微分近似法进行对比，比较三者所得的中间插补点和终点插补点的误差大小，如表1和表2所示：表1中间插补点误差分析插补方法计算出的中间插补点坐标u向绝对偏差v向绝对偏差三点高斯积分法(-6.825046.6794)-6.8250-3.3206弧微分(-0.134150.0000)-0.13410.0000本发明方法(0.000050.0000)0.00000.0000表2终点插补点误差分析插补方法计算出的终点坐标u向绝对偏差v向绝对偏差三点高斯积分法(-169.36687.8857)-16.86687.8857弧微分(-151.4998-0.0221)1.0002-0.0221本发明方法(-152.50000.0000)0.00000.0000由表1和表2所示，本发明的方法计算椭圆曲线插补点的精度均比三点高斯积分法和弧微分近似法高。对于椭圆曲线插补点的求法，三点高斯积分法和弧微分法均需要将椭圆曲线进行分段插补，过程复杂且容易造成相邻段之间的插补点计算不准确。本发明提出的椭圆曲线插补算法不需要对椭圆曲线进行分段插补，因此效率较高，并且计算所得的插补点精度高，通过将当前插补时刻与下一插补时刻椭圆弧长计算和误差控制计算进行模块化，使得程序可移植性好。综上所述，本发明首先建立了空间椭圆曲线与平面椭圆曲线齐次坐标变换的精确数学模型，然后采用加速romberg数值积分算法计算出椭圆弧长，并将控制精度控制在em＝0.0000005mm之内，以满足工程实际需求，再将椭圆弧长段的求法进行模块化，并通过romberg数值积分算法和牛顿迭代算法依次迭代计算直至得出所有插补时刻对应插补点的离心角，根据离心角与插补点坐标之间的对应关系，快速准确地计算出插补点坐标点信息，最后将整个方法进行模块化处理以便实现椭圆曲线实时插补。当前第1页12