[發明專利]一種水下機器人軌跡上障礙物檢測方法

在審

• 申請號: 202211470480.9
• 申請日: 2022-11-22
• 公開（公告）號: CN115856899A
• 公開（公告）日: 2023-03-28
• 發明（設計）人: 李根;黃小華;袁太平;龐國良;陶啟友;胡昱
• 申請（專利權）人: 中國水產科學研究院南海水產研究所;三亞熱帶水產研究院
• 主分類號: G01S15/93
• 分類號: G01S15/93;G01S15/89;G01S15/58
• 代理公司: 廣州知友專利商標代理有限公司 44104
• 代理人: 宣國華;何秋林

1.一種水下機器人軌跡上障礙物檢測方法，其特征在于，包括如下步驟：

(1)可行軌跡集生成；

(2)腳印柵格地圖集生成

在柵格地圖上標記可行軌跡集中每條軌跡的軌跡腳印，并記錄每個腳印位置的警示級別，為每條軌跡分別構建一張腳印柵格地圖，所述警示級別根據機器人避開對應腳印位置具有的反應時間設定；

(3)概率腳印柵格地圖集生成

在柵格地圖上標記水下機器人沿某條軌跡移動時，實際移動軌跡可能經過的區域，并在每個柵格中保存軌跡經過該柵格的概率，構建軌跡概率柵格地圖M_probTraj(j)，然后根據該軌跡概率柵格地圖M_probTraj(j)生成相應的腳印柵格地圖，稱該地圖為概率腳印柵格地圖M_probfp(j)，并最終獲得概率腳印柵格地圖集；

(4)概率腳印柵格地圖集保存；

(5)傳感器柵格地圖集構建

給定傳感器測量值，確定障礙物的真實位置在傳感器坐標系中可能存在的區域，并由此標記坐標系中各個柵格處可能存在障礙物的概率，稱該地圖為傳感器柵格地圖，對應不同測量值的傳感器柵格地圖的集合，稱為傳感器柵格地圖集；

(6)傳感器數據整合

采集機器人上各個傳感器的測量值{d_sensor1，d_sensor2…d_sensorN}，并根據測量值索引到相應的傳感器柵格地圖{M_sensor(d1)，M_sensor(d2)....M_{sensor(dsensorN)}}，根據各個傳感器坐標系在機器人坐標系下的位姿{X_sensor1，X_sensor2....X_sensorN}，將{M_sensor(d1)，M_sensor(d2)....M_{sensor(dsensorN)}}轉化到機器人坐標系，并且進行對應柵格的占據概率p_occ(x，y)累加，形成一張傳感器融合柵格地圖M_{AllSensorInRobot}，并對傳感器融合柵格地圖中的占據概率進行歸一化操作；

(7)碰撞檢測

將傳感器融合柵格地圖和概率腳印柵格地圖集中的每一張柵格地圖的對應柵格的概率值相乘，獲取每條軌跡每個軌跡腳印柵格可能發送碰撞的概率。

2.根據權利要求1所述的水下機器人軌跡上障礙物檢測方法，其特征在于，步驟(1)可行軌跡集生成的具體步驟為：

將連續的軌跡離散化，表示機器人的第j條連續軌跡Tra_j離散化后的樣本點的集合，X_i≡[x_i，y_i，z_i，row_i，pitch_i，yaw_i]為該離散軌跡上的第i個樣本點，其包括三個位置坐標x_i、y_i、z_i和三個姿態坐標row_i、pitch_i、yaw_i，位置表示選定的機器人坐標系原點在指定坐標系下的坐標，姿態表示機器人坐標系的翻滾角，俯仰角，航向角在指定坐標系下的表示，其中，N_ps為該離散軌跡上離散點的個數；可行軌跡集由一系列可行軌跡組成，即其中N_traj表示可行軌跡集合的個數。

3.根據權利要求2所述的水下機器人軌跡上障礙物檢測方法，其特征在于，N_ps通過如下方式確定：Nps＝Tbreak·N/ΔTps，其中，ΔTps表示相鄰兩個軌跡樣本點X_i和X_i+1的時間間隔，Tbreak為機器人從最高速到剎停的時間，N為為留設安全余量而設置的對Tbreak的放大倍數。