CN114299039A - 一种机器人及其碰撞检测装置和方法 - Google Patents

Abstract

本发明公开了一种机器人及其碰撞检测装置和方法，属于机器人碰撞检测技术领域。所述碰撞检测装置包括：信息采集模块，用于采集机器人周围真实场景中的物体及人的三维点云信息；轨迹规划模块，用于规划机器人末端执行器的运动轨迹；位置获取模块，用于获取机器人各关节的位姿；三维建图模块，用于构建机器人及其周围真实场景、人的三维点云地图等；碰撞检测模块，用于在更新的三维点云地图中检测机器人末端执行器与周围真实场景中的物体及人是否发生碰撞，并输送出检测结果。本发明发明可以减少在进行碰撞检测时，机器人继续运动所带来的误差和延时，同时还减少了进行碰撞检测的数据量，提高检测速度。

Description

一种机器人及其碰撞检测装置和方法

技术领域

本发明涉及机器人碰撞检测技术领域，具体涉及一种机器人及其碰撞检测装置和方法。

背景技术

目前，机器人需要与用户合作完成很多任务，因此用户会经常性使用到协作机器人。在执行任务过程中，机器人需能检测是否与其周围的人或物体发生了碰撞，并采用合适的策略消除碰撞，以保障人的安全，并避免物体受撞破损。

现有的机器人碰撞检测多以力矩传感器和接近传感器等方式来进行碰撞检测，但是存在造价高的问题。基于视觉的检测方法存在计算量大，实时性不好等问题，通过构建地图进行碰撞检测的方法也有计算量大或者检测方法简单、精度不够的问题。

发明内容

针对上述问题，本发明的目的是提供一种机器人及其碰撞检测装置和方法，本发明利用安装在靠近机器人末端执行器的小臂上的kinect相机来获得机器人周围物体和人的三维点云信息，以及对于机器人也根据D-H参数建立相应的三维点云模型，在同一坐标系下建立出三维点云地图，并提出了一种机器人位置获取方法进行位置获取，以及一种减少数据量的碰撞检测方法，提高了检测速度。

为了实现上述目的，本发明采用的技术方案为：

一种机器人的碰撞检测装置，包括：

信息采集模块，用于采集机器人周围真实场景中的物体以及人的三维点云信息；

轨迹规划模块，用于规划机器人末端执行器的运动轨迹；

位置获取模块，用于根据机器人末端执行器的运动轨迹，求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；

三维建图模块，用于建立第一三维点云模型和第二三维点云模型，所述第一三维点云模型为机器人的三维点云模型，所述第二三维点云模型为根据机器人周围真实场景中的物体以及人的三维点云信息建立得到的三维点云模型，再将第一三维点云模型和第二三维点云模型融合构建机器人及其周围真实场景、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述一三维点云模型，根据信息采集模块采集到的三维点云信息变换更新所述第二三维点云模型，，进而得到更新后的三维点云地图；及

碰撞检测模块，用于在更新的三维点云地图中检测机器人末端执行器与周围真实场景中的物体及人是否发生碰撞，并输送出检测结果。

进一步的，所述机器人的碰撞检测装置还包括避障模块，所述避障模块用于接收输送出的检测结果并根据检测结果制定碰撞检测后机器人的响应方式。

进一步的，所述信息采集模块包括深度相机和图像处理模块；所述深度相机安装于接近机器人末端执行器的小臂上，用于采集机器人运动过程中末端执行器周围真实场景中的物体以及人的图像；所述图像处理模块安装于机器人上用于对深度相机采集的图像进行图像预处理，然后对图像中的物体、人进行分类，并对属于不同类别的像素点进行标记。

进一步的，所述位置获取模块还包括用于在下一时刻将机器人各关节编码器信息与求解得到的下一时刻机器人各关节变量对比得到偏差值，将偏差值结合求解出的下下一时刻的机器人各关节变量得到纠正后的下下一时刻的机器人各关节变量，应用齐次变换矩阵及根据纠正后的下下一时刻的机器人各关节变量得到下下一时刻机器人各连杆的位姿。

进一步的，所述第一三维点云模型为根据D-H法对机器人建立的三维点云模型，所述第二三维点云模型为对经过图像处理模块处理得到的机器人周围真实场景中的物体以及人的三维点云信息进行转换得到的三维点云模型；所述将第一三维点云模型和第二三维点云模型融合具体为将第一三维点云模型和第二三维点云模型均放于同一坐标系下，融合成一个包含机器人及其周围真实场景中的物体和人的三维点云地图。

