CN1623741A - 一种多机械臂机器人关节间的碰撞检测方法 - Google Patents

Abstract

一种多机械臂机器人关节间的碰撞检测方法，采用一种计算机程序控制方法检测多机械臂机器人关节的碰撞，通过对多臂机器人分析，发现只要在多臂机器人的各关节两端安装上传感器，再在多臂机器人工作过程中，利用这些传感器，可容易地得到机械臂上各关节的端点空间坐标，将多臂机器人机械臂上各关节所处的空间位置以空间坐标的数据信号通过传感器用电信号传递到计算机系统，再根据任意空间两线段之间的最短距离用程序控制的方法来判别两个有长度限制的圆柱状状物体的机械干涉关系，采用离散检测算法就可以进行多臂机器人关节间的碰撞检测。

Description

一种多机械臂机器人关节间的碰撞检测方法

技术领域

本发明属于一种以臂的运动为特点的程序控制机械手的程序控制方法，特别是指一种多机械臂机器人系统机械臂各关节防止机械干涉的碰撞程序控制检测方法。

技术背景

对于多机械臂机器人系统工作时，各机械臂关节均处在动态的、不确定的非结构性环境中，碰撞现象可能经常发生，如何判断和避免机器人机械臂关节的碰撞是实现多臂机器人自动作业的前提。在碰撞检测方法应用于机器人研究方面，目前主要有两种系统，两种水平，即单机系统和多机系统，整机水平和关节水平。多机系统的机械干涉研究多见于整机水平，而且单机系统关节水平上已成熟的计算方法难以直接应用于多机系统。而有些多机械臂系统，由于构成机械臂的各关节结构相对庞大，在工作空间内，尚不能将其简化处理作为一个单独的整体，如何处理这种情况，还是一个相当大的难题。

发明内容

本发明的目的在于针对现有的多机械臂机器人系统各机械臂关节碰撞检测方法的不足，提出一种可简便检测出多机械臂系统机器人各机械臂关节碰撞的检测方法。

为了便于说明问题，本发明先引入有关一些概念。

根据多机械臂系统的各机械臂之间的相互关系，建立各机械臂各关节的简化模型，将多臂机器人关节间碰撞检测转变为主要是考虑各机械臂关节级之间的碰撞检测。首先必须分析机械臂各关节的结构，并进行适度的简化，建立各关节的简化模型。一般而言，机械臂的各关节可视为由圆柱状的杆件所组成，因此可将机械臂的各关节简化为两端部分具有半径R的半球，其中间部分简化为半径R的圆柱体的杆件，简化后的模型为图1所示。从机器人的简化建模中可看出，机器人关节间的干涉判别问题，可分为运动杆件之间，运动杆件与静止杆件之间碰撞检测问题。从简化模型的几何特性来看，其碰撞检测方法就是如何进行关节间的碰撞检测。根据简化模型，多臂机器人关节间的碰撞检测可简化为多个两圆柱状物体间的最短距离干涉判别问题。当空间两圆柱状物体简化为两线段时，两圆柱状物体间的碰撞检测就转化为一个两空间线段相互关系的空间干涉问题。

从简化建模中可看出，多臂机器人关节间的碰撞检测可简化为多个两圆柱状物体间的最短距离求解。当空间两圆柱状物体简化为两线段时，两圆柱状物体间的碰撞检测就转化为一个求两空间线段相互关系的空间解析几何问题。由于空间线段的长度有限制，如何判断两空间线段之间的相互关系或者找出两空间线段之间的最短距离时，只需对各种可能出现的情况进行总结、简化成规律性的东西就能找出防止干涉的办法。

定义：令AB为简化模型任意一关节的两端点；CD为另一关节的两端点；设两个运动关节的空中间隙为Δ，当Δ≠0，线段AB，CD异面，否则共面。对于任给两空间线段表示机器人两个运动关节简化模型的中线，线段的四个端点的坐标值已知。从解析几何的一些基本理论可知，

