[發明專利]基于梯度估計的自主式水下機器人場源搜索方法及系統

在審

• 申請號: 201810211983.1
• 申請日: 2018-03-15
• 公開（公告）號: CN108664039A
• 公開（公告）日: 2018-10-16
• 發明（設計）人: 游科友;李卓;宋士吉
• 申請（專利權）人: 清華大學
• 主分類號: G05D1/12
• 分類號: G05D1/12
• 代理公司: 北京清亦華知識產權代理事務所(普通合伙) 11201
• 代理人: 廖元秋

1.一種基于梯度估計的自主式水下機器人場源搜索方法，其特征在于，該方法包括以下步驟：

1)構建自主式水下機器人AUV場源搜索優化模型；

1-1)建立AUV的動力學模型；

AUV場源搜索優化模型的決策變量為AUV的狀態向量η，AUV的動力學模型在地球坐標系中表示為以下形式：

其中，η＝[x y z φ θ ψ]^T是AUV的狀態向量，向量中的六個元素分別表示AUV在三維空間中的六個自由度，x，y，z表示三維空間中的平移運動，x表示前涌運動坐標，y表示橫移運動坐標，z表示升沉運動坐標，φ，θ，ψ表示三維空間中的旋轉運動，φ表示繞x軸轉動的滾動角，θ表示繞y軸轉動的偏航角，ψ表示繞z軸轉動的俯仰角，為狀態向量η的一階導數，為狀態向量η的二階導數；M(η)表示慣性矩陣，表示科里奧利力及向心力矩陣，表示水動力學引入的阻尼矩陣，g(η)表示重力及力矩矩陣，表示施加在AUV上的力及力矩；

1-2)確定優化模型的目標函數；

AUV場源搜索優化模型的矢量目標函數如下：

其中，η₁＝[x y]^T，η₂＝[z φ θ ψ]^T，z^d為預先設定的AUV理想高度，[φ^d θ^d ψ^d]^T為預先設定的AUV理想姿態；

假定待搜索的場源在地球坐標系的xoy平面的原點，則h(η₁)與AUV三維空間位置的二范數‖η₁‖成反比，h(η₁)表示AUV在平面位置η₁處的信號場強度的負對數；

1-3)確定優化模型的假設條件；具體如下：

1-3-1)AUV狀態假設：AUV的狀態向量η保持在非奇異狀態；

1-3-2)矢量目標函數假設：函數h(η₁)二階連續可微，海森矩陣Hessian表示為H_h(η)，f₁(η)的Hessian表示為H_f1(η)，且兩個Hessian矩陣有界；

1-3-3)矢量目標函數假設：h(η₁)為強制函數，即當‖η₁‖→∞時，h(η₁)→∞；

2)根據場源搜索任務的需求，確定AUV的型號，并安裝傳感器；

在AUV主體及周圍安裝n+1個傳感器，包括1個位于AUV主體上的傳感器作為中心傳感器，記作0，其余n個邊緣傳感器記作1，2，3...n；在地球坐標系的xoy平面上，中心傳感器0固定在AUV的主體上，表示為p⁰，則有p⁰＝η₁；以傳感器0為原點，以當前AUV的前進速度方向為y軸，x軸與y軸垂直建立傳感器系統的坐標系；邊緣傳感器1，2，3..n位于以中心傳感器0為圓心，半徑為r且y軸坐標大于等于0的半圓上，且相鄰兩個邊緣傳感器之間構成的圓心角相等，記邊緣傳感器在地球坐標系下的位置向量為pⁱ,i＝1,2,…,n為邊緣傳感器的編號，則邊緣傳感器的位置滿足到傳感器0的距離相同，相鄰傳感器距離相等，即：

其中，r表示任意邊緣傳感器到中心傳感器的距離；

3)AUV執行場源搜索任務；具體步驟如下：

3-1)選定待探測海域的任意位置作為初始位置進行AUV的布放，令當前時刻t＝t₁，AUV下水到達初始位置，傳感器開始工作，將初始位置記為AUV的當前位置；

3-2)傳感器測量得到當前位置的信號場強度并傳送給AUV；

3-3)對AUV當前位置的信號場強度進行判定：

若當前位置的信號場強度大于預定場強閾值，則進入步驟3-4)，進行當前時刻場源梯度估計；

若當前位置的信號場強度小于預定場強閾值，則AUV進行U型軌跡對任務待探測海域進行遍歷的地毯式搜索，直至當前位置的信號場強度大于預定場強閾值，則進入步驟3-4)，進行當前時刻的場源梯度估計；

3-4)梯度估計；

當前時刻，記位于地球坐標系pⁱ位置的傳感器i的信號場強度測量值為h(pⁱ)，將h(pⁱ)進行泰勒展開得到：

其中，h(p⁰)為中心傳感器p⁰位置處的信號場強度測量值，為中心傳感器p⁰位置處的梯度的轉置，為高階項，將該高階項忽略，得到梯度估計為：

其中，矩陣

目標函數的第一個分量與AUV的二維平面位置有關：

其中，為目標函數中第一個分量相對于AUV狀態變量η的梯度；

根據下式計算得到：

其中，為目標函數中第二個分量相對于AUV狀態變量η的梯度，η₂為AUV自身旋轉運動的姿態向量，為對AUV自身旋轉運動的期望姿態；

3-5)判斷AUV是否達到場源位置；

判斷和是否為零，若二者均為零，則AUV達到場源位置，場源搜索任務結束；若二者之中任意一個不為零，則進入步驟3-6)，基于梯度估計對AUV進行運動控制；

3-6)基于梯度估計的AUV運動控制；

利用步驟3-4)得到的和計算得到AUV當前時刻的控制輸入，表達式如下：

其中，k>0,α>0,β>0；為速度阻尼項，和分別為目標函數中第一個分量的負梯度估計和第二個分量的負梯度；

將當前時刻控制輸入施加于AUV，AUV進行運動直至下一個時刻，將AUV到達新的位置記為當前位置，重新返回步驟3-2)。

2.一種基于如權利要求1所述方法的自主式水下機器人場源搜索系統，包括一臺自主式水下機器人AUV和若干個相同的傳感器，所述每個傳感器分別與AUV通過有線方式連接，其特征在于，其中1個傳感器安裝在AUV主體上作為中心傳感器，以中心傳感器為原點，以當前AUV的前進速度方向為y軸，x軸與y軸垂直，建立傳感器系統的坐標系，其余傳感器分別位于以中心傳感器為圓心，以固定距離為半徑且y軸坐標大于等于0的半圓上作為邊緣傳感器，相鄰兩個邊緣傳感器之間構成的圓心角相等；所述傳感器用于測量AUV當前位置的場強信息并發送給AUV，AUV用于根據接收到的場強信息實時進行信號場的梯度估計以及AUV的控制輸入的計算，并根據計算結果執行場源搜索任務。