CN104155909B - 基于pc+fpga的带速度观测器的有限时间稳定性控制系统 - Google Patents

Abstract

本发明公开了一种基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，包括IPC(1)、FPGA运动控制器(2)、伺服驱动器(3)、工业机器人(4)、以太网通讯模块(5)，FPGA运动控制器(2)包括插补模块(21)、实时控制模块(22)、编码器模块(23)、D/A输出模块(24)，在IPC上给定工业机器人的惯性参数和控制参数，FPGA运动控制器进行有限时间稳定性控制算法的实时计算得出控制力矩，并转换为电压信号，最后通过D/A输出模块输出模拟电压信号到伺服驱动器，以力矩模式驱动工业机器人完成运动控制。本发明可同时控制3～6个伺服电机，最小采样时间为0.01ms，瞬态响应时间为0.5s，能够实现工业机器人高性能的运动控制要求，可用于高精度机器人的轨迹跟踪控制。

Description

基于PC+FPGA的带速度观测器的有限时间稳定性控制系统

技术领域

本发明属于工业机器人运动控制系统的技术领域，涉及一种基于PC+FPGA的带速度观测器的有限时间稳定性控制系统。

背景技术

通常，一台工业机器人的运动控制系统由轨迹插补和轨迹跟踪两大功能模块构成，而轨迹跟踪模块常采用独立伺服的PID控制原理。在工业机器人高速运动的情况下，机器人的非线性特性愈加明显，使得传统独立伺服PID控制方法存在以下问题：速度越高，跟踪精度越低；瞬态响应慢；抗干扰的鲁棒性差，最终影响机器人的工作效率和工作质量。

为了获得高速情况下满意的跟踪性能要求，需要考虑机器人的复杂动力学特性，一些先进的控制方法相继提出。Yu提出了一种基于终端滑模的有限时间控制方法，Su提出了一种基于逆动力学的全局有限时间控制方法，Zhao提出了一种基于反步技术的鲁棒有限时间控制方法。他们的工作都是在提供一种先进的控制算法，却很少关注实时性的算法实现问题。

目前的工业机器人一般采用工业级计算机IPC+DSP的串行计算架构，其中PC完成人机交互的功能，DSP用来处理运动控制算法的计算和一些高速I/O信号，如反馈控制等。基于动力学的有限时间稳定性控制需要实时计算复杂的动力学方程和分数幂函数，使得计算量很大，尤其是被控电机较多时，在通常的PC+DSP的架构中，在较短的采样时间内(如1ms)，很难完成有限时间稳定性伺服控制的计算。而采用多片DSP的系统架构，无疑增加了系统成本，该种硬件架构设计难度也大。

另外，目前大多数机器人通常采用测速发电机获取速度信号，这无疑增加了系统成本，并严重影响本体结构的紧凑性。

上述论述内容目的在于向读者介绍可能与下面将被描述和/或主张的本发明的各个方面相关的技术的各个方面，相信该论述内容有助于为读者提供背景信息，以有利于更好地理解本发明的各个方面，因此，应了解是以这个角度来阅读这些论述，而不是承认现有技术。

发明内容

本发明的目的在于避免现有技术中的不足而提供一种基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，其通过FPGA的并行计算能力和有限时间稳定性理论以解决现有技术中，独立伺服PID控制性能不高的问题，同时提高控制系统的实时性，充分发挥FPGA实时数据处理的能力。

本发明的目的通过以下技术方案实现：

提供一种基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，包括IPC、FPGA运动控制器、伺服驱动器、工业机器人、以太网通讯模块，FPGA运动控制器包括插补模块、实时控制模块、编码器模块、D/A输出模块，工业机器人的上位机在IPC上给定工业机器人的惯性参数和控制参数，IPC通过以太网通讯模块将数据传送到FPGA运动控制器完成动力学参数和控制参数的设置，实时控制模块根据插补模块提供的插补数据、编码器模块提供的位置反馈信号，进行有限时间收敛速度观测器算法的实时计算，先获得速度信号，再进行有限时间稳定性控制算法的实时计算得出控制力矩，并转换为电压信号，最后通过D/A输出模块输出模拟电压信号到伺服驱动器，以力矩模式驱动工业机器人完成运动控制。

其中，在IPC上实现的步骤为：

(1).将插补完成的数据存放至FPGA运动控制器的FIFO区；

