CN104267598A - 一种Delta机器人运动机构的模糊PI控制器设计方法 - Google Patents

Abstract

本发明提供了一种基于LabVIEW图形化编程软件，设计Delta机器人运动机构控制器的方法，用于改善***的动态响应性能，提高跟踪的精度，属于机器人控制技术领域。首先基于Delta机器人的结构，求解机器人的运动学关系，构建末端执行器到运动各轴的逆运动学方程；利用五次多项式曲线拟合各轴的运动路径；由伺服电机的编码器反馈得到的信息，经FPGA模块处理得到位置偏差e，输入到模糊PI控制器内，控制器输出速度的脉冲信号；通过NI的PCI-7842R板卡上的FPGA模块驱动伺服电机驱动器，从而使电机准确快速地旋转到给定位置。

Description

一种Delta机器人运动机构的模糊PI控制器设计方法

技术领域

本发明涉及Delta机器人控制领域，具体是基于LabVIEW图形化编程软件，设计一种Delta机器人运动机构的模糊PI控制器，改善***的动态响应性能，提高跟踪的精度，属于机器人控制技术领域。

背景技术

自1962年第一台工业机器人应用于生产线以来，机器人技术不断发展、完善和成熟，目前已广泛应用于制造业、服务业、科学探索、医学、军事和航天等各个领域。工业机器人可以代替人类完成大量高质量要求的工作，如汽车制造、电子产品制造、食品包装、物料的搬运等。由于Delta机器人具备刚性强度大、速度快、精度高、承载能力强、动负荷小、结构紧凑等特点，在工业领域分拣、插装等工序中有广泛的应用。通过何种控制方法使Delta机器人手臂更加快速、精准、稳定地抓取物品，一直是Delta机器人的研究热点。Delta机器人控制***是一个非线性、多输入、多输出、强耦合的复杂***，PI控制器处理***不确定性的能力有限，会造成控制的不准确和不稳定。模糊PI控制是根据控制规则决定控制量的大小，其控制算法是根据现场实际运行情况及操作人员的经验，通过建立控制规则表，按照模糊合成推理规则求出相应的控制量，再经过反复调试和修正，加上PI控制较好的稳态性能，使被控对象具有良好的静态特性和动态特性。故模糊PI控制不需要知道控制对象精确的数学模型，从而克服了复杂***无法建立确定的数学模型的缺点。

发明内容

本发明的目的在于以LabVIEW图形化编程软件为平台，设计一种Delta机器人运动机构的模糊PI控制器，改善***的动态响应性能，提高跟踪的精度。

按照本发明提供的技术方案，所述Delta机器人运动机构的模糊PI控制器技术方案包括如下步骤：

第一步，在立体钢结构支架内安装Delta机器人，所述的Delta机器人能够在立体支架内运动；在钢结构立体支架前端安装工业智能相机，Delta机器人下方安装垂直于机器人本体的传送带；在工业智能相机所在的位置建立相机坐标系，在Delta机器人的静平台上建立机器人的基坐标系；在工控机上安装NI的PCI-7842R板卡，通过信号转接板连接到伺服电机驱动器。

第二步，当传送带上有物件传送时，工业智能相机识别物件的像素位置，并转化到相机坐标系的位置，由相机坐标系的位置通过外参数矩阵转化到基于机器人基坐标的位置；在运动过程中，通过传送带上编码器反馈的信息对物件的位置进行实时更新，当物件到达机器人工作区域时，机器人完成将传送带上的物件抓取到另一固定位置。

第三步，建立Delta机器人的运动学模型，确定各运动轴位置(θ₁，θ₂，θ₃)与末端执行器位置坐标(x，y)的对应关系，即位置逆解模型。通过已知机械臂末端的位置，求解主动臂的转角。

第四步，在关节空间中根据作业任务的要求，由起始点和终止点的位置、速度和加速度，计算出预计的运动轨迹，采用五次多项式插值规划关节空间的运动轨迹。

第五步，Delta机器人的运动机构即伺服电机，由伺服电机的编码器反馈得到伺服电机的当前位置信息，经NI板卡上的FPGA模块处理后传送到工控机，由给定转角位置与当前的角度位置得到位置偏差e，输入到模糊PI控制器，控制器输出速度的脉冲信号，通过NI板卡上的FPGA模块输出到伺服驱动器，使电机旋转到给定位置。

所述第二步包括如下步骤：

2.1、在获取像素点坐标后，需要通过标定得到的内外参数求取基于并联机器人的基坐标。所谓相机标定，就是根据给定的相机模型求取相机的内部参数、外部参数。采用张正友方法进行相机标定，相机坐标系下的参数求取如下：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]$

其中，(u，v)为成像平面上的像素坐标，(x_c，y_c，z_c)为物件在相机坐标系下的表示。k_x，k_y分别为x和y方向的放大倍数。(u₀，v₀)为图像中心点的像素坐标。通过张正友标定的方案，计算出矩阵 $\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right],$ 在通过乘以逆矩阵获得基于相机坐标系的位置(x_c，y_c，z_c)。

另有物件坐标在相机坐标中的描述：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

(x_w，y_w，z_w)表示物件在机器人基坐标系的坐标， $\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right].$ 这里，外参数矩阵就是相机坐标系的原点与机器人基坐标原点的关系矩阵，可以通过测量得到。

所述第四步包括如下步骤：

4.1、获取起始点和终止点的限制条件，即起始点和终止点的关节角度θ₀、θ_f，起始点和终止点的速度起始点和终止点的加速度采用五次多项式插值规划关节空间的运动轨迹，其参数求取如下：

θ(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

其中，θ(t)为平滑的插值函数，a₀、a₁、a₂、a₃、a₄、a₅为插值函数的各项系数。

由起始点和终止点的限制条件可得：

$\left\{\begin{array}{c}{\mathrm{\θ}}_0=a_0\\ {\mathrm{\θ}}_f=a_0+a_1{t}_f+a_2{t_f}^2+a_3{t_f}^3+a_4{t_f}^4+a_5{t_f}^5\\ {\stackrel{\·}{\mathrm{\θ}}}_0=a_1\\ {\stackrel{\·}{\mathrm{\θ}}}_f=a_1+2a_2{t}_f+3a_3{t_f}^2+{4a}_4{t_f}^3+5a_5{t_f}^4\\ {\stackrel{\·\·}{\mathrm{\θ}}}_0=2a_2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_f={2a}_2+{6a}_3{t}_f+12a_4{t_f}^2+20a_5{t_f}^3\end{array}\right.$

其中，t_f表示终止时刻。由上述6个条件可以唯一地求解出插值函数的各项系数：

$\left\{\begin{array}{c}{a}_0={\mathrm{\θ}}_0\\ a_1={\stackrel{\·}{\mathrm{\θ}}}_0\\ a_2=\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_0}{2}\\ a_3=\frac{20{\mathrm{\θ}}_f-20{\mathrm{\θ}}_0-(8{\stackrel{\·}{\mathrm{\θ}}}_f+12{\stackrel{\·}{\mathrm{\θ}}}_0)t_f-(3{\stackrel{\·\·}{\mathrm{\θ}}}_0-{\stackrel{\·\·}{\mathrm{\θ}}}_f)t_f^2}{2t_f^3}\\ a_4=\frac{30{\mathrm{\θ}}_f-30{\mathrm{\θ}}_f+(14{\stackrel{\·}{\mathrm{\θ}}}_f+16{\stackrel{\·}{\mathrm{\θ}}}_f)t_f+(3{\stackrel{\·\·}{\mathrm{\θ}}}_0-{\stackrel{\·\·}{\mathrm{\θ}}}_f)t_f^2}{2t_f^3}\\ a_5=\frac{12{\mathrm{\θ}}_f-12{\mathrm{\θ}}_0-(6{\stackrel{\·}{\mathrm{\θ}}}_f+6{\stackrel{\·}{\mathrm{\θ}}}_0)t_f-({\stackrel{\·\·}{\mathrm{\θ}}}_0-{\stackrel{\·\·}{\mathrm{\θ}}}_f)t_f^2}{2t_f^3}\end{array}\right.$

所述第五步包括如下步骤：

5.1、读取伺服电机编码器反馈的A、B、Z三路脉冲信号，通过X4解码器可以得到电机当前的位置。将给定位置与当前的位置作差得到位置偏差e，将其对单位时间求导得到偏差变化率ec。

5.2、设计模糊控制器的结构，选定输入输出变量。采用二维模糊控制器，将***的位置偏差e和偏差变化率ec作为输入，经过模糊控制器输出PI的2个调整参数ΔK_p和ΔK_i，用于在线修正PI的参数。

5.3、确定输入变量e和输出变量ec的基本论域，确定模糊量E和EC的论域，选择e和ec的量化因子，选择ΔK_p和ΔK_i的比例因子。

5.4、定义E和EC的模糊子集，选定其隶属函数。确定Δk_p和Δk_i模糊控制规则。选定解模糊的方法。

5.5、设计PI控制器，将位置偏差e作为输入，选定PI的参数，模糊控制器输出PI的2个调整参数ΔK_p和ΔK_i，用于在线修正PI的参数。

5.6、根据模糊PI控制器的运算结果，FPGA输出位置脉冲信号，并给电机使能信号和旋转方向信号。

本发明的有益效果：

模糊PI控制不需要知道控制对象精确的数学模型，就能对被控对象实施控制，从而克服了复杂***无法建立确定数学模型的缺点。模糊PI控制器具有更快的动态响应过程，并且模糊PI控制兼具了传统PI控制稳态性能好的特点。

使用LabVIEW的FPGA模块和PCI-7842R模块，用户可以在Windows主机上开发FPGA程序，然后用LabVIEW进行编译并在硬件上运行。利用FPGA模块，无需先掌握硬件设计知识，就可以自己定制I/O或控制硬件电路，加速嵌入式***原型化的过程。

附图说明

图1是本发明硬件整体结构框架

图2是E和EC的隶属函数

图3是模糊PI控制器结构图

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。

具体步骤为：

(1)在立体钢结构支架内安装Delta机器人，并建立相机坐标系、机器人基坐标系，并将工业智能相机识别到的物件坐标转换到机器人的基坐标系

(1.1)在立体支架130内安装四自由度Delta机器人110，所述四自由度Delta机器人110能在立体支架130内运动；在立体支架130的前端采用全局方式安装工业智能相机100，立体支架130内设置传送带120，传送带120的运动方向垂直于四自由度Delta机器人110的运动平面。

(1.2)在工业智能相机100所在的位置建立相机坐标系，在四自由度Delta机器人110的顶部中心建立机器人基坐标系，即Delta机器人3个运动轴构成三角形的形心。

(1.3)通过上述设置后，完成视觉引导四自由度Delta机器人***环境的搭建。相机坐标系的原点与机器人基坐标系的原点有一段平移距离与旋转角度；工业相机100获取传送带120上的轨迹点超前于四自由度Delta机器人110的运动；从而工业智能相机100获取的轨迹点时，机器人110能够完成与工业智能相机100的通信，将视觉捕获物件形心的像素坐标发送到机器人。

(1.4)对于工业智能相机模型，存在：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]$

其中，(u，v)为成像平面上的像素坐标，(x_c，y_c，z_c)为物件在相机坐标系下的表示。k_x，k_y分别为x和y方向的放大倍数。(u₀，v₀)为图像中心点的像素坐标。通过张正友标定的方案，计算出矩阵 $\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right],$ 在通过乘以逆矩阵获得基于相机坐标系的位置(x_c，y_c，z_c)。

