一种腱驱动机械手张力约束末端操作控制方法

摘要

本发明公开了一种腱驱动机械手张力约束末端操作控制方法，包括步骤为：机械手单指指尖参数设置、计算指尖接触力误差、计算期望指尖笛卡尔空间位置、计算关节角位置误差、计算最佳期望关节力矩、计算期望腱张力、计算腱张力偏差、然后将腱张力偏差转化为腱位置偏差，接着，张力控制模块把腱位置偏差输送给腱驱动器，腱驱动器控制腱绳的拉伸。本发明响应速度快，关节角测量值与期望值之间的偏差能够消除，从而能够有效提高系统末端操作的力和位置控制的精度，减小机械手接触物体时的冲击，通过约束腱张力，降低腱的磨损，提高其寿命，最终能够有效地实现腱驱动机械手的操作控制，提高机械手的灵巧性和寿命。

说明书

技术领域

本发明涉及腱驱动机械手控制领域，特别是一种腱驱动机械手张力约束末端操作控制方法。

背景技术

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

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

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

腱只能传递张力且具有一定的传输延迟，机构与腱之间的摩擦以及腱的其它未建模动力学等因素也影响着腱驱动机械手操作控制算法的设计。

另外，现有控制算法主要采用阻抗控制算法，响应速度慢。加之，现有的关节角测量值和给定值之间存在偏差，而且这个偏差不能消除。因而，无法完全满足腱驱动机械手在精确控制接触力和接触位置的同时保持腱张力处于合适约束范围的要求，从而导致无法很好地满足灵巧操作和强力抓持操作的需求。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，而提供一种腱驱动机械手张力约束末端操作阻抗控制装置，该腱驱动机械手张力约束末端操作阻抗控制装置响应速度快，关节角测量值与期望值之间的偏差能够消除，从而能够有效提高系统末端操作的力和位置控制的精度，减小机械手接触物体时的冲击，通过约束腱张力，降低腱的磨损，提高其寿命，最终能够有效地实现腱驱动机械手的操作控制，提高机械手的灵巧性和寿命。

为解决上述技术问题，本发明采用的技术方案是：

一种腱驱动机械手张力约束末端操作阻抗控制方法，包括如下步骤。

步骤1，机械手单指指尖参数设置：设定机械手单指运动的期望指尖接触力F_d和期望指尖笛卡尔位置X_d；同时，同时，预先设定期望腱张力f的取值范围[f_min，f_max]；其中，f_min为保证腱绳始终处于绷紧状态时的最小腱张力，f_max为保护腱绳不超过负荷时的最大腱张力。

步骤2，计算指尖接触力误差F_e：指尖接触力误差F_e的计算公式为：

F_e＝F_d-F_a(1)

式中，F_a为指尖接触力传感器测量到的指尖接触力值，也即实测指尖接触力值。

步骤3，计算期望指尖笛卡尔空间位置X_r，具体计算方法如下。

步骤31，计算笛卡尔空间位置补偿值X_f：阻抗控制模块采用如下公式将指尖接触力误差F_e转换为笛卡尔空间位置补偿值X_f。

式中，M_d、B_d、K_d分别为阻抗控制模块中的阻抗控制器处于临界阻尼或过阻尼状态时的目标惯性矩阵、阻尼矩阵和刚度矩阵；s为复变量；

步骤32，计算期望指尖笛卡尔空间位置X_r：对期望指尖笛卡尔位置X_d进行补偿与计算，得到期望指尖笛卡尔空间位置X_r，具体计算公式为：

X_r＝X_d+X_f(3)

步骤4，计算关节角位置误差θ_e，具体计算方法如下。

步骤41，计算期望关节角位置θ_r，具体包括如下步骤。

步骤411，逆运动学解算模块转换：将步骤3计算的期望指尖笛卡尔空间位置X_r，通过逆运动学解算模块转换为期望关节角位置θ_r；其中，期望指尖笛卡尔空间位置X_r在手指基坐标系中的笛卡尔位置坐标为(x_T,y_T,z_T)，逆运动学解算模块按照如下公式(4)进行转换。

式中，L₁、L₂、L₃、L₄分别表示手指中基关节、近指节、中指节和远指节的长度；θ₁、θ₂、θ₃、θ₄分别为侧摆关节角、基关节角、中关节角和顶关节角；s_i＝sinθ_i，c_i＝cosθ_i，s_ij＝sin(θ_i+θ_j)，c_ij＝cos(θ_i+θ_j)，s_ijk＝sin(θ_i+θ_j+θ_k)，c_ijk＝cos(θ_i+θ_j+θ_k)。

