[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN108959753B - 碰撞检测方法、系统、可读存储介质及计算机设备 - Google Patents

碰撞检测方法、系统、可读存储介质及计算机设备 Download PDF

Info

Publication number
CN108959753B
CN108959753B CN201810670546.6A CN201810670546A CN108959753B CN 108959753 B CN108959753 B CN 108959753B CN 201810670546 A CN201810670546 A CN 201810670546A CN 108959753 B CN108959753 B CN 108959753B
Authority
CN
China
Prior art keywords
model
obstacle
claw
point
judging
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
CN201810670546.6A
Other languages
English (en)
Other versions
CN108959753A (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.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Guangzhou Shirui Electronics Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Guangzhou Shirui Electronics Co Ltd
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 Guangzhou Shiyuan Electronics Thecnology Co Ltd, Guangzhou Shirui Electronics Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201810670546.6A priority Critical patent/CN108959753B/zh
Publication of CN108959753A publication Critical patent/CN108959753A/zh
Application granted granted Critical
Publication of CN108959753B publication Critical patent/CN108959753B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Numerical Control (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明提供一种碰撞检测方法、系统、可读存储介质及计算机设备，所述方法包括：在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹；当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞。本发明当中的碰撞检测方法、系统、可读存储介质及计算机设备，通过合理简化对象模型和检测轨迹，并采用简单的图形接触判定方式来最终确定是否发生碰撞，整个检测过程简单，简单，计算量小，大幅度提高了碰撞检测的效率。

Description

碰撞检测方法、系统、可读存储介质及计算机设备
技术领域
本发明涉及机械臂技术领域，特别涉及一种碰撞检测方法、系统、可读存储介质及计算机设备。
背景技术
随着机械制造技术及自动化技术的不断发展，诞生出了各式各样的机械臂(如SCARA四轴机械臂)，机械臂通常由多个关节及设置在末端的卡爪组成，各关节相互配合来改变卡爪的位姿，从而完成特点的任务(如转运产品)。
在机械臂的出厂前或研发过程当中，通常需要对机械臂进行碰撞检测，即需要判断卡爪在某一位姿下是否与所处环境中的障碍物产生碰撞，以规划机械臂的运动路径。碰撞检测方法是保证机械臂找到一条从起始点到终点的无碰撞路径的关键技术，其优劣性直接决定了运动规划的效果。
现有技术当中，目前机械臂采用的碰撞检测方法分为两种，一种是直接调用现有的碰撞检测库，这种库集成了太多的内容，影响整体的碰撞检测算法效率，另一种方法是以一定的密度在机械臂的每一轴上采样，判断每一个采样点是否与其他点以及障碍物碰撞，这种方法耗时而且不准确。
发明内容
基于此，本发明的目的是提供一种碰撞检测方法、系统、可读存储介质及计算机设备，以提高机械臂碰撞检测的效率。
根据本发明实施例的一种碰撞检测方法，包括：
在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；
根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹；
当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞。
另外，根据本发明上述实施例的一种碰撞检测方法，还可以具有如下附加的技术特征：
进一步地，所述障碍物模型和所述卡爪模型均为圆形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别计算出所述障碍物模型和所述卡爪模型的半径，并计算两者半径之和，以得到参考总半径；
判断所述障碍物模型和所述卡爪模型的中心距是否小于所述参考总半径；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
计算出所述卡爪模型的参考半径，并分别计算出所述卡爪模型的圆心点到所述障碍物模型各条边线的点边距；
判断是否存在小于所述参考半径的所述点边距；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，计算所述圆心点到所述障碍物模型边线的点边距的步骤包括：
获取所述圆心点的坐标，并获取所述障碍物模型任一目标边线的长度及两个端点坐标；
以所述目标边线的任一目标端点为基点，建立所述目标端点到所述目标边线的另一个端点的第一向量，并建立所述目标端点到所述圆心点的第二向量；
计算所述第一向量和所述第二向量的向量点积，并判断所述向量点积是否小于0；
若是小于0，则计算所述目标端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若不是小于0，则判断所述向量点积是否大于等于所述目标边线的长度的平方；
若是，计算所述目标边线的另一个端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若否，则计算所述第一向量和所述第二向量叉积的模，并除以所述目标边线的长度，以得到所述圆心点到所述目标边线的点边距。
进一步地，所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型的圆心点，向所述障碍物模型的各条边线所在的直线上投影，以在每条所述直线上投影出一线段及一投影点；
分别在每条所述线段的两端向外扩延预设长度，以在每条所述直线上得到一延长线段，所述预设长度与所述卡爪模型的半径一致；
判断每条所述直线上的所述投影点和所述延长线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述卡爪模型及所述障碍物模型均为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型，向所述障碍物模型和所述卡爪模型的各条边线所在的直线上投影，以在每条所述直线上投影出两条一线段；
判断每条所述直线上的两条所述线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述在一预设参考平面内，分别建立障碍物模型及卡爪模型的步骤包括：
建立障碍物及卡爪的三维模型；
分别将所述障碍物及所述卡爪的三维模型投影到所述预设参考平面内，以得到所述障碍物模型及所述卡爪模型。
根据本发明实施例的一种碰撞检测系统，包括：
模型建立模块，用于在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；
轨迹计算模块，用于根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹；
碰撞判定模块，用于当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞。
本发明还提出一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的碰撞检测方法。
本发明还提出一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述的碰撞检测方法。
本发明当中的碰撞检测方法、系统、可读存储介质及计算机设备，由于卡爪和障碍物的高度对碰撞检测不受影响，故在进行碰撞模型建立时，直接忽略卡爪和障碍物的高度特征，并在同一预设参考平面内，分别建立与障碍物和卡爪的俯视图对应的平面几何模型，以反映两者的最大轮廓，由于无论卡爪在平面移动或上下移动时与障碍物，此时两者的俯视图必然会存在接触，因此本碰撞检测方法，根据机械臂各关节的参数，并直接忽略卡爪纵向移动的轨迹，计算出卡爪在平面内的运行轨迹，然后即可通过分析卡爪模型在运行轨迹的任一位置点上是否与障碍物模型发生接触，来判断卡爪与障碍物是否存在碰撞，若存在接触，则判定卡爪与障碍物存在碰撞。因此本发明当中的碰撞检测方法，通过合理简化对象模型和检测轨迹，并采用简单的图形接触判定方式来最终确定是否发生碰撞，整个检测过程简单，简单，计算量小，大幅度提高了碰撞检测的效率。
附图说明
图1为本发明第一实施例中的碰撞检测方法的流程图；
图2为本发明第二实施例中的碰撞检测方法的流程图；
图3为本发明第二实施例中计算点边距的图形解释图；
图4为本发明第二实施例中投影方法的图形解释图；
图5为本发明第二实施例中投影方法的另一图形解释图；
图6本发明第三实施例中的碰撞检测系统的结构示意图。
主要元件符号说明：
以下具体实施方式将结合上述附图进一步说明本发明。
具体实施方式
为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的若干实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。
需要说明的是，当元件被称为“固设于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。
以下各实施例均可运用在对SCARA四轴机械臂的碰撞检测当中，但可以理解的，这并不代表限定了本申请碰撞检测的应用环境，在其它实施例当中，其还可以运用在其它的机械臂的碰撞检测当中。
请参阅图1，所示为本发明第一实施例中的碰撞检测方法，包括步骤S01至步骤S03。
步骤S01，在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形。
其中，在具体建立时，可根据所述障碍物和所述卡爪的俯视图形，来建立对象的碰撞检测模型，此外，还可以采用三维投影的方法来投影生成碰撞检测模型。同时，所建立的所述障碍物模型和所述卡爪模型的摆放位置，应当按照障碍物和卡爪在实际使用过程当中的摆放位置来设定，以确保碰撞检测的可靠性。
需要指出的是，由于本碰撞检测方法后续采用图像接触分析方法来判断障碍物及卡爪是否发生碰撞，显然障碍物及卡爪的高度对本碰撞检测方法不受影响，故不考虑两者的高度，在此直接忽略掉。
此外，选用俯视图形来作为障碍物及卡爪模型的目的在于，在模型当中反映障碍物及卡爪的最大轮廓。本领域技术人员可以理解的，在其它实施例当中，还可以选用障碍物及卡爪的仰视图形来作为对应的碰撞检测模型。
步骤S02，根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹。
其中，关节参数包括关节长度及各关节之间的相对旋转角度。
需要指出的是，由于无论卡爪在平面移动或上下移动时与障碍物，此时两者的俯视图必然会存在接触，系统均将判断两者为发生了接触，因此卡爪上下移动的运行轨迹对整个碰撞检测不作影响，故不考虑卡爪上下移动的运行轨迹，在此直接忽略掉，并直接计算出卡爪平面移动的运行轨迹。
步骤S03，当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞。
可以理解的，所述障碍物模型和所述卡爪模型之间点接触，或所述障碍物模型和所述卡爪模型存在重叠，均会认作为所述障碍物模型和所述卡爪模型发送了接触，而图像之间的这种接触可以采用距离计算方式或图像分析技术来进行判断。
综上，本发明上述实施例当中的碰撞检测方法，本发明当中的碰撞检测方法、系统、可读存储介质及计算机设备，由于卡爪和障碍物的高度对碰撞检测不受影响，故在进行碰撞模型建立时，直接忽略卡爪和障碍物的高度特征，并在同一预设参考平面内，分别建立与障碍物和卡爪的俯视图对应的平面几何模型，由于无论卡爪在平面移动或上下移动时与障碍物，此时两者的俯视图在碰撞点位置必然会存在接触，因此本碰撞检测方法，根据机械臂各关节的参数，并直接忽略卡爪纵向移动的轨迹，计算出卡爪在平面内的运行轨迹，然后即可通过分析卡爪模型在运行轨迹的任一位置点上是否与障碍物模型发生接触，来判断卡爪与障碍物是否存在碰撞，若存在接触，则判定卡爪与障碍物存在碰撞。因此本发明当中的碰撞检测方法，通过合理简化对象模型和检测轨迹，并采用简单的图形接触判定方式来最终确定是否发生碰撞，整个检测过程简单，简单，计算量小，大幅度提高了碰撞检测的效率。
请参阅图2，所示为本发明第二实施例中的碰撞检测方法，包括步骤S11至步骤S17。
步骤S11，建立障碍物及卡爪的三维模型。
其中，根据障碍物及卡爪的设计参数(如长、宽、高等)，即可分别建立出障碍物及卡爪的三维模型。同时障碍物及卡爪的三维模型是摆放位置，应当按照障碍物和卡爪在实际使用过程当中的摆放位置来设定，以确保碰撞检测的可靠性。
步骤S12，选取一预设参考平面，所述预设参考平面当中具有预先建立好的二维坐标系。
优选地，所述预设参考平面选用反映所述障碍物及所述卡爪三维模型的俯视图的平面。
步骤S13，将所述障碍物及所述卡爪的三维模型投影到所述预设参考平面内，以得到障碍物模型及卡爪模型。
其中，由于所述障碍物模型和所述卡爪模型为投影到一投影面上的图像，故所述障碍物模型和所述卡爪模型均为平面几何图形。此外，由于所述预设参考平面选用俯视方向的平面，故所述障碍物模型为所述障碍物的三维模型的俯视图形，而所述卡爪模型为所述卡爪的三维模型的俯视图形。
可以理解的，在其它实施例当中，所述预设参考平面还可以选用反映所述障碍物及所述卡爪三维模型的仰视图的平面，但无论是俯视图还是仰视图，都能够反映所述障碍物模型和所述卡爪的外围最大轮廓，例如圆锥的俯、仰视图均为直径较大一端投影出的圆。
步骤S14，根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹。
其中，关节参数包括关节长度及各关节之间的相对旋转角度。由于无论卡爪在平面移动或上下移动时与障碍物，此时两者的俯视图必然会存在接触，因此在计算所述卡爪的运行轨迹时，可以忽略卡爪上下移动的运行轨迹，直接计算出所述卡爪在平面上的运行轨迹。
举例来说，假设SCARA四轴机械臂的第一轴的关节长度为a1，第二轴的关节长度为a2，那么我们能计算得到卡抓的运行轨迹为：
x＝a1cos(θ1)+a2cos(θ1+θ2)
y＝a1sin(θ1)+a2sin(θ1+θ2)
其中，(x,y)表示卡爪中心点的坐标，表示卡爪绕着自身中心点转过的角度。在上述表达中，没有出现θ3，是因为θ3影响的是卡爪所处的高度，此外SCARA四轴机械臂的第三轴为带动卡爪上下移动的轴，而第四轴为带动卡爪旋转的轴，在此均可忽略。
除此之外，在其它实施例当中，所述步骤S14还可以按照以下步骤进行具体实施：
根据所述机械臂各关节的长度及各关节之间的相对旋转角度，计算出所述卡爪的行进路径；
将所述行进路径投影到所述预设参考平面内，以得到所述运行轨迹。
可以理解的，本实施方式是先计算出所述卡爪的空间行进路径，在将空间行进路径投影到预设参考平面内，以得到所述卡爪的平面运行轨迹，因此本实施方式相比上述实施方式，计算量大一些，故在具体实施时，优选上述实施方式。
步骤S15，判断所述卡爪模型在所述运行轨迹的任一位置点上是否与所述障碍物模型发生接触。
其中，当判断到所述卡爪模型在所述运行轨迹的任一位置点上会与所述障碍物模型发生接触时，则执行步骤S17，当判断到所述卡爪模型在所述运行轨迹的任一位置点上均不与所述障碍物模型发生接触时，否则代表卡爪不会与障碍物发生碰撞，检测通过。
步骤S17，判定所述卡爪与所述障碍物存在碰撞，并发出碰撞提示信号。
其中，发出碰撞提示信号的方式包括振动、闹铃、报警、蜂鸣、弹框提示等。
以下结合具体情况来详细说明，判断所述障碍物模型和所述卡爪模型发生接触的具体实施步骤：
其一，当所述障碍物模型和所述卡爪模型均为圆形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别计算出所述障碍物模型和所述卡爪模型的半径，并计算两者半径之和，以得到参考总半径；
判断所述障碍物模型和所述卡爪模型的中心距是否小于所述参考总半径；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
可以理解的，由于障碍物模型及卡爪模型均为圆形结构，而判断两个圆之间是否发生接触，只需要判断两者中心距是否大于两者半径，若大于则不接触，否则发生接触。
此外，由于所述预设参考平面预先建立好的二维坐标系，因此根据障碍物模型及卡爪模型的圆心点坐标，并根据两点之间的距离计算公式，即可计算出两者的中心距。
其二，当所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
计算出所述卡爪模型的参考半径，并分别计算出所述卡爪模型的圆心点到所述障碍物模型各条边线的点边距；
判断是否存在小于所述参考半径的所述点边距；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
可以理解的，由于障碍物模型为多边形，卡爪模型为圆形结构，而判断圆和多边形之间是否发生接触，只需要判断圆心到多边形每条边的距离是否均大于圆的半径，若是则不会接触，否则会接触。
具体地，请参阅图3，计算所述圆心点到所述障碍物模型边线的点边距的步骤包括：
获取所述圆心点的坐标，并获取所述障碍物模型任一目标边线的长度及两个端点坐标；
以所述目标边线的任一目标端点为基点，建立所述目标端点到所述目标边线的另一个端点的第一向量，并建立所述目标端点到所述圆心点的第二向量(如图3所示)；
计算所述第一向量和所述第二向量的向量点积(利用两向量的点极计算公式)，并判断所述向量点积是否小于0；
若是，计算所述目标端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若否，则判断所述向量点积是否大于等于所述目标边线的长度的平方；
若是，计算所述目标边线的另一个端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若否，则计算所述第一向量和所述第二向量叉积的模，并除以所述目标边线的长度，以得到所述圆心点到所述目标边线的点边距。
其中，由于图形位于已建好坐标系的参考平面当中，故可直接采用高数中的点边及向量相关的计算公式(如向量点积、两点距离公式等)来进行处理。
可以理解的，当向量A1和向量A2的点积小于0时，则代表这两个向量相互垂直，此时点边距即为圆心到基点(其中一个端点)的距离(如图3所示)，当向量A1和向量A2的点积大于等于边线长度的平方时，也代表这两个向量相互垂直，此时点边距即为圆心到另一个端点的距离(如图3所示)，其它情况下，计算两者向量的叉积模并除以边线长度，即可得到点边距。
其三，当所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤还可以包括：
分别将所述障碍物模型及所述卡爪模型的圆心点，向所述障碍物模型的各条边线所在的直线上投影，以在每条所述直线上投影出一线段及一投影点；
分别在每条所述线段的两端向外扩延预设长度，以在每条所述直线上得到一延长线段，所述预设长度与所述卡爪模型的半径一致；
判断每条所述直线上的所述投影点和所述延长线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
可以理解的，由于障碍物模型为多边形(假设有n条边)，卡爪模型为圆形结构，而判断圆和多边形之间是否发生接触，还可以将障碍物模型和卡爪的中心点分别向障碍物n条边所在的直线投影。对每一次投影都可以得到一条线段和一个点，将这条投影得到的线段向两边扩展r的长度，如果这个投影点在扩展的线段之外，就是无碰撞的。如果每一次投影得到的点都在投影得到的扩展线段之内，那么卡爪就会和障碍物相撞。
举例来说，请查阅图4至图5，假设障碍物的n边形的n个顶点坐标分别为(x1,y1),…(xn,yn),那么计算这个多边形向第一、二个顶点所连边所在直线投影后得到的线段可以通过以下方式计算：
顶点(xi,yi)向这条直线投影得到的点可以表示为
令lmin为li，i＝1,...,n中最小的值，令lmax为li，i＝1,...,n中最大的值，那么多边形的投影线段就就可以用数对(lmin,lmax)来表示。扩展线段就可以用(lmin-r,lmax+r)来表示。如果将圆心(x,y)向这条直线投影得到的点表示为
如果l<lmin-r或l>lmax+r，那么卡爪与障碍物就是无碰撞的(如图5所示)，否则，进行下一次投影，即对第二条边的投影。如果在所有投影之后都有lmin-r≤l≤lmax+r，那么卡爪与障碍物就是碰撞的(如图6所示)。
除此之外，还需要指出的是，当卡爪模型为多边形结构，而所述障碍物模型为圆形结构时，这种结构的判断接触的方式，与卡爪模型为圆形结构，障碍物模型为多边形结构的时候相似，同样可采取上述第二或第三的分析方式来进行接触分析。
此外，在处理多边形卡爪的时候，需要从正运动学得到卡爪中心点的位置和卡爪相对标准位置转过的角度，然后就可以求得多边形卡爪的所有顶点的坐标和线段边的表达式。例如，当时，某一个顶点的坐标为/>那么在经过正运动学计算后，这个顶点的坐标为
其四，当所述卡爪模型及所述障碍物模型均为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型，向所述障碍物模型和所述卡爪模型的各条边线所在的直线上投影，以在每条所述直线上投影出两条一线段；
判断每条所述直线上的两条所述线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
可以理解的，针对于这种情况，只能使用基于投影(上述第三种方式)的方法。假设手抓为m边形，障碍物为n边形，那么需要进行m+n次投影，分别对应着手抓的m条边和障碍物的n条边所在的直线。每进行一次投影，两个多边形都可以在这个投影直线上得到两条线段，如果这两条线段无重叠，那么可以直接返回无碰撞，否则进行下一次投影。如果对于所有的投影，两条线段都是有重叠的，那么手抓和障碍物就是有碰撞的。
除此之外，还需要补充说明的是，如果多边形具有某种对称性，则可以减少投影的次数。例如，如果多边形是长方形，则只需要向相邻的两条边各做一次投影即可，而没必要向四条边都进行一次投影，因为长方形的两对边是平行的。
此外，如果卡爪或障碍物不是圆或多边形，而是其他形状，比如扇形，则可以扩展得到一个多边形或圆形，使其能够包住卡爪或障碍物。因为在碰撞检测中重要的是，算法能够将碰撞的情况输出为碰撞，所以可以将卡爪或障碍物进行适当扩展成为算法容易处理的形状。当然如果扩展的空间过多，得到的碰撞检测结果就会过于保守。
本发明另一方面还提供一种碰撞检测系统，请查阅图6，所示为本发明第三实施例中的碰撞检测系统，包括：
模型建立模块11，用于在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；
轨迹计算模块12，用于根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹；
碰撞判定模块13，用于当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞。
进一步地，所述障碍物模型和所述卡爪模型均为圆形结构，所述碰撞判定模块13包括：
第一计算单元131，用于分别计算出所述障碍物模型和所述卡爪模型的半径，并计算两者半径之和，以得到参考总半径；
第一判断单元132，判断所述障碍物模型和所述卡爪模型的中心距是否小于所述参考总半径；
当所述障碍物模型和所述卡爪模型的中心距小于所述参考总半径时，则所述碰撞判定模块13判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，所述碰撞判定模块13包括：
第二计算单元133，用于计算出所述卡爪模型的参考半径，并分别计算出所述卡爪模型的圆心点到所述障碍物模型各条边线的点边距；
第二判断单元134，用于判断是否存在小于所述参考半径的所述点边距；
当判断到存在小于所述参考半径的所述点边距时，则所述碰撞判定模块13判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述第二计算单元133包括：
参数获取子单元1331，用于获取所述圆心点的坐标，并获取所述障碍物模型任一目标边线的长度及两个端点坐标；
向量建立子单元1332，用于以所述目标边线的任一目标端点为基点，建立所述目标端点到所述目标边线的另一个端点的第一向量，并建立所述目标端点到所述圆心点的第二向量；
第一点积判断子单元1333，用于计算所述第一向量和所述第二向量的向量点积，并判断所述向量点积是否小于0；
第一点距计算子单元1334，用于当判断到所述向量点积是小于0时，计算所述目标端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
第二点积判断子单元1335，用于当判断到所述向量点积不是小于0时，判断所述向量点积是否大于等于所述目标边线的长度的平方；
第二点距计算子单元1336，用于当判断到所述向量点积是大于等于所述目标边线的长度的平方时，计算所述目标边线的另一个端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
叉模计算子单元1337，用于当判断到所述向量点积不是大于等于所述目标边线的长度的平方时，计算所述第一向量和所述第二向量叉积的模，并除以所述目标边线的长度，以得到所述圆心点到所述目标边线的点边距。
进一步地，所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，所述碰撞判定模块13包括：
第一投影单元135，用于分别将所述障碍物模型及所述卡爪模型的圆心点，向所述障碍物模型的各条边线所在的直线上投影，以在每条所述直线上投影出一线段及一投影点；
扩延单元136，用于分别在每条所述线段的两端向外扩延预设长度，以在每条所述直线上得到一延长线段，所述预设长度与所述卡爪模型的半径一致；
第一重叠判断单元137，用于判断每条所述直线上的所述投影点和所述延长线段是否重叠；
当判断到每条所述直线上的所述投影点和所述延长线段会重叠时，所述碰撞判定模块13判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述卡爪模型及所述障碍物模型均为多边形结构，所述碰撞判定模块13包括：
第二投影单元138，分别将所述障碍物模型及所述卡爪模型，向所述障碍物模型和所述卡爪模型的各条边线所在的直线上投影，以在每条所述直线上投影出两条一线段；
第二重叠判断单元139，判断每条所述直线上的两条所述线段是否重叠；
当判断到每条所述直线上的两条所述线段会重叠时，所述碰撞判定模块13判断到所述障碍物模型和所述卡爪模型发生接触。
进一步地，所述模型建立模块11包括：
模型建立单元111，用于建立障碍物和卡爪的三维模型；
模型投影单元112，用于分别将所述障碍物和所述卡爪的三维模型投影到所述预设参考平面内，以得到所述障碍物模型及所述卡爪模型。
进一步地，所述碰撞检测系统还包括：
平面选取模块14，用于选取一预设参考平面，所述预设参考平面当中具有预先建立好的二维坐标系。
进一步地，所述碰撞检测系统还包括：
碰撞提示模块15，用于在判定所述卡爪与所述障碍物存在碰撞时，并发出碰撞提示信号。
本发明还提出一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述的碰撞检测方法。
本发明还提出一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述的碰撞检测方法。
本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (9)

1.一种碰撞检测方法，其特征在于，包括：
在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；
根据机械臂各关节参数，计算出所述卡爪在所述预设参考平面内的运行轨迹，其中，所述关节参数包括关节长度及各关节之间的相对旋转角度；
当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞；
所述障碍物模型和所述卡爪模型为圆形结构与多边形结构的任意组合；
其中，当所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型的圆心点，向所述障碍物模型的各条边线所在的直线上投影，以在每条所述直线上投影出一线段及一投影点；
分别在每条所述线段的两端向外扩延预设长度，以在每条所述直线上得到一延长线段，所述预设长度与所述卡爪模型的半径一致；
判断每条所述直线上的所述投影点和所述延长线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触；
设所述障碍物模型为n条边的多边形，所述障碍物模型的n个顶点坐标分别为(x1，y1)......(xn，yn)，所述卡爪模型的半径为r，圆心坐标为(x，y)，以所述障碍物模型的第一个顶点及第二个顶点所连直线为基准，则所述障碍物模型的顶点(xi，yi)向这条直线投影得到的点li表示为：
令lmin为li，i＝1......n中最小的值，令lmax为li，i＝1......n中最大的值，所述障碍物模型对应的多边形的投影线段表示为数对(lmin，lmax)，所述延长线段表示为(lmin-r，lmax+r)；
所述卡爪模型的圆心(x，y)向这条直线投影得到的点l表示为：
若l＜lmin-r或l＞lmax+r，则所述卡爪与所述障碍物无碰撞；
否则，进行对所述障碍物模型下一条边的投影计算，若在对所述障碍物模型的所有边投影之后都有lmin-r≤l≤lmax+r，则所述卡爪与所述障碍物碰撞。
2.根据权利要求1所述的碰撞检测方法，其特征在于，所述障碍物模型和所述卡爪模型均为圆形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别计算出所述障碍物模型和所述卡爪模型的半径，并计算两者半径之和，以得到参考总半径；
判断所述障碍物模型和所述卡爪模型的中心距是否小于所述参考总半径；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
3.根据权利要求1所述的碰撞检测方法，其特征在于，所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤还包括：
计算出所述卡爪模型的参考半径，并分别计算出所述卡爪模型的圆心点到所述障碍物模型各条边线的点边距；
判断是否存在小于所述参考半径的所述点边距；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
4.根据权利要求3所述的碰撞检测方法，其特征在于，计算所述圆心点到所述障碍物模型边线的点边距的步骤包括：
获取所述圆心点的坐标，并获取所述障碍物模型任一目标边线的长度及两个端点坐标；
以所述目标边线的任一目标端点为基点，建立所述目标端点到所述目标边线的另一个端点的第一向量，并建立所述目标端点到所述圆心点的第二向量；
计算所述第一向量和所述第二向量的向量点积，并判断所述向量点积是否小于0；
若是小于0，则计算所述目标端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若不是小于0，则判断所述向量点积是否大于等于所述目标边线的长度的平方；
若是，计算所述目标边线的另一个端点到所述圆心点的距离，以得到所述圆心点到所述目标边线的点边距；
若否，则计算所述第一向量和所述第二向量叉积的模，并除以所述目标边线的长度，以得到所述圆心点到所述目标边线的点边距。
5.根据权利要求1所述的碰撞检测方法，其特征在于，所述卡爪模型及所述障碍物模型均为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型，向所述障碍物模型和所述卡爪模型的各条边线所在的直线上投影，以在每条所述直线上投影出两条一线段；
判断每条所述直线上的两条所述线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触。
6.根据权利要求1所述的碰撞检测方法，其特征在于，所述在一预设参考平面内，分别建立障碍物模型及卡爪模型的步骤包括：
建立障碍物及卡爪的三维模型；
分别将所述障碍物及所述卡爪的三维模型投影到所述预设参考平面内，以得到所述障碍物模型及所述卡爪模型。
7.一种碰撞检测系统，其特征在于，包括：
模型建立模块，用于在一预设参考平面内，分别建立障碍物模型及卡爪模型，所述障碍物模型为所述障碍物的俯视图形，所述卡爪模型为所述卡爪的俯视图形；
轨迹计算模块，用于根据机械臂各关节的参数，计算出所述卡爪在所述预设参考平面内的运行轨迹；其中，所述关节参数包括关节长度及各关节之间的相对旋转角度；
碰撞判定模块，用于当所述卡爪模型在所述运行轨迹的任一位置点上与所述障碍物模型发生接触时，判定所述卡爪与所述障碍物存在碰撞；
所述障碍物模型和所述卡爪模型为圆形结构与多边形结构的任意组合；
其中，当所述卡爪模型为圆形结构，所述障碍物模型为多边形结构，判断所述障碍物模型和所述卡爪模型发生接触的步骤包括：
分别将所述障碍物模型及所述卡爪模型的圆心点，向所述障碍物模型的各条边线所在的直线上投影，以在每条所述直线上投影出一线段及一投影点；
分别在每条所述线段的两端向外扩延预设长度，以在每条所述直线上得到一延长线段，所述预设长度与所述卡爪模型的半径一致；
判断每条所述直线上的所述投影点和所述延长线段是否重叠；
若是，则判断到所述障碍物模型和所述卡爪模型发生接触；
设所述障碍物模型为n条边的多边形，所述障碍物模型的n个顶点坐标分别为(x1，y1)......(xn，yn)，所述卡爪模型的半径为r，圆心坐标为(x，y)，以所述障碍物模型的第一个顶点及第二个顶点所连直线为基准，则所述障碍物模型的顶点(xi，yi)向这条直线投影得到的点li表示为：
令lmin为li，i＝1......n中最小的值，令lmax为li，i＝1......n中最大的值，所述障碍物模型对应的多边形的投影线段表示为数对(lmin，lmax)，所述延长线段表示为(lmin-r，lmax+r)；
所述卡爪模型的圆心(x，y)向这条直线投影得到的点l表示为：
若l＜lmin-r或l＞lmax+r，则所述卡爪与所述障碍物无碰撞；
否则，进行对所述障碍物模型下一条边的投影计算，若在对所述障碍物模型的所有边投影之后都有lmin-r≤l≤lmax+r，则所述卡爪与所述障碍物碰撞。
8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6任一所述的方法。
9.一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任一所述的方法。
CN201810670546.6A 2018-06-26 2018-06-26 碰撞检测方法、系统、可读存储介质及计算机设备 Active CN108959753B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810670546.6A CN108959753B (zh) 2018-06-26 2018-06-26 碰撞检测方法、系统、可读存储介质及计算机设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810670546.6A CN108959753B (zh) 2018-06-26 2018-06-26 碰撞检测方法、系统、可读存储介质及计算机设备

Publications (2)

Publication Number Publication Date
CN108959753A CN108959753A (zh) 2018-12-07
CN108959753B true CN108959753B (zh) 2023-08-08

Family

ID=64486799

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810670546.6A Active CN108959753B (zh) 2018-06-26 2018-06-26 碰撞检测方法、系统、可读存储介质及计算机设备

Country Status (1)

Country Link
CN (1) CN108959753B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109741461B (zh) * 2018-12-18 2023-04-07 南京东巴电子科技有限公司 三维动态模拟施工方案过程中实现碰撞检测的方法
CN110941940B (zh) * 2019-12-04 2023-06-13 杭州广立微电子股份有限公司 一种基于碰撞检测的3d绕线方法、存储设备和系统
CN113721618B (zh) * 2021-08-30 2024-05-24 中科新松有限公司 一种平面确定方法、装置、设备和存储介质
CN114359867A (zh) * 2021-12-31 2022-04-15 中国第一汽车股份有限公司 障碍物的投影信息的处理方法和装置
CN115272379B (zh) * 2022-08-03 2023-11-28 上海新迪数字技术有限公司 一种基于投影的三维网格模型外轮廓提取方法及系统
CN116541939B (zh) * 2023-05-25 2024-01-30 驻马店市博瑞建筑工程有限公司 一种桥梁设计中碰撞检测方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6429617B1 (en) * 1999-10-22 2002-08-06 Kawasaki Jukogyo Kabushiki Kaisha Drive-controlling method and apparatus and robot having the apparatus
CN101925444A (zh) * 2008-01-22 2010-12-22 松下电器产业株式会社 机械臂
CN104029203A (zh) * 2014-06-18 2014-09-10 大连大学 实现空间机械臂避障的路径规划方法
CN104392081A (zh) * 2014-06-30 2015-03-04 管小清 一种冗余机械臂运动障碍物避障算法
CN106003043A (zh) * 2016-06-20 2016-10-12 先驱智能机械（深圳）有限公司 机械臂的避障方法及避障系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6429617B1 (en) * 1999-10-22 2002-08-06 Kawasaki Jukogyo Kabushiki Kaisha Drive-controlling method and apparatus and robot having the apparatus
CN101925444A (zh) * 2008-01-22 2010-12-22 松下电器产业株式会社 机械臂
CN104029203A (zh) * 2014-06-18 2014-09-10 大连大学 实现空间机械臂避障的路径规划方法
CN104392081A (zh) * 2014-06-30 2015-03-04 管小清 一种冗余机械臂运动障碍物避障算法
CN106003043A (zh) * 2016-06-20 2016-10-12 先驱智能机械（深圳）有限公司 机械臂的避障方法及避障系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于A~*改进算法的机械臂避障路径规划;汪首坤等;《北京理工大学学报》;20111115;第31卷(第11期);第1302-1306页 *

Also Published As

Publication number Publication date
CN108959753A (zh) 2018-12-07

Similar Documents

Publication Publication Date Title
CN108959753B (zh) 碰撞检测方法、系统、可读存储介质及计算机设备
US12109710B2 (en) Method and apparatus for motion planning of robot, method and apparatus for path planning of robot, and method and apparatus for grasping of robot
EP3410246B1 (en) Robot obstacle avoidance control system and method, robot, and storage medium
US10562182B2 (en) Robot, robot system, control device, and control method
JP7314411B2 (ja) 移動ロボットの障害物情報感知方法、装置
US9089971B2 (en) Information processing apparatus, control method thereof and storage medium
CN103678754A (zh) 信息处理装置及信息处理方法
Karaszewski et al. Assessment of next-best-view algorithms performance with various 3D scanners and manipulator
CN112264991B (zh) 一种适用于空间机械臂的分层次快速在轨碰撞检测方法
CN113211495B (zh) 一种机械臂碰撞检测方法、系统、存储介质和机械臂
CN113269835A (zh) 基于轮廓特征的工业零件位姿识别方法、装置和电子设备
JP2018169660A (ja) オブジェクト姿勢検出装置、制御装置、ロボットおよびロボットシステム
EP2732935B1 (en) Method for checking a travelable area of a plane of travel for a robot, robot and recording medium
US6597967B2 (en) System and method for planning a tool path along a contoured surface
JP4389663B2 (ja) 画像処理方法及び画像処理装置
JP7500947B2 (ja) 干渉評価装置、方法、及びプログラム
JP6848761B2 (ja) 物体間距離評価方法及び相対的に移動する物体間の干渉評価方法
JP2001250122A (ja) 物体の位置姿勢決定処理方法およびそのためのプログラム記録媒体
CN108858162B (zh) 四轴机械臂的位置确定方法和装置
CN114998381A (zh) 管板焊接中焊接轨迹拟合方法、装置、设备及存储介质
US20240246179A1 (en) Scan planning and scan operations for welding an object
Koszyk et al. Hough Transform for Detection of 3D Point Cloud Rotation
CN118528248A (zh) 一种作业路径点阵规划方法、装置、设备及存储介质
CN117629203A (zh) 复合机器人作业中底盘停靠点的生成方法、装置以及设备
CN116859932A (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