进一步的，所述三维建图模块还包括对所述第一三维点云模型建立的球形包围盒以及对第二三维点云模型建立的的球形包围盒。

本发明还提供了一种机器人的碰撞检测方法(即采用上述碰撞检测装置进行碰撞检测的方法)，包括以下步骤：

(1)信息采集：采集机器人周围真实场景中的物体以及人的三维点云信息；

(2)轨迹规划：规划机器人末端执行器的运动轨迹；

(3)位置获取：根据机器人末端执行器的运动轨迹，求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；(4)三维建图：建立机器人的三维点云模型，即第一三维点云模型，并根据机器人周围真实场景中的物体以及人的三维点云信息建立三维点云模型，即第二三维点云模型，再将这两个三维点云模型融合构建机器人及其周围物体、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述第一三维点云模型，根据信息采集模块采集到的三维点云信息变化更新所述第二三维点云模块，，进而得到更新后的三维点云地图；

(5)碰撞检测：在更新后的三维点云地图中检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞，并输送出检测结果；

(6)避障：根据输送出的检测结果制定碰撞检测后机器人的响应方式。

进一步的，步骤(4)还包括在三维点云地图上分别建立机器人末端执行器的三维点云模型的球形包围盒和机器人周围真实场景中的物体以及人的三维点云模型的球形包围盒。

进一步的，步骤(5)所述检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞的具体步骤如下：

51)根据深度相机中拍摄得到当前在机器人末端执行器周围的物体，确认碰撞检测物体是物体i；

52)设物体i球形包围盒的球心坐标为(x_oi,y_oi,z_oi)半径为R_i，机器人末端执行器球形包围盒的球心为(x_oj,y_oj,z_oj)，半径为R_j；物体i的点云坐标记为(x_i,y_i,z_i)；机器人末端执行器的点云坐标记为(x_j,y_j,z_j)；根据两个球形包围盒的球心距离D进行球形包围盒碰撞检测：

判断

是否成立，不成立则不发生碰撞，进入59)；成立则可能发生碰撞，继续53)；

53)在两球形包围盒相交部分的中心点处，取一个将该相交部分包含在内的长方体包围盒区域，记为检测区域，并对在该检测区域内的物体i和机器人末端执行器的点进行碰撞检测；

54)对检测区域内的点，按切片法进行碰撞检测，且从长方体检测区域的中心位置开始往上下两边选择切面进行检测，切面为平面且其上的点的坐标相同；对切面上的点先建立平行于X、Y轴的两个分别包围物体i的点和机器人末端执行器的点的矩形包围盒，这两个矩形包围盒的特征点坐标分别为(x_i，min,y_i，min)、(x_i，max,y_i，max)、(x_j，min,y_j，min)和(x_j，max,y_j，_max)；先判断两矩形包围盒包围盒是否相交，即y_i，min>y_j，max或y_j，min>y_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续检测x_i，min>x_j，_max或x_j，min>x_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续55)；

55)取两个矩形包围盒相交区域里的物体i对应的三维点云模型的点和机器人末端执行器对应的三维点云模型的点进行碰撞检测；取y_i＝y_j的点进行比较，若x_oj>x_oi，则进行步骤56)；若x_oj<x_oi，则进行步骤57)；

56)若x_j>x_i，则该点不发生碰撞，进行下一点的碰撞检测；若x_j≤x_i，则发生碰撞，将碰撞检测结果发送给避障模块；

57)若x_i>x_j，则该点不发生碰撞，进行下一点的碰撞检测；若x_i≤x_j，则发生碰撞，将碰撞检测结果发送给避障模块；

58)碰撞检测结束，反馈检测结果。

本发明还提供了一种机器人，所述机器人包括上述任一项所述的机器人的碰撞检测装置。

综上所述，由于采用了上述技术方案，本发明的有益效果为：

本发明通过三维建图模块、位置获取模块和避障模块等的设置，并提出了机器人各关节位置的位置获取方法和碰撞检测模块具体的碰撞检测方法，使得本发明可以减少在进行碰撞检测时，机器人继续运动所带来的误差和延时，同时还减少了进行碰撞检测的数据量，提高检测速度。此外，本发明无需借助力矩传感器和接近传感器等进行碰撞检测，降低了成本。