步骤412，计算θ₁：由公式(4)，能直接计算出θ₁。

步骤413，计算θ₃和θ₄：顶关节通过四连杆机构与中关节相连，根据四连杆机构原理，以及如下公式(8)和(10)，并利用非线性函数数值解法求解出θ₃和θ₄。

假设顶关节与中关节相连的四连杆机构的四个角点分别为ABCD，其中，A点表示中关节，D点表示顶关节，B点为近指节上的一个固定点，C点为远指节上的一个固定点；则公式(8)和(10)中，l_m表示四连杆机构中AD长度，也即中指节长度；l_g表示四连杆机构中BC长度；r₁表示四连杆机构中AB长度；r₂表示四连杆机构中CD长度；l表示四连杆机构中BD长度。

步骤414，计算θ₂：根据求出的θ₃和θ₄，再利用公式(4)求解θ₂。

步骤42，计算关节角位置误差θ_e：关节角位置误差θ_e按如下公式(11)计算。

θ_e＝θ_r-θ_a(11)

式中，θ_a为四个关节角位置传感器分别测量得到的侧摆关节角、基关节角、中关节角和顶关节角。

步骤5，计算最佳期望关节力矩τ：最佳期望关节力矩τ的计算方法如下：

步骤51，计算期望关节力矩τ_d：采用雅克比矩阵J，实现期望指尖接触力F_d到期望关节力矩τ_d的转换，该期望关节力矩τ_d能使手指指尖接触物体且保持静态平衡；期望关节力矩τ_d的计算公式如下：

τ_d＝J^TF_d(12)

其中，雅克比矩阵J的计算公式如下：

步骤52，计算补偿关节力矩τ_e：采用如下公式(14)，将步骤4计算出的关节角位置误差θ_e转化为补偿关节力矩τ_e，实现关节力矩的补偿：

τ_e＝K(θ_r-θ_a)(14)

其中，K为比例控制系数，根据实际应用进行调节。

步骤53，计算最佳期望关节力矩τ：按如下公式(15)，计算出最佳期望关节力矩τ；

τ＝τ_d+τ_e(15)

步骤6，计算期望腱张力f_d：腱张力分配模块将步骤5计算出的最佳期望关节力矩τ，转换为期望腱张力f_d。

步骤7，计算腱张力偏差f_e：腱张力偏差f_e的计算公式为：

f_e＝f_d-f_a(22)

式中，f_a为实际腱张力，由各个腱张力传感器测量所得到。

步骤8，腱张力偏差控制：张力控制模块先按照如下公式(23)，将步骤7计算出的腱张力偏差f_e转化为腱位置偏差△x；

式中，K_p、K_d和f_d各代表比例系数、微分系数和腱张力期望值。

然后，张力控制模块把腱位置偏差△x输送给腱驱动器，腱驱动器控制腱绳的拉伸。

步骤6中，腱张力分配模块将最佳期望关节力矩τ转换为期望腱张力f_d的转换方法如下。

步骤61，根据f_min确定t的值，使其满足如下公式(17)：

式中，t为腱内部张力值；f_min为保证腱绳始终处于绷紧状态时的最小腱张力；A_i和a_i分别表示A的行向量和a的列元素；通过将步骤5计算出的最佳期望关节力矩τ代入公式(17)中，即可确定腱内部张力值t。

步骤62，将步骤61确定的腱内部张力值t及步骤5计算出的最佳期望关节力矩τ，代入如下的张力分配等式(16)，得到4维张力列向量f，且

f＝R⁺τ+W^Tt＝Aτ+at(16)

式中，R为腱映射矩阵，W^T正交于R的行空间且正定；和a为预先计算出来的常值，A＝R⁺,a＝W⁺。

步骤63，令f_l为张力最小元素，f_h为张力最大元素，将步骤62中4维张力列向量f的四个元素f₁、f₂、f₃和f₄，通过逐元素比较法，得出张力最小元素f_l和张力最大元素f_h。

步骤64，判断张力最大元素f_h是否超过上界f_max，如果f_h≤f_max，表明没有超界，则步骤62中4维张力列向量f即为期望腱张力f_d，将4维张力列向量f中的四个元素f₁、f₂、f₃和f₄依次分配给对应的四根腱绳，并结束张力分配过程。

步骤65，如果f_h>f_max，则采用比例缩放方式构造如下新方程，得到新的4维张力列向量f：

将步骤63计算得到的f_l和f_h及其对应的相关参量A_l、a_l和A_h、a_h，代入式(18)中，得到参量α和t′。

其中，d＝a_hf_l-a_lf_h(20)

公式(21)中的t为步骤61确定的腱内部张力值；然后，令t＝t′，此时，t为更新后的腱内部张力。

步骤66，将步骤65中新的4维张力列向量f中的四个元素，通过逐元素比较法，得出新的张力最小元素f_l和新的张力最大元素f_h。

步骤67，若新的张力最大元素f_h≤f_max且新的张力最小元素f_l≥f_min，则步骤65中新的4维张力列向量f即为期望腱张力f_d，将新的4维张力列向量f中的四个元素依次分配给对应的四根腱绳，并结束张力分配过程；否则从步骤65开始进行迭代处理。

步骤4中，L₁＝9mm、L₂＝45mm、L₃＝30mm、L₄＝21mm。

步骤4中，l_m＝30mm，l_g＝26.7mm，r₁＝4mm，r₂＝6.2mm，γ＝1.52rad。

本发明具有如下效果：

1.通过结合接触力偏差的阻抗控制和期望指尖接触力到关节力矩的直接转换，减轻了机械手接触物体时的冲击，同时加快了张力跟踪和接触力响应速度。

2.经过阻抗控制的位置补偿，相当于在手指末端加了弹簧，能提高手指在操作目标物体时的柔顺性，保护接触力不会剧烈变化而损坏手指。

3.通过约束腱张力，降低了腱的磨损，提高了系统的寿命。

4.能应用于以腱为传动方式的多关节机械臂、灵巧手单指等多关节多连杆机构中，因此该实现方法具有广阔的应用前景。

附图说明

图1显示了本发明一种腱驱动机械手张力约束末端操作阻抗控制装置的结构示意图。

图2显示了本发明一种腱驱动机械手张力约束末端操作阻抗控制方法的原理框图。

图3显示了单手指的坐标系图。

图4显示了顶关节与中关节相连的四连杆机构的结构示意图。

图5显示了顶关节与中关节相连的四连杆机构的原理示意图。

其中有：1.控制器；2.腱驱动器；3.腱绳；4.腱张力传感器；5.机械手单指；6.关节角位置传感器；7.指尖接触力传感器；11.侧摆关节；12.基关节；13.中关节；14.连杆；15.基座；16.近指节；17.中指节；18.远指节；19.顶关节。

具体实施方式

下面结合附图和具体较佳实施方式对本发明作进一步详细的说明。

如图1所示，一种腱驱动机械手张力约束末端操作阻抗控制装置，包括控制器1、腱驱动器2、腱绳3、腱张力传感器4、机械手单指5、关节角位置传感器6和指尖接触力传感器7。

如图1和图3所示，机械手单指均包括基座15、近指节16、中指节17和远指节18，基座通过侧摆关节11和基关节12与近指节相连接，近指节通过中关节13与中指节相连接，中指节通过顶关节19与远指节相连接，中关节与顶关节之间通过连杆14相耦合。

中关节与顶关节之间优选通过两根连杆相耦合，其中一根连杆分别与中关节的顶端和顶关节的底端相铰接，另一根连杆分别与中关节的底端和顶关节的顶端相铰接，两根连杆、中关节和顶关节相组合构成一个如图4所示的四连杆机构。

假设顶关节与中关节相连的四连杆机构的四个角点分别为ABCD，如图4和图5所示，A点表示中关节，D点表示顶关节，B点为近指节上的一个固定点，C点为远指节上的一个固定点；其中，l_m表示四连杆机构中AD长度，也即中指节长度；l_g表示四连杆机构中BC长度；r₁表示四连杆机构中AB长度；r₂表示四连杆机构中CD长度；l表示四连杆机构中BD长度。

机械手单指具有绕侧摆关节、基关节和中关节旋转的三个自由度。

控制器通过四个腱驱动器和四根腱绳分别与侧摆关节、基关节、中关节和顶关节相连接，每根腱绳的一端与对应的关节相连接，每根腱绳的另一端连接一个腱驱动器，腱驱动器的另一端与控制器相连接。

每根腱绳上均设置一个所述腱张力传感器。

侧摆关节、基关节、中关节和顶关节上各设置一个关节角位置传感器。

指尖接触力传感器设置在远指节的指尖上。

每个腱张力传感器、每个关节角位置传感器和指尖接触力传感器均与控制器相连接。

进一步，每个腱驱动器均优选包括无刷直流电机和能把旋转运动转换为直线运动的滚珠丝杠。

进一步，控制器中优选内置有阻抗控制模块、逆向运动学解算模块、关节力矩补偿模块、接触力到关节力矩转换模块、张力分配模块和张力控制模块。

