CN117584117A - 一种基于谐振控制的双关节柔性机械臂力矩补偿方法 - Google Patents

Abstract

本发明属于柔性机械臂力矩补偿技术领域，公开了一种基于谐振控制的双关节柔性机械臂力矩补偿方法，包括S1、对双自由度串联弹性机器人进行动力学建模；S2，进行机器人关节的谐振控制；S3、进行关节解耦补偿和对应的重力补偿；本发明通过动力学建模，基于该模型进行机器人关节的谐振控制，最后进行关节解耦补偿，解决了现有基于串联弹性驱动器的多自由度机器人在运动时，某个关节的运动会导致其他关节负载端的晃动，导致位置跟踪效果恶化的问题，提高位置跟踪时的准确性；还能够实现减小弹性机器人关节到位过程中的抖动与连杆实际角度的超调，克服由重力带来的连杆侧实际角度与指令角度间的误差，提高控制精度。

Description

一种基于谐振控制的双关节柔性机械臂力矩补偿方法

技术领域

本发明涉及柔性机械臂力矩补偿技术领域，具体为一种基于谐振控制的双关节柔性机械臂力矩补偿方法。

背景技术

在机器人平台上，实现控制的动态解耦、高精度的位置和力控制，关键因素就是精确的传递力矩。传统工业机器人传递力矩的结构通常采用刚性设计，依靠精确的位置控制，在限定的环境里按照既定的轨迹完成操作，虽然其能够实现高精度与高速度传递力矩，但其存在较大的安全隐患；而当下机器人与外部未知环境的物理接触需求增加，尤其在实现人机交互、人机协作时，安全性是需要优先考虑的。因此，为了实现良好的位置与力控制的同时保证安全性的问题，柔顺驱动器被应用于机器人关节。

串联弹性驱动器是目前应用最为广泛的柔顺驱动器之一，其特点是在驱动机构与负载端之间串联弹簧或者扭簧作为弹性元件，将负载输出与电机惯量隔离，既能降低驱动器与环境接触的阻尼，同时当弹性元件刚度已知时，串联弹性驱动器可以通过测量位移值间接得到输出力的大小；此外，利用弹性元件的储能性质能够满足人机交互的安全性。但串联弹性驱动器的柔性组成结构会使得该结构的控制更加复杂，尤其在要求高速度运动的应用场景下，往往会由于控制器的响应带宽逼近机器人自身机械结构的谐振频率，弹性元件发生扭转振动，此时串联弹性驱动器的输出性能遭到破坏。

目前，关于抑制弹性元件扭转振动的控制方法，谐振控制通过在传统的PD控制基础上增加弹性元件力矩的反馈并配合干扰观测器，实现了在抑制弹性元件扭转振动的同时补偿弹性驱动器内部摩擦、参数不确定性等因素带来的干扰。另外，基于串联弹性驱动器的多自由度机器人在运动时，不同关节之间存在耦合，具体表现在位置跟踪时某个关节的运动会导致其他关节负载端的晃动，导致位置跟踪效果的恶化。因此，通过解耦补偿算法以确保多自由度机器人位置跟踪的精确性成为解决上述问题的有效方法之一。基于此，本发明对双自由度串联弹性机器人关节位置跟踪与力矩补偿，对于提高基于串联弹性驱动器的多自由度机器人位置跟踪时的准确性具有重要意义。

发明内容

本发明意在提供一种基于谐振控制的双关节柔性机械臂力矩补偿方法，通过对双自由度串联弹性机器人进行动力学建模，基于该模型进行机器人关节的谐振控制，最后进行关节解耦补偿。解决了现有的基于串联弹性驱动器的多自由度机器人在运动时，某个关节的运动会导致其他关节负载端的晃动，导致位置跟踪效果恶化的问题，提高位置跟踪时的准确性；另外，本发明还能够实现减小弹性机器人关节到位过程中的抖动与连杆实际角度的超调，同时克服由重力带来的连杆侧实际角度与指令角度间的误差，提高控制精度。

为了实现上述目的，本发明提供如下技术方案：

一种基于谐振控制的双关节柔性机械臂力矩补偿方法，包括以下步骤：

S1、对双自由度串联弹性机器人进行动力学建模；

S2，进行机器人关节的谐振控制，将PD控制、柔性元件扭矩的反馈、干扰观测器结合，实现对机器人关节的位置控制同时，减小机器人连杆的振动；

S3、进行关节解耦补偿和对应的重力补偿，基于双关节柔性机械臂连杆侧动力学方程的二阶及四阶微分形式计算解耦补偿输入与重力补偿输入，实现减小机器人关节由耦合作用带来的抖动，克服由重力带来的连杆侧实际角度与指令角度间的误差。

进一步地，在S1中，动力学建模的过程中，忽略摩擦和外力干扰项，电机侧动力学方程为：

式中，τ_m、J_m、θ与q分别为电机总力矩、电机侧的转动惯量、电机侧角度与负载侧角度；K_f为柔性元件的刚度；

负载侧的两个关节正转的方向相反，其动力学方程为：

其中；

式中，J₁＝m₂l₁d₂，/>G₁＝m₁gd₁+m₂gl₁，G₂＝m₂gd₂；等号右侧第一项为惯性项，第二项为科氏力与离心力项，第三项为重力项。经过动力学参数辨识，选取双自由度串联弹性机器人的电机与负载侧动力学相关参数如下表所示：

参数	数值
		Jm1	0.1814[kgm2]
Jm2	0.4383[kgm2]
		Kf1	126.0507[Nm/rad]
Kf2	63.0253[Nm/rad]
		J0	0.8067
J1	0.1921
		J2	0.1432
G1	19.3451
		G2	6.6429

进一步地，在S2中，将串联弹性驱动器的机器人关节等效为“电机-弹簧-负载”双质量系统，谐振控制用于实现双质量系统振动抑制，在被控的双质量系统输入端引入柔性元件力矩的反馈：

式中，τ_reac＝K(θ-q)为柔性元件力矩，K_r为柔性元件力矩反馈的增益；谐振频率令/>对于双自由度柔性机器人，J_ai是第i个关节处的等效负载惯量；

对负载侧动力学方程忽略科氏力与离心力项及重力项，令 为一关节与二关节负载侧的等效惯量，J_a1与J_a2随q₂变化，K_p、K_v、K_r也随q₂变化，利用上述表中的J₀、J₁、J₂值与机器人当前q₂值求出当前q₂下的K_p、K_v、K_r；

对于受重力影响的双自由度柔性机器人，为消除重力带来的静差，加入重力补偿，设定q_cmd＝0，则电机侧角度对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，在输入端有：

式中，G_c为待推导的重力补偿传递函数矩阵，整理得到：

进一步地，在S3中，进行关节解耦补偿和对应的重力补偿的方法，包括以下步骤：

A1、把系统总输入分为实现运动控制的输入与实现解耦的输入；

A2、分析双关节柔性机械臂关于负载侧角度与扭转角的二次微分状态方程；

A3、对实现运动控制的输入作进一步拆分，计算各关节受另一关节影响的输入成分；

A4、对步骤A2的二次微分状态方程再次微分得到四次微分状态方程，并令四次微分状态方程中的耦合相关项总和为0，得到双关节柔性机械臂各关节的解耦补偿输入；

A5、对竖直状态下的双关节柔性机械臂，求得解耦补偿输入后再加入重力补偿，消除重力带来的负载侧角度静差。

进一步地，在A1中，取参考加速度为机器人关节系统总输入， 为实现两个关节各自的控制，其控制输入u_11ref、u_22ref分别加入补偿项u_1Cref、u_2Cref以抵消干扰，于是1关节的总输入u_1ref＝u_11ref+u_1Cref，2关节的总输入u_2ref＝u_22ref+u_2Cref。

进一步地，在A2中，得到的二次微分状态方程为：

式中，δ₁＝θ₁-q₁，δ₂＝θ₂-q₂。

进一步地，在A3中，计算各关节受另一关节影响的输入成分的方法为：控制输入u_11ref与u_22ref分别受另一关节的扭转角δ的影响，其中u_11ref＝u_11Sref+u_11Dref，u_22ref＝u_22Sref+u_22Dref；u_11Sref、u_22Srefwei通过本关节的状态计算的输入，u_11Dref、u_22Dref为通过另一个关节的状态计算的输入；由于θ₁＝q₁+δ₁，θ₂＝q₂+δ₂，θ₁受δ₂影响，θ₂受δ₁影响，忽略q_cmd和q，令代入谐振控制表达式计算出u_11Dref与u_22Dref。

进一步地，在A4中，得到的四次微分状态方程为：

式中，等号右侧的第三项与第四项是待补偿的耦合相关项，令耦合相关项的总和为0，并将状态方程组中与相关的方程代入，得到：

式中，

进一步地，在A5中，消除重力带来的负载侧角度静差的方法为：设定q_cmd＝0，电机侧角度对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，在输入端有：

式中，G_c是待推导的重力补偿传递函数矩阵，设定为2×2，u_c是2×1的解耦补偿输入，加入干扰观测器的双自由度柔性机器人关节系统总输入为：

技术方案的有益效果是：

1、本发明提供的基于谐振控制的双关节柔性机械臂力矩补偿方法，通过对双自由度串联弹性机器人进行动力学建模，能够准确描述机械臂的运动学和动力学特性，为后续的补偿控制提供准确的模型基础；

2、本发明提供的基于谐振控制的双关节柔性机械臂力矩补偿方法，通过综合使用PD控制、柔性元件扭矩的反馈和干扰观测器等技术手段，实现对机器人关节的位置控制，并减小机器人连杆的振动；谐振控制能够有效抑制由于控制器响应带宽逼近谐振频率而导致的振动问题，提高机器人的输出性能；

3、本发明提供的基于谐振控制的双关节柔性机械臂力矩补偿方法，通过基于双关节柔性机械臂连杆侧动力学方程的二阶及四阶微分形式，计算解耦补偿输入与重力补偿输入；可以减小机器人关节由耦合作用造成的抖动，并克服由重力引起的连杆侧实际角度与指令角度之间的误差；通过解耦补偿和重力补偿，机器人能够更精确地实现指定位置和姿态的运动。

综上，本发明提供的基于谐振控制的双关节柔性机械臂力矩补偿方法，解决了现有的基于串联弹性驱动器的多自由度机器人在运动时，某个关节的运动会导致其他关节负载端的晃动，导致位置跟踪效果恶化的问题，提高位置跟踪时的准确性；另外，还能够实现减小弹性机器人关节到位过程中的抖动与连杆实际角度的超调，同时克服由重力带来的连杆侧实际角度与指令角度间的误差，提高控制精度；其对机器人应用领域中的精确操作和人机交互的安全性都带来实质性的改进。

附图说明

图1为本发明提供的实施例步骤S1中，双自由度串联弹性机器人结构框图；

图2为本发明提供的实施例步骤S2中，J_a1和J_a2随q₂变化的趋势图；

图3为本发明提供的实施例步骤S2中，得到的谐振控制框图；

图4为本发明提供的实施例步骤S2中，在重力补偿的谐振控制下得到的MATLAB仿真图一；

图5为本发明提供的实施例步骤S2中，在重力补偿的谐振控制下得到的MATLAB仿真图二；

图6为本发明提供的实施例步骤S3中，加入干扰观测器的双自由度柔性机器人关节系统总输入框图；

图7为本发明提供的实施例步骤S3中，结合解耦补偿与重力补偿的谐振控制下得到的仿真图一；

图8为本发明提供的实施例步骤S3中，结合解耦补偿与重力补偿的谐振控制下得到的仿真图二。

具体实施方式

下面结合附图和实施方式对本发明作进一步的详细说明：

一种基于谐振控制的双关节柔性机械臂力矩补偿方法，包括以下步骤：

S1、对双自由度串联弹性机器人进行动力学建模；忽略摩擦、外力等干扰项，双自由度串联弹性机器人结构框图，如图1所示，其电机侧动力学方程如下：

式中，τ_m、J_m、θ与q分别为电机总力矩、电机侧的转动惯量、电机侧角度与负载侧角度；K_f为柔性元件的刚度；

负载侧由于两个关节正转的方向相反，动力学方程为：

其中，

式中，J₁＝m₂l₁d₂，/>G₁＝m₁gd₁+m₂gl₁，G₂＝m₂gd₂；等号右侧第一项称作惯性项，第二项称作科氏力与离心力项，第三项为重力项。经过动力学参数辨识，选取双自由度串联弹性机器人的电机与负载侧动力学相关参数下表所示：

参数	数值
		Jm1	0.1814[kgm2]
Jm2	0.4383[kgm2]
		Kf1	126.0507[Nm/rad]
Kf2	63.0253[Nm/rad]
		J0	0.8067
J1	0.1921
		J2	0.1432
G1	19.3451
		G2	6.6429

S2、进行机器人关节的谐振控制，具体步骤如下：

对于基于串联弹性驱动器的机器人关节，将其等效为“电机-弹簧-负载”双质量系统，而谐振控制被用于实现双质量系统振动抑制，其原理是在被控的双质量系统输入端引入柔性元件力矩的反馈：

式中，τ_reac＝K(θ-q)为柔性元件力矩，K_r为柔性元件力矩反馈的增益；机器人关节的谐振控制需要确定K_p、K_v、K_r三个参数的取值，引入谐振频率 并令/>此时双质量系统传递函数将拥有四个相同的负实根；

对于双自由度柔性机器人，J_ai是第i个关节处的等效负载惯量；对负载侧动力学方程忽略科氏力与离心力项及重力项，并令 为一关节与二关节负载侧的等效惯量，易知J_a1与J_a2随q₂变化，将上表中J₀、J₁、J₂的具体数值代入并取q₂∈[0,π]，易知J_a1∈[0.4058,0.6635]，J_a2∈[0.0488,0.1432]，其随q₂变化的趋势，如图2所示。因此，增益K_p、K_v、K_r均随q₂变化，将上表中J₀、J₁、J₂的具体数值与机器人当前q₂值代入便能求出当前q₂下的K_p、K_v、K_r；

对于受重力影响的双自由度柔性机器人，为了消除重力带来的静差还应该加入重力补偿，假定q_cmd＝0，易知电机侧角度对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，那么在输入端有：

式中，G_c为待推导的重力补偿传递函数矩阵，上式最终推得：

其谐振控制框图如图3所示。

在实施例中，采用MATLAB软件进行仿真验证。在仿真中代入上表的参数，设定阶跃输入结合重力补偿的谐振控制下得到如图4和图5所示的仿真结果，两个关节的负载侧实际角度都经过了衰减振荡的过程才最终稳定在目标角度。

S3、从谐振控制出发推导解耦补偿，并以加入解耦补偿的谐振控制为基础推导相应的重力补偿，在无静差的前提下对比解耦补偿带来的效果，具体步骤如下：

A1、把系统总输入分为实现运动控制的输入与实现解耦的输入两部分，取参考加速度为机器人关节系统总输入，为实现两个关节各自的控制，它们的控制输入u_11ref、u_22ref分别加入补偿项u_1Cref、u_2Cref以抵消干扰，于是1关节的总输入u_1ref＝u_11ref+u_1Cref，2关节的总输入u_2ref＝u_22ref+u_2Cref；

A2、分析双关节柔性机械臂关于负载侧角度与扭转角的二次微分状态方程：

考虑加入干扰观测器的双自由度柔性机器人关节系统，有以下状态方程组：

式中，δ₁＝θ₁-q₁，δ₂＝θ₂-q₂；

A3、控制输入u_11ref与u_22ref分别受另一关节的扭转角δ的影响，因此它们又可以作进一步拆分，其中，u_11ref＝u_11Sref+u_11Dref，u_22ref＝u_22Sref+u_22Dref；u_11Sref、u_22Sref是通过本关节的状态计算的输入，u_11Dref、u_22Dref是通过另一个关节的状态计算的输入；由于θ₁＝q₁+δ₁，θ₂＝q₂+δ₂，由状态方程组知θ₁受δ₂影响，θ₂受δ₁影响，忽略q_cmd和q，并令代入谐振控制表达式便能求出u_11Dref与u_22Dref；

A4、对步骤A2的二次微分状态方程再次微分得到四次微分状态方程：

式中，等号右侧的第三项与第四项是待补偿的耦合相关项；令耦合相关项的总和为0并将状态方程组中与相关的方程代入，最终得到：

式中，

A5：使重力G(q)对系统输出q没有影响，假定q_cmd＝0，可知电机侧角度 对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，那么在输入端有：

式中，G_c为待推导的重力补偿传递函数矩阵，设定为2×2，u_c是2×1的解耦补偿输入，于是加入了干扰观测器的双自由度柔性机器人关节系统总输入为其框图如图6所示。

最后，本实施例对上述补偿方法进行仿真验证，在仿真中代入上表的参数，设定阶跃输入结合解耦补偿与重力补偿的谐振控制下得到了如图7和图8所示的仿真结果，两个关节的负载侧实际角度不再发生衰减振荡，最终稳定在目标角度；其中，一关节负载侧实际角度最大超调量由30.85％下降至6.1％，一关节负载侧实际角度不再超调。

以上所述的仅是本发明的实施例，方案中公知的具体技术方案或特性等常识在此未作过多描述。应当指出，对于本领域的技术人员来说，在不脱离本发明技术方案的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。

Claims (9)

1.一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，包括以下步骤：

S1、对双自由度串联弹性机器人进行动力学建模；

S2，进行机器人关节的谐振控制，将PD控制、柔性元件扭矩的反馈、干扰观测器结合，实现对机器人关节的位置控制同时，减小机器人连杆的振动；

S3、进行关节解耦补偿和对应的重力补偿，基于双关节柔性机械臂连杆侧动力学方程的二阶及四阶微分形式计算解耦补偿输入与重力补偿输入，实现减小机器人关节由耦合作用带来的抖动，克服由重力带来的连杆侧实际角度与指令角度间的误差。

2.根据权利要求1所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在S1中，动力学建模的过程中，忽略摩擦和外力干扰项，电机侧动力学方程为：

式中，τ_m、J_m、θ与q分别为电机总力矩、电机侧的转动惯量、电机侧角度与负载侧角度；K_f为柔性元件的刚度；

负载侧的两个关节正转的方向相反，其动力学方程为：

其中；

式中，J₁＝m₂l₁d₂，/>G₁＝m₁gd₁+m₂gl₁，G₂＝m₂gd₂；等号右侧第一项为惯性项，第二项为科氏力与离心力项，第三项为重力项。经过动力学参数辨识，选取双自由度串联弹性机器人的电机与负载侧动力学相关参数如下表所示：

参数 数值 J_m1 0.1814[kgm²] J_m2 0.4383[kgm²] K_f1 126.0507[Nm/rad] K_f2 63.0253[Nm/rad] J₀ 0.8067 J₁ 0.1921 J₂ 0.1432 G₁ 19.3451 G₂ 6.6429

3.根据权利要求2所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在S2中，将串联弹性驱动器的机器人关节等效为“电机-弹簧-负载”双质量系统，谐振控制用于实现双质量系统振动抑制，在被控的双质量系统输入端引入柔性元件力矩的反馈：

式中，τ_reac＝K(θ-q)为柔性元件力矩，K_r为柔性元件力矩反馈的增益；谐振频率令/>对于双自由度柔性机器人，J_ai是第i个关节处的等效负载惯量；

对负载侧动力学方程忽略科氏力与离心力项及重力项，令 为一关节与二关节负载侧的等效惯量，J_a1与J_a2随q₂变化，K_p、K_v、K_r也随q₂变化，利用上述表中的J₀、J₁、J₂值与机器人当前q₂值求出当前q₂下的K_p、K_v、K_r；

对于受重力影响的双自由度柔性机器人，为消除重力带来的静差，加入重力补偿，设定q_cmd＝0，则电机侧角度对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，在输入端有：

式中，G_c为待推导的重力补偿传递函数矩阵，整理得到：

。

4.根据权利要求2所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在S3中，进行关节解耦补偿和对应的重力补偿的方法，包括以下步骤：

A1、把系统总输入分为实现运动控制的输入与实现解耦的输入；

A2、分析双关节柔性机械臂关于负载侧角度与扭转角的二次微分状态方程；

A3、对实现运动控制的输入作进一步拆分，计算各关节受另一关节影响的输入成分；

A4、对步骤A2的二次微分状态方程再次微分得到四次微分状态方程，并令四次微分状态方程中的耦合相关项总和为0，得到双关节柔性机械臂各关节的解耦补偿输入；

A5、对竖直状态下的双关节柔性机械臂，求得解耦补偿输入后再加入重力补偿，消除重力带来的负载侧角度静差。

5.根据权利要求4所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在A1中，取参考加速度为机器人关节系统总输入， 为实现两个关节各自的控制，其控制输入u_11ref、u_22ref分别加入补偿项u_1Cref、u_2Cref以抵消干扰，于是1关节的总输入u_1ref＝u_11ref+u_1Cref，2关节的总输入u_2ref＝u_22ref+u_2Cref。

6.根据权利要求4所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在A2中，得到的二次微分状态方程为：

式中，δ₁＝θ₁-q₁，δ₂＝θ₂-q₂。

7.根据权利要求4所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在A3中，计算各关节受另一关节影响的输入成分的方法为：控制输入u_11ref与u_22ref分别受另一关节的扭转角δ的影响，其中u_11ref＝u_11Sref+u_11Dref，u_22ref＝u_22Sref+u_22Dref；u_11Sref、u_22Srefwei通过本关节的状态计算的输入，u_11Dref、u_22Dref为通过另一个关节的状态计算的输入；由于θ₁＝q₁+δ₁，θ₂＝q₂+δ₂，θ₁受δ₂影响，θ₂受δ₁影响，忽略q_cmd和q，令 代入谐振控制表达式计算出u_11Dref与u_22Dref。

8.根据权利要求4所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在A4中，得到的四次微分状态方程为：

式中，等号右侧的第三项与第四项是待补偿的耦合相关项，令耦合相关项的总和为0，并将状态方程组中与相关的方程代入，得到：

式中，

9.根据权利要求4所述的一种基于谐振控制的双关节柔性机械臂力矩补偿方法，其特征在于，在A5中，消除重力带来的负载侧角度静差的方法为：设定q_cmd＝0，电机侧角度对理想模型，干扰观测器的输出为扭矩K_f(θ-q)＝G，在输入端有：

式中，G_c是待推导的重力补偿传递函数矩阵，设定为2×2，u_c是2×1的解耦补偿输入，加入干扰观测器的双自由度柔性机器人关节系统总输入为：

。