CN104626171A - 基于六维力传感器的机械臂碰撞检测与响应方法 - Google Patents

Abstract

本发明公开了一种基于六维力传感器的机械臂碰撞检测与响应方法，该方法在机械臂末端法兰与负载之间设置六维力传感器，按照设定的采样频率不断采集传感器的测量数据；机械臂的控制系统通过负载的重力补偿得到外部作用对机械臂负载的力与力矩分别在机械臂带动负载做平移运动或旋转运动时，根据预先内设的力常数阈值F_s和力矩常数阈值M_s进行碰撞检测，根据机械臂预置线速度/角速度与外部力/力矩的相对方向控制相应的运动。本发明通过使用六维力传感器检测碰撞，检测精度高，响应速度快；通过消除负载重力及负载姿态的影响，对于重量较大的负载仍能灵敏的检测碰撞；一旦碰撞发生后可立即停止使碰撞进一步加剧的运动，而允许相反方向的运动，方法合理实用。

Description

基于六维力传感器的机械臂碰撞检测与响应方法

技术领域

本发明属于机械臂控制技术领域，具体涉及一种利用安装在机械臂末端的六维力传感器进行碰撞检测与响应的方法。

背景技术

机械臂是具有模仿人类手臂功能并可完成各种作业的自动控制设备，有多关节连结，能够接受指令并精确地定位到三维空间或二维平面中的点进行作业。机械臂承载能力大，刚性好，按照接收到的指令进行运动，通常不具备灵敏检测与响应和外界碰撞的能力。现有的机械臂产品中，通常采用关节电机电流检测机械臂与外界的碰撞，当电流值高于设定的阈值时，机械臂停止运动。

在采用机械臂进行精密零部件安装的应用中，希望机械臂能够精确无碰撞地将待装物体搬运至指定位姿，一旦发生碰撞，若不能够有效地检测与响应，将对被装配的产品带来损伤。机械臂产品自有的电机电流检测功能能够保护机械臂本体的电机、结构等不受损伤，但其灵敏程度不足以保证被操作产品的安全，当电流保护动作时，机械臂末端产生的力已足以对产品造成破坏。

六维力传感器是一种能够测量空间任意力系中三维正交力和三维正交力矩的传感器，具有较高的采样频率与测量分辨率。在现有六维力传感器产品中，采样频率可达7000Hz，力测量分辨率可达到量程的十万分之五以内，力矩测量分辨率可达到量程的十万分之四以内，其测量信号的响应速度与分辨率可以满足机械臂装配应用中碰撞检测的需求。

本发明提出一种基于六维力传感器的机械臂碰撞检测与响应方法，通过安装在机械臂末端法兰与负载之间的六维力传感器检测负载受到的外部碰撞力，并通过设计控制算法使机械臂做出响应，检测到碰撞发生时，控制机械臂不产生使碰撞进一步加剧的运动，而允许相反方向的运动。

发明内容

本发明的目的在于提供一种基于六维力传感器的机械臂碰撞检测与响应方法，解决应用机械臂进行产品装配过程中的碰撞检测与响应问题，碰撞发生时能够及时控制机械臂做出响应，保证产品的安全。

本发明是通过如下技术方案实现的：

基于六维力传感器的机械臂碰撞检测与响应方法，包括如下步骤：

1)在机械臂末端法兰与负载之间设置六维力传感器，六维力传感器与机械臂控制系统进行电连接，控制系统按照设定的采样频率不断采集六维力传感器的测量数据；

2)六维力传感器实时感测作用在其上的力与力矩值并传送给控制系统，控制系统通过负载的重力补偿得到外部作用对机械臂负载的力与力矩

3)在机械臂带动负载做平移运动时，控制系统得到将要输出给机械臂执行的负载平移速度为并根据预先内设的力常数阈值Fs进行碰撞检测与响应，如果a)时，机械臂执行平移速度如果b)且与的夹角大于90°时，机械臂停止运动；如果c)且与的夹角小于等于90°时，机械臂执行速度

4)在机械臂带动负载做旋转运动时，控制系统得到将要输出给机械臂执行的负载转动角速度为并根据预先内设的力矩常数阈值Ms进行碰撞检测与响应：如果a)时，机械臂执行角速度b)且与的夹角大于90°时机械臂停止运动；c)且与的夹角小于等于90°时机械臂执行速度

上述技术方案中，所述机械臂是串联式的机器人，通常具有6个自由度。

上述技术方案中，所述负载是指机械臂本体及六维力传感器以外的安装在机械臂末端的载荷。

上述技术方案中，所述外部作用为除负载重力以外对六维力传感器产生作用的外部作用力，如碰撞力。

上述技术方案中，六维力传感器安装在机械臂末端法兰与负载之间，在操作中，六维力传感器感受到的力与力矩信息是负载重力和外力的综合作用结果。

上述技术方案中，机械臂运动的速度和角速度由一定的控制输入产生，在控制系统将速度或角速度输出给机械臂执行运动前，在已经检测到碰撞的情况下，通过判断速度(角速度)与外力(外力矩)的相对方向，可在运动执行前明确运动是否会使碰撞加剧，会加剧则不执行运动，不会则执行运动，这样在检测到碰撞发生后，并没有禁止所有的运动，与碰撞相反方向的运动仍是被允许的，这极大地方便了实际操作。

上述技术方案步骤3)中，在平移控制中，时即认为碰撞发生，与的夹角大于90°时，若负载按速度运动会使增大，使碰撞加剧，所以此时停止机械臂运动；与的夹角小于等于90°时，负载按速度运动不会使增大，不会使碰撞加剧，所以此时可控制机械臂执行速度

上述技术方案步骤4)中，在旋转控制中，时即认为碰撞发生，与的夹角大于90°时，若负载按角速度运动会使增大，使碰撞加剧，所以此时停止机械臂运动；与的夹角小于等于90°时，负载按角速度运动不会使增大，不会使碰撞加剧，所以此时可控制机械臂执行角速度

与现有技术相比，本发明的机械臂控制方法通过使用六维力传感器检测碰撞，检测精度高，响应速度快；通过消除负载重力及负载姿态的影响，对于重量较大的负载仍能灵敏的检测碰撞；一旦碰撞发生后可立即停止使碰撞进一步加剧的运动，而允许相反方向的运动，方法合理实用。

附图说明

图1为本发明的六维力传感器安装在机械臂末端法兰与负载之间的机械臂装配应用示意图。

图2为本发明的检测方法中碰撞力与机械臂移动速度相对角度的示意图。

1－机械臂、2－六维力传感器、3－夹具、4－工件、5－装配腔体。

具体实施方式

下面结合附图对本发明的机械臂碰撞检测与响应方法进行进一步说明，该说明仅仅是示例性的，并不旨在限制本发明的保护范围。

如图1所示，六维力传感器2安装在机械臂1的末端与夹具3之间，工件4被夹具3夹持，机械臂1在控制系统(图中未画出)的控制下将工件4运送至装配腔体5中。上述技术方案中，所述六维力传感器是一种业内人员熟知的成熟的传感器产品，容易购买获得(如ATI公司的六维力传感器)，其每次测量得到的数据有六个分量，包括在六维力传感器自身坐标系中的三个坐标轴方向的力分量，以及三个坐标轴方向的力矩分量。

夹具3与工件4的组合体是六维力传感器2的负载，根据“航天器机械臂柔性随动控制重力补偿方法”(专利公开号：CN103600354A)，预先对夹具3与工件4组合体的重力、重心位置等重力信息进行测定，而后进行重力补偿计算，消除负载重力对六维力传感器感知的影响。

控制系统(图中未画出)实时获取六维力传感器2感知到的力与力矩信息，从六维力传感器2得到的力与力矩信息是负载所受外部作用(如碰撞)与负载重力的综合作用结果。由“航天器机械臂柔性随动控制重力补偿方法”(专利公开号：CN103600354A)，根据预先测定得到的夹具3与工件4组合体的重力信息，将从六维力传感器2得到的力与力矩信息减去夹具3与工件4组合体重力的作用分量，得到外部作用对机械臂负载的力与力矩预先为外部作用力大小设定的常数阈值为F_s，为外部作用力矩大小设定的常数阈值为M_s。

在机械臂1带动负载做平移运动时，控制系统得到将要输出给机械臂1执行的负载平移速度为碰撞检测与响应按如下原则进行：

a)时，机械臂执行速度

b)且与的夹角大于90°时机械臂停止运动；

c)且与的夹角小于等于90°时机械臂执行速度

其中与的夹角是否大于90°可通过向量计算进行判断，用φ表示与的夹角，则可以得到：

$\cos \mathrm{\φ}=\frac{\stackrel{\→}{F}\·\stackrel{\→}{v}}{\left|\stackrel{\→}{F}\right|\left|\stackrel{\→}{v}\right|}---\left(1\right)$