附图说明

图1为本发明实施例的一种机器人的碰撞检测装置的方框示意图；

图2为本发明实施例的一种机器人的碰撞检测方法的流程图；

图3为本发明实施例的一种机器人的碰撞检测方法中碰撞检测模块的具体方法流程图；

图中，1-信息采集模块，11-深度相机，12-图像处理模块，2-轨迹规划模块，3-位置获取模块，4-三维建图模块，41-第一三维点云模型，42-第二三维点云模型，5-碰撞检测模块，6-避障模块。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

实施例

请参见图1，一种机器人的碰撞检测装置，包括：

信息采集模块1，用于采集机器人周围真实场景中的物体以及人的三维点云信息；

轨迹规划模块2，用于规划机器人末端执行器的运动轨迹；

位置获取模块3，用于根据机器人末端执行器的运动轨迹，求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；

三维建图模块4，用于建立第一三维点云模型41和第二三维点云模型42，所述第一三维点云模型41为机器人的三维点云模型，所述第二三维点云模型42为根据机器人周围真实场景中的物体以及人的三维点云信息建立得到的三维点云模型，其中所述第一三维点云模型41和第二三维点云模型42的建立方法均为现有技术，在此不再复述。再将第一三维点云模型41和第二三维点云模型42融合构建机器人及其周围真实场景、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述一三维点云模型41，根据信息采集模块采集到的三维点云信息变换更新所述第二三维点云模型42，进而得到更新后的三维点云地图；

碰撞检测模块5，用于在更新的三维点云地图中检测机器人末端执行器与周围真实场景中的物体及人是否发生碰撞，并输送出检测结果；及

避障模块6，用于接收输送出的检测结果并根据检测结果制定碰撞检测后机器人的响应方式。

作为进一步优选的技术方案，所述信息采集模块1包括深度相机11和图像处理模块12；所述深度相机11安装于机器人上接近机器人末端执行器的小臂上，用于采集机器人运动过程中末端执行器周围真实场景中的物体以及人的图像。本实施例中，深度相机11为kinect相机。所述图像处理模块12安装于机器人上用于对深度相机11采集的图像进行图像预处理，如去噪等，然后对图像中的物体、人等进行分类，并对属于不同类别的像素点进行标记(可用数字、字符进行标记)。

作为进一步优选的技术方案，所述第一三维点云模型41为根据D-H法对机器人建立的三维点云模型，所述第二三维点云模型42为对经过图像处理模块处理得到的机器人周围真实场景中的物体以及人的三维点云信息进行转换得到的三维点云模型；所述将第一三维点云模型41和第二三维点云模型42融合具体为将第一三维点云模型41和第二三维点云模型42均放于同一坐标系下，融合成一个包含机器人及其周围真实场景中的物体和人的三维点云地图。

作为进一步优选的技术方案，所述三维建图模块4还包括对所述第一三维点云模型41建立的球形包围盒以及对第二三维点云模型42建立的的球形包围盒；所述第一三维点云模型41根据机器人的运动轨迹规划得到的各连杆的位姿后进行更新，所述第二三维点云模型42根据深度相机11得到的图像来判断是否更新；所述两个球形包围盒分别根据所述第一三维点云模型41和第二三维点云模型42的更新而更新。

本发明还提供了一种机器人的碰撞检测方法，如图2，包括以下步骤：

(1)信息采集：采集机器人周围真实场景中的物体以及人(如操作员)的三维点云信息；具体为：

采用安装在接近机器人末端执行器的小臂上的kinect相机来采集机器人运动过程中机器人末端执行器周围真实场景中的物体和人的图像，并对所采集的图像进行图像预处理，如去噪等，然后对图像中的物体、人等进行分类，并对属于不同类别的像素点进行标记(可用数字、字符进行标记)。

(2)轨迹规划：采用现有方法规划机器人末端执行器的运动轨迹；

(3)位置获取：根据机器人末端执行器的运动轨迹，用机器人逆运动学求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；具体为：

在开始工作T0时刻时，根据机器人末端执行器的的运动轨迹，根据D-H法得到的齐次变换矩阵结合机器人逆运动学求解出T1时刻机器人各关节变量

