CN103760816B - 基于任务极坐标系的伺服系统轮廓控制方法 - Google Patents

Abstract

本发明涉及伺服系统轮廓控制方法，尤其涉及一种基于任务极坐标系的伺服系统轮廓控制方法。本发明提供了一种基于任务极坐标系的伺服系统轮廓控制方法，包括以下步骤：S1、建立XY运动平台动力学方程；S2、根据期望轨迹信息，基于密切圆逼近建立任务极坐标系，并计算相应的坐标变换关系；S3、将笛卡尔坐标系下的系统动力学方程转换为任务极坐标下的误差动力学方程；S4、设计基于前馈补偿的反馈比例?微分（PD）控制器，实现对误差动力学的解耦控制。本发明的有益效果是：基于任务极坐标系的解耦轮廓控制方法能够大幅度降低高速大曲率轮廓加工过程中的轮廓误差。

Description

基于任务极坐标系的伺服系统轮廓控制方法

技术领域

本发明涉及伺服系统轮廓控制方法，尤其涉及一种基于任务极坐标系的伺服系统轮廓控制方法。

背景技术

轮廓控制技术是现代高端自动化设备的关键技术之一，广泛应用于现代制造业的核心装备中，如数控机床、工业机器人、激光加工设备等。轮廓控制的目的是控制多轴伺服系统沿着期望的轨迹运动，尤其是减少与运动方向垂直的方向上的误差。数控机床多轴伺服系统的高速高精度轮廓控制，可以实现良好的工件表面质量，对现代工业具有重要的现实意义和广阔的应用前景。现有的轮廓控制方法存在轮廓误差估计能力不足和高速下轮廓性能严重退化等问题，迫切需要对其进行改进完善。

轮廓误差，即当前实际位置与期望轮廓轨迹的最短距离，是衡量产品加工质量的核心指标。轮廓控制问题主要包括轮廓误差估计和轮廓控制算法两方面。

在轮廓误差估计方面，对于简单的轮廓轨迹，如直线和圆形，可以得到准确的计算公式。但是对于较复杂的自由曲线，轮廓误差的准确计算非常复杂，难以实时实现。对于数控系统而言，通常需要进行轮廓误差在线估计。常用的估计方法可以分为三种：基于局部几何特性的估计方法，基于代数方程的估计方法，基于进给命令的估计方法。

基于局部几何特性的估计方法目前可以分为两类，一类是使用切线辅助估计轮廓误差，另一类则使用密切圆。基于代数方程的估计方法主要有等价轮廓误差方法和正交全局任务坐标系方法等。随着计算机技术的发展，数控系统的数据计算与存储能力大为提高，可以基于进给命令，使用一组参考位置来实时估计轮廓误差。

现有的轮廓控制算法可以分为三类：单轴控制，交叉耦合控制，解耦轮廓控制。

在传统的单轴控制器设计中，一般通过提高单轴跟踪精度来减小轮廓误差。典型的方法包括前馈控制和摩擦力补偿等。

交叉耦合控制在原有的多个单轴控制回路基础上增加了一个轮廓误差控制回路，通过对单轴跟踪误差进行交叉耦合计算得到估计的轮廓误差，然后设计轮廓误差控制器并将控制量通过交叉耦合增益分配给原有的单轴控制回路。

考虑到轮廓误差与参考轮廓运动轨迹相互垂直，可以认为在一般加工过程中，存在两种解耦的运动形式，轨迹跟踪运动（沿着参考轮廓轨迹进行运动）和轮廓跟踪运动（与轨迹跟踪运动方向垂直的运动），就此部分学者提出了解耦轮廓控制思路。

中国发明专利《一种复杂轨迹的轮廓控制方法》（专利号：200710030228.5）公开了一种复杂轨迹的轮廓控制方法，该方法结合一种具有轮廓误差预补偿功能的交叉耦合控制框架，对参与伺服运动的各轴建立自适应数据模型，根据当前目标位置点和若干历史位置点值，确定伺服被控对象待辨识参数，通过极点配置算法实时整定控制参数。

另一种轮廓控制方法是中国发明专利《基于预测控制和交叉耦合的直驱XY平台轮廓控制方法》（专利号：201210359218.7）。这种方法基于预测控制和交叉耦合控制实现了直驱XY平台轮廓控制方法。在单轴的控制中，使用预测控制器减小系统中的跟踪误差，间接提高双轴的定位精度；在双轴上使用交叉耦合控制器进行解耦，直接补偿系统的轮廓误差，提高加工精度。

