CN107102617A - 一种高精度空间椭圆曲线实时插补方法 - Google Patents
一种高精度空间椭圆曲线实时插补方法 Download PDFInfo
- Publication number
- CN107102617A CN107102617A CN201710495450.6A CN201710495450A CN107102617A CN 107102617 A CN107102617 A CN 107102617A CN 201710495450 A CN201710495450 A CN 201710495450A CN 107102617 A CN107102617 A CN 107102617A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- curve
- msup
- interpolation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/404—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control arrangements for compensation, e.g. for backlash, overshoot, tool offset, tool wear, temperature, machine construction errors, load, inertia
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/35—Nc in input of data, input till input file format
- G05B2219/35408—Calculate new position data from actual data to compensate for contour error
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Complex Calculations (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种高精度空间椭圆曲线实时插补方法，通过将空间椭圆曲线转换到平面椭圆曲线，然后给定计算精度，通过加速Romberg数值积分法精确地计算出椭圆曲线长度，再对其进行速度规划，通过加速Romberg数值积分法结合牛顿迭代法的插补算法得到下一插补时刻对应插补点的坐标值，然后经过齐次坐标反变换，得到空间椭圆曲线对应插补点的坐标值。本发明可以保证空间椭圆曲线插补点的精度，并且整个方法流程可以实现程序模块化，对于机器人进行拾放操作的轨迹规划以及数控技术等领域具有重要意义。
Description
技术领域
本发明涉及数控机床和工业机器人领域，特别涉及一种高精度空间椭圆曲线实时插补方法。
背景技术
随着《中国制造2025》以及“工业4.0”的提出，工业机器人以及高档数控技术的发展上升到国家战略层面。无论对于工业机器人还是数控机床而言，其轨迹规划的优劣直接决定了控制效果，而插补算法作为轨迹规划的核心，其目的在于获得的插补点精确地着落在指定的运动轨迹上，并使得机器在运行过程中的振动较小，以快速精准的动作指令进行操作。
在工业机器人和数控机床实际操作过程中，常常需要以椭圆曲线轨迹进行拾放物体和加工零部件。通常机器人和机床在进行椭圆轨迹插补时，首先确定其轨迹长度，然后根据设置的柔性加减速规划，将连续的椭圆曲线轨迹离散为一系列的插补点，在每一个脉冲指令下执行插补点的运动，直到达到总的执行时间就终止椭圆曲线的插补。那么椭圆曲线插补的核心问题就是提高其插补精度。提高插补精度就是建立椭圆曲线弧长以及插补点计算的精确数学模型。由于椭圆曲线弧长属于第二类椭圆积分，使得其没有解析解，现有的算法中一是采用三点高斯数值积分法求取得椭圆弧长，二是采用弧微分近似算法求得椭圆曲线对应的离心角，两种方法法均没有对插补精度进行控制，因此不能保证椭圆轨迹插补终点达到工程实际要求；进一步的，在零部件加工轨迹为椭圆曲线时，三点高斯数值积分法无法求取椭圆曲线中间插补点信息，弧微分法精度较差，不能确保所获得的插补点精确地落在理想椭圆轨迹上。
发明内容
本发明的目的在于克服现有技术存在的上述不足，提供一种高精度空间椭圆曲线插补方法，通过将空间椭圆曲线转换到平面椭圆曲线，然后给定计算精度，通过加速Romberg数值积分法精确地计算出椭圆曲线长度，再对其进行速度规划，通过加速Romberg数值积分法结合牛顿迭代法的插补算法得到下一插补时刻对应插补点的坐标值，然后经过齐次坐标反变换，得到空间椭圆曲线对应插补点的坐标值。本发明可以保证空间椭圆曲线插补点的精度，并且整个方法流程可以实现程序模块化，对于机器人进行拾放操作的轨迹规划以及数控技术等领域具有重要意义。
为了实现上述发明目的，本发明提供了以下技术方案：
一种高精度空间椭圆曲线实时插补方法，包括以下步骤：
步骤一：将空间椭圆曲线经过齐次坐标变换转换为平面椭圆曲线；
步骤二：根据转换后的平面椭圆曲线用加速Romberg数值积分法计算出空间椭圆曲线弧长，并控制所述空间椭圆曲线弧长的计算精度满足实际工程要求；
步骤三：根据设置的柔性加减速曲线，进行所述平面椭圆曲线的速度运动规划，得出总插补时间T，获得所有插补时刻对应平面椭圆曲线起始点的平面椭圆曲线段弧长；
步骤四：从平面椭圆曲线起始点的插补时刻开始，对所述平面椭圆曲线当前插补时刻的下一插补时刻对应插补点的离心角进行初始化，根据初始化的离心角通过加速Romberg数值积分法计算起始点对应插补时刻与当前插补时刻对应的下一插补时刻之间的平面椭圆曲线段弧长，计算精度根据步骤三所获得的所述下一插补时刻相对应的插补时刻的平面椭圆曲线段弧长通过牛顿迭代法进行控制，在满足计算精度的条件下得到下一插补时刻的平面椭圆曲线段弧长所对应的离心角；
步骤五：根据平面椭圆曲线离心角与插补点坐标之间的对应关系，计算出所述平面椭圆曲线所在的参数坐标系的下一插补时刻插补点的坐标，然后经过齐次反变换转换到对应空间椭圆曲线的下一插补时刻插补点的坐标；
步骤六：将下一插补时刻赋值为当前插补时刻，判断当前插补时刻是否为总插补时间T，若是则终止插补，否则返回步骤四。
进一步的，所述的步骤一中，所述齐次坐标转换过程如下：
令XYZ直角坐标系上的空间椭圆曲线PAPB上点的坐标为：P＝(x y z)T，其中PA为曲线上的起始点，PB为曲线上的终点，经过齐次坐标变换将所述空间椭圆曲线转换为XZ平面上的平面椭圆曲线，且令XZ平面上的平面椭圆曲线上点的坐标为：P′＝(x′ 0 z′)，那么将平面椭圆曲线上的点转换到空间椭圆曲线上的点的公式为：
P＝T*P′+P0
其中矩阵T为3×3矩阵，表示平面椭圆曲线转换到空间椭圆曲线的旋转矩阵，P0表示平面椭圆曲线转换到空间椭圆曲线的平移量，并且P0为空间椭圆曲线的中心点坐标值；
将空间椭圆曲线上的点经过齐次坐标转换到平面椭圆曲线上的点的公式为：
P′＝T-1*(P-P0)
其中矩阵T-1是矩阵T的逆矩阵；
设空间椭圆曲线的起始点PA坐标为：(xA yA zA)T，终点PB坐标为：(xB yB zB)T，且所述空间椭圆曲线上中点PC的坐标为：(xC yC zC)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＝(x y z)T，经过齐次坐标变换可将此空间椭圆曲线转换为XZ平面上的椭圆曲线，且令XZ平面上的椭圆曲线上点的坐标为：P′＝(x′ 0 z′)；那么将平面椭圆曲线上的点转换到空间椭圆曲线上的点为：
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坐标为：(xA yA zA)T，终点PB坐标为：(xB yB zB)T，且所述空间椭圆曲线上中点PC的坐标为：(xC yC zC)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所示可得：
然后经过齐次变换转换到对应空间椭圆曲线对应的插补点坐标为：(xi yi zi)，因此，各轴的进给量为：
本发明提出的一种高精度的空间椭圆曲线实时插补算法，其插补终止的判断条件如下：在给定空间椭圆曲线后，通过步骤一将其转换为平面椭圆曲线，然后根据设置的柔性加减速规划(如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.8250 46.6794) | -6.8250 | -3.3206 |
| 弧微分 | (-0.1341 50.0000) | -0.1341 | 0.0000 |
| 本发明方法 | (0.0000 50.0000) | 0.0000 | 0.0000 |
表2终点插补点误差分析
| 插补方法 | 计算出的终点坐标 | u向绝对偏差 | v向绝对偏差 |
| 三点高斯积分法 | (-169.3668 7.8857) | -16.8668 | 7.8857 |
| 弧微分 | (-151.4998 -0.0221) | 1.0002 | -0.0221 |
| 本发明方法 | (-152.5000 0.0000) | 0.0000 | 0.0000 |
由表1和表2所示，本发明的方法计算椭圆曲线插补点的精度均比三点高斯积分法和弧微分近似法高。对于椭圆曲线插补点的求法，三点高斯积分法和弧微分法均需要将椭圆曲线进行分段插补，过程复杂且容易造成相邻段之间的插补点计算不准确。本发明提出的椭圆曲线插补算法不需要对椭圆曲线进行分段插补，因此效率较高，并且计算所得的插补点精度高，通过将当前插补时刻与下一插补时刻椭圆弧长计算和误差控制计算进行模块化，使得程序可移植性好。
综上所述，本发明首先建立了空间椭圆曲线与平面椭圆曲线齐次坐标变换的精确数学模型，然后采用加速Romberg数值积分算法计算出椭圆弧长，并将控制精度控制在em＝0.0000005mm之内，以满足工程实际需求，再将椭圆弧长段的求法进行模块化，并通过Romberg数值积分算法和牛顿迭代算法依次迭代计算直至得出所有插补时刻对应插补点的离心角，根据离心角与插补点坐标之间的对应关系，快速准确地计算出插补点坐标点信息，最后将整个方法进行模块化处理以便实现椭圆曲线实时插补。
Claims (5)
1.一种高精度空间椭圆曲线实时插补方法，其特征在于，包括以下步骤：
步骤一：将空间椭圆曲线经过齐次坐标变换转换为平面椭圆曲线；
步骤二：根据转换后的平面椭圆曲线用加速Romberg数值积分法计算出空间椭圆曲线弧长，并控制所述空间椭圆曲线弧长的计算精度满足实际工程要求；
步骤三：根据设置的柔性加减速曲线，进行所述平面椭圆曲线的速度运动规划，得出总插补时间T，获得所有插补时刻对应平面椭圆曲线起始点的平面椭圆曲线段弧长；
步骤四：从平面椭圆曲线起始点的插补时刻开始，对所述平面椭圆曲线当前插补时刻的下一插补时刻对应插补点的离心角进行初始化，根据初始化的离心角通过加速Romberg数值积分法计算起始点对应插补时刻与当前插补时刻对应的下一插补时刻之间的平面椭圆曲线段弧长，计算精度根据步骤三所获得的所述下一插补时刻相对应的插补时刻的平面椭圆曲线段弧长通过牛顿迭代法进行控制，在满足计算精度的条件下得到下一插补时刻的平面椭圆曲线段弧长所对应的离心角；
步骤五：根据平面椭圆曲线离心角与插补点坐标之间的对应关系，计算出所述平面椭圆曲线所在的参数坐标系的下一插补时刻插补点的坐标，然后经过齐次反变换转换到对应空间椭圆曲线的下一插补时刻插补点的坐标；
步骤六：将下一插补时刻赋值为当前插补时刻，判断当前插补时刻是否为总插补时间T，若是则终止插补，否则返回步骤四。
2.根据权利要求1所述的高精度空间椭圆曲线实时插补方法，其特征在于，所述的步骤一中，所述齐次坐标转换过程如下：
令XYZ直角坐标系上的空间椭圆曲线PAPB上点的坐标为：P＝(x y z)T，其中PA为曲线上的起始点，PB为曲线上的终点，经过齐次坐标变换将所述空间椭圆曲线转换为XZ平面上的平面椭圆曲线，且令XZ平面上的平面椭圆曲线上点的坐标为：P′＝(x′ 0 z′)，那么将平面椭圆曲线上的点转换到空间椭圆曲线上的点的公式为：
P＝T*P′+P0
其中矩阵T为3×3矩阵，表示平面椭圆曲线转换到空间椭圆曲线的旋转矩阵，P0表示平面椭圆曲线转换到空间椭圆曲线的平移量，并且P0为空间椭圆曲线的中心点坐标值；
将空间椭圆曲线上的点经过齐次坐标转换到平面椭圆曲线上的点的公式为：
P′＝T-1*(P-P0)
其中矩阵T-1是矩阵T的逆矩阵；
设空间椭圆曲线的起始点PA坐标为：(xA yA zA)T，终点PB坐标为：(xB yB zB)T，且所述空间椭圆曲线上中点PC的坐标为：(xC yC zC)T，将空间椭圆曲线经过齐次坐标变换到平面椭圆曲线，令平面椭圆曲线表达式为：a>0且b>0，其中参数a和b的求法如下：
<mrow>
<mi>a</mi>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>B</mi>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>A</mi>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mi>B</mi>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>A</mi>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>B</mi>
</msub>
<mo>-</mo>
<msub>
<mi>z</mi>
<mi>A</mi>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mrow>
<mrow>
<mi>b</mi>
<mo>=</mo>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>C</mi>
</msub>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>x</mi>
<mi>A</mi>
</msub>
<mo>+</mo>
<msub>
<mi>x</mi>
<mi>B</mi>
</msub>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mi>C</mi>
</msub>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>y</mi>
<mi>A</mi>
</msub>
<mo>+</mo>
<msub>
<mi>y</mi>
<mi>B</mi>
</msub>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>C</mi>
</msub>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>z</mi>
<mi>A</mi>
</msub>
<mo>+</mo>
<msub>
<mi>z</mi>
<mi>B</mi>
</msub>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mrow>
那么平面椭圆曲线上的点在参数坐标系下表示为：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
<mo>=</mo>
<mi>a</mi>
<mi> </mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&theta;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
<mo>=</mo>
<mi>b</mi>
<mi> </mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&theta;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
其中θ为平面椭圆曲线对应的离心角。
3.根据权利要求2所述的高精度空间椭圆曲线实时插补方法，其特征在于，所述的步骤二中，所述平面椭圆曲线弧长计算过程如下：
平面椭圆曲线弧长表达式如下:
<mrow>
<mi>l</mi>
<mo>=</mo>
<mn>4</mn>
<mo>&times;</mo>
<msubsup>
<mo>&Integral;</mo>
<mn>0</mn>
<mfrac>
<mi>&pi;</mi>
<mn>4</mn>
</mfrac>
</msubsup>
<msqrt>
<mrow>
<msup>
<mi>a</mi>
<mn>2</mn>
</msup>
<msup>
<mi>sin</mi>
<mn>2</mn>
</msup>
<mi>&theta;</mi>
<mo>+</mo>
<msup>
<mi>b</mi>
<mn>2</mn>
</msup>
<msup>
<mi>cos</mi>
<mn>2</mn>
</msup>
<mi>&theta;</mi>
</mrow>
</msqrt>
<mi>d</mi>
<mi>&theta;</mi>
</mrow>
根据平面椭圆曲线用加速Romberg数值积分法计算出空间椭圆曲线弧长，具体步骤如下：
步骤1：确定积分上下限θmin和θmax、被积函数f(x)、积分次数k以及精度要求em，所述被积函数f(x)的表达式为：
步骤2：求出梯形公式所得的积分值其表达式如下：
<mrow>
<msubsup>
<mi>T</mi>
<mn>1</mn>
<mrow>
<mo>(</mo>
<mn>0</mn>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
</mrow>
</msub>
</mrow>
<mn>2</mn>
</mfrac>
<mo>&lsqb;</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
步骤3：对积分区间进行k等分，并通过外推，得到复化梯形公式其表达式如下：
<mrow>
<msubsup>
<mi>T</mi>
<mn>1</mn>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mfenced open = "{" close = "}">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>T</mi>
<mn>1</mn>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msubsup>
<mo>+</mo>
<mfrac>
<mrow>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
</mrow>
</msub>
</mrow>
<msup>
<mn>2</mn>
<mrow>
<mi>l</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msup>
<mn>2</mn>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</munderover>
<mi>f</mi>
<mo>&lsqb;</mo>
<mi>a</mi>
<mo>+</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mfrac>
<mrow>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
</mrow>
</msub>
</mrow>
<msup>
<mn>2</mn>
<mi>l</mi>
</msup>
</mfrac>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中：m＝1,2,…l，k＝1,2…,l-m+1；
步骤4：采用Richardson外推法，构造新序列其表达式如下：
<mrow>
<msubsup>
<mi>T</mi>
<mrow>
<mi>m</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<mrow>
<msup>
<mn>4</mn>
<mi>m</mi>
</msup>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msup>
<mn>4</mn>
<mi>m</mi>
</msup>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mfrac>
</mrow>
步骤5：判断是否满足积分精度，其判别式为：若满足，则返回弧长值否则返回到步骤4，计算出直到判别式成立。
4.根据权利要求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θ，平面椭圆曲线弧微分表达式为：
<mrow>
<mi>d</mi>
<mi>s</mi>
<mo>=</mo>
<msqrt>
<mrow>
<msup>
<mi>a</mi>
<mn>2</mn>
</msup>
<msup>
<mi>sin</mi>
<mn>2</mn>
</msup>
<mi>&theta;</mi>
<mo>+</mo>
<msup>
<mi>b</mi>
<mn>2</mn>
</msup>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&theta;</mi>
</mrow>
</msqrt>
<mi>d</mi>
<mi>&theta;</mi>
</mrow>
且令根据牛顿迭代法有：
temp_θ＝θup-Fx/Fxd；
步骤5：判断精度条件：|temp_θ-θup|<em是否成立，若成立，则下一插补时刻插补点对应的离心角为：θ2＝temp_θ，否则，令θup＝temp_θ，返回到步骤2中，直到步骤5中的判据条件成立；
由上计算出t2插补时刻对应插补点的坐标，ti(i＝3,…,T)插补时刻所对应的离心角θi(i＝3,…,T)以此类推进行计算，最终求出所有插补时刻对应插补点的离心角。
5.根据权利要求4所述的高精度空间椭圆曲线实时插补方法，其特征在于，空间椭圆曲线上插补点的坐标计算过程如下：
根据所有插补时刻ti(i＝1,…,T)对应插补点的离心角θi(i＝1,…,T)，离心角θi与插补点坐标的关系所示可得：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mi>a</mi>
<mi> </mi>
<msub>
<mi>cos&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>v</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mi>b</mi>
<mi> </mi>
<msub>
<mi>sin&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mo>...</mo>
<mo>,</mo>
<mi>T</mi>
<mo>)</mo>
</mrow>
</mrow>
如此得到平面椭圆曲线所有插补时刻对应的插补点坐标，然后经过齐次反变换得到对应空间椭圆曲线的插补点坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710495450.6A CN107102617B (zh) | 2017-06-26 | 2017-06-26 | 一种高精度空间椭圆曲线实时插补方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710495450.6A CN107102617B (zh) | 2017-06-26 | 2017-06-26 | 一种高精度空间椭圆曲线实时插补方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107102617A true CN107102617A (zh) | 2017-08-29 |
| CN107102617B CN107102617B (zh) | 2019-04-09 |
Family
ID=59663478
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710495450.6A Active CN107102617B (zh) | 2017-06-26 | 2017-06-26 | 一种高精度空间椭圆曲线实时插补方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107102617B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107885166A (zh) * | 2017-10-31 | 2018-04-06 | 北京航空航天大学 | 一种空间圆锥曲线通用插补方法 |
| CN108941845A (zh) * | 2018-08-28 | 2018-12-07 | 苏州艾利特机器人有限公司 | 一种弧焊机器人空间圆弧摆焊插补方法 |
| CN110561433A (zh) * | 2019-08-31 | 2019-12-13 | 中山大学 | 一种提高机械臂运动规划控制精度的方法 |
| CN111208171A (zh) * | 2020-01-12 | 2020-05-29 | 中国人民解放军国防科技大学 | 一种基于电磁周期结构阻抗特性判别透过率性能的方法 |
| CN111580453A (zh) * | 2020-05-26 | 2020-08-25 | 南大傲拓科技江苏股份有限公司 | 基于plc的提高二维圆弧插补轨迹精度的实现方法 |
| CN113325806A (zh) * | 2021-06-01 | 2021-08-31 | 北华航天工业学院 | 一种参数曲线直接插补进给速度规划方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102147599A (zh) * | 2011-03-30 | 2011-08-10 | 华南理工大学 | 椭圆弧插补方法 |
| CN103149879A (zh) * | 2011-12-07 | 2013-06-12 | 沈阳高精数控技术有限公司 | 一种基于弧长的数控***椭圆插补方法 |
| CN104408226A (zh) * | 2014-10-23 | 2015-03-11 | 郑州斯倍思机电有限公司 | 一种基于离心角变化的管板焊接的椭圆插补方法 |
-
2017
- 2017-06-26 CN CN201710495450.6A patent/CN107102617B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102147599A (zh) * | 2011-03-30 | 2011-08-10 | 华南理工大学 | 椭圆弧插补方法 |
| CN103149879A (zh) * | 2011-12-07 | 2013-06-12 | 沈阳高精数控技术有限公司 | 一种基于弧长的数控***椭圆插补方法 |
| CN104408226A (zh) * | 2014-10-23 | 2015-03-11 | 郑州斯倍思机电有限公司 | 一种基于离心角变化的管板焊接的椭圆插补方法 |
Non-Patent Citations (1)
| Title |
|---|
| 陈贵银: "比较积分法椭圆插补的研究", 《武汉船舶职业技术学院学报》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107885166A (zh) * | 2017-10-31 | 2018-04-06 | 北京航空航天大学 | 一种空间圆锥曲线通用插补方法 |
| CN107885166B (zh) * | 2017-10-31 | 2020-01-21 | 北京航空航天大学 | 一种空间圆锥曲线通用插补方法 |
| CN108941845A (zh) * | 2018-08-28 | 2018-12-07 | 苏州艾利特机器人有限公司 | 一种弧焊机器人空间圆弧摆焊插补方法 |
| CN108941845B (zh) * | 2018-08-28 | 2021-08-24 | 苏州艾利特机器人有限公司 | 一种弧焊机器人空间圆弧摆焊插补方法 |
| CN110561433A (zh) * | 2019-08-31 | 2019-12-13 | 中山大学 | 一种提高机械臂运动规划控制精度的方法 |
| CN110561433B (zh) * | 2019-08-31 | 2022-07-19 | 中山大学 | 一种提高机械臂运动规划控制精度的方法 |
| CN111208171A (zh) * | 2020-01-12 | 2020-05-29 | 中国人民解放军国防科技大学 | 一种基于电磁周期结构阻抗特性判别透过率性能的方法 |
| CN111580453A (zh) * | 2020-05-26 | 2020-08-25 | 南大傲拓科技江苏股份有限公司 | 基于plc的提高二维圆弧插补轨迹精度的实现方法 |
| CN111580453B (zh) * | 2020-05-26 | 2023-03-31 | 傲拓科技股份有限公司 | 基于plc的提高二维圆弧插补轨迹精度的实现方法 |
| CN113325806A (zh) * | 2021-06-01 | 2021-08-31 | 北华航天工业学院 | 一种参数曲线直接插补进给速度规划方法 |
| CN113325806B (zh) * | 2021-06-01 | 2022-04-15 | 北华航天工业学院 | 一种参数曲线直接插补进给速度规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107102617B (zh) | 2019-04-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107102617A (zh) | 一种高精度空间椭圆曲线实时插补方法 | |
| CN108673509B (zh) | 一种六自由度手腕偏置型串联机械臂的运动控制方法 | |
| CN105014677B (zh) | 基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法 | |
| CN109159151A (zh) | 一种机械臂空间轨迹跟踪动态补偿方法和*** | |
| CN105353725A (zh) | 用于工业机器人的过辅助点姿态空间圆弧插补方法 | |
| Rezende et al. | Constructive time-varying vector fields for robot navigation | |
| CN110757454A (zh) | 一种双机器人协同旋转的路径规划方法和装置 | |
| CN104408299B (zh) | 基于距离识别冗余运动学参数的机器人位置误差补偿方法 | |
| CN105382835A (zh) | 一种可穿越腕部奇异点的机器人路径规划方法 | |
| CN105159294A (zh) | 针对叉车模糊逻辑运动控制器的设计方法 | |
| CN107992647A (zh) | 一种受几何结构影响的局部并联尺寸链误差获取方法 | |
| CN106346480A (zh) | 一种基于ug和matlab的多自由度注塑机械臂建模方法 | |
| CN104240263B (zh) | 一种用于Delta并联机械手的运动特征分析*** | |
| CN112720480B (zh) | 一种基于分级误差的机器人轨迹修正方法及*** | |
| Xu et al. | Inverse kinematics for 6-DOF serial manipulators with offset or reduced wrists via a hierarchical iterative algorithm | |
| CN106338966B (zh) | 一种工业机器人轨迹规划编程方法 | |
| CN104793568A (zh) | 多轴插补方法 | |
| CN110450165A (zh) | 一种基于零力控制的机器人标定方法 | |
| CN112693631B (zh) | 飞行器在线序列凸优化中的初始轨迹生成方法及*** | |
| CN115741672B (zh) | 一种基于刚体变换的dh推导方法 | |
| CN104133423B (zh) | 一种空间椭圆弧插补方法 | |
| Somasundar et al. | Inverse Jacobian programming approach to robotic path planning of various path profiles | |
| CN116476081B (zh) | 一种机器人节律性技能学习方法及*** | |
| Ye et al. | Uncalibrated visual predictive control using Broyden estimation | |
| CN108073162A (zh) | 一种通过***运动段实现机器人平滑运动的控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20190212 Address after: 215000 Suzhou Industrial Park and 28 Heshun Road, Jiangsu Province Applicant after: Suzhou elite robot Co., Ltd. Address before: Room C202A, 2nd floor, B-6 Building, Dongsheng Science Park, Zhongguancun, 66 Xixiaokou Road, Haidian District, Beijing, 100192 Applicant before: Beijing Ailite Technology Co. Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |