基于隐式离散化超螺旋算法的水下机器人姿态控制

丁宁宁，唐元贵，姜志斌

(1.中国科学院中沈阳自动化研究所中机器人学国家重点实验室，辽宁 沈阳 110016；2.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110169；3.辽宁省水下机器人重点实验室，辽宁 沈阳 110169；4.中国科学院大学，北京 100049)

0 引 言

海洋中深度大于 6 500 m 的海域被称为“深渊”，深渊区域大深度、超高压的工作环境对于水下机器人是十分严峻的挑战，因此人类对深渊的认知还在初级阶段[1]。面向深渊探测，全海深自主遥控水下机器人(ARV) 应运而生。ARV 作为一种混合型水下机器人，其融合了自主水下机器人(AUV)和遥控水下机器人(ROV)的部分技术。相较于传统 ROV，ARV 的作业范围增大、对母船支持要求降低但作业能力也相应减弱。相较于AUV，其特点为巡航作业时可进行实时监控，一旦发现兴趣点，即可切换为 ROV 模式利用机械手完成作业[2]。作业能力是 ARV 的一项重要能力，当ARV 进行悬浮作业时，由于机械臂对载体的耦合作用，会导致载体姿态发生较大变化，反过来又会影响机械臂作业。因此载体在机械臂扰动下的姿态稳定控制对于顺利完成水下作业十分重要。

目前已经有很多控制算法被应用于水下机器人的控制中，如 PID 控制[3]、预测控制[4-5]、模糊逻辑控制[6-7]、基于神经网络的控制[8-9]、滑模控制(SMC)[10-14]等。其中滑模控制对满足匹配条件的干扰和摄动具有较强鲁棒性，且算法简单易于工程实现，在水下机器人上得到了广泛应用。SMC的主要缺点在于存在抖振问题。对于水下机器人而言，抖振会增加推进器能耗甚至损坏推进器。在抖振抑制方面，高阶滑模控制近年来受到了广泛关注，其将不连续项隐藏到高阶导数中，经过积分器的滤波作用，抖振得到了很大抑制。高阶滑模中的超螺旋算法 (Super-Twisting Algorithm，STA) 是其中最流行的算法，因为其不需要知道滑模变量的导数信息。STA 在保持滑模控制鲁棒性的同时具有良好的抖振抑制能力，然而它的离散实现多采用显式欧拉法，其在无扰动时也会引发额外的抖振现象，称为离散化抖振，而且离散化抖振会随着采样周期或控制增益值的增大而加剧[15]。为了抑制SMC采用显式离散化引发的抖振，隐式(后向)欧拉离散法近年来得到了发展[16]，理论和实验都验证了其可有效抑制离散化抖振[17-18]。隐式法最近也被用到了STA上[19-20]，仿真与实验结果显示基于隐式离散化的STA有效抑制了离散化抖振且对采样周期和过高的控制增益不敏感。

为了稳定机械臂扰动下的载体姿态，本文首次将隐式离散化后的STA[19]应用到全海深ARV在悬浮作业时的姿态稳定控制上。

1 系统建模

ARV的运动学和动力学模型涉及到2个坐标系，一个固定的大地坐标系{e}用于确定ARV在流场中的位置，一个体坐标系{b}固定于ARV上并随其一起运动, 如图1所示。

图1 ARV坐标系Fig.1 The coordinate frames of ARV

1）横倾力矩方程

ARV六自由度动力学模型在文献[21]中进行了详细的说明。由于机械臂运动主要影响的是横倾和纵倾这2个自由度，因此仅对这2个自由度的控制进行研究。考虑机械臂扰动，未知环境扰动和未建模动力学后的纵倾和横倾自由度的运动学和动力学模型如下：

2）纵倾力矩方程

其中：m为载体的质量,W为重力大小，B为浮力大小,[xGyGzG]T表示重心在体坐标系中的坐标，浮心坐标为[xByBzB]T, [IxIyIz]T和 [IxyIyzIzx]T分别表示惯性矩和惯性积，φ 表示横倾角， θ表示纵倾角，[p q r]T表示姿态角速度， δs表示舵角， Δfφ和 Δfθ为横倾和纵倾通道中模型不确定性项和未建模动力学项，uφ和uθ和为施加在xb轴和yb轴 上的广义力矩，dm,φ和dm,θ为机械臂扰动项在xb轴和yb轴 上的分量，du,φ和du,θ为施加在xb轴和yb轴上的未知时变扰动。

