CN113386124A - 绳驱柔性机械臂的闭环运动控制方法及系统 - Google Patents

Abstract

本发明涉及绳驱柔性机械臂的闭环运动控制方法和系统。其中方法包括：获取绳驱柔性机械臂当前时刻的期望目标构型；根据绳驱柔性机械臂运动学模型计算得到要形成的期望目标构型的期望绳长；根据电机编码器数据，计算当前的电机演算绳长；通过臂杆组件中的关节编码器数据，获取等效绳长；将电机演算绳长与期望绳长的差值进行绳长闭环控制的输入量，将输出的绳长控制量进一步转换为电机的旋转角度值，以作为电机位置闭环输入量；通过电机位置闭环控制的方式，根据电机位置闭环输入量，输出驱动电机的PWM波，以驱动电机带动绳索运动。其中的系统包括运动控制器以实施上述控制方法。本发明能够显著提升绳驱柔性机械臂的运动控制精度。

Description

绳驱柔性机械臂的闭环运动控制方法及系统

技术领域

本发明属于机器人或机械臂自动化控制领域，具体涉及一种绳驱柔性机械臂的闭环运动控制方法及系统。

背景技术

相对于传统机器人，绳驱柔性机械臂有着臂杆直径小、弯曲效果好、高自由度等优点。在复杂、多障碍的工作情况中，绳驱柔性机械臂还有着极强的动作灵活性，因此被广泛应用于核电、航天、搜救等狭小环境中的精细作业。这类作业任务通常要求机械臂具备良好的运动精度以满足抓取、焊接、探测等等功能。然而实际中，有以下因素制约着绳驱柔性机械臂的运动精度：

(1)驱动绳索与臂杆绳孔之间的间隙，以及驱动绳索弯曲曲率导致柔性臂的运动学模型误差；

(2)绳索经过绳孔产生较大摩擦力以及绳索自身驱动臂杆需要较大拉力，使得绳索出现弹性形变；

为提高绳驱柔性臂的运动精度，需要测量并补偿柔性机械臂在运动过程中绳索由于以上因素产生的绳长误差。然而在机械臂的实际工作情况中，实时测量实际绳索长度十分困难。

发明内容

本发明提供一种绳驱柔性机械臂的闭环运动控制方法及系统，旨在至少解决现有技术中存在的技术问题之一，例如提高机械臂的运动精度。

本发明的技术方案一方面涉及一种绳驱柔性机械臂的闭环运动控制方法。该方案所基于的硬件平台的机械臂包括驱动箱和串联的多段的臂杆组件，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块之间通过轴承转动地连接，所述的中心块与所述的臂杆之间的相对转角通过编码器测量；每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱并且由所述的驱动箱中的各组的电机和传动机构带动；所述的传动机构包括滚珠丝杆和由所述滚珠丝杆配合的螺母带动的牵绳机构，在每一组的电机和传动机构中所述电机的输出轴联接至所述的滚珠丝杆的输入端。在本发明的方案中，所述方法包括以下步骤：

S110、获取所述的绳驱柔性机械臂当前时刻的期望目标构型q_d；

S120、根据所述的绳驱柔性机械臂运动学模型计算得到要形成的期望目标构型q_d的期望绳长L_d；

S130、根据电机编码器数据，计算当前的电机演算绳长L_m；

S140、通过所述的臂杆组件中的关节编码器数据，获取等效绳长L_real；

S150、将所述的电机演算绳长L_m与所述的期望绳长L_d的差值进行绳长闭环控制的输入量，将输出的绳长控制量进一步转换为电机的旋转角度值，以作为电机位置闭环输入量；

S160、通过电机位置闭环控制的方式，根据所述的电机位置闭环输入量，输出驱动电机的PWM波，以驱动电机带动绳索运动。

进一步，在所述步骤S110中：所述的机械臂的构型包括分割为多个自由度的关节角度，以串联形成机械臂末端的轨迹构型q_tar；所述的关节角度为相邻的中心块与臂杆之间的绕轴承轴线的各个相对转角。

进一步，所述步骤S130包括：

通过电机旋转角度值Δθ_m、滚珠丝杠导程S和电机减速比n，计算电机演算绳长变化量为

进一步，所述步骤S140包括：获取当前绳驱柔性机械臂的实际构型q_real，并进一步根据运动学模型计算得到所述的机械臂的实际绳长L_real，以作为绳长闭环控制反馈量。

进一步，所述步骤S150包括：获取期望绳长L_d与实际绳长L_real的当前差值作为绳长误差，对所述绳长误差值作为一个输入量进行比例微分控制，以计算下一步的电机演算绳长。

进一步，所述步骤S160包括：在电机运动过程中通过自身的电机编码器实时记录电机旋转角度值，并使电机编码器采集值作为负反馈输入至电机位置控制系统，以实现比例积分微分控制。

进一步，在所述步骤S160后还包括：步骤S170、如果机械臂末端的轨迹构型q_tar没有达到期望目标构型q_d，则返回步骤S110。

进一步，所述的方法还包括：步骤S180、采用三次样条插值规划计算得机械臂的构型时间运动曲线，进而得到每一个控制周期的期望构型。

本发明的技术方案另一方面涉及一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施上述的方法。

本发明的技术方案又一方面涉及一种绳驱柔性机械臂的闭环运动控制系统，包括：集成所述绳驱柔性机械臂的运动学模型应用程序的运控控制器，该运控控制器包括上述的计算机可读存储介质。

本发明的有益效果如下。

提出完全新颖的绳驱柔性机械臂的双闭环运动控制方案，减少了因驱动绳索变形导致的误差，提高了机械臂的运动精度；此外，本发明的方案在原有的位置环控制的基础上加入一个绳长外闭环控制，就算出现电机内部的冲击与绳索通过绳孔运动时的迟滞现象，也能减少绳驱柔性机械臂在运动控制过程中产生振动。

附图说明

图1所示为绳驱柔性机械臂的机构模型图。

图2所示为图1的机械臂的任意臂杆组件中的中心块结构示意图。

图3所示为根据本发明实施例的方法总体流程图。

图4所示为根据本发明实施例的方法中的电机位置闭环控制框图。

图5所示为根据本发明实施例的方法中的绳驱双闭环的控制框图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。

此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。

参照图1和图2，根据本发明的方法和系统用在一种绳驱柔性机械臂(也称绳驱柔性机器人)上。所述机械臂包括驱动模块1、机械臂模块2和多条驱动绳索。其中机械臂模块由多个中心块201与臂杆202交替串联而成。中心块201则包括支撑块203、端盖204、关节编码器205、编码器安装盘206和连接件207。其中，关节编码器205固定在编码器安装盘206上，与支撑块203相对固定；连接件207连接编码器旋转轴与臂杆，使编码器旋转轴与臂杆202相对固定。一个中心块201可以同时连接两个上下臂杆，其中两个上下臂杆分别相对于该中心块201的旋转轴相交90°。由于臂杆202与中心块201相对旋转，关节编码器将记录关节角度的变化值。

然后，每个臂杆组件中的臂杆通过各自的驱动绳索连接至驱动箱并且由驱动箱中的各组的电机和传动机构带动。传动机构可以包括滚珠丝杆和配合的螺母，其中，在每一组的电机和传动机构中所述电机的输出轴联接至滚珠丝杆的输入端。例如，所有的驱动绳索分别从臂杆202底部穿过多个臂杆绳孔至所述绳索直接驱动的臂杆202的下端，电机的旋转运动则通过与滚珠丝杠平行的丝杠导轨101、由螺母带动的牵绳机构102转为绳索的直线运动，使得可以通过每组电机和传动机构驱动对应的绳索进行拉伸或放松来驱动对应的臂关节运动。其余关于绳驱柔性机械臂的细节描述可以参照本申请人在先的一个名称为“一种带末端力反馈的柔性机械臂”的公开号为CN111993400A的发明公布文件。考虑到节约原则，将该发明公布文件以全文引用的方式并入本文，这里就不重复描述机械臂的结构了。

基于上述的机械臂结构，下文提及的一些关键术语说明如下。

构型：在本文是指柔性机械臂的机构模型的统称，可以由机械臂关节的角度的矩阵表示。如图1的示例绳驱柔性机械臂有10个自由度，因此机械臂构型角度是一个10×1矩阵q，可以设为：q＝[q₁ q₂ q₃ q₄ q₅ q₆ q₇ q₈ q₉ q₁₀]^T。

