(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210530735.X (22)申请日 2022.05.16 (71)申请人 华北电力大 学 （保定） 地址 071003 河北省保定市莲池区永华北 大街619号 华北电力大 学 (72)发明人 苑朝　赵亚冬　黄诺飞　徐大伟　 张耀　 (74)专利代理 机构 北京高沃 律师事务所 1 1569 专利代理师 韩雪梅 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01) (54)发明名称 一种排管电缆机 器人及其控制方法 (57)摘要 本发明涉及一种排管电缆机器人及其控制 方法， 控制方法包括： 建立相机坐标系、 惯 性坐标 系与世界坐标系之间的映射关系； 基于当前帧管 道图像和上一帧管道 图像得到当前帧移动距离 和当前帧旋转矩阵， 基于当前帧移动距离和拍摄 周期得到当前帧移动速度； 对当前帧管道图像进 行判断， 若不存在障碍物， 则基于当前帧移动距 离和映射关系得到当前帧世界坐标位置； 基于当 前帧世界坐标位置、 当前帧移动速度、 给定位置 和给定速度控制机器人直线运动； 若存在障碍 物， 则基于当前帧移动距离和拍摄周期得到当前 帧移动速度； 基于当前帧旋转矩阵、 当前帧移动 速度、 给定姿态角和给定速度控制机器人旋转运 动。 本发明实现了高精度控制机器人完成直线和 旋转运动。 权利要求书2页 说明书6页 附图4页 CN 114851197 A 2022.08.05 CN 114851197 A 1.一种排管电缆机器人的控制方法， 其特 征在于， 包括： 步骤S1， 建立相机坐标系 、 惯性坐标系与世界坐标系之间的映射关系； 步骤S2， 基于当前帧管道图像和上一帧管道图像得到当前帧移动距离和当前帧旋转矩 阵， 对所述当前帧管道图像进 行判断， 若不存在障碍物， 则执行步骤S 3， 若存在障碍物， 则执 行步骤S4； 步骤S3， 基于所述当前帧移动距离和拍摄周期得到当前帧移动速度， 基于所述当前帧 移动距离和所述映射关系得到当前帧世界坐标位置； 基于所述当前帧世界坐标位置、 所述 当前帧移动速度、 给定位置和给定 速度控制机器人直线运动； 步骤S4， 基于所述当前帧移动距离和所述拍摄周期得到当前帧移动速度； 基于所述当 前帧旋转矩阵、 所述当前帧移动速度、 给定姿态角和所述给定 速度控制机器人旋转 运动。 2.根据权利要求1所述的排管电缆机器人的控制方法， 其特 征在于， 所述 步骤S2包括： 对第一帧管道图像和第二帧管道图像进行进行匹配， 得到点匹配特 征和线匹配特 征； 基于所述 点匹配特 征和所述线匹配特 征， 采用对极约束法得到第二帧初始 移动距离； 基于惯性测量单元获取第 二帧实际移动距离， 将所述第 二帧实际移动距离和所述第 二 帧初始移动距离相除， 得到误差比值； 基于所述当前帧管道图像和所述上一帧管道图像得到当前帧初始移动距离和所述当 前帧初始旋转矩阵； 将所述当前帧初始 移动距离和所述 误差比值相乘得到当前帧优化移动距离； 基于惯性测量单 元获取当前帧实际移动距离； 基于所述当前帧实际移动距离、 所述当前帧优化移动距离和所述当前帧初始旋转矩阵 进行卡尔曼 滤波， 得到所述当前帧移动距离和所述当前帧旋转矩阵。 3.根据权利要求1所述的排管电缆机器人的控制方法， 其特征在于， 所述控制机器人直 线运动采用双闭环控制方式， 内环速度环， 外环为 位置环； 将所述给定位置和所述当前帧世界坐标位置做差， 得到位置差； 将所述位置差输入PID 控制器， 得到位置控制输出量； 基于所述给定速度和所述当前帧移动速度做差， 得到速度差； 将所述速度差输入PID控 制器， 得到 速度控制输出量； 将所述位置控制输出量进行限幅后与所述速度控制输出量进行积分， 得到直线控制输 出， 基于所述 直线控制输出控制机器人直线运动。 4.根据权利要求1所述的排管电缆机器人的控制方法， 其特征在于， 所述控制机器人旋 转运动采用双闭环控制方式， 内环为速度环， 外环为角度环； 基于所述当前帧旋转矩阵得到当前帧姿态角； 将所述给定姿态角和当前帧姿态角做差， 得到角度差； 将所述角度差输入PID控制器， 得到角度控制输出量； 基于所述给定速度和所述当前帧移动速度做差， 得到速度差； 将所述速度差输入PID控 制器， 得到 速度控制输出量； 将所述角度控制输出量进行限幅后与所述速度控制输出量进行积分， 得到旋转控制输 出， 基于所述旋转控制输出控制机器人旋转 运动。 5.一种排管电缆机器人， 所述排管电缆机器人基于权利要求1 ‑4任意一项所述的控制权　利　要　求　书 1/2 页 2 CN 114851197 A 2方法进行控制， 其特征在于， 所述排管电缆机器人包括支撑架、 前进轮、 旋转轮、 相机、 惯性 测量单元、 控制器和温度传感器； 所述相机设置在所述支撑架的前端部， 所述相机用于拍摄管道图像； 所述温度传感器设置在所述支撑架上， 所述温度传感器用于实时获取电缆的温度信 息 并发送至所述控制器； 所述惯性测量单 元设置在所述支撑架的内部， 所述惯性测量单 元用于测量移动距离； 所述前进轮和所述旋转轮均设置在所述支撑架的上端部， 且所述前进轮和所述旋转轮 垂直设置； 所述控制器基于所述前进轮带动所述支撑架直线运动； 所述控制器基于所述旋转轮带 动所述支撑架旋转 运动。 6.根据权利要求5所述的排管电缆机器人， 其特征在于， 所述排管电缆机器人还包括第 一电机和第二电机； 所述第一电机的输出轴与 所述前进轮 固定连接； 所述第 二电机的输出轴与 所述旋转轮 固定连接； 所述控制器驱动所述第一电机转动， 继而带动所述前进轮转动， 所述前进轮转动带动 所述支撑架直线运动； 所述控制器驱动所述第二电机转动， 继而带动所述旋转轮转动， 所述旋转轮转动带动 所述支撑架旋转 运动。 7.根据权利要求5所述的排管电缆机器人， 其特征在于， 所述排管电缆机器人还包括若 干个从动轮； 若干个所述从动轮均匀分布在所述支撑架的两侧。 8.根据权利要求7所述的排管电缆机器人， 其特征在于， 所述前进轮、 所述旋转轮和各 所述从动轮均采用福来轮。权　利　要　求　书 2/2 页 3 CN 114851197 A 3