用于摄像机器人的碰撞检测系统及方法与流程

本发明涉及一种碰撞检测系统及方法，特别是涉及一种用于摄像机器人的碰撞检测系统及方法。

背景技术：

随着现代工业的发展，机器人已越来越多的被用来代替人从事复杂、烦琐的工作，它们不仅减轻了人的劳动强度，还大大提高了工作效率。运用于影视拍摄中的摄像机器人，在机器人末端安装工作平台来承载摄像机，通过控制系统完成对机器人运动轨迹的规划和运行，完成不同镜头的拍摄。由于拍摄过程中场景的复杂性以及人员流动性大等因素，因此对机器人运行安全性要求就十分严格。

机器人防碰撞是机器人技术中最基础也是最关键的一项技术，它目的在于使机器人运动过程中保证不发生碰撞，避免结构的过度老化、脱落，防止安全事故的产生，机器人防碰撞技术的核心包括了传感器的选择和规划算法的选择。比较常用的防碰撞传感器是基于力学原理，当机器人与物体发生碰撞后，相互间产生的接触应力通过相关硬件设备转换发出电信号，使机器人停止工作，从而对机器人起到保护作用。摄像机器人由于使用环境中存在场景复杂性高和人员流动性大的特点，对摄像机器人的安全性要求极高。一般的基于力学的碰撞检测传感器工作的前提都是机器人已经与物体发生接触，但是这就会对机器人造成一定的损伤，不符合摄像机器人的工作要求。而基于视觉或者激光测距的防碰撞方法对于多轴机器人，尤其是对于较为常见的如七轴机器人的机械臂自身防碰撞检测仍然存在一定的局限性。

技术实现要素：

发明目的：本发明要解决的技术问题是提供一种用于摄像机器人的碰撞检测系统及方法，解决了目前的防碰撞技术不适用于摄像机器人的缺陷，通过在机器人上的关键节点处安装传感器构建了一套碰撞检测系统，并通过传感器反馈的实时数据建立了一套运动规划方法，实现了摄像机器人的全方位碰撞检测功能。

技术方案：本发明所述的用于摄像机器人的碰撞检测系统，与摄像机器人配合使用，系统包括传感器模块和服务控制模块，服务控制模块分别与传感器模块和摄像机器人建立通信连接；

所述的传感器模块设置在摄像机器人上，用于检测摄像机器人距离障碍物的距离，形成传感器数据并发送给服务控制模块；

所述的服务控制模块，用于接收传感器模块发送的传感器数据和摄像机器人发送的机器人数据，根据所收到的数据形成机器人控制指令，并将控制指令发送给摄像机器人。

进一步的，所述的传感器模块包括若干组传感器和传感器数据收发装置，若干组传感器将各自检测数据发送给传感器数据收发装置，传感器数据收发装置所收到的数据处理打包后发送给服务控制模块。

进一步的，所述的若干组传感器为超声波传感器。

进一步的，所述的摄像机器人为七轴机器人，传感器为12组，1号传感器安装在摄像机的镜头上方，探测方向为沿镜头方向向前，用于探测镜头前方距离障碍物的距离，2号传感器安装在摄像机的镜头下方，探测方向为垂直镜头方向向下，用于探测镜头下方距离障碍物的距离，3号传感器、4号传感器分别安装在摄像机两侧，用于探测摄像机两侧距离障碍物的距离，5号传感器安装在旋转腕部和摆动腕部之间腕部中点的下端，探测方向向下，用于探测机械臂腕部距离下方障碍物的距离，6号传感器安装在上臂关节和旋转腕部之间上臂中点的下方，探测方向向下，用于探测机械臂上臂距离下方障碍物的距离，7号传感器、8号传感器安装在上臂关节和旋转腕部之间上臂中点的两侧，用于探测机械臂上臂距离两侧障碍物的距离，9号传感器、10号传感器安装在上臂关节的两侧，用于探测机械臂上臂关节两侧障碍物的距离，11号传感器、12号传感器安装在下臂关节沿轨道轴方向的两侧，探测方向为沿轨道运行方向，用于探测机械臂沿轨道轴运行过程中轨道上障碍物的距离。

进一步的，所述的服务控制模块还包括传感器参数配置保存装置，用于设置和保存与传感器有关的参数到本地数据库，方便下次控制系统使用时直接从本地读取之前用户的设置，所述的传感器参数包括传感器响应距离参数、使用/禁用状态、传感器模块通讯ip号和端口号。

进一步的，所述的服务控制模块还包括异常处理装置，所述异常处理装置将当前接收到的传感器数据挂起，然后将其与前后接收的传感器数据进行对比，如果当前传感器数据存在跳变，则判定为异常数据，放弃该数据，如果不存在跳变，服务控制模块进行正常的处理。

本发明所述的用于摄像机器人的碰撞检测方法，所述方法包括：

传感器通过发射和接收的信号，计算出传感器距离障碍物的距离，并将其作为传感器数据发送给服务器；

服务器根据接收到的传感器数据，处理并计算出机器人控制指令；

服务器将机器人控制指令发送给摄像机器人。

进一步的，所述计算距离障碍物的距离的方法为：所述的传感器设置为超声波传感器，超声波传感器发射超声波的同时设置计数器开始计时，当传感器收到反射波停止计时，记录的时间为t,传播速度v＝340m/s，根据s＝v*t/2计算出该传感器距障碍物面的距离s。

进一步的，所述的计算机器人控制指令的方法为：

(1)根据传感器的配置参数定义数据响应区间{0～s0}、{s0～s1}、{s1～s2}、{s2～+∞}，0＜s0＜s1＜s2，上述四个区间分别对应四种不同的机器人状态：紧急制动，缓停，减速，正常；

(2)计算机器人的关节速度，具体方程如下：

其中s为传感器距离障碍物的距离，vel为机器人各关节原先运行的关节速度，vel′为根据传感器数据处理后的机器人各关节运行的关节速度，t为插值采样系数，τ为减速系数；

(3)根据服务器与摄像机器人之间的通讯协议，将vel′打包成机器人控制指令。

有益效果：本发明能够基于所设计的碰撞检测系统和方法，通过测量关键节点与目标物体的距离结合运动规划算法实现防碰撞功能，实现了一套碰撞检测运动规划方案。与现有碰撞检测方案相比，在保证摄像机器人安全性的同时也有效提高了检测的灵活性。本发明可以应用于影视拍摄现场，为机器人操作人员提供一套灵活有效的机器人碰撞检测方案。

附图说明

图1是本系统的结构图；

图2是传感器模块安装示意图。

具体实施方式

本实施例的系统架构如图1所示，系统包括传感器模块和服务控制模块，与摄像机器人配合使用。摄像机器人为七轴机器人，如图2所示，由基座22、下臂关节23、上臂关节24、上臂25、旋转腕部26、摆动腕部27、腕部28和云台29组成，摄像机30设置在云台29上，基座22可以在轨道轴21上前后运动。外部电脑等设备可以通过摄像机器人所带的控制接口，通过固定格式的控制指令来对摄像机器人进行运动控制。

所述的传感器模块设置在摄像机器人上，单独供电和传输数据，用于检测摄像机器人距离障碍物的距离，形成传感器数据并将所有传感器数据打包成指定格式发送给服务控制模块。传感器模块具体包括了12组传感器和传感器数据收发装置，12组传感器通过数据线将传感数据发送给传感器数据收发装置，然后传感器数据收发装置将所收到的数据处理打包后通过网线发送给服务控制模块。传感器为超声波传感器，每一个超声波传感器探测的范围为一个顶角为30°的锥形体，每一个传感器包括了发送传感器、接收传感器、控制部分与电源部分，其工作原理为，由发送器与陶瓷振子换能器构成发送传感器将陶瓷振子的电振动能量转换成超声波并向空中辐射；接收传感器由陶瓷振子换能器与放大电路组成，其作用是将接收的机械振动变换成电能量，作为传感器接收器的输出，从而对发送的超声波进行检测。通过在七轴机器人的关键节点处安装12组传感器实现全方位的碰撞检测。具体安装方案为：

首先考虑到机器人末端摄像机的运行空间，在其四周安装4组传感器。1号传感器1安装在摄像机30的镜头上方，探测方向为沿镜头方向向前，用于探测镜头前方距离障碍物的距离，2号传感器2安装在摄像机30的镜头下方，探测方向为垂直镜头方向向下，用于探测镜头下方距离障碍物的距离，3号传感器3、4号传感器4分别安装在摄像机30两侧，用于探测摄像机两侧距离障碍物的距离。然后根据机器人的运动范围以及机器人的自碰撞关系在机器人机械臂上安装6组传感器。5号传感器5安装在旋转腕部26和摆动腕部27之间腕部28中点的下端，探测方向向下，用于探测机械臂腕部28距离下方障碍物的距离，6号传感器6安装在上臂关节24和旋转腕部26之间上臂25中点的下方，探测方向向下，用于探测机械臂上臂25距离下方障碍物的距离，7号传感器7、8号传感器8安装在上臂关节24和旋转腕部26之间上臂25中点的两侧，用于探测机械臂上臂25距离两侧障碍物的距离，9号传感器9、10号传感器10安装在上臂关节24的两侧，用于探测机械臂上臂关节两侧障碍物的距离。由于我们采用的摄像机器人的上臂长度一般为1.5-2米，加之实际应用过程中整个上臂的运动范围大约为135°，因此为了提高机器人运行的安全性采用4组传感器探测上臂关节和上臂两侧的障碍物距离。11号传感器11、12号传感器12安装在下臂关节23沿轨道轴21方向的两侧，探测方向为沿轨道运行方向，用于探测机械臂沿轨道轴21运行过程中轨道上障碍物的距离。传感器数据收发装置采取轮询的方式实时获取12组传感器的数据，并将数据打包发送。

