臂架防碰撞系统及方法与流程
本发明涉及工程机械领域，具体地涉及臂架防碰撞系统及方法。
背景技术：
多关节机械臂在施工范围内存在障碍物的工况(如隧道工况)作业时，需要考虑机械臂的臂节与环境障碍物之间的碰撞。无论是机械臂全自动施工还是手动施工，对防碰撞计算的实时性要求很高。目前，国内外对机械臂工作过程中的防碰撞机制有一定的基础，但是尚缺乏一种用于在有限工作范围内且同时实现计算的高效和完全可靠的防碰撞系统。
目前国内外对机械臂工作过程中的防碰撞机制主要存在以下三种方案：1)通过对机械臂及周围环境障碍物进行三维建模，采用碰撞检测算法，在路径规划过程中实时进行碰撞检测；2)在机械臂关节或末端加装力矩检测设备，当碰撞发生时，触发紧急处理策略；以及3)在机械臂关节或末端外置测距设备，实时检测机械臂与环境障碍物的间距，当间距小于阈值，触发紧急处理策略。
然而，这三种方案均存在相应缺陷。例如，对于方案1)而言，通过现有防碰撞算法，在路径规划过程中实时进行防碰撞检测，计算效率低，难以满足机械臂实时运行过程中的响应性能要求。对于方案2)而言，通过在机械臂关节或末端加装力矩检测设备，容易造成机械臂与环境障碍物发生严重的接触造成对障碍物及机械臂的破坏，从而造成人员及财产风险；对于方案3)而言，多关节机械臂在折叠式放置时，通过外置的测距设备测量臂节与周围的距离，会因为臂节阻挡而无法形成有效输出。
技术实现要素：
本发明实施例的目的是提供一种臂架防碰撞系统及方法，其可提高防碰撞过程的计算效率、满足实时响应的需求。
为了实现上述目的，本发明实施例提供一种臂架防碰撞方法，该方法包括：确定所述臂架的各个关节及臂架末端中的至少一者的位置；确定与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面；以及计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离，并在该距离小于第一预定阈值的情况下，确定所述臂架存在碰撞风险。
可选的，在所述臂架的各个关节及臂架末端中的所述至少一者为关节的情况下，所述第一预定阈值不小于该关节的关节圆周半径尺寸与运动误差绝对值之和；在所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端的情况下，所述第一预定阈值不小于所述运动误差绝对值。
可选的，所述环境障碍物截面是通过用于生成有关所述臂架所处的环境的环境障碍物数据的环境障碍物模型生成的。
可选的，所述环境障碍物数据为三维点云数据或三角网格数据。
可选的，所述计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离包括：计算所述臂架的所有关节及臂架末端与所述环境障碍物截面之间的最小距离。
可选的，所述环境障碍物截面为凹面。
可选的，所述环境障碍物截面包含满足预定特征点密度的多个截面特征点。
可选的，所述臂架所处的环境为隧道，所述与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面为所述隧道在与所述臂架的各个关节及臂架末端中的所述至少一者在所述隧道轴向上的位置相对应的隧道截面。
可选的，该方法还包括：确定所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离；在所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离与所述计算的最小距离之差大于第二预定阈值的情况下，判定所述关于所述臂架的碰撞风险的确定无效。
可选的，所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端。
相应地，本发明一实施例还提供一种臂架防碰撞系统，该系统包括：位置确定装置，用于确定所述臂架的各个关节及臂架末端中的至少一者的位置；截面确定装置，用于确定与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面；以及控制器，用于计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离，并在该距离小于第一预定阈值的情况下，确定所述臂架存在碰撞风险。
可选的，在所述臂架的各个关节及臂架末端中的所述至少一者为关节的情况下，所述第一预定阈值不小于该关节的关节圆周半径尺寸与运动误差绝对值之和；在所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端的情况下，所述第一预定阈值不小于所述运动误差绝对值。
可选的，所述环境障碍物截面是通过用于生成有关所述臂架所处的环境的环境障碍物数据的环境障碍物模型生成的。
可选的，所述环境障碍物数据为三维点云数据或三角网格数据。
可选的，所述环境障碍物截面包含满足预定特征点密度的多个截面特征点。
可选的，所述计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离包括：计算所述臂架的所有关节及臂架末端与所述环境障碍物截面之间的最小距离。
可选的，所述环境障碍物截面为凹面。
可选的，所述臂架所处的环境为隧道，所述与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面为所述隧道在与所述臂架的各个关节及臂架末端中的所述至少一者在所述隧道轴向上的位置相对应的隧道截面。
可选的，该系统还包括：距离检测装置，用于检测所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离，所述控制器还用于在所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离与所述计算的最小距离之差大于第二预定阈值的情况下，判定所述关于所述臂架的碰撞风险的确定无效。
可选的，所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端。
相应地，本发明一实施例还提供一种工程机械，该工程机械包含上述臂架防碰撞系统。
通过上述技术方案，可预先或实时确定所述臂架所处环境的环境障碍物信息，根据该信息确定与臂架相关部分的位置相对应的环境障碍物截面，之后计算所述臂架相关部分的位置与所述环境障碍物截面上的各个点之间的最小距离，从而可将该臂架相关部分的位置与环境障碍物之间的最小距离的确定通过计算臂架相关部分的位置与所述环境障碍物截面上的各个点之间的距离而实现。通过将臂架的相关部分与环境障碍物的最小距离计算简化为点到点的距离(即，臂架相关部分的位置与所述环境障碍物截面上的各个点之间的距离)计算，提高了防碰撞的计算效率，使其可满足实时响应的需求。本发明的上述方案可适用于路径规划阶段(例如，在路径规划期间，对规划的每一条路径中进行躲避障碍物的规划，如果按照上述防碰撞方案确定该路径存在碰撞风险，可放弃该条规划路径)，也可适用于所述臂架实时施工期间(例如，在臂架施工期间，可实时实施上述防碰撞方案，确定碰撞风险)。
本发明实施例的其它特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
附图是用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明实施例，但并不构成对本发明实施例的限制。在附图中：
图1为臂架与环境障碍物的坐标系设置的示意图；
图2为本发明一实施例提供的臂架防碰撞系统的结构示意图；
图3为针对臂架各关节的坐标系示意图；
图4为臂架于隧道内的作业工况示意图；
图5为本发明另一实施例提供的臂架防碰撞系统的结构示意图；
图6为本发明一实施例提供的臂架防碰撞方法的流程图；
图7为本发明另一实施例提供的臂架防碰撞方法的流程图；以及
图8为本发明再一实施例提供的臂架防碰撞方法的处理流程。
附图标记说明
10位置确定装置20截面确定装置
30控制器40距离检测装置
具体实施方式
以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。
图1为臂架与环境障碍物的坐标系设置的示意图如图1所示，该臂架包含四个关节，且图中示出了针对每一关节的坐标系(例如，o1x1y1、o2x2y2、o3x3y3、o4x4y4)以及全局坐标系xyz，该全局坐标系xyz可适用于臂架及其所处的环境。为了实现坐标系的统一，可将每一关节在其关节坐标系下的位置转换为所述全局坐标系xyz下，以方便计算。该臂架可为各种工程机械的臂架，诸如混凝土泵车、隧道作业车等，该臂架需要作伸缩、回转、折叠等等操作，且其可极大得方便工程机械的作业。
图2为本发明一实施例提供的臂架防碰撞系统的结构示意图。如图2所示，本发明一实施例提供一种臂架防碰撞系统，该系统包括：位置确定装置10，用于确定所述臂架的各个关节及臂架末端中的至少一者的位置；截面确定装置20，用于确定与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面；以及控制器30，用于计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离，并在该距离小于第一预定阈值的情况下，确定所述臂架存在碰撞风险。在确定所述臂架存在碰撞风险之后，可进行碰撞报警和/或停止所述臂架运动，以避免发生碰撞。通过该方案，可将臂架的相关部分与环境障碍物的最小距离计算简化为点到点的距离计算，提高了防碰撞的计算效率，使其可满足实时响应的需求。
所述位置确定装置可安装于臂架的各个关节处，其可采集臂架各个关节的位置数据。当然，该位置数据可为各个关节在其自身坐标系下的位置数据。例如，如图3所示，各关节的坐标系的设定可基于d-h方法。臂架是由一系列的关节和连杆(即，臂节)组成的，这些关节是可以滑动的或旋转的。首先可给每一关节指定一个参考坐标系，然后确定从一个关节到下一个关节进行坐标变换。图3示出了三个顺序的关节和两个连杆。每一关节都是可以转动或平移的。第一关节为关节n，第二关节为关节n+1，第三关节为关机n+2。在这些关节之前或之后还可能具有其他关节。连杆也是如此，连杆n位于关节n与n+1之间，连杆n+1位于关节n+1与n+2之间。为了用d-h表示法对臂架进行建模，首先可为每一关节指定一个本地的参考坐标系。因此，对于每一关节，都必须指定z轴和x轴。关节是可旋转的，z轴位于按右手规旋转的方向，绕z轴的旋转角θ为关节变量。在图3中，θ角表示绕z轴的旋转角，表示z轴上的两条相邻共垂线之间的距离，α表示两个相邻的z轴之间的角度(也叫关节扭转)。需要说明的是，该图3旨在说明d-h方法的工作过程，其并不对本案造成限制，且可不必考虑如图3内所示坐标及参数标记与本案其他部分可能存在的冲突。
如图1所示，o1到o4为关节，且各关节为转动关节。o4处坐标系的位姿可以表示为
其中，θ1为关节oi绕其yi轴的旋转角，ai为关节oi的yi轴与其上一关节oi-1的yi-1轴之间的角度，di为关节oi与其上一关节oi-1之间的臂节的长度。
通过上述变换，可依据正运动学求解而将各个关节处所布设的位置确定装置所采集的位置数据转换为臂架的各个关节及其末端在图1所示的全局坐标系xyz下的位置坐标。当然，以上仅仅是获取臂架的各个关节及其末端的位置的一具体实施例，实际上任何获取臂架的各个关节及其末端的位置的方法均是可行的。另外，需要说明的是，上述位置确定装置可为安装于臂架的各个关节处的实体检测装置或者可为虚拟装置(例如，通过计算机程序来实现)，例如，该虚拟装置用于在臂架路径规划阶段确定臂架不同姿态下各个关节及臂架末端的位置数据。
在实施本发明的方案之前，通过各种建模技术来可对臂架所处的环境进行建模，以得出环境障碍物数据。该环境障碍物数据可为三维点云数据或三角网格数据，其可标识臂架所处的环境的情况，例如该环境内的那些空间坐标处是存在障碍物的(例如，隧道内壁等)，对于有障碍物的坐标点，可进行标示。
在获取臂架的各个关节及其末端的位置之后，可对于感兴趣位置(例如，某一关节或者臂架末端所处的位置)，确定该感兴趣位置所对应的环境障碍物截面。图4为臂架于隧道内的作业工况示意图。如图4所示，首先可获取该感兴趣位置在z轴上的位置，之后可在环境障碍物模型所建模的环境障碍物数据内获取该z轴上的该位置的环境障碍物截面，也就是隧道的截面(xy平面)。之后，可针对遍历该截面的每一特征点，确定每一特征点与所述感兴趣位置之间的距离，从而确定出所述感兴趣位置与所述环境障碍物截面之间的最小距离，即从遍历结果中取最小距离值。根据该最小距离，可判定臂架的感兴趣位置与环境障碍物截面内的特征点相碰撞的可能性，如果该最小距离非常小，例如低于某一阈值，则发生碰撞的可能性会很高，否则可视为发生碰撞的可能性很低。
所述阈值的设定可为：在所述臂架的各个关节及臂架末端中的所述至少一者为关节的情况下，所述第一预定阈值不小于该关节的关节圆周半径尺寸与运动误差绝对值之和；在所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端的情况下，所述第一预定阈值不小于所述运动误差绝对值。所述运动误差绝对值可考虑臂架摆动等因素来设置。一般来说，该阈值设置得越大，表明对碰撞的容忍度越高；该阈值设置得越小，表明对碰撞的容忍度越低。该阈值可根据实际需要来进行设置。
所述环境障碍物截面内的特征点的密度是可以根据需要来进行调整的，例如如果特征点密度过小，可能因导致该环境障碍物截面无法正式反映出环境障碍物情形，导致防碰撞效果不佳；如果特征点密度过大，又会导致在计算所述感兴趣位置与所述环境障碍物截面之间的最小距离时需遍历的特征点数目过于庞大，进而影响计算效率，使得本发明的防碰撞方案无法达到实时防碰撞的效果。为了增强计算效率，所述环境障碍物截面可仅包含满足预定特征点密度的截面特征点，从而既可以提高遍历速度，又能够满足防碰撞精度的要求。如果截面特征点密度过低，可采用多项式插值法来增大截面特征点的密度，所采用的阶数可视场景而定。另外，所述环境障碍物截面优选为凹面(例如，隧道的截面)，相比于凸面，在保证相同防碰撞效果的情况下，凹面所需的特征点密度可以更低一些，这是因为对于凸面的情况，如果特征点的密度不够高，很可能会忽略到一些可能距离臂架相关部位更近的特征点，而对这些特征点的忽略很可能会导致碰撞发生。
为了尽可能降低臂架碰撞方面，可获取臂架的所有关节及其末端的位置，之后针对其中的每一位置：确定与该位置相对应的环境障碍物截面，并计算该位置与所确定的环境障碍物截面之间的最小距离，并在该距离小于第一预定阈值的情况下，确定所述臂架存在碰撞风险。由此，通过考虑臂架的所有部位发生碰撞的可能性，可提升减小臂架发生碰撞的概率。
图5为本发明另一实施例提供的臂架防碰撞系统的结构示意图。如图5所示，为了检验上述防碰撞方案的可靠性，所述防碰撞系统还可包括：距离检测装置，用于检测所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离，其可为布置在所述臂架的各个关节及臂架末端中的所述至少一者(例如，臂架末端)上的距离传感器。如果所述臂架的各个关节及臂架末端中的所述至少一者(例如，臂架末端)与周围环境障碍物之间的实际最小距离与所述计算的最小距离之差大于第二预定阈值，则表明臂架所处的环境可能已经发生了变化(例如，臂架所处的实际环境可能已与建模时的环境不同)或者是距离检测装置存在故障(也就是说，上述根据位置数据及环境障碍物截面所计算的最小距离是准确的，但距离检测装置所检测的实际距离不准确)，当然也可能是因为其他原因造成的，例如也可能是安装于臂架的各个关节处的位置确定装置可存在故障、或者最小距离计算过程可能存在差错等等。无论是因为何种原因导致的差错，安全起见，所述控制器还用于在所述臂架的各个关节及臂架末端中的所述至少一者(例如，臂架末端)与周围环境障碍物之间的实际最小距离与所述计算的最小距离之差大于第二预定阈值的情况下，判定所述关于所述臂架的碰撞风险的确定无效。在排查差异过大的原因之后，所述控制器可增大所述第一预定阈值，以提高防碰撞容忍度；或者可增大所述第二预定阈值，并清除之前有关所述臂架的碰撞风险的确定无效的判定，提高防碰撞可靠性容忍度。
图6为本发明一实施例提供的臂架防碰撞方法的流程图。如图6所示，本发明一实施例提供一种臂架防碰撞方法，该方法包括：确定所述臂架的各个关节及臂架末端中的至少一者的位置；确定与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面；以及计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离，并在该距离小于第一预定阈值的情况下，确定所述臂架存在碰撞风险。
可选的，在所述臂架的各个关节及臂架末端中的所述至少一者为关节的情况下，所述第一预定阈值不小于该关节的关节圆周半径尺寸与运动误差绝对值之和；在所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端的情况下，所述第一预定阈值不小于所述运动误差绝对值。
可选的，所述环境障碍物截面是通过用于生成有关所述臂架所处的环境的环境障碍物数据的环境障碍物模型生成的。
可选的，所述环境障碍物数据为三维点云数据或三角网格数据。
可选的，所述计算所述臂架的各个关节及臂架末端中的所述至少一者的位置与所述环境障碍物截面之间的最小距离包括：计算所述臂架的所有关节及臂架末端与所述环境障碍物截面之间的最小距离。
可选的，所述环境障碍物截面为凹面。
可选的，所述环境障碍物截面包含满足预定特征点密度的多个截面特征点。
可选的，所述臂架所处的环境为隧道，所述与所述臂架的各个关节及臂架末端中的所述至少一者的位置相对应的环境障碍物截面为所述隧道在与所述臂架的各个关节及臂架末端中的所述至少一者在所述隧道轴向上的位置相对应的隧道截面。
图7为本发明另一实施例提供的臂架防碰撞方法的流程图。如图7所示，其与图6所示实施例的不同之处在于，其还包含以下步骤：确定所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离；在所述臂架的各个关节及臂架末端中的所述至少一者与周围环境障碍物之间的实际最小距离与所述计算的最小距离之差大于第二预定阈值的情况下，判定所述关于所述臂架的碰撞风险的确定无效。
可选的，所述臂架的各个关节及臂架末端中的所述至少一者为所述臂架末端。
有关上述参考图6和图7给出的本发明实施例提供的臂架防碰撞方法的具体细节及益处可参阅上述针对防碰撞系统的描述，于此不再赘述。需要说明的是，在能够实现本发明的目的的情况下，上述图6和图7所示的步骤的执行顺序是可以调整的，本发明并不限于上述图6和图7所示的步骤的特定执行顺序。
本发明的防碰撞方案可适用于实时运行过程中臂架与环境障碍物之间的防碰撞，还可适用于臂架运动路径规划过程。多关节类机械臂在多关节联动工作过程中，末端运动路径规划以及关节运动路径规划需要进行路径防碰撞甄选，利用本发明所提供的防碰撞方案对臂架的运行路径结果进行防碰撞甄选。图8为本发明再一实施例提供的臂架防碰撞方法的处理流程。如图8所示，路径规划器可根据设置在臂架各关节处的位置传感器所检测的位置数据进行路径规划，该路径规划器可输出各关节的关节坐标系原点在全局坐标系下的坐标。最小距离求解器可根据臂架的感兴趣部位(例如，某一关节、臂架末端、某些关节、或者所有关节及臂架末端等)在全局坐标系下的坐标确定与该坐标相对应的环境障碍物截面，并计算感兴趣部位与所述环境障碍物截面之间的最小距离，并根据该最小距离确定臂架的所述感兴趣部位的碰撞风险。
本发明针对路径规划以及实时运行过程中，碰撞检测对系统实时性影响较大的现实，通过将防碰撞检测过程简化为点到点的最小距离计算，提高了计算效率，降低了系统硬件性能的要求。同时，引入了计算可靠性的校核，保证了防碰撞计算的有效性。
以上结合附图详细描述了本发明实施例的可选实施方式，但是，本发明实施例并不限于上述实施方式中的具体细节，在本发明实施例的技术构思范围内，可以对本发明实施例的技术方案进行多种简单变型，这些简单变型均属于本发明实施例的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施例对各种可能的组合方式不再另行说明。
本领域技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。
- 还没有人留言评论。精彩留言会获得点赞！