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

本发明属于工业机器人碰撞检测技术领域，具体是一种基于二分法的平面冗余机械臂碰撞位置检测方法。

背景技术：

目前大多数机器人都依靠雷达或者机器视觉检测环境中的障碍物。机器视觉主要通过采集环境图像，利用机器学习识别障碍物，以实现避障，但是这种检测方法在浓雾、黑暗、水下等能见度低的恶劣环境下并不适用，恶劣环境会使拍摄的图像不清晰，容易导致检测不准。雷达虽然能够检测障碍物，但是在红外、声波传播受限的环境中信号容易受到干扰，进而影响检测精度。

申请号为201911041252.8的文献提出了一种基于主动红外双目视觉的避障系统和避障方法、申请号为201611197327.8的文献所提出的一种基于视觉的机械臂避障方法，这些现有的避障方法都是基于机器视觉实现的，对于环境的能见度要求较高。

因此，亟待研发一种能够在恶劣受限环境下依旧可以对障碍物的位置信息进行检测以及避障的方法。

技术实现要素：

针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于二分法的平面冗余机械臂碰撞位置检测方法。

本发明解决所述技术问题采用的技术方案是：

一种基于二分法的平面冗余机械臂碰撞位置检测方法，其特征在于，该方法包括以下步骤：

步骤一、搭建平面冗余机械臂碰撞位置检测系统，构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；

步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m根臂杆与障碍物发生碰撞，发生碰撞后上位机控制第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转；设定第m根臂杆期望绕二分点旋转的次数为n，在平面冗余机械臂旋转过程中，首先第m根臂杆绕其臂杆二分点做首次旋转运动至一号理想位置，通过旋转运动的旋转方向结合电机理论工作电流和实际工作电流的差值与阈值的关系判断碰撞位置所在区间，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点；二分点指第m根臂杆上由第m+1关节指向第m个关节方向上的位于臂杆长度(k为不大于2ⁿ的自然数)处的点。

与现有技术相比，本发明的有益效果在于：

(1)本发明分别以发生碰撞的臂杆上不同位置处的二分点为转动中心，多次旋转发生碰撞的臂杆，进一步确定碰撞位置；在碰撞位置检测过程中并没有用到相机等其他传感器，使得该方法在浓雾、黑暗、水下等能见度低或者红外、声波传播受限的恶劣环境下依旧可以检测碰撞位置，适用范围更广，同时为平面冗余机械臂在非视觉情况下的避障提供理论基础。

(2)本发明在碰撞位置检测和避障的过程中只用到了各个关节内部安装的关节位置编码器和电机驱动器，并没有用到平面冗余机械臂以外的传感器，不仅减少了传感器的使用，而且大大减小了空间占用率，在一些空间受限的情况下依旧可以检测碰撞位置。

(3)本发明由于不需要借助外部传感器来确定碰撞位置，减小了信息采集和电线布局的难度，同时降低了多传感器之间信息融合的复杂性。

(4)由于现有的平面冗余机械臂的关节处都安装有检测关节参数的传感器，因此本发明的方法能够直接适用于现有的平面冗余机械臂，而不需要更改平面冗余机械臂的结构或增加其他机械零部件和传感器，适用性更强。

附图说明

图1为本发明的平面冗余机械臂的结构示意图以及二分点在平面冗余机械臂上的分布示意图；

图2为本发明的平面冗余机械臂绕第m根臂杆二分点的旋转示意图；

图3为本发明的平面冗余机械臂绕第m根臂杆二分点的旋转示意图；

图4为本发明的碰撞位置在二分点与二分点的中点的位置示意图；

图5为本发明的平面冗余机械臂绕第m根臂杆二分点的旋转示意图；

图6为本发明的碰撞位置在二分点和二分点的中点的位置示意图；

图7为本发明的包络圆的位置示意图。

具体实施方式

下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。

本发明同时提供了一种基于二分法的平面冗余机械臂碰撞位置检测方法(简称方法，参见图1-7)，其特征在于，该方法包括以下步骤：

