[发明专利]一种钻杆装卸控制方法、装置及钻杆装卸设备

权利要求书

1.一种钻杆装卸控制方法，应用于钻杆装卸设备，其特征在于，所述方法包括：

获取所述钻杆装卸设备的当前位置、当前位姿以及环境图像；

基于所述环境图像，确定目标钻杆的目标位置及所述钻杆装卸设备在所述目标钻杆上预设抓取点进行抓取的目标位姿；

基于所述环境图像、所述当前位姿及所述目标位姿，确定所述钻杆装卸设备从所述当前位置移动到所述目标位置的规划路径，所述规划路径包括所述钻杆装卸设备的移动路线及所述钻杆装卸设备在所述移动路线上的移动位姿；

基于所述钻杆装卸设备在所述移动路线上的移动位姿的变化情况，在所述移动路线上确定位姿控制节点及其对应的位姿变换指令；

基于所述移动路线、所述位姿控制节点及其对应的位姿变换指令控制所述钻杆装卸设备移动至所述目标位置抓取所述目标钻杆进行装杆或卸杆。

2.根据权利要求1所述的方法，其特征在于，所述基于所述环境图像、所述当前位姿及所述目标位姿，确定所述钻杆装卸设备从所述当前位置移动到所述目标位置的规划路径，包括：

基于所述环境图像，确定所述当前位置到所述目标位置间的障碍物信息；

基于所述障碍物信息及所述钻杆装卸设备的预设控制要求，确定所述钻杆装卸设备从所述当前位置移动到所述目标位置的各候选规划路径；

分别计算所述钻杆装卸设备执行各候选规划路径的总时长；

基于所述总时长从小到大的排序结果，确定规划路径。

3.根据权利要求2所述的方法，其特征在于，所述候选规划路径包括候选移动路线及所述钻杆装卸设备在所述候选移动路线上的移动位姿，所述计算所述钻杆装卸设备执行各候选规划路径的总时长，包括：

获取所述当前候选移动路线的路线长度及所述钻杆装卸设备的预设移动速度；

基于所述路线长度及所述预设移动速度，确定所述钻杆装卸设备执行当前候选移动路线的移动时长；

基于所述当前位姿、目标位姿及当前候选移动路线上的移动位姿，计算所述钻杆装卸设备执行当前候选移动路线的位姿调整时长；

基于所述移动时长和所述位姿调整时长，确定所述钻杆装卸设备执行所述当前候选规划路径的总时长。

4.根据权利要求1所述的方法，其特征在于，所述基于所述钻杆装卸设备在所述移动路线上的移动位姿的变化情况，在所述移动路线上确定位姿控制节点及其对应的位姿变换指令，包括：

沿所述移动路线方向判断当前移动位姿的变换方向与上一移动位姿的变换方向是否一致；

在当前移动位姿的变换方向与上一移动位姿的变换方向不一致时，将所述当前移动位姿对应的所述移动路线上的位置确定为位姿控制节点；

基于所述当前移动位姿的变换方向生成所述位姿控制节点对应的位姿变换指令。

5.根据权利要求1所述的方法，其特征在于，所述基于所述移动路线、所述位姿控制节点及其对应的位姿变换指令控制所述钻杆装卸设备移动至所述目标位置抓取所述目标钻杆进行装杆或卸杆，包括：

基于预设移动速度控制所述钻杆装卸设备按照所述移动路线移动；

在所述钻杆装卸设备到达所述位姿控制节点时，控制所述钻杆装卸设备暂停移动；

基于所述位姿变换指令控制所述钻杆装卸设备进行位姿变换，并在位姿变换完成后继续控制所述钻杆装卸设备按照所述移动路线移动，直至所述钻杆装卸设备以所述目标位姿到达所述目标位置；

基于预设装卸指令控制所述钻杆装卸设备抓取所述目标钻杆进行装杆或卸杆。

6.根据权利要求1所述的方法，其特征在于，所述基于所述环境图像，确定目标钻杆的目标位置及所述钻杆装卸设备在所述目标钻杆上预设抓取点进行抓取的目标位姿，包括：

对所述环境图像进行图像识别，确定所述目标钻杆在所述环境图像中的位置；

基于所述目标钻杆在所述环境图像中的位置以及所述环境图像所在图像坐标系与世界坐标系的预设转换关系，确定所述目标位置；

基于所述目标位置确定所述目标钻杆上预设抓取点的空间位置，并基于所述空间位置确定所述钻杆装卸设备在预设抓取点进行抓取的目标姿态。