破拆机器人的控制系统、方法、破拆机器人以及介质与流程

本发明涉及工程机械，尤其涉及一种破拆机器人的控制系统、控制方法、破拆机器人以及存储介质。

背景技术：

1、近年来，针对于某一特定领域、特定用途设计的机器人(特种机器人)得以快速发展。特种机器人具备工程机械的工程属性，可用于防爆、消防、挖掘、破拆等方面。破拆机器人是一种应用较为广泛的特种机器人。目前，针对于复杂应用环境(如矿洞、震后、钢铁厂等)的破拆机器人，因环境和自身结构等原因和限制，自动化程度并不高。其臂架的自由度多，且无法按照机器人学得出解析解，只能由经验丰富的操作手，肉眼观察与判断，依次操作遥控手柄，实现对末端破拆装置的定位，工作效率低；同时在运动和破拆过程中，操作手和机器人本身无法获得臂架的关节受力情况，易发生过大的受力损伤本体的事故。

技术实现思路

1、有鉴于此，本发明要解决的一个技术问题是提供一种破拆机器人的控制系统、控制方法、破拆机器人以及存储介质。

2、根据本公开的第一方面，提供一种破拆机器人的控制系统，其中，所述破拆机器人包括：回转基座、机械臂和破拆头；所述回转基座安装在底盘上；所述机械臂由依次铰接的多个连杆组成，在所述连杆上设置有液压缸，用以控制所述连杆的运动；所述机械臂的一端与回转基座铰接，另一端安装所述破拆头；所述控制系统包括：期望轨迹生成模块，用于根据所述破拆头末端的当前位置以及待破拆目标的位置，确定所述破拆头末端的期望位置；逆运动学求解模块，用于使用具有约束条件的逆运动学求解方法对连杆之间的关节角进行求解，并基于所述关节角确定所述液压缸的第一长度；环境交互模块，用于获取倾角传感器采集的所述连杆的倾角，基于所述倾角与关节角的变换关系、以及所述关节角与所述液压缸长度的变换关系，获得所述液压缸的第二长度；反馈控制模块，用于基于所述液压缸的第一长度和第二长度之间的差值，对所述液压缸的活塞杆的运动进行控制。

3、可选地，所述期望轨迹生成模块，包括：工具姿态调整单元，用于基于视觉传感器采集的数据，获得所述待破拆目标的破拆面法线，将所述破拆头的轴线调整至与所述破拆面法线重合或平行；末端期望位置输入单元，用于基于视觉传感器采集的数据获得所述待破拆目标的位置，根据所述破拆头末端的当前位置以及待破拆目标的位置进行空间轨迹规划处理，确定所述破拆头末端的期望位置。

4、可选地，所述期望轨迹生成模块，还包括：末端位置初始化单元，用于建立与所述破拆机器人相对应的多个d-h坐标系并确定与所述破拆机器人相对应的d-h参数；根据所述d-h参数和所述多个坐标系确定d-h坐标系之间的变换矩阵。

5、可选地，所述破拆机器人为五自由度破拆机器人；所述多个连杆的数量为四个，各个连杆之间的相对转动位于同一平面内；所述逆运动学求解模块，包括：腰部逆解单元，用于根据所述破拆头末端的期望位置和所述变换矩阵，确定所述回转基座与底盘之间的转动角度θ1。

6、可选地，所述机械臂包括：铰接在回转基座上的第二连杆、铰接在所述第二连杆一端的第三连杆、铰接在所述第三连杆一端的第四连杆和铰接在第四连杆一端的第五连杆；所述第五连杆的一端安装所述破拆头；所述第二连杆与所述回转基座之间的关节角为θ2，所述第二连杆与所述第三连杆之间的关节角为θ3，所述第三连杆与所述第四连杆之间的关节角为θ4，所述第四连杆与所述第五连杆之间的关节角为θ5；θ5为固定值；所述逆运动学求解模块，包括：机械臂三关节逆解单元，用于根据所述破拆头末端的期望位置、所述变换矩阵以及约束条件，分别进行在θ2已知的情况下求解θ3和θ4、在θ3已知的情况下求解θ2和θ4、在θ4已知的情况下求解θ2和θ3，用以获得θ2、θ3、θ4；其中，所述约束条件包括：所述破拆头与所述破拆面法线的法线重合或平行，并且所述液压缸对于转动关节的力臂总和最大。

7、可选地，所述逆运动学求解模块，包括：液压缸长度求解单元，用于根据θ2、θ3、θ4和θ5并基于关节角和液压缸长度之间的变换关系，获得所述液压缸的第一长度。

8、可选地，所述反馈控制模块，用于基于所述液压缸的第一长度和第二长度之间的差值，使用pid控制或模型预测控制策略对所述液压缸的活塞杆的运动进行控制。

9、根据本公开的第二方面，提供一种破拆机器人的控制方法，其中，所述破拆机器人包括：回转基座、机械臂和破拆头；所述回转基座安装在底盘上；所述机械臂由依次铰接的多个连杆组成，在所述连杆上设置有液压缸，用以控制所述连杆的运动；所述机械臂的一端与回转基座铰接，另一端安装所述破拆头；所述方法包括：根据所述破拆头末端的当前位置以及待破拆目标的位置，确定所述破拆头末端的期望位置；使用具有约束条件的逆运动学求解方法对连杆之间的关节角进行求解，并基于所述关节角确定所述液压缸的第一长度；获取倾角传感器采集的所述连杆的倾角，基于所述倾角与关节角的变换关系、以及所述关节角与所述液压缸长度的变换关系，获得所述液压缸的第二长度；基于所述液压缸的第一长度和第二长度之间的差值，对所述液压缸的活塞杆的运动进行控制。

10、可选地，所述根据所述破拆头末端的当前位置以及待破拆目标的位置，确定所述破拆头末端的期望位置包括：基于视觉传感器采集的数据，获得所述待破拆目标的破拆面法线，将所述破拆头的轴线调整至与所述破拆面法线重合或平行；基于视觉传感器采集的数据获得所述待破拆目标的位置，根据所述破拆头末端的当前位置以及待破拆目标的位置进行空间轨迹规划处理，确定所述破拆头末端的期望位置。

11、可选地，建立与所述破拆机器人相对应的多个d-h坐标系并确定与所述破拆机器人相对应的d-h参数；根据所述d-h参数和所述多个坐标系确定d-h坐标系之间的变换矩阵。

12、可选地，所述破拆机器人为五自由度破拆机器人；所述多个连杆的数量为四个，各个连杆之间的相对转动位于同一平面内；所述使用具有约束条件的逆运动学求解方法对连杆之间的关节角进行求解包括：根据所述破拆头末端的期望位置和所述变换矩阵，确定所述回转基座与底盘之间的转动角度θ1。

13、可选地，所述机械臂包括：铰接在回转基座上的第二连杆、铰接在所述第二连杆一端的第三连杆、铰接在所述第三连杆一端的第四连杆和铰接在第四连杆一端的第五连杆；所述第五连杆的一端安装所述破拆头；所述第二连杆与所述回转基座之间的关节角为θ2，所述第二连杆与所述第三连杆之间的关节角为θ3，所述第三连杆与所述第四连杆之间的关节角为θ4，所述第四连杆与所述第五连杆之间的关节角为θ5；θ5为固定值；所述使用具有约束条件的逆运动学求解方法对连杆之间的关节角进行求解包括：根据所述破拆头末端的期望位置、所述变换矩阵以及约束条件，分别进行在θ2已知的情况下求解θ3和θ4、在θ3已知的情况下求解θ2和θ4、在θ4已知的情况下求解θ2和θ3，用以获得θ2、θ3、θ4；其中，所述约束条件包括：所述破拆头与所述破拆面法线的法线重合或平行，并且所述液压缸对于转动关节的力臂总和最大。

14、可选地，所述基于所述关节角确定所述液压缸的第一长度包括：根据θ2、θ3、θ4和θ5并基于关节角和液压缸长度之间的变换关系，获得所述液压缸的第一长度。

15、可选地，所述基于所述液压缸的第一长度和第二长度之间的差值，对所述液压缸的活塞杆的运动进行控制包括：基于所述液压缸的第一长度和第二长度之间的差值，使用pid控制或模型预测控制策略对所述液压缸的活塞杆的运动进行控制。

16、根据本公开的第三方面，提供一种破拆机器人的控制系统，包括：存储器；以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行如上所述的方法。

17、根据本公开的第四方面，提供一种破拆机器人，包括：如上所述的破拆机器人的控制系统。

18、根据本公开的第五方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述指令被处理器执行如上所述的方法。

19、本公开的破拆机器人的控制系统、控制方法、破拆机器人以及存储介质，能够实现对破拆机器人的自动控制，提高了破拆工作效率；通过使用具有约束条件的逆运动学求解方法进行平面多自由度臂的逆解求解，能够有效保护机器人本体结构，提高了破拆工作的安全性，并与原控制系统具有良好的兼容性，提高了用户的使用感受度。