CN112157678B - 一种基于二分法的平面冗余机械臂碰撞位置检测方法 - Google Patents

一种基于二分法的平面冗余机械臂碰撞位置检测方法 Download PDF

Info

Publication number
CN112157678B
CN112157678B CN202011018189.9A CN202011018189A CN112157678B CN 112157678 B CN112157678 B CN 112157678B CN 202011018189 A CN202011018189 A CN 202011018189A CN 112157678 B CN112157678 B CN 112157678B
Authority
CN
China
Prior art keywords
joint
arm
collision position
mth
point
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
CN202011018189.9A
Other languages
English (en)
Other versions
CN112157678A (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.)
Hebei University of Technology
Original Assignee
Hebei University 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 Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202011018189.9A priority Critical patent/CN112157678B/zh
Publication of CN112157678A publication Critical patent/CN112157678A/zh
Application granted granted Critical
Publication of CN112157678B publication Critical patent/CN112157678B/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
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0095—Means or methods for testing manipulators
    • 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/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones

Landscapes

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

Abstract

本发明公开了一种基于二分法的平面冗余机械臂碰撞位置检测方法，包括步骤一、搭建平面冗余机械臂碰撞位置检测系统，构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m根臂杆与障碍物发生碰撞，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点。该方法在浓雾、黑暗、水下等能见度低或者红外、声波传播受限的恶劣环境下依旧可以检测碰撞位置，适用范围更广，同时为平面冗余机械臂在非视觉情况下的避障提供理论基础。

Description

一种基于二分法的平面冗余机械臂碰撞位置检测方法
技术领域
本发明属于工业机器人碰撞检测技术领域，具体是一种基于二分法的平面冗余机械臂碰撞位置检测方法。
背景技术
目前大多数机器人都依靠雷达或者机器视觉检测环境中的障碍物。机器视觉主要通过采集环境图像，利用机器学习识别障碍物，以实现避障，但是这种检测方法在浓雾、黑暗、水下等能见度低的恶劣环境下并不适用，恶劣环境会使拍摄的图像不清晰，容易导致检测不准。雷达虽然能够检测障碍物，但是在红外、声波传播受限的环境中信号容易受到干扰，进而影响检测精度。
申请号为201911041252.8的文献提出了一种基于主动红外双目视觉的避障系统和避障方法、申请号为201611197327.8的文献所提出的一种基于视觉的机械臂避障方法，这些现有的避障方法都是基于机器视觉实现的，对于环境的能见度要求较高。
因此，亟待研发一种能够在恶劣受限环境下依旧可以对障碍物的位置信息进行检测以及避障的方法。
发明内容
针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于二分法的平面冗余机械臂碰撞位置检测方法。
本发明解决所述技术问题采用的技术方案是：
一种基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，该方法包括以下步骤：
步骤一、搭建平面冗余机械臂碰撞位置检测系统，构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；
步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m根臂杆与障碍物发生碰撞，发生碰撞后上位机控制第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转；设定第m根臂杆期望绕二分点旋转的次数为n，在平面冗余机械臂旋转过程中，首先第m根臂杆绕其臂杆
Figure BDA0002699808750000011
二分点做首次旋转运动至一号理想位置，通过旋转运动的旋转方向结合电机理论工作电流和实际工作电流的差值与阈值的关系判断碰撞位置所在区间，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点；二分点指第m根臂杆上由第m+1关节指向第m个关节方向上的位于臂杆长度
Figure BDA0002699808750000021
(k为不大于2n的自然数)处的点。
与现有技术相比，本发明的有益效果在于：
(1)本发明分别以发生碰撞的臂杆上不同位置处的二分点为转动中心，多次旋转发生碰撞的臂杆，进一步确定碰撞位置；在碰撞位置检测过程中并没有用到相机等其他传感器，使得该方法在浓雾、黑暗、水下等能见度低或者红外、声波传播受限的恶劣环境下依旧可以检测碰撞位置，适用范围更广，同时为平面冗余机械臂在非视觉情况下的避障提供理论基础。
(2)本发明在碰撞位置检测和避障的过程中只用到了各个关节内部安装的关节位置编码器和电机驱动器，并没有用到平面冗余机械臂以外的传感器，不仅减少了传感器的使用，而且大大减小了空间占用率，在一些空间受限的情况下依旧可以检测碰撞位置。
(3)本发明由于不需要借助外部传感器来确定碰撞位置，减小了信息采集和电线布局的难度，同时降低了多传感器之间信息融合的复杂性。
(4)由于现有的平面冗余机械臂的关节处都安装有检测关节参数的传感器，因此本发明的方法能够直接适用于现有的平面冗余机械臂，而不需要更改平面冗余机械臂的结构或增加其他机械零部件和传感器，适用性更强。
附图说明
图1为本发明的平面冗余机械臂的结构示意图以及二分点在平面冗余机械臂上的分布示意图；
图2为本发明的平面冗余机械臂绕第m根臂杆
Figure BDA0002699808750000022
二分点的旋转示意图；
图3为本发明的平面冗余机械臂绕第m根臂杆
Figure BDA0002699808750000023
二分点的旋转示意图；
图4为本发明的碰撞位置在
Figure BDA0002699808750000024
二分点与
Figure BDA0002699808750000025
二分点的中点的位置示意图；
图5为本发明的平面冗余机械臂绕第m根臂杆
Figure BDA0002699808750000026
二分点的旋转示意图；
图6为本发明的碰撞位置在
Figure BDA0002699808750000027
二分点和
Figure BDA0002699808750000028
二分点的中点的位置示意图；
图7为本发明的包络圆的位置示意图。
具体实施方式
下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。
本发明同时提供了一种基于二分法的平面冗余机械臂碰撞位置检测方法(简称方法，参见图1-7)，其特征在于，该方法包括以下步骤：
步骤一、搭建平面冗余机械臂碰撞位置检测系统，并构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；
平面冗余机械臂碰撞位置检测系统包括上位机、平面冗余机械臂和控制箱，平面冗余机械臂的动力学模型存储在上位机中；平面冗余机械臂包括基座、多根臂杆(本实施例有M根臂杆)和与臂杆数量相同的关节；每根臂杆的一端与各自的关节固连，另一端与下一个关节转动连接；每个关节处均设有电机、关节位置编码器和电机驱动器，电机驱动器采集电机的实际工作电流，关节位置编码器采集关节角度、关节角速度、关节角加速度和关节扭矩；然后将各个关节的关节角度、关节角速度、关节角加速度代入平面冗余机械臂的动力学模型中，得到平面冗余机械臂各个关节的理论扭矩，并通过关节的理论扭矩得到相应电机的理论工作电流；上位机通过电机的理论工作电流与实际工作电流的差值是否超过阈值判断平面冗余机械臂是否与障碍物发生碰撞；当电机的理论工作电流与实际工作电流的差值超过阈值时，则表示平面冗余机械臂与障碍物发生碰撞，且能够得到发生碰撞的具体臂杆；当电机的理论工作电流与实际工作电流的差值未超过阈值，表示平面冗余机械臂与障碍物未发生碰撞；
步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m(m<M)根臂杆与障碍物发生碰撞，发生碰撞后上位机控制第m-2个关节、第m-1个关节、第m个关节继续转动，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转；设定第m根臂杆期望绕二分点旋转的次数为n，在平面冗余机械臂旋转过程中，首先第m根臂杆绕其臂杆
Figure BDA0002699808750000031
二分点(即第m根臂杆的中点)做首次旋转运动至一号理想位置，通过旋转运动的旋转方向结合电机理论工作电流和实际工作电流的差值与阈值的关系判断碰撞位置所在区间，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处(即两个极限点)的两个二分点的中点为下一次旋转运动的转动中心继续做旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点；二分点指第m根臂杆上由第m+1关节指向第m个关节方向上的位于臂杆长度
Figure BDA0002699808750000032
(k为不大于2n的自然数)处的点，如图1所示。
2-1、设定平面冗余机械臂的碰撞位置X＝[θm-2,θm-1,θm]，其中θm-2、θm-1、θm分别为第m-2个关节、第m-1个关节、第m个关节在碰撞位置时的关节角度；上位机控制第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆
Figure BDA0002699808750000033
二分点(即第m根臂杆的中点)做首次旋转运动至一号理想位置
Figure BDA0002699808750000034
Figure BDA0002699808750000035
Figure BDA0002699808750000036
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至一号理想位置需要转动的角度(如图2所示)，分别满足公式(1)-(3)；
Figure BDA0002699808750000037
Figure BDA0002699808750000038
Figure BDA0002699808750000041
式(2)中：
Figure BDA0002699808750000042
Figure BDA0002699808750000043
式(3)和式(4)中的
Figure BDA0002699808750000044
表示第m个关节在一号理想位置的坐标；
Figure BDA0002699808750000045
分别满足公式(6)和(7)：
Figure BDA0002699808750000046
Figure BDA0002699808750000047
式(1)-(7)中，
Figure BDA0002699808750000048
为第m根臂杆从碰撞位置运动至一号理想位置需要转动的角度；θi表示前m个关节中的第i个关节在碰撞位置的关节角度，i＝1,2…m；θj表示前i个关节中任意一个关节在碰撞位置的关节角度，j＝1,2…i；li为前m根臂杆中任意一根臂杆的长度，i＝1,2…m；
在第m根臂杆绕其臂杆
Figure BDA0002699808750000049
二分点旋转过程中，上位机继续监测前m个关节处的电机理论工作电流与实际工作电流的差值是否超过阈值；
当第m根臂杆绕其臂杆
Figure BDA00026998087500000410
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，则表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000411
二分点靠近第m+1个关节的一侧，取第m根臂杆
Figure BDA00026998087500000412
二分点(即第m根臂杆与第m+1个关节连接的端点)与第m根臂杆
Figure BDA00026998087500000413
二分点的中点(即第m根臂杆
Figure BDA00026998087500000414
二分点)作为转动中心，继续执行步骤2-2；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000415
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure BDA00026998087500000416
二分点与第m根臂杆
Figure BDA00026998087500000417
二分点(即第m根臂杆与第m个关节连接的端点)的中点(即第m根臂杆
Figure BDA00026998087500000418
二分点)作为转动中心，继续执行步骤2-3；
当第m根臂杆绕其臂杆
Figure BDA00026998087500000419
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000420
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure BDA00026998087500000421
二分点与第m根臂杆
Figure BDA00026998087500000422
二分点(即第m根臂杆与第m个关节连接的端点)的中点(即第m根臂杆
Figure BDA00026998087500000423
二分点)作为转动中心，继续执行步骤2-3；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000424
二分点靠近第m+1个关节的一侧，取第m根臂杆
Figure BDA0002699808750000051
二分点(即第m根臂杆与第m+1个关节连接的端点)与第m根臂杆
Figure BDA0002699808750000052
二分点的中点(即第m根臂杆
Figure BDA0002699808750000053
二分点)作为转动中心，继续执行步骤2-2；
2-2、首先上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆
Figure BDA0002699808750000054
二分点作第二次旋转运动至二号理想位置
Figure BDA0002699808750000055
Figure BDA0002699808750000056
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至二号理想位置需要转动的角度，参见图3；其中，
Figure BDA0002699808750000057
Figure BDA0002699808750000058
Figure BDA0002699808750000059
式(9)中：
Figure BDA00026998087500000510
Figure BDA00026998087500000511
式(10)和式(11)中的
Figure BDA00026998087500000512
表示第m个关节在二号理想位置的坐标；
Figure BDA00026998087500000513
分别满足公式(13)和(14)：
Figure BDA00026998087500000514
Figure BDA00026998087500000515
式(8)-(14)中，
Figure BDA00026998087500000516
为第m根臂杆从碰撞位置运动至二号理想位置需要转动的角度；
当第m根臂杆绕其臂杆
Figure BDA00026998087500000517
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000518
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure BDA00026998087500000519
二分点与第m根臂杆
Figure BDA00026998087500000520
二分点的中点作为转动中心继续旋转；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000521
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure BDA00026998087500000522
二分点与第m根臂杆
Figure BDA00026998087500000523
二分点的中点作为转动中心继续旋转，参见图4；
当第m根臂杆绕其臂杆
Figure BDA00026998087500000524
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA0002699808750000061
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure BDA0002699808750000062
二分点与第m根臂杆
Figure BDA0002699808750000063
二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA0002699808750000064
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure BDA0002699808750000065
二分点与第m根臂杆
Figure BDA0002699808750000066
二分点的中点作为转动中心继续旋转；
2-3、首先上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节、第m个关节转动，使第m根臂杆绕其臂杆
Figure BDA0002699808750000067
二分点作第二次旋转运动至三号理想位置
Figure BDA0002699808750000068
Figure BDA0002699808750000069
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至三号理想位置需要转动的角度，参见图5；其中，
Figure BDA00026998087500000610
Figure BDA00026998087500000611
Figure BDA00026998087500000612
式(16)中：
Figure BDA00026998087500000613
Figure BDA00026998087500000614
式(17)和式(18)中的
Figure BDA00026998087500000615
表示第m个关节在三号理想位置的坐标；
Figure BDA00026998087500000616
分别满足公式(20)和(21)；
Figure BDA00026998087500000617
Figure BDA00026998087500000618
式(15)-(21)中，
Figure BDA00026998087500000619
为第m根臂杆从碰撞位置运动至三号理想位置需要转动的角度；
当第m根臂杆绕其臂杆
Figure BDA00026998087500000620
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000621
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure BDA00026998087500000622
二分点与第m根臂杆
Figure BDA00026998087500000623
二分点的中点作为转动中心继续旋转，参见图6；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure BDA00026998087500000624
二分点远离第m+1个关节的一侧，此时取第m根臂杆
Figure BDA0002699808750000071
二分点与第m根臂杆
Figure BDA0002699808750000072
二分点的中点作为转动中心继续旋转；
当第m根臂杆绕其臂杆
Figure BDA0002699808750000073
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA0002699808750000074
二分点远离第m+1个关节的一侧，此时取第m根臂杆
Figure BDA0002699808750000075
二分点与第m根臂杆
Figure BDA0002699808750000076
二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆
Figure BDA0002699808750000077
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure BDA0002699808750000078
二分点与第m根臂杆
Figure BDA0002699808750000079
二分点的中点作为转动中心继续旋转；
重复执行上述操作，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处(两个极限点)的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置为第n次旋转运动确定的区间的中点；
本发明还提供一种基于二分法的平面冗余机械臂的避障方法，其特征在于，该方法的具体过程为：
根据上述位置检测方法得到的碰撞位置，以第m根臂杆的长度lm的1/4为直径作一个与第m根臂杆相切的包络圆，使得障碍物完全位于包络圆内，包络圆与第m根臂杆的切点即为碰撞位置，进而确定包络圆的位置和大小，参见图7；然后上位机根据包络圆的位置和大小规划平面冗余机械臂的避障路径，确保平面冗余机械臂的运动空间不会与包络圆的空间重叠，保证平面冗余机械臂在避障过程中不会进入包络圆的区域。
优选地，当上位机无法规划出平面冗余机械臂避障路径时，通过以下两种方式改善：
第一种：增加第m根臂杆期望绕二分点旋转的次数n，更加精准地检测碰撞位置；但是这个方式存在局限性，当绕二分点旋转的次数超过8次时，对于碰撞位置的检测已经不会有太多改变，表示此时碰撞位置的检测已经十分精确；
第二种：将包络圆的直径增大至D1，D1＝α·D0，α为大于1的系数，D0为包络圆的初始直径，即用一个更大的圆来包络障碍物，实现避障；当包络圆的直径大于第m根臂杆的长度lm时，表示此时的障碍物太大，平面冗余机械臂无法对这个障碍物进行避障；
若采用上述两种方式平面冗余机械臂都无法避障时，则采取人工避障。
本发明未述及之处适用于现有技术。

