CN1900864A - 一种用于五轴五环并联运动机构运动控制的逆解数学算法 - Google Patents

Abstract

本发明属于机械技术领域，具体涉及一种用于五轴五环并联运动机构的逆解数学算法。其具体步骤为：建立坐标系，选取其中一个支链，将选取的单支链由空间矢量转化成平面矢量，求得刀位矢量，然后根据矢量关系求得矢量qα。最后通过数学软件Matlab求得杆件长度与刀具位姿和刀尖位置的函数关系，杆长L_i即为矢量qα的模(长度)。本发明所作的逆解数学算法，是用于五轴五环并联机床运动控制的。当五根杆环住的主轴中心刀尖需走出既定路线时，五根杆应按照怎样的模式伸缩，这需要用数学函数定义出刀尖轨迹与杆长的关系式。本发明是研制并联机床的关键技术，同时也是并联机床速度分析、加速度分析、工作空间分析、运动学性能评价、数控系统算法、运动误差及动力学研究的基础。本发明对于我国跟上世界并联机床发展步伐是很有意义的。

Description

一种用于五轴五环并联运动机构运动控制的逆解数学算法

技术领域

本发明属于机械技术领域，具体涉及一种用于五轴五环并联运动机构运动控制的逆解数学算法。

背景技术

随着工业现代化的发展，对机床的技术指标提出了更高的要求，新一代机床的发展趋势是进一步满足超精密、超高速、激光和细微加工等新工艺提出的高性能和高集成度要求。近年来，并联运动机床的出现就顺应了此趋势。并联运动机床是以空间并联机构为基础，充分利用计算机数字控制潜力，以软件取代部分硬件，以电气装置和电子器件取代部分机械传动，使将近两个世纪来的以笛卡尔坐标直线位移为基础的机床机构和运动学原理发生了根本变化。

从机床布局上来看，并联运动机床与传统数控机床的区别主要表现在：

传统机床布局的基本特点是以床身、立柱、横梁等作为支承部件，主轴部件和工作台的滑板沿支承部件上的直线导轨移动，按照X、Y、Z坐标运动叠加的串联运动学原理，形成加工表面轨迹。

并联运动机床布局的基本特点是，以机床框架为固定平台和若干杆件组成空间并联机构，主轴部件安装在并联机构的动平台上，改变杆件的长度或移动杆的支点，按照并联运动学原理形成刀头点的加工表面轨迹。并联运动机床具有刚度强、动态性能好、机床模块化程度高、易于重构以及机械结构简单等优点，它的出现引起国际学术界和工程界的极大关注，一些发达国家政府和研究机构纷纷投入大量人力、物力，研究和开发可加工复杂曲面的并联机床。并联运动机床结构成为新一代机床结构的发展方向。并联运动机床与传统机床比较，具有以下优点：

1)运动部件质量小，运动惯性小；

2)高运动速度和高加速度，适合高速加工；

3)主要部件具有重复性，通用程度高；

4)容易通过预加载荷，提高机床部件的刚度；

5)通过控制系统可以实现运动精度的补偿。

并联运动机床在国内外的发展历程将近十年，1994年，在美国芝加哥国际机床展览会上，美国Giddings&Lewis公司首次展出了Variax型并联运动机床，引起轰动。它是一台以Stewart平台为基础的5坐标立式加工中心，标志着机床设计开始采用并联机构，是机床结构重大改革的里程碑。1997年在德国汉诺威国际机床博览会(EMO97)和1999年巴黎国际机床博览会(EMO99)上有很多并联机床参展，其中德国Mikromat公司生产的6X型高速立式加工中心，美国Ingersoll公司生产的HOH-600型卧式并联加工中心，英国Geodetic公司生产的EvolutionG500和G1000两款并联加工中心，瑞士联邦技术学院研制的HexaGlide并联机床，日本Toyada公司生产的基于铣削加工线的六条腿机床，瑞典NeosRobotics公司生产的Tricept845和600型并联机床等。俄罗斯Lapic公司将Stewart平台并联机构用于TM-1000型精密加工中心和КИМ-750型3坐标测量机。美国Hexel公司推出低价位的Tornado型5坐标加工中心和P2000铣床工作台，以及6自由度的定位平台和微型机器人等一系列产品。

