水下機器人最優剎車軌跡規劃設計

朱昱琛

（上海交通大學船舶海洋與建筑工程學院，上海 200240）

0 引言

軌跡規劃問題實質為運動優化問題，通過進行軌跡規劃能實現時間最短、能量消耗最少、路程最短等目標[1-3]。

目前進行軌跡規劃的方法[4]主要分為兩類：數值積分[5]方法與數值優化方法（包換動態規劃[6]及凸優化[7]等）。數值積分法通常需結合最優控制的相關理論進行，進行數值積分時，通常將最優參數假設為“bang-bang”形式[8]。利用數值積分法進行軌跡規劃時，其難點在于切換點的尋找問題[9]。相比較而言，數值優化方法求解較為簡單。然而，采用凸優化解法時，可能因為軌跡規劃問題的約束過多而無法將其轉化為凸優化問題。另外，數值優化解法受到問題的復雜度及維數影響較大，當問題維數增加時，求解時間消耗將以指數方式增長[10]。

軌跡的光滑性[11]對于軌跡跟蹤及實際執行等是極為重要的。通常而言，為獲取光滑軌跡，在進行軌跡規劃時需引入更多約束[12]。前人在進行光滑軌跡規劃時，通常將軌跡約束定義為速度約束、加速約束及急動度約束[13]。而本文通過對水下機器人剎車運動的分析，將軌跡約束轉化為推進器控制電壓約束，從而使問題得到簡化。

本研究在完成水下機器人最優剎車軌跡規劃問題建模后，基于Pontryagin 極值原理與數值積分提出一種剎車軌跡規劃方法。該方法相比于數值優化方法，能極大減少求解時間，具有較強實時性。

1 最優剎車軌跡規劃問題建模

1.1 水下機器人垂向運動模型

為簡化問題，本文僅研究水下機器人的垂蕩運動，忽略轉動及側向平臺影響,且運動過程，假定無外界干擾。水下機器人外形為近似四方體結構，該機器人上布置有8 個垂向推進器，并搭載著若干作業載荷，結構如圖1 所示。

圖1 水下機器人簡圖

以垂直向下為坐標軸正向，則水下機器人的垂蕩方程為：

其中，w為垂蕩速度，FT為垂向控制推力，△B為凈浮力（經測定為恒為142.8 N），m為質量，Zw˙為附加質量，Zw和分別為一、二階阻尼力系數。

定義水下機器人的總質量為M=（m-Zw˙），則可將方程轉化為水下機器人模型參數如表1 所示。

表1 水下機器人模型參數

水下機器人剎車上浮過程中，其制動力為流體阻力與推進器產生的垂向推力。假設垂向推力由N個完全相同的電動推進器產生。將單個推進器產生的推力記作T0，則有垂向推力FT=NT0。

推進電機動態方程為

其中R為電樞內阻，kcmf為感應電動勢系數，k為電機減速比，J為轉動慣量，kf為黏性摩擦系數。ω為電機角速度（rad/s），Vm為電機控制電壓，該電壓滿足飽和約束。

推進電機模型參數見表2。

表2 電機模型參數

推進器中螺旋槳推力T0與轉矩Q0為

其中，ρ為海水密度，D為螺旋槳直徑，n為螺旋槳轉速（rps）。推力系數KT（J0）和力矩系數KQ（J0）

可通過擬合為進速J0的線性表達式[14]：

螺旋槳模型參數見表3。

表3 螺旋槳模型參數

新定義參數如下：

經計算，新定義模型參數見表4。

表4 新定義模型參數

綜上，可建立潛水器垂蕩狀態空間方程為：

假設懸停時的系統狀態量為xf，控制輸入為uf；此時x˙=0。則根據方程可得懸停條件如下：

1.2 軌跡規劃問題建模

水下機器人開始運動時，滿足x（0）=x0=（w0，0）T。假定此時水下機器人運動速度為，該參數已知。最優剎車軌跡規劃問題可建模如下：

其中，T為潛水器開始運動時刻，該參數為未知常數。

軌跡起點z0未知而終點zf已知。可根據x1（t）的表達式與zf完成全過程軌跡規劃。

2 最優剎車軌跡規劃問題求解

本文采用一種基于Pontrygain 原理與數值積分的方式完成最優剎車軌跡規劃，該方法簡述如下。

根據方程，可建立Hamilton 函數為：

其中為Lagrange 算子。

根據Pontryagin 極值原理，最優控制輸入u*需滿足H（x*，u，λ*）≥H（x*，u*，λ*）；則u*的形式為

根據方程，結合剎車過程的物理特性，可構造最優控制輸入為：

其中tc為未知常量。則最優剎車軌跡規劃問題轉變為控制電壓切換時刻tc及停止時刻T的尋找問題，其中始末狀態x0，xf已知。

當tc及此時的水下機器人狀態量xc，則可基于數值積分完成T求解。因此，只需完成tc的搜尋即可。tc的搜索算法基于二分法與數值積分法進行，算法實現步驟見表5。其中，δt為極小時間常量。

表5 控制切換點搜索算法

3 仿真驗證

基于表1～ 表4 的模型參數，初始條件為x0=（-2.31，0）T，終端條件為xf=（0，22.01）T。利用表5 所示算法解得切換點為tc=11.33 s。根據tc及xc進行數值積分可知T=11.355 s。仿真中，終端位置zf=25。最優剎車軌跡可根據水下機器人運動速度積分獲取。根據公式進行仿真，結果如圖1 所示。

圖1 仿真結果

問題也可通過數值優化方法進行求解，例如可利用偽譜法將問題轉化為非線規劃問題，而后通過求解非線性規劃問題進行求解。

本文利用MATLAB GPOPS-SNOPT 工具箱進行問題求解，對比結果見圖2。由圖2 知，盡管采用數值優化方法時，控制輸入并非“bang-bang”形式。但最終所得的水下機器人速度變化基本一致，因此采用兩種方法進行軌跡規劃所得的最優軌跡相同。

但采用前述數值積分法求解時間只需數十秒，而采用GPOPS-SNOPT 工具箱求解耗時高達數十分鐘。因此，本文提出的基于最優控制理論與數值積分法的剎車軌跡規劃算法能在保證所得結果正確的前提下，極大減小求解時間。對比結果如圖2 所示。

圖2 對比結果

4 結束語

本研究針對水下機器人剎車上浮的運動特性，提出一種基于數值積分與最優控制理論的最優剎車軌跡規劃算法。該算法相較于基于數值優化的方法，能極大降低軌跡規劃所花費的時間，且不影響軌跡規劃精度。

后續的研究將通過實物實驗驗證本文提出的軌跡規劃算法可行性與最優性。同時，后續將考慮空間平動、轉動影響，將本文的垂向軌跡規劃推廣至多維運動中。