Claims (6)

1.一种基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，该方法包括以下步骤：
步骤一、搭建平面冗余机械臂碰撞位置检测系统，构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；
步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m根臂杆与障碍物发生碰撞，发生碰撞后上位机控制第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转；设定第m根臂杆期望绕二分点旋转的次数为n，在平面冗余机械臂旋转过程中，首先第m根臂杆绕其臂杆
Figure FDA0003218813910000011
二分点做首次旋转运动至一号理想位置，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点；二分点指第m根臂杆上由第m+1关节指向第m个关节方向上的位于臂杆长度
Figure FDA0003218813910000012
(k为不大于2n的自然数)处的点；具体过程为：
2-1在第m根臂杆绕其臂杆
Figure FDA0003218813910000013
二分点作首次旋转运动至一号理想位置过程中，上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值是否超过阈值；
当第m根臂杆绕其臂杆
Figure FDA0003218813910000014
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，则表示碰撞位置位于第m根臂杆
Figure FDA0003218813910000015
二分点靠近第m+1个关节的一侧，取第m根臂杆
Figure FDA0003218813910000016
二分点与第m根臂杆
Figure FDA0003218813910000017
二分点的中点作为转动中心，继续执行步骤2-2；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure FDA0003218813910000018
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure FDA0003218813910000019
二分点与第m根臂杆
Figure FDA00032188139100000110
二分点的中点作为转动中心，继续执行步骤2-3；
当第m根臂杆绕其臂杆
Figure FDA00032188139100000111
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000112
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure FDA00032188139100000113
二分点与第m根臂杆
Figure FDA00032188139100000114
二分点的中点作为转动中心，继续执行步骤2-3；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000115
二分点靠近第m+1个关节的一侧，取第m根臂杆
Figure FDA00032188139100000116
二分点与第m根臂杆
Figure FDA00032188139100000117
二分点的中点作为转动中心，继续执行步骤2-2；
2-2上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆
Figure FDA0003218813910000021
二分点作第二次旋转运动至二号理想位置；
当第m根臂杆绕其臂杆
Figure FDA0003218813910000022
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA0003218813910000023
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure FDA0003218813910000024
二分点与第m根臂杆
Figure FDA0003218813910000025
二分点的中点作为转动中心继续旋转；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure FDA0003218813910000026
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure FDA0003218813910000027
二分点与第m根臂杆
Figure FDA0003218813910000028
二分点的中点作为转动中心继续旋转；
当第m根臂杆绕其臂杆
Figure FDA0003218813910000029
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000210
二分点远离第m+1个关节的一侧，取第m根臂杆
Figure FDA00032188139100000211
二分点与第m根臂杆
Figure FDA00032188139100000212
二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000213
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure FDA00032188139100000214
二分点与第m根臂杆
Figure FDA00032188139100000215
二分点的中点作为转动中心继续旋转；
2-3上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节转动，使第m根臂杆绕其臂杆
Figure FDA00032188139100000216
二分点作第二次旋转运动至三号理想位置；
当第m根臂杆绕其臂杆
Figure FDA00032188139100000217
二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000218
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure FDA00032188139100000219
二分点与第m根臂杆
Figure FDA00032188139100000220
二分点的中点作为转动中心继续旋转；若未超过阈值，则表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000221
二分点远离第m+1个关节的一侧，此时取第m根臂杆
Figure FDA00032188139100000222
二分点与第m根臂杆
Figure FDA00032188139100000223
二分点的中点作为转动中心继续旋转；
当第m根臂杆绕其臂杆
Figure FDA00032188139100000224
二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA00032188139100000225
二分点远离第m+1个关节的一侧，此时取第m根臂杆
Figure FDA0003218813910000031
二分点与第m根臂杆
Figure FDA0003218813910000032
二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆
Figure FDA0003218813910000033
二分点靠近第m+1个关节的一侧，则取第m根臂杆
Figure FDA0003218813910000034
二分点与第m根臂杆
Figure FDA0003218813910000035
二分点的中点作为转动中心继续旋转；
重复执行上述操作，第m根臂杆始终以与上一次旋转运动检测出的碰撞位置所在区间的两端处的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置为第n次旋转运动确定的区间的中点。
2.根据权利要求1所述的基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，步骤2-1中第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至一号理想位置需要转动的角度的计算过程为：
设定碰撞位置X＝[θm-2,θm-1,θm]，其中θm-2、θm-1、θm分别为第m-2个关节、第m-1个关节、第m个关节在碰撞位置时的关节角度；一号理想位置
Figure FDA0003218813910000036
Figure FDA0003218813910000037
Figure FDA0003218813910000038
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至一号理想位置需要转动的角度，分别满足公式(1)-(3)；
Figure FDA0003218813910000039
Figure FDA00032188139100000310
Figure FDA00032188139100000311
式(2)中：
Figure FDA00032188139100000312
Figure FDA00032188139100000313
式(3)和(4)中
Figure FDA00032188139100000314
表示第m个关节在一号理想位置的坐标；
Figure FDA00032188139100000315
分别满足公式(6)和(7)：
Figure FDA00032188139100000316
Figure FDA00032188139100000317
式(1)-(7)中，
Figure FDA00032188139100000318
为第m根臂杆从碰撞位置运动至一号理想位置需要转动的角度；θi表示前m个关节中第i个关节在碰撞位置的关节角度，i＝1,2…m；θj表示前i个关节中任意一个关节在碰撞位置的关节角度，j＝1,2…i；li为前m根臂杆中任意一根臂杆的长度，i＝1,2…m。
3.根据权利要求1所述的基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，步骤2-2中第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至二号理想位置需要转动的角度的计算过程为：
设定碰撞位置X＝[θm-2,θm-1,θm]，其中θm-2、θm-1、θm分别为第m-2个关节、第m-1个关节、第m个关节在碰撞位置时的关节角度；二号理想位置
Figure FDA0003218813910000041
Figure FDA0003218813910000042
Figure FDA0003218813910000043
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至二号理想位置需要转动的角度，分别满足公式(8)-(10)；
Figure FDA0003218813910000044
Figure FDA0003218813910000045
Figure FDA0003218813910000046
式(9)中：
Figure FDA0003218813910000047
Figure FDA0003218813910000048
式(10)和(11)中的
Figure FDA0003218813910000049
表示第m个关节在二号理想位置的坐标；
Figure FDA00032188139100000410
分别满足公式(13)和(14)：
Figure FDA00032188139100000411
Figure FDA00032188139100000412
式(8)-(14)中，
Figure FDA00032188139100000413
为第m根臂杆从碰撞位置运动至二号理想位置需要转动的角度；θi表示前m个关节中第i个关节在碰撞位置的关节角度，i＝1,2…m；θj表示前i个关节中任意一个关节在碰撞位置的关节角度，j＝1,2…i；li为前m根臂杆中任意一根臂杆的长度，i＝1,2…m。
4.根据权利要求1所述的基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，步骤2-3中第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至三号理想位置需要转动的角度的计算过程为：
设定碰撞位置X＝[θm-2,θm-1,θm]，其中θm-2、θm-1、θm分别为第m-2个关节、第m-1个关节、第m个关节在碰撞位置时的关节角度；三号理想位置
Figure FDA00032188139100000414
Figure FDA00032188139100000415
Figure FDA00032188139100000416
分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至三号理想位置需要转动的角度，分别满足公式(15)-(17)；
Figure FDA00032188139100000417
Figure FDA00032188139100000418
Figure FDA0003218813910000051
式(16)中：
Figure FDA0003218813910000052
Figure FDA0003218813910000053
式(17)(18)中的x'm3、y'm3表示第m个关节在三号理想位置的坐标；x'm3、y'm3分别满足公式(20)和(21)：
Figure FDA0003218813910000054
Figure FDA0003218813910000055
式(15)-(21)中，
Figure FDA0003218813910000056
为第m根臂杆从碰撞位置运动至三号理想位置需要转动的角度；θi表示前m个关节中第i个关节在碰撞位置的关节角度，i＝1,2…m；θj表示前i个关节中任意一个关节在碰撞位置的关节角度，j＝1,2…i；li为前m根臂杆中任意一根臂杆的长度，i＝1,2…m。
5.一种基于二分法的平面冗余机械臂的避障方法，其特征在于，该方法的具体过程为：
根据权利要求1得到的碰撞位置，以第m根臂杆的长度lm的1/4为直径作一个与第m根臂杆相切的包络圆，使得障碍物完全位于包络圆内，包络圆与第m根臂杆的切点为碰撞位置，确定包络圆的位置和大小；上位机根据包络圆的位置和大小规划平面冗余机械臂的避障路径。
6.根据权利要求5所述的基于二分法的平面冗余机械臂的避障方法，其特征在于，当上位机无法规划出平面冗余机械臂的避障路径时，通过以下两种方式改善：
第一种：增加第m根臂杆期望绕二分点旋转的次数n，n≤8；
第二种：将包络圆的直径增大至D1，D1＝α·D0，α为大于1的系数，D0为包络圆的初始直径；当D1大于第m根臂杆长度时，表示障碍物太大，平面冗余机械臂无法避障；
若采用上述两种方式平面冗余机械臂都无法避障时，则采取人工避障。
CN202011018189.9A 2020-09-24 2020-09-24 一种基于二分法的平面冗余机械臂碰撞位置检测方法 Active CN112157678B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011018189.9A CN112157678B (zh) 2020-09-24 2020-09-24 一种基于二分法的平面冗余机械臂碰撞位置检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011018189.9A CN112157678B (zh) 2020-09-24 2020-09-24 一种基于二分法的平面冗余机械臂碰撞位置检测方法