现有的轮廓误差估计方法中，基于切线逼近的轮廓误差估计方法，对于线性或者曲率较小的轮廓能够取得良好的轮廓误差估计效果，但是对于一般光滑曲线，特别是大曲率曲线，则误差较大。基于局部逼近的轮廓误差估计方法都要求跟踪误差较小，才能取得较好的轮廓误差估计效果，在高速大曲率条件下其估计的有效性会恶化。基于代数方程的估计方法利用了期望轮廓轨迹的几何信息，而与实际运动时间无关，和局部逼近方法相比，能够在更大的范围内保证轮廓误差估计结果的准确性。然而，这种方法需要预先获得整个期望轮廓的解析代数表达式，这对于任意复杂轮廓而言较为困难，故而其适用范围也受到了限制。基于进给命令的估计方法，其效果和插补密度相关，若插补密度较低，则估计效果较差，并且对数控系统计算和存储能力有一定要求。

现有的轮廓控制算法中，单轴跟踪控制都是基于跟踪误差，即实际运动位置与期望运动位置之差。虽然轮廓误差可以认为和跟踪误差相关，但二者并不一致。所以传统的基于跟踪误差的控制方法在实际加工中可能导致轮廓误差增大。单轴跟踪控制着眼于降低系统的跟踪误差，对轮廓误差是开环控制。交叉耦合控制实现了对轮廓误差的闭环控制，但轮廓控制回路和单轴控制回路之间存在耦合作用，导致轮廓性能受到跟踪性能的影响。解耦轮廓控制将笛卡尔世界坐标系下的跟踪误差分解为切向误差和法向的轮廓误差。若双轴系统之间动力学匹配较好，则切向和轮廓误差之间的耦合作用可以忽略，即切向和法向的误差动力学系统是相互解耦的。

中国发明专利《一种复杂轨迹的轮廓控制方法》（专利号：200710030228.5）和《基于预测控制和交叉耦合的直驱XY平台轮廓控制方法》（专利号：201210359218.7）都采用了交叉耦合控制策略，在已有的多个单轴控制回路基础上，通过集成一个轮廓控制器来实现对轮廓误差的闭环控制，其轮廓控制和跟踪控制之间存在耦合作用，导致轮廓性能受到跟踪性能的影响，高速加工时轮廓误差较大。

发明内容

为了解决现有技术中的问题，本发明提供了一种针对典型的XY双轴运动平台的、适用于任意自由曲线的基于任务极坐标系的伺服系统轮廓控制方法。

本发明提供了一种基于任务极坐标系的伺服系统轮廓控制方法，包括以下步骤：

S1、建立XY运动平台动力学方程；

S2、根据期望轨迹信息，基于密切圆逼近建立任务极坐标系，并计算相应的坐标变换关系；

S3、将笛卡尔坐标系下的系统动力学方程转换为任务极坐标下的误差动力学方程；

S4、设计基于前馈补偿的反馈PD控制器，实现对误差动力学的解耦控制。

本发明的有益效果是：基于任务极坐标系的解耦轮廓控制方法能够大幅度降低高速大曲率轮廓加工过程中的轮廓误差。

附图说明

图1是基于PC机的伺服系统组成示意图；

图2.是单轴伺服系统示意图；

图3是单轴伺服系统数学模型；

图4是库伦摩擦力模型；

图5是单轴伺服系统简化动力学模型；

图6.是任务极坐标系；

图7是基于任务极坐标系的轮廓控制系统框图。

具体实施方式

下面结合附图说明及具体实施方式对本发明进一步说明。

一种基于任务极坐标系的伺服系统轮廓控制方法，包括以下步骤：

S1、建立XY运动平台动力学方程；

S2、根据期望轨迹信息，基于密切圆逼近建立任务极坐标系，并计算相应的坐标变换关系；

S3、将笛卡尔坐标系下的系统动力学方程转换为任务极坐标下的误差动力学方程；

S4、设计基于前馈补偿的反馈PD控制器，实现对误差动力学的解耦控制。

步骤S1“建立动力学方程”具体实现方式如下：

如图1至图7所示，为了设计高性能的轮廓控制器，对运动平台中所有运动轴的动力学特性进行精确的建模和辨识是非常重要的步骤。完整的建模过程应该包括：刚体运动模型，摩擦力，驱动装置饱和特性，数字量化误差和测量噪声，齿侧间隙等。

对于实际系统，建立完整的数学模型来全面描述其动力学特性是非常困难的。一般都通过建立简化模型来实现对系统的分析和研究。下面以由滚珠丝杆驱动的运动轴为例，对其动力学和摩擦力特性进行研究，如图2所示的单轴伺服系统，包括驱动器1、电动机2、耦合键3、丝杠4、工作台5、主轴6和位置传感器7。

其中，u_x(V)是由控制系统输出的电压信号，一般由上位机PC或者嵌入式系统的控制模块计算并输出。控制电压u_x经过电流放大器转换为电机电枢电流i_x(A)，电流放大器增益为K_t(A/V)。电机输出力矩T_m(Nm)等比于电枢电流i_x，电机力矩常数为K_t(Nm/A)。除了由电枢电流产生的电机力矩之外，电机转动轴还受到扰动力矩T_d(Nm)的影响，扰动力矩T_d一般由摩擦力以及切削力等组成。电机力矩T_m和扰动力矩T_d之间的差值，用于驱动和电机相连的机械机构，在模型中处理为由等效惯量J_x(kg m2)和粘滞阻尼B_x(kg m2/s)组成的一阶等效环节。电机转动角速度为ω(rad/s)，通过一个螺杆，增益r_g(mm/rad)，转换为单轴进给速度(mm/s)，其积分值即为单轴位置x(mm)。如果在电机转轴和运动轴位置之间还有其他齿轮传动装置，都可以折算到r_g中。

根据图3中得到的简化线性模型，在拉普拉斯域，可以建立运动轴位置x(s)和控制器输入电压u_x(s)以及扰动力矩T_d(s)之间的关系式

$x\left(s\right)=\frac{r_g}{s}\·\frac{1}{J_{x}s+B_x}(K_t{K}_a{u}_x\left(s\right)-T_d\left(s\right))---\left(1\right)$

为了简化控制器设计，通常将扰动力矩作用位置移动到在控制电压输入端，即d(s)＝T_d(s)/K_tK_a。上式中的扰动力矩T_d(s)可以认为由两部分组成的，T_d(s)＝T_f(s)+T_c(s)，其中T_f(s)表示作用在电机转轴的等效摩擦力，T_c(s)表示作用在电机转轴的切削力。对于非加工类应用，切削力一般并不存在，对于数控加工类应用，切削力受多种因素综合影响。对于摩擦力T_f(s)部分，针对润滑金属表面接触的典型摩擦力特性通常使用Stribeck曲线等进行描述，在本发明的研究中，因为更关注高速情况下的轮廓控制问题，所以在摩擦力部分采用了简化的库伦和滑动摩擦力模型，库伦摩擦力特性详见图4。

最后可以得到典型的双轴运动系统的动力学方程，如图5所示，

$J\stackrel{\·\·}{\mathrm{\ω}}\left(t\right)+B\stackrel{\·}{\mathrm{\ω}}\left(t\right)+F\left(\stackrel{\·}{\mathrm{\ω}}\left(t\right)\right)=\mathrm{Ku}\left(t\right)---\left(2\right)$

其中，J，B，Ｋ∈Ｒ^２×２都是常数对角矩阵，分别表示惯量，阻尼和驱动器增益。Ｆ是二维的库伦摩擦力向量，和速度方向相关。ω和u都是二维向量，分别表示系统实际反馈位置和控制输入电压。

步骤S2“在期望轨迹上建立任务极坐标系”具体实现方式如下：

不失一般性，要求参考轨迹为正则曲线并且至少三阶可导。定义期望轨迹为c_d（t)，其中参数t表示时间。在世界笛卡尔坐标系中，期望轨迹的参数方程为

$c_d\left(t\right)={\mathrm{\ω}}_d\left(t\right)=\left[\begin{array}{c}{x}_d\left(t\right)\\ y_d\left(t\right)\end{array}\right]---\left(3\right)$

对于期望轨迹上的任意一点，必然存在唯一的二维Frenet坐标系与之相连。定义Frenet坐标系的正交基底为t和n，计算公式如下所示