(2).IPC将机器人的动力学参数和控制参数通过工业以太网接口和集成在FPGA硬件电路上的以太网接口发送至FPGA运动控制器，实现对FPGA运动控制器中动力学参数和控制参数的设定；

FPGA运动控制器进行以下步骤：

(1).将插补完成后的数据传送至FIFO进行缓冲存储；

(2).编码器模块对工业机器人各轴实际运动位置光电编码器反馈信号进行解码，并传送至实时控制模块；

(3).实时控制模块根据FIFO区插补数据和位置反馈信号数据先进行有限时间速度观测器的计算，获得速度信号，再进行有限时间稳定性控制算法的实时计算得到控制力矩，并转换为电压信号，最后通过D/A输出模块将模拟电压信号实时输出；

(4).伺服驱动器接收来自D/A输出模块的模拟电压信号，采用三环控制，从里到外分别为电流环、速度环、位置环，以力矩控制模式控制工业机器人的机械本体进行协调运动，最终实现工业机器人末端位置的精确跟踪。

其中，所述有限时间稳定性控制算法如下：

其中分别为当前周期的关节位置、关节估计速度和期望加速度向量；M₀(q)∈R^n×n为对称正定的惯性矩阵；为哥氏力和向心力矩阵；G₀(q)∈Rⁿ为重力向量项；τ∈Rⁿ为各关节的力矩输入向量；Sig(·)^α＝|·|^αsgn(·),0＜α＜1，β＝2α/(α+1),K₁，K₂为正定的对角矩阵，此采样周期内的跟踪位置误差为：e＝q-q_d,跟踪速度误差其中当前速度由有限时间收敛的速度观测器估计得出，令设为其估计值，即有并定义估计误差为则有：

其中γ＞0，L₁,L₂为正定的对角矩阵，从而得出速度估计值；

再根据伺服驱动器输入电压信号与输出力矩的线性关系λ，求出控制器输出的模拟电压信号u＝λτ。

本发明的主要逻辑控制器件为FPGA核心控制芯片，主要算法为有限时间稳定性控制方法和有限时间收敛的速度观测器算法，两者软硬结合，共同完成高性能的运动控制功能，相对于现有技术，具有如下的优点及效果：

(1)、有限时间稳定的系统相比传统的渐近稳定性系统具有更快的瞬态响应特性、更高的跟踪精度和更强的鲁棒性。采用基于FPGA控制器来进行带速度观测器的有限时间稳定性算法的设计优势在于考虑了机器人的动力学特性、跟踪精度高、实时性好、应用灵活。实时性好的优势来源于FPGA的硬件逻辑，特别是FPGA的并行执行和流水线操作。FPGA控制器的逻辑功能全部用硬件电路实现，因此所有的延迟只来源于门电路，而一般门电路的延迟都在纳秒级别。与传统的基于DSP的PID运动控制器相比，控制精度更高，系统更加小型化，集成度更高，运行速度更快，同时减少物理连接，因此采用FPGA实现有限时间稳定性控制，可以弥补上面所得到的缺憾，很好地达到有限时间稳定性控制和降低系统成本设计维护难度的双重目的。

(2)、本发明的速度观测器保证了估计误差是有限时间收敛的，因而收敛速度快、估计精度高、鲁棒性强。

(3)、本发明基于FPGA控制器实现有限时间稳定性算法的计算，可同时控制3～6个伺服电机，采样时间减少到0.01ms,瞬态响应时间0.5s，伺服控制环可通过改善采样周期，位置跟踪性能得到提高。

(4)、本发明中使用基于力矩的反馈控制原理，考虑了被控对象的动力学特性，该方法可以用于机器人高速运动的场合。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1是基于PC+FPGA的带速度观测器的有限时间稳定性控制系统的结构图。

图2是工业机器人的D-H坐标系。

图3是有限时间稳定性控制框图。

图4是控制信号数据流程图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

本发明系统的硬件结构如图1所示，本发明的主要逻辑控制器件为FPGA核心控制芯片，主要算法为有限时间稳定性控制方法和有限时间收敛的速度观测器算法，两者软硬结合，共同完成高性能的运动控制功能。基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，包括IPC1、FPGA运动控制器2、伺服驱动器3、工业机器人4、以太网通讯模块5，FPGA运动控制器2包括插补模块21、实时控制模块22、编码器模块23、D/A输出模块24，工业机器人的上位机在IPC1上给定工业机器人的惯性参数和控制参数，IPC1通过以太网通讯模块5将数据传送到FPGA运动控制器2完成动力学参数和控制参数的设置，实时控制模块22根据插补模块21提供的插补数据、编码器模块23提供的位置反馈信号，进行有限时间收敛速度观测器算法的实时计算，先获得速度信号，再进行有限时间稳定性控制算法的实时计算得出控制力矩，并转换为电压信号，最后通过D/A输出模块24输出模拟电压信号到伺服驱动器3，以力矩模式驱动工业机器人4完成运动控制。