如图2所示，一种腱驱动机械手张力约束末端操作阻抗控制方法，包括如下步骤。

步骤1，机械手单指指尖参数设置：设定机械手单指运动的期望指尖接触力F_d和期望指尖笛卡尔位置X_d；同时，预先设定期望腱张力f的取值范围[f_min，f_max]。

其中，f_min为保证腱绳始终处于绷紧状态时的最小腱张力，f_max为保护腱绳不超过负荷时的最大腱张力。

本发明中，腱的张力范围优选设置为：f_min＝8N，f_max＝200N。

步骤2，计算指尖接触力误差F_e：指尖接触力误差F_e的计算公式为：

F_e＝F_d-F_a(1)

式中，F_a为指尖接触力传感器测量到的指尖接触力值，也即实测指尖接触力值。

步骤3，计算期望指尖笛卡尔空间位置X_r，具体计算方法如下。

步骤31，计算笛卡尔空间位置补偿值X_f：阻抗控制模块采用如下公式将指尖接触力误差F_e转换为笛卡尔空间位置补偿值X_f。

式中，M_d、B_d、K_d分别为阻抗控制模块中的阻抗控制器处于临界阻尼或过阻尼状态时的目标惯性矩阵、阻尼矩阵和刚度矩阵。

本发明中M_d、B_d、K_d的优选取值如下：

另外，s为复变量，计算方法及取值具体参见胡寿松著作的《自动控制原理》，科学出版社，第6版，第29页，控制系统的复数域数学模型。

步骤32，计算期望指尖笛卡尔空间位置X_r：对期望指尖笛卡尔位置X_d进行补偿与计算，得到期望指尖笛卡尔空间位置X_r，具体计算公式为：

X_r＝X_d+X_f(3)

本发明中，上述X_d、X_f、X_r是三维列向量，以下公式中提及的θ_a、θ_r、θ_e、f、f_d、△X均为四维列向量。

步骤4，计算关节角位置误差θ_e，具体计算方法如下。

步骤41，计算期望关节角位置θ_r，具体包括如下步骤。

步骤411，逆运动学解算模块转换：将步骤3计算的期望指尖笛卡尔空间位置X_r，通过逆运动学解算模块转换为期望关节角位置θ_r。

其中，期望指尖笛卡尔空间位置X_r在如图3所示手指基坐标系中的笛卡尔位置坐标为(x_T,y_T,z_T)，期望关节角位置

机器人手指的逆运动是已知手指末端在基坐标系中的位置和姿态，求相应各关节的关节角，建立手指末端笛卡尔空间到关节空间的映射。

逆运动学解算模块按照如下公式(4)进行转换。

式中，L₁、L₂、L₃、L₄分别表示手指中基关节、近指节、中指节和远指节的长度；优选值为：L₁＝9mm、L₂＝45mm、L₃＝30mm、L₄＝21mm。θ₁、θ₂、θ₃、θ₄分别为侧摆关节角、基关节角、中关节角和顶关节角；s_i＝sinθ_i，c_i＝cosθ_i，s_ij＝sin(θ_i+θ_j)，c_ij＝cos(θ_i+θ_j)，s_ijk＝sin(θ_i+θ_j+θ_k)，c_ijk＝cos(θ_i+θ_j+θ_k)。

步骤412，计算θ₁：由公式(4)，能直接计算出θ₁。

步骤413，计算θ₃和θ₄：顶关节通过四连杆机构与中关节相连，根据四连杆机构原理，以及如下公式(8)和(10)，并利用非线性函数数值解法求解出θ₃和θ₄。

如图5所示，手指四连杆机构简化为几何问题，θ₄由θ₃确定。建立辅助线BD，其长度l如下计算，

综合式(5)～(7)可得：

其中l_m＝30mm，l_g＝26.7mm，r₁＝4mm，r₂＝6.2mm，γ＝1.52rad，因此当θ₃确定时，θ₄可以唯一确定。

步骤414，计算θ₂：根据求出的θ₃和θ₄，再利用公式(4)求解θ₂。

步骤42，计算关节角位置误差θ_e：关节角位置误差θ_e按如下公式(11)计算。

θ_e＝θ_r-θ_a(11)

式中，θ_a为四个关节角位置传感器分别测量得到的侧摆关节角、基关节角、中关节角和顶关节角。

步骤5，计算最佳期望关节力矩τ：最佳期望关节力矩τ的计算方法如下：

