[0056] 为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
[0057] 需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
[0058] 关节伺服控制系统是机器人控制技术的核心，选择合理的伺服控制策略关系到机器人整体的运动性能。直流伺服控制系统，以直流力矩电机为伺服控制系统执行元件，并开发出相关的关节驱动器，在驱动器中，合理的控制策略对机器人的定位精度、跟踪精度、快速性和抗干扰性有很大的意义。实践表明，经典的PID控制往往不能保证机器人在存在干扰或者系统发生改变时仍有最佳的控制效果。而自抗扰控制摆脱了系统对精确模型的依赖，从本质上实现了非线性控制。
[0059] 工业机器人是面向工业领域的多关节机械手或多自由度的机器人。常见的工业机器人由机器人主体、驱动部分和控制系统三个基本部分组成，一般有3-6个运动自由度，主要应用于切割、焊接、涂装或码垛、物品分拣等领域，在结构上一般有圆柱坐标型、直角坐标型和关节型等几种。其中，关节型机器人的空间运动是由关节的回转运动来实现的。从结构上看，关节型机器人动作灵活，空间结构紧凑，占地面积小。由于结构上的优点，关节型机器人在工业中应用十分广泛。但是，其运动学模型复杂，控制难度大。
[0060] 为了使得机器人能够获得更高的位置跟踪精度和更强的鲁棒性，系统补偿的控制策略开始越来越受到关注。
[0061] 在前馈控制中，可以通过机器人动力学方程预测出系统的负载扰动，通过前馈的方式对机器人模型的非线性部分进行补偿，在一定程度上平衡了稳定性和控制精度之间的矛盾。虽然前馈控制可以对系统的非线性部分进行补偿，并且前馈项可以通过离线计算获得，但是前馈补偿无法对关节型机器人的高度耦合的关节力矩进行补偿。
[0062] 现有的机器人控制方法大多依赖于系统的数学模型，但是由于机器人控制系统的模型复杂，加上一些位置因素的影响，难以建立精确的数学模型。经典PID控制器不需要事先知道系统模型结构，由于其控制策略简单并易于实现，在工业机器人领域得到了广泛的应用。但是系统存在扰动的情况下，或者系统响应频率要求很高的情况下，PID控制效果无法得到保障。自抗扰控制技术，利用其不依赖于被控对象精确模型的特点，实现机器人多关节力矩输出的解耦控制，为机器人多关节协调控制提供了一种新的方法。
[0063] 自抗扰控制器(Auto Disturbance Rejection Controller，ADRC)是针对一类非线性不确定系统提出的一种非线性控制器，由跟踪微分器(Tracking differentiator，TD)、扩展状态观测器(Extended States Observe，ESO)和非线性误差反馈律(Non-Linear States Error Feedback，NLSEF)三部分构成。其中，TD用于安排系统过渡过程并提取各阶广义微分信号v2；ESO作为一种特殊的状态观测器，用于准确实时估计系统各阶状态变量及系统总扰动(包括系统未建模动态与未知外扰的总和)，从而提取为实现“状态反馈”和“模型外扰补偿”所需的信息，是自抗扰控制器得以实现的关键；NLSEF利用系统输入与输出之间误差及误差的各阶微分，通过非线性组合来提取控制量u，并通过对系统总扰动进行补偿，可以将整个非线性系统化为串联积分型的形式，从而易于构造理想的控制器，以达到系统的控制效果。
[0064] 图2是根据本申请实施例的一种自抗扰控制器结构示意图，v是控制目标，v1是对v安排的过渡过程，v2是过渡过程v1的微分信号，u0为状态误差反馈控制量，u为系统输出的最终控制量，b0为补偿因子。ADRC对经典PID进行了4个方面的改进，分别是安排过度过程；采用跟踪微分器对被控对象提取微分信息；由非线性扩张观测器实现扰动估计和补偿；由误差的P、I、D非线性组合构成非线性PID控制器。
[0065] 由于ADRC控制器结构简单、处理效率高、抗干扰能力强，可以解决工业机器人运动过程不确定外界扰动问题。
[0066] 另外，现有技术在工业机器人领域，基于动力学模型的控制方法主要采用的是反馈补偿控制，对动力学反馈控制结构而言，当动力学模型足够精确时，通过反馈补偿方式能很好解决机器人非线性时变问题，同时减少外反馈环路误差调控功耗，提高控制器动态响应和跟踪性能。但是实际上，机器人动力学模型参数因负载变化、采样计算误差而很难精确获取；由于动力学反馈控制结构对模型计算实时性要求较高，需满足力矩环路控制算法时耗约束；最后，需实时获取关节加速度信息，而实际上往往因微分噪声放大效应，高阶微分信号品质低下。
[0067] 在上述应用场景下，本申请提供了一种自抗扰控制方法实施例，该实施例可以应用于但不限于工业机器人领域，以及电机励磁控制、机械手位姿控制、航天器姿态控制、精密车床中刀具的伺服控制、四旋翼无人机控制、噪音控制、磁悬浮、液压机控制等领域。
[0068] 下面本申请以关节型机器人为例进行说明。关节型机器人是由多个连杆通过旋转关节连接而成的多自由度机构。关节是整个机器人结构中最主要的组成部分，其重量、结构和尺寸的选择对机器人的性能影响很大。
[0069] 本申请提供的自抗扰控制方法，通过动力学前馈补偿和不确定性扰动补偿，实现多关节控制解耦，解决了机器人非线性、强耦合时变问题，提高了运动控制的实时性、鲁棒性和精度。
[0070] 图3是根据本申请实施例的一种自抗扰控制方法流程图，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。如图3所示，该方法包括如下步骤：
[0071] 步骤S301，采用并行的多个自抗扰控制器实现受控对象的解耦控制，构成多个子控制系统，其中，受控对象为包含多个输入变量、多个输出变量的多输入多输出系统，每个子控制系统为包含一个输入变量和一个输出变量的单输入输出系统。
[0072] 步骤S302，基于动力学前馈模型对每个子控制系统的确定性扰动进行补偿，并通过扩张观测器对每个子控制系统的不确定性扰动进行补偿；
[0073] 步骤S303，基于每个子控制系统的输入输出关系，根据补偿后的输入量确定每个子控制系统的输出量。
[0074] 由动力学前馈控制结构知，电流指令作为系统输入，电机转角作为系统输出。如图4所示，对电机而言，机器人动力学特性可看作是电机输出力矩τm克服摩擦力矩τmf和惯量矩的结果。
[0075] 多关节工业机器人的动力学前馈模型为：
[0076]
[0077] 其中，τm为电机的转矩矢量，τmf为电机克服摩擦力造成的总阻转矩矢量，Jm为电机轴的转动惯量， 为电机轴的角加速度， 为电机输出轴的惯性转矩矢量，τml为电机负载转矩矢量，用于克服关节动力学特性，包含关节惯量矩、摩擦矩、重力矩及关节负载等。
[0078] 为建立电机输入电流Iq与输出位置qm关系，首先根据机械传动关系以及电磁转矩方程可得出机器人关节输出力矩，即：
[0079] 多关节工业机器人的关节输出力矩矢量τ为
[0080]
[0081] 其中，τ为多关节工业机器人的关节输出力矩矢量，q、 分别为关节转过角度向量、关节角速度向量、关节角加速度向量，M(q)为对称惯性矩阵， 为离心力和哥氏力矢量，G(q)为重力向量， 为摩擦力矩矢量，τload为关节负载力矩矢量。
[0082] 继而可得，电机轴运动方程：
[0083] 多关节工业机器人的电机轴的运动方程为：
[0084]
[0085] 其中，
[0086]
[0087] τ＝τpre+Δτ；
[0088] 其中，
[0089]
[0090] 其中，
[0091] Λτpre＝KmIpre，F1＝-B0Ipre；
[0092] 其中， 为总扰动，Km为电机转矩常数，Iq为电机的实际驱动电流，τpre为基于动力学前馈模型估算的力矩，Δτ为通过扩张观测器补偿的力矩，Λτpre为电机理论输出力矩矢量，Ipre为与电机理论输出力矩对应的驱动电流。
[0093] 在ADRC的构建中，通常将 看作系统综合加速度扰动，包含机器人动力学特性扰动及其他不确定扰动。在机器人高速大惯量变化运动中， 实际变化较大，大幅度扰动变化可能引起扩张观测器(ESO)收敛缓慢。可从两方面着手解决，其一设计具有高稳定性的ESO，保证其能有效预估所有动力学干扰；其二，充分利用已辨识的理论动力学模型，并进行不确定性补偿，ESO仅观测除模型外的小量误差部分。
[0094] 从电机端向关节端看，关节实际输出力矩τ可看作是电机输出轴“扰动”，其中τpre部分可通过辨识模型补偿，Δτ模型外不确定性部分可通过ESO进行观测。
[0095] 记uff＝Ipre，U＝B0u，则，受控对象内电机的输出状态方程为：
[0096]
[0097] 其中，u为受控对象的实际输入；U为虚拟控制量，Y为系统输出量，F为电机的总扰动量，F1为基于动力学前馈模型补偿的确定性扰动，F2为通过扩张观测器补偿的不确定性扰动，B0为系统增益。
[0098] 显然，机器人系统为二阶MIMO系统，u为受控对象实际输入，即电机驱动系统电流控制量，U可看作系统“虚拟控制量”，Y为系统输出量，F看作电机系统总扰动量。另外，F1为与已辨识动力学模型相关的理论扰动，可用于前馈补偿；F2为除模型外的不确定性加速度扰动，可采用ESO观测。值得注意的是，F2包含关节力矩误差，其中可能含有少量时变加速度分量，无法被ESO观测，理论上需补偿相应控制电流。本文通过修正系统增益B0满足实际惯量变化需求，即：
[0099] B0＝Jm-1Kmζ (10)
[0100] 其中，ζ为修正系数对角矩阵，ζi≥1。大量仿真研究表明，在ADRC控制中，参数B0的估计要求并不高，相对误差在30％之内，甚至更高时也不会影响闭环控制品质，因此系数ζ在实际应用中可取为常值。
[0101] 机器人各关节控制解耦能减轻控制难度。在ADRC控制系统中，称系统控制量之外的模型部分F为“动态耦合部分”，称U为“静态耦合”部分。在该MIMO系统中，通道i的输入输出关系为：
[0102]
[0103] 假定Ui为通道i的“虚拟控制量”，则其与输出量yi之间构成单输入-单输出关系，即通道i的受控输出yi与控制量Ui间被完全解耦；而Fi被看成作用于通道i上的总和扰动，包含机器人动力学对应加速度扰动和模型外不确定干扰。这样，在控制量U和输出向量Y之间并行嵌入n个ADRC控制器便能实现多变量系统结构控制，解耦过程如图5所示。
[0104] 需要说明的是，实际控制量u与虚拟控制量U间转换关系为u＝B0-1U。显然B0是可逆的和时变有界的，在ADRC控制中，其估值精度要求并不高。值得注意的是，动态耦合部分F在解耦过程中被看作总和扰动被估计并补偿，其估计精度决定了ADRC控制性能。由于机器人运动过程中惯量变化很大，相应的“扰动”加速度变化也很大，若全部由ESO观测，势必会增加ESO负担。通过机器人力矩前馈方式弥补总和扰动已知部分，仅未知少量部分由ESO承担，从而较大程度减少ESO观测负担，提高系统鲁棒性。
[0105] 解耦后的机器人系统转化为了6个独立的SISO系统，各自由度成独立控制通道。为提高ADRC控制性能和稳定性，采用计算力矩前馈控制策略。值得注意的是此时控制对象不再是机器人关节位置，而是力矩模式(电流模式)下驱动部件与机构本体。
[0106] 为了减少扰动观测阶次，提高扰动观测精度，本申请还提供了一种自抗扰控制器实施例，该自抗扰控制器采用串级控制结构。如图6所示，ADRC串级系统结构指的是控制量u(电流)先驱动中间变量x2(速度)，而x2再去驱动x1(位置)以达到控制目的。
[0107] 图7是根据本申请实施例提供的一种可选的自抗扰控制器结构示意图，如图7所示，该自抗扰控制器包括：微分跟踪器，微分跟踪器的输入端与受控对象的输出端连接，用于估计受控对象的位置和速度；由第一控制器和第一扩张观测器构成的位置环路，其中，第一扩张观测器的输入端与微分跟踪器的输出端连接，第一扩张观测器的第一输出端接入到第一控制器的输入端，第一扩张观测器的第二输出端接入到第一控制器的输出端，第一控制器基于位置误差反馈控制律根据第一控制器输入端的位置控制量和第一扩张观测器的第一输出端输出的位置观测值，生成速度控制量，其中，第一扩张观测器的第二输出端输出的速度观测值用于补偿速度控制量；由第二控制器和第二扩张观测器构成的速度环路，其中，第二扩张观测器的输入端与微分跟踪器的输出端连接，第二扩张观测器的第一输出端接入第二控制器的输入端，第二扩张观测器的第二输出端接入到第二控制器的输出端，第二控制器基于速度误差反馈控制律根据补偿后的速度控制量和第二扩张观测器的第一输出端输出的速度观测值生成加速度控制量，其中，第二扩张观测器的第二端输出的加速度扰动观测值用于补偿加速度控制量。
[0108] 其中，自抗扰控制器采用的控制算法设计如下：
[0109]
[0110] 其中，fal()为幂次函数，δ为微分跟踪器的速度因子，fal(e，α，δ)为扩张状态观测器的参数；sign()为符号函数；
[0111] 其中，
[0112] 微分跟踪器采用的微分状态估计算法设计如下：
[0113]
[0114] 其中，
[0115]
[0116] 其中，r为速度因子，h0为微分跟踪器的采样步长，fhan()最速控制综合函数，x1为输入信号的跟踪信号，x2为提取输入信号的微分信号。
[0117] 位置环路采用的控制算法设计如下：
[0118]
[0119] 其中，h1为速度环路的采样步长，z11、z12分别为第二扩张观测器输出的位置观测值和速度扰动观测值，u为输出控制量，β01、β02为由采样步长决定的控制器的参数，β1为状态误差的反馈增益。
[0120] 速度环路采用的控制算法设计如下：
[0121]
[0122] 其中，h2为速度环路的采样步长，z21、z22分别为第二扩张观测器输出的速度观测值和加速度扰动观测值，u为输出控制量，β03、β04为由采样步长决定的控制器的参数，b0为补偿因子，β2为状态误差的反馈增益。
[0123] 可选地，位置误差反馈控制律和速度误差反馈控制律均采用比例控制律。
[0124] 可选地，第一扩张观测器和第二扩张观测器均为一阶扩张观测器。
[0125] 本申请实施例还提供了一种工业机器人，包括：上述任意一项可选的自抗扰控制器。
[0126] 可选地，上述工业机器人可以为多关节工业机器人。优选地，该多关节工业机器人为6自由度关节机器人。
[0127] 基于本申请实施例提供的自抗扰控制方法，下面结合空间圆弧轨迹跟踪来验证本申请控制算法的性能。选取常用PI控制算法作为对比。针对响应速度和轨迹跟踪精度指标，分别考察关节坐标系下各关节跟踪轨迹和笛卡尔空间坐标系下末端位置轨迹跟踪情况。
[0128] 机器人采用华数6R工业机器人HSR-JR605-C。为验证本文提出控制方法的有效性，在六自由度串联机器人平台上进行空间圆弧轨迹跟踪实验。选取笛卡尔空间圆弧运动轨迹作为测试轨迹。机器人带负载3.6Kg，初始姿态设为q＝[0，-π/2，0，0，π/2，0]T，如图8所示，空间圆弧轨迹可由末端点P的3个空间坐标点确定，其中A＝[376.5，0，331]，B＝[396.5，20，331]，C＝[416.5，0，331]，圆弧的圆心O＝[396.5，20，331]，单位mm。点P从A点出发，沿弧顺时针方向加速运动至C点停止，然后从C出发沿 顺时针方向返回至点A。通过笛卡尔空间圆弧插补算法，结合逆运动学计算，可获得关节运动状态指令。该指令通过传动比转化为电机端运动指令作为控制器输入。
[0129] 表1机器人控制器参数列表
[0130]
[0131]
[0132] 图9(a)、图9(b)、图9(c)和图9(d)分别示出了不同控制器下关节2轨迹跟踪结果及误差曲线(w＝2rad/s)。其中，图9(a)是PI控制器轨迹跟踪曲线；图9(b)是PI控制器轨迹跟踪误差；图9(c)ADRC前馈控制器轨迹跟踪曲线；图9(d)ADRC前馈控制器轨迹跟踪误差。
[0133] 图10(a)、图10(b)、图10(c)和图10(d)示出了基于ADRC前馈控制器的机器人末端笛卡尔空间跟踪轨迹(w＝2rad/s)。其中，图10(a)是三维图；图10(b)是XOY平面投影；图10(c)是XOZ平面投影；图10(d)YOZ平面投影。
[0134] 图11(a)、图11(b)、图11(c)和图11(d)示出了基于PI控制器的机器人末端笛卡尔空间跟踪轨迹(w＝2rad/s)。其中，图11(a)是三维图；图11(b)是XOY平面投影；图11(c)是XOZ平面投影；图11(d)YOZ平面投影。
[0135] 图12(a)、图12(b)、图12(c)和图12(d)示出了基于PI控制器的机器人末端笛卡尔空间跟踪轨迹(w＝2rad/s)。其中，图12(a)是三维图；图12(b)是XOY平面投影；图12(c)是XOZ平面投影；图12(d)YOZ平面投影。
[0136] 由上分析可知，本例中PI控制器和ADRC前馈控制器均能实现轨迹稳定跟踪，但跟随精度和响应速度不尽相同。如图9(a)、图9(b)、图9(c)和图9(d)所示，关节坐标系下，对大惯量关节2而言，PI控制器的轨迹跟随误差较大，最大约为0.005rad(如图9(b)所示)，高于ADRC前馈控制器跟随误差(如图9(d)所示)；如图9(a)和图9(c)所示，从响应速度来看，ADRC前馈控制器也明显优于传统PI控制算法。可见，ADRC前馈控制算法在控制精度和响应速度方面明显优于传统PI算法，反应在笛卡尔空间坐标系下，关节末端跟踪轨迹表现更加精准。从平面投影来看，ADRC前馈控制器对机器人末端控制精度可高达0.015mm(如图10(c)和图
10(d)所示)，远高于PI控制器的0.3mm(如图11(c)和图11(d))。另外从误差分布来看，机器人在启动时刻末端跟踪误差较大，其很大程度为关节摩擦引起，一方面开始运动时关节静摩擦力较大，纯PI控制器很难快速调整，一方面摩擦模型存在误差，静摩擦“突变时刻”很难监测、无法及时补偿，显然相较于传统PI控制器而言，本文提出的ADRC前馈控制器对摩擦不确定扰动具有良好抑制作用，总体上，在控制精度和响应速度上均具有较大优势。此外，针对不同速率，尤其是高速(9rad/s)下，ADRC前馈控制器均具有良好轨迹跟踪品质和鲁棒性(如图12(a)、图12(b)、图12(c)和图12(d)所示)。
[0137] 本申请还提供了一种自抗扰控制系统实施例，该系统包括：电机；多关节工业机器人，包括：多个关节，其中，多关节工业机器人为包含多个输入变量、多个输出变量的多输入多输出系统；
[0138] 多个自抗扰控制器，并行于电机与多关节工业机器人的每个关节之间，其中，通过多个自抗扰控制器构成独立控制的多个子控制系统以实现对多关节工业机器人的解耦控制；控制器，与电机连接，用于基于动力学前馈模型对每个子控制系统的确定性扰动进行补偿，并通过扩张观测器对每个子控制系统的不确定性扰动进行补偿，并基于每个子控制系统的输入输出关系，根据补偿后的输入量确定每个子控制系统对应的关节的输出力矩。
[0139] 本申请还提供了一种自抗扰控制装置，该装置包括：第一处理单元，用于采用并行的多个自抗扰控制器实现受控对象的解耦控制，构成多个子控制系统，其中，受控对象为包含多个输入变量、多个输出变量的多输入多输出系统，每个子控制系统为包含一个输入变量和一个输出变量的单输入输出系统；第二处理单元，用于基于动力学前馈模型对每个子控制系统的确定性扰动进行补偿，并通过扩张观测器对每个子控制系统的不确定性扰动进行补偿；确定单元，用于基于每个子控制系统的输入输出关系，根据补偿后的输入量确定每个子控制系统的输出量。
[0140] 本申请还提供了一种存储介质，存储介质包括存储的程序，其中，程序执行上述任意一项可选的自抗扰控制方法。
[0141] 本申请还提供了一种处理器，处理器用于运行程序，其中，程序运行时执行上述任意一项可选的自抗扰控制方法。
[0142] 上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。
[0143] 在本申请的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
[0144] 在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
[0145] 所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0146] 另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0147] 所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
[0148] 以上所述仅是本申请的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。
