CN116968040A - 用于血管介入机器人的防碰撞系统 - Google Patents

Abstract

本发明提供了一种用于血管介入机器人的防碰撞系统，属于安全控制领域。其中方法包括：建立机器人的模型，通过机器人手臂模型中电机传出的位置信息，速度信息，加速度等信息，从而计算出机器人的手臂各个关节及执行手上的耗材的位置。结合操作前设定的病人位置，计算出系统中可能碰撞点的距离。并根据各个位置、距离、命令来判断各个部分是否会发生碰撞，在发生碰撞前对机械臂进行停止控制，从而保护了病人，机械手臂，执行手的耗材。本系统的技术方案，最大限度的保护了各方，并且成本较低。

Description

用于血管介入机器人的防碰撞系统

技术领域

本发明涉及血管介入手术机器人控制的技术领域，具体地，涉及用于血管介入机器人的防碰撞系统。

背景技术

血管介入机器人是针对需要进行血管介入疾病而需要的一类机器人。血管介入手术机器人是可以进行输送导丝、导管、支架的一类机器人。同时还可以释放支架。因为在手术过程中，医生需要更换导丝，移动手臂，释放支架等动作会移动手臂。因此为防止医生的误操作，保护病人和机器人，以及执行手的手术耗材，从而建立了血管介入机器人防碰撞系统。防碰撞是指血管介入手术机器人对病人的防碰撞，手术机器人手臂之间的防碰撞，手术机器人肩，肘，腕，执行手的防碰撞。血管介入手术机器人的防碰撞算法，需要保证血管介入手术机器人在工作时，既要防止机器人对于病人的伤害，又要保证机器人手臂之间的相互碰撞。从而保证机器人的手臂，执行手上的耗材的不被碰撞损坏。

在公开号为CN111449752A的专利文献中公开了一种用于血管腔内介入手术机器人推进机构的被动定位机械臂，所述机械臂安装在手术台上；该机械臂包括：基座模块、水平移动模块、升降模块、第一旋转关节、第二旋转关节及俯仰角调节模块；所述基座模块安装在手术台上，所述水平移动模块安装在基座模块上，所述升降模块的底部安装在水平移动模块上，第一旋转关节的一端安装升降模块的顶部，另一端与第二旋转关节的一端连接；所述俯仰角调节模块安装在第二旋转关节的另一端；所述手术机器人安装在俯仰角调节模块上。

目前的介入手术机器人都采用滚轮式的盒装结构，没有手臂的结构。因此手臂结构在血管介入机器人中还属于首次。碰撞算法的理论与规则也在探讨之中。因此血管介入手术机器人的防碰撞算法，有研究和实际的使用价值。

因此，需要提出一种新的技术方案以改善上述技术问题。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种用于血管介入机器人的防碰撞系统。

根据本发明提供的一种用于血管介入机器人的防碰撞系统，包括数据处理模块、碰撞问询模块、碰撞计算模块、配置模块和血管介入机器人手臂控制模块；

所述数据处理模块收集从血管介入机器人中电机的脉冲位置、脉冲速度和脉冲加速度；

所述碰撞问询模块在给机器人发送一切运动命令前，询问需要询问的模块；

所述碰撞计算模块对血管介入机器人在各个运动状态下进行计算碰撞；

所述配置模块配置血管介入机器人各电机的脉冲归零位置；

所述血管介入机器人手臂控制模块对机器人手臂的进行整体控制，以及在碰撞模块计算出相关位置有碰撞的时候发送相应的停止命令。

优选地，所述数据处理模块将电机的脉冲位置、脉冲速度和脉冲加速度结合机器人的模型，计算出血管介入机器人在世界坐标系中对应的位置、速度和加速度，计算出机器人末端及发生碰撞的各个部分在世界坐标系中的位姿，显示当前执行手与水平方向血管的夹角。

优选地，所述数据处理模块采集血管介入机器人的各个电机的脉冲数据，根据配置模块配置的电机零点位置，计算机器人各个部件的点的位置；计算出各个点的位姿信息，得到机器人各点在笛卡尔坐标系下各点的位姿。