步骤51，计算期望关节力矩τ_d：采用雅克比矩阵J，实现期望指尖接触力F_d到期望关节力矩τ_d的转换，该期望关节力矩τ_d能使手指指尖接触物体且保持静态平衡；期望关节力矩τ_d的计算公式如下：

τ_d＝J^TF_d(12)

其中，雅克比矩阵J的计算公式如下：

步骤52，计算补偿关节力矩τ_e：采用如下公式(14)，将步骤4计算出的关节角位置误差θ_e转化为补偿关节力矩τ_e，实现关节力矩的补偿：

τ_e＝K(θ_r-θ_a)(14)

其中，K为比例控制系数，为常数，具体根据实际应用进行调节，本发明中优选K＝10。

本步骤中的τ_e优选值为0，也即关节角位置误差θ_e为0，也即是说，实测的关节角度优选与期望关节角度相等，从而消除关节角测量值与期望值之间的偏差。

步骤53，计算最佳期望关节力矩τ：按如下公式(15)，计算出最佳期望关节力矩τ；

τ＝τ_d+τ_e(15)

步骤6，计算期望腱张力f_d：腱张力分配模块将步骤5计算出的最佳期望关节力矩τ，转换为期望腱张力f_d，具体转换方法如下。

步骤61，根据f_min确定t的值，使其满足如下公式(17)：

式中，t为腱内部张力值；f_min为保证腱绳始终处于绷紧状态时的最小腱张力；A_i和a_i分别表示A的行向量和a的列元素；通过将步骤5计算出的最佳期望关节力矩τ代入公式(17)中，即可确定腱内部张力值t。

步骤62，将步骤61确定的腱内部张力值t及步骤5计算出的最佳期望关节力矩τ，代入如下的张力分配等式(16)，得到4维张力列向量f，且

f＝R⁺τ+W^Tt＝Aτ+at(16)

上式中，R为腱映射矩阵，R的元素由腱布线路径关节半径给出；和为预先计算出来的常值，A＝R⁺,a＝W⁺。

公式(16)的具体推算过程如下：

由于R能将腱张力f映射为关节力矩τ，也即τ＝Rf。

引入矩阵W^T正交于R的行空间且正定，有下列关系：

其中t表示腱内部张力。

根据图2所示腱的排列可以得出腱映射矩阵P，如下：

系统中腱张力可控的充要条件为P可逆，要求R行满秩，由此可以得到：

令P^-1＝[A>

步骤63，令f_l为张力最小元素，f_h为张力最大元素，将步骤62中4维张力列向量f的四个元素f₁、f₂、f₃和f₄，通过逐元素比较法，得出张力最小元素f_l和张力最大元素f_h。

步骤64，判断张力最大元素f_h是否超过上界f_max，如果f_h≤f_max，表明没有超界，则步骤62中4维张力列向量f即为期望腱张力f_d，将4维张力列向量f中的四个元素f₁、f₂、f₃和f₄依次分配给对应的四根腱绳，并结束张力分配过程。

步骤65，如果f_h>f_max，则采用比例缩放方式构造如下新方程，得到新的4维张力列向量f：

将步骤63计算得到的f_l和f_h及其对应的相关参量A_l、a_l和A_h、a_h，代入式(18)中，得到参量α和t′。

其中，d＝a_hf_l-a_lf_h(20)

公式(21)中的t为步骤61确定的腱内部张力值；然后，令t＝t′，此时，t为更新后的腱内部张力。

步骤66，将步骤65中新的4维张力列向量f中的四个元素，通过逐元素比较法，得出新的张力最小元素f_l和新的张力最大元素f_h。

步骤67，若新的张力最大元素f_h≤f_max且新的张力最小元素f_l≥f_min，则步骤65中新的4维张力列向量f即为期望腱张力f_d，将新的4维张力列向量f中的四个元素依次分配给对应的四根腱绳，并结束张力分配过程；否则从步骤65开始进行迭代处理，也即重复步骤65至步骤67。

步骤7，计算腱张力偏差f_e：腱张力偏差f_e的计算公式为：

f_e＝f_d-f_a(22)

式中，f_a为实际腱张力，由各个腱张力传感器测量所得到。

步骤8，腱张力偏差控制：张力控制模块先按照如下公式(23)，将步骤7计算出的腱张力偏差f_e转化为腱位置偏差△x；

式中，f_d为腱张力期望值；K_p和K_d分别代表比例系数和微分系数，均为常数，均根据实际应用进行调节，本发明中优选K_p＝0.3，K_d＝0。

然后，张力控制模块把腱位置偏差△x输送给腱驱动器，腱驱动器控制腱绳的拉伸。

以上详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。