我国的并联机床领域起步不晚，燕山大学于1991年研制出我国第一台并联机器人样机，并在此基础上作了很多理论研究。清华大学、哈尔滨工业大学、北京理工大学等都先后推出基于Stewart平台的虚拟轴机床的样机。继美国于1994年推出世界第一台VARIAX虚拟轴机床之后，我国也于1996年由清华大学推出了国内第一台VAMTIY虚拟轴机床.随后很多单位开始了并联机床的研制。东北大学于1998年成功研制了五轴联动三杆并联机床DSX5-70。大连机床厂研制了五轴联动并联机床DCB510。并联机床因其诸多优点很可能成为能够适应21世纪灵活多变环境的新一代高速、高柔性、高经济性的数控加工设备，具有广泛的市场前景。

并联运动机床具有2个共同的缺点：第一、有效工作空间与机床体积之比较小。第二、由于运动平台受约束铰链工作角度范围的局限，运动平台偏转角小，虽然有6个自由度，但不能进行五面加工。

在世界各国著名的机床公司相继推出的新产品中，都针对并联机床的缺点进行变型改进，新一代并联运动机床开始进入了实用阶段。其中德国弗朗霍夫机床和压力加工研究所(Fraunhofer Institut fuer Werzeugmaschenen und Umformtechnik)与德国Metrom公司(www.metrom.com)合作研制成功采用5杆并联机构和5环驱动的主轴部件，在并联运动机构理论上有所突破，总体配置设计是德国Michael Schwaar博士的发明，用5个转环替代固定安装的约束铰链，从而实现主轴部件的偏转角大于90°，能够真正实现5轴联动、5面加工。德国Michael Schwaar博士在2001年发明以上五轴五环并联机构，并将其开发成产品，在德国申请了专利，其逆解的数学模型没有报道，在国内的各类期刊及公开场合也没有关于逆解数学模型的报道。

发明内容

本发明的目的在于提出一种用于五轴五环并联运动机构运动控制的逆解数学算法。

并联机床由于其自身的结构特点，不存在沿固定方向导向的导轨，数控加工所需的刀具运动轴X，Y，Z，A，B，C等并不真正存在，不能对其进行直接控制，在任何时刻并联机构的运动都必须是五杆联动，故五个伺服驱动系统之间具有强耦合，非线性的关系。

机构运动学分析的最基本任务是建立输入与输出构件之间位置关系的数学模型，并联机床的运动学数学模型包括运动学方程的正解和逆解问题，它是并联机床的速度分析、加速度分析、受力分析、误差分析及动力学分析的基础。正解问题是指已知各驱动副的长度(或转角)，求活动平台位姿；逆解问题是指已知活动平台位姿，求各驱动副的长度(或转角)。以机床本身加工状况来说，工件的加工轨迹是已知条件，即活动平台位姿是已知的，从而求得各驱动副的长度(或转角)，因此对于机床，逆解更为实用。

并联运动机构用5个转环替代固定安装的约束铰链，机构运动学数学模型和以前的各种并联运动机构相比发生了极大的变化，即五个约束铰链的位置由已知变为未知，本发明提出的用于五轴五环并联运动机构的运动控制逆解数学算法，是采用数学中的矢量法，求得该并联运动机构运动控制逆解的数学模型。

本发明提出的用于五轴五环并联运动机构运动控制的逆解数学算法，其具体步骤如下：

(1)建立坐标系：

将五轴五环并联机构放入坐标系(如图1所示)，以机床工作台作为静平台，静坐标系原点O位于静平台中心，B_i点为机架上的固定点，是Q_i点在静坐标系中的垂直投影，则得：

B_i＝(B cos β_i，B sin β_i，0)^T，i＝1、2…5，

其中，B为静平台半径，β_i为B_i点与x轴成的夹角；

以五环围住的主轴头部为动平台，主轴头部的顶部中心O’为动坐标系原点，动坐标系中的Z’轴和主轴头部轴心线重合，Q_i点则是杆件固联机架的点，