优选地，所述碰撞问询模块根据血管介入机器人各个部位的位姿，并根据当前传入的运动命令，判断将要运动的方向是否发生碰撞，如果发生碰撞，将返回碰撞状态，碰撞的模块闪烁。

优选地，所述碰撞问询模块根据机器人的命令和当前的状态，判断当前指令是否发生碰撞；当判断发生碰撞后，系统使碰撞的位置的仿真部位闪烁；当判断不发生碰撞时，机器人根据命令，调用轨迹规划的模块，进行轨迹规划，调用电机，完成相关的动作。

优选地，所述碰撞计算模块根据配置的安全距离以及数据处理模块中的点的位姿，计算各个部分的距离；计算点到点的距离、点到一个机械平面的距离和点到非规则平面的距离的处理，根据这些距离和机器人的命令判断各位置是否碰撞，当有距离达到配置的距离后，碰撞计算模块向控制模块发送停止命令，停止导致碰撞的命令；当再次重复碰撞的命令时，碰撞问询模块发现命令，计算出碰撞的结果，最终命令无法运行。

优选地，碰撞的距离能够配置；当碰撞计算模块检测有相关逻辑点达到安全距离，发送相应命令停止的命令。

优选地，所述防碰撞系统还包括对病人保护的配置的动态设置，根据病身材的不同设置不同的安全保护距离。

优选地，所述防碰撞系统还包括碰撞系统对血管介入机器人有允许命令运行和停止命令两种状态。

优选地，所述血管介入机器人手臂控制模块是手臂控制的状态机，状态机特征包括静止状态、准备状态、处理状态、运动状态和停止状态。

与现有技术相比，本发明具有如下的有益效果：

1、本发明的技术方案，最大限度的保护了各方，并且成本较低；

2、该发明可以有效的防止机器人手臂、执行手、基座之间的碰撞，同时可以保护机器人对病人的碰撞。

3、本发明的系统易测试，每个功能单独传入相关参数，能够很好的进行单元测试；且易调试，模块化后，每个模块都有自己的功能所属，可以更好的调试，找到问题所在。

4、本发明的系统模块易配置，有单独的配置模块，可以适应机器人狭小空间或特殊需要的配置工作；且易复用，模块化后，部分功能可以重复在即将开发的功能中复用。

5、本发明的系统易理解，整个过程的逻辑更加清晰，各模块的职能更加明确；且计算过程更少，提供高了计算的效率，降低了CPU的负载；可以更好的完成了碰撞的需求，可以防止以前无法阻止的碰撞。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明实例中血管介入机器人的模型、杆零位的示意图；

图2位本发明血管介入机器人防碰撞系统的示例流程图；

图3为本发明中手臂控制模块的状态机示意图。

其中：

配置模块100 数据处理模块103

碰撞问询模块101 碰撞计算模块104

手臂控制模块102 其他步骤模块105

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1：

本发明提供一种用于血管介入机器人的防碰撞系统，包括数据处理模块103、碰撞问询模块101、碰撞计算模块104、配置模块100和血管介入机器人手臂控制模块102。

数据处理模块103收集从血管介入机器人中电机的脉冲位置、脉冲速度和脉冲加速度；数据处理模块103将电机的脉冲位置、脉冲速度和脉冲加速度结合机器人的模型，计算出血管介入机器人在世界坐标系中对应的位置、速度和加速度，计算出机器人末端及发生碰撞的各个部分在世界坐标系中的位姿，显示当前执行手与水平方向血管的夹角。数据处理模块103采集血管介入机器人的各个电机的脉冲数据，根据配置模块100配置的电机零点位置，计算机器人各个部件的点的位置；计算出各个点的位姿信息，得到机器人各点在笛卡尔坐标系下各点的位姿。

碰撞问询模块101在给机器人发送一切运动命令前，询问需要询问的模块；碰撞问询模块101根据血管介入机器人各个部位的位姿，并根据当前传入的运动命令，判断将要运动的方向是否发生碰撞，如果发生碰撞，将返回碰撞状态，碰撞的模块闪烁。碰撞问询模块101根据机器人的命令和当前的状态，判断当前指令是否发生碰撞；当判断发生碰撞后，系统使碰撞的位置的仿真部位闪烁；当判断不发生碰撞时，机器人根据命令，调用轨迹规划的模块，进行轨迹规划，调用电机，完成相关的动作。

