遙控水下機器人模型辨識及預測控制算法研究

朱迎谷，張定華，涂紹平，宋俊輝

（上海中車艾森迪海洋裝備有限公司，上海 201306）

0 引言

遙控水下機器人（remotely operated vehicle， ROV）是從事水下科考、資源開采和打撈救援的必備工具之一，通常要求具有可靠的穩定性和適應不同場景的水下作業能力。傳統水下作業機器人采用遠程遙控方式進行操作，作業成本高但效率低下。為此，需要提高水下機器人自動化和智能化水平［1］。

水下機器人作業環境復雜，不確定因素多，導致難以通過精確數學建模來描述其受力情況和運動學特性。隨著搭載工具的不同，水下機器人布局、重心位置及浮心位置等均會發生改變，且推進器性能偏差或者故障都會導致水下機器人運動耦合特性發生變化，造成其操作困難或者存在被損壞的風險［2］。對此，圍繞提高水下機器人安全性、可靠性、控制魯棒性［3］和人機協同操控性能［4］，最終實現全自主水下作業［5］目標，學者們提出了眾多適用于不同海況、不同作業任務需求的ROV控制算法，比如比例-積分-微分（proportional integral derivative，PID）控制算法、模糊控制算法［6］、模型預測控制（model predictive control， MPC）算法［7］、基于視覺定位反饋的伺服控制算法［8］和非線性魯棒控制算法［9］等。其中，PID控制算法應用廣泛、性能穩定、魯棒性高，但難以適應復雜工況或在非線性動態情況下表現較差。隨著MPC算法和數值優化算法的進一步發展，具備自適應功能的MPC算法成為研究熱點之一［10］。

目前的研究成果主要集中在假定ROV動態過程可以通過線性數學模型進行描述的基礎上［11］，且MPC多采用直接求解方法，控制指令超出了實際水下機器人執行機構工作能力，或者采用復雜的優化求解算法，但算法執行的實時性差［7］。為此，本文提出一種考慮水下機器人典型非線性特性的在線模型辨識方法，解決了傳統辨識方法難以應對模型強非線性特性的問題；推導了基于所辨識模型的多維預測控制算法，并通過將控制算法應用在仿真模型上，驗證了該算法的有效性和先進性。

1 水下機器人數學模型

在推導建立水下機器人數學模型之前，先定義地球坐標系和水下機器人坐標系，如圖1 所示。圖中，地球坐標系原點OE為任意一點位置，機器人坐標系原點OB與水下機器人重心位置重合。

圖1 坐標系定義Fig.1 Definition of coordinate systems

通過分析機器人受力和運動過程，建立水下機器人六自由度運動學模型：

式中：η——水下機器人在地球坐標系下位置關系，包括XE、YE和ZE軸上的位置（x、y和z）、滾轉角φ、俯仰角θ和偏航角Ψ，η=[x y z φ θ ψ]T；J(η) ——從機器人坐標系向地球坐標系變換的變換矩陣；V——水下機器人在機器人坐標系XB軸、YB軸、ZB軸上的速度變量，包括線速度變量（u、v和w）和角速度變量（p、q和r），V=[u v w p q r]T；RB，E——機器人坐標系內的線速度對地球坐標系內線速度的旋轉變換矩陣；T——機器人坐標系內的角速度對地球坐標系內角速度的旋轉變換矩陣；Q0，3——維數為（3，3）的二維零矩陣。

在水下機器人坐標系內，根據拉格朗日動力學建模原理，建立水下機器人六自由度非線性動力學模型：

式中：M—— 慣性矩陣；CRB（V） —— 離心力（Centrifugal）矩陣；CA（V） ——科里奧利力（Coriolis）矩陣；D（V） ——水阻尼矩陣；G（η）——在機器人坐標系內水下機器人在重力和浮力作用下產生的力和力矩矢量；τ——推進器在機器人坐標系下作用在水下機器人本體上的力和力矩矢量；w——外部干擾力矢量。

