CN112318492B - 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法 - Google Patents

一种绳驱蛇形机械臂及其绳索故障状态下的控制方法 Download PDF

Info

Publication number
CN112318492B
CN112318492B CN202011090130.0A CN202011090130A CN112318492B CN 112318492 B CN112318492 B CN 112318492B CN 202011090130 A CN202011090130 A CN 202011090130A CN 112318492 B CN112318492 B CN 112318492B
Authority
CN
China
Prior art keywords
rope
length
mechanical arm
met
failure condition
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
Application number
CN202011090130.0A
Other languages
English (en)
Other versions
CN112318492A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202011090130.0A priority Critical patent/CN112318492B/zh
Publication of CN112318492A publication Critical patent/CN112318492A/zh
Application granted granted Critical
Publication of CN112318492B publication Critical patent/CN112318492B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
    • 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/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B25J9/065—Snake robots
    • 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
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • 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/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/1625—Truss-manipulator for snake-like motion
    • 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/1628—Programme controls characterised by the control loop

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Transmission Devices (AREA)

Abstract

本发明公开了一种绳驱蛇形机械臂绳索故障状态下的控制方法。列举绳驱蛇形机械臂运动方程；判断绳索出现故障否，若出现则跳转至确定绳索编号，若没出现则结束；确定绳索编号后判断绳索破断否，若未破断则求解绳索长度后信号故障；若破断则判断绳索卡滞否，若卡滞则求解绳索长度，若未卡滞则信号故障；根据绳索卡滞求解绳索长度后则信号故障结束；避障求解过程值，并判断是否满足条件1，若满足则动力不足结束，若不满足则判断是否满足条件2，若满足则动力不足结束，若不满足则判断是否满足条件3，若满足则运动干涉结束，若不满足则正常运动结束。本发明为最大程度地发挥机械臂的性能，更好地利用绳驱蛇形机械臂的冗余特性。

Description

一种绳驱蛇形机械臂及其绳索故障状态下的控制方法
技术领域
本发明属于蛇形机械臂领域；具体涉及一种绳驱蛇形机械臂及其绳索故障状态下的控制方法。
背景技术
绳驱蛇形机械臂是使用绳索驱动的超冗余机械臂，具有自由度多，运动灵活，适应狭小空间环境的能力。但是绳索存在破断和卡滞的可能性极高，目前针对绳索卡滞和破断情况的处理方式是更换绳索，及其繁琐且增加成本。
发明内容
本发明提供了一种绳驱蛇形机械臂绳索故障状态下的控制方法，为最大程度地发挥机械臂的性能，更好地利用绳驱蛇形机械臂的冗余特性，开发出处故障状态下蛇形机械臂的运动控制方法，给出了绳索破断和卡滞情况下的处理方法，及对后期运动情况的预测。
本发明通过以下技术方案实现：
一种绳驱蛇形机械臂，所述绳驱蛇形机械臂包括控制柜1、支撑脚2、基座3和机械臂4，所述控制柜1的底端设置多个支撑脚2，所述控制柜1的上端设置基座3，所述基座3的前端设置机械臂4，
所述机械臂4包括绳索5、锁紧机构6、锥节下板7、连接柱8、万向节轴9和锥节上板10，所述锥节上板10与锥节下板7之间设置多个连接柱8与多根绳索5，所述锥节下板7上设置锁紧机构6，每一对所述锥节上板10、锥节下板7、锁紧机构6与锥节上板10和锥节下板7之间的绳索5和连接柱8组成一个机械臂关节，相邻两个所述机械臂关节之间通过万向轴9连接；
所述基座3上的电机12通过力传感器11与丝杠连接，所述丝杠的滑块安装在直线导轨13上，所述力传感器11的一端安装在丝杠的滑块上，所述绳索5固定在力传感器11的另一端。
一种绳驱蛇形机械臂绳索故障状态下的控制方法，所述控制方法包括以下步骤：
步骤1：基于绳驱蛇形机械臂列举绳驱蛇形机械臂运动方程；
步骤2：判断绳驱蛇形机械臂的绳索是否出现故障，若出现故障则跳转至步骤3，若没出现故障则跳转至步骤15；
步骤3：确定绳索编号后再进行步骤4；
步骤4：判断绳驱蛇形机械臂的绳索是否破断，若绳索破断则进行步骤6，若绳索并未破断进行步骤5；
步骤5：根据绳索破断求解绳索长度后再进行步骤8；
步骤6：判断绳驱蛇形机械臂的绳索是否卡滞，若绳索卡滞则进行步骤7，若绳索并未卡滞则进行步骤8；
步骤7：根据绳索卡滞求解绳索长度后再进行步骤8；
步骤8：报告信号故障后进行步骤15；
步骤9：避障求解过程值，并判断是否满足失效条件1，若满足失效条件1则进行步骤10，若不满足失效条件1则进行步骤11；
步骤10：动力不足则进行步骤15；
步骤11：判断是否满足失效条件2，若满足失效条件2则进行步骤10，若不满足失效条件2则进行步骤12；
步骤12：判断是否满足失效条件3，若满足失效条件3则进行步骤13，若不满足失效条件3则进行步骤14；
步骤13：运动干涉后再步骤15；
步骤14：正常运动后再进行步骤15；
步骤15：结束。
进一步的，所述步骤1中绳驱蛇形机械臂运动方程具体为，
绳驱蛇形机械臂的运动主要由末端位置、绳索长度和关节角度表示，其中末端位置使用向量P＝[x y z φ θ ψ]T，
其中，x、y和z分别表示末端坐标系的位置，φ、θ和ψ分别表示末端坐标系的滚动、俯仰和偏航角，关节角度用广义角度向量q＝[q1 q2 … qI]T表示，
其中，I为关节转动轴的总数量，qi(1≤i≤I)为第i个关节的转动角度，绳索长度使用广义绳长向量l＝[l1 l2 … lJ]T表示，
其中，J为绳索总数量，其中lj(1≤j≤J)表示第j条绳索的长度，
绳长与末端位置的关系表示为：
f(l)＝P (1)
其中，f(l)为关于绳长的非线性函数，
由于绳驱蛇形机械臂为超冗余机构，其运动过程中设置优化函数确定冗余自由度的运动，优化函数为关于关节角度的函数g(q)取为：
Figure BDA0002721747430000031
关节角度q与绳索长度l之间的关系为确定的，表示为：
q＝h(l) (3)
其中，h(l)为关于绳长的非线性函数，
由式(1)-(3)可得由P求解l的过程为：
Figure BDA0002721747430000032
正常情况下，使用雅可比矩阵伪逆法即可求得所需的绳索控制量l，其具体过程为：
Figure BDA0002721747430000033
Figure BDA0002721747430000034
Figure BDA0002721747430000035
Figure BDA0002721747430000036
其中，t0为初始时间，
Figure BDA0002721747430000037
为绳索初始速度。
进一步的，所述步骤5具体为，当绳索发生破断后，其对应的拉力传感器的力检测值将会发生突变，从而得到破断绳索的编号j，
在方程中去掉破断绳索j的影响，得到由末端位置求解绳索长度的模型为：
Figure BDA0002721747430000038
其中，
Figure BDA0002721747430000039
为l去除分量lj后的向量，
Figure BDA00027217474300000310
为f(l)＝P中去分量lj所得的方程组，
Figure BDA0002721747430000041
为q＝h(l)去除lj，由过程-依然求得绳索长度，但是该过程无法与绳索卡滞过程通用，因此使用如下方式求解绳索长度：
将
Figure BDA0002721747430000042
带入
Figure BDA0002721747430000043
同时引入拉格朗日乘子λ＝[λ1 λ2 … λJ-1]T中得到无约束的优化函数：
Figure BDA0002721747430000044
式关于
Figure BDA0002721747430000045
和λ求导，得到绳索破断情况下绳索长度的计算模型为：
Figure BDA0002721747430000046
使用代数方程数值计算方法，其用绳长雅可比矩阵判断方法为：
如果，Jl的秩不变，即：
Figure BDA0002721747430000047
其中，
Figure BDA0002721747430000048
则机构使用式的方法仍能准确运行，
同时，若
Figure BDA0002721747430000049
存在某一行符号相同，由于绳索只能提供拉力不能提供支持力，则机构动力不足仍按无法按照期望运动。
进一步的，所述步骤7具体为，绳索卡滞后是由于驱动不能运动引起的，为此通过驱动部件的位置反馈可得到卡滞绳索的编号j，相比于绳索破断，绳索卡滞将增加绳索理论绳索长度不超过实际绳索长度的约束，即：
Figure BDA00027217474300000410
其中，
Figure BDA00027217474300000411
为卡滞时候的绳索长度；
由于使用冗余驱动方法，此时卡滞绳索的理论长度由正常绳索计算得到，通过相关推导得到卡滞绳索与正常绳索之间的关系为：
Figure BDA00027217474300000412
其中，
Figure BDA00027217474300000413
为l中去除卡滞绳索长度后的绳索向量；
在式中增加松弛因子，得到绳索卡滞状态下机构驱动量的优化程序为：
Figure BDA0002721747430000051
其中，v为松弛因子；
通过引入拉格朗日乘子，将式转为无约束的优化函数：
Figure BDA0002721747430000052
其中，λ为
Figure BDA0002721747430000053
对应的拉格朗日乘子向量，χ为
Figure BDA0002721747430000054
对应的拉格朗日乘子；式对
Figure BDA0002721747430000055
λ、χ和v求导得到绳索卡滞状态下的驱动绳长求解模型：
Figure BDA0002721747430000056
使用代数方程数值计算方法，当求解过程中，绳长雅可比矩阵的
Figure BDA0002721747430000057
或Jl中某行不存在异号元素，则机构动力不足，无法满足运动需求；
当v没有实数解时，则机构受限于卡滞绳索限制，会出现运动干涉；
综上所示，满足一下判据之一，则机构无法正常运动：
失效条件(1)
Figure BDA0002721747430000058
失效条件(2)Jl存在没有异号元素的行；
失效条件(3)v没有实数解时；
其中，失效条件(1)和/或失效条件(2)同时满足，则机构动力不足；存在失效条件(3)则机构运动干涉。
本发明的有益效果是：
1.本发明当绳索破断或卡滞后不用停机，可以继续直线在工作空间内的任务。
2.本发明当绳索破断或卡滞后，可以不用更换绳索继续执行任务，最大程度发挥机构的冗余驱动能力。
附图说明
附图1本发明绳索故障处理及判断流程图。
附图2本发明蛇形机械臂示意图。
附图3本发明蛇形机械臂关节示意图。
附图4本发明蛇形机械臂基座示意图。
具体实施方式
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
一种绳驱蛇形机械臂，所述绳驱蛇形机械臂包括控制柜1、支撑脚2、基座3和机械臂4，所述控制柜1的底端设置多个支撑脚2，所述控制柜1的上端设置基座3，所述基座3的前端设置机械臂4，
所述机械臂4包括绳索5、锁紧机构6、锥节下板7、连接柱8、万向节轴9和锥节上板10，所述锥节上板10与锥节下板7之间设置多个连接柱8与多根绳索5，所述多个连接柱8与多根绳索5间隔设置，所述锥节下板7上设置锁紧机构6，每一对所述锥节上板10、锥节下板7、锁紧机构6与锥节上板10和锥节下板7之间的绳索5和连接柱8组成一个机械臂关节，相邻两个所述机械臂关节之间通过万向轴9连接；
所述基座3上的电机12通过力传感器11与丝杠连接，所述丝杠的滑块安装在直线导轨13上，所述力传感器11的一端安装在丝杠的滑块上，所述绳索5固定在力传感器11的另一端。
一种绳驱蛇形机械臂绳索故障状态下的控制方法，所述控制方法包括以下步骤：
步骤1：基于绳驱蛇形机械臂列举绳驱蛇形机械臂运动方程；
步骤2：判断绳驱蛇形机械臂的绳索是否出现故障，若出现故障则跳转至步骤3，若没出现故障则跳转至步骤15；
步骤3：确定绳索编号后再进行步骤4；
步骤4：判断绳驱蛇形机械臂的绳索是否破断，若绳索破断则进行步骤6，若绳索并未破断进行步骤5；
步骤5：根据绳索破断求解绳索长度后再进行步骤8；
步骤6：判断绳驱蛇形机械臂的绳索是否卡滞，若绳索卡滞则进行步骤7，若绳索并未卡滞则进行步骤8；
步骤7：根据绳索卡滞求解绳索长度后再进行步骤8；
步骤8：报告信号故障后进行步骤15；
步骤9：避障求解过程值，并判断是否满足失效条件(1)，若满足失效条件(1)则进行步骤10，若不满足失效条件(1)则进行步骤11；
步骤10：动力不足则进行步骤15；
步骤11：判断是否满足失效条件(2)，若满足失效条件(2)则进行步骤10，若不满足失效条件(2)则进行步骤12；
步骤12：判断是否满足失效条件(3)，若满足失效条件(3)则进行步骤13，若不满足失效条件(3)则进行步骤14；
步骤13：运动干涉后再步骤15；
步骤14：正常运动后再进行步骤15；
步骤15：结束。
进一步的，所述步骤1中绳驱蛇形机械臂运动方程具体为，
绳驱蛇形机械臂的运动主要由末端位置、绳索长度和关节角度表示，其中末端位置使用向量P＝[x y z φ θ ψ]T，
其中，x、y和z分别表示末端坐标系的位置，φ、θ和ψ分别表示末端坐标系的滚动、俯仰和偏航角，关节角度用广义角度向量q＝[q1 q2 … qI]T表示，
其中，I为关节转动轴的总数量，qi(1≤i≤I)为第i个关节的转动角度，绳索长度使用广义绳长向量l＝[l1 l2 … lJ]T表示，
其中，J为绳索总数量，其中lj(1≤j≤J)表示第j条绳索的长度，
绳长与末端位置的关系表示为：
f(l)＝P (1)
其中，f(l)为关于绳长的非线性函数，
由于绳驱蛇形机械臂为超冗余机构，其运动过程中设置优化函数确定冗余自由度的运动，优化函数为关于关节角度的函数g(q)取为：
Figure BDA0002721747430000081
关节角度q与绳索长度l之间的关系为确定的，表示为：
q＝h(l) (3)
其中，h(l)为关于绳长的非线性函数，
由式(1)-(3)可得由P求解l的过程为：
Figure BDA0002721747430000082
正常情况下，使用雅可比矩阵伪逆法即可求得所需的绳索控制量l，其具体过程为：
Figure BDA0002721747430000083
Figure BDA0002721747430000084
Figure BDA0002721747430000085
Figure BDA0002721747430000086
其中，t0为初始时间，
Figure BDA0002721747430000087
为绳索初始速度。
进一步的，所述步骤5具体为，当绳索发生破断后，其对应的拉力传感器的力检测值将会发生突变，从而得到破断绳索的编号j，
在方程中去掉破断绳索j的影响，得到由末端位置求解绳索长度的模型为：
Figure BDA0002721747430000088
其中，
Figure BDA0002721747430000089
为l去除分量lj后的向量，
Figure BDA00027217474300000810
为f(l)＝P中去分量lj所得的方程组，
Figure BDA00027217474300000811
为q＝h(l)去除lj，由过程-依然求得绳索长度，但是该过程无法与绳索卡滞过程通用，因此使用如下方式求解绳索长度：
将
Figure BDA0002721747430000091
带入
Figure BDA0002721747430000092
同时引入拉格朗日乘子λ＝[λ1 λ2 … λJ-1]T中得到无约束的优化函数：
Figure BDA0002721747430000093
式关于
Figure BDA0002721747430000094
和λ求导，得到绳索破断情况下绳索长度的计算模型为：
Figure BDA0002721747430000095
使用代数方程数值计算方法，例如牛顿迭代等技术可以求解式从而得到绳索破断时候的绳长向量
Figure BDA0002721747430000096
由于绳索破断后可能存在不能有效驱动的情况，因此使用绳长雅可比矩阵判断在故障情况下是否能够准确执行相关操作，其用绳长雅可比矩阵判断方法为：
如果，Jl的秩不变，即：
Figure BDA0002721747430000097
其中，
Figure BDA0002721747430000098
则机构使用式的方法仍能准确运行，
同时，若
Figure BDA0002721747430000099
存在某一行符号相同，由于绳索只能提供拉力不能提供支持力，则机构动力不足仍按无法按照期望运动。
进一步的，所述步骤7具体为，绳索卡滞后是由于驱动不能运动引起的，为此通过驱动部件的位置反馈可得到卡滞绳索的编号j，相比于绳索破断，绳索卡滞将增加绳索理论绳索长度不超过实际绳索长度的约束，即：
Figure BDA00027217474300000910
其中，
Figure BDA00027217474300000911
为卡滞时候的绳索长度；
由于使用冗余驱动方法，此时卡滞绳索的理论长度由正常绳索计算得到，通过相关推导得到卡滞绳索与正常绳索之间的关系为：
Figure BDA00027217474300000912
其中，
Figure BDA00027217474300000913
为l中去除卡滞绳索长度后的绳索向量；
在式中增加松弛因子，得到绳索卡滞状态下机构驱动量的优化程序为：
Figure BDA0002721747430000101
其中，v为松弛因子；
通过引入拉格朗日乘子，将式转为无约束的优化函数：
Figure BDA0002721747430000102
其中，λ为
Figure BDA0002721747430000103
对应的拉格朗日乘子向量，χ为
Figure BDA0002721747430000104
对应的拉格朗日乘子；式对
Figure BDA0002721747430000105
λ、χ和v求导得到绳索卡滞状态下的驱动绳长求解模型：
Figure BDA0002721747430000106
使用代数方程数值计算方法，例如牛顿迭代等技术可以求解式从而得到绳索破断时候的绳长向量
Figure BDA0002721747430000107
当求解过程中，绳长雅可比矩阵的
Figure BDA0002721747430000108
或Jl中某行不存在异号元素，则机构动力不足，无法满足运动需求；
当v没有实数解时，则机构受限于卡滞绳索限制，会出现运动干涉；
综上所示，满足一下判据之一，则机构无法正常运动：
失效条件(1)
Figure BDA0002721747430000109
失效条件(2)Jl存在没有异号元素的行；
失效条件(3)v没有实数解时；
其中，失效条件(1)和/或失效条件(2)同时满足，则机构动力不足；存在失效条件(3)则机构运动干涉。

Claims (4)

1.一种绳驱蛇形机械臂绳索故障状态下的控制方法，其特征在于，所述绳驱蛇形机械臂包括控制柜(1)、支撑脚(2)、基座(3)和机械臂(4)，所述控制柜(1)的底端设置多个支撑脚(2)，所述控制柜(1)的上端设置基座(3)，所述基座(3)的前端设置机械臂(4)，
所述机械臂(4)包括绳索(5)、锁紧机构(6)、锥节下板(7)、连接柱(8)、万向节轴(9)和锥节上板(10)，所述锥节上板(10)与锥节下板(7)之间设置多个连接柱(8)与多根绳索(5)，所述锥节下板(7)上设置锁紧机构(6)，每一对所述锥节上板(10)、锥节下板(7)、锁紧机构(6)与锥节上板(10)和锥节下板(7)之间的绳索(5)和连接柱(8)组成一个机械臂关节，相邻两个所述机械臂关节之间通过万向节轴(9)连接；
所述基座(3)上的电机(12)通过力传感器(11)与丝杠连接，所述丝杠的滑块安装在直线导轨(13)上，所述力传感器(11)的一端安装在丝杠的滑块上，所述绳索(5)固定在力传感器(11)的另一端；
所述控制方法包括以下步骤：
步骤1：基于绳驱蛇形机械臂列举绳驱蛇形机械臂运动方程；
步骤2：判断绳驱蛇形机械臂的绳索是否出现故障，若出现故障则跳转至步骤3，若没出现故障则跳转至步骤15；
步骤3：确定绳索编号后再进行步骤4；
步骤4：判断绳驱蛇形机械臂的绳索是否破断，若绳索破断则进行步骤5，若绳索并未破断则进行步骤6；
步骤5：根据绳索破断求解绳索长度后再进行步骤8；
步骤6：判断绳驱蛇形机械臂的绳索是否卡滞，若绳索卡滞则进行步骤7，若绳索并未卡滞则进行步骤8；
步骤7：根据绳索卡滞求解绳索长度后再进行步骤8；
步骤8：报告信号故障后进行步骤15，未报告信号故障则进行步骤9；
步骤9：避障求解过程值，并判断是否满足失效条件(1)，若满足失效条件(1)则进行步骤10，若不满足失效条件(1)则进行步骤11；
步骤10：动力不足则进行步骤15；
步骤11：判断是否满足失效条件(2)，若满足失效条件(2)则进行步骤10，若不满足失效条件(2)则进行步骤12；
步骤12：判断是否满足失效条件(3)，若满足失效条件(3)则进行步骤13，若不满足失效条件(3)则进行步骤14；
步骤13：运动干涉后再进行步骤15；
步骤14：正常运动后再进行步骤15；
步骤15：结束；
所述失效条件具体为：
失效条件(1)
Figure FDA0003272227710000021
失效条件(2)Jl存在没有异号元素的行；
失效条件(3)v没有实数解时。
2.根据权利要求1所述一种绳驱蛇形机械臂绳索故障状态下的控制方法，其特征在于，所述步骤1中绳驱蛇形机械臂运动方程具体为，
绳驱蛇形机械臂的运动主要由末端位置、绳索长度和关节角度表示，其中末端位置使用向量P＝[x y z φ θ ψ]T，
其中，x、y和z分别表示末端坐标系的位置，φ、θ和ψ分别表示末端坐标系的滚动、俯仰和偏航角，关节角度用广义角度向量q＝[q1 q2…qI]T表示，
其中，I为关节转动轴的总数量，qi(1≤i≤I)为第i个关节的转动角度，绳索长度使用广义绳长向量l＝[l1 l2…lJ]T表示，
其中，J为绳索总数量，其中lj(1≤j≤J)表示第j条绳索的长度，
绳长与末端位置的关系表示为：
f(l)＝P (1)
其中，f(l)为关于绳长的非线性函数，
由于绳驱蛇形机械臂为超冗余机构，其运动过程中设置优化函数确定冗余自由度的运动，优化函数为关于关节角度的函数g(q)取为：
Figure FDA0003272227710000022
关节角度q与绳索长度l之间的关系为确定的，表示为：
q＝h(l) (3)
其中，h(l)为关于绳长的非线性函数，
由式(1)-(3)可得由P求解l的过程为：
Figure FDA0003272227710000031
正常情况下，使用雅可比矩阵伪逆法即可求得所需的绳索控制量l，其具体过程为：
Figure FDA0003272227710000032
Figure FDA0003272227710000033
Figure FDA0003272227710000034
Figure FDA0003272227710000035
其中，t0为初始时间，
Figure FDA0003272227710000036
为绳索初始速度。
3.根据权利要求1所述一种绳驱蛇形机械臂绳索故障状态下的控制方法，其特征在于，所述步骤5具体为，当绳索发生破断后，其对应的拉力传感器的力检测值将会发生突变，从而得到破断绳索的编号j，
在方程中去掉破断绳索j的影响，得到由末端位置求解绳索长度的模型为：
Figure FDA0003272227710000037
其中，
Figure FDA0003272227710000038
为l去除分量lj后的向量，
Figure FDA0003272227710000039
为f(l)＝P中去分量lj所得的方程组，
Figure FDA00032722277100000310
为q＝h(l)去除lj，由过程-依然求得绳索长度，但是该过程无法与绳索卡滞过程通用，因此使用如下方式求解绳索长度：
将
Figure FDA00032722277100000311
带入
Figure FDA00032722277100000312
同时引入拉格朗日乘子λ＝[λ1 λ2…λJ-1]T中得到无约束的优化函数：
Figure FDA0003272227710000041
式关于
Figure FDA0003272227710000042
和λ求导，得到绳索破断情况下绳索长度的计算模型为：
Figure FDA0003272227710000043
使用代数方程数值计算方法，其用绳长雅可比矩阵判断方法为：
如果，Jl的秩不变，即：
Figure FDA0003272227710000044
其中，
Figure FDA0003272227710000045
则机构使用式的方法仍能准确运行，
同时，若
Figure FDA0003272227710000046
存在某一行符号相同，由于绳索只能提供拉力不能提供支持力，则机构动力不足仍按无法按照期望运动。
4.根据权利要求1所述一种绳驱蛇形机械臂绳索故障状态下的控制方法，其特征在于，所述步骤7具体为，绳索卡滞后是由于驱动不能运动引起的，为此通过驱动部件的位置反馈可得到卡滞绳索的编号j，相比于绳索破断，绳索卡滞将增加绳索理论绳索长度不超过实际绳索长度的约束，即：
Figure FDA0003272227710000047
其中，
Figure FDA0003272227710000048
为卡滞时候的绳索长度；
由于使用冗余驱动方法，此时卡滞绳索的理论长度由正常绳索计算得到，通过相关推导得到卡滞绳索与正常绳索之间的关系为：
Figure FDA0003272227710000049
其中，
Figure FDA00032722277100000410
为l中去除卡滞绳索长度后的绳索向量；
在式中增加松弛因子，得到绳索卡滞状态下机构驱动量的优化程序为：
Figure FDA0003272227710000051
其中，v为松弛因子；
通过引入拉格朗日乘子，将式转为无约束的优化函数：
Figure FDA0003272227710000052
其中，λ为
Figure FDA0003272227710000053
对应的拉格朗日乘子向量，χ为
Figure FDA0003272227710000054
对应的拉格朗日乘子；式对
Figure FDA0003272227710000055
λ、χ和v求导得到绳索卡滞状态下的驱动绳长求解模型：
Figure FDA0003272227710000056
使用代数方程数值计算方法，当求解过程中，绳长雅可比矩阵的
Figure FDA0003272227710000057
或Jl中某行不存在异号元素，则机构动力不足，无法满足运动需求；
当v没有实数解时，则机构受限于卡滞绳索限制，会出现运动干涉；
综上所示，满足一下失效条件之一，则机构无法正常运动：
其中，失效条件(1)和/或失效条件(2)同时满足，则机构动力不足；存在失效条件(3)则机构运动干涉。
CN202011090130.0A 2020-10-13 2020-10-13 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法 Active CN112318492B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011090130.0A CN112318492B (zh) 2020-10-13 2020-10-13 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011090130.0A CN112318492B (zh) 2020-10-13 2020-10-13 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法

Publications (2)

Publication Number Publication Date
CN112318492A CN112318492A (zh) 2021-02-05
CN112318492B true CN112318492B (zh) 2022-04-08

Family

ID=74313881

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011090130.0A Active CN112318492B (zh) 2020-10-13 2020-10-13 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法

Country Status (1)

Country Link
CN (1) CN112318492B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6347085A (ja) * 1986-08-08 1988-02-27 株式会社東芝 関節装置
JPH09254074A (ja) * 1996-03-22 1997-09-30 Canon Inc 落下防止機構
CN104121846A (zh) * 2014-08-18 2014-10-29 南京晨光森田环保科技有限公司 一种长度传感器拉线状态的检测方法
CN206869870U (zh) * 2017-06-21 2018-01-12 浙江大学 一种可检测关节姿态的高冗余柔性机械臂装置
CN109278032A (zh) * 2018-10-15 2019-01-29 享奕自动化科技（上海）有限公司 欠驱动绳驱机械臂
CN109692105A (zh) * 2017-10-20 2019-04-30 松下知识产权经营株式会社 辅助装置、辅助方法以及程序
CN109760051A (zh) * 2019-01-16 2019-05-17 哈尔滨工业大学 一种绳驱超冗余自由度机器人的绳索长度变化确定方法
CN111421531A (zh) * 2020-03-27 2020-07-17 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 驱动绳冗余备份的柔性机械臂

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6347085A (ja) * 1986-08-08 1988-02-27 株式会社東芝 関節装置
JPH09254074A (ja) * 1996-03-22 1997-09-30 Canon Inc 落下防止機構
CN104121846A (zh) * 2014-08-18 2014-10-29 南京晨光森田环保科技有限公司 一种长度传感器拉线状态的检测方法
CN206869870U (zh) * 2017-06-21 2018-01-12 浙江大学 一种可检测关节姿态的高冗余柔性机械臂装置
CN109692105A (zh) * 2017-10-20 2019-04-30 松下知识产权经营株式会社 辅助装置、辅助方法以及程序
CN109278032A (zh) * 2018-10-15 2019-01-29 享奕自动化科技（上海）有限公司 欠驱动绳驱机械臂
CN109760051A (zh) * 2019-01-16 2019-05-17 哈尔滨工业大学 一种绳驱超冗余自由度机器人的绳索长度变化确定方法
CN111421531A (zh) * 2020-03-27 2020-07-17 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 驱动绳冗余备份的柔性机械臂

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
绳驱连续型冗余自由度机器人控制研究;谷海宇;《中国优秀硕士学位论文全文数据库 信息科技辑》;20180215;全文 *
蛇形机械臂的设计及控制策略;王丰 等;《机器人》;20170531;第39卷(第3期);全文 *

Also Published As

Publication number Publication date
CN112318492A (zh) 2021-02-05

Similar Documents

Publication Publication Date Title
CN107717994B (zh) 基于主从空间映射的主从异构机器人通用控制方法及系统
CN102785253B (zh) 具有机器人的异常检测功能的机器人系统及其控制方法
CN102431043B (zh) 减速器的异常判定方法、装置和机器人及机器人系统
CN101192062B (zh) 用于监测工业机器人的状况的方法和装置
JP4383476B2 (ja) プラント制御システム及び方法
WO2008001713A1 (fr) Robot articulé et son programme de commande
CN108058188A (zh) 机器人健康监测和故障诊断系统及其控制方法
CN102591331A (zh) 自动化设备故障可视化诊断系统
CN101116968A (zh) 六维力传感器装配机械手姿态及力的控制方法
CN102825603A (zh) 网络遥操作机器人系统及时延克服方法
CN107351088B (zh) 一种机器人外部运动路径控制方法
JP2012171071A (ja) ロボットのねじ締め作業異常検知方法
CN112318492B (zh) 一种绳驱蛇形机械臂及其绳索故障状态下的控制方法
CN108527375A (zh) 控制系统、控制器、控制方法及记录媒体
Gorjup et al. Combining compliance control, cad based localization, and a multi-modal gripper for rapid and robust programming of assembly tasks
DE102021113636B3 (de) Verfahren zum Bestimmen externer Interaktionskräfte und/oder Interaktionsmomente eines Roboters, Roboter und Computerprogrammprodukt
Ding et al. Collaborative behavior design of industrial robots for multiple human-robot collaboration
Lee et al. Knowledge Based Error Recovery in Industrial Robots.
DE102018214257B3 (de) Roboterregelung
JP6863951B2 (ja) ワイヤ断線予測装置
Moldovan et al. Dynamic numerical simulation of the 6-PGK parallel robot manipulator
CN112809667A (zh) 工业机器人的力控制方法、装置及其应用
CN111823235A (zh) 一种用于采摘机械臂的碰撞检测方法
WO2019065426A1 (ja) ロボットハンドおよびロボットハンド制御方法
DE102019122416B4 (de) Roboter

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