设l₁为A点至线段CD的距离，l₂为B点至线段CD的距离，l₁′为C点至线段AB的距离，l₂′为D点至线段AB距离；l_AC为A点至C点距离，l_AD为A点至D点距离，l_BC为B点至C点距离，l_BD为B点至D点距离，l_AB为线段AB的长度，l_CD为线段CD的长度。

根据两线段的端点坐标易得到l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，它们均大于或等于零，我们将其记为：

min(l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD)≥0         (1)

若min(l₁，l₂)＞0，可在线段AB上找到一点E，使l_AE/L_BE＝l₁/l₂成立，令点E至线段CD的距离为l₃；  同理若min(l₁′，L₂′)＞0，亦可在线段CD上找到一点E′，让l_CE′/l_DE′＝l₁′/l₂′成立，令点E′至线段AB的距离为l₃′；若min(l₁，l₂)＝0，则令l₃＝0；同理，若min(l₁′，l₂′)＝0，则令l₃′＝0。同样，l₃和l₃′也易得到。

通过分析两杆件在空间位置只有两种状况，一种是共面，一种是异面。因此判断线段AB，CD的空间位置情况应分共面和异面两种情况来考虑。

1、共面状况

通过分析可以得知共面两线段相互位置的关系可总结为以下四种情况：

1)、相交且交点在两线段上；

2)、相交但交点在线段的延长线上；

3)、共线；

4)、平行。

结合解析几何中的相关理论，我们可得到下述推论：

推论1

若min(l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′)＝0，则线段AB，CD共面。

推论2

若min(l₃，l₃′)＝max(l₃，l₃′)＝0，max(l₁，l₂，l₁′，l₂′)＞0，则线段AB，CD相互交叉(即相交)，且交点一定在线段AB，CD上。

推论3

若min(l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′)≠0，Δ＝0，线段AB，CD共面且无交点，且有线段AB不与过线段CD的直线相交，线段CD也不与过线段AB的直线相交。

若在共面两线段AB，CD中，令

$l_1^2+l_{\mathrm{CD}}^2<{\left(\max (l_{\mathrm{AC}},l_{\mathrm{AD}})\right)}^2---\left(2\right)$

若上式成立，我们可知点A在线段CD上无相应投影点，其投影点在线段CD的延长线上，点A至线段CD的最短距离l＝min(l_AC，l_AD)；若上式不成立，点A在线段CD有相应投影点，点A至线段CD的最短距离l＝l₁。

$l_2^2+l_{\mathrm{CD}}^2<{\left(\max (l_{\mathrm{BC}},l_{\mathrm{BD}})\right)}^2---\left(3\right)$   $l_1^{\&prime;2}+l_{\mathrm{AB}}^2<{\left(\max (l_{\mathrm{AC}},l_{\mathrm{BC}})\right)}^2---\left(4\right)$

$l_2^{\&prime;2}+l_{\mathrm{AB}}^2<{\left(\max (l_{\mathrm{AD}},l_{\mathrm{BD}})\right)}^2--\left(5\right)$

推论4

若min(l₁，l₂)＝max(l₁，l₂)＝0，即l₁＝0且l₂＝0，则线段AB，CD共线；同时必有min(l₁′，l₂′)＝max(l₁′，l₂′)＝0，反之亦然。同时有下述结论成立：

1)、若min(l_AC，l_AD，l_BC，l_BD)＝0，线段AB，CD有端点重合，则它们将的最短距离l＝0；

2)、min(l_AC，l_AD，l_BC，l_BD)≠0，max(l_AC，l_AD，l_BC，l_BD)-min(l_AC，l_AD，l_BC，l_BD)＞(L_AB+l_CD)成立则线段AB，CD间的最短距离l＝min(l_AC，l_AD，l_BC，l_BD)；