应用D-H法得到的齐次变换矩阵及根据T1时刻机器人各关节变量

获取T1时刻机器人各连杆的位姿，以使三维建图模块根据T1时刻得到的

以及机器人各连杆的位姿更新机器人的三维点云模型，进而得到更新后的三维点云地图。

(4)三维建图：建立机器人的三维点云模型，即第一三维点云模型41，并根据机器人周围真实场景中的物体以及人的三维点云信息建立三维点云模型，即第二三维点云模型42，再将这两个三维点云模型融合构建机器人及其周围物体、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述第一三维点云模型41，根据信息采集模块1采集到的三维点云信息变化更新所述第二三维点云模块42，进而得到更新后的三维点云地图；并在三维点云地图上分别建立第一三维点云模型41的球形包围盒和第二三维点云模型42的球形包围盒。

(5)碰撞检测：在更新后的三维点云地图中检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞，并输送出检测结果；具体为：

进一步的，如图3，步骤(5)所述检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞的具体步骤如下：

本实施例中，为了提高检测效率，只检测在机器人末端执行器附近的物体i是否与末端执行器发生碰撞。

51)根据深度相机中拍摄得到当前在机器人末端执行器周围的物体，确认碰撞检测物体是物体i；

52)设物体i球形包围盒的球心坐标为(x_oi,y_oi,z_oi)半径为R_i，机器人末端执行器球形包围盒的球心为(x_oj,y_oj,z_oj)，半径为R_j；物体i的点云坐标记为(x_i,y_i,z_i)；机器人末端执行器的点云坐标记为(x_j,y_j,z_j)；根据两个球形包围盒的球心距离D判断是否发生碰撞：

判断

是否成立，不成立则不发生碰撞，进入59)；成立则可能发生碰撞，继续53)；

53)在两球形包围盒相交部分的中心点处，取一个将该相交部分包含在内的长方体包围盒区域，记为检测区域，并对在该检测区域内的物体i和机器人末端执行器的点进行碰撞检测；

54)对检测区域内的点，按切片法进行碰撞检测，且从长方体检测区域的中心位置开始往上下两边选择切面进行检测，切面为平面且其上的点的坐标相同；对切面上的点先建立平行于X、Y轴的两个分别包围物体i的点和机器人末端执行器的点的矩形包围盒，这两个矩形包围盒的特征点坐标分别为(x_i，min,y_i，min)、(x_i，max,y_i，max)、(x_j，min,y_j，min)和(x_j，max,y_j，_max)；先判断两矩形包围盒包围盒是否相交，即y_i，min>y_j，max或y_j，min>y_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续检测x_i，min>x_j，max或x_j，min>x_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续55)；

55)取两个矩形包围盒相交区域里的物体i对应的三维点云模型的点和机器人末端执行器对应的三维点云模型的点进行碰撞检测；取y_i＝y_j的点进行比较，若x_oj>x_oi，则进行步骤56)；若x_oj<x_oi，则进行步骤57)；

56)若x_j>x_i，则该点不发生碰撞，进行下一点的碰撞检测；若x_j≤x_i，则发生碰撞，将碰撞检测结果发送给避障模块；

57)若x_i>x_j，则该点不发生碰撞，进行下一点的碰撞检测；若x_i≤x_j，则发生碰撞，将碰撞检测结果发送给避障模块；

58)碰撞检测结束，反馈检测结果。

(6)避障：根据输送出的检测结果制定碰撞检测后机器人的响应方式；响应方式包括当无碰撞时，继续运动；当有碰撞且速度较慢时，机器人减速并重新规划轨迹，避免碰撞；当有碰撞且速度较快时，机器人快速远离碰撞目标。

对应上述实施例，本发明还提供了一种机器人。本发明提供的机器人，包括上述实施例提出的机器人的碰撞检测装置，其具体实施方式可参照上述实施例，为避免冗余，在此不再赘述。需要注意的是，本发明所述的机器人为固定的工业机器人，即机器人的位置不能移动，仅机器人的各关节可移动。

此外，在其他实施例中，所述位置获取模块3还可用于在下一时刻将机器人各关节编码器信息与求解得到的下一时刻机器人各关节变量对比得到偏差值，将偏差值结合求解出的下下一时刻的机器人各关节变量(同样也是根据机器人末端执行器的运动轨迹，根据D-H法得到的齐次变换矩阵结合机器人逆运动学求解出)得到纠正后的下下一时刻的机器人各关节变量，应用齐次变换矩阵及根据纠正后的下下一时刻的机器人各关节变量得到下下一时刻机器人各连杆的位姿。其位置获取方法具体为：

