付雯¹, 温浩¹, 黄俊珲¹, 孙镔轩¹, 陈嘉杰², 陈武², 冯跃¹, 段星光¹
1. 北京理工大学 机电学院，北京 100081;
2. 中国广核集团有限公司，中广核研究院有限公司，深圳 518000
收稿日期：2022-11-11
基金项目：国家重点研发计划项目(2019YFB1310503)
作者简介：付雯(1998—)，女，硕士研究生
通讯作者：段星光，教授，E-mail: duanstar@bit.edu.cn

摘要：为满足特大型水利水电工程中的大直径超长距离引水隧洞定期检测的重大需求，智能化水下机器人系统成为当前的研究热点。为提高水下机械臂建模的准确性与控制能力的精准性，该文首先提出一种融合Newton-Euler方程、Morison方程与非线性摩擦力的水下机械臂动力学模型建模及参数辨识方法，并在补偿已辨识模型的基础上，设计了一种利用径向基函数(radial basis function，RBF)神经网络补偿系统未建模与建模误差的自适应滑模控制方法。通过仿真，该文证明了该方法比传统比例积分微分(proportional integral differential，PID)控制和一般RBF网络自适应滑模控制具有更高的控制精度。
关键词：水下机械臂动力学参数辨识径向基函数神经网络补偿自适应滑模控制
Adaptive sliding mode control of underwater manipulator based on nonlinear dynamics model compensation
FU Wen¹, WEN Hao¹, HUANG Junhui¹, SUN Binxuan¹, CHEN Jiajie², CHEN Wu², FENG Yue¹, DUAN Xingguang¹
1. School of Mechatronical Engineering, Beijing Insitute of Technology, Beijing 100081, China;
2. China Nuclear Power Technology Research Institute Co., Ltd., China General Nuclear Power Group, Shenzhen 518000, China

Abstract: Objectives The South-to-North water diversion project is a strategic project in China. Since its construction, it has become the main source of water conservancy in more than 280 cities.The diversion tunnel is the key building to support the South-to-North water diversion project. Due to its long line, large diameter, high water pressure, complex surrounding rock geology, as well as many years of water conservancy erosion, biochemical substances erosion, geological effect and other influences, typical defects such as cracks, collapse, exposed steel bars are prone to occur. Artificial detection of defects in the tunnel not only consumes time and energy, but also has low accuracy and timeliness. Therefore, underwater robot inspection technology has become a hotspot of current research.Among them, the underwater manipulator can not only be installed on the underwater vehicle, but also can be selectively installed on the required platform to complete the tasks of cleaning the water surface, laying and repairing cables, salvaging sunken objects, cutting off ropes and so on. However, the control of the underwater manipulator is more complicated and difficult due to its time-varying mechanics, nonlinear properties, external interference and hydrodynamic influence. The main purpose of this paper is to establish the dynamics model of the underwater manipulator and improve the accuracy of the trajectory tracking of the manipulator. Methods In this paper, a modeling method combining Newton-Euler equation and Morrison's dynamic model is proposed, and then the dynamic parameters are identified. Then, in order to improve the precise control ability of the manipulator in complex transient underwater environment, an adaptive sliding mode control method is designed based on compensating nonlinear dynamics model and using radial basis function (RBF) neural network to compensate the unmodeled and modeling errors of the system. Through the dynamic modeling in Section 4, a detailed dynamic simulation environment of the underwater manipulator is obtained. Gaussian noise errors with amplitudes of 5, 20, 15, 10, 8, and 5 N·m are set for each joint. On this basis, Experiment 1(P1): double loop proportional integral differential (PID) controller is designed for control simulation. Then, in experiment 2(P2), RBF neural network is used to make fitting compensation for system modeling errors and unmodeled items. In experiment 3(P3), dynamic model compensation is added on the basis of P2. Results The trajectory tracking effect ratio of P2 and P3 was obviously better than that of P1 experiment, and the tracking effect of P3 experiment was also better than that of P2 experiment after compensating the dynamic model. Conclusions Through simulation, this paper has proved the effectiveness of the proposed hydrodynamic modeling of the manipulator, and on the basis of compensating nonlinear dynamic model, The adaptive sliding mode control method using RBF neural network to compensate the unmodeled and modeling errors of the system has higher trajectory tracking accuracy than the traditional PID control and the general RBF network adaptive sliding mode control.
Key words: underwater manipulatordynamic parameters identificationradial basis function neural network compensationadaptive sliding mode control
中国水利水电工程进入国际先进行列，其中南水北调工程是中国的战略性工程，自建成以来已成为280多个县市的主要水利来源。由于支撑南水北调工程的关键建筑物——引水隧洞具有洞线长、洞径大、水压高、围岩地质复杂的情况，且常年受水力冲蚀、生化材料侵蚀等地质作用的影响，开始出现裂缝、塌方和露筋等典型缺陷。人工检测缺陷不仅耗时耗力，且检测的精度和时效性都很低，因此亟须取得水下机器人巡检技术的新突破，以便快速、高效地完成水下复杂环境的检测。智能水下机器人是目前研究的热点，其中水下机械臂不仅可以安装在水下航行器上，还可以选择性地安装在需要的平台上，完成清理水面、电缆敷设和修理、打捞沉物及切断绳索等任务^[1-4]。水下机械臂因自身耦合动力学及水动力学环境影响，使水下机械臂的动力学模型具有非线性瞬变特性，因此其动力学建模^[5]和运动关节的高精度控制^[6-8]成为面临的主要挑战。
本文首先提出了一种融合Newton-Euler方程、Morison方程与非线性摩擦力的水下机械臂动力学模型建模方法，并在此基础上完成了动力学参数辨识。为提高机械臂在复杂瞬变水下环境的精准控制能力，设计了一种在补偿非线性动力学模型基础上利用径向基函数(radial basis function，RBF)神经网络补偿系统未建模与建模误差的自适应滑模控制方法。通过仿真，证明了该方法比传统的比例积分微分(proportional integral differential，PID)控制和一般的RBF网络自适应滑模控制具有更高的轨迹跟踪精度，即控制精度。
1 改进的机械臂水动力学模型水下机械臂动力学模型需要考虑重力、惯性力、离心力、Coriolis力、摩擦力、水阻力、附加质量力和浮力等环境因素，导致模型复杂难以求解。本文在现有的水下机械臂动力学模型基础上，提出一种改进的易于计算的动力学模型。建模方式为：1) 引入考虑非线性黏滞与温度影响的摩擦力模型；2) 将水阻力、附加质量力以外力的形式引入机械臂动力学模型的计算过程，简化模型的求解过程。
1.1 机械臂动力学建模本文研究的六轴水下机械臂由k个关节与k个连杆组成，假设机械臂各个关节都是刚性连接，根据文[9-10]可建立机械臂的刚性动力学模型。各个关节坐标系速度与加速度的传递关系如下：

$\begin{array}{c}{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen, }}n + 1}} = _n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen, }}n}} + {{\dot \theta }_{n + 1}}^{n + 1}{{\mathit{\boldsymbol{\hat Z}}}_{n + 1}}, \\{\mathit{\boldsymbol{\theta }}_{{\rm{gen}}, 1}} = {\left[ {\begin{array}{*{20}{l}}{0, }&{0, }&0\end{array}} \right]^{\rm{T}}}{\rm{. }}\end{array}$

(1)

其中：$\mathit{\boldsymbol{\dot \theta }}$_{gen, n}∈R^3×1，为第n(n=1, 2，…, k)个连杆相对于广义坐标系的角速度，R^a×b为a×b矩阵，R^a×1为a×1向量；_nⁿ⁺¹T_ro∈R^3×3为机械臂第n个关节移动到第n+1个关节的旋转矩阵；θ_n、$\dot \theta $_n和$\ddot \theta $_n分别表示第n个关节的角度、角速度和角加速度；ⁿ$\mathit{\boldsymbol{\hat Z}}$_n∈R^3×1，为第n个连杆相对于广义坐标系的旋转轴方向向量。
角加速度的传递关系如下：

$\begin{array}{c}{{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n + 1}} = _n^{n + 1}{T_{{\rm{ro}}}}{{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n}} + \left( {_n^{n + 1}{T_{{\rm{ro}}}}{\mathit{\boldsymbol{{\dot \theta }}}_{{\rm{gen}}, n}}} \right) \times \\\left( {{{\dot \theta }_{n + 1}}{{\mathit{\boldsymbol{\hat Z}}}_{n + 1}}} \right) + {{\ddot \theta }_{n + 1}}{{\mathit{\boldsymbol{\hat Z}}}_{n + 1}}{\rm{, }}\\{{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, 1}} = {\left[ {\begin{array}{*{20}{c}}{0, 0, 0}\end{array}} \right]^{\rm{T}}}.\end{array}$

