CN106383495A - 基于非线性双闭环控制的曲面轮廓恒力跟踪方法及应用装置 - Google Patents

Abstract

本发明公开了一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，该方法根据曲面轮廓跟踪运动模型，对工具末端与工件的接触力模型进行受力分析，通过受力分析以及运动坐标转换方程，得到实际运动倾角与法向接触力误差之间的关系及双闭环力控制方法，再根据双闭环力控制方法得到非线性PID力控制器的控制流程及非线性PID控制方程，通过运动控制器控制三个坐标方向上运动机构的运动，从而实现工具末端以恒定的法向接触力跟踪未知曲面轮廓运动。本发明还提供了实现该方法的应用装置，包括PC主机、嵌入式运动控制器、端子板、信号放大器、三维力传感器、伺服驱动器和三坐标驱动及传动机构，以及工作台架上安装的x、y、z轴方向运动机构、传感器装夹架、加工工具和装夹曲面工件的工件夹具。

Description

基于非线性双闭环控制的曲面轮廓恒力跟踪方法及应用装置

技术领域

本发明属于机器人加工应用技术领域，涉及一种未知曲面轮廓跟踪技术，具体涉及一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法及应用装置。

背景技术

现有技术中，机器人的表面加工作业现场应用以示教再现或离线编程的方式为主，该作业方式过程中不能根据工件的形位误差、装夹误差、机器人定位误差等对运动轨迹进行调整，从而增加了示教再现及离线编程的工作量，也降低了机器人加工作业的自动化程度，限制了机器人加工的应用范围。在抛光、打磨、去毛刺等机器人应用中，机器人末端工具与工件之间应该保持期望的接触力。如果接触力过大会损坏工件或工具，相反，如果接触力过小则会脱离接触。为了实现恒力轮廓跟踪，Antonio Lopes和Fernando Almeida^[1]设计了一种机器人主动辅助力-阻抗控制装置，该装置采用六自由度并联设计，可以实现插孔、轮廓跟踪等任务。采用辅助装置的方式虽然可以增加机器人的柔顺性，但其机械结构较复杂；Kazuo Kiguchi^[2]在机器人跟踪未知曲面工件的任务中，提出了一种模糊力向量的方法，用来确定力/位置混合控制中的力控制方向。模糊向量法可以确定力控制方向的范围，但其精确程度受到传感器干扰或噪声的影响。

发明内容

为了克服以上现有技术存在的不足，本发明提供了一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法及实现这种控制方法的装置，其通过实时接触力反馈控制，使机器人末端与工件之间保持稳定的接触力。本发明所使用的方法简单有效，而且容易实施，不需要知道工件轮廓参数，可以使机器人末端完全自主的跟踪工件轮廓进行加工，并保持恒定的接触力，解决了现有技术存在的上述问题。

本发明提供的技术方案是，一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，该方法包括以下步骤：

1)根据曲面轮廓跟踪运动模型，对工具末端与工件的接触力模型进行受力分析，得到工具与工件之间的法向和切向接触力与三维传感器检测到的两个方向的接触力之间的关系，从而可以根据传感器数据得到实际法向接触力的大小，并通过将检测到的法向接触力进行反馈，根据控制算法产生一个法向调整速度，得到运动坐标转换方程；

2)根据步骤1)的运动过程中受力分析以及步骤1)的运动坐标转换方程，得到实际运动倾角与法向接触力误差之间的关系，并设计双闭环力控制方法；

3)根据步骤2)的双闭环力控制方法得到非线性PID力控制器的控制流程，及基于惯性滤波的非线性PID控制方程；

4)运动控制器采用速度控制方式控制三个坐标方向上的运动，将步骤2)所述的双闭环力控制方法编写成运动控制程序，程序采用模块化设计，分别有运动控制模块、数据采集模块、软限位模块。

本发明所述的一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，其特征还在于：

所述步骤1)具体包括：

步骤11)、将曲面轮廓跟踪运动模型进行简化，建立各坐标系之间的关系，分别是力传感器坐标系{T}和控制坐标系{C}；

步骤12)、F_n和F_z分别表示工具与工件之间的法向和切向作用力，和分别表示由传感器检测到的两个方向的接触力，根据步骤11)所述的坐标关系可得：

$\left[\begin{array}{c}{F}_x^S\\ F_y^S\end{array}\right]=\left[\begin{array}{cc}-cos\mathrm{\θ}& -sin\mathrm{\θ}\\ -sin\mathrm{\θ}& cos\end{array}\right]\left[\begin{array}{c}{F}_{\mathrm{\τ}}\\ F_n\end{array}\right]$

通过上式可以求出法向力和切向力：

$F_{\mathrm{\τ}}=-F_x^{S}cos\mathrm{\θ}-F_y^{S}sin\mathrm{\θ}$

$F_n=-F_x^{S}sin\mathrm{\θ}+F_y^{S}cos\mathrm{\θ}$

其中，θ表示两个坐标系的X轴之间夹角；

步骤13)、通过将法向接触力反馈，并根据控制算法产生一个法向调整速度v_yc，将工具沿曲面法向和切向的运动，转换到末端运动坐标系{T}中，得到：

v_xt＝v_xc cosθ-v_yc sinθ

v_yt＝v_xc sinθ+v_yc cosθ

其中，v_xc和v_yc分别表示工具沿曲面轮廓法向和切向的运动速度，v_xt和v_yt分别表示末端运动坐标系的运动速度。

所述步骤2)具体包括：

步骤21)、根据步骤1)所述的接触力分析，得到运动过程中实际法向接触力与运动倾角之间的关系：

F_n′＝k·δd+kvt_s·tan(θ-θ′)

其中，F_n′表示实际法向接触力，k和δd分别表示法向刚度和变形，v和t_s分别表示末端运动速度和控制周期，θ和θ′分别表示轮廓倾角和实际运动倾角；

步骤22)、根据步骤21)所述的公式，可以得到运动倾角与法向接触力误差之间的关系：

$\mathrm{\θ}=arctan\left(\frac{F_n^{\′}-k\·\mathrm{\δ}d}{{\mathrm{kvt}}_s}\right)+{\mathrm{\θ}}^{\′}$

进而可以得到基于法向接触力反馈的运动倾角校正公式为：

θ′(0)＝θ₀

${\mathrm{\θ}}^{\′}\left(k\right)={\mathrm{\θ}}^{\′}(k-1)-arctan\left(\frac{F_n-k\·\mathrm{\δ}d}{{\mathrm{kvt}}_s}\right)$

其中，θ′(k)表示第k时刻的运动倾角，θ₀表示初始运动倾角。

本发明还提供了一种实现基于非线性双闭环控制的曲面轮廓恒力跟踪方法的应用装置，应用装置主要包括控制部分和机械部分；所述控制部分有：PC主机、嵌入式运动控制器、端子板、信号放大器、三维力传感器、伺服驱动器和三坐标驱动及传动机构，其中机械部分包括：工作台架、x轴方向运动机构、y轴方向运动机构、z轴方向运动机构、传感器装夹架、加工工具、装夹曲面工件的工件夹具。

所述的三坐标驱动及传动机构分别由x轴、y轴、z轴三个方向运动机构组成，x轴方向运动机构固定在工作台架上，y轴方向运动机构固定在x轴方向运动机构的滚珠滑块上，z轴方向运动机构固定在y轴方向运动机构的滚珠滑块上，三个方向运动机构分别通过各自伺服电机连接的滚珠丝杠，驱动各自滚珠丝杠上安装的滚珠滑块移动；于所述z轴滚珠滑块连接固定的传感器装夹架上，装有三维力传感器；三维力传感器的测力面与末端为球形的加工工具连接，所述装夹曲面工件的工件夹具装在工作台架的台面上。

本发明所述的应用装置，所述三维力传感设备还包括连接三维力传感器的信号放大器，所述三维力传感器通过信号导线与信号放大器连接，所述信号放大器输出的模拟量信号与端子板连接，端子板中的A/D转换功能将转换后的数字量信号传入工控主机，从而形成力信号的反馈控制。

本发明基于非线性双闭环控制的曲面轮廓恒力跟踪方法的有益效果是，在机器人加工未知曲面的过程中，通过实时接触力反馈控制，使机器人末端与加工工件之间保持稳定的接触力。本发明所使用的方法简单有效，而且容易实施，不需要知道工件轮廓参数，可以使机器人末端完全自主的跟踪工件轮廓进行加工，并保持恒定的接触力。

附图说明

图1是本发明的实现所述的基于非线性双闭环控制曲面轮廓恒力跟踪方法的应用装置示意图；

图2是本发明控制系统示意图；

图3是本发明控制方法原理图；

图4是本发明非线性双闭环控制原理示意图；

图5是本发明运动倾角与法向接触力的调整过程图；

图6是本发明工具末端运动轨迹图；

图7是本发明实测的法相接触力变化图。

图中：1.PC主机，2.嵌入式运动控制器，3.端子板，4.信号放大器，5.三维力传感器，6.伺服驱动器，7.三坐标驱动及传动机构，8.工作台架，9.x轴方向运动机构，10.y轴方向运动机构，11.z轴方向运动机构，12.传感器装夹架，13.加工工具，14.曲面工件，15.工件夹具，16.滚珠滑块，17.伺服电机，18.滚珠丝杠，19.工作台面。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

实现本发明基于非线性双闭环控制的曲面轮廓恒力跟踪方法的应用装置，如图1和图2所示，主要包括控制部分和机械部分；其中控制部分有：PC主机1、嵌入式运动控制器2、端子板3、信号放大器4、三维力传感器5、伺服驱动器6和三坐标驱动及传动机构7，其中机械部分包括：工作台架8、x轴方向运动机构9、y轴方向运动机构10、z轴方向运动机构11、传感器装夹架12、加工工具13、装夹曲面工件14的工件夹具15.

本发明的三坐标驱动及传动机构7分别由x轴、y轴、z轴三个方向运动机构组成，x轴方向运动机构9固定在工作台架8上，y轴方向运动机构10固定在x轴方向运动机构9的滚珠滑块16上，z轴方向运动机构11固定在y轴方向运动机构10的滚珠滑块16上，三个方向运动机构分别通过各自伺服电机17连接的滚珠丝杠18，驱动各自滚珠丝杠18上安装的滚珠滑块16移动；于所述z轴滚珠滑块16连接固定的传感器装夹架12上，装有三维力传感器5，三维力传感器5的测力面与末端为球形的加工工具13连接，所述装夹曲面工件14的工件夹具15装在工作台架8的台面19上。

本发明的三维力传感设备还包括连接三维力传感器5的信号放大器4，所述三维力传感器5通过信号导线与信号放大器4连接，信号放大器4输出的模拟量信号与端子板3连接，端子板中的A/D转换功能将转换后的数字量信号传入嵌入式运动控制器2，从而形成力信号的反馈控制。

如图2所示，控制部分主要由PC主机1、嵌入式运动控制器2、端子板3、信号放大器4、三维力传感器5、伺服驱动器6、三坐标驱动及传动机构7；三维力传感器5通过信号导线与信号放大器4连接，将低压模拟量信号传递给信号放大器4，信号放大器4将电压信号放大后，传递给与端子板3，经过端子板3中的AD转换后得到数字量信号，同时端子板3通过串口将数字量信号传输到嵌入式运动控制器2，在嵌入式运动控制器2中，经过算法运算之后，产生PWM信号，同时将PWM信号传输到各伺服驱动器，从而让各个伺服运动模块执行期望的运动，同时，嵌入式运动控制器2通过以太网与PC主机1连接，以便于在PC主机1上编写程序、设置运动控制参数、设计人机界面等。

