本发明涉及柔性机械臂级联控制器领域，尤其是一种基于干扰观测器的柔性机械臂级联控制方法及系统。
背景技术：
基于新型的电力电子技术与新型电机的发展，工业应用型机械臂得到了长足的发展。大多数针对工业机械臂的控制器设计是基于机器人刚体模型。具有柔性关节的轻型机械臂,因其质量轻、惯性小、能耗低等优点而被广泛应用。而在控制器的设计中，如果更多的考虑机械臂之中的柔性环节，则可以进一步提高控制性能。同样的，柔性关节由于谐波减速器、力矩传感器等弹性部件的存在,因此在运行过程中的变形和运行结束后的残余抖动是不可避免的。残余抖动将导致机械臂的定位精度和控制稳定性降低。
传统的柔性机械臂控制策略常使用线性控制方法如pd控制，因其结构简单、容易实现而被广泛应用。然而，实际的柔性机械臂系统是一个非线性系统，同时机械臂易于受到包括系统未建模动态、参数不确定以及负载变化等各种类型干扰的影响。在这些干扰的作用下，传统的线性控制方法无法使系统获得满意的控制性能和较强的干扰抑制能力。伴随着技术的发展，越来越多的先进控制方法被不断提出。这些先进控制方法从不同的方面对柔性机械臂系统性能进行了改进。但是这些控制方法通常是基于单一的反馈控制，无法有效观测系统中存在的各类干扰并抑制干扰。
技术实现要素：
鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于干扰观测器的柔性机械臂级联控制方法及系统，用于解决现有技术中的至少一个缺陷。
为实现上述目的及其他相关目的，本发明提供一种基于干扰观测器的柔性机械臂级联控制方法，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
所述控制方法包括：
获取臂侧实际位置q；
获取电机侧实际位置θ；
根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
可选地，所述臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
可选地，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
可选地，所述电机侧位置控制器的表达式为：
其中，kp2,kd2为控制器增益,
可选地，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
为实现上述目的及其他相关目的，本发明提供一种基于干扰观测器的柔性机械臂级联控制系统，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
所述控制系统包括：
臂侧实际位置获取模块，用于获取臂侧实际位置q；
电机侧实际位置获取模块，用于获取电机侧实际位置θ；
电机侧参考位置获取模块，用于根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
输出电压获取模块，用于根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
驱动模块，用于根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
可选地，所述臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
可选地，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
可选地，所述电机侧位置控制器的表达式为：
其中，kp2,kd2为控制器增益,
可选地，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
如上所述，本发明的一种基于干扰观测器的柔性机械臂级联控制方法及系统，具有以下有益效果：
1、相比传统的柔性机械臂调速系统控制方法而言，本发明所述控制方法算法结构简单，并且基于本发明可以使柔性机械臂系统达到良好的动态性能和稳态性能。
2、相比以往的级联控制系统，本发明所述的控制方案实现策略包括反馈控制和基于干扰估计的前馈补偿，可以有效提高系统的抗干扰能力。
3、对于柔性机械臂系统易受各类未知时变干扰影响的特性，本发明所述的控制方法可以有效抑制表现形式为常值、斜坡和抛物线等多种形式的时变干扰。
附图说明
图1是本发明的控制系统原理简图；
图2是本发明的广义对象详细原理框图；
图3是本发明的一种基于干扰观测器的柔性机械臂级联控制方法的流程图；
图4是本发明的干扰观测器原理框图：
图5是本发明的臂侧位置控制器原理框图；
图6是本发明的电机侧位置控制器原理框图；
图7是两种控制方式下的系统臂侧位置信号仿真结果对比图；
图8是两种控制方式下的系统电机侧位置信号仿真结果图；
图9是本发明控制方式下的系统控制电压仿真结果图；
图10是干扰观测器1得到的扰动估计结果图；
图11是干扰观测器2得到的扰动估计结果图；
图12是本发明的一种基于干扰观测器的柔性机械臂级联控制方法系统的示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
本发明所述柔性机械臂的动力学方程为：
其中，q,θ为臂侧位置角度与电机侧位置角度，m(q)为广义惯性矩阵，
通常，假设柔性关节机器人系统受到未知时变不确定性与对象不确定性的扰动，为了便于对柔性机械臂系统实施控制，可将其整理写为如下形式，即:
本数学模型将不确定的时变干扰以及对象的不确定性确定为集总干扰，据此可以将上述非线性系统模型整理得到下述的模型。
其中，m＝j-1。
附图1是本发明方案的控制系统原理简图，附图2是本发明方案的控制系统详细原理框图，如图3所示，一种基于干扰观测器的柔性机械臂级联控制器设计方法，包括臂侧位置控制器、电机侧位置控制器。
本发明提供一种基于干扰观测器的柔性机械臂级联控制方法，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
s11所述控制方法包括：
s12获取臂侧实际位置q；
s13获取电机侧实际位置θ；
s14根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
s15根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
s16根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
根据附图5可知，臂侧位置控制包括臂侧位置反馈控制和基于干扰观测器的干扰估计的前馈补偿控制；通过分析扰臂侧位置控制器原理框图，臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
根据附图6可知，电机侧位置控制包括电机侧位置信号反馈控制和基于干扰观测器的干扰估计
其中，kp2,kd2为控制器增益,
由于臂侧位置控制器和电机侧位置控制器以串级方式相连；在一实施例中，臂侧位置控制器的输入端为臂侧参考位置qr与臂侧实际位置q的差值，经过臂侧位置控制器后，得到电机侧位置控制器输入端的电机侧参考位置θr；
电机侧参考位置θr与电机侧实际位置θ的差值，经过电机侧位置控制器后，得到电机侧位置控制器的输出电压u。
在一实施例中，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,qs为二阶低通滤波器模型，τ是时间常数，ed(s)为扰动估计误差，gp(s)为系统传递函数，u(s)为对象输入，y(s)为系统输出。
在一实施例中，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
如图4所示为干扰观测器模型框图，传递函数gp(s),gn(s),k(s),q(s)表示对象模型，标称模型，反馈控制以及滤波器模型。信号r(s),u'(s),d(s),
如附图7所示，将本发明(一种基于干扰观测器的柔性机械臂级联控制器设计方法)，与传统控制方法(pd控制)进行对比，相比传统的pd控制，本发明方案的控制方法可以使柔性机械臂位置输出快速跟踪给定位置，系统超调量小，调节时间短；且在(t＝8s)施加干扰后，系统波动小，调节时间极短，迅速跟踪给定信号。
由附图7可知本发明的控制方法使柔性机械臂位置信号跟踪在设置给定后迅速达到稳定值，系统超调量小，调节时间短；且在(t＝8s)施加干扰后，该系统迅速恢复，调节时间极短，与传统控制方法相比本发明设计方案几乎不受外部负载干扰影响。
如附图8所示为本发明的控制策略与级联pd控制方式下的系统实际电机侧位置信号仿真结果对比图；
如附图9所示，为本发明控制方式下的系统实际控制电压与仅使用级联pd控制方式下的控制电压仿真结果图；
如附图10、11所示，为本发明控制方式下的干扰观测器输出结果图。图中所示曲线为干扰观测器对于外部干扰的估计，易知该图随时间的变化趋势与给定干扰是一致的，且观测值
如图12所示，一种基于干扰观测器的柔性机械臂级联控制系统，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
所述控制系统包括：
臂侧实际位置获取模块11，用于获取臂侧实际位置q；
电机侧实际位置获取模块12，用于获取电机侧实际位置θ；
电机侧参考位置获取模块13，用于根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
输出电压获取模块14，用于根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
驱动模块15，用于根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
在一实施例中，所述臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
在一实施例中，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
在一实施例中，所述电机侧位置控制器的表达式为：
其中，kp2,kd2为控制器增益,
在一实施例中，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
由于系统实施例与方法实施例相对应，系统实施例的详细描述部分可以参考方法实施部分。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom,read-onlymemory)、随机存取存储器((ram,randomaccessmemory)、电载波信号、电信信号以及软件分发介质等。
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
1.一种基于干扰观测器的柔性机械臂级联控制方法，其特征在于，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
所述控制方法包括：
获取臂侧实际位置q；
获取电机侧实际位置θ；
根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
2.根据权利要求1所述的基于干扰观测器的柔性机械臂级联控制方法，其特征在于，所述臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
3.根据权利要求2所述的基于干扰观测器的柔性机械臂级联控制方法，其特征在于，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
4.根据权利要求1所述的基于干扰观测器的柔性机械臂级联控制方法，其特征在于，所述电机侧位置控制器的表达式为：
其中，kp2,kd2为控制器增益,
5.根据权利要求4所述的基于干扰观测器的柔性机械臂级联控制方法，其特征在于，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
6.一种基于干扰观测器的柔性机械臂级联控制系统，其特征在于，所述柔性机械臂包括臂侧位置控制器和电机侧位置控制器，臂侧位置控制器和电机侧位置控制器以串级方式相连；电机侧位置控制包括电机侧位置信号反馈控制和干扰观测器的干扰估计
所述控制系统包括：
臂侧实际位置获取模块，用于获取臂侧实际位置q；
电机侧实际位置获取模块，用于获取电机侧实际位置θ；
电机侧参考位置获取模块，用于根据所述实际臂侧位置q与臂侧参考位置qr以及所述臂侧位置控制器得到电机侧参考位置θr；
输出电压获取模块，用于根据所述电机侧参考位置θr与所述实际电机侧位置θ以及所述电机侧位置控制器得到电机侧位置控制器的输出电压u；
驱动模块，用于根据所述电机侧位置控制器的输出电压u驱动所述柔性机械臂。
7.根据权利要求6所述的基于干扰观测器的柔性机械臂级联控制系统，其特征在于，所述臂侧位置控制器的表达式为：
其中，kd1，kp1为控制器增益，
8.根据权利要求7所述的基于干扰观测器的柔性机械臂级联控制系统，其特征在于，在臂侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
9.根据权利要求6所述的基于干扰观测器的柔性机械臂级联控制系统，其特征在于，所述电机侧位置控制器的表达式为：
其中，kp2,kd2为控制器增益,
10.根据权利要求9所述的基于干扰观测器的柔性机械臂级联控制系统，其特征在于，在电机侧位置控制中，所述干扰观测器的表达式为：
其中,y(s)为系统输出，gp(s)为系统传递函数，gn(s)为系统标称模型，d(s)为系统扰动，