碰撞计算模块104对血管介入机器人在各个运动状态下进行计算碰撞；碰撞计算模块104根据配置的安全距离以及数据处理模块103中的点的位姿，计算各个部分的距离；计算点到点的距离、点到一个机械平面的距离和点到非规则平面的距离的处理，根据这些距离和机器人的命令判断各位置是否碰撞，当有距离达到配置的距离后，碰撞计算模块104向控制模块发送停止命令，停止导致碰撞的命令；当再次重复碰撞的命令时，碰撞问询模块101发现命令，计算出碰撞的结果，最终命令无法运行。碰撞的距离能够配置；当碰撞计算模块104检测有相关逻辑点达到安全距离，发送相应命令停止的命令。

配置模块100配置血管介入机器人各电机的脉冲归零位置；血管介入机器人手臂控制模块102对机器人手臂的进行整体控制，以及在碰撞模块计算出相关位置有碰撞的时候发送相应的停止命令。血管介入机器人手臂控制模块102是手臂控制的状态机，状态机特征包括静止状态、准备状态、处理状态、运动状态和停止状态。

防碰撞系统还包括对病人保护的配置的动态设置，根据病身材的不同设置不同的安全保护距离。防碰撞系统还包括碰撞系统对血管介入机器人有允许命令运行和停止命令两种状态。

实施例2：

实施例2为实施例1的优选例，以更为具体地对本发明进行说明。

血管介入机器人的防碰撞系统。该系统是在血管介入机器人在正常工作过程中，包括机器人手臂的运动，执行手的运动。包括：

数据处理模块103，用于收集从血管介入机器人中电机的脉冲位置，脉冲速度，脉冲加速度。然后将电机的脉冲位置，脉冲速度，脉冲加速度，结合机器人人的模型，计算出血管介入机器人在世界坐标系中对应的位置，速度，加速度。从而进一步计算出机器人末端及可能碰撞的各个部分在世界坐标系中的位姿。显示出当前执行手与水平方向血管的夹角，利于医生的判断。

碰撞问询模块101，用于在给机器人发送一切运动命令前，需要询问的模块。问询模块后，问询模块会根据血管介入机器人各个部位的位姿，并根据当前传入的运动命令，判断将要运动的方向是否会发生碰撞。如果会发生碰撞，将会返回碰撞状态，使会碰撞的模块闪烁。

碰撞计算模块104，用于对血管介入机器人在各个运动状态下计算碰撞。碰撞计算模块104将数据处理模块103中各个点位置在世界坐标系的位姿，抽提出结合血管介入机器人平面可能碰撞的点和线。将可能碰撞的点和线组合起来，组合成可以判断血管介入机器人可能碰撞的位置。比如，执行手2与执行手3选择了在模型中2手的机械结构中的3个点，同样3手也选择了3手机械结构中的3个点。然后计算出2,3手一组判断碰撞的条件，从判断出执行手2,3是否碰撞。然后根据碰撞计算模块104计算的结果发送相应的命令。

配置模块100，用于配置血管介入机器人各电机的脉冲归零位置。配置血管介入机器人的肩，肘，腕的零点位置。根据配置模块100的零点位置，数据处理模块103才能根据零点位置，计算出当前状态下，血管介入机器人各点在世界坐标系中的位姿，从而给后面的判断提供基础数据。碰撞距离的配置。碰撞计算模块104中的碰撞距离是根据配置模块100中的距离进行判断。同时也配置了默认保护病人的最低位置。

血管介入机器人手臂控制模块102，用于机器人手臂的整体控制，以及在碰撞模块计算出相关位置有碰撞的时候发送相应的停止命令，从而实现了血管介入机器人防碰撞，保护了病人，也保护了血管介入机器人及相关的耗材。手臂控制模块102同时控制机器人手臂的同轴运动，轴向运动，垂向运动，角顺运动，角逆运动。

