[發明專利]基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制算法

有效

• 申請號: 201610029520.4
• 申請日: 2016-01-16
• 公開（公告）號: CN105717923B
• 公開（公告）日: 2019-02-19
• 發明（設計）人: 蒲華燕;丁峰;瞿棟;羅均;謝少榮;彭艷;李小毛;吳翔;程啟興;劉崇鋒;胡辛明
• 申請（專利權）人: 上海大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 上海上大專利事務所(普通合伙) 31205
• 代理人: 何文欣

1.一種基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制方法，其特征在于操作步驟如下：

第1步在軟件中對動態船只障礙物按照其尺寸形態進行聚類成動態橢圓障礙物，并且采集其運動狀態；

第2步規劃系統初始參數，包括目標距離、動態船只障礙物中心點相對于無人艇質點的方位角、質點速度--無人艇速度、橢圓速度--動態障礙物速度、橢圓長軸、橢圓短軸、質點速度方向--無人艇速度方向、橢圓速度方向--動態障礙物速度方向；

第3步根據橢圓的相關參數確定橢圓的兩個焦點位置--焦點C1和焦點C2，并且計算兩個焦點與無人艇質點之間的方位角；

第4步根據橢圓兩個焦點與無人艇質點之間的位置關系分別計算出兩個焦點與無人艇質點之間的相對距離；

第5步根據橢圓和無人艇質點的運動狀態以及位置關系分別計算出無人艇質點與橢圓中心、焦點C1和焦點C2之間的相對距離變化速度和相對方位角的變化速度；

第6步通過以上得出的無人艇質點和橢圓動態障礙物之間的相對參數，利用推演的點和橢圓之間的碰撞錐計算方法對兩者進行碰撞檢測，并計算出碰撞錐區間；

第7步通過第6步計算的碰撞結果，依據所得出的碰撞錐區間，無人艇采取相應的措施；

第8步按照動態障礙物的運動狀態重復第2步到第7步；當無人艇所搭載的傳感器檢測到動態船只障礙物的運動狀態發生改變，則應當重新進行碰撞計算。

2.根據權利要求1所述的一種基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制方法，其特征在于，在第3步中，橢圓的兩個焦點與無人艇質點之間的位置關系為：

焦點C1的相對坐標位置為(r·cosα-c·cosβ，r·sinα-c·sinβ)，焦點C2的相對坐標位置為：(r·cosα-c·cosβ，r·sinα-c·sinβ)，

其中r為無人艇質點與動態船只障礙物中心的距離、α為無人艇質點的運動方向、β為動態障礙物的運動方向，其中a為橢圓障礙物的長軸長、b為橢圓障礙物的短軸長、c為橢圓障礙物焦點到橢圓中心點的距離。

3.根據權利要求2所述的一種基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制方法，其特征在于，在第4步中兩個焦點(焦點C1和焦點C2)與無人艇質點之間的相對距離為：

其中r₀₁為焦點C1與無人艇質點的距離、r₀₂為焦點C2與無人艇質點的距離、θ為動態船只障礙物中心點相對于無人艇質點的方位角。

4.根據權利要求1所述的一種基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制方法，其特征在于，在第5步中無人艇質點與橢圓中心、焦點C1和焦點C2之間的相對距離變化速度和相對方位角的變化速度為：

其中，無人艇質點與橢圓中心的相對距離變化速度為V_r0、相對方位角變化速度為V_θ0；無人艇質點與橢圓的焦點1之間的相對距離變化速度為V_r01、相對方位角變化速度為V_θ01；無人艇質點與橢圓的焦點2之間的相對距離變化速度為V_r02、相對方位角變化速度為V_θ02；r為無人艇質點與動態船只障礙物中心的距離、α為無人艇質點的運動方向、β為動態障礙物的運動方向、V_a為無人艇質點的運動速度、V_b為動態障礙物的運動速度、r₀₁為焦點C1與無人艇質點的距離、r₀₂為焦點C2與無人艇質點的距離、θ為動態船只障礙物中心點相對于無人艇質點的方位角、θ₁為動態船只障礙物焦點C1相對于無人艇質點的方位角、θ₂為動態船只障礙物焦點C2相對于無人艇質點的方位角。

5.根據權利要求1所述的一種基于橢圓聚類-碰撞錐推演的無人艇海洋動態避障控制方法，其特征在于，在第7步中無人艇采取相應的措施為：

如檢測到無人艇和動態障礙物不會發生碰撞，則可不采取措施；

如檢測到無人艇和動態障礙物會發生碰撞，則可采取以下措施：

(1)改變無人艇的速度方向，不改變速度大小；依據所計算出的碰撞錐區間，無人艇的速度方向只要取在該區間之外就可完成避障行為；

(2)改變無人艇的速度大小，不改變速度方向；無人艇的速度增大或減小可進行有效的避障；改變速度大小后進行碰撞檢測，如檢測到不會發生碰撞則說明采取了有效的避障措施；

(3)既 改變無人艇的速度方向，也改變速度大小；這種措施能更加有效迅速的進行有效的避障行為。