Publications (2)

Publication Number Publication Date
CN112157678A CN112157678A (zh) 2021-01-01
CN112157678B true CN112157678B (zh) 2021-10-22

Family

ID=73863733

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011018189.9A Active CN112157678B (zh) 2020-09-24 2020-09-24 一种基于二分法的平面冗余机械臂碰撞位置检测方法

Country Status (1)

Country Link
CN (1) CN112157678B (zh)

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102880783A (zh) * 2012-07-27 2013-01-16 西安空间无线电技术研究所 一种确定同轴结构微放电阈值的方法
JP2016040066A (ja) * 2014-08-13 2016-03-24 キヤノン株式会社 経路生成方法、経路生成装置、ロボット装置、プログラム及び記録媒体
CN106482739A (zh) * 2016-11-30 2017-03-08 英华达（上海）科技有限公司 自动导引运输车导航方法
CN106737689A (zh) * 2017-01-19 2017-05-31 哈尔滨工业大学深圳研究生院 基于模式函数的超冗余机械臂混合逆向求解方法及系统
CN108000521A (zh) * 2017-12-06 2018-05-08 天津大学 一种无传感器式协作机器人碰撞检测方法
WO2018214825A1 (zh) * 2017-05-26 2018-11-29 杭州海康机器人技术有限公司 一种检测未知位置的障碍物存在概率的方法和装置
CN109227549A (zh) * 2018-11-08 2019-01-18 汕头大学 一种基于切线递推的机器人平滑避障运动规划方法
CN110000815A (zh) * 2019-04-09 2019-07-12 深圳前海达闼云端智能科技有限公司 一种碰撞检测方法、装置、电子设备及存储介质
CN110047143A (zh) * 2019-03-04 2019-07-23 南昌大学 一种基于空间细分与动态包围盒的连续碰撞检测方法
CN110161966A (zh) * 2019-07-04 2019-08-23 合肥工业大学 一种多轴联动轮廓误差的实时检测方法
CN110696000A (zh) * 2019-11-21 2020-01-17 河北工业大学 一种机械臂试探感知的避障方法
CN111230881A (zh) * 2020-02-24 2020-06-05 华南理工大学 一种六自由度机械臂空间轨迹优化方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10571902B2 (en) * 2016-10-12 2020-02-25 Sisu Devices Llc Robotic programming and motion control
US11119488B2 (en) * 2018-01-17 2021-09-14 Agco International Gmbh Steering controller for an autonomous vehicle with velocity regulation
CN108356820B (zh) * 2018-01-17 2020-05-12 浙江大学 一种多关节机械臂手动操控的逆运动学求解方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102880783A (zh) * 2012-07-27 2013-01-16 西安空间无线电技术研究所 一种确定同轴结构微放电阈值的方法
JP2016040066A (ja) * 2014-08-13 2016-03-24 キヤノン株式会社 経路生成方法、経路生成装置、ロボット装置、プログラム及び記録媒体
CN106482739A (zh) * 2016-11-30 2017-03-08 英华达（上海）科技有限公司 自动导引运输车导航方法
CN106737689A (zh) * 2017-01-19 2017-05-31 哈尔滨工业大学深圳研究生院 基于模式函数的超冗余机械臂混合逆向求解方法及系统
WO2018214825A1 (zh) * 2017-05-26 2018-11-29 杭州海康机器人技术有限公司 一种检测未知位置的障碍物存在概率的方法和装置
CN108000521A (zh) * 2017-12-06 2018-05-08 天津大学 一种无传感器式协作机器人碰撞检测方法
CN109227549A (zh) * 2018-11-08 2019-01-18 汕头大学 一种基于切线递推的机器人平滑避障运动规划方法
CN110047143A (zh) * 2019-03-04 2019-07-23 南昌大学 一种基于空间细分与动态包围盒的连续碰撞检测方法
CN110000815A (zh) * 2019-04-09 2019-07-12 深圳前海达闼云端智能科技有限公司 一种碰撞检测方法、装置、电子设备及存储介质
CN110161966A (zh) * 2019-07-04 2019-08-23 合肥工业大学 一种多轴联动轮廓误差的实时检测方法
CN110696000A (zh) * 2019-11-21 2020-01-17 河北工业大学 一种机械臂试探感知的避障方法
CN111230881A (zh) * 2020-02-24 2020-06-05 华南理工大学 一种六自由度机械臂空间轨迹优化方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A method for collision free sensor network based navigation of flying robots among moving and steady obstacles;Hang Li; Andrey V. Savkin;《 2017 36th Chinese Control Conference》;20170911;全文 *
受限空间机械臂运动规划研究;王占鹏;《中国优秀硕士学位论文全文数据库信息科技辑》;20180315(第03期);全文 *
射线与地形的碰撞检测;CSDN用户;《CSDN》;20121023;第1页 *