数据处理模块103将血管介入机器人的各个电机的脉冲数据采集出来，根据配置模块100配置的电机零点位置，也就是各杆件的水平位置为偏移位置，计算机器人各个部件的点的位置。并进一步计算出各个点的位姿信息，最终得到机器人各点在笛卡尔坐标系下各点的位姿。数据处理模块103中选择的点是后期在碰撞问询与碰撞检测中需要计算的点。而这些点的选取是通过实践获得的。这些点包括各个关节点，执行手的机箱的点。机箱不同平面的点。从而构成防碰撞系统计算的基础点的位姿。在血管介入机器人运动的状态下，还会计算机器在相应命令下运动的方向，速度，加速度信息，成为防碰撞计算的数据基础。

血管介入机器人防碰撞方法，先碰撞问询，碰撞问询给出不碰撞的命令后，机器人才能继续运行。对于碰撞问询模块101。根据机器人的命令和当前的状态，判断当前指令是否会发生碰撞。当判断会发送碰撞后，系统使碰撞的位置的仿真部位闪烁。当判断不会发生碰撞时，机器人会根据命令，调用轨迹规划的模块，进行轨迹规划，然后调用电机，完成相关的动作。

防碰撞算方法，机器人在正常工作情况下全部的碰撞保护。碰撞计算模块104会根据配置的安全距离以及数据处理模块103中的点的位姿，计算各个部分的距离。具体的方案包括将计算点到点的距离。点到一个机械平面的距离。点到非规则平面的距离的处理。然后根据这些距离和机器人的命令判断各各位置会不会碰撞。当有距离达到配置的距离后，碰撞计算模块104就会向控制模块发送停止命令，从而导致碰撞的命令停止。这在世界当再次重复碰撞的命令时，会被碰撞问询模块101发现，从而计算出碰撞的结果，最终使命令无法运行。

防碰撞系统的碰撞的距离可配置。当碰撞计算模块104检测有相关逻辑点达到安全距离后，会发送相应命令停止的命令。从而保护机器人手臂，病人，医疗耗材。

防碰撞系统还包括对病人保护的配置的动态设置。根据病身材的不同设置不同的安全保护距离。

防碰撞系统还包括碰撞系统对血管介入机器人只有允许命令运行或者停止命令两种状态。

手臂控制模块102是手臂控制的状态机。状态机特征包括静止状态，准备状态，处理状态，运动状态，停止状态。

整个发明的防碰撞系统的工作过程如下：首先是配置模块工作，配置各个碰撞点设置的安全距离，配置每个电机的活动范围，配置机器人与人体或床的安全距离。程序配置好后，整个机器人系统可以进入正常的工作状态。在正常工作的状态下，数据处理模块一直运行，提供所有电机的当前状态，包括电机的状态、速度、加速度、脉冲值等电机所有的信息。然后医生可以操作软件界面或者控制台的按钮操作血管介入机器人系统。当医生操作机器人使机器人手臂运动时，首先通过碰撞问询模块，问询是否有碰撞。当没有碰撞时，会进入血管介入机器人手臂控制模块，机器人手臂控制模块会发送相关的控制命令给运动所需的电机，使电机运动，从而使血管介入机器人按照规划的路径和姿态运动。当机器人在运动状态的过程中，碰撞计算模块会一直运行。碰撞计算模块根据数据处理模块、手臂控制模块的命令、轨迹规划的数据计算当前血管介入机器人各部分位姿是否在配置模块配置的安全距离内。当碰撞计算模块计算出血管介入机器人有任何部位超出配置模块的安全距离时，就会判读出碰撞，从而通过血管介入机器人手臂控制模块发送相关位置的停止命令。此时血管介入机器人手臂运动停止，碰撞计算模块停止工作。当想要血管介入机器人重新进入运动状态时，需要按照刚刚所述逻辑重新进入本发明的防碰撞系统。

鉴于血管介入手术机器人的情况，发明了一种血管介入手术机器人的防碰撞系统。本发明提供了血管介入机器人的防碰撞实践与理论。首先是机器人的建模过程。机器人是3轴手臂加了一个移动机构是一个RRRP机器人。为了简化机器人的模型，将机器人简化为RRR以及另外的移动机构。这样血管介入机器人就变成了一个标准的3R手臂结构。根据血管介入手术的特殊用途，设置机器人的肩关节的零点，轴关节的零点，腕关节的零点都是采用水平仪在特定位置选取，从而方便后面的计算和理解。总体的放置图，如图1所示，机器人建模后，零点位置，保证AB杆，BC杆，CD杆与x轴在一条直线时为电机的脉冲归零位置。也是计算电机角度的0度位置。以逆时针方向为正。此时建立机器人的模型，找到了机器人旋转电机各轴的零点位置。血管介入手术机器人有两个手臂，4只手。另外一个手臂和手的建模方式与此相同。因为手臂，手只在x，y平面内运动，因此暂时不考虑z轴的方向问题。因此此种建模方式可以极大的简化问题，使之更好理解。机器人的建模过程。

首先是配置模块100。配置模块100用于配置血管介入手术机器人各个碰撞点之间的最小距离。配置机器人手臂零度时的电机的脉冲位置。配置机器人与病人的默认位置。

数据处理模块103，用于接收电机的脉冲信息，速度信息，加速度信息。数据处理模块103根据配置信息，将电机的脉冲信息转换成距离信息或者角度信息。再根据模型计算出可能碰撞的点在世界坐标系下的位姿信息。数据处理模块103一直处理得到机器人手臂末端的位姿信息。

数据处理模块103还包括对一些异常的数据，进行紧急的处理，让机器人的手臂保持暂时不能运动的状态。

碰撞问询模块101，用于对机器人的命令进行运动前的判断与控制。当碰撞问询的结果为无碰撞的时候，机器人的命令才能发到电机的执行部件，机器人从而运动。当碰撞问询的结构为碰撞的时候，机器人的控制命令无法下发，同时将机器人将要碰撞的部位进行闪烁表示。

碰撞计算模块104，用于将数据处理好后的关键点的位姿信息进行计算。计算各个关键点之间的距离。同时计算关键点到某点形成的直线的距离。计算当前执行手的最低距离。将距离计算好后，通过将相关点再次进行相关距离计算，保证相关的位置不会碰撞。

碰撞计算模块104包括计算关键点与关键点的距离。也包括计算关键点到相关直线的距离。还包括关键点到相关非常规平面下，取出特征点能代表非常规面的点的距离。还包括计算执行手末端达到离病人最近的位置。这些距离都会配置好相应的安全距离。

碰撞计算还包括，将某几个距离判断的组合，形成血管介入机器人某两个位置碰撞的判断标准。

碰撞计算还包括当判断出碰撞状态后，还需要判断当前的机器人的命令是否会解开相应的碰撞，如果可以解开相应的碰撞。碰撞计算模块104会让此命令执行。

碰撞计算还包括针对马上的碰撞，向机器人手臂发送停止命令，从而让机器人在安全的状态停止，保证机器人和病人的防碰撞。在判断不会碰撞时，碰撞计算模块104不会给机器人任何指令。从而保证防碰撞系统不会影响血管介入机器人的正常工作。增强系统的稳定性。

手臂的控制模块是手臂运动状态控制的状态机。手臂的状态跟随着命令的不同，会在不同的状态，控制手臂进行不同的运动。其中运动状态包括轴向运动，垂向运动，水平运动，执行手的运动。手臂的控制模块，建立了一个手臂运动的状态机。状态机的运行方式见图3。每次手臂状态分为静止状态，准备状态，处理状态，运动状态，停止状态。当手臂运动时，主线程发送给手臂运动状态的指令。手臂状态机接到指令后会进行相应的准备。一边计算数据，计算防碰撞，一边发送相关的数据。当数据计算出碰撞后，会发送停止状态命令，手臂从而进入相关的状态机运作停止。此种方式将多种不同的运动方式以状态机的形式进行控制，更好，更明确的控制手臂的运动状态。

本发明中机器人模型的建立是根据DH建模方法。

具体的距离计算公式包括两点距离公式：

点到直线的距离公式计算公式：

其中设两个点A、B以及坐标分别为A(x1,y1),B(x2,y2)，则A和B两点之间的距离为：

其中设直线L的方程为Ax+By+C＝0，点P的坐标为(x0,y0)，则点P到直线L的距离为：

当血管介入机器人在工作状态时，医生可能操作执行手的运动，或者手臂的运动。此时通过计算简易模型的各点的距离，点到部分直线的距离，点到部分点群的距离，最后通过逻辑的推导，判断某两个部分是否会碰撞。从而实现血管介入机器人的防碰撞，保护了机器人的手臂，病人，医疗耗材。

为使本发明更加清楚，以下举实例对本发明进一步详细的说明。

图1为本发明实施实例中血管介入机器人防碰撞的示例性结构图。本示例中，其中一个机械臂的运动，由安装的驱动电机和驱动器控制图中电机与驱动器在圆形之中未标出。执行手的端还有直线电机和驱动器。每个电机的编码器的数值可以通过协议传递给防碰撞系统。整个防碰撞系统包括配置模块100、碰撞问询模块101、手臂控制模块102、数据处理模块103、防碰撞计算模块104。

图1是简易的血管介入机器人模型。从图中可以看出机器人有两个手臂。机器人的标注点位与其他部分都有可能碰撞。就以简易图中AB杆与基座J点的碰撞为例，结合流程图2和手臂控制图3说明防碰撞系统的一个碰撞的过程。

首先是系统的启动，系统启动会进行相关的自检程序。检测血管介入机器人是否可以达到能够正常工作的状态即步骤106。

然后系统进入配置模块100将事先配置的安全距离的标准读入到程序中。同时进行机械臂的操作，确定包括病人的最低位置和最靠近头的位置。这样就配置了保护病人的标准。

然后系统就等待机器人命令的输入。机器人的命令包括手臂的命令和手的命令。手臂的命令包括水平运动，垂直运动等。手的运动包括前进，后退，旋转，加紧等。当机器人有命令输入后，就会进入碰撞问询模块101。碰撞问询模块101会判断当前命令是否会导致碰撞，如果碰撞就会进入碰撞显示模块105，显示碰撞的机构。如果不碰撞就会运行当前的命令。以AB杆与J点的碰撞来说。当给予机器人1臂垂直往下的命令时，AB杆会靠近J点。当系统判断AB杆会与J点碰撞时，会给手臂碰撞模块发送停止命令。这样手臂就会停止。当再次想让机器人1臂垂向向下运动时，碰撞检测模块就会检测出当前的命令会导致碰撞，从而显示碰撞的模块，并且不执行向手臂控制模块102发送相应控制的命令。当比如要使机器人手臂1水平向x轴负方向运动时，因为该命令不会导致碰撞，因此会将此命令继续传递给手臂控制模块102，从而让机器人手臂完成相应的动作。这就是碰撞问询的功能，让碰撞的命令无法执行，让不会碰撞的命令继续执行。

然后当碰撞问询模块101判断不会碰撞时，会将控制命令下发手臂控制模块102。手臂控制模块102会根据图3手臂状态机的运行原理，经过轨迹规划和插补算法将相关的路径信息，脉冲信息发送给电机控制器，使电机进行相应的运动，从而让机器人手臂按照规划的轨迹进行运动。同时启动了数据处理模块103。

数据处理模块103102，根据机器人模型和传入的电机信息会计算出图1中各个点的位姿信息。同时会组合出非常规形状的特殊点。计算点与点的距离，点与直线的距离。计算出相关点的速度信息，加速度信息，方向信息。

然后就进入碰撞计算模块104103。碰撞计算模块104会根据配置信息，数据处理中的信息，机器人的命令信息，再进行相关的逻辑运算，从而计算出当前各个碰撞点是否会发生碰撞。以AB杆与J点的碰撞为例。AB杆的运动只与A肩关节的电机运动有关。因此在配置模块100中，配置肩关节电机的可以运行的范围。同时血管介入机器人1臂的垂向向下运动，水平向左运动，轴向向左运动等可以让AB杆运动的机器人规划运动都可能导致AB杆与J点的碰撞。碰撞计算模块104会先计算轨迹的碰撞点，从而让机器人手臂在规划可能碰撞前的点停止。同时碰撞计算模块104还根据肩关节的范围，AB杆与J点的安全距离判断是否碰撞，当达到安全距离后，就会判断出碰撞。然后结合状态机向手臂控制模块102发送停止命令。然后血管介入机器人相应的运动停止，显示相应碰撞机构。当发送相同命令，或者可能导致AB杆与J点碰撞的命令后，碰撞问询模块101会直接拒绝下放机器人的控制命令，同时显示当前的碰撞机构。当碰撞计算模块104计算出没有碰撞时，会让碰撞计算的线程等待10ms,然后重新进入碰撞计算模块104103进行判断。