3)、min(l_AC，l_AD，l_BC，l_BD)≠0，max(l_AC，l_AD，l_BC，l_BD)-min(l_AC，l_AD，l_BC，l_BD)≤(l_AB+l_CD)成立，则线段AB，CD间的最短距离l＝0。

推论5

若max(l₁，l₂，l₁′，l₂′)≠min(l₁，l₂，l₁′，l₂′)＝0，即有l₁，l₂不全为0，l₁′，l₂′不全为0，则存在：

1)、若max(l₁′，l₂′)≠min(l₁′，l₂′)＝0且max(l₁，l₂)≠min(l₁，l₂)＝0，线段AB，CD必相交于某个端点，它们间的最短距离l＝0；

2)、若max(min(l₁′，l₂′)，min(l₁，l₂))≠min(min(l₁′，l₂′)，min(l₁，l₂))＝0，即min(l₁，l₂)与min(l₁′，l₂′)只有一个为0。如果min(l₁，l₂)＝0，则线段AB，CD间的最短距离l＝min(l₁′，l₂′)；如果min(l₁′，l₂′)＝0，则线段AB，CD间的最短距离l＝min(l₁，l₂)。

推论6

若min(l₁，l₂，l₁′，l₂′)≠0，而min(l₃，l₃′)＝0，则过线段AB，CD的两直线一定相交；同时存在：

1)、若max(l₃，l₃′)＝min(l₃，l₃′)＝0，即l₃＝0并且l₃′＝0，从推论2可知线段AB，CD相交于E点，两线段间的最短距离l＝0；

2)、若max(l₃，l₃′)≠min(l₃，l₃′)＝0，即l₃＝0，l₃′＝0不同时成立，如果l₃＝0，线段AB，CD间的最短距离l＝min(l₁′，l₂′)；如果l₃′＝0，则线段AB，CD间的最短距离l＝min(l₁，l₂)。

推论7

如果min(l₃，l₃′)≠0(即有min(l₁，l₂，l₁′，l₂′，l₃，l₃′)≠0)，并且Δ＝0，由推论3可知线段AB，CD只有在线段AB的延长线和线段CD的延长线上存在交点，或者线段AB，CD平行；亦可得如下结论：

1)、若式(2)，(3)(4)，(5)四式都成立，则线段AB，CD上相互无投影线段，它们间的最短距离l＝min(l_AC，l_AB，l_BC，l_BD)；

2)、若式(2)，(3)(4)，(5)四式都不成立，则线段AB，CD必定平行且成长方形分布，它们间的最短距离l＝l₁；

3)、若式(2)，(3)(4)，(5)任意三式成立，任设式(2)不成立，则线段AD，CD间的最短距离即为l＝l₁，其他情形依此类推；

4)、若式(2)，(3)(4)，(5)只有任意两式成立，任设式(2)，式(4)成立，则线段AB，CD间的最短距离即为l＝min(l₁，L₁′)，其他情形依此类推。

5)、若式(2)，(3)，(4)，(5)只有任意一式成立，任设式(2)成立，则线段AB，CD间的最短距离即为l＝min(l₁′，l₂，l₂′)。

2、异面状况

Δ≠0，可知空间两线段AB，CD异面，必定存在min(l₃，l₃′)≠0。

任意过线段AB，CD的两空间两异面直线可构造为正三棱锥，且只有三种情况。这样，异面线段AB，CD亦可用正三棱锥上的线段AB和与线段C₁D₁方向矢量相同的线段CD来表示，即线段CD在过线段C₁D₁的直线上，具体位置不定。则 $l_1={l_{\mathrm{AC}}}_1,$ $l_2={l_{\mathrm{BD}}}_1,$ $l_0=l_{C_1{A}_2}={l_{\mathrm{OO}}}_1.$

由解析几何的基本定理可知，根据线段两端点坐标值，易知线段AB，CD间的夹角μ。

根据前面描述可知： ${l_{\mathrm{AA}}}_1=l_{\mathrm{AB}}\&times;\sin \mathrm{\&mu;};---\left(6\right)$

将线段AB，CD互换，设： ${l_{\mathrm{CC}}}_1=l_{\mathrm{CD}}\&times;\sin \mathrm{\&mu;};---\left(7\right)$

任设l₁＝max(l₁，l₂)，令： $k=l_1^2-l_0^2-l_{{\mathrm{AA}}_1}^2;---\left(8\right)$

可知，若k＜0，线段AB，CD的公垂线的垂足落在线段AB间；若k＝0，线段AB，CD的公垂线的垂足落在线段AB的某一个端点上；若k＞0，线段AB，CD的公垂线的垂足落在线段AB外。

同理可得： $k^{\&prime;}={l_1^{\&prime;}}^2-l_0^2-l_{{\mathrm{CC}}_1}^2;---\left(9\right)$

对于k′，一样有上述结论。

因此，我们可得出推论8：

推论8

1、若k≤0，k′≤0，异面两线段AB，CD上都有公垂线的垂足，它们间的最短距离l＝l₀；

2、若k≤0，k′＞0，线段AB上有公垂线的垂足，而线段CD上没有公垂线的垂足。同时，若式(4)，(5)成立，则它们间的最短距离l＝min(l_AC，l_AD，l_BC，l_BD)；若式(4)，(5)中只有一式成立，任设式(5)成立，则l＝l₂′；同样k′≤0，k＞0时，亦有相似结论；

3、若k＞0，k′＞0，线段AB上没有公垂线的垂足，并且线段CD上没有公垂线的垂足，此时，线段AB，CD见的最短距离与推论7中的1，3，4，5相同，即有：

若式(2)，(3)，(4)，(5)成立，则它们间的最短距离l＝min(l_AC，l_AD，l_BC，l_BD)；

若任意三式成立，任设式(2)不成立，则l＝l₁，其他依此类推；

若任意两式成立，任设式(2)，(3)不成立，则l＝min(l₁，l₂)，其他依此类推；

若式(2)，(3)，(4)，(5)中任意一式成立，任设式(2)成立，则l＝min(l₁′，l₂，l₂′)，其他依此类推；

至此，我们应用推论1、...、推论8可求出任意空间有向两线段之间的最短距离。据此，我们在研究多臂机器人关节间碰撞检测时，可设计一个程序运用计算机求出任意空间两线段之间的最短距离来判别两个有长度限制的圆柱状状物体的机械干涉关系，进而检测出多臂机器人关节间是否碰撞，简称为最短距离求解碰撞检测方法。该程序检测方法包括以下几个子程序：

子程序1：求解l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′，Δ；

令k_k＝min(l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′)；

子程序2：若k_k＝0，则按推论4，5和推论6求解空间两线段间的最短距离l；

子程序3：若k_k≠0，Δ＝0则依推论7求解空间两线段间的最短距离l；

子程序4：若Δ≠0则依推论7求解空间两线段间的最短距离l。

根据以上程序算法所的到的检测程序如程序框图如图2示。

在T＝T₁-t时间内，采用离散检测算法时，当T＝t+Δt_i时，求出空间线段端点在笛卡尔空间直角坐标系下的坐标值以后，最短距离求解碰撞检测算法主程序的步骤如下：

1、在T＝t+Δt_i时，运行子程序1；

2、判断式k_k＝0成立与否，若成立，运行子程序2，若不成立则转入下一步；

3、继续判断式Δ＝0成立与否，若成立，则运行子程序3，否则运行子程序4；

4、求出空间线段的距离后，判断空间杆状物体是否碰撞，若碰撞则采取规避措施；否则继续；

5、判断T＝t₁是否成立，若不成立，则T_i+1＝T_i+Δt_i+1，继续执行上述程序，若成立，程序结束。

通过该算法可不必求具体的交点位置而迅速求出两空间线段之间的最短距离从而来判定两杆状物体间是否发生碰撞。

