CN112171671B - 工程车辆、机械臂与柔性管路的干涉检测方法及系统 - Google Patents

工程车辆、机械臂与柔性管路的干涉检测方法及系统 Download PDF

Info

Publication number
CN112171671B
CN112171671B CN202011009184.XA CN202011009184A CN112171671B CN 112171671 B CN112171671 B CN 112171671B CN 202011009184 A CN202011009184 A CN 202011009184A CN 112171671 B CN112171671 B CN 112171671B
Authority
CN
China
Prior art keywords
model
catenary
bounding box
discrete
mechanical arm
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
CN202011009184.XA
Other languages
English (en)
Other versions
CN112171671A (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.)
China Railway Construction Heavy Industry Group Co Ltd
Original Assignee
China Railway Construction Heavy Industry Group 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 China Railway Construction Heavy Industry Group Co Ltd filed Critical China Railway Construction Heavy Industry Group Co Ltd
Priority to CN202011009184.XA priority Critical patent/CN112171671B/zh
Publication of CN112171671A publication Critical patent/CN112171671A/zh
Application granted granted Critical
Publication of CN112171671B publication Critical patent/CN112171671B/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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种工程车辆、机械臂与柔性管路的干涉检测方法及系统，机械臂与柔性管路的干涉检测方法包括：获取柔性管路在三维空间下的离散悬链线模型；获取机械臂的包围盒模型；判断所述包围盒模型与所述离散悬链线模型是否相交。上述机械臂与柔性管路的干涉检测方法简便快捷，且节约开发成本，可以对机械臂和柔性管路进行干涉检测，从而减少施工事故的发生，提高施工作业的效率。

Description