(2)

线速度的传递关系如下：

${\mathit{\boldsymbol{v}}_{n + 1}} = _n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}\left( {{\mathit{\boldsymbol{v}}_n} + {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times _n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{po}}}}} \right), {\mathit{\boldsymbol{v}}_1} = {[0, 0, 0]^{\rm{T}}}.$

(3)

其中：$\mathit{\boldsymbol{ v}}$_n∈R^3×1，为第n个连杆相对于广义坐标系的线速度; _nⁿ⁺¹T_po∈R^3×1, 为机械臂第n个关节移动到第n+1个关节的平移矩阵。
线加速度的传递关系如下：

$\begin{array}{c}{{\mathit{\boldsymbol{\dot v}}}_{n + 1}} = _n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}\left( {{{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n}} \times _n^{n + 1}{{\bf{T}}_{{\rm{po}}}} + {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times } \right.\\\left. {\left( {{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times _n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{po}}}}} \right) + {{\mathit{\boldsymbol{\dot v}}}_n}} \right), \\{{\mathit{\boldsymbol{\dot v}}}_1} = {[0, 0, g]^{\rm{T}}}{\rm{. }}\end{array}$

(4)

其中：${\mathit{\boldsymbol{\dot v}}}$_n∈R^3×1，为第n个连杆相对于广义坐标系的线加速度，g为重力加速度。
质心位置线速度和线加速度为：

${\mathit{\boldsymbol{v}}_{{\rm{cen}}, n}} = {\mathit{\boldsymbol{\dot \theta }}_{{\rm{gen}}, n}} \times {\mathit{\boldsymbol{P}}_{{\rm{cen}}, n}} + {\mathit{\boldsymbol{v}}_n}, \quad {\mathit{\boldsymbol{v}}_{{\rm{cen}}, 0}} = {[0, 0, 0]^{\rm{T}}};$

(5)

$\begin{array}{c}{{\mathit{\boldsymbol{\dot v}}}_{{\rm{cen}}, n}} = {{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n}} \times {\mathit{\boldsymbol{P}}_{{\rm{cen}}, n}} + {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times \left( {{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times {\mathit{\boldsymbol{P}}_{{\rm{cen, }}n}}} \right) + \\{{\mathit{\boldsymbol{\dot v}}}_n}, \quad {{\mathit{\boldsymbol{\dot v}}}_{{\rm{cen}}, 0}} = {[0, 0, g]^{\rm{T}}}{\rm{.}}\end{array}$

(6)

其中：v_{cen, n}∈R^3×1, ${\mathit{\boldsymbol{\dot v}}}$_{cen, n}∈R^3×1, 分别为第n个连杆相对于质心坐标系的线速度和线加速度; P_{cen, n}∈ R^3×1, 为第n个连杆相对于广义坐标系下质心位置向量。
利用Newton-Euler公式分别计算位于质心位置处的惯性力和质心力矩为：

${\mathit{\boldsymbol{F}}_{{\rm{gen}}, n + 1}} = {m_{n + 1}}{\mathit{\boldsymbol{\dot v}}_{{\rm{cen}}, n + 1}}{\rm{, }}$

(7)

${\mathit{\boldsymbol{N}}_{n + 1}} = {\mathit{\boldsymbol{I}}_{{\rm{cen}}, n + 1}}{\mathit{\boldsymbol{\ddot \theta }}_{{\rm{gen}}, n + 1}} + {\mathit{\boldsymbol{\dot \theta }}_{{\rm{gen}}, n + 1}} \times \left( {{\mathit{\boldsymbol{I}}_{{\rm{cen}}, n + 1}}{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n + 1}}} \right).$

(8)

其中: F_{cen, n}∈R^3×1, 为第n个关节相对于质心坐标系的惯性力; N_n∈R^3×1, 为第n个关节相对于质心坐标系的力矩; m_n为第n个连杆的质量; I_{cen, n}∈ R^3×3, 为第n个连杆相对于质心坐标系的惯性。
分别根据力平衡方程和力矩平衡方程可得：

${\mathit{\boldsymbol{F}}_n} = _{n + 1}^n{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}{\mathit{\boldsymbol{F}}_{n + 1}} + {\mathit{\boldsymbol{F}}_{{\rm{cen}}, n}}, \quad {\mathit{\boldsymbol{F}}_k} = {\left[ {\begin{array}{*{20}{c}}{0, 0, 0}\end{array}} \right]^{\rm{T}}};$

(9)

$\begin{array}{c}{\mathit{\boldsymbol{\tau }}_{{\rm{NE}}, n}} = {\mathit{\boldsymbol{N}}_n} + _{n + 1}^n{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}{\mathit{\boldsymbol{\tau }}_{{\rm{NE}}, n}} + {\mathit{\boldsymbol{P}}_{{\rm{cen}}, n}} \times {\mathit{\boldsymbol{F}}_{{\rm{cen}}, n}} + \\_n^{n + 1}{\mathit{\boldsymbol{T}}_{{\rm{po}}}} \times \left( {_{n + 1}^n{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}{\mathit{\boldsymbol{F}}_{n + 1}}} \right).\end{array}$

(10)

其中：F_n∈R^3×1，为机械臂各个连杆的受力；τ_{NE, n}∈R^3×1，为基于Newton-Euler公式计算的第n个关节的关节力矩。
由机械臂惯性力、Coriolis力、离心力和重力组成的关节力矩τ_MCG∈R^k×1，表示如下：

${\mathit{\boldsymbol{\tau }}_{{\rm{MCG}}}}(n) = \mathit{\boldsymbol{\tau }}_{{\rm{NE}}{,n}}^{\rm{T}}{\mathit{\boldsymbol{\hat Z}}_n}.$

(11)

此外，τ_MCG可表示为

${\mathit{\boldsymbol{\tau }}_{{\rm{MCG}}}} = {\mathit{\boldsymbol{M}}_{{\rm{PDI}}}}(\mathit{\boldsymbol{q}})\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}(\mathit{\boldsymbol{q}}, \mathit{\boldsymbol{\dot q}})\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{G}}(\mathit{\boldsymbol{q}}).$

(12)

其中：$\mathit{\boldsymbol{q、\dot q、\ddot q}}$∈R^k×1，分别表示机械臂k个关节的角度、角速度和角加速度向量，其由θ_n、$\dot \theta $_n和$\ddot \theta $_n组成，q =[θ₁, θ₂, …, θ_k]^T, $\mathit{\boldsymbol{\dot q}}$=[$\dot \theta $₁, $\dot \theta $₂, …, $\dot \theta $_k]^T, $\mathit{\boldsymbol{\ddot q}}$=[$\ddot \theta $₁, $\ddot \theta $₂, …, $\ddot \theta $_k]^T；M_PDI(q)∈R^k×k, 为正定的惯性矩阵; C(q, $\mathit{\boldsymbol{\dot q}}$)∈R^k×k, 为Coriolis-向心矩阵; G(q)∈R^k+1, 为重力。
机械臂各个关节处由于具有高减速比的谐波减速器，因此电机惯性力矩也应被考虑，表示如下：

${\mathit{\boldsymbol{\tau }}_{{\rm{iner}}}} = {\mathit{\boldsymbol{I}}_{{\rm{motor}}}}\mathit{\boldsymbol{\ddot q}}.$

(13)

其中：I_motor∈R^k×1, 为电机的转动惯量; τ_iner∈R^k×1, 为机械臂各个关节电机的惯性力矩。
1.2 非线性摩擦力建模非零速区域的电机摩擦模型通常可描述为：

${\mathit{\boldsymbol{\tau }}_{{F_{{\rm{fri}}}}}} = \left( {{\mathit{\boldsymbol{\tau }}_{{F_{{\rm{coul}}}}}} + {\mathit{\boldsymbol{\tau }}_{{F_{{\rm{vis}}}}}}} \right){\rm{sign}}(\mathit{\boldsymbol{\dot q}}), $

(14)

${\mathit{\boldsymbol{\tau }}_{{F_{{\rm{vis}}}}}} = {\mathit{\boldsymbol{f}}_{{\rm{vis}}}}|\mathit{\boldsymbol{\dot q}}|.$

(15)