由于横倾自由度和纵倾自由度分析完全类似，因此这里仅取横倾自由度进行分析。为了便于控制器设计，式(1)可简化为：

其中：

联立式(5)和式(2), 可以得到惯性坐标系下横倾自由度的动力学方程：

其中：

为了便于控制器设计，横倾和纵倾自由度的动力学方程不失一般性地可写成如下通用形式：

其中x可表示横倾角φ或纵倾角 θ变量。

2 控制器设计

2.1 连续域下的控制器设计

为了得到STA的离散化形式，需要回顾STA在连续域下的设计。为了在水下机器人这个2阶系统上应用STA，首先要进行滑模面设计，使得控制输入出现在滑模变量的1阶导上。为了简便，这里选择如下的线性滑模面：

其中：e=x-xd，xd为状态变量的期望值， λ为线性滑模面的增益，影响误差的收敛速率。

滑模趋近率采用STA：

其中：k1，k2为STA控制增益，均为正常数。

对滑模变量微分可得:

比较式(11)和式(10), 在连续域下的控制器可设计为：

为了测试算法的鲁棒性以及降低控制器对模型的依赖，控制器并没有使用名义动力学模型信息。连续域下闭环系统的稳定性证明可参考文献[22]。

2.2 控制算法的隐式离散化

为了抑制离散化引发的抖振，采用隐式欧拉法对式(12)进行离散化。

对于式(11)，其可改写成如下紧凑的形式：

式(13)的离散化模型为:

其中：h为采样周期；fk为f(tk)的简化表示；和分别为和的近似。对于简单的显式欧拉法有

针对式(14), 采用隐式欧拉法的STA控制器可设计为[19]：

不同于显式欧拉法使用的单值符号函数，式(15)的 sg n()为集值符号函数，即当x=0,s g n(x)∈[-1,1]而不是等于0。为了求解需要引入一些凸分析的概念。定义K⊆Rn为一个非空闭凸集，它在x⊆Rn上的正交锥定义为:

对于x⊆[-1,1]，有

根据凸分析，N[-1,1](x)为集值符号函数的逆函数，即x∈sgn(y)⇒y∈N[-1,1](x)。

将控制输入式(15)代入式(18)中，可得到无扰动虚拟闭环系统：

式(19)可写成如下的等价形式：

注意到 s gn(0)=[-1,1]，因此式(20)的确是集值的，且是一个含有未知变量的广义方程。下面证明在t=tk时具有解析解，并且是唯一的。

广义方程式(20)可写成如下紧凑的形式：

最终可得到如下广义方程：

定理1：广义方程式(23)和式(21)仅有唯一解

证明：注意到函数g()和f()均是连续的.根据文献[23]中推论2.2.5可得到式(23)至少有一个解, 而且解集是紧集。定义式(23)等价于

可以验证对于所有x≠ck，有当x=ck，即f()是一个单调递增函数，且x=ck时其函数值和导数值均为0，故可大致画出其图形，如图2所示。根据文献[23]中命题2.3.2和定理2.3.3,可得到广义方程式(23)总有唯一解，这意味着集值映射和单值映射的曲线在图2中有唯一的交点。

图2 广义等式(24)的图解Fig.2 Graphical interpretation of the generalized equation (24)

由于式(21)中的2个映射为式(23)中相应副本的逆，因此它们的交点也是唯一的。这意味着在t=tk时刻,是唯一的，因此式(15)中定义的控制器也是唯一的。为了求解根据图2可做出如下分类：

3 仿真实验

为了验证所提出控制算法的有效性和优越性，在Matlab/Simscape环境下进行仿真实验。

3.1 任务描述

仿真中载体和机械臂的部分参数如表1所示。为了问题的简化及排除其他因素的影响，仿真中忽略推进器动力学特性，传感器获取的信号假设是理想的。在仿真过程中，机械臂进行运动，规划的关节轨迹如3所示。与此同时载体则依靠自身的控制维持姿态的稳定。控制器分别采用显式欧拉法和隐式欧拉法离散的STA，且仅考虑横倾和纵倾这2个自由度的控制。其中载体的初始位姿和速度均设置为0，纵倾角和横倾角的目标值也设置为0。为了测试控制算法的鲁棒性，模型参数假设存在30% ～ 40%的不确定性，施加的时变外部扰动如下：

表1 ARV部分参数值Tab.1 Partial parameter values of the ARV

为了研究采样周期对2种离散化方法的影响，仿真中的采样周期分别设置为0.001 s，0.01 s和0.1 s。

图3 规划的关节轨迹Fig.3 Planned joint trajectories

3.2 控制参数优化

为了排除参数选择对控制性能的影响，确保对比仿真实验的公平性，采用多目标优化算法进行控制参数调优。

为了评价控制参数的优劣，引入如下2个目标函数：

第1个目标函数计算误差的均方根，用来评价姿态的稳定控制性能；第2个目标函数计算了控制输入的平均变化值，可用来衡量控制输入的抖振大小。这2个目标函数值都是越小越好。

待优化的参数是 λ，k1和k2这3个参数，即它们将作为多目标优化的设计变量。为了提升效率，先手动调参确定大致的范围。控制参数整定范围为：

为了避免推进器饱和，控制输入也限定在一定范围内：uφ∈[-250,50]，uθ∈[-70,230]。

多目标优化采用Matlab中Global Optimization Toolbox中的gamultiobj函数实现。

3.3 仿真结果与分析

图4为不同采样周期下经多目标优化得到的Pareto前沿, Pareto前沿上的点称为Pareto最优解。可以发现隐式法对应的Pareto最优解CHAT值总体来看远小于显式法，且集中在一个较小值, 这表明隐式法有效地抑制了离散化抖振。显式法的Pareto最优解CHAT值则分布在一个较大范围内，表明显式法容易引发较大的抖振。对比RMSE值可发现显式法可达到更小的值，代价是产生了更大的抖振。对比不同采样周期的Pareto前沿，发现随着采样周期的增加，显式法和隐式法对应的Pareto前沿均向右移动，然而隐式法Pareto最优解的CHAT值增加较小，仍然集中在一个较小值。显式法Pareto最优解对应的CHAT值分布范围明显扩大，这说明显式法对采样周期的增加较敏感，采样周期的增加明显增大了离散化抖振。

图4 不同采样周期对应的Pareto前沿Fig.4 Pareto fronts for different sampling time

为了分析2种离散化方法对增益的敏感性，图5给出了采样时间周期为0.1 s时得到的Pareto最优解对应的设计变量值分布图，即控制器参数值分布图。从图5可以发现，显式法的滑模面增益λ集中在较大处；对比隐式法对应的滑模面增益分布和Pareto前沿RMSE值的分布，可发现滑模面增益越大，RMSE值越小。对于STA趋近律相关增益k1和k2，显式法只能取很小的值，否则便会引发剧烈抖振甚至使系统不稳定。这说明显式法对较大增益很敏感，这会导致手动调参变得困难。隐式法则可在整个限定区间取值，可取较大增益值，说明隐式法对较大增益不敏感，控制参数的调优更加容易。

图5 设计变量分布图Fig.5 The scattergram of design variables

为了更加直观地展示隐式法相对显式法的区别和优势, 在采样周期0.1s对应的 Pareto 前沿上选择最优设计点进行仿真实验。为了确保公平的对比，在 RMSE值相似的情况下选取对应的最优点，选取的 Pareto 最优设计点对应的目标函数值如表2所示，相应的设计变量值如表3所示。

表2 选取的最优设计点目标函数值Tab.2 Objective function values for selected optimum design points

表3 选取的设计变量值Tab.3 The selected design variable values

图6为横倾角和纵倾角随时间变化曲线图。由于选择了相似的RMSE值，可看到两者的控制性能大致相当。显式法对应的姿态角在初始阶段振荡较大，不过可实现较小的稳态误差，隐式法对应的姿态角则比较平滑，但稳态误差较大，不过根据RMSE的性能指标，两者可认为大致相当。图7为控制输入随时间变化曲线图，从图中的放大视图可以发现显式法对应的控制输入存在明显的抖振现象，隐式法则得到平滑的的控制输入。

图6 横倾角和纵倾角随时间变化曲线Fig.6 Time trajectories of the roll angle and pitch angle

图7 控制输入随时间变化曲线Fig.7 Time trajectories of control inputs

4 结 语

本文针对具有作业能力的全海深ARV悬浮作业时载体的姿态控制问题提出了一种隐式离散化广义超螺旋算法，并基于多目标优化算法完成了控制器参数优化。仿真结果表明，所提出的算法有效抑制了STA离散化带来的抖振，且控制算法对采样周期和过高控制增益不敏感。未来的工作包括将隐式离散化方法扩展到广义超螺旋算法[24]以及进行水池实验。