Q_i＝(B cos β_i，B sin β_i，q_i)^T，

其中，q_i为铰接点的高度；

杆与环的铰接处设为A_i点，则A_i点在动平台上的坐标为

A_i＝(A cos α_i，A sin α_i，H_i)^T

其中，A为动平台的半径，H_i为每个环到动坐标系x′-y′面的距离，α_i为A_i点与动坐标系的x轴成的夹角，显然，这是个不定变量，因为它是在无限制转动环上的点；

(2)选取其中一个支链

由于五轴五环并联机构每个支链基本相同，故取出一个支链分析：

R点代表刀尖(如图2所示)，距动平台主轴底部中心处正好为刀的伸出长度，

R＝(x0，y0，z0)^T；

(3)将步骤(2)中选取的单支链由空间矢量转化成平面矢量：

与图2所对应的字母用小写字母表示(如图3所示)，即A_i点用a表示，因为a处的铰链是一自由度的转动副，根据机构学原理可判定，矢量qa和矢量o′r必相交，而矢量o′a的模为动平台主轴头部半径，由空间几何学可证明出三矢量qa、o′r和o′a共面；这样单支链由空间矢量问题转化成平面矢量问题；

(4)减少一个未知数：

以加工一个平面为例，且设定刀的轴心线与加工表面垂直，其方程为hx0(x0-x₁)-hy0(y0-y₁)-hz0(z0-z₁)＝0，

该平面的法向矢量即(hx0，hy0，hz0)，x₁，y₁，z₁是已知常数，令法向为单位矢量，则有hx0²+hy0²+hz0²＝1，这样在三个未知数中就可以用其中两个表示另一个了，减少了一个未知数；hx0，hy0，hz0，x0，y0，z0是方程中的函数变量，法向矢量(hx0，hy0，hz0)决定了动平台主轴刀位的姿态，而x0，y0，z0变量决定了动平台主轴刀尖的位置；

(5)从步骤(4)中得到刀位矢量为o′r＝H_i×(hx0，hy0，hz0)，其中，H_i第i个环中心到刀尖轨迹的距离，即可得出o′点的坐标；

(6)用数学软件Matlab求得杆件长度与刀具位姿的函数关系，即矢量qα的长度求得L_i；

(A)由o′点坐标和q点坐标可得矢量qo′；

(B)由矢量qo′和矢量o′r可得qo′r平面法矢量n₁，矢量n₁为矢量qo′和矢量o′r的叉乘；

(C)由矢量n₁和o′r可得矢量o′α方向的某一矢量n₂，矢量n₂为矢量o′α和矢量o′r的叉乘；

(D)单位化矢量n₂，再乘以环半径A，得到矢量o′α；

(E)得到a坐标；

(F)得到L_i通项：

