CN112060098A - 主从随动力反馈控制方法、装置、计算机设备和存储介质 - Google Patents

主从随动力反馈控制方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN112060098A
CN112060098A CN202011278283.8A CN202011278283A CN112060098A CN 112060098 A CN112060098 A CN 112060098A CN 202011278283 A CN202011278283 A CN 202011278283A CN 112060098 A CN112060098 A CN 112060098A
Authority
CN
China
Prior art keywords
joint
slave
motor
torque
theta
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.)
Granted
Application number
CN202011278283.8A
Other languages
English (en)
Other versions
CN112060098B (zh
Inventor
金杰峰
来建良
金丁灿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Smart Technology Co ltd
Original Assignee
Hangzhou Smart Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hangzhou Smart Technology Co ltd filed Critical Hangzhou Smart Technology Co ltd
Priority to CN202011278283.8A priority Critical patent/CN112060098B/zh
Publication of CN112060098A publication Critical patent/CN112060098A/zh
Application granted granted Critical
Publication of CN112060098B publication Critical patent/CN112060098B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J3/00Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1679Programme controls characterised by the tasks executed

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本申请涉及一种主从随动力反馈控制方法、装置、计算机设备和存储介质,方法包括:获取从设备各关节对应的电机的实际电机扭矩和从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵;根据从设备各关节对应的电机的实际电机扭矩和转换矩阵,确定从设备各关节的实际关节扭矩;确定从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;根据从设备各关节的实际关节扭矩和初始理论关节扭矩,确定从设备各关节因从设备的末端负载引起的关节扭矩增量;根据从设备各关节因从设备的末端负载引起的关节扭矩增量,对从设备对应的主设备进行力反馈控制。本申请实现了核工业环境中的主设备对随动从设备的力反馈控制。

Description

主从随动力反馈控制方法、装置、计算机设备和存储介质
技术领域
本申请涉及工控技术领域,特别是涉及一种主从随动力反馈控制方法、装置、计算机设备和存储介质。
背景技术
在核工业中,许多工作要在密闭的箱室中进行。为防止核辐射泄漏,箱室内各种设备的移动、操作和检验维修需要以远程方式实现,远程遥控操作技术大幅减轻操作人员的辐照伤害与操作疲劳。然而,核工业箱室中常常充满辐射,由于辐射的穿透性与危害性,需150mm以上的铅层才能完全屏蔽,因此使用电子元器件的设备无法在该环境中使用,如具有电机内置结构的传统设备、通过内部机械力矩传感器获取关节力矩与末端碰撞力的设备。
现有的相关力控制方法多采用控制系统根据各个参数采用D-H参数法和雅克比矩阵法对电随动主从手的位置、速度以及力进行映射与转换,实现电随动从手对电随动机械主手的精确力反馈。这类方法不适用于在核辐射环境中,特别是多级齿轮传递结构下进行力反馈控制。
发明内容
本申请实施例提供了一种主从随动力反馈控制方法、装置、计算机设备和存储介质,以至少解决相关技术中力反馈控制方法在核辐射环境中不适用的问题。
第一方面,本申请实施例提供了一种主从随动力反馈控制方法,包括:
获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动;
根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩;
确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;
根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量;
根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
在其中一些实施例中,获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,包括:
根据所述从设备各关节的差动耦合结构确定各关节的传动关系;
基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,并根据所述自驱转角aθn和所述补偿转角bθn计算对应电机的实际转角Rθn
根据各关节电机的实际转角Rθn和关节转角θn,计算得到转换矩阵。
在其中一些实施例中,当关节对应的电机齿轮水平联动时,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:
当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角为aθn=in×θn+in×θn-1,其他各关节对应的电机的补偿值为in×θn-2,补偿转角为bθn=in×θn-2+……+in×θ1,n-2≥0,in为电机n与上级齿轮的传动比;
当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角为aθn= in×θn+1-in×θn,其他各关节对应的电机的补偿值为in×θn-1,补偿转角为bθn=in×θn-1+……+in×θ1,n-1≥0。
在其中一些实施例中,当关节对应的电机齿轮垂直联动时,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:
当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角为aθn=(in×θn+in×θn-1)×K,其他各关节对应的电机的补偿值为in×θn-2×K,补偿转角为bθn=(in×θn-2+……+in×θ1)×K,n-2≥0,in为电机n与上级齿轮的传动比,K为竖直齿轮与水平齿轮的传动比;
当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角为aθn=(in×θn+1-in×θn)×K,其他各关节对应的电机的补偿值为in×θn-1×K,补偿转角为bθn=(in×θn-1+……+in×θ1)×K,n-1≥0。
在其中一些实施例中,当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;
当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负。
在其中一些实施例中,根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩包括:
将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积,得到对应关节的实际关节扭矩。
在其中一些实施例中,确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩,包括:
获取从设备各关节之间的位置变化矩阵;
基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵,计算得到从设备各关节的初始理论关节扭矩。
在其中一些实施例中,根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,包括:
将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值,确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。
在其中一些实施例中,根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制,包括:
将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较,得到关节扭矩增量偏差;
当所述关节扭矩增量偏差大于第一预设阈值时,根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值,并根据所述实际负载值调整所述预设关节扭矩增量;
当所述关节扭矩增量偏差小于第二预设阈值时,基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。
第二方面,本申请实施例提供了一种主从随动力反馈控制装置,包括:
转换矩阵获取单元,用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动;
实际关节扭矩确定单元,用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩;
初始理论关节扭矩确定单元,用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;
关节扭矩增量确定单元,用于根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量;
力反馈控制单元,用于根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
第三方面,本申请实施例提供了一种计算机设备,包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现如上述第一方面所述的主从随动力反馈控制方法。
第四方面,本申请实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上述第一方面所述的主从随动力反馈控制方法。
相比于相关技术,本申请实施例提供的主从随动力反馈控制方法,通过所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩进行分析计算,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,并根据所述关节扭矩增量对所述从设备对应的主设备进行力反馈控制,不涉及传感器信号的直接读取,实现了在充满放射性的核工业环境中的对具有齿轮差动耦合纯机械结构的随动从设备的力反馈控制。
本申请的一个或多个实施例的细节在以下附图和描述中提出,以使本申请的其他特征、目的和优点更加简明易懂。
附图说明
此处所说明的附图用来提供对本申请的进一步理解,构成本申请的一部分,本申请的示意性实施例及其说明用于解释本申请,并不构成对本申请的不当限定。在附图中:
图1是本申请实施例提供的主从随动力反馈控制方法的应用场景示意图;
图2是本申请实施例中从设备的传动结构原理图;
图3是本申请其中一个实施例中主从随动力反馈控制方法的流程示意图;
图4是本申请其中一个实施例中主从随动力反馈控制原理框图;
图5是本申请其中一个实施例中主从随动力反馈控制方法的流程框图;
图6是本申请其中一个实施例中主从随动力反馈控制装置的结构框图;
图7是本申请其中一个实施例中计算机设备的结构示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行描述和说明。应当理解,此处所描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。基于本申请提供的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本申请保护的范围。
显而易见地,下面描述中的附图仅仅是本申请的一些示例或实施例,对于本领域的普通技术人员而言,在不付出创造性劳动的前提下,还可以根据这些附图将本申请应用于其他类似情景。此外,还可以理解的是,虽然这种开发过程中所作出的努力可能是复杂并且冗长的,然而对于与本申请公开的内容相关的本领域的普通技术人员而言,在本申请揭露的技术内容的基础上进行的一些设计,制造或者生产等变更只是常规的技术手段,不应当理解为本申请公开的内容不充分。
在本申请中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是,本申请所描述的实施例在不冲突的情况下,可以与其它实施例相结合。
除非另作定义,本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制,可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形,意图在于覆盖不排他的包含;例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可以还包括没有列出的步骤或单元,或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接,而是可以包括电气的连接,不管是直接的还是间接的。本申请所涉及的“多个”是指两个或两个以上。“和/或”描述关联对象的关联关系,表示可以存在三种关系,例如,“A和/或B”可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象,不代表针对对象的特定排序。
本实施例提供的主从随动力反馈控制方法可应用于核工业中,图1是本实施例提供的主从随动力反馈控制方法的应用场景示意图。如图1所示,主从随动结构包括主设备10和从设备11,所述从设备11设置于屏蔽室12内,所述从设备11各关节对应的电机设置于外置电机箱13内,所述外置电机箱13设置于所述屏蔽室12外面。所述外置电机箱13通过信号通讯线缆14与所述主设备10以及控制柜15连接。其中,所述主设备和所述从设备有相同和相似结构,通过对主设备关节转动信号输入,来控制屏蔽室12内的从设备进行对应关节的转动执行,并将从设备负载作用于从设备的各关节扭矩通过信号通讯线缆14反馈至主设备。
示例性的,图2是本申请实施例从设备的传动结构原理图,如图2所示,从设备包括七个关节,七个关节分别为第一关节111、第二关节112、第三关节113、第四关节114、第五关节115、第六关节116和第七关节117,它们的运动方向如图1-图2中1#、2#、3#、4#、5#、6#和7#箭头所示,每个关节由相对应的电机进行驱动,七个关节对应的七个电机置于外置电机箱13内,分别为第一电机1c、第二电机2c、第三电机3c、第四电机4c、第五电机5c、第六电机6c和第七电机7c。其中,所述第一关节至所述第六关节6个自由度的驱动力,均由外置电机箱13内对应的电机通过多级齿轮差动耦合的方式传递至各个关节,第七电机控制夹爪开合。
本实施例提供了一种主从随动力反馈控制方法。图3是根据本申请实施例的主从随动力反馈控制方法的流程图,如图3所示,该流程包括如下步骤:
步骤S201,获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动。
在本实施例中,作业过程中,主设备的关节转动动力来自于关节内置电机,关节转动信号直接由关节内置电机直接输出至所述从设备。所述从设备的关节由外置电机箱内对应的电机通过多级齿轮差动耦合传递至所述屏蔽室内驱动关节转动。所述从设备的受力分布在每个关节上,各关节的实际关节扭矩由对应的实际电机扭矩经转换矩阵进行耦合计算后获得。其中,所述转换矩阵由实际的齿轮布局确定,可以通过齿轮差动耦合下,电机与对应关节的传动关系计算确定。
步骤S202,根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩。
在本实施例中基于所述转换矩阵,可以得到从设备实际电机扭矩与实际关节扭矩之间的转换算法。通过采集所述从设备各关节对应关节的实际电机扭矩,与所述转换矩阵进行关系换算,可以实现在齿轮差动耦合下的力矩传递求解。
步骤S203,确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩。
步骤S204,根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。
在本实施例中,从设备齿轮差动耦合产生多重齿隙叠加导致各关节的实际关节扭矩和理论关节扭矩产生偏差。通过获取从设备在空载状态下因自重产生的各关节的初始理论关节扭矩,并与所述实际关节扭矩进行比对,可以得到复杂齿轮耦合关系下由从设备的末端负载引起的关节扭矩增量。其中,将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值,确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。
步骤S205,根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
在本实施例中,将从设备各关节的实际关节扭矩,除去空载状态下因自重产生的各关节的初始理论关节扭矩得到末端负载引起的关节扭矩增量后,可以将该值通过信号通讯线缆传输至所述从设备对应的主设备,并基于同构控制原理,对应所述主设备的各个关节扭矩进行力矩叠加,使得操作人员感测到主设备各个关节扭矩,得到临场操作手感。
综上,本申请实施例提供的主从随动力反馈控制方法,通过所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩进行分析计算,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,并根据所述关节扭矩增量对所述从设备对应的主设备进行力反馈控制,不涉及传感器信号的直接读取,实现了在充满放射性的核工业环境中的对具有齿轮差动耦合纯机械结构的随动从设备的力反馈控制。
下面通过优选实施例对本申请实施例进行描述和说明。
在上述实施例的基础上,在一个优选实施例中,获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,包括:
步骤S2011,根据所述从设备各关节的差动耦合结构确定各关节的传动关系;
步骤S2012,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,并根据所述自驱转角aθn和所述补偿转角bθn计算对应电机的实际转角Rθn
在本实施例中,首先根据从设备各关节的差动耦合结构确定各关节的传动关系。其中,竖直齿轮与水平齿轮之间的传动比为K,电机n与上级齿轮的传动比为in,则
当第n个关节轴由上级齿轮直接驱动时,自驱转角aθn=in×θn,补偿转角bθn=in×θn-1+……+in×θ1
当关节对应的电机齿轮水平联动时,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=in×θn+in×θn-1,其他各关节对应的电机的补偿值为in×θn-2,补偿转角bθn=in×θn-2+……+in×θ1,n-2≥0;当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角aθn= in×θn+1-in×θn,其他各关节对应的电机的补偿值为in×θn-1,补偿转角bθn=in×θn-1+……+in×θ1,n-1≥0。其中,当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负。
如图2所示,在一种具体的实施方式中,以从设备七个关节,说明从设备各关节对应电机的实际转角Rθn的计算过程:
计算第一关节111对应的第一电机1c的实际转动角度Rθ1
当如图2所示的第一关节111转动关节转角θ1时,上级齿轮组所对应的第七电机1c的自驱转角aθ1=i1×θ1,补偿转角bθn=in×θn-1+……+in×θ1,n-1≥1,由于第一关节111对应的齿轮为最上层齿轮,n-1=0,不满足补偿转角公式,故第一关节111转动关节转角θ1时对应的第一电机1c只需要旋转自驱转角aθ1=i1×θ1,即第一电机1c的实际转角Rθ1=i1×θ1,以实现第一关节111转动θ1
计算第二关节112对应第二电机2c的实际转动角度Rθ2
当如图2所示的第二关节112转动关节转角θ2时,第二上级齿轮由上级齿轮直接驱动,第二上级齿轮所对应的第二电机2c的自驱转角aθ2=i2×θ2,补偿转角bθn=in×θn-1+……+in×θ1,n-1≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负,第二电机2c的补偿转角bθ2=i2×θ1,即第二电机2c的实际转角Rθ2=i2×θ2+i2×θ1,以实现第二关节112转动θ2
计算第三关节113对应第三电机3c的实际转动角度Rθ3
当如图2所示的第三关节113转动关节转角θ3时,第三上级齿轮所对应的第三电机3c的自驱转角aθ3=i3×θ4-i3×θ3,补偿转角bθn=in×θn-1+……+in×θ1,n-1≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负,第三电机3c的补偿转角bθ3=-i3×θ2+i3×θ1,即第三电机3c的实际转角Rθ3=i3×θ4-i3×θ3-i3×θ2+i3×θ1,以实现第三关节113转动关节转角θ3
计算第四关节114对应第四电机4c的实际转动角度Rθ4
当如图2所示的第四关节114转动关节转角θ4时,第四上级齿轮所对应的第四电机4c的自驱转角aθ4=i4×θ4+i4×θ3,补偿转角bθn=in×θn-2+……+in×θ1,n-2≥0。当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负,第四电机4c的补偿转角bθ4=i4×θ2+i4×θ1,即第四电机4c的实际转角Rθ4=i4×θ4+i4×θ3+i4×θ2+i4×θ1,以实现第四关节114转动关节转角θ4
计算第五关节115对应第五电机5c的实际转动角度Rθ5,因均为上级齿轮异向差动摆动结构,第五电机5c的实际转动角度Rθ5的求解方法与第三电机3c的实际转角Rθ3相同。
计算第六关节116对应第六电机6c的实际转动角度Rθ6,同理,第六电机6c的实际转动角度Rθ6的求解方法与第四电机4c的实际转角Rθ4相同。
计算第七关节117对应第七电机7c的实际转动角度Rθ7
当如图2所示的第七关节117转动关节转角θ7时,第七上级齿轮所对应的第七电机7c的转角受到第六关节~第一关节的影响,因此,
7=i7×θ1+i7×θ2-i7×θ3-i7×θ4+i7×θ5+i7×θ6-i7×θ7
综上,电机n的实际转动角度Rθn为:
7=i7×θ1+i7×θ2-i7×θ3-i7×θ4+i7×θ5+i7×θ6-i7×θ7
6=i6×θ6-i6×θ5-i6×θ4-i6×θ3-i6×θ2-i6×θ1
5=i5×θ6-i5×θ5-i5×θ4+i5×θ3+i5×θ2-i5×θ1
4=i4×θ4+i4×θ3+i4×θ2+i4×θ1
3=i3×θ4-i3×θ3-i3×θ2+i3×θ1
2=i2×θ2+i2×θ1
1=i1×θ1
在另一种具体的实施方式中,当关节对应的电机齿轮垂直联动时,竖直齿轮与水平齿轮之间的传动比为K,则基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=(in×θn+in×θn-1)×K,其他各关节对应的电机的补偿值为in×θn-2×K,补偿转角bθn=(in×θn-2+……+in×θ1)×K,n-2≥0,K为竖直齿轮与水平齿轮的传动比;当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角aθn=(in×θn+1-in×θn)×K,其他各关节对应的电机的补偿值为in×θn-1×K,补偿转角bθn=(in×θn-1+……+in×θ1)×K,n-1≥0。可以理解,当关节对应的电机齿轮水平联动时,从设备各关节对应电机的实际转角Rθn的计算方法与电机齿轮水平联动时相同,本申请在此不做赘述。
需要说明的是,在本实施例中,从设备的关节和对应的电机可根据实际作业要求设置更多或更少的数量,所述实际转角Rθn的计算式可根据各关节之间的传动耦合关系进行适应性变形,本申请不作具体限定。
步骤S2013,根据各关节电机的实际转角Rθn和关节转角θn,计算得到转换矩阵。
在本实施例中,当计算得到从设备各电机的实际转角Rθn后,联立对应关节的关节转角θn,可以得到两者之间的转换矩阵,即转换矩阵A。具体可通过下式计算得到:
Rθ{7,6,5,4,3,2,1}=A*θ{7,6,5,4,3,2,1},求得A:
Figure DEST_PATH_IMAGE001
其中,Rθ{7,6,5,4,3,2,1}表示Rθn引入矩阵对应位置后的列向量;θ{7,6,5,4,3,2,1}表示θn引入θ{n,…2,1}矩阵对应位置后的列向量。
在上述实施例的基础上,在一个优选实施例中,根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩,包括:将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积,得到对应关节的实际关节扭矩。具体的,可通过下式计算得到对应关节的实际关节扭矩:
GM{7,6,5,4,3,2,1}=DM{7,6,5,4,3,2,1}*A,即GM{n}=DM{n}*A;
其中,GM{7,6,5,4,3,2,1}与DM{7,6,5,4,3,2,1}分别表示行向量。
在上述实施例的基础上,在一个优选实施例中,步骤S203,包括:
首先获取从设备各关节之间的位置变化矩阵,具体的,可通过下式计算得到各关节之间的位置变化矩阵R{n}:
Figure DEST_PATH_IMAGE002
其中,
Figure DEST_PATH_IMAGE003
可通过上述各关节对应的关节转角
Figure 611889DEST_PATH_IMAGE003
得到的关节转角序列,
Figure DEST_PATH_IMAGE004
为由D-H参数表得到的固定值。
然后基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵,计算得到从设备各关节的初始理论关节扭矩LGM{n}。具体的:
Figure 529029DEST_PATH_IMAGE005
其中,LGM{7}=0,f{7}=G{7};
R{n}表示
Figure 246450DEST_PATH_IMAGE003
Figure 641659DEST_PATH_IMAGE004
计算得到各关节之间的位置变化矩阵;
P{n,n+1}表示n+1坐标系原点在n坐标系的位置,坐标系可根据描述从设备各关节运动关系的D-H参数表建立;
r{n}表示从设备质心在当前关节n坐标系下空间位置的1×3向量;
G{n}表示从设备质心在当前关节n坐标系下所受力值的1×3向量;
G{n}= inv ( ∏ (R{1}*R{n} ))*m{n},m{n}(i=1~6)为根据从设备结构得到的1×3向量,m{7}为根据负载预设的多组1×3向量;
f{n}表示当前关节的坐标系所受力,即在1~n坐标系下的受力集合。
在上述实施例的基础上,在一个优选实施例中,步骤S205,包括:
步骤S2051,将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较,得到关节扭矩增量偏差。
在本实施例中,所述关节扭矩增量由从设备的预设负载引起的实际的关节扭矩增量值,所述预设关节扭矩增量是基于对应的预设关节转角输入值和预设理论负载值计算得到的理论的关节扭矩增量值,可根据预设条件计算得到。因此,将所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较得到的关节扭矩增量偏差,即由从设备末端预设负载引起的扭矩偏差。
步骤S2052,当所述关节扭矩增量偏差大于第一预设阈值时,根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值,并根据所述实际负载值调整所述预设关节扭矩增量;当所述关节扭矩增量偏差小于第二预设阈值时,基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。
在本实施例中,当获取到关节扭矩增量偏差后,需要判定所述关节扭矩增量偏差是否在预设的合理范围内。其中,所述第一预设阈值可以是根据预设负载计算得到的理论的关节扭矩增量偏差上限,所述第二预设阈值可以是根据预设负载计算得到的理论的关节扭矩增量偏差下限。
当所述关节扭矩增量偏差大于第一预设阈值时,表明所述关节扭矩增量超出了预设合理范围,此时从设备对应的负载改变或触碰到了障碍物,预设关节扭矩增量不合理,需要根据实际负载值调节所述预设关节扭矩增量,使得理论的关节扭矩增量向实际的关节扭矩增量趋近,减小力反馈误差。当所述关节扭矩增量偏差小于第二预设阈值时,表明所述关节扭矩增量在预设合理范围内且存在系统误差,需要所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。可选地,可以根据所述关节扭矩增量偏差获取反馈力和关节转角的补偿值发送至所述主设备进行反馈控制,使主设备根据所述反馈力和关节转角的补偿值对从设备进行运动补偿。可选地,所述补偿值可以是基于关节扭矩增量偏差得到的调试值或经验值,本申请不做具体限定。
如图4所示为本申请其中一个实施例提供的主从随动力反馈控制原理框图,如图4所示,主从随动力反馈控制原理框图包括主控模块31、通讯模块32和随动控制模块33,随动控制模块33包括力反馈控制模块34,主控模块是一个开环的力控制器,将从设备的反馈力作为输入,将自身的关节转角作为输出,从设备采用了PID位置闭环控制,将主设备的关节转角作为输入,将自身的反馈力作为输出。同时,在该PID位置闭环控制中引入了关节扭矩增量偏差e,将关节扭矩增量偏差e引入从设备系统刚度系数Ke。其中,所述从设备系统刚度系数Ke用于描述从设备实际关节转角θn与反馈力Fe之间的系统关系。通过调整系统刚度系数Ke改变从设备反馈力Fe,将从设备反馈力Fe传递给主设备,改变主设备的力反馈值,此时会改变从设备的关节角度输出值;同时引入了主设备输出的力经PID控制求解之后的环节,作为判定条件来保证从设备关节角度的稳定输出。该控制原理为本领域常用原理,本实施例和相关技术中的PID控制框图相比,主要区别包括引入了关节扭矩增量偏差e的判定。
图中标号含义如下:
θm、θs:电随动机械主手的关节转角输入与电随动机械从手的关节转角输出;
s:s指微分环节,1/s指积分环节,1/s²指两个积分环节的串联;
vm:电随动机械主手速度;
Cd:电随动机械主手阻尼系数;
Ke:电随动机械从手系统刚度系数;
Fh、Fe:电随动机械主手输入力与电随动机械从手反馈力;
Kp-p、Kf-f:位置前馈增益和力反馈增益;
Mm、Ms:电随动机械主手与电随动机械从手质量;
Kp、Ki、Kd:电随动机械从手的PID控制参数。
如图5所示,在上述实施例的基础上,在其中一个实施例中,主从随动力反馈控制方法包括以下步骤:
输入外部电随动机械主手的关节转动信号关节转角θn,然后根据同构控制原理,得到从设备的对应关节转角θn,然后根据各关节电机的实际转角Rθn和关节转角θn,计算得到转换矩阵。然后根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩。根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。当所述关节扭矩增量偏差大于第一预设阈值时,根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值,并根据所述实际负载值调整所述预设关节扭矩增量;当所述关节扭矩增量偏差小于第二预设阈值时,根据所述关节扭矩增量偏差获取反馈力和关节转角的补偿值发送至所述主设备进行反馈控制。
需要说明的是,在上述流程中或者附图的流程图中示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行,并且,虽然在流程图中示出了逻辑顺序,但是在某些情况下,可以以不同于此处的顺序执行所示出或描述的步骤。
本实施例还提供了一种主从随动力反馈控制装置,该装置用于实现上述实施例及优选实施方式,已经进行过说明的不再赘述。如以下所使用的,术语“模块”、“单元”、“子单元”等可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现,但是硬件,或者软件和硬件的组合的实现也是可能并被构想的。
图6是根据本申请实施例的主从随动力反馈控制装置的结构框图,如图6所示,该装置包括:转换矩阵获取单元301、实际关节扭矩确定单元302、初始理论关节扭矩确定单元303、关节扭矩增量确定单元304和力反馈控制单元305。
转换矩阵获取单元301,用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动;
实际关节扭矩确定单元302,用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩;
初始理论关节扭矩确定单元303,用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;
关节扭矩增量确定单元304,用于根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量;
力反馈控制单元305,用于根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
在其中一个实施例中,转换矩阵获取单元301,包括:传动关系获取模块、转角获取模块和转换矩阵计算模块。
传动关系获取模块,用于根据所述从设备各关节的差动耦合结构确定各关节的传动关系;
转角获取模块,用于基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,并根据所述自驱转角aθn和所述补偿转角bθn计算对应电机的实际转角Rθn
转换矩阵计算模块,用于根据各关节电机的实际转角Rθn和关节转角θn,计算得到转换矩阵。
在其中一个实施例中,当关节对应的电机齿轮水平联动时,转角获取模块包括:第一计算模块和第二计算模块。
第一计算模块,用于当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=in×θn+in×θn-1,其他各关节对应的电机补偿值为in×θn-2,补偿转角bθn=in×θn-2+……+in×θ1,n-2≥0,in为电机n与上级齿轮的传动比;
第二计算模块,用于当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角aθn= in×θn+1-in×θn,其他各关节对应的电机补偿值为in×θn-1,补偿转角bθn=in×θn-1+……+in×θ1,n-1≥0。
在其中一个实施例中,当关节对应的电机齿轮垂直联动时,转角获取模块还包括:第三计算模块和第四计算模块。
第三计算模块,用于当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=(in×θn+in×θn-1)×K,其他各关节对应的电机的补偿值为in×θn-2×K,补偿转角bθn=(in×θn-2+……+in×θ1)×K,n-2≥0,in为电机n与上级齿轮的传动比,K为竖直齿轮与水平齿轮的传动比;
第四计算模块,用于当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角aθn=(in×θn+1-in×θn)×K,其他各关节对应的电机的补偿值为in×θn-1×K,补偿转角bθn=(in×θn-1+……+in×θ1)×K,n-1≥0。
在其中一个实施例中,当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;
当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负。
在其中一个实施例中,实际关节扭矩确定单元302,具体用于:
将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积,得到对应关节的实际关节扭矩。
在其中一个实施例中,初始理论关节扭矩确定单元303,包括:位置变化矩阵获取模块和初始理论关节扭矩计算模块。
位置变化矩阵获取模块,用于获取从设备各关节之间的位置变化矩阵;
初始理论关节扭矩计算模块,用于基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵,计算得到从设备各关节的初始理论关节扭矩。
在其中一个实施例中,关节扭矩增量确定单元304具体用于:
将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值,确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。
在其中一个实施例中,力反馈控制单元305,包括:关节扭矩增量偏差获取模块、第一判断模块和第二判断模块。
关节扭矩增量偏差获取模块,用于将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较,得到关节扭矩增量偏差;
第一判断模块,用于当所述关节扭矩增量偏差大于第一预设阈值时,根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值,并根据所述实际负载值调整所述预设关节扭矩增量;
第二判断模块,用于当所述关节扭矩增量偏差小于第二预设阈值时,基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。
需要说明的是,上述各个模块可以是功能模块也可以是程序模块,既可以通过软件来实现,也可以通过硬件来实现。对于通过硬件来实现的模块而言,上述各个模块可以位于同一处理器中;或者上述各个模块还可以按照任意组合的形式分别位于不同的处理器中。
另外,结合图3描述的本申请实施例主从随动力反馈控制方法可以由计算机设备来实现。图7为根据本申请实施例的计算机设备的硬件结构示意图。
计算机设备可以包括处理器71以及存储有计算机程序指令的存储器72。
具体地,上述处理器71可以包括中央处理器(CPU),或者特定集成电路(Application Specific Integrated Circuit,简称为ASIC),或者可以被配置成实施本申请实施例的一个或多个集成电路。
其中,存储器72可以包括用于数据或指令的大容量存储器。举例来说而非限制,存储器72可包括硬盘驱动器(Hard Disk Drive,简称为HDD)、软盘驱动器、固态驱动器(SolidState Drive,简称为SSD)、闪存、光盘、磁光盘、磁带或通用串行总线(Universal SerialBus,简称为USB)驱动器或者两个或更多个以上这些的组合。在合适的情况下,存储器72可包括可移除或不可移除(或固定)的介质。在合适的情况下,存储器72可在数据处理装置的内部或外部。在特定实施例中,存储器72是非易失性(Non-Volatile)存储器。在特定实施例中,存储器72包括只读存储器(Read-Only Memory,简称为ROM)和随机存取存储器(RandomAccess Memory,简称为RAM)。在合适的情况下,该ROM可以是掩模编程的ROM、可编程ROM(Programmable Read-Only Memory,简称为PROM)、可擦除PROM(Erasable ProgrammableRead-Only Memory,简称为EPROM)、电可擦除PROM(Electrically Erasable ProgrammableRead-Only Memory,简称为EEPROM)、电可改写ROM(Electrically Alterable Read-OnlyMemory,简称为EAROM)或闪存(FLASH)或者两个或更多个以上这些的组合。在合适的情况下,该RAM可以是静态随机存取存储器(Static Random-Access Memory,简称为SRAM)或动态随机存取存储器(Dynamic Random Access Memory,简称为DRAM),其中,DRAM可以是快速页模式动态随机存取存储器(Fast Page Mode Dynamic Random Access Memory,简称为FPMDRAM)、扩展数据输出动态随机存取存储器(Extended Date Out Dynamic RandomAccess Memory,简称为EDODRAM)、同步动态随机存取内存(Synchronous Dynamic Random-Access Memory,简称SDRAM)等。
存储器72可以用来存储或者缓存需要处理和/或通信使用的各种数据文件,以及处理器71所执行的可能的计算机程序指令。
处理器71通过读取并执行存储器72中存储的计算机程序指令,以实现上述实施例中的任意一种主从随动力反馈控制方法。
在其中一些实施例中,计算机设备还可包括通信接口73和总线70。其中,如图7所示,处理器71、存储器72、通信接口73通过总线70连接并完成相互间的通信。
通信接口73用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口73还可以实现与其他部件例如:外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
总线70包括硬件、软件或两者,将计算机设备的部件彼此耦接在一起。总线70包括但不限于以下至少之一:数据总线(Data Bus)、地址总线(Address Bus)、控制总线(Control Bus)、扩展总线(Expansion Bus)、局部总线(Local Bus)。举例来说而非限制,总线70可包括图形加速接口(Accelerated Graphics Port,简称为AGP)或其他图形总线、增强工业标准架构(Extended Industry Standard Architecture,简称为EISA)总线、前端总线(Front Side Bus,简称为FSB)、超传输(Hyper Transport,简称为HT)互连、工业标准架构(Industry Standard Architecture,简称为ISA)总线、无线带宽(InfiniBand)互连、低引脚数(Low Pin Count,简称为LPC)总线、存储器总线、微信道架构(Micro ChannelArchitecture,简称为MCA)总线、外围组件互连(Peripheral Component Interconnect,简称为PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(Serial AdvancedTechnology Attachment,简称为SATA)总线、视频电子标准协会局部(Video ElectronicsStandards Association Local Bus,简称为VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下,总线70可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线,但本申请考虑任何合适的总线或互连。
该计算机设备可以基于获取到的程序指令,执行本申请实施例中的主从随动力反馈控制方法,从而实现结合图3描述的主从随动力反馈控制方法。
另外,结合上述实施例中的主从随动力反馈控制方法,本申请实施例可提供一种计算机可读存储介质来实现。该计算机可读存储介质上存储有计算机程序指令;该计算机程序指令被处理器执行时实现上述实施例中的任意一种主从随动力反馈控制方法。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。

Claims (12)

1.一种主从随动力反馈控制方法,其特征在于,包括:
获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动;
根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩;
确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;
根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量;
根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
2.根据权利要求1所述的主从随动力反馈控制方法,其特征在于,获取所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,包括:
根据所述从设备各关节的差动耦合结构确定各关节的传动关系;
基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,并根据所述自驱转角aθn和所述补偿转角bθn计算对应电机的实际转角Rθn
根据各关节电机的实际转角Rθn和关节转角θn,计算得到转换矩阵。
3.根据权利要求2所述的主从随动力反馈控制方法,其特征在于,当关节对应的电机齿轮水平联动时,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:
当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=in×θn+in×θn-1,其他各关节对应的电机的补偿值为in×θn-2,补偿转角为bθn=in×θn-2+……+in×θ1,n-2≥0,in为电机n与上级齿轮的传动比;
当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角 aθn=in×θn+1-in×θn,其他各关节对应的电机的补偿值为in×θn-1,补偿转角为bθn=in×θn-1+……+in×θ1,n-1≥0。
4.根据权利要求2所述的主从随动力反馈控制方法,其特征在于,当关节对应的电机齿轮垂直联动时,基于所述传动关系,确定各关节对应的电机的自驱转角aθn和补偿转角bθn,包括:
当第n个关节电机对应的关节由上级齿轮同向差动驱动转动时,自驱转角aθn=(in×θn+in×θn-1)×K,其他各关节对应的电机的补偿值为in×θn-2×K,补偿转角bθn=(in×θn-2+……+in×θ1)×K,n-2≥0,in为电机n与上级齿轮的传动比,K为竖直齿轮与水平齿轮的传动比;
当第n个关节电机对应的关节由上级齿轮异向差动驱动摆动时,自驱转角aθn=(in×θn+1-in×θn)×K,其他各关节对应的电机的补偿值为in×θn-1×K,补偿转角bθn=(in×θn-1+……+in×θ1)×K,n-1≥0。
5.根据权利要求2所述的主从随动力反馈控制方法,其特征在于,
当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮相同时,当前关节对应的电机的补偿转角bθn取正;
当引起当前齿轮转动的上级齿轮的转动方向与当前齿轮不同时,当前关节对应的电机的补偿转角bθn取负。
6.根据权利要求1所述的主从随动力反馈控制方法,其特征在于,根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩包括:
将从设备各关节的实际电机扭矩与所述转换矩阵的逆矩阵求积,得到对应关节的实际关节扭矩。
7.根据权利要求1所述的主从随动力反馈控制方法,其特征在于,确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩,包括:
获取从设备各关节之间的位置变化矩阵;
基于从设备各关节的坐标系、各关节的受力集合、从设备质心在各关节的坐标系中的位置和受力集合以及所述位置变化矩阵,计算得到从设备各关节的初始理论关节扭矩。
8.根据权利要求1所述的主从随动力反馈控制方法,其特征在于,根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,包括:
将所述从设备各关节的实际关节扭矩与所述初始理论关节扭矩的差值,确定为所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量。
9.根据权利要求2所述的主从随动力反馈控制方法,其特征在于,根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制,包括:
将对应于所述从设备各关节的关节扭矩增量和对应末端负载的预设关节扭矩增量进行比较,得到关节扭矩增量偏差;
当所述关节扭矩增量偏差大于第一预设阈值时,根据所述关节扭矩增量偏差和预设负载力矩区间来确定实际负载值,并根据所述实际负载值调整所述预设关节扭矩增量;
当所述关节扭矩增量偏差小于第二预设阈值时,基于所述关节扭矩增量偏差对所述从设备各关节进行力反馈控制。
10.一种主从随动力反馈控制装置,其特征在于,包括:
转换矩阵获取单元,用于获取从设备各关节对应的电机的实际电机扭矩和所述从设备的各关节对应的电机与相应关节之间的转动角度的转换矩阵,其中,所述从设备采用齿轮差动耦合的方式控制各关节的转动;
实际关节扭矩确定单元,用于根据所述从设备各关节对应的电机的实际电机扭矩和所述转换矩阵,确定所述从设备各关节的实际关节扭矩;
初始理论关节扭矩确定单元,用于确定所述从设备在空载状态下因自重产生的各关节的初始理论关节扭矩;
关节扭矩增量确定单元,用于根据所述从设备各关节的实际关节扭矩和所述初始理论关节扭矩,确定所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量;
力反馈控制单元,用于根据所述从设备各关节因所述从设备的末端负载引起的关节扭矩增量,对所述从设备对应的主设备进行力反馈控制。
11.一种计算机设备,包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至9中任一项所述的主从随动力反馈控制方法。
12.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1至9中任一项所述的主从随动力反馈控制方法。
CN202011278283.8A 2020-11-16 2020-11-16 主从随动力反馈控制方法、装置、计算机设备和存储介质 Active CN112060098B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011278283.8A CN112060098B (zh) 2020-11-16 2020-11-16 主从随动力反馈控制方法、装置、计算机设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011278283.8A CN112060098B (zh) 2020-11-16 2020-11-16 主从随动力反馈控制方法、装置、计算机设备和存储介质

Publications (2)

Publication Number Publication Date
CN112060098A true CN112060098A (zh) 2020-12-11
CN112060098B CN112060098B (zh) 2021-03-02

Family

ID=73655409

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011278283.8A Active CN112060098B (zh) 2020-11-16 2020-11-16 主从随动力反馈控制方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN112060098B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113084816A (zh) * 2021-04-14 2021-07-09 中国核电工程有限公司 新型智能化主从机械手力反馈控制方法及系统
CN113442118A (zh) * 2021-06-30 2021-09-28 山东大学 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN115302506A (zh) * 2022-08-08 2022-11-08 科大讯飞股份有限公司 关节模组的测试方法、装置、设备和存储介质
CN117565063A (zh) * 2024-01-16 2024-02-20 泓浒(苏州)半导体科技有限公司 晶圆搬运机械手传动部件磨损自适应定位补偿方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102819272A (zh) * 2012-08-24 2012-12-12 广东工业大学 一种遥操作机器人力反馈控制系统及控制方法
WO2015119908A1 (en) * 2014-02-07 2015-08-13 Control Interfaces LLC Remotely operated manipulator and rov control systems and methods
CN105573249A (zh) * 2014-10-29 2016-05-11 发那科株式会社 具备用于确认负荷信息的设定的功能的控制装置
CN111604874A (zh) * 2020-05-22 2020-09-01 中国核工业电机运行技术开发有限公司 一种主从机械手的力反馈主手
CN111604875A (zh) * 2020-05-22 2020-09-01 中国核工业电机运行技术开发有限公司 一种辐照环境下安装的力反馈主从随动机械手系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102819272A (zh) * 2012-08-24 2012-12-12 广东工业大学 一种遥操作机器人力反馈控制系统及控制方法
WO2015119908A1 (en) * 2014-02-07 2015-08-13 Control Interfaces LLC Remotely operated manipulator and rov control systems and methods
CN105573249A (zh) * 2014-10-29 2016-05-11 发那科株式会社 具备用于确认负荷信息的设定的功能的控制装置
CN111604874A (zh) * 2020-05-22 2020-09-01 中国核工业电机运行技术开发有限公司 一种主从机械手的力反馈主手
CN111604875A (zh) * 2020-05-22 2020-09-01 中国核工业电机运行技术开发有限公司 一种辐照环境下安装的力反馈主从随动机械手系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张国伟等: "核工业电随动主从机械手控制系统研制", 《仪器仪表学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113084816A (zh) * 2021-04-14 2021-07-09 中国核电工程有限公司 新型智能化主从机械手力反馈控制方法及系统
CN113442118A (zh) * 2021-06-30 2021-09-28 山东大学 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN113442118B (zh) * 2021-06-30 2022-12-09 山东大学 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN115302506A (zh) * 2022-08-08 2022-11-08 科大讯飞股份有限公司 关节模组的测试方法、装置、设备和存储介质
CN117565063A (zh) * 2024-01-16 2024-02-20 泓浒(苏州)半导体科技有限公司 晶圆搬运机械手传动部件磨损自适应定位补偿方法及系统
CN117565063B (zh) * 2024-01-16 2024-03-29 泓浒(苏州)半导体科技有限公司 晶圆搬运机械手传动部件磨损自适应定位补偿方法及系统

Also Published As

Publication number Publication date
CN112060098B (zh) 2021-03-02

Similar Documents

Publication Publication Date Title
CN112060098B (zh) 主从随动力反馈控制方法、装置、计算机设备和存储介质
CN108568814B (zh) 机器人以及机器人的控制方法
Kanade et al. Real-time control of CMU direct-drive arm II using customized inverse dynamics
CN113799136B (zh) 一种基于全状态反馈的机器人关节高精度控制系统及方法
CN112894802B (zh) 多级并联手术机械臂的控制方法及多级并联手术机械臂
CN104182577B (zh) 一种行李箱盖气弹簧四连杆铰链设计方法
CN109648564B (zh) 一种基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法
CN107292005B (zh) 一种基于模态解耦技术的轨道交通变流器振动优化方法
CN111610721A (zh) 模型参数完全未知的带负载四旋翼无人机速度控制方法
CN112528434B (zh) 信息识别方法、装置、电子设备和存储介质
CN106549608A (zh) 马达控制装置
CN109213184A (zh) 挠性航天器的有限时间多模态滑模姿态控制算法
CN117428791B (zh) 一种用于肩部四轴康复机器人的逆运动学求解方法及系统
CN112077853B (zh) 主从随动系统的力反馈方法、装置以及主从随动系统
Zhao et al. Symmetrical characteristics of the workspace for spatial parallel mechanisms with symmetric structure
CN112091981B (zh) 主从运动映射方法和系统
CN115157274B (zh) 一种滑模控制的机械臂系统及其滑模控制方法
CN110569582B (zh) 一种用于天线的二维指向机构的指向精度计算方法及装置
Huang et al. Adjustable inertia ratio control for low-frequency torsional resonance suppression in industrial servo system
CN114310874A (zh) 协作机器人关节力矩控制方法、系统及计算机设备
Xin et al. A Vibration Suppression Controller Design for a Two-Link Manipulator with Flexible Joints
CN112192538B (zh) 主从运动映射方法和系统
JP2022002422A (ja) モータユニット、及びモータユニットの制御方法
Hovland et al. Benchmark of the 3-dof gantry-tau parallel kinematic machine
CN113997282B (zh) 机械臂控制方法、装置、电子装置和存储介质

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