CN115781677A - 一种防止机械臂软管管路缠绕的轨迹规划算法 - Google Patents
一种防止机械臂软管管路缠绕的轨迹规划算法 Download PDFInfo
- Publication number
- CN115781677A CN115781677A CN202211509960.1A CN202211509960A CN115781677A CN 115781677 A CN115781677 A CN 115781677A CN 202211509960 A CN202211509960 A CN 202211509960A CN 115781677 A CN115781677 A CN 115781677A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- matrix
- cartesian
- track
- hose
- 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
Links
- 238000004804 winding Methods 0.000 title claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 56
- 230000001133 acceleration Effects 0.000 claims abstract description 11
- 238000012545 processing Methods 0.000 claims abstract description 10
- 238000012795 verification Methods 0.000 claims abstract description 7
- 230000036544 posture Effects 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 15
- 238000000034 method Methods 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 12
- 238000013178 mathematical model Methods 0.000 claims description 12
- 238000013519 translation Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 11
- 230000008030 elimination Effects 0.000 claims description 3
- 238000003379 elimination reaction Methods 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000000844 transformation Methods 0.000 claims 1
- 238000005507 spraying Methods 0.000 abstract description 4
- 238000003466 welding Methods 0.000 abstract description 3
- 238000004458 analytical method Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 238000005260 corrosion Methods 0.000 description 1
- 230000007797 corrosion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明涉及家具喷涂、汽车喷涂、焊接领域，具体是一种防止机械臂软管管路缠绕的轨迹规划算法，其具体步骤如下：S1、原始轨迹；S2、依次遍历计算所有坐标点；S3、通过正运算计算当前机械臂在q_j角度下的笛卡尔轨迹q_c，S4、目标笛卡尔位置与当前笛卡尔位置做差，计算插补次数；S5、通过插补值进行运动学逆运算；S6、通过雅可比矩阵JF(x1,...,xn)，获得机械臂速度、加速度、奇异点信息，进行轨迹可行性验证，S7、计算笛卡尔坐标差值，S8、末端旋转角度与阈值对比；S9、将步骤S8中得到的关节角度带入下一次遍历；S10、算法处理完成，从算法层面通过正、逆运动学，计算轨迹运动中机械臂旋转导致的绕管姿态，从而防止管路缠绕和断裂。
Description
技术领域
本发明涉及家具喷涂、汽车喷涂、焊接领域，具体是一种防止机械臂软管管路缠绕的轨迹规划算法。
背景技术
工业机械臂通过末端加装工具来完成作业，有些工具需要连接电线、导气管、供料管等软管设施，在作业过程中随着末端旋转，这些软管管路有时会与机械臂本体发生缠绕，当缠绕角度或圈数过大时，软管就有断裂风险，造成生产事故。中国专利号为211490182U公开了一种防止电线缠绕的焊接机械臂，中国专利号为214818684U和203805002U各公开了一种用于机械臂的防缠绕装置。但是以上三篇专利都是从硬件层面去解决机械臂软管缠绕问题：例如CN211490182U是在机械臂外侧增加5个固定结构和可拆卸的滑动装置，CN214818684U是一种限位盘，CN 203805002 U更加复杂，包含了轮轴、谐波减速器、支架等部件。这些方法或需要更改机械臂设计布局，或需要给现有机械臂增加额外组件，不仅改动较大，增加成本，还会影响机械臂本身的运动性能，而且在高污染、高腐蚀等恶劣工作环境下会增加故障几率，影响生产效率。
发明内容
为了解决上述问题，本发明提出一种防止机械臂软管管路缠绕的轨迹规划算法。
一种防止机械臂软管管路缠绕的轨迹规划算法，其具体步骤如下：
S1、原始轨迹：
S1.1、解析并获得笛卡尔运动坐标点：对轨迹文件进行预处理，根据不同机器人制造商的指令规则，提取对应轨迹文件中的笛卡尔空间运动信息，具体包括空间坐标X、Y、Z和工具姿态A即滚转角、B即俯仰角、C即偏航角六个参数；
S1.2、获取机械臂DH矩阵：将机械臂用数学模型表达出来的一种常用方法，本算法需要使用机械臂的DH矩阵；
S1.3、获取机械臂工具的偏置和Euler旋转矩阵：偏置和旋转构造变化矩阵，本算法需要使用偏置来确定空间坐标，使用Euler旋转矩阵计算工具姿态；
S1.4使用S1.2-S1.3的数据数学模型；
S2、依次遍历计算所有坐标点：将步骤S1.1预处理完毕的轨迹依次传入本算法：
a、输入的第一个参数为目标笛卡尔轨迹qc，第二个参数为当前关节角度qj；
b、算法中初始化笛卡尔轨迹q_c和关节角度q_j均为0，若实际关节qj不为0，则q_j＝qj，q_c用于接收计算结果；
c、后续使用q_c和q_j进行计算，避免原始数据qc和qj被意外修改；
S3、通过正运算计算当前机械臂在q_j角度下的笛卡尔轨迹q_c：
S3.1、通过公式1rad＝θ*180°/π将q_j中的角度换算为弧度；
S3.2、将步骤S1.2中的DH参数构造为DH矩阵；
S3.3、将DH矩阵从第一关节连乘至最后关节；
S3.4、将步骤S3.3的结果与1.3中的工具变化矩阵相乘，得到笛卡尔坐标XYZ；
S3.5、使用Atan2公式求出枪姿ABC；
S3.6、将结果赋值给q_c；
S4、目标笛卡尔位置与当前笛卡尔位置做差，计算插补次数：
S5、通过插补值进行运动学逆运算：
a、取步骤S4中计算的cycles[0]～cycles[5]中的最大值max_cycle作为插补步长，使用for循环从0到max_cycle进行插补计算；
b、步骤S3中的q_c的X、Y、Z、A、B、C数据每次循环分别增加ΔX/max_cycle、ΔY/max_cycle、ΔZ/max_cycle、ΔA/max_cycle、ΔB/max_cycle和ΔC/max_cycle步长；
c、之后传入逆运算算法，当循环结束时，q_c坐标值与qc完全相等；
S6、通过雅可比矩阵JF(x1,...,xn)，获得机械臂速度、加速度、奇异点信息，进行轨迹可行性验证；
S7、计算笛卡尔坐标差值，利用高斯-约旦消元法求解：对消元结果进行积分，得到机械臂当前位姿对应的关节角度；
S8、末端旋转角度与阈值对比：通过软管缠绕阈值进行角度计算，具体流程如下：
a、算法处理的下限为360°，即缠绕一圈，上限由人工根据机械臂的性能和软管的安全拉伸长度给出；
b、人工操作机器人绕管，直到绕管情况达到可接受范围的最大值，记录为绕管上限阈值；
c、将所求得的关节角度与软管缠绕阈值做对比，因为工具旋转360°姿态与旋转前相同，所以解绕管默认旋转360°，根据工具实际情况，这个值可以修改；
S8.1、末端反向旋转：超过阈值就添加解绕管旋转，默认逆向旋转360度，将此解绕管动作加入当前轨迹段之前，并将解绕管后的关节角度代替原始关节角度；
S8.2、当前轨迹正常，不做处理：未超过阈值，则对此轨迹段不做处理，保留步骤S7得到的关节角度数据；
S9、将步骤S8中得到的关节角度带入下一次遍历：作为插补逆运算的输入值之一，重复步骤3-步骤8，直至所有轨迹遍历完成；
S10、算法处理完成，输出处理过的无缠绕轨迹。
所述的步骤S1.1的笛卡尔空间运动信息的空间坐标X、Y、Z和工具姿态A、B、C，按照文件中的先后顺序存储进vector结构体里。
所述的vector结构体每一项都包含6个参数：既X、Y、Z、A、B、C。
所述的步骤S1.2的标准DH参数的计算先后顺序位：
(1).Z轴旋转，使用θ表示，旋转矩阵用Rot表示；
(2).Z轴平移，使用d表示，平移矩阵用Trans表示；
(3).X轴平移，使用a表示，平移矩阵用Trans表示；
(4).X轴旋转，使用α表示，旋转矩阵用Rot表示；
从第n-1轴到第n轴的变化表示为：n-1Tn＝Rot(zn-1,θn)·Trans(zn-1,dn)·Trans(xn,an)·Rot(xn,αn-1)。
所述的步骤S1.3中，根据Euler定理，在三维空间中，任意一种旋转变换都可以归结为若干个沿着坐标轴旋转的组合，组合的个数不超过三个并且两个相邻的旋转必须沿着不同的坐标轴。
所述的步骤S1.4中，建立数学模型具体是使用S1.2的DH矩阵获得从机械臂基坐标到法兰的变换矩阵0T1、1T2、2T3、3T4、4T5和5T6，使用S1.3的工具参数获得法兰到TCP，即工具末端的变换矩阵6TTCP然后进行连乘即可获得机械臂基坐标到TCP的数学模型0TTCP：
0TTCP＝0T1*1T2*2T3*3T4*4T5*5T6*6TTCP 。
所述的步骤S4中，计算插补次数具体步骤如下：
a、将步骤S3中计算出的笛卡尔坐标q_c与步骤S2传入的笛卡尔坐标qc做差然后取绝对值，得到正double值ΔX、ΔY、ΔZ、ΔA、ΔB和ΔC；
b、对其中的ΔX、ΔY、ΔZ除以5得到插补次数cycles[0]、cycles[1]、和cycles[2]，其中5意味着每5mm进行一次插补；
c、对其中的ΔA、ΔB、ΔC除以0.5得到插补次数cycles[3]、cycles[4]、和cycles[5]，其中0.5意味着每0.5°进行一次插补。
所述的步骤S6的可行性验证具体包括检查安全速度是否超出范围、加速度是否超出范围、断雅可比矩阵是否满秩。
所述的步骤S8中的机械臂的性能可为轴限位。
所述的步骤S8中的软管长度的冗余量与实际安装情况相关，需要现场进行实验获得绕管上限。
本发明的有益效果是：本发明不需要增加额外硬件，不需要改变机械臂设计布局，从算法层面通过正、逆运动学，计算轨迹运动中机械臂旋转导致的绕管姿态，使机械臂可以在运动过程中当姿态达到缠绕阈值时增加解绕管动作，从而防止管路缠绕和断裂。对于可以给出DH矩阵的机械臂，该算法都适用。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1为本发明的流程示意图；
图2为本发明的正运算算法流程示意图；
图3为本发明的部分制造商指令的解析示意图。
具体实施方式
为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对本发明进一步阐述。
如图1至图3所示，一种防止机械臂软管管路缠绕的轨迹规划算法，其具体步骤如下：
S1、原始轨迹：
S1.1、解析并获得笛卡尔运动坐标点：对轨迹文件进行预处理，根据不同机器人制造商的指令规则，提取对应轨迹文件中的笛卡尔空间运动信息，具体包括空间坐标X、Y、Z和工具姿态A即滚转角、B即俯仰角、C即偏航角六个参数；
S1.2、获取机械臂DH矩阵：将机械臂用数学模型表达出来的一种常用方法，本算法需要使用机械臂的DH矩阵；
S1.3、获取机械臂工具的偏置和Euler旋转矩阵：偏置和旋转构造变化矩阵，本算法需要使用偏置来确定空间坐标，使用Euler旋转矩阵计算工具姿态；
S1.4使用S1.2-S1.3的数据数学模型；
S2、依次遍历计算所有坐标点：将步骤S1.1预处理完毕的轨迹依次传入本算法：
a、输入的第一个参数为目标笛卡尔轨迹qc，第二个参数为当前关节角度qj；
b、算法中初始化笛卡尔轨迹q_c和关节角度q_j均为0，若实际关节qj不为0，则q_j＝qj，q_c用于接收计算结果；
c、后续使用q_c和q_j进行计算，避免原始数据qc和qj被意外修改；
S3、通过正运算计算当前机械臂在q_j角度下的笛卡尔轨迹q_c，正运算算法输入输出如图2：
S3.1、通过公式1rad＝θ*180°/π将q_j中的角度换算为弧度；
S3.2、将步骤S1.2中的DH参数构造为DH矩阵；
S3.3、将DH矩阵从第一关节连乘至最后关节；
S3.4、将步骤S3.3的结果与1.3中的工具变化矩阵相乘，得到笛卡尔坐标XYZ；
S3.5、使用Atan2公式求出枪姿ABC；
S3.6、将结果赋值给q_c；
S4、目标笛卡尔位置与当前笛卡尔位置做差，计算插补次数：
S5、通过插补值进行运动学逆运算：
a、取步骤S4中计算的cycles[0]～cycles[5]中的最大值max_cycle作为插补步长，使用for循环从0到max_cycle进行插补计算；
b、步骤S3中的q_c的X、Y、Z、A、B、C数据每次循环分别增加ΔX/max_cycle、ΔY/max_cycle、ΔZ/max_cycle、ΔA/max_cycle、ΔB/max_cycle和ΔC/max_cycle步长；
c、之后传入逆运算算法，当循环结束时，q_c坐标值与qc完全相等；
S6、通过雅可比矩阵JF(x1,...,xn)，获得机械臂速度、加速度、奇异点信息，进行轨迹可行性验证；
S7、计算笛卡尔坐标差值，利用高斯-约旦消元法求解：对消元结果进行积分，得到机械臂当前位姿对应的关节角度；
S8、末端旋转角度与阈值对比：通过软管缠绕阈值进行角度计算，具体流程如下：
a、算法处理的下限为360°，即缠绕一圈，上限由人工根据机械臂的性能和软管的安全拉伸长度给出；
b、人工操作机器人绕管，直到绕管情况达到可接受范围的最大值，记录为绕管上限阈值；
c、将所求得的关节角度与软管缠绕阈值做对比，因为工具旋转360°姿态与旋转前相同，所以解绕管默认旋转360°，根据工具实际情况，这个值可以修改；
S8.1、末端反向旋转：超过阈值就添加解绕管旋转，默认逆向旋转360度，将此解绕管动作加入当前轨迹段之前，并将解绕管后的关节角度代替原始关节角度；
S8.2、当前轨迹正常，不做处理：未超过阈值，则对此轨迹段不做处理，保留步骤S7得到的关节角度数据；
S9、将步骤S8中得到的关节角度带入下一次遍历：作为插补逆运算的输入值之一，重复步骤3-步骤8，直至所有轨迹遍历完成；
S10、算法处理完成，输出处理过的无缠绕轨迹。
具体的，本发明不需要增加额外硬件，不需要改变机械臂设计布局，从算法层面通过正、逆运动学，计算轨迹运动中机械臂旋转导致的绕管姿态，使机械臂可以在运动过程中当姿态达到缠绕阈值时增加解绕管动作，从而防止管路缠绕和断裂。对于可以给出DH矩阵的机械臂，该算法都适用。
所述的步骤S1.1的笛卡尔空间运动信息的空间坐标X、Y、Z和工具姿态A、B、C，按照文件中的先后顺序存储进vector结构体里。
所述的vector结构体每一项都包含6个参数：既X、Y、Z、A、B、C。
所述的步骤S1.2的标准DH参数的计算先后顺序位：
(1).Z轴旋转，使用θ表示，旋转矩阵用Rot表示；
(2).Z轴平移，使用d表示，平移矩阵用Trans表示；
(3).X轴平移，使用a表示，平移矩阵用Trans表示；
(4).X轴旋转，使用α表示，旋转矩阵用Rot表示；
从第n-1轴到第n轴的变化表示为：n-1Tn＝Rot(zn-1,θn)·Trans(zn-1,dn)·Trans (xn,an)·Rot(xn,αn-1)。
所述的步骤S1.3中，根据Euler定理，在三维空间中，任意一种旋转变换都可以归结为若干个沿着坐标轴旋转的组合，组合的个数不超过三个并且两个相邻的旋转必须沿着不同的坐标轴。
具体的，可以用三个沿着坐标轴旋转的角度来表示一个变换，称为Euler角。
具体的，旋转变换是不可交换的，根据旋转顺序的不同，有12种表示方式，分别为：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ，在指定Euler角时应当首先约定旋转顺序。
具体的，如图2所示，通过正运算计算当前机械臂在q_j角度下的笛卡尔轨迹q_c，正运算算法输入输出。
所述的步骤S1.4中，建立数学模型具体是使用S1.2的DH矩阵获得从机械臂基坐标到法兰的变换矩阵0T1、1T2、2T3、3T4、4T5和5T6，使用S1.3的工具参数获得法兰到TCP，即工具末端的变换矩阵6TTCP然后进行连乘即可获得机械臂基坐标到TCP的数学模型0TTCP：
0TTCP＝0T1*1T2*2T3*3T4*4T5*5T6*6TTCP 。
所述的步骤S4中，计算插补次数具体步骤如下：
a、将步骤S3中计算出的笛卡尔坐标q_c与步骤S2传入的笛卡尔坐标qc做差然后取绝对值，得到正double值ΔX、ΔY、ΔZ、ΔA、ΔB和ΔC；
b、对其中的ΔX、ΔY、ΔZ除以5得到插补次数cycles[0]、cycles[1]、和cycles[2]，其中5意味着每5mm进行一次插补；
c、对其中的ΔA、ΔB、ΔC除以0.5得到插补次数cycles[3]、cycles[4]、和cycles[5]，其中0.5意味着每0.5°进行一次插补。
具体的，针对管道缠绕的情形，通过算法控制机械臂解开软管缠绕。
所述的步骤S6的可行性验证具体包括检查安全速度是否超出范围、加速度是否超出范围、断雅可比矩阵是否满秩，具体内容如下：
具体的，雅可比矩阵由各关节笛卡尔速度矢量和关节角速度矢量构成，与机械臂说明书上的安全速度范围作比较，检查是否超出范围；
具体的，对速度求导获得加速度，与机械臂说明书上的安全加速度范围作比较，是否超出范围；
具体的，判断雅可比矩阵是否满秩，不满秩就是奇异点；
具体的，超出速度、加速度范围，或者机器人到达奇异点，轨迹都无法执行。
具体的，算法使用了轨迹解析、逆运动学、雅可比矩阵类推算解绕管动作的时机，避免软管断裂或报警停机现象的发生，确保了安全稳定的生产活动。
所述的步骤S8中的机械臂的性能可为轴限位。
所述的步骤S8中的软管长度的冗余量与实际安装情况相关，需要现场进行实验获得绕管上限。
如图3所示，为本发明的部分制造商指令的解析示意图，具体内容如下：
CMA机器人使用CMA语言，指令构成为：运动类型+数据，运动类型例如POS即快速移动、LIN即直线移动类，数据结构为(X，Y，Z，A，B，C)，例如：
POS 100,200,300,10,20,30
指令解释：机器人快速移动指令，移动到(100，200，300)位置，枪姿为(10，20，30)
指令解析：X＝100,Y＝200,Z＝300,A＝10,B＝20,C＝30；
Efort机器人使用XPL语言，指令构成为：运动类型+数据+外部轴+机器人构型，运动类型例如POINTC即不带外部轴的直线移动、EPOINTC即带外部轴的直线移动类，数据结构为(X，Y，Z，A，B，C)+(6位外部轴)+(CFG)，例如：
POINTC(100,200,300,10,20,30,CFG0)
指令解释：机器人直线移动指令，移动到(100，200，300)位置，枪姿为(10，20，30)，使用CFG0构型；
指令解析：X＝100,Y＝200,Z＝300,A＝10,B＝20,C＝30；
KUKA机器人使用KRL语言，指令构成为：运动类型+数据+外部轴，运动类型例如LIN即带外部轴的直线移动类，数据结构为(X，Y，Z，A，B，C)+(S行走轴+T旋转机构)，例如：
LIN{100,200,300,10,20,30,400,50}
指令解释：机器人加外部轴直线移动指令，移动到(100，200，300)位置，枪姿为(10，20，30)，行走轴移动400，旋转机构转50°；
指令解析：X＝100,Y＝200,Z＝300,A＝10,B＝20,C＝30。
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (10)
1.一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：其具体步骤如下：
S1、原始轨迹：
S1.1、解析并获得笛卡尔运动坐标点：对轨迹文件进行预处理，根据不同机器人制造商的指令规则，提取对应轨迹文件中的笛卡尔空间运动信息，具体包括空间坐标X、Y、Z和工具姿态A即滚转角、B即俯仰角、C即偏航角六个参数；
S1.2、获取机械臂DH矩阵：将机械臂用数学模型表达出来的一种常用方法，本算法需要使用机械臂的DH矩阵；
S1.3、获取机械臂工具的偏置和Euler旋转矩阵：偏置和旋转构造变化矩阵，本算法需要使用偏置来确定空间坐标，使用Euler旋转矩阵计算工具姿态；
S1.4使用S1.2-S1.3的数据数学模型；
S2、依次遍历计算所有坐标点：将步骤S1.1预处理完毕的轨迹依次传入本算法：
a、输入的第一个参数为目标笛卡尔轨迹qc，第二个参数为当前关节角度qj；
b、算法中初始化笛卡尔轨迹q_c和关节角度q_j均为0，若实际关节qj不为0，则q_j＝qj，q_c用于接收计算结果；
c、后续使用q_c和q_j进行计算，避免原始数据qc和qj被意外修改；
S3、通过正运算计算当前机械臂在q_j角度下的笛卡尔轨迹q_c：
S3.1、通过公式1rad＝θ*180°/π将q_j中的角度换算为弧度；
S3.2、将步骤S1.2中的DH参数构造为DH矩阵；
S3.3、将DH矩阵从第一关节连乘至最后关节；
S3.4、将步骤S3.3的结果与1.3中的工具变化矩阵相乘，得到笛卡尔坐标XYZ；
S3.5、使用Atan2公式求出枪姿ABC；
S3.6、将结果赋值给q_c；
S4、目标笛卡尔位置与当前笛卡尔位置做差，计算插补次数：
S5、通过插补值进行运动学逆运算：
a、取步骤S4中计算的cycles[0]～cycles[5]中的最大值max_cycle作为插补步长，使用for循环从0到max_cycle进行插补计算；
b、步骤S3中的q_c的X、Y、Z、A、B、C数据每次循环分别增加ΔX/max_cycle、ΔY/max_cycle、ΔZ/max_cycle、ΔA/max_cycle、ΔB/max_cycle和ΔC/max_cycle步长；
c、之后传入逆运算算法，当循环结束时，q_c坐标值与qc完全相等；
S6、通过雅可比矩阵JF(x1,...,xn)，获得机械臂速度、加速度、奇异点信息，进行轨迹可行性验证；
S7、计算笛卡尔坐标差值，利用高斯-约旦消元法求解：对消元结果进行积分，得到机械臂当前位姿对应的关节角度；
S8、末端旋转角度与阈值对比：通过软管缠绕阈值进行角度计算，具体流程如下：
a、算法处理的下限为360°，即缠绕一圈，上限由人工根据机械臂的性能和软管的安全拉伸长度给出；
b、人工操作机器人绕管，直到绕管情况达到可接受范围的最大值，记录为绕管上限阈值；
c、将所求得的关节角度与软管缠绕阈值做对比，因为工具旋转360°姿态与旋转前相同，所以解绕管默认旋转360°，根据工具实际情况，这个值可以修改；
S8.1、末端反向旋转：超过阈值就添加解绕管旋转，默认逆向旋转360度，将此解绕管动作加入当前轨迹段之前，并将解绕管后的关节角度代替原始关节角度；
S8.2、当前轨迹正常，不做处理：未超过阈值，则对此轨迹段不做处理，保留步骤S7得到的关节角度数据；
S9、将步骤S8中得到的关节角度带入下一次遍历：作为插补逆运算的输入值之一，重复步骤3-步骤8，直至所有轨迹遍历完成；
S10、算法处理完成，输出处理过的无缠绕轨迹。
2.根据权利要求1所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S1.1的笛卡尔空间运动信息的空间坐标X、Y、Z和工具姿态A、B、C，按照文件中的先后顺序存储进vector结构体里。
3.根据权利要求2所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的vector结构体每一项都包含6个参数：既X、Y、Z、A、B、C。
4.根据权利要求3所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S1.2的标准DH参数的计算先后顺序位：
(1).Z轴旋转，使用θ表示，旋转矩阵用Rot表示；
(2).Z轴平移，使用d表示，平移矩阵用Trans表示；
(3).X轴平移，使用a表示，平移矩阵用Trans表示；
(4).X轴旋转，使用α表示，旋转矩阵用Rot表示；
从第n-1轴到第n轴的变化表示为：n-1Tn＝Rot(zn-1,θn)·Trans(zn-1,dn)·Trans(xn,an)·Rot(xn,αn-1)。
5.根据权利要求4所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S1.3中，根据Euler定理，在三维空间中，任意一种旋转变换都可以归结为若干个沿着坐标轴旋转的组合，组合的个数不超过三个并且两个相邻的旋转需要沿着不同的坐标轴。
6.根据权利要求5所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：根据权利要求1所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S1.4中，建立数学模型具体是使用S1.2的DH矩阵获得从机械臂基坐标到法兰的变换矩阵0T1、1T2、2T3、3T4、4T5和5T6，使用S1.3的工具参数获得法兰到TCP，即工具末端的变换矩阵6TTCP然后进行连乘即可获得机械臂基坐标到TCP的数学模型0TTCP：
0TTCP＝0T1*1T2*2T3*3T4*4T5*5T6*6TTCP 。
7.根据权利要求2所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S4中，计算插补次数具体步骤如下：
a、将步骤S3中计算出的笛卡尔坐标q_c与步骤S2传入的笛卡尔坐标qc做差然后取绝对值，得到正double值ΔX、ΔY、ΔZ、ΔA、ΔB和ΔC；
b、对其中的ΔX、ΔY、ΔZ除以5得到插补次数cycles[0]、cycles[1]、和cycles[2]，其中5意味着每5mm进行一次插补；
c、对其中的ΔA、ΔB、ΔC除以0.5得到插补次数cycles[3]、cycles[4]、和cycles[5]，其中0.5意味着每0.5°进行一次插补。
8.根据权利要求1所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S6的可行性验证具体包括检查安全速度是否超出范围、加速度是否超出范围、断雅可比矩阵是否满秩。
9.根据权利要求1所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S8中的机械臂的性能可为轴限位。
10.根据权利要求1所述的一种防止机械臂软管管路缠绕的轨迹规划算法，其特征在于：所述的步骤S8中的软管长度的冗余量与实际安装情况相关，需要现场进行实验获得绕管上限。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211509960.1A CN115781677B (zh) | 2022-11-29 | 2022-11-29 | 一种防止机械臂软管管路缠绕的轨迹规划算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211509960.1A CN115781677B (zh) | 2022-11-29 | 2022-11-29 | 一种防止机械臂软管管路缠绕的轨迹规划算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115781677A true CN115781677A (zh) | 2023-03-14 |
| CN115781677B CN115781677B (zh) | 2024-08-20 |
Family
ID=85442937
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211509960.1A Active CN115781677B (zh) | 2022-11-29 | 2022-11-29 | 一种防止机械臂软管管路缠绕的轨迹规划算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115781677B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117733872A (zh) * | 2024-02-18 | 2024-03-22 | 华南理工大学 | 一种基于方向性能的串联机器人逆运动学控制方法 |
| CN117817654A (zh) * | 2023-11-23 | 2024-04-05 | 佛山科学技术学院 | 一种重载机器人运动规划方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4975856A (en) * | 1986-02-18 | 1990-12-04 | Robotics Research Corporation | Motion controller for redundant or nonredundant linkages |
| WO2018086226A1 (zh) * | 2016-11-09 | 2018-05-17 | 广州视源电子科技股份有限公司 | 机械臂的控制方法和装置 |
| CN112589797A (zh) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | 一种非球形手腕机械臂奇异点规避方法和系统 |
| CN113601512A (zh) * | 2021-08-23 | 2021-11-05 | 太原理工大学 | 一种机械臂奇异点的通用规避方法与系统 |
| CN114670191A (zh) * | 2022-03-14 | 2022-06-28 | 南京航空航天大学 | 一种七自由度肘偏置机械臂轨迹规划方法 |
-
2022
- 2022-11-29 CN CN202211509960.1A patent/CN115781677B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4975856A (en) * | 1986-02-18 | 1990-12-04 | Robotics Research Corporation | Motion controller for redundant or nonredundant linkages |
| WO2018086226A1 (zh) * | 2016-11-09 | 2018-05-17 | 广州视源电子科技股份有限公司 | 机械臂的控制方法和装置 |
| CN112589797A (zh) * | 2020-12-11 | 2021-04-02 | 中国科学院合肥物质科学研究院 | 一种非球形手腕机械臂奇异点规避方法和系统 |
| CN113601512A (zh) * | 2021-08-23 | 2021-11-05 | 太原理工大学 | 一种机械臂奇异点的通用规避方法与系统 |
| CN114670191A (zh) * | 2022-03-14 | 2022-06-28 | 南京航空航天大学 | 一种七自由度肘偏置机械臂轨迹规划方法 |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117817654A (zh) * | 2023-11-23 | 2024-04-05 | 佛山科学技术学院 | 一种重载机器人运动规划方法 |
| CN117733872A (zh) * | 2024-02-18 | 2024-03-22 | 华南理工大学 | 一种基于方向性能的串联机器人逆运动学控制方法 |
| CN117733872B (zh) * | 2024-02-18 | 2024-04-19 | 华南理工大学 | 一种基于方向性能的串联机器人逆运动学控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115781677B (zh) | 2024-08-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115781677A (zh) | 一种防止机械臂软管管路缠绕的轨迹规划算法 | |
| Barrientos-Diez et al. | Real-time kinematics of continuum robots: modelling and validation | |
| Dong et al. | Development of a slender continuum robotic system for on-wing inspection/repair of gas turbine engines | |
| CN113352319B (zh) | 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 | |
| Palmer et al. | Real-time method for tip following navigation of continuum snake arm robots | |
| Liu et al. | Elephant’s trunk robot: An extremely versatile under-actuated continuum robot driven by a single motor | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| Mohammad et al. | An efficient follow-the-leader strategy for continuum robot navigation and coiling | |
| EP0672507A1 (en) | Method for controlling the movements of an industrial robot at and near singularities | |
| US20120303318A1 (en) | Kinematic Predictor for Articulated Mechanisms | |
| CN107490965A (zh) | 一种空间自由漂浮机械臂的多约束轨迹规划方法 | |
| CN113146600B (zh) | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 | |
| JP2016027951A (ja) | ロボット装置の制御方法、およびロボット装置 | |
| CN114102606B (zh) | 机器人运动信息规划方法及相关装置 | |
| CN106844951B (zh) | 基于分段几何法求解超冗余机器人逆运动学的方法及系统 | |
| CN113276121B (zh) | 一种基于二次规划的冗余度机械臂移动障碍物躲避方法 | |
| CN114603558B (zh) | 混合空间过渡轨迹规划方法及装置 | |
| CN109176487A (zh) | 一种联动关节段标定方法、系统、设备、存储介质 | |
| CN111890349A (zh) | 一种四自由度机械臂运动规划方法 | |
| JP4760732B2 (ja) | 経路作成装置 | |
| CN115741695A (zh) | 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 | |
| JPH07182017A (ja) | 線条材のシミュレーション方法 | |
| Wild et al. | Efficient and scalable inverse kinematics for continuum robots | |
| CN113263496B (zh) | 一种六自由度机械臂优化路径的方法和计算机设备 | |
| CN115922684A (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 |