所述的服务控制模块，用于接收传感器模块发送的传感器数据和摄像机器人发送的机器人数据，基于udp网络通讯在通讯周期内实时接收机器人数据和传感器数据，并且按照指定格式解析传感器数据，根据所收到的数据结合传感器参数设置计算得到机器人控制指令，并将控制指令发送给摄像机器人。服务控制模块一般包括一台pc服务器,在pc服务器上安装机器人的控制软件，由控制软件将计算好的控制指令进行处理，处理好的数据直接发送给摄像机器人所带的控制接口，控制摄像机器人执行。摄像机器人也可以向服务控制模块传输指定格式的数据，主要是关节数据、延迟信息等。

为了避免由于网络波动或者其它外部因素导致的接收到的传感器异常数据，pc服务器添加了传感器数据异常处理装置。所述的异常处理装置的主要原理为：将接收到的数据对应的逻辑处理进行延迟，即当系统接收到的传感器数据后，先将当前数据挂起，然后和前后各2组传感器数据对比，通过算法来判断之前挂起的数据是否是跳变的异常数据，然后再对发送给机器人的运动控制指令进行对应处理。由于传感器模块数据发送周期也在100ms左右，因此逻辑处理的延迟不会对安全性造成影响。首先将传感器的距离数据根据设定的响应区间，转化为传感器状态，考虑到机器人运动的连续性，当目标数据对应的状态与前后数据对应状态都不相同时，则认为是异常数据。也可以设置前后其他数量的传感器数据进行对比，数据组数越多可靠性越高，但是响应延迟也越高，需要进行平衡。

针对影视拍摄过程中现场复杂性的特点，在pc服务器端上构建传感器参数配置装置，完成参数保存功能。具体的功能是设置和保存与传感器有关的参数到本地数据库，方便下次控制系统使用时直接从本地读取之前用户的设置。具体的参数有与传感器通讯的ip和端口号、每个传感器距离响应区间(例如0-100cm,100-200,200-300，不同距离区间处理方式不同)和传感器的使用/禁用状态。

服务控制模块与传感器模块、摄像机器人的控制接口之间可以采用有线或者无线的通信方式，本实施例中采用有线udp网络进行通信。

本发明的另外一个实施例提供了碰撞检测的方法，如下所示：

首先在摄像机器人和pc服务器上进行收发数据模块的初始化和参数配置，摄像机器人提供了一个通讯和控制接口，在接口和pc服务器上进行参数配置和完成初始化，基于xml的数据格式实现数据的实时通讯。

超声波传感器通过发射和接收的信号，计算出传感器距离障碍物的距离，并将其作为传感器数据发送给服务器；计算的具体方法是，超声波发射器发射超声波的同时计数器开始计时，当接收器收到反射波就立即停止计时，根据记录的时间t以及超声波传播速度v(空气中为340m/s)，根据s＝v*t/2计算出发射点距障碍物面的距离s。

服务器根据接收到的传感器数据，根据预先设定的传感器配置参数，调用对应的运动控制指令修正算法，处理并计算出机器人控制指令。传感器配置功能pc服务器提供，可以由用户去设定传感器响应距离参数s0、s1、s2。计算机器人控制指令具体算法如下：

(1)根据传感器的配置参数定义数据响应区间{0～s0}、{s0～s1}、{s1～s2}、{s2～+∞}，0＜s0＜s1＜s2，上述四个区间分别对应四种不同的机器人状态：紧急制动，缓停，减速，正常；

(2)计算机器人的关节速度，具体方程如下：

其中s为传感器距离障碍物的距离，vel为机器人各关节原先运行的关节速度，vel′为根据传感器数据处理后的机器人各关节运行的关节速度，t为插值采样系数，通过插值采样计算机器人缓停轨迹数据，首先通过设置时间t和机器人通讯时间周期，计算得到对应的机器人运动指令个数sum，然后对t进行归一化处理t＝k/sum(k＝1、2、3...sum)，在发送过程中从第一个数据报发送开始，k从1增加到sum，然后再进行循环，τ为减速系数，一般根据现场情况设定；

(3)根据服务器与摄像机器人之间的通讯协议，以通讯协议定义好的数据格式将vel′打包成机器人控制指令，只要将此指令发送给摄像机器人控制接口就可以控制摄像机器人的运动。

服务器将机器人控制指令发送给摄像机器人，pc服务器将机器人控制指令打包，按照预先设置的数据格式，基于udp通信按照通信周期发送给摄像机器人控制接口，如此就可以控制摄像机器人每个关节运动多少度。上述通讯周期由机器人自身决定，本实施例是12ms。

由此，本发明实现了基于超声波传感器的摄像机器人碰撞检测，构建了摄像机器人与目标物体距离的运动关系，并实现了一套碰撞检测运动规划方案，最终实现碰撞检测应用。本发明可以应用于影视拍摄现场，为机器人操作人员提供一套灵活有效的机器人碰撞检测方案。