在开始工作T0时刻时，根据机器人末端执行器的的运动轨迹，根据D-H法得到的齐次变换矩阵结合机器人逆运动学求解出T1时刻机器人各关节变量

应用D-H法得到的齐次变换矩阵及根据T1时刻机器人各关节变量

获取T1时刻机器人各连杆的位姿，以使三维建图模块根据T1时刻得到的

以及机器人各连杆的位姿更新机器人的三维点云模型，进而得到更新后的三维点云地图，再进行碰撞检测，即可完成T0时刻的位置获取和碰撞检测。到了T1时刻，再从机器人的编码器中读出机器人各关节信息θ₁，并将所述编码器中读出的机器人各关节信息θ₁与求解得到的T1时刻机器人各关节变量

对比得到偏差值

将偏差值Δ₁结合求解出的T2时刻的机器人各关节变量

得到纠正后的T2时刻的机器人各关节变量，应用齐次变换矩阵及根据纠正后的T2时刻的机器人各关节变量得到T2时刻机器人各连杆的位姿，以使三维建图模块再次根据T2时刻得到的

以及机器人各连杆的位姿更新机器人的三维点云模型，进而得到更新后的三维点云地图，再进行碰撞检测，即可完成T1时刻的位置获取和碰撞检测。依此类推进行下一时刻的位置获取和碰撞检测。

上述说明是针对本发明较佳可行实施例的详细说明，但实施例并非用以限定本发明的专利申请范围，凡本发明所提示的技术精神下所完成的同等变化或修饰变更，均应属于本发明所涵盖专利范围。

Claims (10)

1.一种机器人的碰撞检测装置，其特征在于，包括：

信息采集模块，用于采集机器人周围真实场景中的物体及人的三维点云信息；

轨迹规划模块，用于规划机器人末端执行器的运动轨迹；

位置获取模块，用于根据机器人末端执行器的运动轨迹，求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；

三维建图模块，用于建立第一三维点云模型和第二三维点云模型，所述第一三维点云模型为机器人的三维点云模型，所述第二三维点云模型为根据机器人周围真实场景中的物体以及人的三维点云信息建立得到的三维点云模型，再将第一三维点云模型和第二三维点云模型融合构建机器人及其周围真实场景、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述一三维点云模型，根据信息采集模块采集到的三维点云信息变换更新所述第二三维点云模型，进而得到更新后的三维点云地图；及

碰撞检测模块，用于在更新的三维点云地图中检测机器人末端执行器与周围真实场景中的物体及人是否发生碰撞，并输送出检测结果。

2.根据权利要求1所述的碰撞检测装置，其特征在于，还包括避障模块，所述避障模块用于接收输送出的检测结果并根据检测结果制定碰撞检测后机器人的响应方式。

3.根据权利要求1所述的碰撞检测装置，其特征在于，所述信息采集模块包括深度相机和图像处理模块；所述深度相机安装于接近机器人末端执行器的小臂上，用于采集机器人运动过程中末端执行器周围真实场景中的物体以及人的图像；所述图像处理模块安装于机器人上用于对深度相机采集的图像进行图像预处理，然后对图像中的物体、人进行分类，并对属于不同类别的像素点进行标记。

4.根据权利要求1所述的碰撞检测装置，其特征在于，所述位置获取模块还包括用于在下一时刻将机器人各关节编码器信息与求解得到的下一时刻机器人各关节变量对比得到偏差值，将偏差值结合求解出的下下一时刻的机器人各关节变量得到纠正后的下下一时刻的机器人各关节变量，应用齐次变换矩阵及根据纠正后的下下一时刻的机器人各关节变量得到下下一时刻机器人各连杆的位姿。

5.根据权利要求3所述的碰撞检测装置，其特征在于，所述第一三维点云模型为根据D-H法对机器人建立的三维点云模型，所述第二三维点云模型为对经过图像处理模块处理得到的机器人周围真实场景中的物体以及人的三维点云信息进行转换得到的三维点云模型；所述将第一三维点云模型和第二三维点云模型融合具体为将第一三维点云模型和第二三维点云模型均放于同一坐标系下，融合成一个包含机器人及其周围真实场景中的物体和人的三维点云地图。