图3所示为一个完整的闭环反馈控制过程。具体的运动控制过程如下：首先在PC主机1上将运动控制程序通过Ethernet/IP下载到运动控制器的RAM中；当PC主机1向嵌入式运动控制器2发送启动命令后，嵌入式运动控制器开始执行运动控制程序，在执行程序过程中，通过产生调制脉冲(即PWM)，并将PWM通过端子板3发送到各轴的伺服驱动器中，从而使各运动模块进行期望的运动。在运动控制中，只需要控制y、z轴的运动就能跟踪工件轮廓，x轴只是用来辅助定位，所以在控制原理图3中并没有画出x轴运动模块。当y、z轴伺服驱动器接收到PWM信号后，就驱动y、z轴伺服电机17运动，通过滚珠丝杠18将转动转换成移动；在运动过程中，当加工工具13末端与工件接触后，就会产生接触力，通过连接在工具一端的三维力传感器5可以将三个方向的力信号转换成低压电信号，并且将该信号传递给信号放大器4，信号经过放大后，发送给端子板,3；与此同时，绝对编码器测量得到的位置信号也发送到端子板3上，端子板3同时将力信号和位置信号通过串口发送到嵌入式运动控制器2，嵌入式运动控制器2收到信号后，通过过压检测、软件滤波、运动控制运算等环节后，产生PWM调整y、z运动模块的运动，从而形成一个完整的闭环运动控制。

图4所示为非线性双闭环控制原理图，这种控制方法中的双闭环包括基于力反馈的运动倾角校正器，以及基于非线性PID的法相接触力控制器。图中F_dn表示期望的法向接触力，F_n表示实际法向接触力，u表示控制量，表示运动变换矩阵：

$T_t^c=\left[\begin{array}{cc}-cos\mathrm{\θ}& -sin\mathrm{\θ}\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]$

其中，θ表示工具末端运动倾角。

如图4所示，非线性PID控制器由跟踪微分器(TD)和状态误差反馈(SEF)构成。非线性PID控制器的输出是法向的运动调整量v_c，运动控制器根据输入的θ′将法向和切向的运动量v_yc和v_xc，根据坐标关系转换成工具坐标系下的运动。用传感器测量得到的两个轴向力F_yt和F_xt，以及θ′来计算实际测量得到的法向接触力F_n，并将F_n作为控制器和校正器的反馈信号。任意给定一个初始的偏转角θ₀，如果θ₀越接近实际θ那么运动倾角θ′就能更快收敛到轮廓倾角θ。校正器根据输入的θ₀和F_n得到校正之后的运动倾角θ′，校正公式为：

$\begin{array}{c}{\mathrm{\θ}}^{\′}\left(0\right)={\mathrm{\θ}}_0\\ {\mathrm{\θ}}^{\′}\left(k\right)={\mathrm{\θ}}^{\′}(k-1)-arctan\left(\frac{F_n-k\·\mathrm{\δ}d}{{\mathrm{kvt}}_s}\right)\end{array}$

其中，θ₀表示初始运动倾角，θ′(k)和θ′(k-1)分别表示当前运动倾角和上一时刻运动倾角，k和δd分别表示法相刚度和法相变形量，v和t_s分别表示和运动速度和控制周期。

为了验证双闭环控制中，基于力反馈的运动倾角调整方法的有效性，进行了法向力控制斜面轮廓跟踪实验。斜面的倾角θ＝84°，根据非线性双闭环力控制方法进行实验，其中运动倾角按照上述公式进行校正。取跟踪运动速度：v＝2mm/s，接触刚度：k≈10N/mm，任意选取初始的θ₀＝80°，期望的法向接触力：F_dn＝k·δd＝10N。

实验中由于力传感器受到干扰作用，以及传感器本身的测量误差比较大，原始的力信号具有较大的波动，如图5所示的原始信号F_n所示。如果以原始的测量数据作为力反馈信号，不仅得不到有效的运动倾角，还会加剧接触力的波动，甚至产生振荡现象。所以在将力信号进行反馈之前，先要进行简单的滤波处理。图5所示为运动倾角θ′与法向接触力F_n的实时调整情况。图中表明运动控制开始后，运动倾角θ′迅速调整到轮廓倾角θ的同时，法向接触力F_n也逐渐稳定到期望接触力F_dn的附近。调整时间t_u≈3s，稳定状态下跟踪误差e_θ＝0.15°，且平均法向接触力平均力控制误差e_F＝0.014N。由此说明了图4所示的法向接触力控制方法，可以实现恒定法向接触力的斜面跟踪运动控制。