另有物件坐标在相机坐标中的描述：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_w\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

(x_w，y_w，z_w)表示物件在机器人基坐标系的坐标，M_w是外参数矩阵。

(2)模糊PI控制器的设计

(2.1)采用二维模糊控制器，将***的位置偏差e和偏差变化率ec作为输入，通过模糊控制器输出PI的2个调整参数ΔK_p和ΔK_i，在线修正PI的参数。

(2.2)确定输入输出变量的基本论域以及模糊量的论域，选择量化因子，比例因子。模糊量E和EC的论域为{-3，-2，-1，0，1，2，3}，ΔK_p和ΔK_i的论域为{-3，-2，-1，0，1，2，3}。e和ec的基本论域区间分别为[-360，360]、[-165，165]，ΔK_p和ΔK_i的基本论域区间为[-45，45]、[-0.15，0.15]。e和ec的量化因子为0.0083、0.0182，ΔK_p和ΔK_i的比例因子为15，0.05。

(2.3)定义模糊子集，选隶属函数。E和EC的模糊集设定为{NB，NM，NS，ZO，PS，PM，PB}，论域的左右两端采用S形的隶属函数，中间部分选用三角形隶属函数。其隶属度函数如图3所示。ΔK_p和ΔK_i的模糊集与E和EC的模糊集相同，隶属函数的选定也相同。选定解模糊的方法为最大隶属度法。(NB为负大，NM负中，NS负小，ZO为零，PS为正小，PM正中，PB正大。)