6.根据权利要求4所述的碰撞检测装置，其特征在于，所述三维建图模块还包括对所述第一三维点云模型建立的球形包围盒以及对第二三维点云模型建立的的球形包围盒。

7.一种机器人的碰撞检测方法，其特征在于，包括以下步骤：

(1)信息采集：采集机器人周围真实场景中的物体以及人的三维点云信息；

(2)轨迹规划：规划机器人末端执行器的运动轨迹；

(3)位置获取：根据机器人末端执行器的运动轨迹，求解出下一时刻机器人各关节变量；应用齐次变换矩阵及根据下一时刻机器人各关节变量，获取下一时刻机器人各连杆的位姿；

(4)三维建图：建立机器人的三维点云模型，即第一三维点云模型，并根据机器人周围真实场景中的物体以及人的三维点云信息建立三维点云模型，即第二三维点云模型，再将这两个三维点云模型融合构建机器人及其周围物体、人的三维点云地图；并根据不同时刻机器人各连杆的位姿更新所述第一三维点云模型，根据信息采集模块采集到的三维点云信息变化更新所述第二三维点云模块，，进而得到更新后的三维点云地图；

(5)碰撞检测：在更新后的三维点云地图中检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞，并输送出检测结果；

(6)碰撞响应：根据输送出的检测结果制定碰撞检测后机器人的响应方式。

8.根据权利要求7所述的碰撞检测方法，其特征在于，步骤(4)还包括在三维点云地图上分别建立机器人末端执行器的三维点云模型的球形包围盒和机器人周围真实场景中的物体以及人的三维点云模型的球形包围盒。

9.根据权利要求7所述的碰撞检测方法，其特征在于，步骤(5)所述检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞的具体步骤如下：

51)根据深度相机中拍摄得到当前在机器人末端执行器周围的物体，确认碰撞检测物体是物体i；

52)设物体i球形包围盒的球心坐标为(x_oi,y_oi,z_oi)半径为R_i，机器人末端执行器球形包围盒的球心为(x_oj,y_oj,z_oj)，半径为R_j；物体i的点云坐标记为(x_i,y_i,z_i)；机器人末端执行器的点云坐标记为(x_j,y_j,z_j)；根据两个球形包围盒的球心距离D判断是否发生碰撞：

判断

是否成立，不成立则不发生碰撞，进入59)；成立则可能发生碰撞，继续53)；

53)在两球形包围盒相交部分的中心点处，取一个将该相交部分包含在内的长方体包围盒区域，记为检测区域，并对在该检测区域内的物体i和机器人末端执行器的点进行碰撞检测；

54)对检测区域内的点，按切片法进行碰撞检测，且从长方体检测区域的中心位置开始往上下两边选择切面进行检测，切面为平面且其上的点的坐标相同；对切面上的点先建立平行于X、Y轴的两个分别包围物体i的点和机器人末端执行器的点的矩形包围盒，这两个矩形包围盒的特征点坐标分别为(x_i，min,y_i，min)、(x_i，max,y_i，max)、(x_j，min,y_j，min)和(x_j，max,y_j，_max)；先判断两矩形包围盒包围盒是否相交，即y_i，min>y_j，max或y_j，min>y_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续检测x_i，min>x_j，_max或x_j，min>x_i，max是否成立，若有一个成立，则该切面的点不发生碰撞，继续检测下一个切面；都不成立则继续55)；

55)取两个矩形包围盒相交区域里的物体i对应的三维点云模型的点和机器人末端执行器对应的三维点云模型的点进行碰撞检测；取y_i＝y_j的点进行比较，若x_oj>x_oi，则进行步骤56)；若x_oj<x_oi，则进行步骤57)；

56)若x_j>x_i，则该点不发生碰撞，进行下一点的碰撞检测；若x_j≤x_i，则发生碰撞，将碰撞检测结果发送给避障模块；

57)若x_i>x_j，则该点不发生碰撞，进行下一点的碰撞检测；若x_i≤x_j，则发生碰撞，将碰撞检测结果发送给避障模块；

58)碰撞检测结束，反馈检测结果。

10.一种机器人，其特征在于，包括根据权利要求1-5中任一项所述的机器人的碰撞检测装置。

Images