(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210643004.6 (22)申请日 2022.06.08 (71)申请人 哈尔滨理工大 学 地址 150080 黑龙江省哈尔滨市南岗区学 府路52号 (72)发明人 黄成　刘亚　 (74)专利代理 机构 北京圣州专利代理事务所 (普通合伙) 11818 专利代理师 黄青青 (51)Int.Cl. G06T 7/215(2017.01) G06T 7/73(2017.01) G06V 10/26(2022.01) G06V 10/762(2022.01) G06V 10/774(2022.01)G06V 10/82(2022.01) G06N 3/04(2006.01) G06N 3/08(2006.01) B25J 18/00(2006.01) (54)发明名称 一种基于ROS的非合作目标抓取控制系统 (57)摘要 本发明公开了一种基于ROS的非合作目标抓 取控制系统， 包括以下步骤： 使用位姿数据集制 作软件采集、 制作非合作的位姿数据集， 对位姿 数据集进行预训练， 得到权重数据； 采集非合作 目标所在区域对齐的RGBD图像数据构建运动场 景和进行位姿估计， 结合训练过的数据集和权重 数据处理RGBD图像数据； 把所要捕获的非合作目 标的位姿数据送入空间机械臂轨迹规划功能进 行轨迹规划， 通过插值方式拟合出一条平滑的轨 迹曲线； 控制机械臂依次通过这些路径点， 当机 械臂通过最后一个路径点后， 闭合夹爪， 实现对 空间非合作目标的捕获。 本发明解决了现有捕获 方式对进行维修或补充燃料的航天器极易造成 损坏的问题， 适应性强、 应用面广、 易于控制、 成 本低且能够重复利用的优点。 权利要求书2页 说明书4页 附图2页 CN 115018876 A 2022.09.06 CN 115018876 A 1.一种基于ROS的非合作目标抓取控制系统， 其特 征在于， 包括以下步骤： S1、 利用深度相机和位姿数据集制作软件采集、 制作非合作的位姿数据集， 并且利用深 度学习网络对位姿数据集进行 预训练， 得到 权重数据； S2、 利用深度相机采集非合作目标所在区域对齐的RGBD图像数据构建运动场景， 包括 构建障碍物地图(Octomap)和规划场景(Planning  Scene)， 并根据RGBD图像数据实时更新 运动场景， 为轨 迹规划提供碰撞检测 和约束检测； S3、 利用深度相机采集非合作目标所在区域对齐的RGBD图像数据进行位姿估计， 将 RGBD图像数据 送入深度学习网络， 结合步骤S1 中训练过的数据集和权重数据处理深度相机 采集的RGBD图像数据， 以得到所要捕获的非合作目标的位姿数据(位置数据和姿态数据)； S4、 把步骤S3中得到的所要捕获的非合作目标的位置数据和姿态数据送入运动场景， 剔除障碍物地图中非合作目标的障碍物标记， 更新规划 场景， 进行轨迹规划， 以得到机械臂 所要经过的初始路径点， 把初始路径点送入轨迹优化功能进行轨迹优化， 通过插值的方式 拟合出一条平 滑的轨迹曲线； S5、 根据步骤S4中得到的优化后的轨迹， 控制机械臂末端执行器依次通过这些路径点， 当机械臂末端执 行器通过最后一个路径点后， 闭合夹爪， 实现对空间非合作目标的捕获。 2.根据权利要求1所述的一种基于ROS的非合作目标抓取控制系统， 其特征在于： 所述 步骤S1中非合作目标的位姿数据集的制作和预训练过程具体为使用深度相 机采集非合作 目标对齐后的RGBD图像数据， 送入ObjectDatasetTools软件裁剪出非合作目标的点云数据 并作为标准点云数据， 围绕非合作目标放置好标记二维码后， 稳定且缓慢地移动深度相 机 录制位姿数据集， 并将制作完成的位姿数据集送入深度学习网络进行预训练， 得到权重数 据。 3.根据权利要求1所述的一种基于ROS的非合作目标抓取控制系统， 其特征在于： 所述 步骤S2中构建运动场景， 以获取环 境中障碍物信息具体为利用深度相机采集空间非合作目 标所在区域对齐的RGBD图像数据， 将Depth图像数据送入八叉树地图(Octomap)以八叉树的 数据形式构建出相 机视场内的障碍物分布地图， 形成立体的障碍物地图(非合作目标被标 记为障碍物)， 使用碰撞检测算法(FCL)为轨迹规划提供碰撞检测和约束检测， 包括 自我碰 撞检测、 环 境碰撞检测， 同步构建规划场景(Planning  Scene)， 为轨迹规划提供运动学约束 检测。 4.根据权利要求1所述的一种基于ROS的非合作目标抓取控制系统， 其特征在于： 所述 步骤S3中对非合作目标进 行位姿估计， 以获取非合作目标的位姿信息具体为利用深度相机 采集空间非合作目标所在区域对齐的RGBD图像数据， 将RGBD图像数据 送入加载了预训练数 据集和权重数据的深度学习网络， 将RGB图像数据 送入卷积神经网络(CNN)用于提取图像中 每个像素的表面特征， 将Depth图像数据送入点云神经网络(PointNet++)用于提取每个像 素的几何特征， 将提取到的每个像素的表面特征与几何特征送入像素级的异构融合网络 (Densefusion)融合两种特征， 融合特征后的数据分别送入3D关键点检测模块、 实例语义分 割模块， 然后使用聚类算法和霍夫投票算法实现非合作目标上的点为非合作目标上的关键 点投票， 最后使用最小二乘法拟合关键点， 估计出非合作目标的姿态数据， 最后利用ROS系 统消息发布机制广播出去非合作目标的位姿信息 。 5.根据权利要求 4所述的一种基于ROS的非合作目标抓取控制系统， 其特 征在于：权　利　要　求　书 1/2 页 2 CN 115018876 A 2所述3D关键点检测模块使用聚类算法选出投票点并将聚类后集群点的中心作为投票 的关键点， 使用最远采样点 算法和霍夫投票 算法选出3D关键点； 所述实例语义分割模块包括语义分割 模块和中心 投票模块两部分， 语义分割 模块是把 融合特征后的像素数据标记语义标签， 实现语义分割不同的物体并根据加载的预训练数据 集和权重数据找到非合作目标， 中心投票模块是对融合特征后的像素进行投票， 预测出不 同物体的中心点， 其中包括非合作目标的中心点即为 位置数据。 6.根据权利要求1所述的一种基于ROS的非合作目标抓取控制系统， 其特征在于： 所述 步骤S4中对机械臂 抓取非合作目标进 行轨迹规划， 以规划一条平滑的轨迹曲线 具体为轨迹 规划功能根据ROS系统消息发布机制订阅并接收到非合作目标的位姿信息， 将步骤S3中得 到的所要捕获非合作目标 的位置数据和姿态数据运动场景和机械臂轨迹规划功 能进行轨 迹规划， 然后对蚁群算法规划出 的一条从起始点到目标点且实现避障的轨迹进行优化， 利 用三次B‑样条插值曲线方法对以蚁群算法规划的轨迹进行轨迹优化， 得到平滑的机械臂空 间避障轨 迹。 7.根据权利要求1所述的一种基于ROS的非合作目标抓取控制系统， 其特征在于： 所述 步骤S5中实现对非合作目标的捕获具体为将步骤S4中得到的优化后的轨迹依次输入执行 器， 控制机械臂末端执行器依 次通过这些路径点， 当执行机构通过最后一个路径点时即可 执行捕获操作， 当夹取机构的力学传感器返回的数据达 到阈值时， 即为捕获完成。权　利　要　求　书 2/2 页 3 CN 115018876 A 3