Δk_p的模糊控制规则表

Δk_i的模糊控制规则表

(2.4)PI调节器的设计。位置误差e作为PI调节器的输入量，在t时刻，有t＝kT，k为当前采样次数，T为***采样周期，此时输出信号为 $y\left(k\right)=k_{p}e\left(k\right)+k_{i}T\underset{i=0}{\overset{k}{\mathrm{\Σ}}}e\left(i\right).$ 设定k_p为40，k_i为0.001。

Claims (5)

1.提出了一种Delta机器人运动机构的模糊PI控制器设计方法，其特征是在关节空间中，采用五次多项式插值规划运动轨迹；采用LabVIEW的FPGA模块和PCI-7842R模块开发FPGA程序，定制I/O及控制硬件电路；设计模糊PI控制器，实现Delta机器人的运动控制；包含以下几个步骤：

(1)建立支架安装Delta机器人、工业智能相机、传送带编码器等组件；

(2)建立工业智能相机坐标和Delta机器人的坐标转换关系；

(3)采用五次多项式插值规划关节空间的运动轨迹；

(4)编写FPGA程序并设计模糊PI控制器。

2.根据权利要求1所述Delta机器人运动机构的模糊PI控制器设计方法，其特征是：所述步骤(1)中建立支架安装Delta机器人、工业智能相机、传送带编码器等组件，包含以下步骤：

在立体支架130内安装四自由度Delta机器人110，所述四自由度Delta机器人110能在立体支架130内运动；在立体支架130的前端采用全局方式安装工业智能相机100，立体支架130内设置传送带120，传送带120的运动方向垂直于四自由度Delta机器人110的运动平面，工业智能相机也安装在支架130上，传送带120后端安装传送带编码器140。

3.根据权利要求1所述Delta机器人运动机构的模糊PI控制器设计方法，所述步骤(2)中建立工业智能相机坐标和Delta机器人的坐标转换关系，包含以下步骤：

第一步、建立工业智能相机模型，存在：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]$

其中，(u，v)为成像平面上的像素坐标，(x_c，y_c，z_c)为物件在相机坐标系下的表示；通过计算出矩阵 $\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right],$ 以求取物件基于相机坐标系的位置(x_c，y_c，z_c)；

第二步、由[x_c y_c z_c 1]′＝M_w[x_w y_w z_w 1]′建立相机坐标系和机器人基坐标的关系，M_w是外参数矩阵。

4.根据权利要求1所述Delta机器人运动机构的模糊PI控制器设计方法，所述步骤(3)中机器人关节空间轨迹规划，包含以下步骤：

第一步、获取起始点和终止点的限制条件，即起始点和终止点的关节角度θ₀、θ_f，起始点和终止点的速度起始点和终止点的加速度

第二步、由起始点和终止点的限制条件，求解出五次多项式插值函数的各项系数。

5.根据权利要求1所述Delta机器人运动机构的模糊PI控制器设计方法，所述步骤(4)中编写FPGA程序并设计模糊PI控制器，包含以下步骤：

由伺服电机的编码器反馈得到伺服电机的当前位置信息，经NI板卡上的FPGA模块处理后传送到工控机内，将给定转角位置与当前的角度位置作差得到位置偏差e，输入到模糊PI控制器内，控制器输出速度的脉冲信号，通过NI板卡上的FPGA模块驱动伺服电机驱动器，从而使电机旋转到给定位置。