这样就可以保证血管介入机器人的AB杆在任何机器人的命令下都可以保证不与J点碰撞。同样样的方法应用于其他关键点可能碰撞的地方，从而实现了血管介入机器人的防碰撞系统，保证病人，机器人，耗材的安全性。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种用于血管介入机器人的防碰撞系统，其特征在于，包括数据处理模块(103)、碰撞问询模块(101)、碰撞计算模块(104)、配置模块(100)和血管介入机器人手臂控制模块(102)；

所述数据处理模块(103)收集从血管介入机器人中电机的脉冲位置、脉冲速度和脉冲加速度；

所述碰撞问询模块(101)在给机器人发送一切运动命令前，询问需要询问的模块；

所述碰撞计算模块(104)对血管介入机器人在各个运动状态下进行计算碰撞；

所述配置模块(100)配置血管介入机器人各电机的脉冲归零位置；

所述血管介入机器人手臂控制模块(102)对机器人手臂的进行整体控制，以及在碰撞模块计算出相关位置有碰撞的时候发送相应的停止命令。

2.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述数据处理模块(103)将电机的脉冲位置、脉冲速度和脉冲加速度结合机器人的模型，计算出血管介入机器人在世界坐标系中对应的位置、速度和加速度，计算出机器人末端及发生碰撞的各个部分在世界坐标系中的位姿，显示当前执行手与水平方向血管的夹角。

3.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述数据处理模块(103)采集血管介入机器人的各个电机的脉冲数据，根据配置模块(100)配置的电机零点位置，计算机器人各个部件的点的位置；计算出各个点的位姿信息，得到机器人各点在笛卡尔坐标系下各点的位姿。

4.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述碰撞问询模块(101)根据血管介入机器人各个部位的位姿，并根据当前传入的运动命令，判断将要运动的方向是否发生碰撞，如果发生碰撞，将返回碰撞状态，碰撞的模块闪烁。

5.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述碰撞问询模块(101)根据机器人的命令和当前的状态，判断当前指令是否发生碰撞；当判断发生碰撞后，系统使碰撞的位置的仿真部位闪烁；当判断不发生碰撞时，机器人根据命令，调用轨迹规划的模块，进行轨迹规划，调用电机，完成相关的动作。

6.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述碰撞计算模块(104)根据配置的安全距离以及数据处理模块(103)中的点的位姿，计算各个部分的距离；计算点到点的距离、点到一个机械平面的距离和点到非规则平面的距离的处理，根据这些距离和机器人的命令判断各位置是否碰撞，当有距离达到配置的距离后，碰撞计算模块(104)向控制模块发送停止命令，停止导致碰撞的命令；当再次重复碰撞的命令时，碰撞问询模块(101)发现命令，计算出碰撞的结果，最终命令无法运行。

7.根据权利要求6所述的用于血管介入机器人的防碰撞系统，其特征在于，碰撞的距离能够配置；当碰撞计算模块(104)检测有相关逻辑点达到安全距离，发送相应命令停止的命令。

8.根据权利要求6所述的用于血管介入机器人的防碰撞系统，其特征在于，所述防碰撞系统还包括对病人保护的配置的动态设置，根据病身材的不同设置不同的安全保护距离。

9.根据权利要求6所述的用于血管介入机器人的防碰撞系统，其特征在于，所述防碰撞系统还包括碰撞系统对血管介入机器人有允许命令运行和停止命令两种状态。

10.根据权利要求1所述的用于血管介入机器人的防碰撞系统，其特征在于，所述血管介入机器人手臂控制模块(102)是手臂控制的状态机，状态机特征包括静止状态、准备状态、处理状态、运动状态和停止状态。