(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210786379.8 (22)申请日 2022.07.04 (71)申请人 中国科学院沈阳自动化研究所 地址 110016 辽宁省沈阳市沈河区南塔街 114号 (72)发明人 姜勇　王洪光　王竣禾　 (74)专利代理 机构 沈阳科苑专利商标代理有限 公司 210 02 专利代理师 周宇 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/00(2006.01) (54)发明名称 一种位姿解耦的绝缘子更换机器人动态装 配方法 (57)摘要 本发明涉及一种位姿解耦的绝缘子更换机 器人动态装配方法。 包括： 步骤1： 通过深度相机 采集绝缘子表面点云图像， 定位销孔平面， 并提 取其法向量作为销孔轴线方向； 步骤2： 根据销孔 轴线方向和右 手定则， 确定机械臂末端的目标姿 态； 控制机械臂移动， 调整推销工具的目标姿态； 步骤3： 通过柔顺控制算法对位置进行调整， 使得 推销工具将 定位销推出销孔， 完成位姿解耦动态 装配。 权利要求书2页 说明书5页 附图1页 CN 114986515 A 2022.09.02 CN 114986515 A 1.一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在于， 用于在具有柔索特 性的输电线路上 更换绝缘子时， 将推销工具与销孔进行位姿解耦动态装配， 包括以下步骤： 步骤1： 通过深度相机采集绝缘子表面点云图像， 定位销孔平面， 并提取其法向量作为 销孔轴线方向； 步骤2： 根据销孔轴线方向和右手定则， 确定机械臂末端的目标姿态； 控制机械臂移动， 调整推销工具的目标姿态； 步骤3： 通过柔顺控制算法对位置进行调整， 使得推销工具将定位销推出销孔， 完成位 姿解耦动态装配。 2.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 所述深度相机采集的绝 缘子表面 点云图像中包括定位销孔。 3.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 所述定位销孔平面包括： 使用深度相机获取销孔一侧绝缘子表 面平整区域点云， 将该区 域点云法向量当作销孔轴线方向。 4.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 提取绝 缘子表面 点云法向量是采用PCA算法。 5.根据权利要求4所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 使用PCA方法对点云的法向量进行估计， 包括： a.定义点集的中心点记为m， 则有： 其中， 销孔 点云集合X＝{xi}； b.通过最小二乘法， 令点集中的所有点到 中心点的向量与法向量n的点积平方和最小， 并通过优化的方式求 解n， 优化目标为： 将xi‑m定义为yi， 可以将上式改写为： 令 yiyiT＝si 得到： 将上式中元素求和矩阵代替后， 可以得到： 其中S为点集的协方差矩阵， 其表示形式如下： 使用拉格朗日乘子法对带约束问题进行求 解， 则有：权　利　要　求　书 1/2 页 2 CN 114986515 A 2L(n, λ )＝ nTSn‑λ(nTn‑1) 分别令L对n和 λ求偏导， 并令其 等于0， 则有： 求得最终的n 为S矩阵中关于特 征值 λ 的特 征向量； 对S矩阵进行奇异值分解(SVD)， 即： S＝U∑VT 计算最小特征值对应的特 征向量， 即为法向量 n。 6.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 所述根据销孔轴线方向和右手定则， 确定 机械臂末端的目标姿态， 包括： a.将提取到的孔轴线方向单位 化， 将其定义 为目标姿态的z轴； b.找到任意 一个与z轴垂直的单位向量， 将其定义 为目标姿态的y轴； c.根据右手定则得到目标姿态x轴。 7.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 所述控制机 械臂移动调整推销工具的目标姿态包括： 计算机械臂末端连 杆坐标系的当前姿态到目标姿态之间的期望 旋转矩阵； 利用姿态控制算法按照期望旋转矩阵实现姿态调整； 所述姿态控制算法包括角度控 制， 速度控制； 当机械臂末端到达目标姿态时， 其末端的推销工具刚好对准销孔， 推销工具轴线与销 孔轴线一 致。 8.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法， 其特征在 于， 所述对位置进行调整是采用柔 顺控制算法实现推销工具向销孔方向的进给位置控制。 9.根据权利要求1 ‑8任意一项所述的一种位姿解耦的绝缘子更换机器人动态装配方 法， 其特征在于， 所述推销工具为在机 械臂末端安装的推杆。 10.一种位姿解耦的绝缘子更换机器人动态装配系统， 其特征在于， 包括： 深度相机、 控 制器、 机械臂； 所述深度相机， 用于采集 绝缘子销孔图像发送给控制器； 所述控制器， 用于根据销孔位置计算销孔轴线方向， 获取机械臂末端的目标姿态和位 置， 控制机 械臂带动末端的推销工具移动， 将定位销推出销孔； 所述机械臂， 末端安装有用于将定位销推出的推销工具。权　利　要　求　书 2/2 页 3 CN 114986515 A 3