水下機器人最優(yōu)剎車軌跡規(guī)劃設(shè)計

朱昱琛

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

0 引言

軌跡規(guī)劃問題實質(zhì)為運動優(yōu)化問題，通過進行軌跡規(guī)劃能實現(xiàn)時間最短、能量消耗最少、路程最短等目標(biāo)[1-3]。

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

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

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

1 最優(yōu)剎車軌跡規(guī)劃問題建模

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

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

圖1 水下機器人簡圖

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

其中，w為垂蕩速度，F(xiàn)T為垂向控制推力，△B為凈浮力（經(jīng)測定為恒為142.8 N），m為質(zhì)量，Zw˙為附加質(zhì)量，Zw和分別為一、二階阻尼力系數(shù)。

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

表1 水下機器人模型參數(shù)

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

推進電機動態(tài)方程為

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

推進電機模型參數(shù)見表2。

表2 電機模型參數(shù)

推進器中螺旋槳推力T0與轉(zhuǎn)矩Q0為

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

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

螺旋槳模型參數(shù)見表3。

表3 螺旋槳模型參數(shù)

新定義參數(shù)如下：

經(jīng)計算，新定義模型參數(shù)見表4。

表4 新定義模型參數(shù)

綜上，可建立潛水器垂蕩狀態(tài)空間方程為：

假設(shè)懸停時的系統(tǒng)狀態(tài)量為xf，控制輸入為uf；此時x˙=0。則根據(jù)方程可得懸停條件如下：

1.2 軌跡規(guī)劃問題建模

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

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

軌跡起點z0未知而終點zf已知?？筛鶕?jù)x1（t）的表達式與zf完成全過程軌跡規(guī)劃。

2 最優(yōu)剎車軌跡規(guī)劃問題求解

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

根據(jù)方程，可建立Hamilton 函數(shù)為：

其中為Lagrange 算子。

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

根據(jù)方程，結(jié)合剎車過程的物理特性，可構(gòu)造最優(yōu)控制輸入為：

其中tc為未知常量。則最優(yōu)剎車軌跡規(guī)劃問題轉(zhuǎn)變?yōu)榭刂齐妷呵袚Q時刻tc及停止時刻T的尋找問題，其中始末狀態(tài)x0，xf已知。

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

表5 控制切換點搜索算法

3 仿真驗證

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

圖1 仿真結(jié)果

問題也可通過數(shù)值優(yōu)化方法進行求解，例如可利用偽譜法將問題轉(zhuǎn)化為非線規(guī)劃問題，而后通過求解非線性規(guī)劃問題進行求解。

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

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

圖2 對比結(jié)果

4 結(jié)束語

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

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