[發明專利]一種水下機器人近底地形跟蹤的目標值規劃方法

有效

• 申請號: 201911269399.2
• 申請日: 2019-12-11
• 公開（公告）號: CN112945232B
• 公開（公告）日: 2022-11-01
• 發明（設計）人: 姜志斌;劉鐵軍;祁勝
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 許宗富

1.一種水下機器人近底地形跟蹤的目標值規劃方法，其特征在于，包括以下步驟：

通過安裝在水下機器人前下方的聲學測距傳感器采集到距底距離信息，構成最優目標運動軌跡；

建立水下機器人垂直面運動學模型；包括：

假設水下機器人軌跡是由多拍直線運動組成，則水下機器人垂直面運動學模型如下：

其中，x和z表示慣性坐標系下前向和垂向位置，Δt表示時間采樣步長，k表示采樣點，u表示當前前向速度，θ表示當前縱傾角，φ表示當前航向角，q表示當前縱傾角速度；

當選取最優軌跡后，其所對應的垂直面速度(u，q)即為水下機器人的最優運動目標值；

依據水下機器人垂直面運動加減速性能和最大速度/角速度限定采樣空間在一個可行的滑動范圍內，采樣多組前向速度和縱傾角速度目標值，并模擬水下機器人在這些目標值下一段時間內的多組運動軌跡；包括：

采樣多組垂直面前向速度和縱傾角速度，推算垂直面運動軌跡；

所述垂直面前向速度和縱傾角速度存在如下兩個約束：

①水下機器人受自身最大前向速度u和縱傾角速度q的約束為：

u∈[u_min,u_max]，q∈[q_min,u_max]

②由于執行機構所提供的力矩有限，存在最大加減速限制，因此水下機器人軌跡向前模擬的時間段內，存在一個時間滑動窗口，在該時間窗口內的速度是水下機器人能夠實際達到的速度：

u∈[u_c-aΔt,u_c+aΔt]，

其中，u_c和q_c分別表示水下機器人的當前前向速度和縱傾角速度，a和表示水下機器人的最大前向加速度和最大縱傾角加速度；

對多條運動軌跡進行評價，根據與實際地形誤差絕對值之和最小為標準選取最優軌跡；包括：

在采樣的速度組中，有若干組軌跡是可行的，因此采用評價函數的方式為每條軌跡作出評價，評價函數如下：

其中，R_i(u,q)采用目標位置h_i采用垂直面運動學模型中的慣性坐標系下的前向位置x、垂向位置z、當前縱傾角θ，上式用來評價水下機器人在當前設定的采樣速度下，模擬軌跡每一拍位置和目標位置之間的距離之和，使得評價函數獲得最小值的垂直面速度(u，q)即為最優運動目標值；

選取最優軌跡所對應的運動目標距離時間序列值來作為水下機器人的目標值，發送給底層運動控制器來驅動水下機器人的垂直面運動，從而實現水下機器人近底地形跟蹤運動目標值的最優規劃。

2.根據權利要求1所述的一種水下機器人近底地形跟蹤的目標值規劃方法，其特征在于，所述目標距離時間序列為水下機器人利用前下方安裝的聲學測距傳感器實測的前方多拍的距底距離值，形成如下的距離時間序列集合：

H＝{h₁,h₂,……,h_n}

其中，h為實測的前方各拍的距底距離值；

由前方各拍距離值和當前位置可計算出相對應的位置值：

其中，x和z表示慣性坐標系下前向和垂向位置，k表示采樣點，為聲學測距傳感器與水下機器人水平面的安裝偏角，θ表示當前縱傾角；

H集合中每個元素按順序對應于如下目標位置的集合：

3.根據權利要求1所述的一種水下機器人近底地形跟蹤的目標值規劃方法，其特征在于，所述聲學測距傳感器為高度計或多普勒測速儀。