本发明“一种非线性双闭环控制的曲面轮廓恒力跟踪方法”的实施例的列举：

首先，在嵌入式运动控制器2的人机界面中，设置运动模式为Jog(即点动)模式，并且在配置文件中将伺服电动机配置成绝对编码方式，将x、y、z轴通过点动分别运动到正负极限的位置，同时分别记录绝对编码器的读数，将记录的读数分别写入软件保护模块中，防止电动机过冲；

其次，通过Jog模式，将加工工具13末端移动到与曲面工件14的一端刚好接触，并且三维力传感器的读数接近零的位置，该位置表示加工工具13与曲面工件14刚好接触且接触力为零，记录该位置对应的编码器读数，然后在运动控制软件中将该位置设置成闭环反馈控制的起始位置；

再次，在非线性双闭环控制方法中，有许多参数需要设置，其中非线性PID控制器中的参数设置成如下表所示

表1非线性PID控制参数

设置其他的参数如下：初始的运动倾角θ₀＝15°，期望的法向接触力F_dn＝5N，曲面跟踪运动速度v＝2mm/s，运动控制周期和传感器采样周期为t_s＝10ms；

最后，软件保护、起始位置、控制参数都设置好之后，利用开发软件OtoStudio，将程序下载到嵌入式运动控制器2中，然后启动运动控制程序，记录加工工具13末端运动轨迹和三维力传感器5测量的数据，记录的结果如图6、7所示，加工工具13末端的运动轨迹最大跟踪误差Δ≈0.1mm，平均法向接触力法向接触力控制误差为e＝0.688N，处于力传感器测量误差范围之内，整个运动过程中加工工具13末端一直与曲面工件14表面接触，而且法向接触力基本稳定在期望接触力附近。

上述实施方式只是本发明的一个实例，不是用来限制发明的实施与权利范围，凡依据本发明申请专利保护范围所述的内容做出的等效变化和修饰，均应包括在本发明申请专利范围内。

Claims (5)

1.一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，其特征在于，该方法包括以下步骤：

1)根据曲面轮廓跟踪运动模型，对工具末端与工件的接触力模型进行受力分析，得到工具与工件之间的法向和切向接触力与三维传感器检测到的两个方向的接触力之间的关系，从而可以根据传感器数据得到实际法向接触力的大小，并通过将检测到的法向接触力进行反馈，根据控制算法产生一个法向调整速度，得到运动坐标转换方程；

2)根据步骤1)的运动过程中受力分析以及步骤1)的运动坐标转换方程，得到实际运动倾角与法向接触力误差之间的关系，并设计双闭环力控制方法；

3)根据步骤2)的双闭环力控制方法得到非线性PID力控制器的控制流程，及基于惯性滤波的非线性PID控制方程；

4)运动控制器采用速度控制方式控制三个坐标方向上的运动，将步骤2)所述的双闭环力控制方法编写成运动控制程序，程序采用模块化设计，分别有运动控制模块、数据采集模块、软限位模块。

2.根据权利要求1所述的一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，其特征在于：所述步骤1)具体包括：

步骤11)、将曲面轮廓跟踪运动模型进行简化，建立各坐标系之间的关系，分别是力传感器坐标系{T}和控制坐标系{C}；

步骤12)、F_n和F_z分别表示工具与工件之间的法向和切向作用力，和分别表示由传感器检测到的两个方向的接触力，根据步骤11)所述的坐标关系可得：

$\left[\begin{array}{c}{F}_x^S\\ F_y^S\end{array}\right]=\left[\begin{array}{cc}-cos\mathrm{\θ}& -sin\mathrm{\θ}\\ -sin\mathrm{\θ}& cos\end{array}\right]\left[\begin{array}{c}{F}_{\mathrm{\τ}}\\ F_n\end{array}\right]$

