CN108241339B - 仿人机械臂的运动求解和构型控制方法 - Google Patents
仿人机械臂的运动求解和构型控制方法 Download PDFInfo
- Publication number
- CN108241339B CN108241339B CN201711443796.8A CN201711443796A CN108241339B CN 108241339 B CN108241339 B CN 108241339B CN 201711443796 A CN201711443796 A CN 201711443796A CN 108241339 B CN108241339 B CN 108241339B
- Authority
- CN
- China
- Prior art keywords
- joint
- arm
- mechanical arm
- delta
- motion
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 154
- 238000000034 method Methods 0.000 title claims abstract description 85
- 238000005457 optimization Methods 0.000 claims abstract description 8
- 210000000323 shoulder joint Anatomy 0.000 claims description 41
- 210000003857 wrist joint Anatomy 0.000 claims description 39
- 239000011159 matrix material Substances 0.000 claims description 25
- 210000001503 joint Anatomy 0.000 claims description 24
- 210000002310 elbow joint Anatomy 0.000 claims description 17
- 210000000707 wrist Anatomy 0.000 claims description 13
- 238000013461 design Methods 0.000 claims description 9
- 239000004576 sand Substances 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 7
- XOFYZVNMUHMLCC-ZPOLXVRWSA-N prednisone Chemical compound O=C1C=C[C@]2(C)[C@H]3C(=O)C[C@](C)([C@@](CC4)(O)C(=O)CO)[C@@H]4[C@@H]3CCC2=C1 XOFYZVNMUHMLCC-ZPOLXVRWSA-N 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims 1
- 230000035790 physiological processes and functions Effects 0.000 claims 1
- 241000282414 Homo sapiens Species 0.000 abstract description 21
- 230000009471 action Effects 0.000 abstract description 3
- 230000000875 corresponding effect Effects 0.000 description 13
- 230000008859 change Effects 0.000 description 11
- 239000012636 effector Substances 0.000 description 6
- 238000013459 approach Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000001276 controlling effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 241000282412 Homo Species 0.000 description 1
- 101000622237 Homo sapiens Transcription cofactor vestigial-like protein 1 Proteins 0.000 description 1
- 102100023478 Transcription cofactor vestigial-like protein 1 Human genes 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/408—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by data handling or data format, e.g. reading, buffering or conversion of data
- G05B19/4086—Coordinate conversions; Other special calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/35—Nc in input of data, input till input file format
- G05B2219/35356—Data handling
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种仿人机械臂的运动求解和构型控制方法。本发明提出的运动求解方法利用该类机械臂的结构特点,在对应的人类手臂上求出机械臂的整体运动,然后映射回机械臂的关节空间进行执行。本发明提出的运动求解方法不依赖于仿人机械臂的实际关节布置,对于具有不同关节布置的、但是大臂和小臂的臂长相同的仿人机械臂,能产生一致的运动;同时保留了运动优化能力;从对任务的处理方式上模仿人类,能更好地产生类人的动作。本发明基于运动求解方法中的理论,提出了一种构型控制方法,能控制仿人机械臂的完整构型,并且保证了仿人机械臂从运动规划到运动控制的一致性。
Description
技术领域
本发明属于机器人领域,具体涉及一种仿人机械臂的运动求解和构型控制方法。
背景技术
仿人机械臂是一类特殊的机械臂,它是模仿人类手臂的结构进行设计的,具有S-R-S(球副-旋转副-球副)的结构,通常由7个旋转关节组成,如图1和图2所示。图中,S表示肩关节中心,对应肩关节处三个机械关节轴线的交点;E表示肘关节中心;W表示腕关节中心,对应腕关节处三个机械关节轴线的交点;zi(i=1,…,7)表示7个机械臂关节的轴线方向;l1和l2分别为大臂和小臂的臂长;{S}和{W}分别为肩关节和腕关节坐标系,其原点分别位于肩关节和腕关节中心。其中,前3个关节的轴线相交于一点,构成一个球副,是机械臂的肩关节;第4个关节作为机械臂的肘关节;最后3个关节的轴线也相交于一点,同样构成一个球副,是机械臂的腕关节。S-R-S的结构可以有不同的实施方式,即机械臂的关节布置可以不同。然而,由于模仿的都是人类手臂,具有不同关节布置的、但是大臂和小臂臂长相同的仿人机械臂应该被看作是相同的机械臂。所以,在对仿人机械臂进行操作空间轨迹规划时,运动求解方法需要保证得到的运动对于相同的机械臂是一致的。另外,一些场景要求仿人机械臂能严格地执行规划的运动,即需要对机械臂进行构型控制,完全地跟踪规划的运动。例如,仿人机械臂在与人协作时,会在规划阶段完成避碰验证;为了确保无碰撞,机械臂需要严格按照规划的运动运行。由于仿人机械臂的特殊性,机械臂在规划和调整运动时最好能保持前后一致性,以符合人类的动作特性。所以,规划过程中用到的理论应该能应用于构型控制中。
运动求解是机械臂操作空间轨迹规划中的中心问题之一,需要根据末端执行器的运动求解出对应的各关节的运动。由于仿人机械臂具有7个自由度,而操作空间内的任务需求是6个自由度,机械臂具有一个冗余自由度,因此存在无穷多组解。对于仿人机械臂而言,难点不在于求出一组可行的解来,而是在于求出能保证相同的机械臂的运动一致的解。这里的一致是指末端执行器的运动是相同的,同时机械臂臂形的变化也是相同的。目前,对机械臂的运动求解方法有很多种,这些方法均是通用的方法,可以应用到仿人机械臂上。一种方法是广义逆方法,即建立机械臂的雅克比矩阵,利用雅克比矩阵的广义逆来求解机械臂的关节运动,如参考文献[1]:CHAN T F,DUBEY R V.A weighted least-norm solutionbased scheme for avoiding joint limits for redundant joint manipulators[J].IEEE transactions on Robotics and Automation,1995,11(2):286–292。这种方法能保证末端执行器按照任务需求运动。但是,雅克比矩阵是与机械臂的实际关节布置紧密相关的,对于不同关节布置的仿人机械臂,它们的雅克比矩阵各不相同。因此,仿人机械臂会产生不同的臂形变化曲线,即无法满足运动的一致性。另一种方法是引入额外的参数,消除机械臂的冗余性,如参考文献[2]:TONDU B.A closed-form inverse kinematic modellingof a 7R anthropomorphic upper limb based on a joint parametrization[C]Proceedings of the 6th IEEE-RAS International Conference on Humanoid Robots,2006,5:390–397中采用了大臂伸展角和臂形角两种参数化方式。由于机械臂被完全限制住,这种方法能保证运动的一致性。但是,这种方法需要根据引入的参数定义额外的任务需求,因而增加了运动规划的工作量。另外,它也使得机械臂丧失了运动优化的能力。
运动规划完成后,机械臂的运动实际上已经完全确定了。一些场景要求仿人机械臂能严格地执行规划的运动,即需要对机械臂进行构型控制,完全地跟踪规划的运动。关节空间能完整描述机械臂的运动,因此构型控制可以在关节空间中进行。这种构型控制方法是根据关节的误差来控制机械臂的运动的。因为关节误差与实际需要的操作空间的任务需求没有直接的关系,所以末端执行器的运动跟踪性能无法得到有效的保障。另外,由于运动求解不能在关节空间中进行,因此无法保证运动规划和控制的前后一致性。另一种方法是引入额外参数以描述机械臂的完整运动,通过末端执行器的位姿参数和额外参数与关节空间的严格数学关系来设计控制器,实现构型控制,如参考文献[3]:SERAJIH.Configuration control of redundant manipulators:Theory and implementation[J].IEEE Transactions on Robotics and Automation,1989,5(4):472–490。这种方法没有考虑仿人机械臂的结构特点;另外,如上所述,它所对应的运动求解方法具有增加运动规划工作量和使机械臂丧失优化能力等缺点。
发明内容
针对现有方法中存在的问题,本发明提出一种仿人机械臂的运动求解和构型控制方法。本发明提出的运动求解方法不是从机械臂的运动学模型出发,而是利用机械臂的结构特点,将操作空间中的运动需求与机械臂所对应人类手臂的生理关节关联起来,在对应的人类手臂上完成运动的求解,然后映射回机械臂的关节空间中。因此,本发明提出的运动求解方法对于不同关节布置的仿人机械臂能产生一致的运动。另外,本发明提出的运动求解方法保留了机械臂的运动优化能力。本发明进而将运动求解方法中的理论应用于仿人机械臂的构型控制中,能完全跟踪机械臂的运动,同时保持了运动规划和控制的前后一致性。
一种仿人机械臂的运动求解方法,包括以下步骤:
步骤一:将操作空间中的任务需求与机械臂的生理关节进行关联。
人类在控制末端工具的运动时,注意力聚焦在工具坐标系上,一般希望能直接影响到工具坐标系的位置和姿态。因此,在肩关节处建立虚拟关节Δ-P,Δ-As和Δ-L,构成一个球型机器人,以控制工具坐标系的位置。其中,Δ-P和Δ-As为转动副,Δ-L为移动副。同时,由于工具坐标系和腕关节坐标系的相对位姿是固定的,所以对工具坐标系的速度要求VT可以转换到对腕关节坐标系的速度要求VW上。这样,轨迹规划的任务需求与机械臂的生理关节关联起来了。
步骤二:对腕关节的线速度进行分解,确定肩关节和肘关节的运动。
保留步骤一中的肩关节处虚拟关节Δ-P和Δ-As的影响。Δ-P和Δ-As贡献了腕关节线速度vW的一部分,剩余的部分vW,rem由大臂和小臂实现。将剩余线速度vW,rem沿肩-腕连线SW进行分解,其中,垂直分量由肩关节处的虚拟关节Δ-M实现;平行分量有肩关节和肘关节处的虚拟关节Δ-S和Δ-E共同实现。
此时,仿人机械臂的肩关节和肘关节的角速度确定了。肩关节的角速度ωshoulder来源于:Δ-P,Δ-As,Δ-M和Δ-S;肘关节的角速度ωelbow来源于:Δ-E。
步骤三:确定腕关节的运动。
步骤一到步骤二确定了肩关节和肘关节的运动,实现了腕关节坐标系的线速度需求。腕关节坐标系的角速度需求ωW由肩、肘和腕关节共同决定。因此,排除肩关节和肘关节运动的影响,可求得腕关节的运动,即有
ωWrist=ωW-ωshoulder-ωelbow
步骤四:根据具体的关节布置,将生成的运动映射回机械臂的关节空间上。
在忽略机械臂的具体关节布置的前提下,步骤一到步骤四已经完全确定了机械臂的运动。根据机械臂的具体关节布置,将该运动映射回机械臂的关节空间上,以便于机械臂执行所需的规划任务。
所述的运动求解方法在步骤二与步骤三之间,可以添加一个可选的步骤,用于优化机械臂的运动。
机械臂具有7个自由度,对于只有6个自由度的任务需求而言,机械臂具有1个冗余的自由度,可以用于优化。根据额外的任务需要,如避障等,可以让机械臂的大臂和小臂绕肩-腕连线SW转动,即引入虚拟关节Δ-Aψ。这是机械臂的自运动,能调整机械臂的臂形,但不影响腕关节的线速度。
引入虚拟关节Δ-Aψ后,会影响肩关节的角速度ωshoulder,其来源变为:Δ-P,Δ-As,Δ-M,Δ-Aψ和Δ-S。
所述的运动求解方法先将操作空间内末端执行器的运动映射到仿人机械臂所对应的人类手臂的生理关节的运动上;然后将生理关节的运动映射到仿人机械臂的关节空间上用于执行。
所述的运动求解方法通过在肩关节处建立虚拟球型机器人来控制工具坐标系的位置,用球坐标来代替笛卡尔坐标描述工具坐标系的线速度需求。
所述的运动求解方法在步骤一中,虚拟关节Δ-P的旋转方向wP由末端工具的当前位置pT和线速度vT确定,wP垂直于肩-工具连线ST和vT所确定的平面;虚拟关节Δ-L的平移方向vT沿着肩-工具连线SW,vT垂直于wP;虚拟关节Δ-As的旋转方向wA,S与vT和wP保持正交关系。
所述的运动求解方法在步骤二中,虚拟关节Δ-M的旋转方向wM由腕关节的当前位置pW和剩余线速度的垂直分量确定,wM垂直于肩-腕连线SW和所确定的平面;虚拟关节Δ-L的平移方向vT沿着肩-工具连线SW,vT垂直于wP;虚拟关节Δ-S和Δ-E的旋转方向均垂直于大臂和小臂确定的平面,即与仿人机械臂的第4个关节的旋转轴平行。
所述的运动求解方法在步骤一确定了一组先导运动,即虚拟关节Δ-P和Δ-As对应的运动;步骤二及以下需要在保证先导运动的情况下进行。
所述的运动求解方法通过手臂瞬时的、绕肩-腕连线旋转的自运动来调整手臂的臂形。
一种仿人机械臂的构型控制方法,包含以下步骤:
通常,机械臂的动力学方程由关节量来描述,即有
式中:M表示广义质量,C表示离心力和科氏力项,N表示重力项,τ为机械臂的关节力矩;
步骤二、参数化冗余自由度,建立误差模型。
运动规划得到的运动实际包含了仿人机械臂的所有运动。采用臂形角ψ参数化仿人机械臂的冗余自由度。这样,对于机械臂的构型控制变成了对末端工具的位姿和臂形角的控制。利用球坐标来表示位置误差;姿态误差采用指数坐标来表示;臂形角误差为实际值和期望值之差。
步骤三、根据步骤一中的动力学方程和步骤二中的误差模型,对机械臂进行动力学控制。
根据新的动力学方程,基于计算力矩方法,设计出以下控制率
式中:分别表示末端期望的加速度(表示在球坐标上)和角加速度以及虚拟关节Δ-Aψ期望的加速度;ω为末端当前的角速度;ωd为末端期望的角速度;R为末端当前的姿态矩阵;Rd为末端期望的姿态矩阵;kp,χ,kp,ω,kp,ψ,kd,χ,kd,ω,kd,ψ为控制器的参数;χe,ζe,ψe分别为末端的位置误差(表示在球坐标上)和姿态误差以及臂形角的误差;为对应的速度误差。
所述的构型控制方法通过球坐标来表示机械臂末端的位置误差;对于末端位置的控制,是通过控制在肩关节处建立的虚拟球型机械臂的运动来实现的。
所述的构型控制方法通过瞬时的、绕肩-腕轴线转动的自运动来控制手臂的臂形角ψ。
本发明具有以下优点:
1、本发明提出的运动求解方法求得的运动与仿人机械臂的具体关节布置无关,对于具有不同关节布置、但是大臂和小臂臂长相同的仿人机械臂,能产生一致的运动。
2、本发明提出的运动求解方法保留了仿人机械臂的优化能力。在满足操作空间运动要求的前提条件下,还可以针对额外的任务需求,对机械臂的运动进行优化。
3、本发明提出的运动求解方法通过模仿人类处理类似问题的方式,将任务需求与机械臂的生理关节关联起来,因此,产生的运动具有一定的类人动作特性。
4、本发明提出的构型控制方法能控制仿人机械臂的完整运动,从而保证了实际执行的运动与规划出来的运动是一致的。
5、本发明提出的构型控制方法利用了运动求解方法中的理论,使得仿人机械臂在规划和调整自身运动时能保持前后一致性。
6、本发明提出的构型控制方法考虑了人类调整臂形的方式,通过绕肩-腕连线的自运动来控制手臂臂形,更符合人类的行为特点。
附图说明
图1:一种仿人机械臂的关节布置示意图;
图2:人类手臂的结构示意图;
图3:本发明提出的运动求解方法和传统广义逆方法的路线图;
图4:末端运动与生理关节的关联示意图;
图5:末端运动在人类手臂上的实现示意图;
图6:本发明提出的运动求解方法产生的末端轨迹和终止构型;
图7:传统广义逆方法产生的末端轨迹和终止构型;
图8:本发明提出的运动求解方法和传统广义逆方法在两种不同关节布置仿人机械臂上产生的臂形角变化曲线;
图9:仿人机械臂由运动规划到构型控制的流程图;
图10:仿人机械臂的误差模型;
图11:阶跃响应中末端位置的变化曲线;
图12:阶跃响应中末端姿态的变化曲线;
图13:阶跃响应中臂形角的变化曲线;
图14:本发明提出的构型控制方法跟踪圆弧轨迹的实际轨迹图;
图15:轨迹跟踪中末端位置误差的变化曲线;
图16:轨迹跟踪中末端姿态误差的变化曲线;
图17:轨迹跟踪中臂形角误差的变化曲线。
具体实施方式
下面结合附图对本发明进行详细说明。
如图3所示,运动求解的目的是为了找到对应操作空间运动的一组关节空间解。传统的广义逆方法是通过关节空间到操作空间的雅克比矩阵的广义逆来求解的。由于雅克比矩阵依赖于机械臂的关节布置,因此对于不同关节布置的仿人机械臂,这种方法无法保证运动规划的一致性。本发明提出的运动求解方法首先将操作空间中的运动映射到人类手臂上,然后映射回关节空间中。因为运动在人类手臂上已经完全确定,所以求解的运动是与仿人机械臂的实际关节布置是无关的,从而能得到一致的运动。
本发明提出的运动求解方法通过以下步骤实现:
步骤一:将操作空间中的任务需求与机械臂的生理关节关联起来。
图4所示为人类手臂在操作空间中画出一条轨迹。其中,S、E和W分别表示手臂的肩、肘和腕关节中心,T表示末端工具中心;{S}为肩关节坐标系,是手臂的基坐标系,它的X轴指向前方,Z轴竖直向上,如图2所示;{W}为腕关节坐标系,它的X轴指向手指伸直方向,Z轴垂直于手掌面;{T}为末端工具坐标系。人类在画线时,注意力聚焦在工具坐标系{T}上,一般希望能直接影响到工具坐标系的位置和姿态。因此,在肩关节处建立虚拟关节Δ-P,Δ-As和Δ-L,构成一个球型机器人,以控制工具坐标系的位置,如图3所示。其中,Δ-P和Δ-As为旋转副,它们的轴线均通过肩关节中心,用单向箭头表示;Δ-L为平移副,它的平移方向也通过肩关节中心,用双向箭头表示。ΠP表示工作平面,由肩-工具连线ST和末端的线速度vT确定。
Δ-P的轴线wP垂直于平面ΠP,
Δ-L的移动方向vL沿着肩-工具连线ST,
Δ-As的轴线wA,S同时垂直于w wP和v vL,即有
wA,S=wP×vL
虚拟关节Δ-P,Δ-As和Δ-L的参数确定后,可以建立它们的角速度与末端工具坐标系的线速度之间的关系。
同时,由于工具坐标系和腕关节坐标系的相对位姿是固定的,所以对工具坐标系的速度要求VT可以转换到对腕关节坐标系的速度要求VW上。于是有
式中,vW和ωW为腕关节坐标系的线速度和角速度;ωT为工具坐标系的期望角速度;pWT为工具坐标系和腕关节坐标系的相对位置,在基坐标系下表示。
这样,对于工作坐标系的任务需求与机械臂的生理关节关联了起来。
步骤二:对腕关节的线速度进行分解,确定肩关节和肘关节的运动。
保留步骤一中的肩关节处虚拟关节Δ-P和Δ-As的影响。Δ-P和Δ-As贡献了腕关节线速度vW的一部分,另一部分,即腕关节剩余线速度vW,rem,由大臂和小臂实现。
vW,rem=vW-(ωP+ωA,S)×SW
式中,ωP和ωA,S分别对应虚拟关节Δ-P和Δ-As的角速度;SW为从肩关节中心指向腕关节中心的矢量。
如图5所示,将剩余线速度vW,rem沿肩-腕连线SW进行分解。其中,垂直分量由肩关节处的虚拟关节Δ-M实现。虚拟关节Δ-M的轴线垂直于肩-腕连线SW和垂直分量所确定的平面,虚拟关节Δ-M的关节角速度为
式中,ωS和ωE分别为虚拟关节Δ-S和Δ-E的关节角速度;SE||表示从肩关节中心指向肘关节中心的矢量SE平行于SW的分量;EW为从肘关节中心指向腕关节中心的矢量;SW表示矢量SW对应的叉乘矩阵。
此时,仿人机械臂的肩关节和肘关节的角速度确定了。肩关节的角速度ωshoulder来源于:Δ-P,Δ-As,Δ-M和Δ-S;肘关节的角速度ωelbow来源于:Δ-E。根据角速度的叠加性,有
步骤三:确定腕关节的运动。
步骤一到步骤二确定了肩关节和肘关节的运动,实现了腕关节坐标系的线速度需求。腕关节坐标系的角速度需求ωW由肩、肘和腕三个关节共同决定。因此,排除肩关节和肘关节运动的影响,即可求得腕关节的运动。
步骤四:根据具体的关节布置,将生成的运动映射回机械臂的关节空间上。
在忽略机械臂的具体关节布置的前提下,步骤一到步骤四已经完全确定了机械臂的运动。根据机械臂的具体关节布置,将该运动映射回机械臂的关节空间上,以便于机械臂执行所需的规划任务。这是一个1-1映射。
式中,为仿人机械臂的关节角速度;JS,m为机械臂在肩关节处的3个关节到肩关节整体的角速度的雅克比矩阵;w4为机械臂的第4个关节在当前时刻下的轴线方向;JW,m为机械臂在腕关节处的3个关节到腕关节整体的角速度的雅克比矩阵。
在步骤二与步骤三之间,可以添加一个可选的步骤,以优化机械臂的运动。
机械臂具有7个自由度,对于只有6个自由度的任务需求而言,机械臂具有1个冗余的自由度,可以用于优化。根据额外的任务需要,如避障等,可以让机械臂的大臂和小臂绕肩-腕连线SW转动,即引入虚拟关节Δ-Aψ,如图5所示。这是机械臂的自运动,能调整机械臂的臂形,但不影响腕关节的线速度。
Δ-Aψ的轴线方向为
添加虚拟关节Δ-Aψ后,会影响肩关节的角速度。ωshoulder的来源变为:Δ-P,Δ-As,Δ-M,Δ-Aψ和Δ-S。因此,有
从而可以得到新的映射关系
图6和图7分别为通过本发明提出的运动求解方法和传统广义逆方法(即其中为机械臂的雅克比矩阵的Moore-Penrose广义逆,为机械臂的末端速度)进行圆弧轨迹规划得到的末端轨迹和终止时的构型。从图中可以看出,本发明提出的运动求解方法能实现圆弧路径规划。另外,传统广义逆方法得到的终止构型超出了人类手臂能到达的生理极限,而本发明提出的方法没有。因而,本发明提出的方法更适合进行拟人运动规划。图8为这两种方法应用于2种不同关节布置的仿人机械臂上所产生的臂形角变化曲线。可以看出,通过本发明提出的方法得到的臂形角变化曲线是完全重合的,即得到的机械臂的运动是完全一致的;而通过传统广义逆方法得到的臂形角变化曲线是不一样的,即它对于不同关节布置的仿人机械臂产生了不同的运动。
运动规划完成后,实际上得到了机械臂的完整运动。一些场景要求仿人机械臂能严格地执行规划的运动,即需要对机械臂进行构型控制,完全地跟踪规划的运动。例如,仿人机械臂在与人协作时,会在规划阶段完成避碰验证;为了确保无碰撞,机械臂需要严格按照规划的运动运行。图9所示为仿人机械臂从运动规划到构型控制的流程图。为了描述机械臂的完整构型,引入额外参数——臂形角ψ来参数化机械臂的冗余自由度。
机械臂的动力学方程为
式中,式中:M表示广义质量,C表示离心力和科氏力项,N表示重力项,τ为机械臂的关节力矩。
基于计算力矩法,可以设计控制率为
式中,u为新的控制输入。带入系统的动力学方程,可以得到闭环系统的动力学方程。于是有
下面分别对机械臂的位置、姿态和臂形角的控制器进行设计,然后合并为完整的构型控制器。
如图10所示,根据末端的期望位置pd和速度vd,可以确定虚拟关节Δ-P,Δ-As和Δ-L的参数。因而,实际位置p可以由一组球坐标表示,即有
lL=||ST||-||STd||
式中,sign(·)为符号函数,表示括号内变量的正负号;STd表示从肩关节中心指向工具坐标系期望位置的矢量;ST表示从肩关节中心指向工具坐标系当前位置的矢量;ST⊥表示矢量ST垂直于SW的分量;ST||表示矢量ST平行于SW的分量。
定义末端的位置和线速度误差为
χe=χ-χd=χ
则位置误差的动力学方程为
定义末端的姿态误差为
采用指数坐标来描述姿态,则有
ζe=log(Re)
式中,ζe为姿态误差矩阵Re对应的指数坐标。
对上式进行微分,可以得到指数坐标的导数和姿态误差的角速度之间的关系。
式中,J(ζe)为末端的角速度到指数坐标微分的雅克比矩阵;dexpRe表示Re对应的指数映射的微分。
根据姿态误差矩阵,可以得到姿态误差的角速度为
对上式进行微分,可以得到角加速度。
则姿态误差的动力学系统为
式中,kp,ω和kd,ω均为正定对角阵。以(ζe,ωe)为上述误差系统的状态,则姿态误差会稳定在(0,0)。
臂形角ψ为由大臂和小臂确定的人臂平面和过肩-腕连线SW的参考平面的夹角。定义臂形角为
对臂形角进行微分,可以得到它与仿人机械臂的关节空间的关系。
式中,Jψ为从关节空间到臂形角的雅克比矩阵,它是一个行向量。
定义臂形角误差为
ψe=ψ-ψd
则臂形角的速度误差为
展开上式,有
式中,a和b均为标量。
综合以上分析,可以设计新的控制输入u为
则完整的控制律为
在该控制率的作用下,闭环系统的误差会稳定在零点,即能完全跟踪仿人机械臂的构型变化。
图11、图12和图13为该控制器的阶跃响应曲线,其中图11对应位置,图12对应姿态,图13对应臂形角。从图中可以看出,机械臂最终会稳定在期望构型。图14为跟踪一条圆弧轨迹的实际末端轨迹图。实际轨迹几乎和期望轨迹重合,因此在该控制器的控制下,机械臂能够很好地跟踪期望轨迹。图15、16和17为跟踪圆弧轨迹过程中的误差。其中,图15对应位置误差,位置误差定义为实际末端位置到期望轨迹的距离;图16对应姿态误差,姿态误差定义为ζe的模;图17对应臂形角的误差。从图中可以看出,在跟踪过程中,误差较小,说明该控制器能很好地跟踪机械臂的构型变化。
Claims (7)
1.仿人机械臂的运动求解方法,包括以下几个步骤:
步骤一:将操作空间中的任务需求与机械臂的生理关节进行关联;
在肩关节处建立虚拟关节Δ-P,Δ-As和Δ-L,构成一个球型机器人,以控制工具坐标系的位置,其中,Δ-P和Δ-As为转动副,Δ-L为移动副,对工具坐标系的速度要求VT能够转换到对腕关节坐标系的速度要求VW上,则操作空间中的任务需求与机械臂的生理关节进行关联;
步骤二:对腕关节的线速度进行分解,确定肩关节和肘关节的运动;
腕关节线速度vW的一部分由Δ-P和Δ-As实现,剩余部分vW,rem由大臂和小臂实现,将剩余线速度vW,rem沿肩-腕连线SW进行分解,其中,垂直分量由肩关节处的虚拟关节Δ-M实现,平行分量由肩关节和肘关节处的虚拟关节Δ-S和Δ-E共同实现;
步骤三:确定腕关节的运动;
腕关节坐标系的角速度需求ωW由肩、肘和腕三个生理关节共同决定;因此,排除肩关节和肘关节运动的影响,得到腕关节的运动;
ωWrist=ωW-ωshoulder-ωelbow
式中:ωshoulder、ωelbow和ωWrist分别表示肩、肘和腕三个生理关节的角速度;
步骤四:根据具体的关节布置,将生成的运动映射回机械臂的关节空间上;
根据机械臂的具体关节布置,将该运动映射回机械臂的关节空间上,机械臂执行所需的规划任务。
2.根据权利要求1所述的仿人机械臂的运动求解方法,所述的步骤一中,具体为:
虚拟关节Δ-P的旋转方向wP由末端工具的当前位置pT和线速度vT确定,wP垂直于肩-工具连线ST和vT所确定的平面;虚拟关节Δ-L的平移方向vT沿着肩-工具连线ST,vT垂直于wP;虚拟关节Δ-As的旋转方向wA,S与vT和wP保持正交关系。
7.根据权利要求4所述的仿人机械臂的运动求解方法,所述的仿人机械臂的构型控制方法,包括以下几个步骤:
机械臂的动力学方程由关节量来描述,即有
式中:M表示广义质量,C表示离心力和科氏力项,N表示重力项,τ为机械臂的关节力矩;
步骤二、参数化冗余自由度,建立误差模型;
采用臂形角ψ参数化仿人机械臂的冗余自由度,采用球坐标表示位置误差;采用指数坐标表示姿态误差;臂形角误差为实际值和期望值之差;
步骤三、根据步骤一中的动力学方程和步骤二中的误差模型,对机械臂进行动力学控制;
基于计算力矩方法,根据新的动力学方程,获取控制率为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711443796.8A CN108241339B (zh) | 2017-12-27 | 2017-12-27 | 仿人机械臂的运动求解和构型控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711443796.8A CN108241339B (zh) | 2017-12-27 | 2017-12-27 | 仿人机械臂的运动求解和构型控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108241339A CN108241339A (zh) | 2018-07-03 |
CN108241339B true CN108241339B (zh) | 2020-09-04 |
Family
ID=62700592
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711443796.8A Active CN108241339B (zh) | 2017-12-27 | 2017-12-27 | 仿人机械臂的运动求解和构型控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108241339B (zh) |
Families Citing this family (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111345894B (zh) * | 2018-12-21 | 2022-08-02 | 上海微创医疗机器人(集团)股份有限公司 | 机械臂及手术机器人 |
CN110347162A (zh) * | 2019-07-29 | 2019-10-18 | 南京市晨枭软件技术有限公司 | 一种工业机器人路径自动修正的方法及系统 |
CN110487263A (zh) * | 2019-08-15 | 2019-11-22 | 北京致行慕远科技有限公司 | 可移动设备的坐标获取方法及装置 |
CN110464470B (zh) * | 2019-09-10 | 2021-07-09 | 深圳市精锋医疗科技有限公司 | 手术机器人及其臂体的控制方法、控制装置 |
CN110712203B (zh) * | 2019-09-26 | 2021-03-26 | 苏州苏相机器人智能装备有限公司 | 一种7自由度机械臂逆运动学求解算法 |
CN111002292B (zh) * | 2019-12-11 | 2021-04-16 | 南京邮电大学 | 基于相似性度量的机械臂仿人运动示教方法 |
CN113001537B (zh) * | 2019-12-20 | 2022-08-02 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置及终端设备 |
CN113084791B (zh) * | 2019-12-23 | 2022-09-23 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置及终端设备 |
CN111113419B (zh) * | 2019-12-27 | 2022-01-25 | 深圳市越疆科技有限公司 | Scara机器人机械参数的标定方法、装置及scara机器人 |
CN113119104B (zh) * | 2019-12-31 | 2022-08-05 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置、计算设备及系统 |
CN113967911B (zh) * | 2019-12-31 | 2023-06-13 | 浙江大学 | 基于末端工作空间的仿人机械臂的跟随控制方法及系统 |
CN111152220B (zh) * | 2019-12-31 | 2021-07-06 | 浙江大学 | 一种基于人机融合的机械臂控制方法 |
CN111687834B (zh) * | 2020-04-30 | 2023-06-02 | 广西科技大学 | 移动机械手冗余机械臂逆优先级阻抗控制系统及控制方法 |
CN111687832B (zh) * | 2020-04-30 | 2023-06-02 | 广西科技大学 | 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法 |
CN111687833B (zh) * | 2020-04-30 | 2023-06-02 | 广西科技大学 | 机械手逆优先级阻抗控制系统及控制方法 |
CN111687835B (zh) * | 2020-04-30 | 2023-06-02 | 广西科技大学 | 水下机械手冗余机械臂逆优先级阻抗控制系统及控制方法 |
CN113119120B (zh) * | 2021-03-30 | 2022-06-07 | 深圳市优必选科技股份有限公司 | 一种机器人的控制方法、装置及机器人 |
CN113070880B (zh) * | 2021-03-31 | 2022-09-09 | 深圳市优必选科技股份有限公司 | 运动控制方法、装置、机器人控制设备及可读存储介质 |
CN114442643B (zh) * | 2022-04-07 | 2022-07-08 | 遨博(江苏)机器人有限公司 | 机器人的组合路径的速度前瞻方法、装置 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103331756A (zh) * | 2013-06-04 | 2013-10-02 | 浙江工业大学 | 一种机械臂运动控制方法 |
CN104635762B (zh) * | 2015-01-13 | 2017-06-06 | 北京航空航天大学 | 一种面向srs仿人手臂的自运动角计算方法 |
CN104723340B (zh) * | 2015-03-07 | 2016-06-22 | 哈尔滨工业大学 | 基于连接和阻尼配置的柔性关节机械臂的阻抗控制方法 |
CN104866722B (zh) * | 2015-05-26 | 2018-09-28 | 宁波韦尔德斯凯勒智能科技有限公司 | 一种七轴工业机械臂的逆运动学求解方法 |
CN104999463B (zh) * | 2015-07-09 | 2017-03-01 | 哈尔滨工程大学 | 一种基于构形平面的冗余机械臂运动控制方法 |
CN106313049B (zh) * | 2016-10-08 | 2017-09-26 | 华中科技大学 | 一种仿人机械臂体感控制系统及控制方法 |
CN107066645A (zh) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | 一种七自由度偏置机械臂逆解方法 |
CN106737671B (zh) * | 2016-12-21 | 2019-04-19 | 西安科技大学 | 七自由度仿人机械臂的双层拟人运动规划方法 |
CN106584461B (zh) * | 2016-12-21 | 2019-02-15 | 西安科技大学 | 多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法 |
-
2017
- 2017-12-27 CN CN201711443796.8A patent/CN108241339B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN108241339A (zh) | 2018-07-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108241339B (zh) | 仿人机械臂的运动求解和构型控制方法 | |
CN111538949B (zh) | 冗余机器人逆运动学求解方法、装置和冗余机器人 | |
CN108068113B (zh) | 7-dof仿人臂飞行物体作业最小加速度轨迹优化 | |
Luo et al. | Real time human motion imitation of anthropomorphic dual arm robot based on Cartesian impedance control | |
CN108908347B (zh) | 一种面向冗余移动机械臂容错型重复运动规划方法 | |
Mukherjee et al. | Inverse kinematics of a NAO humanoid robot using kinect to track and imitate human motion | |
Patidar et al. | Survey of robotic arm and parameters | |
CN108227506A (zh) | 一种基于自适应最优化方法的机器人导纳控制系统 | |
KR102030141B1 (ko) | 로봇의 팔꿈치 제어시스템 및 그 제어방법 | |
CN115122325A (zh) | 一种具有视场约束的拟人化机械手鲁棒视觉伺服控制方法 | |
CN111208730B (zh) | 一种快速终端滑模阻抗控制算法 | |
Žlajpah | On orientation control of functional redundant robots | |
CN111496783B (zh) | 一种6r工业机器人逆运动学求解方法 | |
Takamatsu et al. | Learning-from-observation system considering hardware-level reusability | |
Yu et al. | Visual servoing with quick eye-vergence to enhance trackability and stability | |
Sekimoto et al. | Experimental study on reaching movements of robot arms with redundant DOFs based upon virtual spring-damper hypothesis | |
Murakami et al. | Motion planning for catching a light-weight ball with high-speed visual feedback | |
Lin et al. | The arm planning with dynamic movement primitive for humanoid service robot | |
Rasch et al. | Combining cartesian trajectories with joint constraints for human-like robot-human handover | |
Hwang et al. | Human interface, automatic planning, and control of a humanoid robot | |
Li et al. | On perpendicular curve-based task space trajectory tracking control with incomplete orientation constraint | |
CN109571483B (zh) | 一种空间机械臂任务轨迹规划域构建方法 | |
Xu et al. | A Novel Robot Teaching System Based on Mixed Reality | |
Li et al. | Intelligent robotic arm for human pose recognition based on teleoperation system | |
Fratu et al. | Robots collision avoidance using learning through imitation |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |