[發明專利]一種基于激光雷達的水下機器人智能運動控制方法

有效

• 申請號: 202011122377.6
• 申請日: 2020-10-19
• 公開（公告）號: CN112363498B
• 公開（公告）日: 2022-09-23
• 發明（設計）人: 張強;胡宴才;張燕;劉洋;張樹豪;許世波;陳麒宇
• 申請（專利權）人: 山東交通學院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于激光雷達的水下機器人智能運動控制方法，其特征在于，所述方法包括：

步驟S1、在水下工作區域使用激光雷達數據創建3D柵格地圖；

步驟S2、向水下機器人的導航模塊發送導航目標點；

步驟S3、水下機器人的導航模塊根據目標點與當前位置，規劃由當前位置到目標點的行走路徑；

步驟S4、水下機器人沿規劃出的路徑運動，導航模塊根據激光雷達數據，計算水下機器人運動過程中與障礙物的相對速度，從而根據速度沖突檢測方法實時計算水下機器人下一時刻的最優運動速度；

步驟S5、根據最優運動速度控制水下機器人移動，重復執行步驟S4，直到水下機器人到達目標點；

其中，所述步驟S4中的導航模塊根據激光雷達數據，計算水下機器人運動過程中與障礙物的相對速度具體為：

步驟S401、取一幀激光數據進行聚類，得出的每一個點集認為是一個障礙物，過濾掉5m以外的點集，同時過濾掉地圖上已有障礙的點集，最終得到n_laser1組障礙物點集；

步驟S402、取后一幀激光數據，按照步驟S401得到n_laser2組點集；

步驟S403：定義n_laser1中點集為pts_1i，其中0i＝n_laser1，定義n_laser2中點集為Pts_2j，其中0j＝n_laser2；將n_laser1中各點集中心位置與n_laser2中各點集中心位置依次比較，距離小于閾值e1的認為是兩幀激光數據下同一障礙物；

步驟S404：針對同一障礙物，設其在第一幀激光數據下的中心位置為p1(x1,y1)，在第二幀激光數據下的中心位置為p2(x2,y2)，兩幀激光數據的時間間隔為dt，則計算出障礙物的移動速度向量Vo＝((x2-x1)/dt,(y2-y1)/dt)；

步驟S405：從里程計上報的信息中獲取水下機器人當前速度向量Vr，則水下機器人與障礙物的相對速度Vro＝Vo-Vr；

其中，步驟S4中的實時計算水下機器人下一時刻的運動速度具體為：

步驟S406：按照dwa方法，計算本時刻速度空間下的速度樣本；

步驟S407：對每個速度樣本，計算dwa得分后，進行速度沖突檢測，若檢測結果為沖突，則將該樣本的得分設置為0；否則，比較該樣本得分與最優樣本的得分，若該樣本得分比最優樣本高，則將該樣本設為最優樣本；

步驟S408：按照步驟S407計算每個速度樣本，取最優樣本速度值作為水下機器人下一時刻的速度；

其中，步驟S407中的速度沖突檢測方法具體為：

步驟S4071：設障礙物點集為pts_2k，其中與水下機器人位置Pr(xr,yr)最近的一點為P(x,y)，過點P作Vo方向的直線L_Vo；

步驟S4072：將L_Vo沿Pr到P的垂線方向向Pr平移，平移距離為水下機器人半徑r，得到直線L_Vo’；

步驟S4073：設沖突檢測的速度樣本V，樣本模擬時間為Δt，則模擬最終位置P’＝Pr+V*Δt；

步驟S4074：判斷Pr與P’與直線L_Vo’的關系，若Pr與P’在直線L_Vo’同側，則檢測結果為不沖突；反之，檢測結果為沖突。