其中：τ_Ffri∈R^k×1, 为关节电机摩擦力矩；τ_Fcoul= f_coul, 为Coulomb摩擦力矩，f_coul∈R^k×1, 为Coulomb摩擦系数向量；τ_Fvis为黏性摩擦力矩；f_vis∈R^k×1, 为黏性摩擦系数向量。
然而文[11-12]中通过实验证明关节电机的黏性摩擦力通常与速度并不呈线性关系，通过引入指数参数向量δ_vis∈R^k×1，可更逼近真实的黏性摩擦力矩，即

${\mathit{\boldsymbol{\tau }}_{{F_{{\rm{vis}}}}}} = {\mathit{\boldsymbol{f}}_{{\rm{vis}}}}|\mathit{\boldsymbol{\dot q}}{|^{{\mathit{\boldsymbol{\delta }}_{{\rm{vis}}}}}}.$

(16)

此外，文[11]中也指出温度对于摩擦力具有明显的非线性影响，在此基础上，本文利用线性模型形式来表征温度K对摩擦模型中f_vis、δ_vis、f_coul的影响，即f_coul由z个温度影响下的Coulomb系数向量f_{coul, z}与温度K计算得到, f_vis由z个温度影响下的黏性摩擦系数向量f_{vis, z}与温度K计算得到, δ_vis由z个温度影响下的指数参数向量δ_{vis, z}与温度K计算得到，分别表示如下：

$\begin{array}{c}{\mathit{\boldsymbol{f}}_{{\rm{coul }}}} = \sum\limits_{z = 0}^1 {{\mathit{\boldsymbol{f}}_{{\rm{coul }}, z}}} {K^z}, \\{\mathit{\boldsymbol{f}}_{{\rm{vis}}}} = \sum\limits_{z = 0}^1 {{\mathit{\boldsymbol{f}}_{{\rm{vis}}, z}}} {K^z}, \\{\mathit{\boldsymbol{\delta }}_{{\rm{vis}}}} = \sum\limits_{z = 0}^1 {{\mathit{\boldsymbol{\delta }}_{{\rm{vis}}, z}}} {K^z}{\rm{. }}\end{array}$

(17)

1.3 机械臂水动力学建模在水下环境中，水下机械臂的各个关节都会受到自身运动和水流流动的影响。本文机械臂主要应用于长引水隧洞静水期检修任务，因此假设工况条件为完全静水状态。根据文[13-14]，机械臂关节n单位长度的物体与水之间的相互作用力F_{water, n}∈R^3×1, 可描述为

$\frac{{{\rm{d}}{\mathit{\boldsymbol{F}}_{{\rm{water, }}n}}}}{{{\rm{d}}{\mathit{\boldsymbol{l}}_n}}} = \frac{{{\rm{d}}{\mathit{\boldsymbol{F}}_{{\rm{drag}}, n}}}}{{{\rm{d}}{\mathit{\boldsymbol{l}}_n}}} + \frac{{{\rm{d}}{\mathit{\boldsymbol{F}}_{{\rm{mass}}, n}}}}{{{\rm{d}}{\mathit{\boldsymbol{l}}_n}}} + \frac{{{\rm{d}}{\mathit{\boldsymbol{F}}_{{\rm{lift, }}n}}}}{{{\rm{d}}{\mathit{\boldsymbol{l}}_n}}} + \frac{{{\rm{d}}{\mathit{\boldsymbol{F}}_{{\rm{float}}, n}}}}{{{\rm{d}}{\mathit{\boldsymbol{l}}_n}}}.$

(18)

其中：F_{drag, n}∈R^3×1，为第n个关节的水阻力；F_{mass, n}∈R^3×1，为第n个关节的附加质量力；F_{lift, n}∈ R^3×1，为第n个关节的升力；F_{float, n}∈R^3×1，为第n个关节的浮力；l_n∈R^3×1，为第n个关节连杆的长度向量；且F_{drag, n}、F_{mass, n}、F_{lift, n}、F_{float, n}和l_n都相对于广义坐标系建立。由于水下机械臂没有翼型结构，因此不考虑F_{lift, n}对水下机械臂的影响。第n个连杆的F_{float, n}可表示为

${F_{{\rm{float, }}n}} = {m_n}g - {b_n}{\rho _{{\rm{water }}}}g = {m_n}g\left( {1 - \frac{{{\rho _{{\rm{water }}}}}}{{{\rho _{{\rm{arm }}, n}}}}} \right).$

(19)

其中：b_n为第n个连杆的体积，ρ_water为流体密度，ρ_{arm, n}为第n个机械臂的密度。
浮力对机械臂各个连杆的作用竖直向上且可认为浮力作用于各连杆质心，因此，通过修改式(6)的初始条件，即将g改为g·ρ_water/ρ_{arm, 1}, 则可将浮力代入机械臂动力学模型中，可得下式：

$\begin{array}{c}{{\mathit{\boldsymbol{\dot v}}}_{{\rm{cen}}, n}} = {{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n}} \times {\mathit{\boldsymbol{P}}_{{\rm{cen}}, n}} + {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times \left( {{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times } \right.\\\left. {{\mathit{\boldsymbol{P}}_{{\rm{cen}}, n}}} \right) + {{\mathit{\boldsymbol{\dot v}}}_n}, \\{{\mathit{\boldsymbol{\dot v}}}_{{\rm{cen,}}0}} = {\left[ {0,0,g \cdot {\rho _{{\rm{water}}}}/{\rho _{{\rm{arm}},1}}} \right]^{\rm{T}}}{\rm{.}}\end{array}$

(20)

利用Morison公式可有效计算水阻力和附加质量力如下：

$\begin{array}{c}{{\mathit{\boldsymbol{F}}}_{{\rm{drag}}, n}} = \\\int_0^{{l_n}} {\frac{1}{2}} {\rho _{{\rm{water}}}}{C_{{\rm{drag}}}}{S_{{\rm{drag}}, n}} \cdot {\mathit{\boldsymbol{v}}_{{\rm{flow}}, n}}(\mathit{\boldsymbol{L}})\left\| {{\mathit{\boldsymbol{v}}_{{\rm{flow, }}n}}(\mathit{\boldsymbol{L}})} \right\| \cdot {\rm{d}}\mathit{\boldsymbol{L, }}\\{\mathit{\boldsymbol{F}}_{{\rm{mass, }}\mathit{n}}} = \int_0^{{l_n}} {{\rho _{{\rm{water }}}}} {C_{{\rm{mass }}}}{S_{{\rm{mass, }}\mathit{n}}} \cdot {{\mathit{\boldsymbol{\dot v}}}_{{\rm{flow, }}\mathit{n}}}(\mathit{\boldsymbol{L}}) \cdot {\rm{d}}\mathit{\boldsymbol{L}}.\end{array}$

(21)

其中：位置L∈R^3×1；C_drag为水阻力系数，C_mass为附加质量力系数，一般情况下，C_drag与C_mass可通过实验获得，本文根据经验设计C_drag=1.0，C_mass=2.0；S_{drag, n}为第n个关节单位长度连杆垂直于流速方向的投影面积，本文机械臂各个连杆可近似认为是圆柱体，因此S_{drag, n}=D_n×1，D_n为第n个连杆的直径；S_{mass, n}为第n个连杆的轴截面积；v_{flow, n}(L)和${\mathit{\boldsymbol{\dot v}}}$_{flow, n}(L)分别为第n个连杆在L位置所受到的水流的速度和加速度。
文[13-14]中通过分别计算各个连杆对第n个连杆的水阻力力矩和附加力矩，获得水阻力和附加质量力的计算公式，但这种方法在计算k≥6的机械臂时，计算过程烦琐，本文根据式(1)、(2)、(5)和(6)求得v_{flow, n}(L)和${\mathit{\boldsymbol{\dot v}}}$_{flow, n}(L)分别为:

${\mathit{\boldsymbol{v}}_{{\rm{flow}}, n}}(\mathit{\boldsymbol{L}}) = \left( {{\mathit{\boldsymbol{v}}_{{\rm{cen, }}n}} - {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times \mathit{\boldsymbol{L}}} \right), \\ {\mathit{\boldsymbol{\dot v}}_{{\rm{flow}}, n}}(\mathit{\boldsymbol{L}}) = \left( {{\mathit{\boldsymbol{\dot v}}_{{\rm{cen, }}n}} - {{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen}}, n}} \times \mathit{\boldsymbol{L}}} \right).$

(22)

可进一步得到下式：

$\begin{array}{c}{\mathit{\boldsymbol{F}}_{{\rm{drag}}, n}} = \frac{1}{2}{\rho _{{\rm{water }}}}{C_{{\rm{drag}}}}{S_{{\rm{drag}}, n}}\left[ {\left( {\int_0^{{l_n}} {{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen, }}n}}} \times } \right.} \right.\\\left. {\left. {\mathit{\boldsymbol{L}}\left\| {{{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{gen}}, n}} \times \mathit{\boldsymbol{L}}} \right\|{\rm{d}}\mathit{\boldsymbol{L}}} \right) + {\mathit{\boldsymbol{l}}_n}{\mathit{\boldsymbol{v}}_{{\rm{cen}}, n}}} \right], \\{\mathit{\boldsymbol{F}}_{{\rm{mass}}, n}} = {\rho _{{\rm{water }}}}{C_{{\rm{mass }}}}{S_{{\rm{mass}}, n}}\left[ {\left( {\int_0^{{l_n}} {{{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{gen, }}n}}} \times \mathit{\boldsymbol{L}} \cdot {\rm{d}}\mathit{\boldsymbol{L}}} \right) + } \right.\\\left. {{\mathit{\boldsymbol{l}}_n}{{\mathit{\boldsymbol{\dot v}}}_{{\rm{cen}}, n}}} \right]{\rm{. }}\end{array}$