工程车辆、机械臂与柔性管路的干涉检测方法及系统
技术领域
本发明涉及工程机械技术领域，特别涉及一种工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统。
背景技术
目前，重载工程装备的机械臂多数采用液压驱动的方式，液压驱动驱动力大、调速简单且平稳，但液压驱动需要设计液压管路系统，在实际施工作业中，机械臂下悬液压管路极易与机械臂发生干涉，影响施工安全与施工效率。在工业机器人机械臂干涉检测的现有技术中，一般需加装力矩传感器或视觉系统等外部装置，以便于实现机械臂与机械臂下悬液压管路的干涉检测，然而，传感器等硬件设备的成本较高，且依据扭矩的干涉检测方法不适用于液压重载机械臂。
因此，如何避免由于依据扭矩的干涉检测方法无法实现液压重载机械臂与其下悬液压管路的干涉检测，是本领域技术人员目前需要解决的技术问题。
发明内容
本发明的目的是提供一种工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统，可以对机械臂和柔性管路进行干涉检测，从而减少施工事故的发生，提高施工作业的效率。
为实现上述目的，本发明提供一种机械臂与柔性管路的干涉检测方法，包括：
获取柔性管路在三维空间下的离散悬链线模型；
获取机械臂的包围盒模型；
判断所述包围盒模型与所述离散悬链线模型是否相交。
可选地，所述获取柔性管路在三维空间下的离散悬链线模型的步骤包括：
获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
将柔性管路投影至重力竖直向上方向向量所在平面，并根据
Figure BDA0002696998170000021
计算重力竖直向上方向向量
Figure BDA0002696998170000022
根据
Figure BDA0002696998170000023
计算重力水平方向向量
Figure BDA0002696998170000024
在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure BDA0002696998170000025
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并将悬链线离散成平面内的点，分别根据
Figure BDA0002696998170000026
和
Figure BDA0002696998170000027
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure BDA0002696998170000028
为悬链线每段线段的长度；
根据
Figure BDA0002696998170000029
计算得到三维离散点坐标pi，其中，i取1-n；
根据所述三维离散点坐标pi创建所述离散悬链线模型。
可选地，所述获取机械臂的包围盒模型的步骤，包括：
创建机械臂正运动学模型；
根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
可选地，所述判断所述包围盒模型与所述离散悬链线模型是否相交的步骤包括：
设定某一分离轴；
将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
可选地，还包括：
设置检测参数、报警检测阈值和极限检测阈值；
根据所述检测参数对需检测的所述包围盒模型和所述悬链线模型进行干涉检测；
根据所述报警检测阈值和所述极限检测阈值对机械臂与柔性管路进行不同预警级别的干涉检测。
本发明还提供一种机械臂与柔性管路的干涉检测系统，包括：
离散悬链线模型获取模块：用于获取柔性管路在三维空间下的离散悬链线模型；
包围盒模型获取模块：用于获取机械臂的包围盒模型；
判断模块：用于判断所述包围盒模型与所述离散悬链线模型是否相交。
可选地，所述离散悬链线模型获取模块包括：
参数获取单元：用于获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第一计算单元：用于将柔性管路投影至重力竖直向上方向向量所在平面，根据
Figure BDA0002696998170000041
计算重力竖直向上方向向量
Figure BDA0002696998170000042
根据
Figure BDA0002696998170000043
计算重力水平方向向量
Figure BDA0002696998170000044
第二计算单元：用于在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure BDA0002696998170000045
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并将悬链线离散成平面内的点，分别根据
Figure BDA0002696998170000046
和
Figure BDA0002696998170000047
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure BDA0002696998170000048
为悬链线每段线段的长度；
第三计算单元：用于根据
Figure BDA0002696998170000049
计算得到三维离散点坐标pi，其中，i取1-n；
第一模型创建单元：用于根据所述三维离散点坐标pi创建所述离散悬链线模型。
可选地，所述包围盒模型获取模块包括：
第二模型创建单元：用于创建机械臂正运动学模型；
变换矩阵计算单元：用于根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
测算单元：用于基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
第三模型创建单元：根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
可选地，所述判断模块包括：
分离轴获取单元：用于设定某一分离轴；
投影区间获取单元：用于将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
投影区间数据获取单元：用于在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断单元：用于判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
本发明还提供一种工程车辆，包括上述机械臂与柔性管路的干涉检测系统。
相对于上述背景技术，本发明提供的机械臂与柔性管路的干涉检测方法，包括：获取柔性管路在三维空间下的离散悬链线模型，获取机械臂的包围盒模型，判断包围盒模型与离散悬链线模型是否相交。可以看出，针对现有技术中缺乏针对柔性管路的建模方法以及对于机械臂和柔性管路干涉检测的方法，本发明实施例所提供的干涉检测方法通过对柔性管路建模以获取柔性管路在三维空间下的离散悬链线模型，并通过创建机械臂的包围盒模型，以将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测；显然地，若检测到包围盒模型和离散悬链线模型相交，则机械臂和柔性管路将发生干涉，否则机械臂和柔性管路不会发生干涉。离散悬链线模型和包围盒模型的建模方法快速准确、拟合精度高，同时，检测包围盒模型和离散悬链线模型是否相交的方法，简便快捷，且节约开发成本，这样即可减少施工事故的发生，提高施工作业的效率。
本发明还提供一种工程车辆和机械臂与柔性管路的干涉检测系统，有益效果如上述，此处将不再赘述。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例所提供的机械臂与柔性管路的干涉检测方法的流程图；
图2为本发明实施例所提供的柔性管路的示意图；
图3为本发明实施例所提供的柔性管路的离散悬链线模型的示意图；
图4为本发明实施例所提供的机械臂与柔性管路的干涉检测方法的仿真测试图；
图5为本发明实施例所提供的机械臂与柔性管路的干涉检测系统的结构框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。
本发明实施例所提供的一种机械臂与柔性管路的干涉检测方法，参考说明书附图1，包括：
S1：获取柔性管路在三维空间下的离散悬链线模型；
S2：获取机械臂的包围盒模型；
S3：判断所述包围盒模型与所述离散悬链线模型是否相交。
需要说明的是，所谓柔性管路是指机械臂的下悬液压管路，机械臂通过液压驱动方式实现驱动，可以保证足够的驱动力，并且液压驱动调速简单、平稳。
针对步骤S1，首先将机械臂下的柔性管路投影到重力竖直向上方向向量所在平面，并对其进行二维离散悬链线的近似建模，然后将以上离散点再转换至三维空间，完成三维空间离散悬链线的建模。
针对步骤S2，机械臂可由多个连杆构成，通过包围盒将连杆包络，近似代替机械臂的连杆，从而构建机械臂的包围盒模型。
针对步骤S2，采用方向包围盒(Oriented Bounding Box：OBB)检测算法进行机械臂与柔性管路的干涉检测，将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测。
可以看出，针对现有技术中缺乏针对柔性管路的建模方法以及对于机械臂和柔性管路干涉检测的方法，本发明实施例所提供的干涉检测方法通过对柔性管路建模以获取柔性管路在三维空间下的离散悬链线模型，并通过创建机械臂的包围盒模型，以将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测。
显然，若包围盒模型和离散悬链线模型相交，则机械臂和柔性管路将发生干涉，否则机械臂和柔性管路不发生干涉。离散悬链线模型和包围盒模型的建模方法快速准确、拟合精度高，同时，检测离散悬链线模型和包围盒模型是否相交的方法，简便快捷，且节约开发成本，这样即可减少施工事故的发生，提高施工作业的效率。
针对上述步骤S1，包括：
第一步，获取包括机械臂与柔性管路的工程装备(一般为台车)的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第二步，将柔性管路投影至重力竖直向上方向向量所在平面，并根据
Figure BDA0002696998170000081
计算重力竖直向上方向向量
Figure BDA0002696998170000082
根据
Figure BDA0002696998170000083
计算重力水平方向向量
Figure BDA0002696998170000084
第三步，在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure BDA0002696998170000085
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并将悬链线离散成平面内的点，假设二维悬链线有n个轨迹点，分别根据
Figure BDA0002696998170000086
和
Figure BDA0002696998170000087
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure BDA0002696998170000088
为悬链线每段线段的长度；
第四步，根据
Figure BDA0002696998170000089
计算得到三维离散点坐标pi，其中，i取1-n；
第五步，根据所述三维离散点坐标pi创建所述离散悬链线模型。
参考说明书附图2和3，基于机械臂下悬液压管路在自重作用下的真实线形为悬链线，且具有粗细与质量分布均匀、两端固定、定长曲线等与悬链线相似的状态特性，因此，本发明实施例采用离散悬链线模型对其近似建模。
需要注意的是，由于悬链线的标准方程是基于二维平面描述的，为了保证干涉检测方法的精确度，本发明实施例在三维空间中对机械臂下悬液压管路进行建模与干涉检测。
在第一步中，即为测量柔性管路建模与干涉检测所需的数据，俯仰角度pitch、横滚角度roll能够表示台车(也即整个装备)的倾斜姿态，为了便于计算第二步中装备的重力水平方向向量
Figure BDA0002696998170000091
还需要在台车坐标系下测得柔性管路的两个悬挂点坐标p1、p2。为了便于计算第三步中二维离散点的横坐标xi，还需要测得柔性管路的长度l。
需要说明的是，在第一步中，可根据实际作业状态，仅对需进行检测的主要柔性管路建模即可。
在第二步中，通过计算投影平面的竖直与水平方向向量，以便于计算三维离散点坐标。具体地，首先将柔性管路投影至重力竖直向上方向向量所在平面，并计算装备重力竖直向上方向向量
Figure BDA0002696998170000092
Figure BDA0002696998170000093
进一步的，计算装备重力水平方向向量
Figure BDA0002696998170000094
Figure BDA0002696998170000095
在第三步中，为了获取悬链线的二维离散点，将悬链线离散成平面内的点。首先，在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)，计算悬链线平面方程
Figure BDA0002696998170000096
的参数a，其中，a为悬链线顶点到横坐标轴的距离。
假定二维悬链线有n个轨迹点，其中第i个离散点的横坐标xi和纵坐标yi分别为：
Figure BDA0002696998170000101
Figure BDA0002696998170000102
其中，i取1-n，x0、y0为离散点的初始坐标，
Figure BDA0002696998170000103
为悬链线每段线段的长度。
在第四步中，将二维离散点转换到三维空间，三维离散点坐标pi为：
Figure BDA0002696998170000104
其中，i取1-n。
最后，根据得到的三维离散点坐标pi得到离散悬链线模型。
针对上述步骤S2，具体可以设置为包括：创建机械臂正运动学模型，根据所述机械臂正运动学模型计算各个包围盒的变换矩阵，基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数，根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
本发明实施例所提供的干涉检测方法是采用方向包围盒检测算法进行检测，采用包围盒将机械臂的连杆包络，近似替代机械臂连杆。机械臂正运动学模型的创建可以参照现有技术的内容。
进一步的，在装备上标记柔性管路所属机械臂编号、两个悬挂点所属连杆编号，并基于机械臂正运动学模型动态测量柔性管路悬挂点所属连杆的坐标变换矩阵。
此外，关于包围盒的几何中心坐标和长宽高参数的测量可以在机械臂的三维模型上测量，也可以直接在装备上测量；结合包围盒的变换矩阵以及测得的几何中心坐标和长宽高参数创建不同机械臂几何构型的包围盒模型。
针对上述步骤S3，在对柔性管路进行三维空间下离散悬链线的建模后，机械臂与柔性管路的干涉检测即可转化为包围盒模型与线段的相交检测，其中，离散悬链线模型是由多条线段构成。
判断包围盒模型与线段是否相交可以采用分离轴定理进行算法实现，具体包括：设定某一分离轴(假定存在某一分离轴)，将离散悬链线模型中的任一线段投影于分离轴上并获得投影区间，在投影区间内获取线段的中心点与包围盒模型的中心点的有符号距离ds、线段在分离轴上的投影半径rs和包围盒模型在分离轴上的投影半径rb，再判断|ds|＞rb+rs是否成立，若是，则包围盒模型与线段不相交，若否，则包围盒模型与线段相交。
当然，上述分离轴需设置为一经过包围盒模型中心点的向量。
为了实现不同预警级别的干涉检测，上述干涉检测方法还包括在机械臂与柔性管路干涉检测的阈值文件中设置检测参数(说明某一包围盒模型或悬链线模型是否参与干涉检测，可灵活设置)、报警检测阈值与极限检测阈值，并将报警检测阈值、极限检测阈值与干涉检测算法关联，从而将多级别预警信息融入机械臂与柔性管路的干涉检测算法中，实现基于不同预警级别的干涉检测。需要说明的是，对于包围盒与线段的干涉检测，需对6个轴分别进行分离轴检测，即获得3条包围盒上的面法线、线段方向向量与3条面法线的叉积向量，其中，只要有一个分离轴存在，说明包围盒与线段不相交，即可停止检测。
此外，对于任一包围盒模型与离散悬链线的干涉检测，需对包围盒模型与悬链线上所有线段进行干涉检测，并且只要存在一条线段与包围盒相交，说明机械臂连杆与悬链线将发生干涉。
显然，对于机械臂与柔性管路的干涉检测，需要对机械臂上所有待检测包围盒模型与离散悬链线进行干涉检测，并且只要存在一个包围盒模型与悬链线干涉，说明机械臂与柔性管路将发生干涉。
参考说明书附图4，附图4给出了一个仿真测试图，在该仿真中，报警检测阈值为0.1，极限检测阈值为0，柔性管路用悬链线表示，机械臂连杆用包围盒表示，算法一旦检测到柔性管路与机械臂连杆将发生干涉，则将要发生干涉的悬链线与包围盒用虚线表示(图4中间位置所示)，附图4可表明本发明实施例所提供的柔性管路建模与干涉检测方法是可行且有效的。
本发明实施例还提供一种机械臂与柔性管路的干涉检测系统，如说明书附图5，机械臂与柔性管路的干涉检测系统的各个部件的功能和工作原理可参考上文机械臂与柔性管路的干涉检测方法，机械臂与柔性管路的干涉检测系统，包括：
离散悬链线模型获取模块101：用于获取柔性管路在三维空间下的离散悬链线模型；
包围盒模型获取模块102：用于获取机械臂的包围盒模型；
判断模块103：用于判断所述包围盒模型与所述离散悬链线模型是否相交。
进一步的，上述离散悬链线模型获取模块101包括：
参数获取单元：用于获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第一计算单元：用于将柔性管路投影至重力竖直向上方向向量所在平面，并根据
Figure BDA0002696998170000121
计算重力竖直向上方向向量
Figure BDA0002696998170000122
根据
Figure BDA0002696998170000123
计算重力水平方向向量
Figure BDA0002696998170000124
第二计算单元：用于在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure BDA0002696998170000131
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并假定二维悬链线有n个轨迹点，分别根据
Figure BDA0002696998170000132
和
Figure BDA0002696998170000133
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure BDA0002696998170000134
为悬链线每段线段的长度；
第三计算单元：用于根据
Figure BDA0002696998170000135
计算得到三维离散点坐标pi，其中，i取1-n；
第一模型创建单元：用于根据所述三维离散点坐标pi创建所述离散悬链线模型。
上述包围盒模型获取模块102包括：
第二模型创建单元：用于创建机械臂正运动学模型；
变换矩阵计算单元：用于根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
测算单元：用于基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
第三模型创建单元：根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
另有，判断模块103，包括：
分离轴获取单元：用于设定某一分离轴；
投影区间获取单元：用于将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
投影区间数据获取单元：用于在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在分离轴上的投影半径rb；
判断单元：用于判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
需要说明的是，本发明实施例所提供的一种机械臂与柔性管路的干涉检测系统采用的是基于不同预警级别的干涉检测方法，因此，该系统本质上是干涉预警系统，可在机械臂距离液压管路不同距离时实现不同安全级别的干涉预警。此外，该系统还可与车载计算机、蜂鸣器、警示灯等硬件模块共同实现不同预警级别的声光预警，此部分可以参照现有技术，本文不再展开。
本发明所提供的一种工程车辆，包括上述具体实施例所描述的机械臂与柔性管路的干涉检测系统；工程车辆的其他部分可以参照现有技术，本文不再展开。
需要说明的是，在本说明书中，诸如第一和第二之类的关系术语仅仅用来将一个实体与另外几个实体区分开来，而不一定要求或者暗示这些实体之间存在任何这种实际的关系或者顺序。
以上对本发明所提供的工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方案及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (8)

