(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202111655419.7 (22)申请日 2021.12.3 0 (65)同一申请的已公布的文献号 申请公布号 CN 114299039 A (43)申请公布日 2022.04.08 (73)专利权人 广西大学 地址 530004 广西壮 族自治区南宁市西乡 塘区大学东路10 0号 (72)发明人 林义忠　谢震鹏　易雨晴　杜柳明　 (74)专利代理 机构 广西中知华誉知识产权代理 有限公司 45140 专利代理师 梁家玉 (51)Int.Cl. G06T 7/00(2017.01) G06T 7/20(2017.01)G06T 7/70(2017.01) G06T 17/05(2011.01) 审查员 彭玉玲 (54)发明名称 一种机器人及其碰撞检测装置和方法 (57)摘要 本发明公开了一种机器人及其碰撞检测装 置和方法， 属于机器人碰撞检测技术领域。 所述 碰撞检测装置包括： 信息采集模块， 用于采集机 器人周围真实场景中的物体及人的三维点云信 息； 轨迹规划模块， 用于规划机器人末端执行器 的运动轨迹； 位置获取模块， 用于获取机器人各 关节的位姿； 三维建图模块， 用于构建机器人及 其周围真实场景、 人的三维点云地图等； 碰撞检 测模块， 用于在更新的三维点云地图中检测机器 人末端执行器与周围真实场景中的物体及人是 否发生碰撞， 并输送出检测结果。 本发明发明可 以减少在 进行碰撞检测时， 机器人 继续运动所带 来的误差和延时， 同时还减少了进行碰撞检测的 数据量， 提高检测速度。 权利要求书3页 说明书7页 附图3页 CN 114299039 B 2022.08.19 CN 114299039 B 1.一种机器人的碰撞检测装置， 其特 征在于， 包括： 信息采集模块， 用于采集机器人周围真实场景中的物体及人的三维点云信息； 轨迹规划模块， 用于规划机器人末端执 行器的运动轨 迹； 位置获取模块， 用于根据机器人末端执行器的运动轨迹， 求解出下一时刻机器人各关 节变量； 应用齐次变换矩阵及根据下一时刻机器人各关节变量， 获取下一时刻机器人各连 杆的位姿； 三维建图模块， 用于建立第一三维点云模型和第二三维点云模型， 所述第一三维点云 模型为机器人的三维点云模型， 所述第二三 维点云模型为根据机器人周围真实场景中的物 体以及人的三维点云信息建立得到的三 维点云模型， 再将第一三 维点云模 型和第二三维点 云模型融合构建机器人及其周围真实场景、 人 的三维点云地图； 并根据不同时刻机器人各 连杆的位姿更新所述第一三 维点云模型， 根据信息采集模块采集到的三 维点云信息变换更 新所述第二 三维点云模型， 进 而得到更新后的三维点云地图； 及 碰撞检测模块， 用于在更新的三维点云地图中检测机器人末端执行器与周围真实场景 中的物体及人 是否发生碰撞， 并输送出检测结果； 还包括所述碰撞检测装置的碰撞检测方法， 所述碰撞检测方法包括以下步骤： (1)信息采集： 采集机器人周围真实场景中的物体以及人的三维点云信息； (2)轨迹规划： 规划机器人末端执 行器的运动轨 迹； (3)位置获取： 根据机器人末端执行器的运动轨迹， 求解出下一时刻机器人各关节变 量； 应用齐次变换矩阵及根据下一时刻机器人各关节变量， 获取下一时刻机器人各连杆 的 位姿； (4)三维建图： 建立机器人的三维点云模型， 即第一三维点云模型， 并根据机器人周围 真实场景中的物体以及人 的三维点云信息建立三维点云模型， 即第二三维点云模型， 再将 这两个三维点云模型融合构建机器人及其周围物体、 人 的三维点云地图； 并根据不同时刻 机器人各连杆的位姿 更新所述第一三 维点云模型， 根据信息采集模块采集到的三 维点云信 息变化更新所述第二 三维点云模块， 进 而得到更新后的三维点云地图； 步骤(4)还包括在三维点云地图上分别建立机器人末端执行器的三维点云模型的球形 包围盒和机器人周围真实场景中的物体以及人的三维点云模型的球形包围盒； (5)碰撞检测： 在更新后的三维点云地图中检测机器人末端执行器与周围真实场景中 的物体或人 是否发生碰撞， 并输送出检测结果； 步骤(5)所述检测机器人末端执行器与周围真实场景中的物体或人是否发生碰撞的具 体步骤如下： 51)根据深度相机中拍摄得到当前在机器人末端执行器周围的物体， 确认碰撞检测物 体是物体i； 52)设物体i球 形包围盒的球心坐标为(xoi,yoi,zoi)半径为Ri， 机器人末端执行器球 形包 围盒的球心为(xoj,yoj,zoj)， 半径为Rj； 物体i的点云坐标记为(xi,yi,zi)； 机器人末端执行 器的点云坐标记为(xj,yj,zj)； 根据两个 球形包围盒的球心 距离D判断是否发生碰撞： 判断 是否成立， 不成立则 不发生碰撞， 进入58)； 成立则可能发生碰撞， 继续5 3)；权　利　要　求　书 1/3 页 2 CN 114299039 B 253)在两球形包围盒相交部分的中心点处， 取一个将该相交部分包含在内的长方体包 围盒区域， 记为检测区域， 并对在该检测区域内的物体i和机器人末端 执行器的点进 行碰撞 检测； 54)对检测区域内的点， 按切片法进行碰撞检测， 且从长方体检测区域的中心位置开始 往上下两边选择切面进 行检测， 切面为平面且其上的点的z坐标相同； 对切面上的点先建立 平行于X、 Y轴的两个分别包围物体i的点和机器人末端 执行器的点的矩形包围盒， 这两个矩 形包围盒的特征点坐标分别为(xi， min,yi， min)、 (xi， max,yi， max)、 (xj， min,yj， min)和(xj， max, yj， max)； 先判断两矩形包围盒是否相交， 即yi， min>yj， max或yj， min>yi， max是否成立， 若有一个成 立， 则该切面的点不发生碰撞， 继续检测下一个切面； 都不成立则继续检测xi， min>xj， max或 xj， min>xi， max是否成立， 若有一个成立， 则该切 面的点不发生碰撞， 继续检测下一个切 面； 都 不成立则继续5 5)； 55)取两个矩形包围盒相交区域里的物体i对应的三维点云模型的点和机器人末端执 行器对应的三维点云模型的点进行碰撞检测； 取yi＝yj的点进行比较， 若xoj>xoi， 则进行步 骤56)； 若xoj<xoi， 则进行步骤57)； 56)若xj>xi， 则该点不发生碰撞， 进行下一点的碰撞检测； 若xj≤xi， 则发生碰撞， 将碰撞 检测结果发送给避障模块； 57)若xi>xj， 则该点不发生碰撞， 进行下一点的碰撞检测； 若xi≤xj， 则发生碰撞， 将碰撞 检测结果发送给避障模块； 58)碰撞检测结束， 反馈检测结果； (6)碰撞响应： 根据输送出的检测结果制定 碰撞检测后机器人的响应方式。 2.根据权利要求1所述的碰撞检测装置， 其特征在于， 还包括避 障模块， 所述避 障模块 用于接收输送出的检测结果并根据检测结果制定 碰撞检测后机器人的响应方式。 3.根据权利要求1所述的碰撞检测装置， 其特征在于， 所述信 息采集模块包括深度相机 和图像处理模块； 所述深度相 机安装于接近机器人末端执行器的小臂上， 用于采集机器人 运动过程中末端 执行器周围真实场景中的物体以及人的图像； 所述图像处理模块安装于机 器人上用于对深度相机采集的图像进 行图像预 处理， 然后对图像中的物体、 人进 行分类， 并 对属于不同类别的像素点进行 标记。 4.根据权利要求1所述的碰撞检测装置， 其特征在于， 所述位置获取模块还包括用于在 下一时刻将机器人各关节编码器信息与求解得到的下一时刻机器人各关节变量对比得到 偏差值， 将偏差值结合求解出的下下一时刻的机器人各关节变量得到纠正后的下下一时刻 的机器人各关节变量， 应用齐次变换矩阵及根据纠正后的下下一时刻的机器人各关节变量 得到下下一时刻机器人 各连杆的位姿。 5.根据权利要求3所述的碰撞检测装置， 其特征在于， 所述第 一三维点云模型为根据D ‑ H法对机器人建立的三维点云模型， 所述第二三维点云模型为对经过图像处理模块处理得 到的机器人周围真实场景中的物体以及人的三维点云信息进 行转换得到的三 维点云模型； 所述将第一三维点云模型和第二三维点云模型融合具体为将第一三维点云模型和第二三 维点云模型均放于同一坐标系下， 融合成一个包含机器人及其周围真实场景中的物体和人 的三维点云地图。 6.根据权利要求4所述的碰撞检测装置， 其特征在于， 所述三维建图模块还包括对所述权　利　要　求　书 2/3 页 3 CN 114299039 B 3