通过上式可以求出法向力和切向力：

$F_{\mathrm{\τ}}=-F_x^{S}cos\mathrm{\θ}-F_y^{S}sin\mathrm{\θ}$

$F_n=-F_x^{S}sin\mathrm{\θ}+F_y^{S}cos\mathrm{\θ}$

其中，θ表示两个坐标系的X轴之间夹角；

步骤13)、通过将法向接触力反馈，并根据控制算法产生一个法向调整速度v_yc，将工具沿曲面法向和切向的运动，转换到末端运动坐标系{T}中，得到：

v_xt＝v_xccosθ-v_ycsinθ

v_yt＝v_xcsinθ+v_yccosθ

其中，v_xc和v_yc分别表示工具沿曲面轮廓法向和切向的运动速度，v_xt和v_yt分别表示末端运动坐标系的运动速度。

3.根据权利要求1所述的一种基于非线性双闭环控制的曲面轮廓恒力跟踪方法，其特征在于：所述步骤2)具体包括：

步骤21)、根据步骤1)所述的接触力分析，得到运动过程中实际法向接触力与运动倾角之间的关系：

F′_n＝k·δd+kvt_s·tan(θ-θ′)

其中，F′_n表示实际法向接触力，k和δd分别表示法向刚度和变形，v和t_s分别表示末端运动速度和控制周期，θ和θ′分别表示轮廓倾角和实际运动倾角；

步骤22)、根据步骤21)所述的公式，可以得到运动倾角与法向接触力误差之间的关系：

$\mathrm{\θ}=arctan\left(\frac{F_n^{\′}-k\·\mathrm{\δ}d}{{\mathrm{kvt}}_s}\right)+{\mathrm{\θ}}^{\′}$

进而可以得到基于法向接触力反馈的运动倾角校正公式为：

θ′(0)＝θ₀

${\mathrm{\θ}}^{\′}\left(k\right)={\mathrm{\θ}}^{\′}(k-1)-arctan\left(\frac{F_n-k\·\mathrm{\δ}d}{{\mathrm{kvt}}_s}\right)$

其中，θ′(k)表示第k时刻的运动倾角，θ₀表示初始运动倾角。

4.一种实现权利要求1所述基于非线性双闭环控制的曲面轮廓恒力跟踪方法的应用装置，其特征在于：所述应用装置包括控制部分和机械部分；所述控制部分有：PC主机(1)、嵌入式运动控制器(2)、端子板(3)、信号放大器(4)、三维力传感器(5)、伺服驱动器(6)和三坐标驱动及传动机构(7)，所述机械部分包括：工作台架(8)、x轴方向运动机构(9)、y轴方向运动机构(10)、z轴方向运动机构(11)、传感器装夹架(12)、加工工具(13)、装夹曲面工件(14)的工件夹具(15)；

所述三坐标驱动及传动机构(7)分别由x轴、y轴、z轴三个方向运动机构组成，x轴方向运动机构(9)固定在工作台架(8)上，y轴方向运动机构(10)固定在x轴方向运动机构(9)的滚珠滑块(16)上，z轴方向运动机构(11)固定在y轴方向运动机构(10)的滚珠滑块(16)上，三个方向运动机构分别通过各自伺服电机(17)连接的滚珠丝杠(18)，驱动各自滚珠丝杠(18)上安装的滚珠滑块(16)移动；于所述z轴滚珠滑块(16)连接固定的传感器装夹架(12)上，装有三维力传感器(5)；三维力传感器(5)的测力面与末端为球形的加工工具(13)连接，所述装夹曲面工件(14)的工件夹具(15)装在工作台架(8)的台面(19)上。

5.根据权利要求4所述的应用装置，其特征在于：所述三维力传感设备还包括连接三维力传感器(5)的信号放大器(4)，所述三维力传感器(5)通过信号导线与信号放大器(4)连接，所述信号放大器(4)输出的模拟量信号与端子板(3)连接，端子板(3)中的A/D转换功能将数字量信号传入嵌入式运动控制器(2)，从而形成力信号的反馈控制。