驱动绳长：是指驱动箱中由电机和传动机构的组合所带动的绳索的一组长度伸长(或收缩)值。如果有15个电机、传动机构和绳索的组合，则驱动绳长可以设为一个15×1矩阵：L＝[L₁ L₂ L₃ L₄ L₅ L₆ L₇ L₈ L₉ L₁₀L₁₁ L₁₂ L₁₃ L₁₄ L₁₅]^T。

期望绳长：是指要达到目标构型时，所对应的驱动绳长。

电机演算绳长：是指根据电机编码器值和传动机构参数所计算的到的驱动绳长。

现在参照图3，在一些实施例中，根据本发明的绳驱柔性机械臂的闭环运动控制方法包括以下步骤：

S101、设置当前绳驱柔性机械臂的构型q_cur和目标轨迹构型q_tar；

S110、获取绳驱柔性机械臂当前时刻的期望构型q_d；

S120、根据绳驱柔性机械臂运动学模型计算得到要形成的期望目标构型q_d的期望绳长L_d；

S130、获取电机编码器的当前转角并进一步换算成当前的电机演算绳长L_m；

S140、获取当前绳驱柔性机械臂的实际构型q_real，并进一步根据运动学模型计算得到绳驱柔性机械臂的实际绳长L_real；

S150、将电机演算绳长L_m与上一步的期望绳长L_d差值作为输入量，通过外闭环输出绳长控制量，进一步转为电机(比如是伺服电机)的电机旋转角度量；

S160、通过电机位置闭环控制的方式，根据电机位置闭环输入量，输出驱动电机的PWM波，以驱动电机带动绳索运动；

S170、判断机械臂末端的轨迹构型q_tar是否达到期望目标构型q_d，如果是则结束本控制周期，否则执行步骤S180后返回步骤S110。

S180、采用三次样条插值规划计算得机械臂的构型时间运动曲线，进而得到每一个控制周期的期望构型。

此外，对于上述步骤S110，对某一时刻的期望构型的获取可以实施步骤S180，即是可以在已知当前机械臂构型与轨迹点构型的条件下，采用三次样条插值规划计算得机械臂的构型时间运动曲线，进而得到每一个控制周期的期望构型。假设机械臂设置的轨迹构型为：

t_tar＝[t₁ t₂ … t_n]^T

则自由边界的三次样条曲线方程组为：

其中，h_i＝q_i+1-q_i。

求解上述方程组后，得：

a_i＝q_i

因此在子区间t∈[t_i，t_i+1]中，插值后的构型时间运动曲线为：

q_d＝a_i+b_i(t_d-t_i)+c_i(t_d-t_i)²+d_i(t_d-t_i)³

其中，t_d为期望构型q_d的时刻。

对于，步骤S160，在具体的实施例中，上述电机位置闭环控制的过程中，在电机运动过程中通过编码器实时记录电机旋转角度值，并编码器采集值作为负反馈输入给电机位置控制系统。参照图4所示的控制框图，采用比例积分微分(PID)控制方法，输出PWM波至电机驱动器以控制电机运动，其中控制方程如下：

然后，根据上述机械臂的驱动箱的配置，电机在PID控制下的旋转运动则通过丝杠导轨101和牵绳机构202以转换为直线运动，实现对绳索的拉伸和缩回控制。

为了阐述本绳驱柔性机械臂闭环绳驱控制方法，假设所述示例柔性机械臂处于运动过程中，当前机械臂处于节点P2中，前一个节点为节点P1。在运动轨迹规划中，节点P1、P2的期望构型分别为q_d1、q_d2，电机演算绳长L_m1、L_m2，实际构型为q_reall、q_real2。

牵绳机构运动距离即为电机演算绳长，与电机位置的关系如下：

其中ΔL_m即电机演算绳长变化量，Δθ_m即电机旋转角度值，S为滚珠丝杠导程，n为电机减速比。

在所述绳驱柔性臂实际运动过程中，驱动绳索不仅需要受拉以驱动机械臂运动，还需要克服所述绳索与绳孔之间的间隙与滑动摩擦力，这导致驱动绳索发生弹性形变，绳驱柔性机械臂的实际构型q_real1不同与期望构型q_d。可以计算得，在节点P1中，绳驱柔性臂的绳长误差为：

L_error1＝cable(q_d1)-cable(q_r1)＝L_d1-L_r1

考虑上述绳驱柔性臂在节点P1的绳长误差，则下一步在节点P2的电机演算绳长应为节点P1电机演算绳长、节点P1绳长误差与节点P1、P2期望绳长之和，即：