Also Published As

Publication number Publication date
CN112157678A (zh) 2021-01-01

Similar Documents

Publication Publication Date Title
CN110696000B (zh) 一种机械臂试探感知的避障方法
JP3207728B2 (ja) 冗長マニピュレータの制御方法
EP0528054B1 (en) Detected position correcting method
CN210038147U (zh) 扫描角度调整装置、激光雷达系统及载具
EP2033746A1 (en) Articulated robot and its control program
JP5743495B2 (ja) ロボット制御装置
US20200376666A1 (en) Robot system and operation method
CN108789416B (zh) 用于控制双机械臂柔性带电作业的系统及方法
JP5370128B2 (ja) ロボットの故障検出装置
CN113268055B (zh) 工程车辆避障控制方法、装置及机械设备
WO2006089887A2 (en) A system for calibration of an industrial robot and a method thereof
CN113138397B (zh) 一种无人机避障装置及无人机
CN109623851B (zh) 一种机械臂关节零位装置和回零方法
EP2377653A1 (en) Method and system for controlling large-scale engineering manipulator
JP7109161B2 (ja) 多関節ロボットの機構モデルパラメータ推定方法
CN108127661A (zh) 机器人控制装置
CN106413995B (zh) 紧凑型机器人设备
EP3666476A1 (en) Trajectory generation system and trajectory generating method
CN109176503B (zh) 一种基于仿生触须的智能路径探测机器人及路径探测方法
CN112157678B (zh) 一种基于二分法的平面冗余机械臂碰撞位置检测方法
JP2010076058A (ja) 多関節型マニピュレータの制御装置及び多関節型マニピュレータの手先動作軌道生成方法
Sonali et al. Obstacle avoidance for a mobile exploration robot using a single ultrasonic range sensor
CN115351818A (zh) 机械臂碰撞检测系统、方法、机械臂、机器人及芯片
CN109311155A (zh) 一种工业机器人的工具坐标系原点的标定方法及装置
CN110181490B (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