(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211015458.5 (22)申请日 2022.08.24 (71)申请人 广东工业大 学 地址 510062 广东省广州市越秀区东 风东 路729号 申请人 广州秉优智能科技有限公司 (72)发明人 赵荣丽　包泽仁　张浩　林贵祥　 郭芳名　 (74)专利代理 机构 佛山市禾才知识产权代理有 限公司 4 4379 专利代理师 梁永健 (51)Int.Cl. B25J 19/00(2006.01) B25J 9/16(2006.01) G06V 10/764(2022.01) (54)发明名称 一种面向关节型机器人的连续碰撞检测方 法 (57)摘要 一种面向关节型机器人的连续碰撞检测方 法， 包括以下步骤： 步骤S1： 获取当前场景图像， 并对场景图像进行预处理， 其中预处理包括对场 景图像中的所有物体进行归类， 步骤S2： 根据碰 撞检测物体的当前帧的 图元信息和/或下一帧的 图元信息构建动态的LB VH， 通过LB VH进行碰撞 检 测， 获取得到碰撞图元对， 步骤S3： 对碰撞图元集 内的碰撞图元对构建出一元连续碰撞检测方程； 求解一元连续碰撞检测方程， 使用基于包含函数 的谓词评估求解的结果， 输出最终碰撞检测结 果。 本方法相较于其它连续碰撞检测算法在确保 高效的同时又做到了零漏报和更低的误报率， 可 以在保证检测质量的前提下为使用本方法的系 统降低计算 开销。 权利要求书4页 说明书10页 附图2页 CN 115179326 A 2022.10.14 CN 115179326 A 1.一种面向关节型机器人的连续碰撞检测方法， 其特 征在于， 包括以下步骤： 步骤S1： 获取当前场景图像， 并对场景图像进行预处理， 其中所述预处理包括对场景图 像中的所有物体进行归类， 分类出碰撞检测物体和非碰撞检测物体， 并获取碰撞检测物体 的所有图元信息， 其中所有图元信息包括当前帧的图元信息以及下一帧的图元信息； 步骤S2： 根据碰撞检测物体的当前帧的图元信息和/或下一帧的图元信息构建动态 的 LBVH， 通过LBVH进行碰撞检测， 获取 得到碰撞图元对， 并保存在碰撞图元集内； 步骤S3： 对碰撞图元集内的碰撞图元对构建出一元 连续碰撞检测方程； 求解所述一元连续碰撞检测方程， 使用基于包含函数的谓词评估求解的结果， 输出最 终碰撞检测结果。 2.根据权利要求1所述的一种面向关节型机器人的连续碰撞检测方法， 其特征在于， 所 述步骤S1中场景图像中的所有物体进行归类包括以下步骤： 步骤S11： 获取场景图像中所有物体的模型； 步骤S12： 判断所述模型为环境模型或是机器人模型， 若所述模型为机器人模型， 则将 其标记为碰撞检测物体， 若所述模型为环境模型， 则判断该所述环境模型是否落入到机器 人的工作范围内， 若没有落入到机器人 的工作范围内， 则将该所述环境模型标记为非碰撞 检测物体， 若环境模型落入到机器人 的工作范围内， 则再次判断该环境模型是否为机器人 的作业目标或待更换末端 执行器， 若 是， 则将所述环境模型标记为 非碰撞检测物体， 若否则 将该所述环境模型 标记为碰撞检测物体。 3.根据权利要求2所述的一种面向关节型机器人的连续碰撞检测方法， 其特征在于， 所 述步骤S2的具体步骤如下： 步骤S21： 对每一个碰撞检测物体的当前帧的图元信息的重心三维坐标转化为莫顿码， 其中每一个坐标值以K位 二进制数表示， 所述莫 顿码长度为3k； 步骤S22： 对所有的莫顿码平均分成n份数据， 每一份数据分别 进行自身的基数排序， 在 基数排序完成后， 对n份数据进排序整合得到新莫 顿码排序； 步骤S23： 对排序后的图元信息进行分组， 其中图元包括动态图元与静态图元， 并发的 将每一组的图元信息划分至主BVH树中； 步骤S24： 获取BVH树中动态图元， 并构建动态图元的包围体， 自底向上依次构造主BVH 中剩余节点的包围体； 其中， 若为叶子节点的包围体构建， 则直接构造该节点对应图元的包围体， 若为父节点 包围体的构造， 则将其所有子节点的包围体进行合并的方式来构造； 步骤S25： 对主BVH树中每一层的包围体进行相交测试， 得到获取 得到碰撞图元对。 4.根据权利要求3所述的一种面向关节型机器人的连续碰撞检测方法， 其特征在于， 在 步骤S23中， 并发的将每一组的图元信息划分至主BVH树中的步骤如下： 获取一组图元信息的莫顿码的最高相异位p， 从莫顿码最高位开始对图元信息进行划 分， 其中由最高位到p的前一位的划分过程整组只用进行一次， 而由p到最低位的划分过程 需要对组内所有的莫 顿码都进行； 对单个莫顿码划分的标准为： 若该位的莫顿码值为0则将对应的图元信息划分至左子 树， 若该位莫顿码的值为 1则将对应的图元信息划分至右子树， 向右获取图元莫顿码的下一 个值继续进行判断， 直至莫 顿码的最后一 位。权　利　要　求　书 1/4 页 2 CN 115179326 A 25.根据权利要求4所述的一种面向关节型机器人的连续碰撞检测方法， 其特征在于， 构 建动态图元的包围体的过程如下： 步骤A1： 使用泰勒模型表示机器人各轴相对于全局坐标系的变换矩阵的全局变换矩 阵， 所述全局变换矩阵标记为泰勒变换矩阵； 步骤A2： 获取将前帧的图元信息的包围体的八个顶点以及下一帧的图元信息的包围体 的八个顶点应用于泰勒变换矩阵， 得到表示顶点在s∈(0,1)时间内运动的泰勒向量； 步骤A3： 使用轴对齐包围体对所述泰勒向量进行包围， 生成八个包围体， 其中每一个包 围体都包 含有时间t∈(0,1)内物体的运动轨 迹； 步骤A4： 以自底向上的方式对八个包围体生成迷你BVH树； 步骤A5： 使用迷你BVH树的根节点代替所述动态图元在所述主BVH树上对应的叶子节 点。 6.根据权利要求4所述的一种面向关节型机器人的连续碰撞检测方法， 其特征在于， 所 述步骤S3具体如下： 步骤S31： 从所述碰撞图元集内获取任一碰撞图元对构建出一元 连续碰撞检测方程； 步骤S32： 使用伯恩斯 坦多项式表示 一元连续碰撞检测方程； 其中伯恩斯 坦多项式表示之后的检测方程如下 所示： 其中 其中i＝0， 1 ···， n， f(t)表示一元连续碰 撞检测方程； 步骤S33： 求解公式(1)， 获取k0～k3的值， 若k0～k3均大于0或均小于0， 则表示该碰撞图 元对的两个图元不会发生碰撞， 若k0～k3其中一个等于0或非均大于0或非均小于0， 则执行 步骤S34； 步骤S34： 求解公式(1)的三阶贝兹曲线方程， 获取三阶贝兹曲线方程的类型， 其中所述 三阶贝兹曲线方程的公式如下： 当三阶贝兹曲线存在有一个拐点时， 为a型曲线； 当三阶贝兹曲线没有一个拐点但是有一个极值 点时， 为b型曲线； 当三阶贝兹曲线没有拐点与极值 点时， 为c型曲线； 步骤S35： 根据所述三阶贝兹曲线的类型判断所述碰撞图元对内的两个图元是否发生 共面， 其中是否共面的判断标准为该碰撞图元的三阶贝兹曲线 是有根， 若有根， 则表示碰撞 图元对内的两个图元发生共面， 其中共面判断过程如下： 构建常规的符号 函数sign()函数； 当三阶贝兹曲线的类型为a型曲线时， 从拐点处对三阶贝兹曲线进行分割， 分割成两段权　利　要　求　书 2/4 页 3 CN 115179326 A 3