与空间相对方向如图2所示，当与的夹角φ满足90°<|φ|≤180°时，在反方向有分量，机械臂按照速度运动会使继续增大，使碰撞加剧，因此在90°<|φ|≤180°时，不允许机械臂执行速度当与的夹角φ满足|φ|≤90°时，机械臂执行速度则不会使继续增大，这样的运动是被允许的。在控制中，按照式(1)判断与的空间相对方向，若cosφ≥0，则夹角φ满足|φ|≤90°，机械臂可以运动；若cosφ<0，则夹角φ满足90°<|φ|≤180°，不允许机械臂运动。

在机械臂带动负载做旋转运动时，控制系统得到将要输出给机械臂执行的负载转动角速度为碰撞检测与响应按如下原则进行：

a)时，机械臂执行角速度

b)且与的夹角大于90°时机械臂停止运动；

c)且与的夹角小于等于90°时机械臂执行速度

可采用与移动控制时相同的计算方式判断与的夹角是否大于90°。

尽管上文对本发明的具体实施方式给予了详细描述和说明，但是应该指明的是，我们可以依据本发明的构想对上述实施方式进行各种等效改变和修改，其所产生的功能作用仍未超出说明书及附图所涵盖的精神时，均应在本发明的保护范围之内。

Claims (8)

1.基于六维力传感器的机械臂碰撞检测与响应方法，包括如下步骤：

1)在机械臂末端法兰与负载之间设置六维力传感器，六维力传感器与机械臂控制系统进行电连接，控制系统按照设定的采样频率不断采集六维力传感器的测量数据；

2)六维力传感器实时感测作用在其上的力与力矩值并传送给控制系统，控制系统通过负载的重力补偿得到外部作用对机械臂负载的力与力矩

3)在机械臂带动负载做平移运动时，控制系统得到将要输出给机械臂执行的负载平移速度为并根据预先内设的力常数阈值F_s进行碰撞检测与响应，如果a)时，机械臂执行平移速度如果b)且与的夹角大于90°时，机械臂停止运动；如果c)且与的夹角小于等于90°时，机械臂执行速度

4)在机械臂带动负载做旋转运动时，控制系统得到将要输出给机械臂执行的负载转动角速度为并根据预先内设的力矩常数阈值M_s进行碰撞检测与响应：如果a)时，机械臂执行角速度b)且与的夹角大于90°时机械臂停止运动；c)且与的夹角小于等于90°时机械臂执行速度

2.权利要求1所述的机械臂碰撞检测与响应方法，其中，所述机械臂是串联式的机器人，通常具有6个自由度。

3.权利要求1所述的机械臂碰撞检测与响应方法，其中，所述负载是指机械臂本体及六维力传感器以外的安装在机械臂末端的载荷。

4.权利要求1所述的机械臂碰撞检测与响应方法，其中，所述外部作用为除负载重力以外对六维力传感器产生作用的碰撞力。

5.权利要求1-4任一项所述的机械臂碰撞检测与响应方法，其中，六维力传感器安装在机械臂末端法兰与负载之间，在操作中，六维力传感器感受到的力与力矩信息是负载重力和外力的综合作用结果。

6.权利要求1-4任一项所述的机械臂碰撞检测与响应方法，其中，机械臂运动的速度和角速度由一定的控制输入产生，在控制系统将速度或角速度输出给机械臂执行运动前，在已经检测到碰撞的情况下，通过判断速度(角速度)与外力(外力矩)的相对方向，可在运动执行前明确运动是否会使碰撞加剧，会加剧则不执行运动，不会则执行运动，这样在检测到碰撞发生后，并没有禁止所有的运动，与碰撞相反方向的运动仍是被允许的，这极大地方便了实际操作。

7.权利要求1-4任一项所述的机械臂碰撞检测与响应方法，步骤3)中，在平移控制中，时即认为碰撞发生，与的夹角大于90°时，若负载按速度运动会使增大，使碰撞加剧，所以此时停止机械臂运动；与的夹角小于等于90°时，负载按速度运动不会使增大，不会使碰撞加剧，所以此时可控制机械臂执行速度

8.权利要求1-4任一项所述的机械臂碰撞检测与响应方法，步骤4)中，在旋转控制中，时即认为碰撞发生，与的夹角大于90°时，若负载按角速度运动会使增大，使碰撞加剧，所以此时停止机械臂运动；与的夹角小于等于90°时，负载按角速度运动不会使增大，不会使碰撞加剧，所以此时可控制机械臂执行角速度