L_m2＝L_m1+(L_q2-L_q1)+L_error1。

在绳驱柔性机械臂运动过程中，由于电机内部的冲击与绳索通过绳孔运动时的迟滞现象，会导致绳驱柔性机械臂在运动过程中产生振动。若采用单纯的补偿方法，根据上述公式计算下一轨迹节点L_m，则不仅无法避免机械臂在运动过程中的自身震动对q_real的影响，反而将臂自身的振动干扰带给了电机内闭环，导致振动更为严重。

为解决以上问题，本发明的方法结合步骤S150至S170，在所述位置环控制的基础上再开启一个绳长外闭环，参照图5的控制原理框图。将绳长误差量作为一个输入量进行比例微分(PD)控制，下一步的电机演算绳长则为：

上述电机演算绳长通过上述传动公式转为电机位置期望，作为电机位置控制闭环的输入量，实现对绳驱柔性机械臂的双闭环控制。

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像系统通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而己，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

Claims (10)

1.一种绳驱柔性机械臂的闭环运动控制方法，所述的机械臂包括驱动箱和串联的多段的臂杆组件，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块之间通过轴承转动地连接，所述的中心块与所述的臂杆之间的相对转角通过编码器测量，

每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱并且由所述的驱动箱中的各组的电机和传动机构带动，

其特征在于，所述方法包括以下步骤：

S110、获取所述的绳驱柔性机械臂当前时刻的期望目标构型(q_d)；

S120、根据所述的绳驱柔性机械臂运动学模型计算得到要形成的期望目标构型(q_d)的期望绳长(L_d)；

S130、根据电机编码器数据，计算当前的电机演算绳长(L_m)；

S140、通过所述的臂杆组件中的关节编码器数据，获取等效绳长(L_real)；

S150、将所述的电机演算绳长(L_m)与所述的期望绳长(L_d)的差值进行绳长闭环控制的输入量，将输出的绳长控制量进一步转换为电机的旋转角度值，以作为电机位置闭环输入量；

S160、通过电机位置闭环控制的方式，根据所述的电机位置闭环输入量，输出驱动电机的PWM波，以驱动电机带动绳索运动。

2.根据权利要求1所述的方法，其中，在所述步骤S110中：

所述的机械臂的构型包括分割为多个自由度的关节角度，以串联形成机械臂末端的轨迹构型(q_tar)；

所述的关节角度为相邻的中心块与臂杆之间的绕轴承轴线的各个相对转角。

3.根据权利要求1或2所述的方法，其中，所述的传动机构包括滚珠丝杆和由所述滚珠丝杆配合的螺母带动的牵绳机构，在每一组的电机和传动机构中所述电机的输出轴联接至所述的滚珠丝杆的输入端，使得所述步骤S130包括：

通过电机旋转角度值(Δθ_m)、滚珠丝杠导程(S)和电机减速比(n)，计算电机演算绳长变化量为

4.根据权利要求1所述的方法，其中，所述步骤S140包括：

获取当前绳驱柔性机械臂的实际构型q_real，并进一步根据运动学模型计算得到所述的机械臂的实际绳长(L_real)，以作为绳长闭环控制反馈量。

5.根据权利要求1或4所述的方法，其中，所述步骤S150包括：

获取期望绳长(L_d)与实际绳长(L_real)的当前差值作为绳长误差，对所述绳长误差值作为一个输入量进行比例微分控制，以计算下一步的电机演算绳长。

6.根据权利要求1所述的方法，其中，所述步骤S160包括：

在电机运动过程中通过自身的电机编码器实时记录电机旋转角度值，并使电机编码器采集值作为负反馈输入至电机位置控制系统，以实现比例积分微分控制。

7.根据权利要求1所述的方法，其中，在所述步骤S160后还包括：

S170、如果机械臂末端的轨迹构型(q_tar)没有达到期望目标构型(q_d)，则返回步骤S110。

8.根据权利要求7所述的方法，还包括：

S180、采用三次样条插值规划计算得机械臂的构型时间运动曲线，进而得到每一个控制周期的期望构型。

9.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。

10.一种绳驱柔性机械臂的闭环运动控制系统，包括：

集成所述绳驱柔性机械臂的运动学模型应用程序的运控控制器，该运控控制器包括根据权利要求9所述的计算机可读存储介质。

Images