(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210433946.1 (22)申请日 2022.04.24 (71)申请人 中国铁建重 工集团股份有限公司 地址 410100 湖南省长 沙市长沙经济技 术 开发区东七 线88号 (72)发明人 刘飞香　秦念稳　肖正航　李正光　 杨孪山　 (74)专利代理 机构 北京集佳知识产权代理有限 公司 11227 专利代理师 耿苑 (51)Int.Cl. B25J 9/16(2006.01) B25J 13/00(2006.01) B25J 13/08(2006.01) (54)发明名称 一种隧道环境下机械臂运动轨迹的规划方 法及相关装置 (57)摘要 本申请公开了一种隧道环境下机械臂运动 轨迹的规划方法， 包括： 构建碰撞检测模 型； 通过 碰撞检测模型对机械臂的起始位姿与 目标位姿 进行碰撞检测； 当起始位姿与目标位姿均无碰撞 时， 调用关节极限表达式在关节空间中进行采 样， 得到采样点； 通过碰撞检测模型对采样点进 行碰撞检测， 并保留无碰撞的采样点； 根据起始 位姿、 目标位姿以及无碰撞的采样点， 规划得到 无碰撞的机械臂运动轨迹， 对机械臂运动轨迹进 行优化与平滑处理， 以及对机械臂运动轨迹进行 时间参数化处理。 该方法能够 有效避免碰撞情况 的发生， 可以适应复杂臂架构型导致的关节极限 耦合变化的运动学约束情况， 保证求解结果正确 有效， 并且可 以使机械臂运动连续平顺， 降低能 耗。 权利要求书2页 说明书9页 附图2页 CN 114633258 A 2022.06.17 CN 114633258 A 1.一种隧道 环境下机 械臂运动轨 迹的规划方法， 其特 征在于， 包括： 构建碰撞检测模型； 通过所述碰撞检测模型对机 械臂的起始位姿与目标位姿 进行碰撞检测； 当所述起始位姿与 所述目标位姿均 无碰撞时， 调用关节极限表达 式在关节 空间中进行 采样， 得到多个采样点； 所述关节极限表达式为多 项式； 通过所述碰撞检测模型对所述采样点进行碰撞检测， 并保留无碰撞的所述采样点； 根据所述起始位姿、 所述目标位姿以及无碰撞的所述采样点， 规划得到无碰撞的机械 臂运动轨 迹； 对所述机 械臂运动轨 迹进行优化与平 滑处理， 得到最优的机 械臂运动轨 迹； 对最优的机械臂运动轨迹进行时间参数化处理， 得到所述最优的机械臂运动轨迹的时 间点、 速度以及加速度信息 。 2.根据权利要求1所述的规划方法， 其特 征在于， 所述构建碰撞检测模型包括： 获取碰撞检测信息； 所述碰撞检测信息包括用于代替机械臂连杆的包围盒模型、 台车 到大地的转换矩阵、 掌子面到台车的转换矩阵、 隧道设计轮廓或隧道点云轮廓、 臂架模型参 数； 根据所述碰撞检测信息， 构建所述碰撞检测模型。 3.根据权利要求1所述的规划方法， 其特征在于， 所述根据所述起始位姿、 所述目标位 姿以及无碰撞的所述采样点， 规划得到无碰撞的机 械臂运动轨 迹包括： 利用快速搜索随机树算法， 从所述起始位姿出发在各无碰撞的所述采样点构 成的空间 中向所述目标位姿搜索， 得到多条局部路径； 通过所述碰撞检测模型对所述局部路径进行碰撞检测， 并保留无碰撞的所述局部路 径； 根据无碰撞的所述局部路径， 规划得到无碰撞的所述机 械臂运动轨 迹。 4.根据权利要求1所述的规划方法， 其特征在于， 对所述机械臂运动轨迹进行轨迹优化 包括： 采样得到所述机 械臂运动轨 迹中所述采样点的无碰撞替代点； 判断所述无碰撞替代点是否满足预设替代条件； 若所述无碰撞替代点满足所述替代条件， 则使用所述无碰撞替代点替代对应的所述采 样点； 若所述无碰撞替代点 不满足所述 替代条件， 则保持所述采样点 不变。 5.根据权利要求1所述的规划方法， 其特征在于， 对所述机械臂运动轨迹进行平滑处 理， 得到平 滑的机械臂运动轨 迹包括： 取所述机械臂运动轨迹中相邻两点的中点， 并依次连接所述起始位姿、 各所述中点以 及所述目标位姿， 得到平 滑的机械臂运动轨 迹。 6.一种隧道 环境下机 械臂运动轨 迹的规划装置， 其特 征在于， 包括： 检测模型构建模块， 用于构建碰撞检测模型； 第一检测模块， 用于通过所述碰撞检测模型对机械臂的起始位姿与目标位姿进行碰撞 检测； 采样模块， 用于当所述起始位姿与所述目标位姿均无碰撞时， 调用关节极限表达式在权　利　要　求　书 1/2 页 2 CN 114633258 A 2关节空间中进行采样， 得到多个采样点； 所述关节极限表达式为多 项式； 第二检测模块， 用于通过所述碰撞检测模型对所述采样点进行碰撞检测， 并保留无碰 撞的所述采样点； 轨迹规划模块， 用于根据 所述起始位姿、 所述目标位姿以及无碰撞的所述采样点， 规划 得到无碰撞的机 械臂运动轨 迹； 轨迹处理模块， 用于对所述机械臂运动轨迹进行优化与平滑处理， 得到最优的机械臂 运动轨迹； 时间参数化模块， 用于对最优的机械臂运动轨迹进行时间参数化处理， 得到所述最优 的机械臂运动轨 迹的时间点、 速度以及加速度信息 。 7.根据权利要求6所述的规划装置， 其特 征在于， 所述轨 迹规划模块包括： 局部路径构建单元， 用于利用快速搜索随机树算法， 从所述起始位姿出发在各无碰撞 的所述采样点构成的空间中向所述目标位姿搜索， 得到多条局部路径； 碰撞检测单元， 用于通过所述碰撞检测模型对所述局部路径进行碰撞检测， 并保留无 碰撞的所述局部路径； 运动轨迹规划单元， 用于根据无碰撞的所述局部路径， 规划得到无碰撞的所述机械臂 运动轨迹。 8.根据权利要求6所述的规划装置， 其特 征在于， 所述轨 迹处理模块包括： 中点选取 单元， 用于取 所述机械臂运动轨 迹中相邻两点的中点； 连接单元， 用于依次连接所述起始位姿、 各所述中点以及所述目标位姿， 得到平滑的机 械臂运动轨 迹。 9.一种隧道 环境下机 械臂运动轨 迹的规划设备， 其特 征在于， 包括： 存储器， 用于存 储计算机程序； 处理器， 用于执行所述计算机程序时实现如权利要求1至5任一项所述的隧道环境下机 械臂运动轨 迹的规划方法的步骤。 10.一种计算机可读存储介质， 其特征在于， 所述计算机可读存储介质上存储有计算机 程序， 所述计算机程序被处理器执行时实现如权利要求 1至5任一项 所述的隧道环境下机械 臂运动轨 迹的规划方法的步骤。权　利　要　求　书 2/2 页 3 CN 114633258 A 3