[發明專利]一種鉆桿裝卸控制方法、裝置及鉆桿裝卸設備

在審

• 申請號: 202110477621.9
• 申請日: 2021-04-29
• 公開（公告）號: CN113062697A
• 公開（公告）日: 2021-07-02
• 發明（設計）人: 宋長法
• 申請（專利權）人: 北京三一智造科技有限公司
• 主分類號: E21B19/16
• 分類號: E21B19/16
• 代理公司: 北京三聚陽光知識產權代理有限公司 11250
• 代理人: 胡曉靜

1.一種鉆桿裝卸控制方法，應用于鉆桿裝卸設備，其特征在于，所述方法包括：

獲取所述鉆桿裝卸設備的當前位置、當前位姿以及環境圖像；

基于所述環境圖像，確定目標鉆桿的目標位置及所述鉆桿裝卸設備在所述目標鉆桿上預設抓取點進行抓取的目標位姿；

基于所述環境圖像、所述當前位姿及所述目標位姿，確定所述鉆桿裝卸設備從所述當前位置移動到所述目標位置的規劃路徑，所述規劃路徑包括所述鉆桿裝卸設備的移動路線及所述鉆桿裝卸設備在所述移動路線上的移動位姿；

基于所述鉆桿裝卸設備在所述移動路線上的移動位姿的變化情況，在所述移動路線上確定位姿控制節點及其對應的位姿變換指令；

基于所述移動路線、所述位姿控制節點及其對應的位姿變換指令控制所述鉆桿裝卸設備移動至所述目標位置抓取所述目標鉆桿進行裝桿或卸桿。

2.根據權利要求1所述的方法，其特征在于，所述基于所述環境圖像、所述當前位姿及所述目標位姿，確定所述鉆桿裝卸設備從所述當前位置移動到所述目標位置的規劃路徑，包括：

基于所述環境圖像，確定所述當前位置到所述目標位置間的障礙物信息；

基于所述障礙物信息及所述鉆桿裝卸設備的預設控制要求，確定所述鉆桿裝卸設備從所述當前位置移動到所述目標位置的各候選規劃路徑；

分別計算所述鉆桿裝卸設備執行各候選規劃路徑的總時長；

基于所述總時長從小到大的排序結果，確定規劃路徑。

3.根據權利要求2所述的方法，其特征在于，所述候選規劃路徑包括候選移動路線及所述鉆桿裝卸設備在所述候選移動路線上的移動位姿，所述計算所述鉆桿裝卸設備執行各候選規劃路徑的總時長，包括：

獲取所述當前候選移動路線的路線長度及所述鉆桿裝卸設備的預設移動速度；

基于所述路線長度及所述預設移動速度，確定所述鉆桿裝卸設備執行當前候選移動路線的移動時長；

基于所述當前位姿、目標位姿及當前候選移動路線上的移動位姿，計算所述鉆桿裝卸設備執行當前候選移動路線的位姿調整時長；

基于所述移動時長和所述位姿調整時長，確定所述鉆桿裝卸設備執行所述當前候選規劃路徑的總時長。

4.根據權利要求1所述的方法，其特征在于，所述基于所述鉆桿裝卸設備在所述移動路線上的移動位姿的變化情況，在所述移動路線上確定位姿控制節點及其對應的位姿變換指令，包括：

沿所述移動路線方向判斷當前移動位姿的變換方向與上一移動位姿的變換方向是否一致；

在當前移動位姿的變換方向與上一移動位姿的變換方向不一致時，將所述當前移動位姿對應的所述移動路線上的位置確定為位姿控制節點；

基于所述當前移動位姿的變換方向生成所述位姿控制節點對應的位姿變換指令。

5.根據權利要求1所述的方法，其特征在于，所述基于所述移動路線、所述位姿控制節點及其對應的位姿變換指令控制所述鉆桿裝卸設備移動至所述目標位置抓取所述目標鉆桿進行裝桿或卸桿，包括：

基于預設移動速度控制所述鉆桿裝卸設備按照所述移動路線移動；

在所述鉆桿裝卸設備到達所述位姿控制節點時，控制所述鉆桿裝卸設備暫停移動；

基于所述位姿變換指令控制所述鉆桿裝卸設備進行位姿變換，并在位姿變換完成后繼續控制所述鉆桿裝卸設備按照所述移動路線移動，直至所述鉆桿裝卸設備以所述目標位姿到達所述目標位置；

基于預設裝卸指令控制所述鉆桿裝卸設備抓取所述目標鉆桿進行裝桿或卸桿。

6.根據權利要求1所述的方法，其特征在于，所述基于所述環境圖像，確定目標鉆桿的目標位置及所述鉆桿裝卸設備在所述目標鉆桿上預設抓取點進行抓取的目標位姿，包括：

對所述環境圖像進行圖像識別，確定所述目標鉆桿在所述環境圖像中的位置；

基于所述目標鉆桿在所述環境圖像中的位置以及所述環境圖像所在圖像坐標系與世界坐標系的預設轉換關系，確定所述目標位置；

基于所述目標位置確定所述目標鉆桿上預設抓取點的空間位置，并基于所述空間位置確定所述鉆桿裝卸設備在預設抓取點進行抓取的目標姿態。