在實際應用過程中，水下機器人需要攜帶各種作業工具和傳感器，其所受的重力和浮力會隨之發生改變，且對應的重心和浮心位置也會偏移原來位置。為了計算仿真機器人重心和浮心的改變對水下機器人動力學的影響，建立如下計算方程：

式中：B——水下機器人的浮力；m——水下機器人的質量；g——重力加速度；lx、ly和lz——浮心在機器人坐標系內的坐標位置。

2 混合模型辨識算法

雖然線性化模型簡單，便于控制算法分析設計［12］，但難以準確地描述非線性系統在全狀態空間內的動態特性［13］。為了提升基于已知模型所設計的模型預測控制算法應對非線性系統的魯棒性，本文以水下機器人姿態穩定控制為研究目標，提出一種包含被控對象典型非線性特性和線性特性混合有源自回歸（autoregressive with extra inputs， ARX）模型的辨識算法。即根據事先對被辨識對象非線性特性的了解和分析，建立包含典型線性特性和典型非線性特性的待辨識模型結構，并通過選擇合適參數辨識算法得到上述模型參數，從而提高所辨識模型的準確性。通過分析上述水下機器人數學模型可知，在模型中存在兩類典型非線性的計算項：水阻力（力矩）和因機器人重心、浮心位置偏差而產生的恢復力（力矩）。因此需建立待辨識的包含線性項和典型非線性項的多輸入多輸出ARX模型：

式中：X——待辨識模型的線性狀態矢量；Z——與水阻尼力矩特性相關的非線性項矢量；U——模型輸入變量；Λ——與機器人姿態角度相關的非線性恢復力矩項矢量 ；k——當前時刻；k+1 ——當前時刻的下一個時刻；na——模型狀態的階數；nb——模型輸入的階數；Ai、Bj——待辨識模型的狀態參數矩陣、輸入參數矩陣，其中i為狀態參數矩陣Ai在模型中的階數，取值范圍1，…，na，j為輸入參數矩陣Bj在模型中的階數，取值范圍1，…，nb；C、D、E——待辨識模型的水阻尼力特性參數矩陣、恢復力特性參數矩陣和干擾項矩陣。

式（4）所示模型各量計算如下：

將單個多輸入多輸出模型參數辨識問題等效為多個多輸入單輸出的模型參數辨識問題，因此采用如式（6）所示帶遺忘因子的遞歸最小二乘辨識算法進行參數辨識。其中，i=1，2，3，表示該算法中包含3個多輸入單輸出模型。

和h的定義分別如下：

3 基于多維復合ARX模型的MPC算法

3.1 基于多維復合ARX模型的狀態預測

根據第2節模型辨識算法得到的包含非線性項的多輸入多輸出ARX模型表達式，下面推導MPC算法。MPC 控制算法推導通?；诒豢貙ο蟮囊阎€性模型，為降低辨識模型預測控制算法的推導難度，假定在模型預測時間范圍l內，模型中的非線性項為常量，則模型方程式（4）等效為

式中：Ξ——常量，與水下機器人角速度、姿態角相關，表示被控對象的非線性特性和未建模干擾項，Ξ=CZ+DΛ+E。

MPC本質是在已知模型基礎上，依據以往na個時刻采集到的狀態信息和以往nb個時刻采集到的控制輸入信息，計算當前時刻至未來（l-1）個時刻的控制輸出，使得預測的未來l個時刻的狀態值滿足預定的控制目標。因此，根據式（7）可以得到該模型的預測狀態計算方程：

式中：lx——未來第lx時刻，且0 ＜lx≤l；?(*|k) ——第*時刻的狀態值，如果*＞k，則?(*|k)表示未來*時刻的狀態預測值，如果*≤k，則?(*|k) =X(*)，即過去*時刻的已知被控對象狀態；?(*|k) —— 第*時刻的控制輸入值，如果*≥k，則?(*|k)表示未來*時刻的控制輸入值，需要通過預測控制算法計算得到，如果*＜k，則(*|k) =U(*)，即過去*時刻的已知控制輸入值；n?a—— 被控對象的第狀態時刻歷史數據數量；n?b—— 輸入歷史數據數量。

為便于后續控制算法的推導，設定和為分別大于na和nb，且大于預測時間長度l的常數，因此式（7）的模型狀態、輸入數據和模型參數矩陣滿足如下條件：

通過將式（7）中的歷史數據和預測數據分離，得到的預測方程表達形式如下：

由式（8）可知，隨著lx逐漸增加，當lx＞nb時，對應中的因此為零；同理，當lx＞na時，對應的X(k-i+l)為零。所以定義如式（8）所示的和假定?和分別大于na和nb且大于l，可以保證式（9）計算結構形式在0＜lx≤l的計算范圍內具有通用性。下面依據式（9）以及已知歷史數據X(i) （其中i=k，…，k-na+1）和U(i) （其中i=k，…，k-nb+1），分別推導預測未來lx(lx=1，2，3)時刻的狀態預測計算方法。

未來第1時刻的預測，令lx=1，代入式（10），有

未來第2時刻的預測，令lx=2，代入式（10），則

將式（11）代入式（12）可得到

未來第3 時刻的預測，令lx=3 ，代入式（10）可得到

將式（11）和式（12）代入式（14），整理方程后可以得到如下方程：

未來第lx時刻的預測，通過式（11）、式（13）和式（15）可以發現，狀態預測值可以通過有規律的迭代關系進行計算，因此可以得到如式（16）所示的通用狀態預測方程。

其中：

3.2 模型預測控制的直接求解方法

根據上述機器人狀態預測計算公式，將預測結果用方便控制算法求解的矩陣形式表示，并給出基于直接求解的模型預測控制算法。

對應的預測初始化條件如下：

式中：r——式（18）對應向量?的維數，即式（4）的狀態維數，因此r= 3；s—— 式（18）對應向量U的維數，即式（4）的控制輸入維數，因此s= 3。

通過將式（10）至式（17）合并，被控對象未來l個時刻的狀態預測結果可以表示成如下矩陣計算形式：

其中：

假設期望的未來l個時刻的狀態向量為，且預測狀態和期望狀態一致，即則對應的控制輸入需要滿足如下條件：

3.3 模型預測控制的優化求解方法

對實際系統而言，控制執行機構往往受到其最大值和最小值的限制，而通過式（20）計算得到的控制指令未考慮控制執行機構的限制，因此計算結構極容易超出實際控制執行機構的能力。解決控制執行機構受限問題的方法通常有兩種：

1） 設計期望狀態變化過渡過程，減少期望狀態與實際系統狀態的差距，從而通過式（20）得到比較平滑的控制指令；

2） 構建優化求解問題，將控制執行機構限制條件轉化成優化問題的約束條件。

方法1）求解過程相對簡單，只需要設計狀態過渡過程，并將過渡過程通過期望狀態引入方程即可，但控制效果嚴重依賴所設計的過渡過程，計算所得的控制指令是否符合控制執行機構限制要求具有較強的不確定性。方法2）能夠在獲得當前估計模型和狀態的前提下滿足一定性能指標的最優控制效果，算法具有更好的通用性和穩定的控制輸出效果，但計算過程復雜，計算量大，難以滿足控制的實時性要求。

為了達到在算法中既考慮控制受限因素又能提高計算效率的目的，本文在推導最優化目標函數的基礎上，將MPC 最優控制問題轉換成二次規劃問題，這樣能夠顯著提升仿真計算效率，滿足算法在線仿真計算要求。具體算法推導過程如下：

1） 根據MPC控制目標，給出最優化求解問題，具體如式（22）所示。

式中：Q——狀態加權矩陣。

根據被控對象辨識模型式（4），Q是維數為3×3的對角陣。該最優控制問題被表示為在控制輸入限制范圍內搜尋的最優解，使滿足預測方程約束的同時，實現指標最優。

2） 將優化指標表示成矩陣運算形式，如式（23）所示。

3） 將式（19）代入該最優控制問題指標函數，推導后得到二次規劃問題，如式（24）所示。

