商标分类 
商标转让 
您当前的位置： 工程车辆、机械臂与柔性管路的干涉检测方法及系统与流程
2021-01-19 17:01:44|
329|
起点商标网
本发明涉及工程机械技术领域，特别涉及一种工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统。
背景技术：
目前，重载工程装备的机械臂多数采用液压驱动的方式，液压驱动驱动力大、调速简单且平稳，但液压驱动需要设计液压管路系统，在实际施工作业中，机械臂下悬液压管路极易与机械臂发生干涉，影响施工安全与施工效率。在工业机器人机械臂干涉检测的现有技术中，一般需加装力矩传感器或视觉系统等外部装置，以便于实现机械臂与机械臂下悬液压管路的干涉检测，然而，传感器等硬件设备的成本较高，且依据扭矩的干涉检测方法不适用于液压重载机械臂。
因此，如何避免由于依据扭矩的干涉检测方法无法实现液压重载机械臂与其下悬液压管路的干涉检测，是本领域技术人员目前需要解决的技术问题。
技术实现要素：
本发明的目的是提供一种工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统，可以对机械臂和柔性管路进行干涉检测，从而减少施工事故的发生，提高施工作业的效率。
为实现上述目的，本发明提供一种机械臂与柔性管路的干涉检测方法，包括：
获取柔性管路在三维空间下的离散悬链线模型；
获取机械臂的包围盒模型；
判断所述包围盒模型与所述离散悬链线模型是否相交。
可选地，所述获取柔性管路在三维空间下的离散悬链线模型的步骤包括：
获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
将柔性管路投影至重力竖直向上方向向量所在平面，并根据
在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
根据
根据所述三维离散点坐标pi创建所述离散悬链线模型。
可选地，所述获取机械臂的包围盒模型的步骤，包括：
创建机械臂正运动学模型；
根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
可选地，所述判断所述包围盒模型与所述离散悬链线模型是否相交的步骤包括：
设定某一分离轴；
将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
可选地，还包括：
设置检测参数、报警检测阈值和极限检测阈值；
根据所述检测参数对需检测的所述包围盒模型和所述悬链线模型进行干涉检测；
根据所述报警检测阈值和所述极限检测阈值对机械臂与柔性管路进行不同预警级别的干涉检测。
本发明还提供一种机械臂与柔性管路的干涉检测系统，包括：
离散悬链线模型获取模块：用于获取柔性管路在三维空间下的离散悬链线模型；
包围盒模型获取模块：用于获取机械臂的包围盒模型；
判断模块：用于判断所述包围盒模型与所述离散悬链线模型是否相交。
可选地，所述离散悬链线模型获取模块包括：
参数获取单元：用于获取包括机械臂与柔性管路的工程装备的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第一计算单元：用于将柔性管路投影至重力竖直向上方向向量所在平面，根据
第二计算单元：用于在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
第三计算单元：用于根据
第一模型创建单元：用于根据所述三维离散点坐标pi创建所述离散悬链线模型。
可选地，所述包围盒模型获取模块包括：
第二模型创建单元：用于创建机械臂正运动学模型；
变换矩阵计算单元：用于根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
测算单元：用于基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
第三模型创建单元：根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
可选地，所述判断模块包括：
分离轴获取单元：用于设定某一分离轴；
投影区间获取单元：用于将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
投影区间数据获取单元：用于在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在所述分离轴上的投影半径rb；
判断单元：用于判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
本发明还提供一种工程车辆，包括上述机械臂与柔性管路的干涉检测系统。
相对于上述背景技术，本发明提供的机械臂与柔性管路的干涉检测方法，包括：获取柔性管路在三维空间下的离散悬链线模型，获取机械臂的包围盒模型，判断包围盒模型与离散悬链线模型是否相交。可以看出，针对现有技术中缺乏针对柔性管路的建模方法以及对于机械臂和柔性管路干涉检测的方法，本发明实施例所提供的干涉检测方法通过对柔性管路建模以获取柔性管路在三维空间下的离散悬链线模型，并通过创建机械臂的包围盒模型，以将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测；显然地，若检测到包围盒模型和离散悬链线模型相交，则机械臂和柔性管路将发生干涉，否则机械臂和柔性管路不会发生干涉。离散悬链线模型和包围盒模型的建模方法快速准确、拟合精度高，同时，检测包围盒模型和离散悬链线模型是否相交的方法，简便快捷，且节约开发成本，这样即可减少施工事故的发生，提高施工作业的效率。
本发明还提供一种工程车辆和机械臂与柔性管路的干涉检测系统，有益效果如上述，此处将不再赘述。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例所提供的机械臂与柔性管路的干涉检测方法的流程图；
图2为本发明实施例所提供的柔性管路的示意图；
图3为本发明实施例所提供的柔性管路的离散悬链线模型的示意图；
图4为本发明实施例所提供的机械臂与柔性管路的干涉检测方法的仿真测试图；
图5为本发明实施例所提供的机械臂与柔性管路的干涉检测系统的结构框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。
本发明实施例所提供的一种机械臂与柔性管路的干涉检测方法，参考说明书附图1，包括：
s1：获取柔性管路在三维空间下的离散悬链线模型；
s2：获取机械臂的包围盒模型；
s3：判断所述包围盒模型与所述离散悬链线模型是否相交。
需要说明的是，所谓柔性管路是指机械臂的下悬液压管路，机械臂通过液压驱动方式实现驱动，可以保证足够的驱动力，并且液压驱动调速简单、平稳。
针对步骤s1，首先将机械臂下的柔性管路投影到重力竖直向上方向向量所在平面，并对其进行二维离散悬链线的近似建模，然后将以上离散点再转换至三维空间，完成三维空间离散悬链线的建模。
针对步骤s2，机械臂可由多个连杆构成，通过包围盒将连杆包络，近似代替机械臂的连杆，从而构建机械臂的包围盒模型。
针对步骤s2，采用方向包围盒(orientedboundingbox：obb)检测算法进行机械臂与柔性管路的干涉检测，将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测。
可以看出，针对现有技术中缺乏针对柔性管路的建模方法以及对于机械臂和柔性管路干涉检测的方法，本发明实施例所提供的干涉检测方法通过对柔性管路建模以获取柔性管路在三维空间下的离散悬链线模型，并通过创建机械臂的包围盒模型，以将机械臂与柔性管路的干涉检测转化为包围盒模型和离散悬链线模型的相交检测。
显然，若包围盒模型和离散悬链线模型相交，则机械臂和柔性管路将发生干涉，否则机械臂和柔性管路不发生干涉。离散悬链线模型和包围盒模型的建模方法快速准确、拟合精度高，同时，检测离散悬链线模型和包围盒模型是否相交的方法，简便快捷，且节约开发成本，这样即可减少施工事故的发生，提高施工作业的效率。
针对上述步骤s1，包括：
第一步，获取包括机械臂与柔性管路的工程装备(一般为台车)的俯仰角度pitch、横滚角度roll，并获取在台车坐标系下柔性管路的两个悬挂点坐标p1、p2及长度l；
第二步，将柔性管路投影至重力竖直向上方向向量所在平面，并根据
第三步，在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
第四步，根据
第五步，根据所述三维离散点坐标pi创建所述离散悬链线模型。
参考说明书附图2和3，基于机械臂下悬液压管路在自重作用下的真实线形为悬链线，且具有粗细与质量分布均匀、两端固定、定长曲线等与悬链线相似的状态特性，因此，本发明实施例采用离散悬链线模型对其近似建模。
需要注意的是，由于悬链线的标准方程是基于二维平面描述的，为了保证干涉检测方法的精确度，本发明实施例在三维空间中对机械臂下悬液压管路进行建模与干涉检测。
在第一步中，即为测量柔性管路建模与干涉检测所需的数据，俯仰角度pitch、横滚角度roll能够表示台车(也即整个装备)的倾斜姿态，为了便于计算第二步中装备的重力水平方向向量
需要说明的是，在第一步中，可根据实际作业状态，仅对需进行检测的主要柔性管路建模即可。
在第二步中，通过计算投影平面的竖直与水平方向向量，以便于计算三维离散点坐标。具体地，首先将柔性管路投影至重力竖直向上方向向量所在平面，并计算装备重力竖直向上方向向量
进一步的，计算装备重力水平方向向量
在第三步中，为了获取悬链线的二维离散点，将悬链线离散成平面内的点。首先，在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)，计算悬链线平面方程
假定二维悬链线有n个轨迹点，其中第i个离散点的横坐标xi和纵坐标yi分别为：
其中，i取1-n，x0、y0为离散点的初始坐标，
在第四步中，将二维离散点转换到三维空间，三维离散点坐标pi为：
其中，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；
第一计算单元：用于将柔性管路投影至重力竖直向上方向向量所在平面，并根据
第二计算单元：用于在平面内基于二维悬挂点p′1(x，y)与p′2(x，y)计算悬链线平面方程
第三计算单元：用于根据
第一模型创建单元：用于根据所述三维离散点坐标pi创建所述离散悬链线模型。
上述包围盒模型获取模块102包括：
第二模型创建单元：用于创建机械臂正运动学模型；
变换矩阵计算单元：用于根据所述机械臂正运动学模型计算各个包围盒的变换矩阵；
测算单元：用于基于所述机械臂正运动学模型的关节坐标原点测算包围盒的几何中心坐标和长宽高参数；
第三模型创建单元：根据所述变换矩阵、所述几何中心坐标和所述长宽高参数创建不同机械臂几何构型的所述包围盒模型。
另有，判断模块103，包括：
分离轴获取单元：用于设定某一分离轴；
投影区间获取单元：用于将所述离散悬链线模型中的任一线段投影于所述分离轴上并获得投影区间；
投影区间数据获取单元：用于在所述投影区间内获取所述线段的中心点与所述包围盒模型的中心点的有符号距离ds、所述线段在所述分离轴上的投影半径rs和所述包围盒模型在分离轴上的投影半径rb；
判断单元：用于判断|ds|＞rb+rs是否成立，若是，则所述包围盒模型与所述线段不相交，若否，则所述包围盒模型与所述线段相交。
需要说明的是，本发明实施例所提供的一种机械臂与柔性管路的干涉检测系统采用的是基于不同预警级别的干涉检测方法，因此，该系统本质上是干涉预警系统，可在机械臂距离液压管路不同距离时实现不同安全级别的干涉预警。此外，该系统还可与车载计算机、蜂鸣器、警示灯等硬件模块共同实现不同预警级别的声光预警，此部分可以参照现有技术，本文不再展开。
本发明所提供的一种工程车辆，包括上述具体实施例所描述的机械臂与柔性管路的干涉检测系统；工程车辆的其他部分可以参照现有技术，本文不再展开。
需要说明的是，在本说明书中，诸如第一和第二之类的关系术语仅仅用来将一个实体与另外几个实体区分开来，而不一定要求或者暗示这些实体之间存在任何这种实际的关系或者顺序。
以上对本发明所提供的工程车辆、机械臂与柔性管路的干涉检测方法和干涉检测系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方案及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】
与客服一对一沟通，为大家解决相关问题。
此文章来源于网络,如有侵权,请联系删除
热门咨询