常用的工业机器人系统具有六自由度，为便于说明本发明的实施内容，这里仅考虑机器人的前三个自由度。图2是工业机器人的D-H坐标系，控制系统内控制信号的数据流程如图4所示，该机器人的基于动力学的有限时间稳定性控制算法的简易框图如图3所示，具体如下：

式中Sig(·)^α＝|·|^αsgn(·),0＜α＜1，β＝2α/(α+1),K₁，K₂为正定的对角矩阵。

其中各矩阵定义如下：

所有矩阵的元素如下：

h₁₂＝I_3yzc₂₃+I_3xzs₂₃+I_2yzc₂+I_2xzs₂+a₂m₃r_3zs₂

h₁₃＝I_3yzc₂₃+I_3xzs₂₃

h₂₃＝I_3zz+a₂m₃(r_3xc₃-r_3ys₃)

h₃₃＝I_3zz

c₃₃＝0

g₁＝0

g₂＝gm₃r_3xc₂₃-gm₃r_3ys₂₃+a₂gm₃c₂+gm₂r_2xc₂-gm₂r_2ys₂(5)

g₃ ^＝gm₃r_3xc₂₃-gm₃r_3ys₂₃

其中c₃＝cosq₃,s₃＝sinq₃,c₂₃＝cos(q₂+q₃)，s₂₃＝sin(q₂+q₃)，c2₂₃＝cos(2q₂+2q₃),s2₂₃＝sin(2q₂+2q₃),c₃2₂＝cos(2q₂+q₃),s₃2₂＝sin(2q₂+q₃)，g为重力加速度。

机器人的动力学参数说明如下：a1,a2为杆长；m1,m2,m3分别为杆1、2、3的质量；I1xx,I1yy,I1zz为杆1对三个轴的转动惯量；I1xy,I1yz,I1xz为杆1对三个轴的惯性积；rc1＝[r1x,r1y,r1z]为杆1在系{1}中的坐标表达式；杆2和杆3与此类同。参数值大小可通过实验辨识得出，精度越高对控制越有利。

机器人的实际位置信号可能通过编码器模块获得，而速度信号则通过有限时间收敛的速度观测器计算得出：

具体的，在IPC1上实现的步骤为：

(1).将插补模块21提供的插补数据存放至FPGA运动控制器2的FIFO区；

(2).机器人上位机将在IPC1上给定机器人的动力学参数和控制参数，通过以太网通讯模块5(包括以太网接口和集成在FPGA硬件电路上的以太网接口)发送至FPGA，实现对FPGA运动控制器2中动力学参数和控制参数的设置；

FPGA运动控制器2进行以下步骤：

(1).将插补模块21提供的插补数据传送至FIFO进行缓冲存储；

(2).编码器模块23对工业机器人各轴实际运动位置光电编码器反馈信号进行解码，并传送至实时控制模块22；

(3).实时控制模块22根据FIFO区的插补数据、编码器模块23提供的位置反馈信号，依式(6)进行有限时间收敛速度观测器算法的实时计算，先获得速度信号；

(4).再根据式(1)进行有限时间稳定性控制算法的实时计算得出控制力矩τ_k，并转换为电压信号u_k＝λτ_k,其中λ为伺服驱动器3输入电压信号与输出力矩的线性关系；

(5).速度观测器的计算和控制力矩的计算是迭代进行的，并且两者之间是并行的。

(6).最后通过D/A输出模块24输出模拟电压信号到伺服驱动器3，各轴的伺服驱动器3接收到各轴的模拟电压信号后，以力矩模式驱动工业机器人4完成运动控制，从而实现高速高精度的轨迹跟踪控制。

(7).根据伺服手册设置相关参数：采用三环控制方式，从里到外分别为电流环、速度环、位置环，以力矩模式驱动工业机器人各轴相应的伺服电机，控制各轴进行协调运动，最终实现精确的轨迹跟踪。

本发明得到广东省高等学校科技创新项目(No.2013KJCX0100)的资助。