采用上述程序可检测每一个机器人关节的状态。并可用MATLAB语言编程对程序检测算法进行仿真分析：

设n组两圆柱状物体其半径分别为R_i，r_i(i＝1，...，n)，在T＝t₀+Δt_j时刻，两圆柱状物体间的中心线端点坐标由一个随机函数产生，用上述算法对这n组两圆柱状物体进行碰撞检测，判别一对杆件是否相互碰撞两算法耗时只需0.003秒左右。

根据本发明的目的所提出的技术实施方案是：采用一种计算机程序控制方法检测多机械臂机器人关节的碰撞，通过对多臂机器人分析，发现只要在多臂机器人的各关节两端安装上传感器，在在多臂机器人工作过程中，利用这些传感器，可容易地得到机械臂上各关节的端点空间坐标，将多臂机器人机械臂上各关节所处的空间位置以空间坐标的数据信号通过传感器用电信号传递到计算机系统，再根据任意空间两线段之间的最短距离用程序控制的方法来判别两个有长度限制的圆柱状状物体的机械干涉关系，采用离散检测算法就可以进行多臂机器人关节间的碰撞检测，简称为最短距离求解碰撞检测程序检测方法。该计算机系统至少包括：

1)信号采集和写入模块，用于向计算机系统提供待检测数据；

2)用于储存信息的数据存储器；

3)用于判断机械臂上各关节是否发生碰撞的判定模块；

4)根据判定模块所分析的数据进行控制机器人机械臂上各关节，以防发生碰撞的控制模块及执行机构。

本检测方法的特点是：

1)该检测系统是利用机器人机械臂上各关节两端点的传感器所发出的空间位置信号来进行判别分析的；

2)检测系统的判定模块是将机器人机械臂各关节简化为线段，根据机器人机械臂上各关节空间最短距离，利用离散检测算法进行多臂机器人关节间碰撞检测判定的；

3)在判定后，判定模块随即将分析数据送至控制模块，并由控制模块下达执行命令，对机器人机械臂上各关节空间位置进行修定，以防机器人机械臂上各关节发生碰撞。

本检测方法的其它特点是：传感器所采集的信号是一种笛卡尔空间直角坐标系下的坐标值。

采用该程序检测多臂机械人的每个臂关节的状态的优点是：

1.该程序检测算法是利用机器人关节的两端点坐标，通过一些简单的几何推导和分析，来判别两个具有长度限制的关节间的相互碰撞状态。

2.该程序检测算法特别适合于计算机实现，易于实现程序设计的模块化，算法复杂性较低。对多机械手系统的关节水平上的碰撞检测具有极大的应用价值。

3.该程序检测算法也是一种基于离散时间的碰撞检测算法，其判别效果的准确性都仪赖于Δt_i的大小和机器人中关节的运动速度，如果能预知各关节的运动速度，则选取合适的Δt_i，可消除离散算法中可能出现的误判。

附图及说明

图1为本发明原理结构示意图；

图2为本发明检测程序框图。

具体实施方式

实施例一

采用一种计算机程序控制方法检测多机械臂机器人关节的碰撞，通过对多臂机器人分析，发现只要在多臂机器人的各关节两端安装上传感器，在多臂机器人工作过程中，利用这些传感器，可容易地得到机械臂上各关节的端点空间坐标，将多臂机器人机械臂上各关节所处的空间位置以空间坐标的数据信号通过传感器用电信号传递到计算机系统，再根据任意空间两线段之间的最短距离用程序控制的方法来判别两个有长度限制的圆柱状状物体的机械干涉关系，采用离散检测算法就可以进行多臂机器人关节间的碰撞检测，简称为最短距离求解碰撞检测程序检测方法。该计算机检测系统至少包括：

5)信号采集和写入模块，用于向计算机系统提供待检测数据；

6)用于储存信息的数据存储器；

7)用于判断机械臂上各关节是否发生碰撞的判定模块；

8)根据判定模块所分析的数据进行控制机器人机械臂上各关节，以防发生碰撞的控制模块及执行机构。