式中：Ui，min——的最小值；Ui，max——的最大值。

4） 通過采用目前業界公認具有很好計算效率的算子分割二次規劃方法（operator splitting quadratic program， OSQP）［14］，能夠快速尋找二次規劃問題最優解，從而實現在線計算MPC控制輸出。

4 算法仿真驗證

為了驗證本文所述的模型辨識算法和基于辨識模型的預測控制算法的有效性，開發了對應的數值仿真軟件。通過修改模型方程中的lx、ly和lz所代表的水下機器人浮心位置，仿真驗證辨識模型和辨識算法能否準確描述浮心位置變化對機器人非線性狀態耦合特性的影響；在通過方程和所獲得的考慮水下機器人非線性特性的模型的基礎上，比對仿真式（24）和式（21）對應的兩種模型預測控制算法，驗證本文所提出的控制算法的優越性；刪除方程中的非線性項，同樣采用方程對模型參數進行辨識，得到水下機器人線性辨識模型，對比仿真式（24）和式（21）對應的兩種模型預測控制算法，驗證本文所提控制算法應對模型誤差的魯棒性。

4.1 線性與非線性模型辨識仿真對比分析

辨識算法仿真條件為：采樣周期為0.1 s，遺忘因子為0.975，na=2，nb=1，對應參數辨識結果如圖2所示，其中pe、qe、re分別表示水下機器人狀態p、q、r的估計狀態。

圖2 考慮非線性特性下的參數辨識效果Fig .2 Parameter identification effect considering nonlinear characteristics

由圖2（a）、圖2（b）和圖2（c）可知，辨識算法在完成500 步運算之后，所有模型參數基本趨于穩定。由圖2（d）可知，基于所辨識模型參數和式（4）計算得到的機器人估計狀態與對應的基于式（2）的機器人仿真狀態基本一致，說明該考慮機器人非線性特性的模型辨識方法能準確地描述水下機器人狀態的動態變化過程。

為了對比在待辨識模型中考慮非線性特性和不考慮非線性特性的模型辨識效果，刪除式（4）中的非線性項CZ、DΛ和E，得到不考慮非線性特性的待辨識模型，即線性模型：

同樣，采用式（16）對式（25）中的參數進行辨識。由于式（25）中沒有式（4）對應的非線性計算項，因此在式（6）中定義的變量和h也需要做如下修改：

即刪除非線性對應的參數和狀態。線性模型辨識結果如圖3所示。

圖3 不考慮非線性特性下的參數辨識效果Fig .3 Parameter identification effect ignoring nonlinear characteristics

由圖3（c）可知，第3 組模型辨識參數在計算到800步時仍然沒有趨于穩定。對比圖2（b）和圖3（b）對應的參數辨識結果可以發現，在采用相同辨識算法式（6）的情況下，線性模型參數辨識結果的變動幅度要明顯大于考慮非線性模型參數辨識結果的。同樣，對比圖2（d）和圖3（d）可以發現，在考慮非線性特性的情況下，對應的狀態估計值與水下機器人狀態仿真值的跟蹤效果要優于不考慮非線性特性情況下的。因此，以上對比情況表明，在模型中加入被辨識對象的典型非線性特性有利于提高模型的辨識精度。

4.2 不同浮心位置下的模型非線性參數辨識

由第2 節所介紹的辨識算法設計過程可知，為辨識水下機器人重心、浮心變化對模型的影響，在辨識模型中增加了D參數矩陣以及與之相對應的Λ=[sinθcosθsin?cosθcos?]T非線性狀態輸入，因此當lz=-0.5、lx=0、ly=0 時，重心位于浮心正下方0.5 m的位置，對應參數結果如下：

D參數矩陣的第1 行第2 列對應的元素和第2 行第1 列對應的元素明顯大于矩陣中的其他元素，可知水下機器人的滾轉角速度和俯仰角速度更容易受到姿態影響；而第3行參數數值均較小，說明水下機器人的偏航角速度基本不受其姿態的影響。辨識結果與模型式（3）的建模理論結果相符。