1.一种机械臂与柔性管路的干涉检测方法，其特征在于，包括：
获取柔性管路在三维空间下的离散悬链线模型；
获取机械臂的包围盒模型；
判断所述包围盒模型与所述离散悬链线模型是否相交；
所述获取柔性管路在三维空间下的离散悬链线模型的步骤包括：
获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
将柔性管路投影至重力竖直向上方向向量所在平面，根据
Figure FDA0003312232820000011
计算重力竖直向上方向向量
Figure FDA0003312232820000012
根据
Figure FDA0003312232820000013
计算重力水平方向向量
Figure FDA0003312232820000014
在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure FDA0003312232820000015
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并将悬链线离散成平面内的点，分别根据
Figure FDA0003312232820000016
和
Figure FDA0003312232820000017
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure FDA0003312232820000018
为悬链线每段线段的长度；
根据
Figure FDA0003312232820000021
计算得到三维离散点坐标pi，其中，i取1-n；
根据所述三维离散点坐标pi创建所述离散悬链线模型。
2.如权利要求1所述的机械臂与柔性管路的干涉检测方法，其特征在于，所述获取机械臂的包围盒模型的步骤，包括：
创建机械臂正运动学模型；
根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
3.如权利要求1所述的机械臂与柔性管路的干涉检测方法，其特征在于，所述判断所述包围盒模型与所述离散悬链线模型是否相交的步骤包括：
设定某一分离轴；
将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
4.如权利要求1-3任意一项所述的机械臂与柔性管路的干涉检测方法，其特征在于，还包括：
设置检测参数、报警检测阈值和极限检测阈值；
根据所述检测参数对需检测的所述包围盒模型和所述悬链线模型进行干涉检测；
根据所述报警检测阈值和所述极限检测阈值对机械臂与柔性管路进行不同预警级别的干涉检测。
5.一种机械臂与柔性管路的干涉检测系统，其特征在于，包括：
离散悬链线模型获取模块：用于获取柔性管路在三维空间下的离散悬链线模型；
包围盒模型获取模块：用于获取机械臂的包围盒模型；
判断模块：用于判断所述包围盒模型与所述离散悬链线模型是否相交；
所述离散悬链线模型获取模块包括：
参数获取单元：用于获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第一计算单元：用于将柔性管路投影至重力竖直向上方向向量所在平面，根据
Figure FDA0003312232820000031
计算重力竖直向上方向向量
Figure FDA0003312232820000032
根据
Figure FDA0003312232820000033
计算重力水平方向向量
Figure FDA0003312232820000034
第二计算单元：用于在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
Figure FDA0003312232820000035
的参数a，其中，a为悬链线顶点到横坐标轴的距离，并将悬链线离散成平面内的点，分别根据
Figure FDA0003312232820000041
和
Figure FDA0003312232820000042
计算得到第i个二维离散点的横坐标xi和纵坐标yi，其中，i取1-n，x0、y0为离散点的初始坐标，
Figure FDA0003312232820000043
为悬链线每段线段的长度；
第三计算单元：用于根据
Figure FDA0003312232820000044
计算得到三维离散点坐标pi，其中，i取1-n；
第一模型创建单元：用于根据所述三维离散点坐标pi创建所述离散悬链线模型。
6.如权利要求5所述的机械臂与柔性管路的干涉检测系统，其特征在于，所述包围盒模型获取模块包括：
第二模型创建单元：用于创建机械臂正运动学模型；
变换矩阵计算单元：用于根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
测算单元：用于基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
第三模型创建单元：根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
7.如权利要求5所述的机械臂与柔性管路的干涉检测系统，其特征在于，所述判断模块包括：
分离轴获取单元：用于设定某一分离轴；
投影区间获取单元：用于将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
投影区间数据获取单元：用于在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断单元：用于判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
8.一种工程车辆，其特征在于，包括如权利要求5至7任意一项所述的机械臂与柔性管路的干涉检测系统。
CN202011009184.XA 2020-09-23 2020-09-23 工程车辆、机械臂与柔性管路的干涉检测方法及系统 Active CN112171671B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011009184.XA CN112171671B (zh) 2020-09-23 2020-09-23 工程车辆、机械臂与柔性管路的干涉检测方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011009184.XA CN112171671B (zh) 2020-09-23 2020-09-23 工程车辆、机械臂与柔性管路的干涉检测方法及系统

Publications (2)

Publication Number Publication Date
CN112171671A CN112171671A (zh) 2021-01-05
CN112171671B true CN112171671B (zh) 2021-11-30

Family

ID=73955388

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011009184.XA Active CN112171671B (zh) 2020-09-23 2020-09-23 工程车辆、机械臂与柔性管路的干涉检测方法及系统

Country Status (1)

Country Link
CN (1) CN112171671B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107116540A (zh) * 2016-02-24 2017-09-01 中国科学院沈阳计算技术研究所有限公司 一种基于scs包围结构的机器人碰撞检测方法
CN108181635A (zh) * 2017-12-11 2018-06-19 中国南方电网有限责任公司超高压输电公司广州局 一种用于输电线路交叉跨越分析的激光点云分类方法
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN109465835A (zh) * 2018-09-25 2019-03-15 华中科技大学 一种动态环境下双臂服务机器人作业的事前安全预测方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9002511B1 (en) * 2005-10-21 2015-04-07 Irobot Corporation Methods and systems for obstacle detection using structured light
JP2009531745A (ja) * 2006-03-30 2009-09-03 モナシュ ユニバーシティー 変形可能な物体間の相互作用をモデル化する方法
WO2013133779A2 (en) * 2012-03-07 2013-09-12 Robat Kontrol Otomasyon Ar Ge Ve Yazilim Catenary measurement robot and method
DE102017010718A1 (de) * 2017-11-17 2019-05-23 Kuka Deutschland Gmbh Verfahren und Mittel zum Betreiben einer Roboteranordnung
CN108326859A (zh) * 2018-05-07 2018-07-27 广东工业大学 一种有效避免机械臂在工作过程中发生干涉的方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107116540A (zh) * 2016-02-24 2017-09-01 中国科学院沈阳计算技术研究所有限公司 一种基于scs包围结构的机器人碰撞检测方法
CN108181635A (zh) * 2017-12-11 2018-06-19 中国南方电网有限责任公司超高压输电公司广州局 一种用于输电线路交叉跨越分析的激光点云分类方法
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN109465835A (zh) * 2018-09-25 2019-03-15 华中科技大学 一种动态环境下双臂服务机器人作业的事前安全预测方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
线束敷设路径评价研究;洪伟;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20170315;全文 *