本检测方法的特点是：

4)该计算机检测系统是利用机器人机械臂上各关节两端点的传感器所发出的空间位置信号来进行判别分析的；

5)计算机检测系统的判定模块是将机器人机械臂各关节简化为线段，根据机器人机械臂上各关节空间最短距离，利用离散检测算法进行多臂机器人关节间碰撞检测判定的；

6)在判定模块对传感器所输入的数据进行判定后，判定模块随即将分析数据送至控制模块，并由控制模块下达执行命令，对机器人机械臂上各关节空间位置进行修定，以防机器人机械臂上各关节发生碰撞。

本检测方法的其它特点是：传感器所采集的信号是一种笛卡尔空间直角坐标系下的坐标值。

其控制程序包括以下几个子程序：

子程序1：求解l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′，Δ；

令k_k＝min(l₁，l₂，l₁′，l₂′，l_AC，l_AD，l_BC，l_BD，l_AB，l_CD，l₃，l₃′)；

子程序2：若k_k＝0，则按推论4，5和推论6求解空间两线段间的最短距离l；

子程序3：若k_k≠0，Δ＝0则依推论7求解空间两线段间的最短距离l；

子程序4：若Δ≠0则依推论7求解空间两线段间的最短距离l。

根据以上程序算法所的到的检测程序如程序框图如图2示。

采用上述程序可检测每一个机器人关节的状态。并可用MATLAB语言编程对程序检测算法进行仿真分析：

设n组两圆柱状物体其半径分别为R_i，r_i(i＝1，...，n)，在T＝t₀+Δt_j时刻，两圆柱状物体间的中心线端点坐标由一个随机函数产生，用上述算法对这n组两圆柱状物体进行碰撞检测，判别一对杆件是否相互碰撞两算法耗时只需0.003秒左右。

Claims (5)

1、一种多机械臂机器人关节间的碰撞检测方法，采用一种计算机程序控制方法检测多机械臂机器人关节的碰撞，其特征在于在多臂机器人的各关节两端安装有传感器，在在多臂机器人工作过程中，利用这些传感器，可容易地得到机械臂上各关节的端点空间坐标，将多臂机器人机械臂上各关节所处的空间位置以空间坐标的数据信号通过传感器用电信号传递到计算机系统，再根据任意空间两线段之间的最短距离用程序控制的方法来判别两个有长度限制的圆柱状状物体的机械干涉关系，采用离散检测算法就可以进行多臂机器人关节间的碰撞检测，该计算机检测系统至少包括：

A、采集和写入模块，用于向计算机系统提供待检测数据；

用于储存信息的数据存储器；

B、于判断机械臂上各关节是否发生碰撞的判定模块；

C、据判定模块所分析的数据进行控制机器人机械臂上各关节，以

防发生碰撞的控制模块及执行机构。

2、如权利要求1所述的一种多机械臂机器人关节间的碰撞检测方法，其特征在于：该计算机系统检测系统是利用机器人机械臂上各关节两端点的传感器所发出的空间位置信号来进行判别分析的；

3、如权利要求1所述的一种多机械臂机器人关节间的碰撞检测方法，其特征在于：计算机检测系统的判定模块是将机器人机械臂各关节简化为线段，根据机器人机械臂上各关节空间最短距离，利用离散检测算法进行多臂机器人关节间碰撞检测判定的；

4、如权利要求1所述的一种多机械臂机器人关节间的碰撞检测方法，其特征在于：在判定模块对传感器所输入的数据进行判定后，判定模块随即将分析数据送至控制模块，并由控制模块下达执行命令，对机器人机械臂上各关节空间位置进行修定，以防机器人机械臂上各关节发生碰撞。

5、如权利要求1所述的一种多机械臂机器人关节间的碰撞检测方法，其特征在于：传感器所采集的信号是一种笛卡尔空间直角坐标系下的坐标值。

Images