當lz=-0.5、lx=0.2、ly=0.2時，對應參考結果如下：

從該參數矩陣可知，同樣，水下機器人的滾轉角速度主要受cosθsin?和cosθcos?影響，俯仰角速度主要受sinθ和cosθcos?影響，偏航角速度主要受sinθ和cosθsin?影響，與式（3）模型相符。此結果說明本論文所引入的與機器人姿態角相關的非線性項能夠準確描述模型參數變化。

4.3 基于非線性辨識模型的預測控制算法對比分析

根據以上模型辨識結果和所設計的MPC控制算法，采用相同的采樣計算周期，設定機器人初始角速度為［0.5 0.1 -0.1］，設定期望的角速度為［0 0 0］，預測周期為30，控制指令最大、最小值分別為1和-1。分別仿真機器人模型的開環狀態、基于式（20）的MPC 閉環控制狀態和基于本論文控制算法的閉環控制狀態，對應仿真結果對比如圖4所示。

圖4 基于復合模型辨識的預測控制效果比較Fig.4 Comparison of MPC performance based on the hybrid model

由圖4可知，兩種基于考慮非線性特性ARX模型的MPC 控制器能夠顯著提高機器人系統振動收斂速度，從而提升系統動力學特性；但由于式（20）沒有考慮輸入限制因素，預測控制算法給出的控制指令超過了機器人指令輸入范圍。通過引入基于模型的預測控制，能夠明顯抑制機器人角速度的振蕩，而且通過將MPC求解問題轉換成考慮輸入限制的優化搜索問題，對應控制指令嚴格控制在機器人最大值和最小值之間，控制效果優于傳統的MPC控制算法。

4.4 基于線性辨識模型的預測控制算法對比分析

在不改變其他條件的基礎上，刪除方程非線性項CZ、DΛ和E?；谝陨暇€性模型及其辨識得到的參數矩陣，分別根據式（21）和式（24），仿真兩種模型預測控制算法的控制效果，結果如圖5所示。

由圖5（a）和圖5（b）可知，由于所采用的線性辨識模型無法準確描述水下機器人的非線性耦合特性，所以通過方程辨識得到的線性模型與水下機器人非線性仿真模型存在較大的建模誤差，且式（20）給出預測控制輸出沒有考慮水下機器人執行機構的飽和特性，因此該控制算法不僅沒有提升機器人系統動態性能，甚至還破壞了原有系統的穩定性。而采用本文提出的基于優化求解的預測控制算法，雖然控制效果不如基于考慮非線性特性的辨識模型對應控制算法的，但由于其將控制指令始終限制在1和-1之間，仍能夠起到穩定系統、優化動態特性的作用，控制效果如圖5（c）和5（d）所示。

因此，基于該仿真結果，可以得到如下結論：

1） 基于模型的MPC 算法對模型精度要求高，對于非線性模型，采用傳統基于線性模型的MPC算法無法達到穩定系統、優化動態性能的控制效果；2） 將模型MPC問題轉化成考慮輸入范圍的最優化求解問題，能夠提高模型MPC算法針對建模誤差的魯棒性。

5 結束語

本文在建立水下機器人全狀態非線性模型基礎上，通過分析模型典型非線性特性因素，建立了包含水下機器人典型非線性特性的辨識模型；采用帶遺忘因子的遞歸最小二乘算法實現了模型參數的在線辨識。通過仿真，將本文提出的模型辨識方法和傳統線性模型辨識方法進行對比分析，驗證了本文所提非線性模型辨識方法的優越性。在辨識得到的典型非線性特征的多輸入多輸出模型基礎上，本文進一步推導建立了多維MPC 算法，通過將MPC 算法求解問題轉化成考慮模型輸入限制的最優二次規劃問題，并采用分割二次規劃算法進行最優二次規劃問題求解，提高了該MPC算法執行效率，實現了算法的在線仿真。最后，通過將文中介紹的兩種MPC方法進行仿真比較，顯示了本文所提出控制算法在提高被控對象動態特性和應對模型誤差等方面的優越性。