CN107414826B - 一种腱驱动机械手张力约束末端操作控制方法 - Google Patents
一种腱驱动机械手张力约束末端操作控制方法 Download PDFInfo
- Publication number
- CN107414826B CN107414826B CN201710584862.7A CN201710584862A CN107414826B CN 107414826 B CN107414826 B CN 107414826B CN 201710584862 A CN201710584862 A CN 201710584862A CN 107414826 B CN107414826 B CN 107414826B
- Authority
- CN
- China
- Prior art keywords
- tendon
- tension
- joint
- calculating
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种腱驱动机械手张力约束末端操作控制方法,包括步骤为:机械手单指指尖参数设置、计算指尖接触力误差、计算期望指尖笛卡尔空间位置、计算关节角位置误差、计算最佳期望关节力矩、计算期望腱张力、计算腱张力偏差、然后将腱张力偏差转化为腱位置偏差,接着,张力控制模块把腱位置偏差输送给腱驱动器,腱驱动器控制腱绳的拉伸。本发明响应速度快,关节角测量值与期望值之间的偏差能够消除,从而能够有效提高系统末端操作的力和位置控制的精度,减小机械手接触物体时的冲击,通过约束腱张力,降低腱的磨损,提高其寿命,最终能够有效地实现腱驱动机械手的操作控制,提高机械手的灵巧性和寿命。
Description
技术领域
本发明涉及腱驱动机械手控制领域,特别是一种腱驱动机械手张力约束末端操作控制方法。
背景技术
机械手是模仿人类的手、臂特定功能的一种自动机械,泛指机械臂、末端执行器、灵巧手手指等多关节多连杆操作机构。腱驱动型机械手是利用腱绳进行传动的机械手,允许驱动器放置于机械手结构体外部,可以减小机械手的体积和重量,从而提高了机械手的灵巧性,同时也在驱动器选型方面为机构设计者提供了更多的灵活性。
由于腱只能传递张力,因此为了实现机械手自由度的完全独立控制,必须保证驱动器的数量多于自由度的数量。通过合理配置,N+1型腱能够独立控制N个自由度,同时保证腱具有正张力(拉力)。该腱配置方式简化了机构,但由于该方式在减少腱绳数量的同时引入了关节位置和腱绳的耦合问题,因此控制器的设计非常复杂。在装配应用中机械手需要与非结构环境物理接触,因此机械手的力矩控制能力非常重要。研究者们提出多种控制策略来解决耦合腱驱动机械手的力矩控制问题。这些控制策略由张力分配算法和控制律两部分组成,其中张力分配算法是确定一组腱张力的过程,目的是产生一组期望的关节力矩,同时解决驱动的冗余问题。该冗余可以生成张力的零空间,从而保证所有腱具有正张力。
腱驱动机械手的控制律根据是否使用腱张力反馈分为两类。无腱张力反馈的控制方法包括计算力矩法以及其它能够预测或估计系统参数的智能方法。这些方法对于能够精确对运动和力矩关系进行建模的系统来说是可行的,但机械手抓握或操作的物体的表面特性差别很大,因此这些方法只能获得粗略的腱张力控制,经常产生较高的内张力,导致腱的磨损,增加摩擦力,降低系统性能,因此这些方法难以实现精确操作。采用张力反馈的腱空间控制器忽略腱的动力学,利用张力分配算法将期望的关节力矩转换为期望的腱张力,然后为每根腱使用独立的张力调节器。例如Salisbury和Craig在Stanford/JPL手上实现了腱空间控制律,Starr在Stanford/JPL手通过采用腱-导管模型实现了类似算法,2N型POSTECH手和Utah/MIT手等灵巧手也采用了该算法。然而采用该类控制器在手指动力学中引入了瞬态耦合,即某个关节的控制或扰动可能导致另一个关节的不期望响应。
腱只能传递张力且具有一定的传输延迟,机构与腱之间的摩擦以及腱的其它未建模动力学等因素也影响着腱驱动机械手操作控制算法的设计。
另外,现有控制算法主要采用阻抗控制算法,响应速度慢。加之,现有的关节角测量值和给定值之间存在偏差,而且这个偏差不能消除。因而,无法完全满足腱驱动机械手在精确控制接触力和接触位置的同时保持腱张力处于合适约束范围的要求,从而导致无法很好地满足灵巧操作和强力抓持操作的需求。
发明内容
本发明要解决的技术问题是针对上述现有技术的不足,而提供一种腱驱动机械手张力约束末端操作阻抗控制装置,该腱驱动机械手张力约束末端操作阻抗控制装置响应速度快,关节角测量值与期望值之间的偏差能够消除,从而能够有效提高系统末端操作的力和位置控制的精度,减小机械手接触物体时的冲击,通过约束腱张力,降低腱的磨损,提高其寿命,最终能够有效地实现腱驱动机械手的操作控制,提高机械手的灵巧性和寿命。
为解决上述技术问题,本发明采用的技术方案是:
一种腱驱动机械手张力约束末端操作阻抗控制方法,包括如下步骤。
步骤1,机械手单指指尖参数设置:设定机械手单指运动的期望指尖接触力Fd和期望指尖笛卡尔位置Xd;同时,同时,预先设定期望腱张力f的取值范围[fmin,fmax];其中,fmin为保证腱绳始终处于绷紧状态时的最小腱张力,fmax为保护腱绳不超过负荷时的最大腱张力。
步骤2,计算指尖接触力误差Fe:指尖接触力误差Fe的计算公式为:
Fe=Fd-Fa (1)
式中,Fa为指尖接触力传感器测量到的指尖接触力值,也即实测指尖接触力值。
步骤3,计算期望指尖笛卡尔空间位置Xr,具体计算方法如下。
步骤31,计算笛卡尔空间位置补偿值Xf:阻抗控制模块采用如下公式将指尖接触力误差Fe转换为笛卡尔空间位置补偿值Xf。
式中,Md、Bd、Kd分别为阻抗控制模块中的阻抗控制器处于临界阻尼或过阻尼状态时的目标惯性矩阵、阻尼矩阵和刚度矩阵;s为复变量;
步骤32,计算期望指尖笛卡尔空间位置Xr:对期望指尖笛卡尔位置Xd进行补偿与计算,得到期望指尖笛卡尔空间位置Xr,具体计算公式为:
Xr=Xd+Xf (3)
步骤4,计算关节角位置误差θe,具体计算方法如下。
步骤41,计算期望关节角位置θr,具体包括如下步骤。
步骤411,逆运动学解算模块转换:将步骤3计算的期望指尖笛卡尔空间位置Xr,通过逆运动学解算模块转换为期望关节角位置θr;其中,期望指尖笛卡尔空间位置Xr在手指基坐标系中的笛卡尔位置坐标为(xT,yT,zT),逆运动学解算模块按照如下公式(4)进行转换。
式中,L1、L2、L3、L4分别表示手指中基关节、近指节、中指节和远指节的长度;θ1、θ2、θ3、θ4分别为侧摆关节角、基关节角、中关节角和顶关节角;si=sinθi,ci=cosθi,sij=sin(θi+θj),cij=cos(θi+θj),sijk=sin(θi+θj+θk),cijk=cos(θi+θj+θk)。
步骤412,计算θ1:由公式(4),能直接计算出θ1。
步骤413,计算θ3和θ4:顶关节通过四连杆机构与中关节相连,根据四连杆机构原理,以及如下公式(8)和(10),并利用非线性函数数值解法求解出θ3和θ4。
假设顶关节与中关节相连的四连杆机构的四个角点分别为ABCD,其中,A点表示中关节,D点表示顶关节,B点为近指节上的一个固定点,C点为远指节上的一个固定点;则公式(8)和(10)中,lm表示四连杆机构中AD长度,也即中指节长度;lg表示四连杆机构中BC长度;r1表示四连杆机构中AB长度;r2表示四连杆机构中CD长度;l表示四连杆机构中BD长度。
步骤414,计算θ2:根据求出的θ3和θ4,再利用公式(4)求解θ2。
步骤42,计算关节角位置误差θe:关节角位置误差θe按如下公式(11)计算。
θe=θr-θa (11)
式中,θa为四个关节角位置传感器分别测量得到的侧摆关节角、基关节角、中关节角和顶关节角。
步骤5,计算最佳期望关节力矩τ:最佳期望关节力矩τ的计算方法如下:
步骤51,计算期望关节力矩τd:采用雅克比矩阵J,实现期望指尖接触力Fd到期望关节力矩τd的转换,该期望关节力矩τd能使手指指尖接触物体且保持静态平衡;期望关节力矩τd的计算公式如下:
τd=JTFd (12)
其中,雅克比矩阵J的计算公式如下:
步骤52,计算补偿关节力矩τe:采用如下公式(14),将步骤4计算出的关节角位置误差θe转化为补偿关节力矩τe,实现关节力矩的补偿:
τe=K(θr-θa) (14)
其中,K为比例控制系数,根据实际应用进行调节。
步骤53,计算最佳期望关节力矩τ:按如下公式(15),计算出最佳期望关节力矩τ;
τ=τd+τe (15)
步骤6,计算期望腱张力fd:腱张力分配模块将步骤5计算出的最佳期望关节力矩τ,转换为期望腱张力fd。
步骤7,计算腱张力偏差fe:腱张力偏差fe的计算公式为:
fe=fd-fa (22)
式中,fa为实际腱张力,由各个腱张力传感器测量所得到。
步骤8,腱张力偏差控制:张力控制模块先按照如下公式(23),将步骤7计算出的腱张力偏差fe转化为腱位置偏差△x;
式中,Kp、Kd和fd各代表比例系数、微分系数和腱张力期望值。
然后,张力控制模块把腱位置偏差△x输送给腱驱动器,腱驱动器控制腱绳的拉伸。
步骤6中,腱张力分配模块将最佳期望关节力矩τ转换为期望腱张力fd的转换方法如下。
步骤61,根据fmin确定t的值,使其满足如下公式(17):
式中,t为腱内部张力值;fmin为保证腱绳始终处于绷紧状态时的最小腱张力;Ai和ai分别表示A的行向量和a的列元素;通过将步骤5计算出的最佳期望关节力矩τ代入公式(17)中,即可确定腱内部张力值t。
f=R+τ+WTt=Aτ+at (16)
步骤63,令fl为张力最小元素,fh为张力最大元素,将步骤62中4维张力列向量f的四个元素f1、f2、f3和f4,通过逐元素比较法,得出张力最小元素fl和张力最大元素fh。
步骤64,判断张力最大元素fh是否超过上界fmax,如果fh≤fmax,表明没有超界,则步骤62中4维张力列向量f即为期望腱张力fd,将4维张力列向量f中的四个元素f1、f2、f3和f4依次分配给对应的四根腱绳,并结束张力分配过程。
步骤65,如果fh>fmax,则采用比例缩放方式构造如下新方程,得到新的4维张力列向量f:
将步骤63计算得到的fl和fh及其对应的相关参量Al、al和Ah、ah,代入式(18)中,得到参量α和t′。
其中,d=ahfl-alfh (20)
公式(21)中的t为步骤61确定的腱内部张力值;然后,令t=t′,此时,t为更新后的腱内部张力。
步骤66,将步骤65中新的4维张力列向量f中的四个元素,通过逐元素比较法,得出新的张力最小元素fl和新的张力最大元素fh。
步骤67,若新的张力最大元素fh≤fmax且新的张力最小元素fl≥fmin,则步骤65中新的4维张力列向量f即为期望腱张力fd,将新的4维张力列向量f中的四个元素依次分配给对应的四根腱绳,并结束张力分配过程;否则从步骤65开始进行迭代处理。
步骤4中,L1=9mm、L2=45mm、L3=30mm、L4=21mm。
步骤4中,lm=30mm,lg=26.7mm,r1=4mm,r2=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点为远指节上的一个固定点;其中,lm表示四连杆机构中AD长度,也即中指节长度;lg表示四连杆机构中BC长度;r1表示四连杆机构中AB长度;r2表示四连杆机构中CD长度;l表示四连杆机构中BD长度。
机械手单指具有绕侧摆关节、基关节和中关节旋转的三个自由度。
控制器通过四个腱驱动器和四根腱绳分别与侧摆关节、基关节、中关节和顶关节相连接,每根腱绳的一端与对应的关节相连接,每根腱绳的另一端连接一个腱驱动器,腱驱动器的另一端与控制器相连接。
每根腱绳上均设置一个所述腱张力传感器。
侧摆关节、基关节、中关节和顶关节上各设置一个关节角位置传感器。
指尖接触力传感器设置在远指节的指尖上。
每个腱张力传感器、每个关节角位置传感器和指尖接触力传感器均与控制器相连接。
进一步,每个腱驱动器均优选包括无刷直流电机和能把旋转运动转换为直线运动的滚珠丝杠。
进一步,控制器中优选内置有阻抗控制模块、逆向运动学解算模块、关节力矩补偿模块、接触力到关节力矩转换模块、张力分配模块和张力控制模块。
如图2所示,一种腱驱动机械手张力约束末端操作阻抗控制方法,包括如下步骤。
步骤1,机械手单指指尖参数设置:设定机械手单指运动的期望指尖接触力Fd和期望指尖笛卡尔位置Xd;同时,预先设定期望腱张力f的取值范围[fmin,fmax]。
其中,fmin为保证腱绳始终处于绷紧状态时的最小腱张力,fmax为保护腱绳不超过负荷时的最大腱张力。
本发明中,腱的张力范围优选设置为:fmin=8N,fmax=200N。
步骤2,计算指尖接触力误差Fe:指尖接触力误差Fe的计算公式为:
Fe=Fd-Fa (1)
式中,Fa为指尖接触力传感器测量到的指尖接触力值,也即实测指尖接触力值。
步骤3,计算期望指尖笛卡尔空间位置Xr,具体计算方法如下。
步骤31,计算笛卡尔空间位置补偿值Xf:阻抗控制模块采用如下公式将指尖接触力误差Fe转换为笛卡尔空间位置补偿值Xf。
式中,Md、Bd、Kd分别为阻抗控制模块中的阻抗控制器处于临界阻尼或过阻尼状态时的目标惯性矩阵、阻尼矩阵和刚度矩阵。
本发明中Md、Bd、Kd的优选取值如下:
另外,s为复变量,计算方法及取值具体参见胡寿松著作的《自动控制原理》,科学出版社,第6版,第29页,控制系统的复数域数学模型。
步骤32,计算期望指尖笛卡尔空间位置Xr:对期望指尖笛卡尔位置Xd进行补偿与计算,得到期望指尖笛卡尔空间位置Xr,具体计算公式为:
Xr=Xd+Xf (3)
本发明中,上述Xd、Xf、Xr是三维列向量,以下公式中提及的θa、θr、θe、f、fd、△X均为四维列向量。
步骤4,计算关节角位置误差θe,具体计算方法如下。
步骤41,计算期望关节角位置θr,具体包括如下步骤。
步骤411,逆运动学解算模块转换:将步骤3计算的期望指尖笛卡尔空间位置Xr,通过逆运动学解算模块转换为期望关节角位置θr。
机器人手指的逆运动是已知手指末端在基坐标系中的位置和姿态,求相应各关节的关节角,建立手指末端笛卡尔空间到关节空间的映射。
逆运动学解算模块按照如下公式(4)进行转换。
式中,L1、L2、L3、L4分别表示手指中基关节、近指节、中指节和远指节的长度;优选值为:L1=9mm、L2=45mm、L3=30mm、L4=21mm。θ1、θ2、θ3、θ4分别为侧摆关节角、基关节角、中关节角和顶关节角;si=sinθi,ci=cosθi,sij=sin(θi+θj),cij=cos(θi+θj),sijk=sin(θi+θj+θk),cijk=cos(θi+θj+θk)。
步骤412,计算θ1:由公式(4),能直接计算出θ1。
步骤413,计算θ3和θ4:顶关节通过四连杆机构与中关节相连,根据四连杆机构原理,以及如下公式(8)和(10),并利用非线性函数数值解法求解出θ3和θ4。
如图5所示,手指四连杆机构简化为几何问题,θ4由θ3确定。建立辅助线BD,其长度l如下计算,
综合式(5)~(7)可得:
其中lm=30mm,lg=26.7mm,r1=4mm,r2=6.2mm,γ=1.52rad,因此当θ3确定时,θ4可以唯一确定。
步骤414,计算θ2:根据求出的θ3和θ4,再利用公式(4)求解θ2。
步骤42,计算关节角位置误差θe:关节角位置误差θe按如下公式(11)计算。
θe=θr-θa (11)
式中,θa为四个关节角位置传感器分别测量得到的侧摆关节角、基关节角、中关节角和顶关节角。
步骤5,计算最佳期望关节力矩τ:最佳期望关节力矩τ的计算方法如下:
步骤51,计算期望关节力矩τd:采用雅克比矩阵J,实现期望指尖接触力Fd到期望关节力矩τd的转换,该期望关节力矩τd能使手指指尖接触物体且保持静态平衡;期望关节力矩τd的计算公式如下:
τd=JTFd (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,计算期望腱张力fd:腱张力分配模块将步骤5计算出的最佳期望关节力矩τ,转换为期望腱张力fd,具体转换方法如下。
步骤61,根据fmin确定t的值,使其满足如下公式(17):
式中,t为腱内部张力值;fmin为保证腱绳始终处于绷紧状态时的最小腱张力;Ai和ai分别表示A的行向量和a的列元素;通过将步骤5计算出的最佳期望关节力矩τ代入公式(17)中,即可确定腱内部张力值t。
f=R+τ+WTt=Aτ+at (16)
公式(16)的具体推算过程如下:
由于R能将腱张力f映射为关节力矩τ,也即τ=Rf。
其中t表示腱内部张力。
根据图2所示腱的排列可以得出腱映射矩阵P,如下:
系统中腱张力可控的充要条件为P可逆,要求R行满秩,由此可以得到:
令P-1=[A a],即得到公式(16)。
步骤63,令fl为张力最小元素,fh为张力最大元素,将步骤62中4维张力列向量f的四个元素f1、f2、f3和f4,通过逐元素比较法,得出张力最小元素fl和张力最大元素fh。
步骤64,判断张力最大元素fh是否超过上界fmax,如果fh≤fmax,表明没有超界,则步骤62中4维张力列向量f即为期望腱张力fd,将4维张力列向量f中的四个元素f1、f2、f3和f4依次分配给对应的四根腱绳,并结束张力分配过程。
步骤65,如果fh>fmax,则采用比例缩放方式构造如下新方程,得到新的4维张力列向量f:
将步骤63计算得到的fl和fh及其对应的相关参量Al、al和Ah、ah,代入式(18)中,得到参量α和t′。
其中,d=ahfl-alfh (20)
公式(21)中的t为步骤61确定的腱内部张力值;然后,令t=t′,此时,t为更新后的腱内部张力。
步骤66,将步骤65中新的4维张力列向量f中的四个元素,通过逐元素比较法,得出新的张力最小元素fl和新的张力最大元素fh。
步骤67,若新的张力最大元素fh≤fmax且新的张力最小元素fl≥fmin,则步骤65中新的4维张力列向量f即为期望腱张力fd,将新的4维张力列向量f中的四个元素依次分配给对应的四根腱绳,并结束张力分配过程;否则从步骤65开始进行迭代处理,也即重复步骤65至步骤67。
步骤7,计算腱张力偏差fe:腱张力偏差fe的计算公式为:
fe=fd-fa (22)
式中,fa为实际腱张力,由各个腱张力传感器测量所得到。
步骤8,腱张力偏差控制:张力控制模块先按照如下公式(23),将步骤7计算出的腱张力偏差fe转化为腱位置偏差△x;
式中,fd为腱张力期望值;Kp和Kd分别代表比例系数和微分系数,均为常数,均根据实际应用进行调节,本发明中优选Kp=0.3,Kd=0。
然后,张力控制模块把腱位置偏差△x输送给腱驱动器,腱驱动器控制腱绳的拉伸。
以上详细描述了本发明的优选实施方式,但是,本发明并不限于上述实施方式中的具体细节,在本发明的技术构思范围内,可以对本发明的技术方案进行多种等同变换,这些等同变换均属于本发明的保护范围。
Claims (3)
1.一种腱驱动机械手张力约束末端操作阻抗控制方法,其特征在于:包括如下步骤:
步骤1,机械手单指指尖参数设置:设定机械手单指运动的期望指尖接触力Fd和期望指尖笛卡尔位置Xd;同时,预先设定期望腱张力f的取值范围[fmin,fmax];其中,fmin为保证腱绳始终处于绷紧状态时的最小腱张力,fmax为保护腱绳不超过负荷时的最大腱张力;
步骤2,计算指尖接触力误差Fe:指尖接触力误差Fe的计算公式为:
Fe=Fd-Fa (1)
式中,Fa为指尖接触力传感器测量到的指尖接触力值,也即实测指尖接触力值;
步骤3,计算期望指尖笛卡尔空间位置Xr,具体计算方法如下:
步骤31,计算笛卡尔空间位置补偿值Xf:阻抗控制模块采用如下公式将指尖接触力误差Fe转换为笛卡尔空间位置补偿值Xf;
式中,Md、Bd、Kd分别为阻抗控制模块中的阻抗控制器处于临界阻尼或过阻尼状态时的目标惯性矩阵、阻尼矩阵和刚度矩阵;s为复变量;
步骤32,计算期望指尖笛卡尔空间位置Xr:对期望指尖笛卡尔位置Xd进行补偿与计算,得到期望指尖笛卡尔空间位置Xr,具体计算公式为:
Xr=Xd+Xf (3)
步骤4,计算关节角位置误差θe,具体计算方法如下:
步骤41,计算期望关节角位置θr,具体包括如下步骤:
步骤411,逆运动学解算模块转换:将步骤3计算的期望指尖笛卡尔空间位置Xr,通过逆运动学解算模块转换为期望关节角位置θr;其中,期望指尖笛卡尔空间位置Xr在手指基坐标系中的笛卡尔位置坐标为(xT,yT,zT),逆运动学解算模块按照如下公式(4)进行转换;
式中,L1、L2、L3、L4分别表示手指中基关节、近指节、中指节和远指节的长度;θ1、θ2、θ3、θ4分别为侧摆关节角、基关节角、中关节角和顶关节角;si=sinθi,ci=cosθi,sij=sin(θi+θj),cij=cos(θi+θj),sijk=sin(θi+θj+θk),cijk=cos(θi+θj+θk);
步骤412,计算θ1:由公式(4),能直接计算出θ1;
步骤413,计算θ3和θ4:顶关节通过四连杆机构与中关节相连,根据四连杆机构原理,以及如下公式(8)和(10),并利用非线性函数数值解法求解出θ3和θ4;
假设顶关节与中关节相连的四连杆机构的四个角点分别为ABCD,其中,A点表示中关节,D点表示顶关节,B点为近指节上的一个固定点,C点为远指节上的一个固定点;则公式(8)和(10)中,lm表示四连杆机构中AD长度,也即中指节长度;lg表示四连杆机构中BC长度;r1表示四连杆机构中AB长度;r2表示四连杆机构中CD长度;l表示四连杆机构中BD长度;
步骤414,计算θ2:根据求出的θ3和θ4,再利用公式(4)求解θ2;
步骤42,计算关节角位置误差θe:关节角位置误差θe按如下公式(11)计算;
θe=θr-θa (11)
式中,θa为四个关节角位置传感器分别测量得到的侧摆关节角、基关节角、中关节角和顶关节角;
步骤5,计算最佳期望关节力矩τ:最佳期望关节力矩τ的计算方法如下:
步骤51,计算期望关节力矩τd:采用雅克比矩阵J,实现期望指尖接触力Fd到期望关节力矩τd的转换,该期望关节力矩τd能使手指指尖接触物体且保持静态平衡;期望关节力矩τd的计算公式如下:
τd=JTFd (12)
其中,雅克比矩阵J的计算公式如下:
步骤52,计算补偿关节力矩τe:采用如下公式(14),将步骤4计算出的关节角位置误差θe转化为补偿关节力矩τe,实现关节力矩的补偿:
τe=K(θr-θa) (14)
其中K为比例控制系数,θr-θa=0,τe=0;
步骤53,计算最佳期望关节力矩τ:按如下公式(15),计算出最佳期望关节力矩τ;
τ=τd+τe (15)
步骤6,计算期望腱张力fd:腱张力分配模块将步骤5计算出的最佳期望关节力矩τ,转换为期望腱张力fd;
腱张力分配模块将最佳期望关节力矩τ转换为期望腱张力fd的转换方法如下:
步骤61,根据fmin确定t的值,使其满足如下公式(17):
式中,t为腱内部张力值;fmin为保证腱绳始终处于绷紧状态时的最小腱张力;Ai和ai分别表示A的行向量和a的列元素;通过将步骤5计算出的最佳期望关节力矩τ代入公式(17)中,即可确定腱内部张力值t;
f=R+τ+WTt=Aτ+at (16)
步骤63,令fl为张力最小元素,fh为张力最大元素,将步骤62中4维张力列向量f的四个元素f1、f2、f3和f4,通过逐元素比较法,得出张力最小元素fl和张力最大元素fh;
步骤64,判断张力最大元素fh是否超过上界fmax,如果fh≤fmax,表明没有超界,则步骤62中4维张力列向量f即为期望腱张力fd,将4维张力列向量f中的四个元素f1、f2、f3和f4依次分配给对应的四根腱绳,并结束张力分配过程;
步骤65,如果fh>fmax,则采用比例缩放方式构造如下新方程,得到新的4维张力列向量f:
将步骤63计算得到的fl和fh及其对应的相关参量Al、al和Ah、ah,代入式(18)中,得到参量α和t′:
其中,d=ahfl-alfh (20)
公式(21)中的t为步骤61确定的腱内部张力值;然后,令t=t′,此时,t为更新后的腱内部张力;
步骤66,将步骤65中新的4维张力列向量f中的四个元素,通过逐元素比较法,得出新的张力最小元素fl和新的张力最大元素fh;
步骤67,若新的张力最大元素fh≤fmax且新的张力最小元素fl≥fmin,则步骤65中新的4维张力列向量f即为期望腱张力fd,将新的4维张力列向量f中的四个元素依次分配给对应的四根腱绳,并结束张力分配过程;否则从步骤65开始进行迭代处理;
步骤7,计算腱张力偏差fe:腱张力偏差fe的计算公式为:
fe=fd-fa (22)
式中,fa为实际腱张力,由各个腱张力传感器测量所得到;
步骤8,腱张力偏差控制:张力控制模块先按照如下公式(23),将步骤7计算出的腱张力偏差fe转化为腱位置偏差△x;
式中,Kp、Kd和fd各代表比例系数、微分系数和腱张力期望值;
然后,张力控制模块把腱位置偏差△x输送给腱驱动器,腱驱动器控制腱绳的拉伸。
2.根据权利要求1所述的腱驱动机械手张力约束末端操作阻抗控制方法,其特征在于:步骤4中,L1=9mm、L2=45mm、L3=30mm、L4=21mm。
3.根据权利要求1所述的腱驱动机械手张力约束末端操作阻抗控制方法,其特征在于:步骤4中,lm=30mm,lg=26.7mm,r1=4mm,r2=6.2mm。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710584862.7A CN107414826B (zh) | 2017-07-18 | 2017-07-18 | 一种腱驱动机械手张力约束末端操作控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710584862.7A CN107414826B (zh) | 2017-07-18 | 2017-07-18 | 一种腱驱动机械手张力约束末端操作控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107414826A CN107414826A (zh) | 2017-12-01 |
CN107414826B true CN107414826B (zh) | 2020-03-31 |
Family
ID=60430756
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710584862.7A Active CN107414826B (zh) | 2017-07-18 | 2017-07-18 | 一种腱驱动机械手张力约束末端操作控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107414826B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108436913B (zh) * | 2018-04-19 | 2020-12-25 | 南京航空航天大学 | 一种力协调的多臂机器人柔顺控制方法 |
CN114609911B (zh) * | 2022-03-15 | 2022-11-25 | 中国科学院重庆绿色智能技术研究院 | 一种抗干扰自适应力位协调控制方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104191429A (zh) * | 2014-07-28 | 2014-12-10 | 南京航空航天大学 | 一种腱驱动机械手位置和腱张力的混合控制方法及控制装置 |
CN104400783A (zh) * | 2014-09-24 | 2015-03-11 | 哈尔滨工业大学 | 具有张力和关节位置反馈的机器人关节用挠性驱动单元控制方法 |
CN105690388A (zh) * | 2016-04-05 | 2016-06-22 | 南京航空航天大学 | 一种腱驱动机械手腱张力约束阻抗控制方法及装置 |
CN105945979A (zh) * | 2016-07-08 | 2016-09-21 | 上海航天控制技术研究所 | 对欠驱动二指爪机构进行柔顺控制的方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8060250B2 (en) * | 2008-12-15 | 2011-11-15 | GM Global Technology Operations LLC | Joint-space impedance control for tendon-driven manipulators |
-
2017
- 2017-07-18 CN CN201710584862.7A patent/CN107414826B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104191429A (zh) * | 2014-07-28 | 2014-12-10 | 南京航空航天大学 | 一种腱驱动机械手位置和腱张力的混合控制方法及控制装置 |
CN104400783A (zh) * | 2014-09-24 | 2015-03-11 | 哈尔滨工业大学 | 具有张力和关节位置反馈的机器人关节用挠性驱动单元控制方法 |
CN105690388A (zh) * | 2016-04-05 | 2016-06-22 | 南京航空航天大学 | 一种腱驱动机械手腱张力约束阻抗控制方法及装置 |
CN105945979A (zh) * | 2016-07-08 | 2016-09-21 | 上海航天控制技术研究所 | 对欠驱动二指爪机构进行柔顺控制的方法 |
Non-Patent Citations (2)
Title |
---|
仿人型假手指尖三维力触觉传感器及动态抓取研究;张庭;《中国博士学位论文全文数据库(电子期刊)信息科技辑》;20150115(第01期);I140-19 * |
空间五指灵巧手单指控制系统设计;王小涛等;《科学技术与工程》;20150630;第15卷(第17期);150-153 * |
Also Published As
Publication number | Publication date |
---|---|
CN107414826A (zh) | 2017-12-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105690388B (zh) | 一种腱驱动机械手腱张力约束阻抗控制方法及装置 | |
Butterfaß et al. | DLR-Hand II: Next generation of a dextrous robot hand | |
US8060250B2 (en) | Joint-space impedance control for tendon-driven manipulators | |
JP6312264B2 (ja) | 冗長自由度を伴うロボットマニピュレータの制約 | |
KR101549818B1 (ko) | 로봇 핸드 및 그 제어방법 | |
US8364314B2 (en) | Method and apparatus for automatic control of a humanoid robot | |
US8504205B2 (en) | Robotic grasping device with multi-force sensing at base of fingers | |
Alberts et al. | Force control of a multi-arm robot system | |
CN107030694A (zh) | 腱驱动机械手腱张力约束末端力位操作控制方法和装置 | |
CN111890348B (zh) | 一种双机器人协同搬运的控制方法及装置 | |
KR20110041950A (ko) | 여유자유도 제어를 이용한 로봇의 교시 및 재현 방법 | |
CN108555914B (zh) | 一种基于腱驱动灵巧手的dnn神经网络自适应控制方法 | |
CN107414826B (zh) | 一种腱驱动机械手张力约束末端操作控制方法 | |
CN111203880A (zh) | 基于数据驱动的图像视觉伺服控制系统及方法 | |
CN116901057A (zh) | 一种基于变阻抗策略的双机械臂末端位置协同控制方法 | |
Karlen et al. | A dual-arm dexterous manipulator system with anthropomorphic kinematics | |
CN114800527A (zh) | 一种移动作业机械臂末端施力控制方法及系统 | |
US20120303161A1 (en) | Robot controller | |
JPH1133952A (ja) | ロボットの制御方法、ロボットおよび把持物体の位置、姿勢修正方法 | |
CN113001549A (zh) | 一种基于广义抓持逆矩阵的多机械臂负载分配方法 | |
CN116766181B (zh) | 一种基于全驱系统理论的机械臂主动柔顺控制方法 | |
Chen et al. | Development of a three-fingered multi-modality dexterous hand with integrated embedded high-dimensional sensors | |
CN115781695B (zh) | 一种触觉传感器模块、装置及移动操作机器人控制方法 | |
CN116852397B (zh) | 负压理疗机器人理疗力度及理疗路径的自适应调节方法 | |
KR102380237B1 (ko) | 능동 구동이 가능한 로봇용 툴 결합 장치 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |