遥控水下机器人模型辨识及预测控制算法研究

朱迎谷，张定华，涂绍平，宋俊辉

（上海中车艾森迪海洋装备有限公司，上海 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方法进行仿真比较，显示了本文所提出控制算法在提高被控对象动态特性和应对模型误差等方面的优越性。