总之，本发明虽然例举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明的范围，否则都应该包括在本发明的保护范围内。

Claims (2)

1.一种基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，其特征在于：包括IPC(1)、FPGA运动控制器(2)、伺服驱动器(3)、工业机器人(4)、以太网通讯模块(5)，FPGA运动控制器(2)包括插补模块(21)、实时控制模块(22)、编码器模块(23)、D/A输出模块(24)，工业机器人的上位机在IPC(1)上给定工业机器人的惯性参数和控制参数，IPC(1)通过以太网通讯模块(5)将数据传送到FPGA运动控制器(2)完成动力学参数和控制参数的设置，实时控制模块(22)根据插补模块(21)提供的插补数据、编码器模块(23)提供的位置反馈信号，进行有限时间收敛速度观测器算法的实时计算，先获得速度信号，再进行有限时间稳定性控制算法的实时计算得出控制力矩，并转换为电压信号，最后通过D/A输出模块(24)输出模拟电压信号到伺服驱动器(3)，以力矩模式驱动工业机器人(4)完成运动控制；

其中，在IPC(1)上实现的步骤为：

(1).将插补完成的数据存放至FPGA运动控制器(2)的FIFO区；

(2).IPC(1)将机器人的动力学参数和控制参数通过工业以太网接口和集成在FPGA硬件电路上的以太网接口发送至FPGA运动控制器(2)，实现对FPGA运动控制器(2)中动力学参数和控制参数的设定；

FPGA运动控制器(2)进行以下步骤：

(1).将插补完成后的数据传送至FIFO进行缓冲存储；

(2).编码器模块(23)对工业机器人各轴实际运动位置光电编码器反馈信号进行解码，并传送至实时控制模块(22)；

(3).实时控制模块(22)根据FIFO区插补数据和位置反馈信号数据先进行有限时间速度观测器的计算，获得速度信号，再进行有限时间稳定性控制算法的实时计算得到控制力矩，并转换为电压信号，最后通过D/A输出模块(24)将模拟电压信号实时输出；

(4).伺服驱动器(3)接收来自D/A输出模块(24)的模拟电压信号，采用三环控制，从里到外分别为电流环、速度环、位置环，以力矩控制模式控制工业机器人(4)的机械本体进行协调运动，最终实现工业机器人末端位置的精确跟踪。

2.根据权利要求1所述的基于PC+FPGA的带速度观测器的有限时间稳定性控制系统，其特征在于，所述有限时间稳定性控制算法如下：

$\mathrm{\τ}=M_0\left(q\right)\[{\stackrel{\·\·}{q}}_d-K_{1}Sig{\left(e\right)}^{\mathrm{\α}}-K_{2}Sig{\left(\stackrel{\·}{\hat{e}}\right)}^{\mathrm{\β}}\]+C_0(q,\stackrel{\·}{\hat{q}})\stackrel{\·}{\hat{q}}+G_0\left(q\right)$

其中分别为当前周期的关节位置、关节估计速度和期望加速度向量；M₀(q)∈R^n×n为对称正定的惯性矩阵；为哥氏力和向心力矩阵；G₀(q)∈Rⁿ为重力向量项；τ∈Rⁿ为各关节的力矩输入向量；Sig(·)^α＝|·|^αsgn(·),0＜α＜1，β＝2α/(α+1),K₁，K₂为正定的对角矩阵，此采样周期内的跟踪位置误差为：e＝q-q_d,跟踪速度误差其中当前速度由有限时间收敛的速度观测器估计得出，令设为其估计值，即有并定义估计误差为则有：

$\left\{\begin{array}{c}{\stackrel{\·}{\hat{p}}}_1={\hat{p}}_2+L_{1}Sig{\left({\tilde{p}}_1\right)}^{\frac{\mathrm{\γ}+1}{2}}\\ {\stackrel{\·}{\hat{p}}}_2={\stackrel{\·\·}{q}}_d-K_{1}Sig{\left(e\right)}^{\mathrm{\α}}-K_{2}Sig{\left(\stackrel{\·}{\hat{e}}\right)}^{\mathrm{\β}}+L_{2}Sig{\left({\tilde{p}}_1\right)}^{\mathrm{\γ}}\end{array}\right.$

其中γ＞0，L₁,L₂为正定的对角矩阵，从而得出速度估计值；

再根据伺服驱动器输入电压信号与输出力矩的线性关系λ，求出控制器输出的模拟电压信号u＝λτ。