步骤一、搭建平面冗余机械臂碰撞位置检测系统，并构建平面冗余机械臂的动力学模型，并判断平面冗余机械臂与障碍物是否发生碰撞；

平面冗余机械臂碰撞位置检测系统包括上位机、平面冗余机械臂和控制箱，平面冗余机械臂的动力学模型存储在上位机中；平面冗余机械臂包括基座、多根臂杆(本实施例有m根臂杆)和与臂杆数量相同的关节；每根臂杆的一端与各自的关节固连，另一端与下一个关节转动连接；每个关节处均设有电机、关节位置编码器和电机驱动器，电机驱动器采集电机的实际工作电流，关节位置编码器采集关节角度、关节角速度、关节角加速度和关节扭矩；然后将各个关节的关节角度、关节角速度、关节角加速度代入平面冗余机械臂的动力学模型中，得到平面冗余机械臂各个关节的理论扭矩，并通过关节的理论扭矩得到相应电机的理论工作电流；上位机通过电机的理论工作电流与实际工作电流的差值是否超过阈值判断平面冗余机械臂是否与障碍物发生碰撞；当电机的理论工作电流与实际工作电流的差值超过阈值时，则表示平面冗余机械臂与障碍物发生碰撞，且能够得到发生碰撞的具体臂杆；当电机的理论工作电流与实际工作电流的差值未超过阈值，表示平面冗余机械臂与障碍物未发生碰撞；

步骤二、当平面冗余机械臂与障碍物发生碰撞时，设定平面冗余机械臂的第m(m<m)根臂杆与障碍物发生碰撞，发生碰撞后上位机控制第m-2个关节、第m-1个关节、第m个关节继续转动，使第m根臂杆绕其臂杆上的二分点从碰撞位置朝向理想位置旋转；设定第m根臂杆期望绕二分点旋转的次数为n，在平面冗余机械臂旋转过程中，首先第m根臂杆绕其臂杆二分点(即第m根臂杆的中点)做首次旋转运动至一号理想位置，通过旋转运动的旋转方向结合电机理论工作电流和实际工作电流的差值与阈值的关系判断碰撞位置所在区间，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处(即两个极限点)的两个二分点的中点为下一次旋转运动的转动中心继续做旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置位于第n次旋转运动确定的区间的中点；二分点指第m根臂杆上由第m+1关节指向第m个关节方向上的位于臂杆长度(k为不大于2ⁿ的自然数)处的点，如图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根臂杆绕其臂杆二分点(即第m根臂杆的中点)做首次旋转运动至一号理想位置分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至一号理想位置需要转动的角度(如图2所示)，分别满足公式(1)-(3)；

式(2)中：

式(3)和式(4)中的表示第m个关节在一号理想位置的坐标；分别满足公式(6)和(7)：

式(1)-(7)中，为第m根臂杆从碰撞位置运动至一号理想位置需要转动的角度；θi表示前m个关节中的第i个关节在碰撞位置的关节角度，i＝1,2…m；θj表示前i个关节中任意一个关节在碰撞位置的关节角度，j＝1,2…i；li为前m根臂杆中任意一根臂杆的长度，i＝1,2…m；

在第m根臂杆绕其臂杆二分点旋转过程中，上位机继续监测前m个关节处的电机理论工作电流与实际工作电流的差值是否超过阈值；

当第m根臂杆绕其臂杆二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，则表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，取第m根臂杆二分点(即第m根臂杆与第m+1个关节连接的端点)与第m根臂杆二分点的中点(即第m根臂杆二分点)作为转动中心，继续执行步骤2-2；若未超过阈值，则表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，取第m根臂杆二分点与第m根臂杆二分点(即第m根臂杆与第m个关节连接的端点)的中点(即第m根臂杆二分点)作为转动中心，继续执行步骤2-3；

当第m根臂杆绕其臂杆二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，取第m根臂杆二分点与第m根臂杆二分点(即第m根臂杆与第m个关节连接的端点)的中点(即第m根臂杆二分点)作为转动中心，继续执行步骤2-3；若未超过阈值，则表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，取第m根臂杆二分点(即第m根臂杆与第m+1个关节连接的端点)与第m根臂杆二分点的中点(即第m根臂杆二分点)作为转动中心，继续执行步骤2-2；