$t=\frac{{\stackrel{\·}{\mathrm{\ω}}}_d}{\left|\right|{\stackrel{\·}{\mathrm{\ω}}}_d\left|\right|}=\frac{1}{\left|\right|{\stackrel{\·}{\mathrm{\ω}}}_d\left|\right|}\left[\begin{array}{c}{\stackrel{\·}{x}}_d\\ {\stackrel{\·}{y}}_d\end{array}\right]---\left(4\right)$

$n=\frac{1}{\left|\right|{\stackrel{\·}{\mathrm{\ω}}}_d\left|\right|}\left[\begin{array}{c}-{\stackrel{\·}{y}}_d\\ {\stackrel{\·}{x}}_d\end{array}\right]---\left(5\right)$

其中，和分别表示各自对参数t的一阶导数。为了简便起见，省略了参数t。

需要注意的是，对于用代数方程表示的参考轨迹，其切向向量，法向向量，以及更高阶的关于时间的微分，都可以通过对代数方程进行全微分得到。对于如数控系统中由一系列离散点构成的期望轨迹，通过差分的方法可以得到需要的各阶次导数。因此，本发明中提出的轮廓误差估计方法适用于采用任意描述方式的光滑曲线。

在Frenet坐标系中定义坐标f＝[τ，η]^T。世界坐标系和Frenet坐标系之间的坐标转换关系为

T_fω：ω＝Rf+ω_d (6)

T_ωf：f＝R^-1ω-R^-1ω_d (7)

其中T_fω表示从到的坐标转换关系，T_ωf为其逆变换。矩阵R＝[t，n]，并且R^T＝R^-1。旋转矩阵R同样可以用旋转角表示

其中

在高速大曲率加工情形下，基于切线逼近方法的Frenet坐标系对轮廓误差的估计结果可能会不准确，这样就会影响最终成品的加工质量。本发明提出了基于密切圆逼近的任务极坐标系，能够显著改善对轮廓误差的估计效果。下面讨论任务极坐标系的建立以及相应的坐标转换关系。

由微分几何知识可知，当期望轨迹为直线。对于正则曲线在曲线上的每一点c_d(t)必然存在一个密切圆与之对应。通常定义的曲率都为正数，计算公式如下

由右手法则可知，切向单位向量逆时针旋转90°可以得到法向单位向量n。

然而，期望曲线的弯曲方向可能和法向向量n不同，如图6所示。所以使用相对曲率来表示内切圆与期望轨迹的相对位置关系，其计算公式为

在图6中，点D处，为负，这表示内切圆圆心位置和法向正方向方向在期望轨迹的不同侧。点处，为正，故内切圆圆心与法向正方向在期望轨迹的同一侧。

