朱昱琛

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

0 引言

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

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

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

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

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

1.1 水下機(jī)器人垂向運(yùn)動(dòng)模型

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

圖1 水下機(jī)器人簡圖

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

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

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

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

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

推進(jìn)電機(jī)動(dòng)態(tài)方程為

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

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

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

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

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

可通過擬合為進(jìn)速J0的線性表達(dá)式[14]：

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

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

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

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

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

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

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

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

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

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

軌跡起點(diǎn)z0未知而終點(diǎn)zf已知?？筛鶕?jù)x1（t）的表達(dá)式與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時(shí)刻tc及停止時(shí)刻T的尋找問題，其中始末狀態(tài)x0，xf已知。

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

表5 控制切換點(diǎn)搜索算法

3 仿真驗(yàn)證

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

圖1 仿真結(jié)果

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

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

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

圖2 對(duì)比結(jié)果

4 結(jié)束語

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

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