Li＝((Bxi-A×(hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-x0-Hi×hx0)^2+(Byi-A×((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-y0-Hi×hy0)^2+(qi-A×(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-z0-Hi×(1-hx0^2-hy0^2)^(1/2))^2)^(1/2)。

本发明的有益效果：本发明所作的逆解数学算法，是用于五轴五环并联机床运动控制的。当五根杆环住的主轴中心刀尖需走出既定路线时，五根杆应按照怎样的模式伸缩，这需要用数学函数定义出刀尖轨迹与杆长的关系式。本发明所作的数学模型即为这种关系式。因此这是研制并联机床的关键技术。同时也是并联机床速度分析、加速度分析、工作空间分析、运动学性能评价、数控系统算法、运动误差及动力学研究的基础。本发明所作的逆解数学算法针对五轴五环并联机床，它对于我国跟上世界并联机床发展步伐是很有意义的。

附图说明

图1为五轴五环坐标系示意图。其中，

(a)动平台、静平台及坐标系的相对位置。

(b)动平台(机床主轴)在动坐标系中的投影。

(c)静平台在静坐标系中的投影。

图2为单支链坐标示意图。

图3为支链矢量图。

图4为实施例1中X方向的位移量。

图5为Y方向的位移量。

具体实施方式

下面结合实施例进一步说明本发明。

实施例1：

Q₄坐标，(-630.03，363.75，900)；H₄＝75；

Q₅坐标，(-472.98，-273.07，1444.92)；H₅＝250；

由Q_i点坐标可得B_i＝(B cos β_i，B sin β_i，0)^T的坐标

B₁＝(B_x1，B_y1，0)＝(472.98，-273.07，0)

B₂＝(B_x2，B_y2 0)＝(630.03，363.75，0)

B₃＝(B_x3，B_y3 0)＝(0，510.09，0)

B₄＝(B_x4，B_y4 0)＝(-630.03，363.75，0)L_i通项公式是个参数化公式，即只要是五轴五环并联机构，机构的结构尺寸是可以任意改变的。结构尺寸包括动平台半径A，杆件固联机架的Q_i点，Q_i＝(B cos β_i，B sin β_i，q_i)^T；每个环到动坐标系x′-y′面的距离H_i。

通过机械三维设计软件SolidWorks建模，做出了虚拟的五轴五环并联机床的实体模型，根据实体模型中的结构尺寸，确定出了常量，表示如下：

A＝123.87

Q₁坐标，(472.98，-273.07，1444.92)；H₁＝330；

Q₂坐标，(630.03，363.75，900)；H₂＝25；

Q₃坐标，(0，510.09，1444.92)；H₃＝170；

B₅＝(B_x5，B_y5 0)＝(-472.98，-273.07，0)

q₁＝1444.92

q₂＝900

q₃＝1444.92

q₄＝900

q₅＝1444.92

将Q_i，B_i，q_i，H_i的常量代入L_i，则杆长方程L_i＝f(h_x0，h_y0，h_z0，x₀，y₀，z₀)

显然，杆长方程为杆长和加工表面数学方程之间的函数关系。

常量代入得各杆长方程

L1＝

((472.98-123.87×(hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×x0-330.×hx0)^2+(-273.07-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×y0-330.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×z0-330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)

L2＝

((630.03-123.87×(hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×x0-25.×hx0)^2+(363.75-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×y0-25.×hy0)^2+(900.-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×z0-25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)

L3＝

((-123.87×(hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×x0-170.×hx0)^2+(510.09-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×y0-170.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×z0-170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)

L4＝

((-630.03-123.87×(hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×x0-75.×hx0)^2+(363.75-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×y0-75.×hy0)^2+(900.-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×z0-75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)

L5＝

((-472.98-123.87×(hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×x0-250.×hx0)^2+(-273.07-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×y0-250.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×z0-250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)

3.验证过程

先定出让刀尖走的轨迹，然后对每根杆输入以上的杆长函数，运行后看看刀尖轨迹是不是走的既定轨迹。我们验证了圆，空间斜线，水平线等，得到的结果是正确的。

举例：为了使主轴在XOY平面上走出一个圆心在(-100，0，0)的圆，代入以下方程组

$\left\{\begin{array}{c}{x}_0=100\×\mathrm{COS}\left(\mathrm{time}\right)-100\\ y_0=100\×\mathrm{SIN}\left(\mathrm{time}\right)\\ z_0=0\end{array}\right.$

代入后将此方程组方程分别代入五根杆长函数，在Adams中对应的五根杆处输入杆长函数，经过仿真，可记录主轴刀尖的位移轨迹。圆方程组代入杆长函数，得到：

L1＝SQRT((472.98-(100×COS(time)-100)-123.87×(472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))×(472.98-(100×COS(time)-100)-123.87×(472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))+(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))×(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))+664094.6064)-917.83

L2＝SQRT((630.03-(100×COS(time)-100)-123.87×(630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))×(630.03-(100×COS(time)-100)-123.87×(630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))+(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))×(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))+330625)-833.66

L3＝SQRT((-(100×COS(time)-100)-123.87×(-(100×COS(time)-100))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))×(-(100×COS(time)-100)-123.87×(-(100×COS(time)-100))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))+(510.09-100×SIN(time)-123.87×(510.09-100×SIN(time))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))×(510.09-100×SIN(time)-123.87×(510.09-100×SIN(time))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))+950469.0064)-1048.63

L4＝SQRT((-630.03-(100×COS(time)-100)-123.87×(-630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))×(-630.03-(100×COS(time)-100)-123.87×(-630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))+(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))×(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))+275625)-799.99

L5＝SQRT((-472.98-(100×COS(time)-100)-123.87×(-472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))×(-472.98-(100×COS(time)-100)-123.87×(-472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))+(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))×(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))+800881.8064)-989.545

3.输出结果

主轴刀尖的位移轨迹记录由Adams输出，如图4和图5所示。确实为圆。

Claims (1)

1、一种用于五轴五环并联运动机构运动控制的逆解数学算法，其特征在于具体步骤如下：

(1)建立坐标系：

将五轴五环并联机构放入坐标系，以机床工作台作为静平台，静坐标系原点O位于静平台中心，B_i点为机架上的固定点，是Q_i点在静坐标系中的垂直投影，则得：

B_i＝(Bcosβ_i，Bsinβ_i，0)^T，i＝1、2…5，

其中，B为静平台半径，β_i为B_i点与x轴成的夹角；

以五环围住的主轴头部为动平台，主轴头部的顶部中心O’为动坐标系原点，动坐标系中的Z’轴和主轴头部轴心线重合，Q_i点为杆件固联机架的点，则得：

Q_i＝(Bcosβ_i，Bsinβ_i，q_i)^T，

其中，q_i为铰接点的高度；

杆与环的铰接处设为A_i点，则A_i点在动平台上的坐标为

A_i＝(Acosα_i，Asinα_i，H_i)^T

其中，A为动平台的半径，H_i为每个环到动坐标系x′-y′‘面的距离，α_i为A_i点与动坐标系的x轴成的夹角，显然，这是个不定变量，因为它是在无限制转动环上的点；

(2)选取其中一个支链

由于五轴五环并联机构每个支链基本相同，故取出一个支链分析：

R点代表刀尖，距动平台主轴底部中心处正好为刀的伸出长度，R＝(x0，y0，z0)^T；

(3)将步骤(2)中选取的单支链由空间矢量转化成平面矢量：

A_i点用a表示，a处的铰链为一自由度的转动副，根据机构学原理判定，矢量qa和矢量o′r必相交，而矢量o′a的模为动平台主轴头部半径，由空间几何学证明三矢量qa、o′r和o′a共面；则单支链由空间矢量问题转化成平面矢量问题；

(4)减少一个未知数：

加工一个平面，且设定刀的轴心线与加工表面垂直，其方程为

hx0(x0-x₁)-hy0(y0-y₁)-hz0(z0-z₁)＝0

该平面的法向矢量即(hx0，hy0，hz0)，x₁，y₁，z₁是已知常数，令法向为单位矢量，则有hx0²+hy0²+hz0²＝1，这样在三个未知数中就减少了一个未知数；hx0，hy0，hz0，x0，y0，z0是方程中的函数变量，法向矢量(hx0，hy0，hz0)决定动平台主轴刀位的姿态，x0，y0，z0变量决定动平台主轴刀尖的位置；

(5)从步骤(4)中得到刀位矢量为o′r＝H_i×(hx0，hy0，hz0)，

其中，H_i第i个环中心到刀尖轨迹的距离，即可得出o′点的坐标；

(6)用数学软件Matlab求得杆件长度与刀具位姿的函数关系，即矢量qα的长度求得L_i；

(A)由o′点坐标和q点坐标可得矢量qo′；

(B)由矢量qo′和矢量o′r可得qo′r平面法矢量n₁，矢量n₁为矢量qo′和矢量o′r的叉乘；

(C)由矢量n₁和o′r可得矢量o′α方向的某一矢量n₂，矢量n₂为矢量o′α和矢量o′r的叉乘；

(D)单位化矢量n₂，再乘以环半径A，得到矢量o′α；

(E)得到a坐标；

(F)得到L_i通项：

Li＝((Bxi-A×(hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-x0-Hi×hx0)^2+(Byi-A×((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-y0-Hi×hy0)^2+(qi-A×(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-z0-Hi×(1-hx0^2-hy0^2)^(1/2))^2)^(1/2)。

Images