因此，期望轨迹上一点c_d(t)对应的密切圆圆心O在Frenet坐标系中坐标可以定义为将密切圆圆心O设为极点P，我们可以建立以(r，θ）为正交坐标的任务极坐标系

如图6所示，坐标r表示从极点P到实际位置A的距离；坐标θ表示从向量到的夹角。当为正时（如图6右边情形），角度正方向为逆时针方向；反之（如图6左边情形），为顺时针方向。

基于圆逼近方法，在任务极坐标系下，轮廓误差的计算公式如下所示

ε_cc＝r-r_d (10)

其中

Frenet坐标系和任务极坐标系之间的坐标变换及其逆变换是非线性并且依赖于相对曲率的符号。

其中函数sgn(·)定义如下

$\mathrm{sgn}\left(x\right)=\left\{\begin{array}{ccc}1& \mathrm{if}& x>0\\ 0& \mathrm{if}& x=0\\ -1& \mathrm{if}& x<0\end{array}\right.$

步骤S3“建立任务极坐标系下的动力学方程”具体实现方式如下：

由步骤S1，可以得到世界笛卡尔坐标系下的系统动力学方程。下面要先将其转换到Frenet坐标系下，然后再转换到任务极坐标系下。

对步骤S2中的关于时间t求解一阶及二阶导数，可以得到

$\stackrel{\&CenterDot;}{\mathrm{\&omega;}}=\stackrel{\&CenterDot;}{R}f+R\stackrel{\&CenterDot;}{f}+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_d---\left(13\right)$

$\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}=\stackrel{\&CenterDot;\&CenterDot;}{R}f+2\stackrel{\&CenterDot;}{R}\stackrel{\&CenterDot;}{f}+R\stackrel{\&CenterDot;\&CenterDot;}{f}+{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d---\left(14\right)$

其中， $\stackrel{\&CenterDot;}{R}=-\mathrm{\&upsi;GR},$ 期望速度 $\mathrm{\&upsi;}=\left|\right|{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_d\left|\right|,$ 因为期望光滑轮廓至少三阶连续可导，所以始终存在。

将上述公式(13)和(14)带入步骤S3中的(2)，可以得到Frenet坐标系下的动力学方程

$M_F\stackrel{\&CenterDot;\&CenterDot;}{f}+C_F\stackrel{\&CenterDot;}{f}+K_{F}f=u_F---\left(15\right)$

其中

M_F=JR

$C_F=2J\stackrel{\&CenterDot;}{R}+\mathrm{BR}$

$K_F=J\stackrel{\&CenterDot;\&CenterDot;}{R}+B\stackrel{\&CenterDot;}{R}$

$u_F=K(u-u_d)-F\left(\stackrel{\&CenterDot;}{\mathrm{\&omega;}}\right)$

$u_d=K^{-1}(J{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+B{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_d)$

将Frenet坐标系中的动力学方程转换到任务极坐标系下，首先要对关于时间t求解一阶以及二阶导数。

一阶导数计算公式为

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&tau;}}\\ \stackrel{\&CenterDot;}{\mathrm{\&eta;}}\end{array}\right]={\mathrm{\&Delta;}}_s\left[\begin{array}{cc}\sin \mathrm{\&theta;}& r\cos \mathrm{\&theta;}\\ \cos \mathrm{\&theta;}& -r\sin \mathrm{\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]---\left(16\right)$

其中，

由此可以得到关于时间t的二阶导数

$\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&tau;}}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&eta;}}\end{array}\right]={\mathrm{\&Delta;}}_s(\left[\begin{array}{cc}\mathrm{s\&theta;}& r\&CenterDot;\mathrm{c\&theta;}\\ \mathrm{c\&theta;}& -r\&CenterDot;\mathrm{s\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{r}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left[\begin{array}{cc}2\mathrm{c\&theta;}& -\mathrm{rs\&theta;}\\ -2\mathrm{s\&theta;}& -\mathrm{rc\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right])---\left(17\right)$

其中sθ＝sinθ，cθ＝cosθ。将上述公式(16)和(17)带入(15)，可以得到任务极坐标系下的系统动力学方程

$M_p\stackrel{\&CenterDot;\&CenterDot;}{p}+F_p(\stackrel{\&CenterDot;}{p},p)=u_p---\left(18\right)$

其中p＝[r，θ]^T， $\stackrel{\&CenterDot;}{p}={[\stackrel{\&CenterDot;}{r},\stackrel{\&CenterDot;}{\mathrm{\&theta;}}]}^T,\stackrel{\&CenterDot;\&CenterDot;}{p}={[\stackrel{\&CenterDot;\&CenterDot;}{r},\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}]}^T,$ u_p＝r_F， $M_p=M_F{\mathrm{\&Delta;}}_s\left[\begin{array}{cc}\mathrm{s\&theta;}& \mathrm{rc\&theta;}\\ -\mathrm{c\&theta;}& \mathrm{rs\&theta;}\end{array}\right],$ $F_p(\stackrel{\&CenterDot;}{p},p)={\mathrm{\&Delta;}}_s(M_F\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left[\begin{array}{cc}2\mathrm{c\&theta;}& -\mathrm{rs\&theta;}\\ -2\mathrm{s\&theta;}& -\mathrm{rc\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+C_F\left[\begin{array}{cc}\mathrm{s\&theta;}& \mathrm{rc\&theta;}\\ -\mathrm{c\&theta;}& \mathrm{rs\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+K_F\left[\begin{array}{c}\mathrm{rs\&theta;}\\ -\mathrm{rc\&theta;}+r_d\end{array}\right]).$

可见，任务坐标系下的系统动力学方程是非线性强耦合的，在步骤S4中，将会把系统动力学方程转换为误差动力学方程，并设计一个控制器实现对径向以及角度方向的解耦控制。

步骤S4“解耦控制器的设计”具体实现方式如下：

首先将上述任务极坐标系下的动力学方程转换为误差动力学方程。定义四个控制系统状态变量，z₁＝ε_c＝ε_r＝r-r_d，z₂＝ε_θ＝θ，和其中，z₁和z₂是估计的轮廓误差和对应的夹角误差。显然，期望的夹角θ_d＝0。

定义Z₁＝[z₁，z₂]^T和可将系统动力学方程(18)转换成如下状态空间方程形式：

${\stackrel{\&CenterDot;}{Z}}_1=Z_2$

${\stackrel{\&CenterDot;}{Z}}_2={M_p}^{-1}[u_p-F_p(Z_1\&CenterDot;Z_2)]$

对于正则曲线而言，矩阵M_p始终可逆。

经过上述转换，世界坐标系下的轮廓控制问题转换为任务极坐标系中非线性强耦合系统的稳定性问题。可以采用反馈线性化的方法设计控制器，实现对径向和夹角方向误差的解耦控制。

u_p＝F_p(Z₁，Z₂)+M_pV(Z₁，Z₂) (19)

其中，设计耦合非线性部分F_p(Z₁，Z₂)对上述系统进行补偿。对于补偿之后的系统，通过设计V(Z₁，Z₂)来实现系统的渐进稳定。

本发明中选择使用比例-微分形式，即V(Z₁，Z₂)＝-K_pZ₁-K_dZ₂，K_P，K_D∈R^2×2都是对角矩阵。使用上述控制器之后，非线性强耦合的系统变为了线性系统，其状态空间形式为

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{Z}}_1\\ {\stackrel{\&CenterDot;}{Z}}_2\end{array}\right]=\left[\begin{array}{cc}0& I\\ -K_P& -K_D\end{array}\right]\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]---\left(20\right)$

由李亚普诺夫稳定性定理可知，当K_P，K_D是正定矩阵时，上述自治系统(20)是渐进稳定的。

在本发明中，期望的误差动力学特性为标准二阶方程形式，

[s²+2ζ_r(2πf_r)s+(2πf_r)²]ε_r＝0 (21)

[s²+2ζ_θ(2πf_θ)s+(2πf_θ)²]ε_θ＝0 (22)

其中f_r和f_θ分别表示径向和夹角方向的系统带宽，ζ_r和ζ_θ分别表示对应方向的阻尼比，因此，可以得到相应的控制增益

$K_P=\left[\begin{array}{cc}{\left(2\mathrm{\&pi;}{f}_r\right)}^2& 0\\ 0& {\left(2\mathrm{\&pi;}{f}_{\mathrm{\&theta;}}\right)}^2\end{array}\right]---\left(23\right)$

$K_D=\left[\begin{array}{cc}4\mathrm{\&pi;}{\mathrm{\&zeta;}}_r{f}_r& 0\\ 0& 4\mathrm{\&pi;}{\mathrm{\&zeta;}}_{\mathrm{\&theta;}}{f}_{\mathrm{\&theta;}}\end{array}\right]---\left(24\right)$

为了使任务极坐标系下的误差控制系统有很好的动态性能，阻尼比ζ_r和ζ_θ通常固定为1，即系统处于临界阻尼状态。

如图7所示，基于任务极坐标系的解耦轮廓控制算法输出为

u(t)＝K^-1[M_p(t)V(t)+F_p(t)+F(t)]+u_d(t) (25)

其中，M_p，F_p，F和V中的参数都被省略，并且矩阵K是非奇异的。

如图1所示，以典型的XY运动平台500为例，说明基于任务极坐标的轮廓控制算法的具体实施过程。

实验平台以普通PC100机作为系统的上位机，负责接收系统运行中反馈的信息，并根据控制算法计算输出电压。运动控制卡200采用固高科技公司的GT-400,这款运动控制卡能够实现对多轴伺服电机进行控制，在本发明中，仅仅将其作为IO口进行操作，读取电机编码盘反馈的位置信息，并将计算输出电压传送给X轴驱动器300和Y轴驱动器400。驱动器采用Elmo公司的HAR-4/100，工作在速度环。伺服电机采用Mecapion的APM-SA01ACN，最高转速3000r/min。传动机构采用滚珠丝杆结构，螺距4mm。采用电机自带的光电编码盘作为位置传感器，所以这套系统是半闭环控制系统。

步骤S1中的建模过程都是在连续域内进行，而一般的计算机控制系统都是离散的，所以对进给伺服系统的系统辨识都是要在离散域内进行的。首先要对上述的连续模型进行离散化，本发明采用脉冲不变z-s变换，能够保证连续时间模型和离散时间模型之间的转换精度。

从连续时间模型G(s)得到离散时间模型G(z)的变换对为

对于本发明中讨论的动力学模型的传递函数

$G\left(s\right)=\frac{K}{J{s}^2+\mathrm{Bs}}=\frac{b}{s^2+\mathrm{as}}$

其中定义了符号a＝B/J和b＝K/B来简化计算，可以得到

可以重写为

$G\left(z\right)=\frac{b_1{z}^{-1}}{1+a_1{z}^{-1}+a_2{z}^{-2}}---\left(27\right)$

其中a₁＝1+e^-aT，a₂＝e^-aT，

G(s)和G(z)中的系数有对应关系

$a=\frac{-\ln a_2}{T}$

$b=\frac{{\mathrm{ab}}_1}{1-a_2}$

在离散域中，通常使用线性自回归（ARX,Auto Regressive eXogenous）模型，其定义为

y(t)+a₁y(t-1）+...+a_nay(t-na)＝b₁u(t-nk)+...+b_nbu(t-nk-nb+1)（28）其中，na为极点个数，nb-1为零点个数，nk为系统延时。

式(27)对应的时域形式为

y(t)+a₁y(t-1)+a₂y(t-2)＝b₁u(t-1)

与(28)相对应，na＝2，bn＝1，nk＝1，所以得到的是ARX211模型。

系统辨识实验采用的输入信号，一般有伪随机二进制信号，扫频信号和混沌信号等几种，这里采用混沌信号作为输入，混沌信号具有功率谱连续平稳等优点。

这里采用的混沌信号发生器使用Duffing振子

${\stackrel{\&CenterDot;}{z}}_1={\mathrm{\&omega;z}}_2$

${\stackrel{\&CenterDot;}{z}}_2=\mathrm{\&omega;}(z_1-z_1^3-0.25z_2+0.3\sin \left(\mathrm{\&omega;t}\right))$

z₁和z₂为系统状态变量，z₂同时为系统输出，ω为震荡频率，实验中取ω＝20rad/s。

本发明中采用的模型是ARX211模型，使用Matlab自带的System IdentificationToolbox进行离线辨识，辨识实验中采样周期为1ms。

对于库伦摩擦力的测量实验，通过给定电机一组增大的电压命令，找到电机开始运动时对应的电压值即可对库伦摩擦力进行补偿。可以通过多次实验求平均值的方法优化摩擦力补偿结果。

需要注意的是，实验装置的动力学和摩擦力特性是时变的，所以在每次操作之前要进行一次辨识实验，以保证辨识结果的准确性。

根据参考轮廓的几何特性，参照步骤S2中的计算方法，可以得到相应的Frenet坐标系和曲率等信息，即可得到相应的坐标变换关系数据。为保证控制系统的实时性，步骤S2的结果可以预先计算并保存到内存中。

根据实验装置反馈的实际位置信息和步骤S1、S2预先得到的相关参数，参照步骤S3和S4，在中断程序实时计算对应的轮廓控制器输出。

上述计算过程在PC机中通过C语言编程实现，操作系统为DOS7.1，通过中断进行实时控制。位置反馈信息由电机编码盘得到，并通过运动控制卡传递给PC机。PC机计算的控制命令通过运动控制卡传递给实验装置。

这一部分中以在XY平台上进行的轮廓控制实验为例，说明本发明所提出的轮廓控制算法的具体实施步骤。对于一般的机床伺服系统，都可以参照此具体案例进行轮廓控制。

为了实现高速高精度的轮廓控制，本发明中提出了基于任务极坐标系的解耦轮廓控制算法。

本发明基于密切圆逼近方法，在期望轨迹上建立任务极坐标系，能够很自然很简单地给出估计轮廓误差的表达式，即当前实际位置的极径坐标与密切圆半径之差。将跟踪误差分解为相互垂直的极径误差和极角误差，其中极径误差即为二阶估计的轮廓误差。通过设计基于反馈线性化和输入前馈补偿的时变PD轮廓控制算法，实现了对轮廓误差估计轮廓误差和前进误差的直接解耦控制，能够充分利用任务极坐标系下系统的径向跟踪能力来提高轮廓控制性能。与采用切线逼近的任务坐标系方法相比，任务极坐标系方法可以得到更准确的轮廓误差估计结果，更适用于高速大曲率轮廓加工情形。理论和实验表明，基于任务极坐标系的解耦轮廓控制方法能够大幅度降低高速大曲率轮廓加工过程中的轮廓误差。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (1)

1.一种基于任务极坐标系的伺服系统轮廓控制方法，其特征在于，包括以下步骤：

S1、建立XY运动平台动力学方程，

建立典型的双轴运动系统的动力学方程，

$J\stackrel{\&CenterDot;\&CenterDot;}{w}\left(t\right)+B\stackrel{\&CenterDot;}{w}\left(t\right)+F\left(\stackrel{\&CenterDot;}{w}\right(t\left)\right)=Ku\left(t\right)$

其中，J，B，K∈R^2×2都是常数对角矩阵，分别表示惯量，阻尼和驱动器增益，F是二维的库伦摩擦力向量，和速度方向相关，w和u都是二维向量，分别表示系统实际反馈位置和控制输入电压；

S2、根据期望轨迹信息，基于密切圆逼近建立任务极坐标系，并计算相应的坐标变换关系，

基于圆逼近方法，在任务极坐标系下，轮廓误差的计算公式如下所示，

ε_cc＝r-r_d

其中Frenet坐标系和任务极坐标系之间的坐标变换及其逆变换是非线性并且依赖于相对曲率的符号，

其中，函数sgn(·)定义如下

$sgn\left(x\right)=\left\{\begin{array}{cc}1& ifx>0\\ 0& ifx=0\\ -1& ifx<0\end{array}\right.;$

S3、将笛卡尔坐标系下的系统动力学方程转换为任务极坐标下的误差动力学方程，

任务极坐标系下的系统动力学方程为

$M_p\stackrel{\&CenterDot;\&CenterDot;}{p}+F_p(\stackrel{\&CenterDot;}{p},p)=u_p$

其中p＝[r，θ]T， $\stackrel{\&CenterDot;}{p}={\&lsqb;\stackrel{\&CenterDot;}{r},\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\&rsqb;}^T,$ $\stackrel{\&CenterDot;\&CenterDot;}{p}={\&lsqb;\stackrel{\&CenterDot;\&CenterDot;}{r},\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\&rsqb;}^T,$ u_p＝u_F， $M_p=M_F{\mathrm{\&Delta;}}_s\left[\begin{array}{cc}s\mathrm{\&theta;}& rc\mathrm{\&theta;}\\ -c\mathrm{\&theta;}& rs\mathrm{\&theta;}\end{array}\right],$ $F_p(\stackrel{\&CenterDot;}{p},p)={\mathrm{\&Delta;}}_s(M_F\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left[\begin{array}{cc}2c\mathrm{\&theta;}& -rs\mathrm{\&theta;}\\ -2s\mathrm{\&theta;}& -rc\mathrm{\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+C_F\left[\begin{array}{cc}s\mathrm{\&theta;}& rc\mathrm{\&theta;}\\ -c\mathrm{\&theta;}& rs\mathrm{\&theta;}\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{r}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+K_F\left[\begin{array}{c}rs\mathrm{\&theta;}\\ -rc\mathrm{\&theta;}+r_d\end{array}\right];$

S4、设计基于前馈补偿的反馈PD控制器，实现对误差动力学的解耦控制，采用反馈线性化的方法设计反馈PD控制器，实现对径向和夹角方向误差的解耦控制，

u_p＝F_p(Z₁，Z₂)+M_pV(Z₁，Z₂)

其中，设计耦合非线性部分F_p(Z₁，Z₂)对上述系统进行补偿，对于补偿之后的系统，通过设计V(Z₁，Z₂)来实现系统的渐进稳定；

基于任务极坐标系的解耦轮廓控制算法输出为

u(t)＝K^-1[M_p(t)V(t)+F_p(t)+F(t)]+u_d(t)

其中，M_p，F_p，F和V中的参数都被省略，并且矩阵K是非奇异的。