一种腱驱动机械手位置和腱张力的混合控制方法及控制装置

摘要

本发明公开一种腱驱动机械手位置和腱张力的混合控制方法及控制装置，方法包括：进行机械手自由空间中的关节角位置路径规划，由手指指尖当前位置和物体期望接触点确定运动轨迹，设定期望关节角位置；由操作对象特性设定期望接触力及腱张力阈值；将腱张力值与阈值比较，小于力阈值时机械手位于自由空间，比较期望角位置和实际测量关节角位置之间偏差，采用位置控制律得到位置补偿量，把补偿量输入到腱驱动器进行控制；腱张力大于阈值时机械手位于约束空间，对规划的关节力矩进行腱张力分配，比较期望腱张力和实际腱张力差别，根据两者差值由张力控制器把力的误差转化为位置补偿量，把该补偿量与位置通道的位置补偿量求和后输入到腱驱动器进行控制。

说明书

技术领域：

本发明涉及一种腱驱动机械手位置和腱张力的混合控制方法及控制装置，其属于机 器人控制领域。

背景技术：

机械手为模仿手、臂的特定功能的一种自动机械，因此泛指机械臂、末端执行器、 灵巧手手指等多关节多连杆操作机构。腱驱动型机械手是利用腱绳进行传动的机械手， 允许驱动器放置于机械手结构体的外部，可以减小机械手体积和重量，从而提高了机械 手的灵巧性，同时也在驱动器选型方面给机构设计者提供了更多灵活性。

由于腱只可以传递张力，因此为了获得完全独立的自由度控制，必须要保证驱动器 的数量多于自由度的个数。有多种腱配置方式，在配置合理的情况下，N+1型腱能够独 立控制N个DOF，同时保证腱具有正张力。该腱配置方式简化了机构，但由于这种方 式在减少腱绳数量的同时引入了关节位置和腱绳的耦合问题，因此控制器的设计非常复 杂。在装配应用中机械手需要与非结构环境物理接触，因此机械手的力矩控制能力非常 重要。研究者们提出多种控制策略来解决耦合腱驱动机械手的力矩控制问题。这些控制 策略由张力分配算法和控制律两部分组成。张力分配算法是确定一组腱张力的过程，目 的是产生期望的一组关节力矩，同时解决驱动的冗余问题。该冗余可以生成张力的零空 间，用来保证所有腱具有正张力。

目前已有的控制律根据是否使用腱张力反馈进行分类。无腱张力反馈的控制方法包 括计算力矩法、其它能够预测或估计系统参数的智能方法，这些方法对于能够精确建模 运动和力矩关系的系统是可行的，但机械手需要抓握或操作的是各种可能的物体，其接 触表面特性差别很大，因此这些方法只能获得粗略的腱张力控制，经常产生更高的内张 力，导致腱的磨损，增加摩擦力降低性能，因此这些方法难以实现精确的操作。采用张 力反馈的腱空间控制器忽略腱的动力学，利用张力分配算法将期望的关节力矩转换为期 望的腱张力，然后为每根腱使用独立的张力调节器。例如Salisbury和Craig在Stanford/JPL 手上实现了腱空间控制律，Starr在Stanford/JPL手通过采用腱‐导管间歇模型实现了类似 算法，2N型POSTECH手和Utah/MIT手等灵巧手也采用了该算法。然而采用该种方式的 控制器在手指动力学中引入了瞬态耦合，即某个关节的控制或扰动可能导致另一个关节 的不期望的响应。

为了稳定可靠地抓握物体，腱驱动机械手的控制应能合理的调节关节的柔顺性和力 矩。相对于传统的齿轮传动，腱驱动的难点在于设计一个控制器使其能够按照期望的位 置运动同时保证腱的张力保持在期望的范围之内。腱只能传递张力，而且有一定的延迟， 以及机构对腱的摩擦以及腱的其它未建模动力学等因素也都影响着腱驱动机械手操作 控制算法的设计。总的来说，现有的控制算法，仍不能完全满足机械手灵巧操作和强力 抓持操作的要求。

发明内容：

本发明的目的：针对腱驱动机械手，提供一种力和位置控制方法和设备，该方法能 够减小腱运动和关节运动之间耦合的影响，提高系统的动态特性和位置精度，减小从自 由空间到约束空间的冲击，有效的实现腱驱动机械手的操作控制，提高机械手的灵巧性。

本发明采用如下技术方案：一种腱驱动机械手位置和腱张力的混合控制方法，其包 括如下步骤：

步骤1，由路径规划模块进行机械手自由空间中的关节角位置路径规划，根据手指 指尖当前位置和物体的期望接触点确定从初始点到接触点的运动轨迹，并且设定期望关 节角位置；根据操作对象特性设定期望接触力，并且设置腱张力阈值；

步骤2，根据腱张力传感器测量得到的腱张力值与腱张力阈值比较确定灵巧手和物 体之间的接触状态，若腱张力值小于腱张力阈值说明机械手位于自由空间，此时由位置 比较模块比较期望关节角位置和实际测量关节角位置之间的偏差，并且由位置解算模块 解算为腱的长度偏差，由一个位置控制模块通过合适的控制律得到腱位置偏差，把该偏 差指令输送给键驱动器进行控制；

步骤3，若腱张力值大于阈值说明机械手位于约束空间，此时首先由接触力到关节 力矩转换模块通过力雅可比矩阵把期望的接触力转换为关节力矩；进一步由腱张力分配 模块转换为腱空间的力，同时进行腱张力分配；然后由腱张力比较器比较期望腱张力和 实际腱张力的差别，根据这个偏差由张力控制模块基于合适的控制律把张力的偏差转化 为位置的偏差，把该偏差与位置控制通道的位置偏差求和后输送给驱动器进行控制。

本发明还采用如下技术方案：一种腱驱动机械手位置和腱张力的混合控制装置，其 包括控制器、与控制器相连的腱驱动器、与腱驱动器相连的腱绳、安装于腱绳上的腱张 力传感器、腱驱动机械手单指机构及关节角位置传感器，所述腱张力传感器和关节角位 置传感器与控制器相连进而将采集的信息发送给控制器，所述控制器发送控制指令给腱 驱动器，所述控制器包括路径规划模块、期望接触力模块、位置解算模块、位置控制模 块、位置比较模块、接触力到关节力矩转换模块、腱张力分配模块及张力控制模块，路 径规划模块输入手指指尖当前位置和物体的期望接触点，路径规划模块输出期望关节角 位置到位置比较模块和腱张力分配模块，位置比较模块的另一个输入为实际测量关节角 位置，位置比较模块输出位置偏差到位置结算模块，位置解算模块输出到位置控制模块， 期望接触力输入到接触力到关节力矩转换模块，转换结果输出到转换腱张力分配模块， 腱张力模块输出的结果和腱张力传感器测量的结果进行比较，结果输出到张力控制模 块，张力控制的输出和位置控制的输出求和然后输出到腱驱动控制器，控制机构动作。

所述腱驱动机械手单指机构包括侧摆关节、基关节、中关节、连杆机构、基座、近 指节、中指节及远指节，所述基座与近指节通过基关节转动连接，近指节与中指节通过 中关节转动连接，远指节通过连杆机构与中指节耦合，所述腱绳的另一端依次穿过基座、 近指节、中指节、远指节并与远指节固接，所述腱张力传感器位于键绳上的键驱动器和 腱驱动机械手单指机构中间的位置上，所述关节角位置传感器固定在侧摆关节、基关节 及中关节上。

本发明具有如下有益效果：本发明设计了一种基于腱张力反馈的腱驱动机械手操作 控制的方法，该方法可有效减轻机械手接触物体时的冲击，并且能够实现稳定的抓取。 该方法可进一步应用于以腱为传动方式的多关节机械臂、灵巧手单指等多关节多连杆机 构中，因此该实现方法具有广阔的应用前景。

附图说明：

图1为腱驱动机械手单指系统组成图。

图2为腱驱动机械手单指位置和腱张力混合控制框图。

图3为腱绳运动状态简化图。

其中：

1-控制器；2-腱驱动器；3-腱绳；4-腱张力传感器；5-腱驱动机械手单指机构；6- 关节角位置传感器；11-侧摆关节；12-基关节；13-中关节；14-连杆机构；15-基座；16- 近指节；17-中指节；18-远指节。

具体实施方式：

机械手泛指机械臂、末端执行器、灵巧手手指等多关节多连杆操作机构，这儿主要 采用了基于腱驱动的灵巧手单手指的柔顺控制为例结合附图说明腱驱动机械手位置和 腱张力的混合控制装置及其控制方法的具体实施方法。但并不因此将本发明限制在实施 范围之内。

如图1所示，腱驱动机械手单指系统主要由控制器1、与控制器1相连的腱驱动器2、 与腱驱动器2相连的腱绳3、安装于腱绳3上的腱张力传感器4、腱驱动机械手单指机构5 及关节角位置传感器6组成。其中腱驱动机械手单指机构5包括侧摆关节11、基关节12、 中关节13、连杆机构14、基座15、近指节16、中指节17及远指节18。其中基座15与近指 节16通过基关节12转动连接，近指节16与中指节17通过中关节13转动连接，远指节18通 过连杆机构14与中指节17耦合。其通过采用“N+1”型腱配置方式，具有3个独立自由度， 其主要实现绕侧摆关节11、基关节12和中关节13的旋转运动。其中腱绳3共包括有四根， 且腱绳3的另一端均依次穿过基座15、近指节16、中指节17、远指节18并与远指节18固 接。在每一根键绳3上位于键驱动器2和腱驱动机械手单指机构5中间的位置上均装有腱 张力传感器4，其中关节角位置传感器6固定在侧摆关节11、基关节12及中关节13上。腱 张力传感器4和关节角位置传感器6与控制器1相连进而将采集的信息发送给控制器1，控 制器1发送控制指令给腱驱动器2。

如图2所示，腱驱动机械手单指系统的控制器1包括路径规划模块、期望接触力模 块、位置解算模块、位置控制模块、位置比较模块、接触力到关节力矩转换模块、腱张 力分配模块、张力控制模块。其中控制器接收来自关节角位置传感器、腱张力传感器的 信息和期望的位置和接触力信息，然后控制器各模块按照后面所提步骤的调度运行，通 过操作处理把位置控制模块和张力控制模块的输出求和后作为腱位置偏差输入到腱驱 动器。腱驱动器包括无刷直流电机和把旋转运动转换为直线运动的滚珠丝杠、螺母等组 件，根据控制器输入的位置偏差驱动腱绳拉伸，导致腱驱动机械手单指机构相应运动， 实现所需的位置和力的调整。

图2控制器中路径规划模块为机械手单指的运动提供期望关节角位置θ_d；位置解算 模块实现关节空间到腱空间的转换运算；位置控制模块对机械手单指的腱位置进行修 正，输出量ΔX作为腱端的位置偏差输入到腱驱动器中；腱驱动器有4个输入端，为位 置控制器输出的腱端位置偏差量v，控制器的输入量为4个关节角位置传感器给出的角 位移量θ_b和腱张力传感器张力值f_b。对于当前的腱驱动机械手单指系统，变量θ_d、θ_b、 θ_r、θ_e都是三维的列向量，f_b、f_d、ΔX都是四维的列向量。

因为腱只能传递张力，因此单独的位置控制很难保证腱张力时刻大于0，而且腱本 身有一定的柔韧性，对力的传递有迟滞性。控制器在初始时刻时，给腱绳施加力f_i≥f_min(其中f_i为4根腱的张力，f_min主要为保证腱在初始时刻具有预张紧。)保证腱在初始时 刻具有预张紧，这时腱可以等效为刚体。控制器各模块按照如下具体步骤运行。

步骤1，由路径规划模块进行机械手单指自由空间中的关节角位置路径规划，根据 手指指尖当前位置和物体的期望接触点确定从初始点到接触点的运动轨迹，并且设定期 望关节角位置；根据操作对象特性设定期望接触力，并且设定腱张力阈值。

步骤2，根据腱张力传感器测量得到的腱张力值与腱张力阈值比较确定灵巧手和物 体之间的接触状态，若腱张力值小于力阈值说明机械手位于自由空间，此时由位置比较 模块比较期望关节角位置和实际测量关节角位置之间的偏差，并且由位置解算模块解算 为腱的长度偏差，由一个位置控制模块通过合适的控制得到腱位置偏差，把该偏差指令 输送给键驱动器进行控制；

步骤3，若腱张力值大于阈值说明机械手位于约束空间，此时首先由接触力到关节 力矩转换模块通过力雅可比矩阵把期望的接触力转换为关节力矩；进一步由腱张力分配 模块转换为腱空间的力，同时进行腱张力分配；然后由腱张力比较器比较期望腱张力和 实际腱张力的差别，根据这个偏差由张力控制模块基于合适的控制律把张力的偏差转化 为位置的偏差，把该偏差与位置控制通道的位置偏差求和后输送给驱动器进行控制。

控制器各模块的实现如下：

1)路径规划模块

机械手手指运动路径规划是指根据手指指尖当前位置和物体的期望接触点确定从 初始点到接触点的运动轨迹。目前路径规划主要分为：笛卡尔空间的路径规划和关节空 间的路径规划。对于多关节的手指，指尖的笛卡尔空间运动轨迹很难用时间的变量表示， 并且计算比较复杂。相比而言，在关节空间，除了耦合关节外，其他的关节的角位移都 是相对独立的，可以分别进行路径规划，规划的方程相对简单而且计算量小、实时性好， 所以这里采用了关节空间的路径规划。机械手自由空间中的关节角位置路径规划可采用 一般多关节机械臂的关节角位置路径规划类似的方法进行，如抛物线过渡的线性插值方 法、三次多项式路径规划、加加速度有界路径规划等等。

为了使灵巧手手指能够顺利的到达目的位置，要求规划路径的导数，即速度，有一 个增加、不变、递减的过程。本实施方案采用抛物线过渡的线性插值方法，该方法具有 运动特性好，易于实现等优点。规划的方程为：

${\theta }_d\left(t\right)=\left(\begin{array}{c}{\theta }_{d0}+\frac{1}{2}{\mathrm{at}}^2,0\le t<t_b\\ {\theta }_{\mathrm{db}}+{\mathrm{at}}_b(t-t_b),t_b\le t\le t_f-t_b\\ {\theta }_{\mathrm{df}}-\frac{1}{2}a{(t_f-t)}^2,t_f-t_b<t\le t_f\end{array}\right)---\left(1\right)$

式中θ_d0是关节的初始角位移，θ_df是关节最后到达的期望角位移，t_f是到达期望位 置的时间，a是加速度。在实际中，先估计待抓物体的笛卡尔位置，然后通过逆运动 学解出各关节所要到达的角位移，最后根据式(1)对手指各关节进行路径规划。

2)位置解算模块

位置解算模块的输入量为：

θ_e＝θ_d-θ_b            (2)

其中θ_d代表规划的期望关节角位置，θ_b为关节角位置传感器反馈的关节角位置。

因该灵巧手采用“N+1”型腱驱动，其位置控制有别于传统的齿轮传动结构，4根 腱的位置对应3个独立关节，本实施方案采用几何建模的方法实现关节角位置到腱位置 的解算，即在各关节处对腱的运动路径做几何简化，得到每根腱的位置与关节角位置的 关系。

对于如图3所示的连杆由虚线位置转到实线位置，关节转动Δθ角，腱绳x₃由虚线 位置运动到实线位置，关节处对应的腱绳长度由l₁变为l₂，根据余弦定理可知：

$\left(\begin{array}{c}{l}_1=\sqrt{{r_1}^2+{r_2}^2-2r_1{r}_2\cos (\alpha +\theta )}\\ l_2=\sqrt{{r_1}^2+{r_2}^2-2r_1{r}_2\cos \left(\alpha \right)}\end{array}\right)---\left(3\right)$

则Δx₃＝Δl＝l₂-l₁。

其它三根腱采用同样的方法，这样就得到了关节角位置到四根腱的位置的转换方 程。

3)位置控制模块

位置控制模块主要是通过设计合适的控制器以满足待定的位置精度要求。控制器可 以采用不同的控制算法(如滑模变结构、PID、神经网络、模糊以及它们的组合算法等) 构建。其中，PID控制具有原理简单、易于实现，鲁棒性强和适应范围广等特点，是一 种应用广泛的控制规律。

得到腱位移的Δx(Δθ)后，为了改善位置控制的调节时间，增加系统阻尼，减少超 调量，本实施方案采用了一个PD控制器，根据公式(3)最终得到位置控制律表达式如 下：

${\mathrm{\Delta x}}_{i1}=K_{p1}\mathrm{\Delta x}({\theta }_d-{\theta }_b)+K_{d1}\frac{\mathrm{d\Delta x}({\theta }_d-{\theta }_b)}{\mathrm{dt}}---\left(4\right)$

K_p1为PD控制器增益，K_d1为PD控制器微分项增益。

前述控制算法滑模变结构、PID、神经网络、模糊以及它们的组合算法等也可实现 类似的控制功能，这里不再详细描述。

4)接触力到关节力矩转换模块

在腱驱动灵巧手系统中，腱之间存在耦合，为了避免解耦的麻烦，在关节空间规划 期望的关节力矩，然后转换成腱空间的张力。

根据手指静力学特性，事先规划出期望的接触力F_d，即可通过雅可比矩阵J反解出 使手指指尖在接触到物体并保持静态平衡时所需的关节力矩τ_d：

τ_d＝J^TF_d             (5)

5)腱张力分配模块

由于腱只传递张力，为了避免腱在运动过程中出现松弛，引入腱张力控制器确保腱 张力的正定性。腱张力控制器算法的基础是n个关节力矩τ和n+1个腱张力f之间的关 系：

$\left(\begin{array}{cc}\left(\begin{array}{c}\tau \\ t\end{array}\right)=\mathrm{Pf}& P=\left(\begin{array}{c}\mathrm{RB}\\ W\end{array}\right)\end{array}\right)---\left(6\right)$

其中t表示腱的内部张力；R∈R^3*3是由三个独立关节的半径组成的对角阵；B表示 腱的结构矩阵；W^T正交于RB的行空间且正定。

系统中腱张力可控的充要条件为P可逆，要求RB行满秩。由式(6)可以得到：

$f=P^{-1}\left(\begin{array}{c}\tau \\ t\end{array}\right)---\left(7\right)$

令P^-1＝[A a]

$f=\left(\begin{array}{cc}A& a\end{array}\right)\left(\begin{array}{c}\tau \\ t\end{array}\right)=\mathrm{A\tau }+\mathrm{at}---\left(8\right)$

设定腱张力取值范围[f_min f_max]，f_min保证腱始终处于绷紧的状态，f_max保护腱不超 负荷。令A_i和a_i分别代表A的行向量和a的列向量。腱张力分配算法的步骤如下。

a)首先根据f_min确定t的取值，则有：

f_i＝A_iτ+a_it≥f_min               (9)

$t=\max \frac{f_{\min }-A_i\tau }{a_i}---\left(10\right)$

b)其次将内部张力值t代入张力分配等式(8)；

c)令f_l为张力中最小的元素，f_h为张力中的最大元素；

d)确定是否张力f₁,...,f_n超过上界f_max，如果f_h≤f_max,表明没有超界，给各根腱分 配张力f₁,...,f_n+1并结束分配过程。

e)如果出现f_h＞f_max，则采用比例缩放的方式构造一个新的方程：

$f=\left(\begin{array}{cc}A& a\end{array}\right)\left(\begin{array}{c}\mathrm{\alpha \tau }\\ t\end{array}\right)---\left(11\right)$

由前述过程中得到的f_l和f_h及其对应位置处相关参量计算得到式(11)中参量：

d＝(a_hA_l-a_lA_h)τ

$\alpha =\frac{a_h{f}_{\min }-a_l{f}_{\max }}{d}$

$t=\frac{f_{\max }{A}_l-f_{\min }{A}_h}{d}\tau$

f)令f_l为利用式(11)所计算出的张力最小的元素，f_h为其中的张力最大元素，

g)若f_h≤f_max,并且f_l>f_min，给各根腱分配张力f₁,...,f_n+1，并结束分配过程。否则， 重新进行e)、f)、g)过程，直到满足要求为止。

最终综合公式(5)～(11)可得腱张力分配算法表达如下：

$f_d=P^{-1}\left(\begin{array}{c}\alpha J^T{F}_d\\ t\end{array}\right)---\left(12\right)$

6)张力控制模块

最终得到腱张力控制模块系统表达式如下：

${\mathrm{\Delta x}}_{i2}=K_{p2}(f_d-f_b)+K_{d2}\frac{d(f_d-f_b)}{\mathrm{dt}}---\left(13\right)$

K_p2为力控制PD控制器增益，K_d2为力控制PD控制器微分项增益。

公式(4)和(13)相加得到机械手单指位置/腱张力混合控制系统控制律：

$\left(\begin{array}{c}{\mathrm{\Delta x}}_i={\mathrm{\Delta x}}_{i1}+{\mathrm{\Delta x}}_{i2}\\ =(K_{p1}\mathrm{\Delta x}({\theta }_d-{\theta }_b)+K_{d1}\frac{\mathrm{d\Delta x}({\theta }_d-{\theta }_b)}{\mathrm{dt}})+(K_{p2}(f_d-f_b)+K_{d2}\frac{d(f_d-f_b)}{\mathrm{dt}})\end{array}\right)---\left(14\right)$

通过合适的机构参数，可以确定相应的映射矩阵；根据具体的性能指标要求，可以 确定相应的控制器参数。下面是具体实施实例一组相关参数设置。

根据如图2中所示腱的排列可以得出结构矩阵B，进而得到腱映射矩阵P，如下：

$\left(\begin{array}{cc}B=\left(\begin{array}{cccc}-1& -1& 1& 1\\ 1& -1& 1& -1\\ 0& 0& 1& -1\end{array}\right)& P=\left(\begin{array}{cccc}-0.06& -0.06& 0.06& 0.06\\ 0.04& -0.04& 0.04& -0.04\\ 0.5& 0.5& 0.5& 0.5\end{array}\right)\end{array}\right)$

同时根据任务要求及腱的性能设置腱张力范围f_min＝8N，f_max＝200N。

另外根据系统参数，腱长和各关节之间的关系如下：

$\left(\begin{array}{c}{\mathrm{\Delta x}}_1=13.9-\sqrt{129.01-128.4\cos (119.99-{\theta }_2)1}-{4.72\theta }_1\\ {\mathrm{\Delta x}}_2=-{4\theta }_2-{4.72\theta }_1\\ {\mathrm{\Delta x}}_3=14.9-\sqrt{143.41-142.78\cos (123.98-{\theta }_3)}+\\ 13.5-\sqrt{123.25-123.07\cos (118.65-{\theta }_2)}+{4.72\theta }_1\\ {\mathrm{\Delta x}}_4=-{4\theta }_3-{4\theta }_2-4.72{\theta }_1\end{array}\right)$

具体腱驱动机械手系统的控制器采用了通用PC、数据采集卡结合电机驱动器来实 现，另外以嵌入式处理器、DSP等为运算核心结合信号调理和电机驱动的控制器结构形 式也是可行的。

根据上述结构参数，进行了位置控制器和张力控制器的PD参数调节。采用该方法 所实现系统获得了良好的性能，能够有效的解耦腱运动和关节运动，减小从自由空间到 约束空间的冲击，有效的实现腱驱动机械手的操作控制，提高机械手的灵巧性。该方法 可进一步应用于以腱为传动方式的多关节操作臂、末端执行器等机构中，因此该实现方 法和装置具有广阔的应用前景。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员 来说，在不脱离本发明原理的前提下还可以作出若干改进，这些改进也应视为本发明的 保护范围。