(23)

由此可计算第n个关节的水阻力F_{drag, n}和附加质量力F_{mass, n}。不同于文[13-14]中单独计算F_{drag, n}和F_{mass, n}并代入总的力矩方程的方法，本文设计将F_{drag, n}和F_{mass, n}直接代入动力学计算公式中，将式(9)修改如下：

$\begin{array}{c}{\mathit{\boldsymbol{F}}_n} = _{n + 1}^n{\mathit{\boldsymbol{T}}_{{\rm{ro}}}}{\mathit{\boldsymbol{F}}_{n + 1}} + {\mathit{\boldsymbol{F}}_{{\rm{cen}}, n}} + {\mathit{\boldsymbol{F}}_{{\rm{water}}}}, \\{\mathit{\boldsymbol{F}}_{{\rm{water}}}} = {\mathit{\boldsymbol{F}}_{{\rm{drag}}}} + {\mathit{\boldsymbol{F}}_{{\rm{mass}}}}.\end{array}$

(24)

综上可计算关节力矩为

$\mathit{\boldsymbol{\tau }} = {\mathit{\boldsymbol{\tau }}_{{\rm{MCG}}}} + {\mathit{\boldsymbol{\tau }}_{{F_{{\rm{fri}}}}}} + {\mathit{\boldsymbol{\tau }}_{{\rm{iner}}}}{\rm{.}}$

(25)

其中：τ ∈R^k×1, 为关节力矩; 此时τ_MCG为机械臂由惯性力、Coriolis力、离心力、重力、水阻力、水浮力和水下附加质量力所组成的关节力矩。
2 机械臂动力学参数辨识2.1 激励轨迹设计在机械臂控制中，依托动力学模型进行前馈补偿可有效提高机械臂的控制效果，因此，动力学参数辨识对于水下机械臂的控制有重要影响^[15-18]。根据式(11)、(13)、(14)、(16)和(25)对机械臂动力学模型的描述，结合本文机械臂关节没有温度传感器和水动力环境复杂多变、难以准确快速辨识的情况，拟开展不考虑水动力学环境和温度影响的动力学参数辨识。针对式(16)中摩擦力的非线性黏滞，本文设计使用二次多项式拟合黏性摩擦力矩，表示如下：

${\mathit{\boldsymbol{\tau }}_{{F_{{\rm{vis}}}}}} = \left( {\mathit{\boldsymbol{f}}_{{\rm{vis}}}^1|\mathit{\boldsymbol{\dot q}}| + \mathit{\boldsymbol{f}}_{{\rm{vis}}}^2|\mathit{\boldsymbol{\dot q}}{|^2}} \right){\mathop{\rm sgn}} (\mathit{\boldsymbol{\dot q}}).$

(26)

其中：f_vis¹∈R^k×1，f_vis²∈R^k×1，分别为二次多项式非线性黏性摩擦力模型中的1次和2次性项系数向量。
根据文[19]，式(12)可改写为线性方程，此外根据式(13)和(26)也可以改写为线性方程，即：

$\mathit{\boldsymbol{\hat \tau }} = \mathit{\boldsymbol{H}}(\mathit{\boldsymbol{q}}, \mathit{\boldsymbol{\dot q}}, \mathit{\boldsymbol{\ddot q}})\mathit{\boldsymbol{\delta }}.$

(27)

其中：$\mathit{\boldsymbol{\hat \tau }}$为观测力矩；H (q, $\mathit{\boldsymbol{\dot q}}$, $\mathit{\boldsymbol{\ddot q}}$)为观测矩阵；机械臂动力学参数向量δ ∈R^14k×1, 且δ由k个δ_n组成，δ_n∈R^14×1, 为第n个关节的动力学参数向量，具体表示如下：

$\begin{array}{c}{\mathit{\boldsymbol{\delta }}_n} \equiv \left[ {{\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(1, 1), {\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(1, 2), {\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(1, 3), } \right.\\{\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(2, 2), {\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(2, 3), {\mathit{\boldsymbol{I}}_{{\rm{cen}}, n}}(3, 3), \\{l_{{\rm{cen}}\mathit{X}{\rm{, }}\mathit{n}}}, {l_{{\rm{cen}}Y, \mathit{n}}}, {l_{{\rm{cen}}Z, n}}, {m_n}, \\\left. {{\mathit{\boldsymbol{I}}_{{\rm{motor}}}}(n), {\mathit{\boldsymbol{f}}_{{\rm{coul}}}}(n), \mathit{\boldsymbol{f}}_{{\rm{vis}}}^1(n), \mathit{\boldsymbol{f}}_{{\rm{vis}}}^2(n)} \right]{\rm{.}}\end{array}$

(28)

其中：l_{cenX, n}、l_{cenY, n}和l_{cenZ, n}分别为第n个连杆的质心相对于广义坐标系3条坐标轴的距离。根据文[16]，δ最小参数集合可简化为10(k－1)+4+4个参数矩阵β。五次Fourier轨迹^[20]常被作为机械臂动力学参数辨识的激励轨迹：

$\begin{array}{c}{\mathit{\boldsymbol{\theta }}_{{\rm{traj, }}, n}}(t) = \sum\limits_{j = 1}^{{N_{{\rm{freq }}}}} {\left[ {\frac{{{\mathit{\boldsymbol{A}}_{j, n}}}}{{{w_{{\rm{freq }}}}j}}\sin \left( {{w_{{\rm{freq }}}}jt} \right) - } \right.} \\\left. {\frac{{{\mathit{\boldsymbol{B}}_{j, n}}}}{{{w_{{\rm{freq }}}}j}}\cos \left( {{w_{{\rm{freq }}}}jt} \right)} \right] + {\mathit{\boldsymbol{q}}_{{\rm{init }}}}(n){\rm{. }}\end{array}$

(29)

其中：t表示时间，t∈[0, t_freq]，t_freq为函数周期；θ_{traj, n}(t) 为第n个关节的Fourier轨迹，其基频w_freq=2π/t_freq；谐波个数N_freq=5；A_{j, n}和B_{j, n}分别表示第n个关节的j阶Fourier轨迹中余弦函数和正弦函数的振幅数值矩阵，j=[1, N_freq], 为振幅系数；q_init∈R^k×1, 为机械臂各个关节的初始角度向量。
2.2 轨迹参数设计及参数辨识假设共有Γ_max组测试实验数据，则可得回归矩阵Φ和实际扭矩?分别为：

$\mathit{\boldsymbol{ \boldsymbol{\varPhi} }} = \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{H}}_1}\left( {\mathit{\boldsymbol{q}}_1^\prime , \mathit{\boldsymbol{\dot q}}_1^\prime , \mathit{\boldsymbol{\ddot q}}_1^\prime } \right)}\\{{\mathit{\boldsymbol{H}}_2}\left( {\mathit{\boldsymbol{q}}_2^\prime , \mathit{\boldsymbol{\dot q}}_2^\prime , \mathit{\boldsymbol{\ddot q}}_2^\prime } \right)}\\ \vdots \\{{\mathit{\boldsymbol{H}}_\varphi }\left( {\mathit{\boldsymbol{q}}_\varphi ^\prime , \mathit{\boldsymbol{\dot q}}_\varphi ^\prime , \mathit{\boldsymbol{\ddot q}}_\varphi ^\prime } \right)}\\ \vdots \\{{\mathit{\boldsymbol{H}}_{{\mathit{\Gamma }_{\max }}}}\left( {\mathit{\boldsymbol{q}}_{\mathit{\Gamma }_{\max }}^\prime , \mathit{\boldsymbol{\dot q}}_{{\mathit{\Gamma }_{\max }}}^\prime , \mathit{\boldsymbol{\ddot q}}_{{\mathit{\Gamma }_{\max }}}^\prime } \right)}\end{array}} \right], \quad \mathit{\boldsymbol{\phi }} = \left[ {\begin{array}{*{20}{c}}{\mathit{\boldsymbol{\tau }}_1^\prime }\\{\mathit{\boldsymbol{\tau }}_2^\prime }\\ \vdots \\{\mathit{\boldsymbol{\tau }}_\varphi ^\prime }\\ \vdots \\{\mathit{\boldsymbol{\tau }}_{{\mathit{\Gamma }_{\max }}}^\prime }\end{array}} \right].$

