[發明專利]水下機器人運動路徑規劃方法及系統

有效

• 申請號: 201711443969.6
• 申請日: 2017-12-27
• 公開（公告）號: CN108189031B
• 公開（公告）日: 2021-02-19
• 發明（設計）人: 莊楠;應佳偉;陳展平;何彩燕
• 申請（專利權）人: 莊楠
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州華進聯浙知識產權代理有限公司 33250
• 代理人: 李麗華

1.一種水下機器人運動路徑規劃方法，其特征在于，包括步驟：

實時獲取所述水下機器人當前的狀態信息；所述狀態信息為機身與XY平面、XZ平面、YZ平面之間的夾角；

基于所述狀態信息控制所述水下機器人前進爬墻，并且在前進爬墻之后后退下墻；

基于所述狀態信息判斷所述水下機器人是否完成后退下墻，若是，則控制所述水下機器人先后退，進行第一次轉向，所述第一次轉向后，再次后退，進行與第一次轉向方向相反角度相同的第二次轉向；

基于所述狀態信息控制所述水下機器人后退爬墻，并且在后退爬墻之后前進下墻；

基于所述狀態信息判斷所述水下機器人是否完成前進下墻，若是，則控制所述水下機器人先前進，進行第三次轉向，所述第三次轉向后，再次前進，進行與第三次轉向方向相反角度相同的第四次轉向；

重復以上步驟，并將往返次數加1；

所述基于所述狀態信息控制所述水下機器人前進爬墻，并且在前進爬墻之后后退下墻的步驟以及基于所述狀態信息控制所述水下機器人后退爬墻，并且在后退爬墻之后前進下墻的步驟具體為：

基于所述狀態信息判斷所述水下機器人是否為水平狀態，若為水平狀態，獲取當前的往返次數；若往返次數小于n次時，則控制所述水下機器人前進/后退；

獲取當前前進/后退時間，將所述前進/后退時間與第一預設時間進行比較；

若前進/后退時間小于第一預設時間，則基于所述狀態信息判斷所述水下機器人是否遇到側墻，若遇到，則控制所述水下機器人前進/后退爬墻；

獲取當前前進/后退爬墻時間，將所述前進/后退爬墻時間與第二預設時間進行比較；

若所述前進/后退爬墻時間不小于所述第二預設時間時，則控制所述水下機器人后退/前進下墻；

所述水平狀態包括：在第一次轉向、第二次轉向、第三次轉向、第四次轉向、第五次轉向后水平狀態為：水下機器人與XY平面的夾角小于預設水平夾角，預設水平夾角為30°；在下墻時水平狀態為，機身與XY平面的夾角小于或等于預設下墻角度；在上墻時水平狀態為，機身與XY平面的夾角小于或等于預設爬墻角度。

2.根據權利要求1所述的方法，其特征在于，所述往返次數增加到n時，對所述往返次數重置為1。

3.根據權利要求2所述的方法，其特征在于，所述基于所述狀態信息判斷所述水下機器人是否為水平狀態，若為水平狀態，獲取當前的往返次數；若往返次數小于n次時，則控制所述水下機器人前進/后退還包括：

若往返次數不小于n次時，獲取標志位信息，基于所述標志位控制所述水下機器人第五次轉向，所述第五次轉向后，前進/后退且標志位加1。

4.根據權利要求3所述的方法，其特征在于，所述基于所述狀態信息判斷所述水下機器人是否為水平狀態，若為水平狀態，獲取當前的往返次數；若往返次數小于n次時，則控制所述水下機器人前進/后退還包括：

若不為水平狀態，則控制所述水下機器人后退/前進下墻且往返次數減1，執行步驟基于所述狀態信息判斷所述水下機器人是否完成后退/前進下墻，若是，則控制所述水下機器人先后退/前進，進行第一/三次轉向，所述第一/三次轉向后，再次后退/前進，進行與第一/三次轉向方向相反角度相同的第二/四次轉向。

5.根據權利要求3所述的方法，其特征在于，所述基于所述標志位第五次轉向具體為：

所述標志位的取值范圍為1≤m≤4；

當所述標志位等于1時，后退時右轉向，前進時不轉向，且所述標志位加1；

當所述標志位等于2時，后退時左轉向，前進時不轉向，且所述標志位加1；

當所述標志位等于3時，后退時不轉向，前進時右轉向，且所述標志位加1；

當所述標志位等于4時，后退時不轉向，前進時左轉向，且所述標志位加1；

當所述標志位大于4時，將標志位重置為1。

6.根據權利要求3所述的方法，其特征在于，

所述第一次轉向、第二次轉向、第三次轉向、第四次轉向的轉動角度為45°，所述第五次轉向的轉動角度為90°。