2-2、首先上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节继续转动，使第m根臂杆绕其臂杆二分点作第二次旋转运动至二号理想位置分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至二号理想位置需要转动的角度，参见图3；其中，

式(9)中：

式(10)和式(11)中的表示第m个关节在二号理想位置的坐标；分别满足公式(13)和(14)：

式(8)-(14)中，为第m根臂杆从碰撞位置运动至二号理想位置需要转动的角度；

当第m根臂杆绕其臂杆二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，则取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；若未超过阈值，则表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转，参见图4；

当第m根臂杆绕其臂杆二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，则取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；

2-3、首先上位机控制平面冗余机械臂的第m-2个关节、第m-1个关节和第m个关节从一号理想位置恢复至碰撞位置时的关节角度，然后控制平面冗余机械臂的第m-2个关节、第m-1个关节、第m个关节转动，使第m根臂杆绕其臂杆二分点作第二次旋转运动至三号理想位置分别为第m-2个关节、第m-1个关节和第m个关节从碰撞位置运动至三号理想位置需要转动的角度，参见图5；其中，

式(16)中：

式(17)和式(18)中的表示第m个关节在三号理想位置的坐标；分别满足公式(20)和(21)；

式(15)-(21)中，为第m根臂杆从碰撞位置运动至三号理想位置需要转动的角度；

当第m根臂杆绕其臂杆二分点逆时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，则取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转，参见图6；若未超过阈值，则表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，此时取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；

当第m根臂杆绕其臂杆二分点顺时针旋转时，若上位机监测前m个关节处的电机理论工作电流与实际工作电流的差值超过阈值，表示碰撞位置位于第m根臂杆二分点远离第m+1个关节的一侧，此时取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；若未超过阈值，表示碰撞位置位于第m根臂杆二分点靠近第m+1个关节的一侧，则取第m根臂杆二分点与第m根臂杆二分点的中点作为转动中心继续旋转；

重复执行上述操作，第m根臂杆始终以上一次旋转运动检测出的碰撞位置所在区间的两端处(两个极限点)的两个二分点的中点为下一次旋转运动的转动中心继续作旋转运动，进一步检测碰撞位置所在区间，每次旋转运动都将碰撞位置所在区间缩小，直到第m根臂杆完成第n次旋转运动，得到碰撞位置，碰撞位置为第n次旋转运动确定的区间的中点；

本发明还提供一种基于二分法的平面冗余机械臂的避障方法，其特征在于，该方法的具体过程为：

根据上述位置检测方法得到的碰撞位置，以第m根臂杆的长度lm的1/4为直径作一个与第m根臂杆相切的包络圆，使得障碍物完全位于包络圆内，包络圆与第m根臂杆的切点即为碰撞位置，进而确定包络圆的位置和大小，参见图7；然后上位机根据包络圆的位置和大小规划平面冗余机械臂的避障路径，确保平面冗余机械臂的运动空间不会与包络圆的空间重叠，保证平面冗余机械臂在避障过程中不会进入包络圆的区域。

优选地，当上位机无法规划出平面冗余机械臂避障路径时，通过以下两种方式改善：

第一种：增加第m根臂杆期望绕二分点旋转的次数n，更加精准地检测碰撞位置；但是这个方式存在局限性，当绕二分点旋转的次数超过8次时，对于碰撞位置的检测已经不会有太多改变，表示此时碰撞位置的检测已经十分精确；

第二种：将包络圆的直径增大至d1，d1＝α·d0，α为大于1的系数，d0为包络圆的初始直径，即用一个更大的圆来包络障碍物，实现避障；当包络圆的直径大于第m根臂杆的长度lm时，表示此时的障碍物太大，平面冗余机械臂无法对这个障碍物进行避障；

若采用上述两种方式平面冗余机械臂都无法避障时，则采取人工避障。

本发明未述及之处适用于现有技术。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除