(30)

其中: H_φ(q′_φ, ${\mathit{\boldsymbol{\dot q}}_\varphi ^\prime }$, ${\mathit{\boldsymbol{\ddot q}}_\varphi ^\prime }$)和τ′_φ分别为机械臂各关节角度、角速度、角加速度和力矩的第φ组测试的采集值集合矩阵，其中φ∈[1, Γ_max]。进而可采用遗传算法通过优化Φ的条件数获得最优的A_{j, n}和B_{j, n}参数^[17]：

$\begin{array}{*{20}{c}}{\min }&{{\rm{cond}}(\mathit{\boldsymbol{ \boldsymbol{\varPhi} }})}\\{{\rm{s}}{\rm{.}}\;{\rm{t}}{\rm{.}}}&{{\theta _{\min , n}} \le {\mathit{\boldsymbol{\theta }}_{{\rm{traj}}, n}} \le {\theta _{\max , n}}, }\\{}&{{{\dot \theta }_{\min , n}} \le {{\mathit{\boldsymbol{\dot \theta }}}_{{\rm{traj, }}n}} \le {{\dot \theta }_{\max , n}}, }\\{}&{{{\ddot \theta }_{\min , n}} \le {{\mathit{\boldsymbol{\ddot \theta }}}_{{\rm{traj}}, n}} \le {{\ddot \theta }_{\max , n}}.}\end{array}$

(31)

其中：θ_{min, n}和θ_{max, n}分别为关节n的最小角度值和最大角度值，${\dot \theta }$_{min, n}和${\dot \theta }$_{max, n}分别为关节n的最小角速度值和最大角速度值，$\ddot \theta $_{min, n}和$\ddot \theta $_{max, n}分别为关节n的最小角加速度值和最大角加速度值。
最后通过最小二乘可求解得出动力学参数β的辨识参数：

$\mathit{\boldsymbol{\hat \beta }} = {\left( {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}^{\rm{T}}}} \right)^{ - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} \phi}} .$

(32)

3 基于RBF神经网络自适应滑模控制3.1 基于RBF神经网络逼近的自适应滑模控制式(25)中τ_iner的I_motor可与τ_MCG的M_PDI(q)合并，可得

$\mathit{\boldsymbol{M}}(\mathit{\boldsymbol{q}}) = {\mathit{\boldsymbol{M}}_{{\rm{PDI}}}}(\mathit{\boldsymbol{q}}) + {\rm{diag}}\left( {{\mathit{\boldsymbol{I}}_{{\rm{motor}}}}} \right).$

(33)

因此式(25)可改写为

$\mathit{\boldsymbol{\tau }} = \mathit{\boldsymbol{M}}(\mathit{\boldsymbol{q}})\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}(\mathit{\boldsymbol{q}}, \mathit{\boldsymbol{\dot q}})\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{G}}(\mathit{\boldsymbol{q}}) + {\mathit{\boldsymbol{\tau }}_{{F_{{\rm{fri}}}}}}.$

(34)

以$\mathit{\boldsymbol{x}} = \left[ {\begin{array}{*{20}{l}}{{\mathit{\boldsymbol{x}}_1} = \mathit{\boldsymbol{q}}}\\{{\mathit{\boldsymbol{x}}_2} = \mathit{\boldsymbol{\dot q}}}\end{array}} \right]$为系统输出，其中x₁∈R^k×1，x₂∈R^k×1，x ∈R^2k×1，系统输入u = τ ∈R^k×1，构建控制系统为

$\left\{ {\begin{array}{*{20}{l}}{{{\mathit{\boldsymbol{\dot x}}}_1} = {\mathit{\boldsymbol{x}}_2}, }\\{{{\mathit{\boldsymbol{\dot x}}}_2} = {\mathit{\boldsymbol{M}}_1}\mathit{\boldsymbol{u}} + {\mathit{\boldsymbol{M}}^{ - 1}}\left( { - \mathit{\boldsymbol{C\dot q}} - \mathit{\boldsymbol{G}} - {\mathit{\boldsymbol{\tau }}_{{F_{{\rm{fri}}}}}}} \right) + \mathit{\boldsymbol{d}}(t).}\end{array}} \right.$

(35)

其中：M₁为由向量M_RBF构造成的角矩阵，M_{RBF, n}为由M逆的对角元组成的向量，即M_{RBF, n}= M^－1(n, n)。令M₁=diag[M_RBF，1, M_RBF，2, …, M_{RBF, k}]，M₁∈R^k×k；d(t)∈R^k×1, 为干扰向量且有界，即d(t)≤ D_max，D_max∈R^k×1, 为最大干扰误差向量。
令fun₁(x)=M₁，fun₂(x)= M^－1(－C${\mathit{\boldsymbol{\dot q}}}$－ G － τ_Ffri)，且fun₁(x)∈R^k×k，fun₂(x)∈R^k×1，控制系统可改写为

$\left\{ {\begin{array}{*{20}{l}}{{{\mathit{\boldsymbol{\dot x}}}_1} = {\mathit{\boldsymbol{x}}_2}, }\\{{{\mathit{\boldsymbol{\dot x}}}_2} = {\rm{fu}}{{\rm{n}}_1}(\mathit{\boldsymbol{x}})\mathit{\boldsymbol{u}} + {\rm{fu}}{{\rm{n}}_2}(\mathit{\boldsymbol{x}}) + \mathit{\boldsymbol{d}}(t).}\end{array}} \right.$

(36)

根据动力学辨识参数$\mathit{\boldsymbol{\hat \beta }}$可获得辨识结果$\mathit{\boldsymbol{\hat M }}$₁、$\mathit{\boldsymbol{\hat M }}$、$\mathit{\boldsymbol{\hat C }}$、$\mathit{\boldsymbol{\hat G }}$和$\mathit{\boldsymbol{\hat \tau }}$_Ffri，然而系统存在未建模量，如水动力学和摩擦力温度等，因此控制系统可表示为

$\left\{ {\begin{array}{*{20}{l}}{{\rm{fu}}{{\rm{n}}_2}(\mathit{\boldsymbol{x}}) = [ - (\mathit{\boldsymbol{\hat C}} + \mathit{\boldsymbol{\tilde C}})\mathit{\boldsymbol{\dot q}} - (\mathit{\boldsymbol{\hat G}} + \mathit{\boldsymbol{\tilde G}}) - }\\{\left. {\quad \;\;\;\;\left( {{{\mathit{\boldsymbol{\hat \tau }}}_{{F_{{\rm{fri}}}}}} + {{\mathit{\boldsymbol{\tilde \tau }}}_{{F_{{\rm{fri}}}}}}} \right)} \right]{{(\mathit{\boldsymbol{\hat M}} + \mathit{\boldsymbol{\tilde M}})}^{ - 1}}, }\\{{\rm{fu}}{{\rm{n}}_1}(\mathit{\boldsymbol{x}}) = \left( {{{\mathit{\boldsymbol{\hat M}}}_1} + {{\mathit{\boldsymbol{\tilde M}}}_1}} \right).}\end{array}} \right.$

(37)

其中${\mathit{\boldsymbol{\tilde C}}}$、${\mathit{\boldsymbol{\tilde G}}}$、${\mathit{\boldsymbol{\tilde \tau }}}$_Ffri、${\mathit{\boldsymbol{\tilde M}}}$和${\mathit{\boldsymbol{\tilde M}}}$₁为误差量。
神经网络可有效拟合系统误差项^[21-22], 本文采用RBF网络分别逼近fun₁(x)与fun₂(x)，RBF网络的输入输出算法可表示如下：

$\begin{array}{*{20}{c}}{{h_j} = \exp \left( {\frac{{{{\left\| {\mathit{\boldsymbol{x - }}{\mathit{\boldsymbol{c}}_{{\rm{neu}}, j}}} \right\|}^2}}}{{2b_{{\rm{RBF}}}^2}}} \right), }\\{\mathit{\boldsymbol{h}}(\mathit{\boldsymbol{x}}) = {{\left[ {{h_1}, {h_2}, \cdots , {h_j}, \cdots , {h_J}} \right]}^{\rm{T}}}, }\\{{\rm{fu}}{{\rm{n}}_2}(\mathit{\boldsymbol{x}}) = {\mathit{\boldsymbol{Q}}^ * }\mathit{\boldsymbol{h}}(x) + {\mathit{\boldsymbol{\varepsilon }} _2}, }\\{{\rm{fun}}_1^\prime (\mathit{\boldsymbol{x}}) = {\mathit{\boldsymbol{V}}^ * }h(\mathit{\boldsymbol{x}}) + {\mathit{\boldsymbol{\varepsilon }}_1}, }\\{{\rm{fu}}{{\rm{n}}_1}(\mathit{\boldsymbol{x}}) = {\rm{diag}}\left[ {{\rm{fun}}_1^\prime (\mathit{\boldsymbol{x}})} \right].}\end{array}$

(38)

其中：h_j为第j个神经元的Gauss基函数; c_{neu, j}∈R^2k×1，为第j个神经元中心向量; b_RBF为神经元Gauss核宽度; J为神经元输出个数; x为网络输入；h (x)∈R^J×1, 为Gauss基函数的输出向量；Q^*∈R^k×J, V^*∈R^k×J, 分别为逼近fun′₁(x)(fun′₁(x)∈R^k×1)和fun₂(x)的理想网络权值矩阵，fun₁(x)由fun′₁(x)重组而成；ε₁∈R^k×1和ε₂∈R^k×1均为网络逼近误差向量，其最大值分别为ε_M1(ε_M1∈R^k×1)和ε_M2(ε_M2∈R^k×1)，即|ε₁|≤ ε_M1，| ε₂|≤ ε_M2。则RBF的输出为

${\widehat {{\rm{fun}}}_1}(\mathit{\boldsymbol{x}}) = \mathit{\boldsymbol{\hat Vh}}(\mathit{\boldsymbol{x}})\quad {\widehat {{\rm{fun}}}_2}(\mathit{\boldsymbol{x}}) = \mathit{\boldsymbol{\hat Qh}}(\mathit{\boldsymbol{x}}){\rm{.}}$

(39)

其中: $\widehat {{\rm{fun}}}$₁(x)∈R^k×k和$\widehat {{\rm{fun}}}$₂(x)∈R^k×1均为RBF网络每次的迭代输出矩阵；$\mathit{\boldsymbol{\hat Q}}$∈R^k×J和$\mathit{\boldsymbol{\hat V}}$∈R^k×J均分别为RBF网络的迭代网络权值矩阵。设计误差函数为e = q_de－ q_re；q_de∈R^k×1，为机械臂各个关节角度期望轨迹向量；q_re∈R^k×1，为机械臂各个关节的实际轨迹向量；e ∈R^k×1，为误差向量。设计滑模函数如下：

$\begin{array}{*{20}{c}}{\mathit{\boldsymbol{s}} = \mathit{\boldsymbol{\dot e}} + \mathit{\boldsymbol{ce}}, }\\{\mathit{\boldsymbol{\dot s}} = \mathit{\boldsymbol{\ddot e}} + \mathit{\boldsymbol{c\dot e}}.}\end{array}$

(40)

其中: c ∈R^k×1，为滑模函数参数向量; ${\mathit{\boldsymbol{\dot e}}}$和${\mathit{\boldsymbol{\ddot e}}}$分别为误差的导数和误差的二次导函数；${\mathit{\boldsymbol{\dot s}}}$为滑膜函数的导函数。基于此设计控制率，即系统输入u为

$\mathit{\boldsymbol{u}} = \frac{1}{{{{\widehat {{\rm{fun}}}}_1}(\mathit{\boldsymbol{x}})}}\left[ { - {{\widehat {{\rm{fun}}}}_2}(\mathit{\boldsymbol{x}}) + {{\mathit{\boldsymbol{\ddot q}}}_{{\rm{de}}}} + \mathit{\boldsymbol{c\dot e}} + \mathit{\boldsymbol{\eta }}{\mathop{\rm sgn}} (\mathit{\boldsymbol{s}})} \right].$

(41)

其中：${\mathit{\boldsymbol{\ddot q}}}$_de∈R^k×1, 为机械臂各个关节的期望角加速度向量；η ∈R^k×1, 为干扰上界向量，η ≥ D_max。
将(41)代入(40)可得

$\begin{array}{*{20}{c}}{\mathit{\boldsymbol{\dot s}} = \left( {{{\widehat {{\rm{fun}}}}_2}(\mathit{\boldsymbol{x}}) - {{\widehat {{\rm{fun}}}}_2}(\mathit{\boldsymbol{x}})} \right) - \mathit{\boldsymbol{\eta }}{\mathop{\rm sgn}} (\mathit{\boldsymbol{s}}) + }\\{\left. {\widehat {\rm{fun}}{_1}(\mathit{\boldsymbol{x}}) - {\rm{fu}}{{\rm{n}}_1}(\mathit{\boldsymbol{x}})} \right)\mathit{\boldsymbol{u}} - \mathit{\boldsymbol{d}}(t){{\widetilde {{\rm{fun}}}}_2}(\mathit{\boldsymbol{x}}) - }\\{\mathit{\boldsymbol{\eta }}{\mathop{\rm sgn}} (\mathit{\boldsymbol{s}}) + {{\widetilde {{\rm{fun}}}}_1}(\mathit{\boldsymbol{x}})\mathit{\boldsymbol{u}} - \mathit{\boldsymbol{d}}(t) = \mathit{\boldsymbol{\tilde Qh}}(\mathit{\boldsymbol{x}}) - {\mathit{\boldsymbol{\varepsilon }}_2} - }\\{\mathit{\boldsymbol{\eta }}{\mathop{\rm sgn}} (\mathit{\boldsymbol{s}}) + \left[ {\mathit{\boldsymbol{\tilde Vh}}(\mathit{\boldsymbol{x}}) - {\mathit{\boldsymbol{\varepsilon }}_1}} \right]\mathit{\boldsymbol{u}} - \mathit{\boldsymbol{d}}(t).}\end{array}$

(42)

其中${\widetilde {{\rm{fun}}}}$₁(x)、${\widetilde {{\rm{fun}}}}$₂(x)、${\mathit{\boldsymbol{\tilde Q}}}$和${\mathit{\boldsymbol{\tilde V}}}$为误差量。
3.2 控制器稳定性证明设计Lyapunnov函数：

$\mathit{\boldsymbol{\mu }} = \frac{1}{2}{\mathit{\boldsymbol{s}}^2} + \frac{1}{{2{\mathit{\boldsymbol{\gamma}} _1}}}\mathit{\boldsymbol{\tilde Q}}{\mathit{\boldsymbol{\tilde Q}}^{\rm{T}}} + \frac{1}{{2{\mathit{\boldsymbol{\gamma}} _2}}}\mathit{\boldsymbol{\tilde V}}{\mathit{\boldsymbol{\tilde V}}^{\rm{T}}}.$

(43)

其中：γ₁, γ₂＞0，γ₁, γ₂∈R^k×1，为自适应率参数向量。
对式(43)求导可得

$\begin{array}{*{20}{c}}{\mathit{\boldsymbol{\dot \mu }} = \mathit{\boldsymbol{s\dot s}} + \frac{1}{{{\gamma _1}}}\mathit{\boldsymbol{\tilde Q}}{{\mathit{\boldsymbol{\dot {\tilde Q}}}}^{\rm{T}}} + \frac{1}{{{\gamma _2}}}\mathit{\boldsymbol{\tilde V}}{{\mathit{\boldsymbol{\dot {\tilde V}}}}^{\rm{T}}} = }\\{\mathit{\boldsymbol{\tilde Q}}\left( {\mathit{\boldsymbol{sh}}(\mathit{\boldsymbol{x}}) - \frac{1}{{{\mathit{\boldsymbol{\gamma }}_1}}}{{\mathit{\boldsymbol{\dot {\hat Q}}}}^{\rm{T}}}} \right) + \mathit{\boldsymbol{\tilde V}}\left( {\mathit{\boldsymbol{sh}}(\mathit{\boldsymbol{x}})\mathit{\boldsymbol{u}} - \frac{1}{{{\mathit{\boldsymbol{\gamma }}_2}}}{{\mathit{\boldsymbol{\dot {\hat V}}}}^{\rm{T}}}} \right) + }\\{\mathit{\boldsymbol{s}}\left( { - {\mathit{\boldsymbol{\varepsilon }}_2} - \mathit{\boldsymbol{\eta }}{\mathop{\rm sgn}} (\mathit{\boldsymbol{s}}) - {\mathit{\boldsymbol{\varepsilon }}_1}\mathit{\boldsymbol{u}} - \mathit{\boldsymbol{d}}(t)} \right).}\end{array}$

(44)

设计自适应控制率为

$\left\{ {\begin{array}{*{20}{l}}{\mathit{\boldsymbol{\dot {\hat Q}}} = {\mathit{\boldsymbol{\gamma }}_1}\mathit{\boldsymbol{sh}}(\mathit{\boldsymbol{x}}), }\\{\mathit{\boldsymbol{\dot {\hat V}}} = {\mathit{\boldsymbol{\gamma }}_2}\mathit{\boldsymbol{sh}}(\mathit{\boldsymbol{x}})\mathit{\boldsymbol{u}}.}\end{array}} \right.$

(45)

令η > ε_M2+ ε_M1u + d_max+ η₀, η₀∈R^k×1, η₀>0，则有$\mathit{\boldsymbol{\dot \mu }}$≤- η₀|s|≤0，且当$\mathit{\boldsymbol{\dot \mu }}$≡0时, s ≡0，根据LaSalle不变集原理，t→∞时，s →0。综上可得Lyapunnov函数是(43)正定的，其导数是负定的，从而证明了控制系统的稳定性。
4 机械臂动力学建模及参数辨识仿真4.1 机械臂动力学建模机械臂的动力学模型参数通过数模或根据经验设计的，参数如表 1所示，并假设该参数为真实的动力学模型参数。此外设定水密度ρ_water=1 000 kg/m³，重力加速度g =9.798 8 m/s²。
表 1 机械臂动力学模型数模参数

参数	n
	1	2	3	4	5	6
Imotor(n)/(kg·m2)	4.300 0	4.300 0	3.200 0	2.600 0	1.300 0	1.300 0
mn/kg	5.400 0	12.100 0	5.122 0	3.011 0	3.011 0	6.200 0
lcenX, n/m	0.000 0	0.318 6	0.137 0	0.000 0	0.000 0	0.000 0
lcenY, n/m	-0.010 5	0.000 0	0.000 0	0.007 3	0.007 3	0.000 0
lcenZ, n/m	-0.009 3	0.001 7	0.002 0	0.006 3	-0.006 3	-0.201 9
Icen, n(1, 1)/(kg·m2)	0.033 8	0.042 6	0.011 3	0.008 2	0.008 2	0.298 0
Icen, n(1, 2)/(kg·m2)	0.000 0	0.000 0	0.000 0	0.000 0	0.000 0	0.000 0
Icen, n(1, 3)/(kg·m2)	0.000 0	-0.016 2	-0.003 6	0.000 0	0.000 0	0.000 0
Icen, n(2, 2)/(kg·m2)	0.030 6	2.334 5	0.219 0	0.005 1	0.005 1	0.298 1
Icen, n(2, 3)/(kg·m2)	0.000 0	0.000 0	0.000 0	0.000 0	0.000 0	0.000 0
Icen, n(3, 3)/(kg·m2)	0.022 3	2.321 3	0.216 7	0.007 6	0.007 6	0.006 7
fcoul, 0(n)	42.000 0	42.000 0	33.200 0	33.200 0	10.400 0	10.400 0
fcoul, 1(n)	-0.001 0	-0.001 0	-0.006 0	-0.006 0	-0.004 0	-0.004 0
fvis, 0(n)	0.510 0	0.560 0	0.540 0	0.560 0	0.620 0	0.620 0
fvis, 1(n)	0.006 3	0.005 8	0.006 3	0.006 3	0.003 3	0.003 3
δvis，0(n)	15.200 0	15.200 0	13.200 0	13.200 0	8.500 0	8.600 0
δvis，1(n)	-0.230 0	-0.230 0	-0.180 0	-0.170 0	-0.120 0	-0.130 0
Sdrag, n/m	0.164 0	0.132 0	0.130 0	0.106 0	0.106 0	0.090 0
Smass, n/m2	0.021 0	0.013 3	0.013 3	0.008 8	0.008 8	0.006 6
ln/m	222.928 0	758.059 0	62.286 0	147.426 0	361.500 0	0.000 0
ρarm, n/(kg·m-3)	3 482.920 0	3 482.920 0	3 482.920 0	3 482.920 0	3 482.920 0	3 482.920 0

表选项






根据表 1中摩擦力的参数和式(14)、(16)及(17)建立机械臂各个关节的非线性摩擦模型。
关节2的非线性摩擦模型如图 1所示。

图 1 关节2非线性摩擦力模型

图选项

4.2 机械臂动力学参数辨识仿真通过遗传算法优化，即式(31)，可得如图 2和3所示的机械臂最优激励轨迹图。

图 2 机械臂动力学参数辨识最优激励轨迹

图选项

图 3 最优激励轨迹曲线

图选项

各机械臂的最优激励参数如表 2所示。
表 2 最优激励轨迹参数

参数	n
	1	2	3	4	5	6
A1, n	6.456	-6.090	-6.960	-1.226	-2.516	-4.262
A2, n	-0.658	-3.999	6.402	7.399	-5.453	-6.304
A3, n	6.674	-4.390	6.148	7.696	6.184	-1.603
A4, n	1.729	6.192	6.661	-6.111	6.982	6.552
A5, n	3.752	0.274	7.985	3.605	7.305	-3.889
B1, n	6.857	-3.340	-1.701	-1.850	-7.570	4.585
B2, n	6.914	5.384	5.473	-6.050	-7.808	-7.085
B3, n	3.448	6.980	0.288	2.183	4.043	-0.635
B4, n	-4.971	7.308	-1.035	3.577	0.603	-6.837
B5, n	-6.934	-1.322	-6.248	-7.173	3.457	0.345

表选项






设计仿真系统的采样频率为1 000 Hz，完成数据采样，基于式(30)和(32)完成机械臂仿真环境下的动力学参数辨识，参数辨识结果如表 3所示。
表 3 动力学参数$\mathit{\boldsymbol{\dot \beta }} $辨识结果

序号	数值	序号	数值	序号	数值	序号	数值
1	-6.559	2	-12.970	3	-2.749	4	-10.240
5	6.976	6	6.030	7	16.279	8	1.771
9	0.677	10	2.273	11	1.664	12	-6.366
13	2.473	14	4.753	15	0.920	16	1.350
17	-4.221	18	1.440	19	-4.137	20	-0.597
21	-0.236	22	0.795	23	-0.612	24	1.677
25	0.674	26	0.903	27	0.133	28	0.081
29	1.141	30	1.599	31	-0.080	32	-0.192
33	0.292	34	-0.709	35	0.023	36	0.004
37	42.657	38	-12.210	39	31.179	40	47.309
41	-1.046	42	7.482	43	33.391	44	33.173
45	-16.113	46	34.251	47	12.458	48	-0.267
49	10.394	50	10.585	51	-4.065	52	10.578
53	6.994	54	-3.157	55	-5.626	56	2.020

表选项






将表 3中的动力学辨识参数代入式(27)中，可得图 4所示的辨识结果，由图 4可知辨识力矩对真实力矩具有较好的辨识效果。

图 4 激励轨迹真实力矩与辨识力矩

图选项

将表 3中的动力学辨识参数代入式(27)中，并设计不同幅度和不同起始位置的Fourier验证轨迹，验证结果如图 5所示，其中6个关节的辨识误差分别为：5.66%、7.74%、9.97%、1.79%、5.16%和1.29%。

图 5 验证轨迹真实力矩与辨识力矩

图选项

4.3 水动力学环境对机械臂的影响基于表 1设定的水动力学参数，依据水动力学公式，即式(18)、(21)、(23)、(24)和(25)建立六自由度水下机械臂的水动力学公式，并完成仿真建模，如图 6所示。

图 6 考虑水动力学的机械臂动力学建模

图选项

由图 6可知，水动力学对于机械臂动力学的稳定性具有重要的影响，其中各个关节的最大水动力对力矩的影响分别为：23.21、101.86、35.49、13.84、1.28和0 N·m。
5 自适应滑模轨迹跟踪控制仿真通过第4章的动力学建模，可获得一个较为详尽的水下机械臂动力学仿真环境。针对各个关节设立幅值分别为：5、20、15、10、8和5 N·m的Gauss随机误差，以表示噪声干扰，使用第4章中的验证轨迹进行验证，系统温度随时间变化而升高，设定仿真开始时系统温度为15 ℃，仿真结束时系统温度为35 ℃，其间呈线性增长。
在此基础上，本章将开展基于动力学模型的机械臂仿真控制。首先设计了实验1(P1)，使用双环PID控制器进行控制仿真；其次，为提高控制精度还开展了实验2(P2)，基于RBF神经网络补偿的滑模控制仿真；最后，为进一步减低滑模控制的振动，在控制系统中加入了动力学模型补偿完成实验3(P3)。
P1使用了双环PID控制，分别对速度环和位置环进行PID控制，6个关节轨迹跟踪误差如图 7所示，可知6个关节轨迹的跟踪误差最大可达0.022 rad。

图 7 P1中6个关节轨迹跟踪误差

图选项

P2使用RBF神经网络对系统建模误差和未建模项进行拟合补偿，P3则在P2的基础上添加了动力学模型补偿，其跟踪误差如图 8所示。由图 8可知：P2与P3的轨迹跟踪效果明显优于P1的效果，P3在对动力学模型进行补偿之后，跟踪效果也优于P2。

图 8 P2与P3的6个关节轨迹跟踪误差

图选项

6 结论针对水下机械臂精准建模及控制技术，本文提出了一种融合Newton-Euler方程、Morison方程与非线性摩擦力的水下机械臂动力学模型建模方法。通过引入多项式函数建立非线性黏滞摩擦力模型，并在此基础上设计了激励轨迹优化方法和动力学参数辨识方法。通过实验证明了动力学参数辨识有效。
为提高机械臂在复杂多变水环境下的精准控制能力，本文设计了一种在补偿非线性动力学模型的基础上利用RBF神经网络补偿系统未建模与建模误差的自适应滑模控制方法。通过仿真，本文证明了该方法具有比传统PID控制和一般的RBF神经网络自适应滑模控制精度更突出的优势。
下一步将进一步开展关于非线性摩擦力自适补偿方法的研究，并在实物平台上开展实验测试。

参考文献

[1]	SIVCEV S, COLEMAN J, OMERDIC E, et al. Underwater manipulators: A review[J]. Ocean engineering, 2018, 163: 431-450. DOI:10.1016/j.oceaneng.2018.06.018
[2]	PAREDIS C J J, BROWN H B, KHOSLA P K. A rapidly deployable manipulator system[J]. Robotics and Autonomous Systems, 1997, 21(3): 289-304. DOI:10.1016/S0921-8890(97)00081-X
[3]	SAGARA S, TAMURA M, YATOH T, et al. Digital RAC for underwater vehicle-manipulator systems considering singular configuration[J]. Artificial Life and Robotics, 2006, 10(2): 106-111. DOI:10.1007/s10015-005-0359-3
[4]	PANDIAN S R, SAKAGAMI N. System integration aspects of underwater vehicle-manipulator systems for oceanic exploration[J]. Journal of the Society of Instrument and Control Engineers, 2008, 47(10): 830-836.
[5]	ARMENIO V. Dynamic loads on submerged bodies in a viscous numerical wave tank at small KC numbers[J]. Ocean Engineering, 1998, 25(10): 881-905. DOI:10.1016/S0029-8018(97)10018-X
[6]	CHEN Y, WANG K L, ZHAI L Y, et al. Feedforward fuzzy trajectory compensator with robust adaptive observer at input trajectory level for uncertain multi-link robot manipulators[J]. Journal of the Franklin Institute, 2017, 354(8): 3237-3266. DOI:10.1016/j.jfranklin.2017.02.034
[7]	CHEN B, LIU X P, LIU K F, et al. Direct adaptive fuzzy control of nonlinear strict-feedback systems[J]. Automatica, 2009, 45(6): 1530-1535. DOI:10.1016/j.automatica.2009.02.025
[8]	XU B, PANDIAN S R, SAKAGAMI N, et al. Neuro-fuzzy control of underwater vehicle-manipulator systems[J]. Journal of the Franklin Institute, 2012, 349(3): 1125-1138. DOI:10.1016/j.jfranklin.2012.01.003
[9]	KHALIL W, DOMBRE E. Modeling, identification & control of robots[M]. London: Hermes Penton Science, 2002.
[10]	布鲁诺·西西里安诺, 洛伦索·夏维科, 路易吉·维拉尼, 等. 机器人学: 建模、规划与控制[M]. 张国良, 曾静, 陈励华, 等, 译. 西安: 西安交通大学出版社, 2015. SICILIANO B, SCIAVICCO L, VILLANI L, et al. Robotics: Modelling, planning and control[M]. ZHANG G L, ZENG J, CHEN L H, et al, trans. Xi'an: Xi'an Jiaotong University Press, 2015. (in Chinese)
[11]	I SKANDAR M, WOLF S. Dynamic friction model with thermal and load dependency: Modeling, compensation, and external force estimation[C]//2019 International Conference on Robotics and Automation (ICRA). Montreal, Canada: IEEE, 2019: 7367-7373.
[12]	KIM S. Moment of inertia and friction torque coefficient identification in a servo drive system[J]. IEEE Transactions on Industrial Electronics, 2019, 66(1): 60-70. DOI:10.1109/TIE.2018.2826456
[13]	杨帆. 水下机械手运动学与动力学控制关键问题研究[D]. 哈尔滨: 哈尔滨工业大学, 2019. YANG F. Research on the key problems of kinematics and dynamics control based on underwater manipulator[D]. Harbin: Harbin Engineering University, 2019. (in Chinese)
[14]	ZHONG Y G, YANG F. Dynamic modeling and adaptive fuzzy sliding mode control for multi-link underwater manipulators[J]. Ocean Engineering, 2019, 187: 106202. DOI:10.1016/j.oceaneng.2019.106202
[15]	HUANG S F, CHEN J H, ZHANG J W, et al. Robust estimation for an extended dynamic parameter set of serial manipulators and unmodeled dynamics compensation[J]. IEEE/ASME Transactions on Mechatronics, 2022, 27(2): 962-973. DOI:10.1109/TMECH.2021.3076519
[16]	INDRI M, TRAPANI S. Framework for static and dynamic friction identification for industrial manipulators[J]. IEEE/ASME Transactions on Mechatronics, 2020, 25(3): 1589-1599. DOI:10.1109/TMECH.2020.2980435
[17]	LEE T, WENSING P M, PARK F C. Geometric robot dynamic identification: A convex programming approach[J]. IEEE Transactions on Robotics, 2020, 36(2): 348-365. DOI:10.1109/TRO.2019.2926491
[18]	GAZ C, COGNETTI M, OLIVA A, et al. Dynamic identification of the Franka Emika panda robot with retrieval of feasible parameters using penalty-based optimization[J]. IEEE Robotics and Automation Letters, 2019, 4(4): 4147-4154. DOI:10.1109/LRA.2019.2931248
[19]	MADSEN E, ROSENLUND O S, BRANDT D, et al. Comprehensive modeling and identification of nonlinear joint dynamics for collaborative industrial robot manipulators[J]. Control Engineering Practice, 2020, 101: 104462. DOI:10.1016/j.conengprac.2020.104462
[20]	SWEVERS J, GANSEMAN C, TUKEL D B, et al. Optimal robot excitation and identification[J]. IEEE transactions on Robotics and Automation, 1997, 13(5): 730-740. DOI:10.1109/70.631234
[21]	PANDIAN S R, SAKAGAMI N. A neuro-fuzzy controller for underwater robot manipulators[C]//2010 11th International Conference on Control Automation Robotics & Vision. Singapore: IEEE, 2010: 2135-2140.
[22]	刘金琨. 滑模变结构控制MATLAB仿真[M]. 4版. 北京: 清华大学出版社, 2019. LIU J K. Sliding mode control design and MATLAB simulation[M]. 4th ed. Beijing: Tsinghua University Press,, 2019. (in Chinese)