[發明專利]一種舉高消防機器人控制方法、裝置及系統

有效

• 申請號: 202011620629.8
• 申請日: 2020-12-30
• 公開（公告）號: CN112870595B
• 公開（公告）日: 2022-07-08
• 發明（設計）人: 黃國方;甘志堅;陳向志;張靜;劉曉銘;單超;王文政;謝芬;郝永奇;廖志勇;陸盛資;吳圣和;謝永麟;彭奕;湯濟民;周興俊;楊明鑫;吳嵩青
• 申請（專利權）人: 國電南瑞科技股份有限公司
• 主分類號: A62C27/00
• 分類號: A62C27/00;A62C37/40;A62C31/00;A62C31/28
• 代理公司: 南京縱橫知識產權代理有限公司 32224
• 代理人: 張賞

1.一種舉高消防機器人控制方法，其特征在于，包括：

建立舉高消防機器人控制系統，包括：車體、塔臂、聯動臂、主臂和伸縮臂；所述車體與塔臂之間以轉動關節形式鏈接，所述塔臂和聯動臂以聯動關節形式鏈接，所述聯動臂和主臂之間以轉動關節鏈接，所述主臂和伸縮臂之間以平移關節鏈接，所述消防炮臺與伸縮臂之間以關節形式鏈接；所述消防炮臺用于安裝消防炮；

控制塔臂與車體關節旋轉角度，調節消防炮粗對準消防目標方向；

聯合控制塔臂、聯動臂、主臂和伸縮臂，調節消防炮舉高高度，使消防炮高度對準消防目標；

控制消防炮臺與伸縮臂關節旋轉角度，調節消防炮俯仰角，使消防炮最終對準消防目標；

所述調節消防炮舉高高度，包括：獲取舉高消防機器人當前關節狀態初始信息并儲存為當前容器Jc，以及獲取目標高度Ht；Jc＝(Jc0,Jc1,Jc2,...,Jcn)，其中0，1，2，…,n為關節序號；

根據當前容器Jc中的元素計算舉高消防機器人當前高度Hc，以及根據當前容器Jc的元素計算目標高度對應的舉高消防機器人的初步目標關節狀態并存儲為目標容器Jt，Jt＝(Jt0,Jt1,Jt2,...,Jtn)；

根據目標高度和當前高度選擇控制策略，如果目標高度大于當前高度，則選擇上升控制策略，反之選擇下降控制策略；

所述上升控制策略為：

第一步：計算目標容器Jt中元素與當前容器Jc中元素的差值并儲存為差值容器Ja，Ja＝(Ja0,Ja1,Ja2,...,Jan)，對差值容器Ja內的元素按照從大到小進行排序；

第二步：基于排序后的差值容器Ja獲取最大元素的關節號i，采用目標容器Jt的第i關節號元素值Jti替換測試容器Jb中的第i關節號元素Jbi，Jbi＝Jti，更新預先構建的測試容器Jb，并根據更新后的測試容器Jb求解測試高度Hb；

第三步：將測試高度Hb與目標高度Ht進行對比，

如果相等，則將Jti以堆棧形式放入控制容器C中，再按進入堆?？刂迫萜鰿的先后順序，發送相對應的關節狀態至舉高消防機器人，使舉高消防機器人運動到達相應位置，高度調節結束；

如果不相等，則判斷測試高度Hb是否大于目標高度Ht，

如果是，則設置中間變量Jm＝(Jti+Jci)/2，把目標容器Jt中的第i關節號元素替換中間變量Jm，即Jti＝Jm；跳到第二步；

如果否，則將Jti以堆棧形式放入控制容器C中；然后將差值容器Ja內的第i關節號元素刪除；跳到第一步；

所述下降控制策略為：

第一步：計算目標容器Jt中元素與當前容器Jc中元素的差值并儲存為差值容器Ja，Ja＝(Ja0,Ja1,Ja2,...,Jan)，對差值容器Ja內的元素按照從小到大進行排序；

第二步：基于排序后的差值容器Ja獲取最小元素的關節號i，采用目標容器Jt的第i關節號元素值Jti替換測試容器Jb中的第i關節號元素Jbi，Jbi＝Jti，更新預先構建的測試容器Jb，并根據更新后的測試容器Jb求解測試高度Hb；

第三步：將測試高度Hb與目標高度Ht進行對比，

如果相等，則將Jti以堆棧形式放入控制容器C中，再按進入堆?？刂迫萜鰿的先后順序，發送相對應的關節狀態至舉高消防機器人，使舉高消防機器人運動到達相應位置，高度調節結束；

如果不相等，則判斷測試高度Hb是否小于目標高度Ht，

如果是，則設置中間變量Jm＝(Jti+Jci)/2，把目標容器Jt中的第i關節號元素替換中間變量Jm，即Jti＝Jm；跳到第二步；

如果否，則將Jti以堆棧形式放入控制容器C中；然后將差值容器Ja內的第i關節號元素刪除；跳到第一步。

2.根據權利要求1所述的一種舉高消防機器人控制方法，其特征在于，所述調節消防炮粗對準消防目標方向，包括：

獲取消防目標點位置，計算得到消防目標點相對于車體位置(Xt,Yt,Hc)，

其中，Xt,Yt,Hc分別為X軸，Y軸和Z軸坐標，以右手定則為基礎，車頭方向X軸方向，垂直于車體為Y軸方向，Z軸為高度；

如果Xt＞0，則控制塔臂相對車體轉動角度為：Jt0＝arctan(Yt/Xt)；

如果Xt＜0，則控制塔臂相對車體轉動角度為：Jt0＝Pi+arctan(Yt/Xt)；

其中，Jt0為塔臂相對車體轉動角度，Pi為圓周率。