Also Published As

Publication number Publication date
CN112171671A (zh) 2021-01-05

Similar Documents

Publication Publication Date Title
CN107116540B (zh) 一种基于scs包围结构的机器人碰撞检测方法
CN110530638A (zh) 基于数字孪生的航空发动机主轴承损伤检测与诊断方法
CN109558694B (zh) 一种水下机器人和机械手系统抓取运动过程的水动力分析方法
Blanchet et al. Interference detection for cable-driven parallel robots (CDPRs)
Gálvez et al. Intrinsic tactile sensing for the optimization of force distribution in a pipe crawling robot
CN103900528A (zh) 一种大型构件三维空间剖面圆度检测方法
CN111737877B (zh) 一种基于三维模型的船体分段吊装设计方法及设计平台
CN112264991A (zh) 一种适用于空间机械臂的分层次快速在轨碰撞检测方法
CN102620890A (zh) 铰接式装载机动态重心位置检测系统及方法
CN116226977A (zh) 施工塔吊的监管方法、装置、电子设备及存储介质
Lee et al. Block turnover simulation considering the interferences between the block and wire ropes in shipbuilding
CN112743575A (zh) 面向加工现场的串联工业机器人静态刚度辨识系统和方法
Chen et al. Linkage characteristics of deep-sea mining lifting system
CN106802979B (zh) 基于有限元分析焊接机器人模型简化方法
CN116734757A (zh) 基于无人机机载激光扫描仪的隧道围岩变形监测预警方法
CN112171671B (zh) 工程车辆、机械臂与柔性管路的干涉检测方法及系统
CN115958611A (zh) 机械臂灵巧度的评估方法、装置及存储介质
TWI492080B (zh) 三次元安全面建立系統及方法
CN118228414A (zh) 一种基于bim技术的管道支吊架自动选型系统及使用方法
KR100994742B1 (ko) 3차원 측정기의 이동 경로에 대한 충돌 검출 및 경과점 생성 방법
CN110238848B (zh) 一种机器人坐标系下重力矢量的计算方法
WO2024060792A1 (zh) 一种分体式集装箱吊具的锁孔定位系统及方法
CN106768636A (zh) 一种可倾翻驾驶室重心测算方法
Kolodziejczyk Preliminary study of hydrodynamic load on an underwater robotic manipulator
Lee et al. Block erection simulation considering frictional contact with wire ropes

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