对水下环境中使用的ROV和AUV等水下探测和作业装备，建立合理和完备的运动控制模型不仅有助于水下的作业活动，而且增强潜水器应对突发情况的能力。Fossen^[1]在其专著中详细阐述了控制理论和动力学模型应用于水下探测设备的原理和方法，Chin^[2]建立了适用于控制一种多推进器的ROV控制模型，Eng等^[3]采用系统辨识的方法确认了控制系统必备的流体动力系数矩阵，减少了流体性能试验周期。应当针对不同外形、总体布置和任务需求的水下ROV和AUV，基于空间运动控制方程应建立不同的控制仿真平台^{[4, 5, 6, 7, 8, 9]}。常压潜水装atmospheric diving suit (ADS)(又称为硬质潜水服)不同于常规的水下探测作业装备^[10]，装具作为一种人型载人系统，在潜水时内部保持常压，人员在内部操纵安装在人形装具腰间的推进器在水下航行，也可依靠自身的人力在水下完成慢速移动，训练有素的装具操作员利用人力轻松活动轻质铝合金关节，操作人形装具手臂端的工具与设备，灵活高效地完成水下作业。由于装具需要携带供氧、供电、通讯、作业装备等下潜，装配浮力块调节压载平衡浮力。装具头部安装了照明灯具和球形的视窗供操作员观察，四肢为一组活动关节组装具有多个空间自由度的四肢可供人员驱动完成水下作业，同时胸腔内有控制各系统的操作面板也可供人员操作，浮力包背在人形装具的背部，腰间悬挂着左右4组推进单元用于上下前后组合推进活动。为减少水面波浪对装具的影响，设置了中继站，潜水装具的动力源由母船通过高强度铠装牵缆传递给中继站，中继站将电力和信号通过柔性脐带缆传递给水下作业的装具。本研究结合潜水装具的外形可变、活动空间自由度和运动稳定性等特殊要求，对Chin等^{[1, 2]}建立的控制模型进行了修改，引入了装具活动时的质量矩阵与运动姿势的变换关系，推导可预报装具姿态变化对装具惯性性能进行预报的数值方法，编制了动力学预报程序，评估初步设计参数进行运动稳定性能。

本文引入一种广泛应用的描述水下机器人运动的动力学模型^[1]，根据常压潜水装具的水下运动属性和常压潜水装具的水动力分布特点修改了水动力项以及控制方程的系数项和右端项，并增加了流体动力计算预报模块,对装具的可动质量编制了模拟分析算法。动力学模型的表达式为

体轴系和惯性系的欧拉转换矩阵为

$${J_1} = \left[ {\begin{array}{*{20}{c}} {cos\psi cos\theta }&{sin\psi cos\varphi + cos\psi sin\theta sin\varphi }&{sin\psi sin\varphi + cos\psi cos\varphi sin\theta }\\ {sin\psi cos\theta }&{cos\psi cos\varphi + sin\varphi sin\theta sin\psi }&{cos\psi sin\varphi + sin\theta sin\psi cos\theta }\\ {sin\theta }&{cos\theta sin\varphi }&{cos\theta cos\varphi } \end{array}} \right]$$

$${J_2} = \left[ {\begin{array}{*{20}{c}} 1&{sin\varphi \tan \theta }&{\cos \varphi \tan \theta }\\ 0&{\cos \varphi }&{ - \sin \varphi }\\ 0&{\sin \varphi /\cos \theta }&{\cos \varphi /\cos \theta } \end{array}} \right]$$

为了便于简化计算模型，惯性系的原点设定为在中继站几何中心处，体轴系的原点建立在装具的重心处，X_G=Y_G=Z_G=0，从而使得M_R其中第一个3×3的子块变为对角矩阵，第4个3×3的子块非对角元素I_xy、I_xz和I_yz与对角元素相比变得较小，降低了矩阵求解的奇异性。附加质量矩阵为对角矩阵，采用Hess-Smith面元方法求解，由于装具在水下做低速运动，水动力耦合影响作用较小，在动力学模型中不予考虑，即附加质量矩阵的非对角元素均为零。对质量矩阵做上述简化对水下活动速度小于0.5 m/s的装具而言具有足够的精度。质量矩阵表述成为如下形式：

C(v)项包含了科氏力和向心力等作用力，是质量和附加质量对随体坐标系的旋转效应，C(v)表达式为

其中

D是水动力阻尼力矩阵，对于非流线型的潜水装具而言，形成水动力的成因包括表面摩擦力、涡流脱落形成的非定常作用力等。实际上随着航行速度的变化，潜水装具受到的水动力将呈现出强非线性，并且相互耦合作用的效应显著，尤其是作用的力矩与潜水装具的航速增加其非线性和耦合效应更加显著。对于低速范围内的水动力而言，潜水装具的水动力可采用二次函数近似如下:

$$D = - {\rm{diag}}\left[ \begin{array}{l} {X_u} + {X_{u|u|}}\left| u \right|\\ {Y_v} + {Y_{v\left| v \right|}}\left| v \right|\\ {Z_w} + {Z_{w\left| w \right|}}\left| w \right|\\ {K_p} + {K_{p\left| p \right|}}\left| p \right|\\ {M_q} + {M_{q\left| q \right|}}\left| q \right|\\ {N_r} + {N_{r\left| r \right|}}\left| r \right| \end{array} \right]$$

在装具的随体系中，装具的恢复力向量g(η)表达式为

由于随体系中x_G=y_G=z_G=0，上式还可进一步简化。定义u₁、u₂为水平推进器对称布置在装具的腰部，u₃、u₄为垂向推进器同样对称布置在推进器的腰部，装具的4只推进器发出的推力和力矩可表述为

式中：u为电信号驱动4只推进器发出的推力向量，为推进器发出的有效推力；l₂为对称布置的水平推进器与随体坐标系的原点的垂向距离；l₃为水平推进器与装具对称面的距离。值得注意的是，装具并没有安装可以控制侧向移动和滚转自由度的推进器，这是由于人形装具的回转阻尼力较小，易通过调节水平推进器的输入电流发出的推力差实现转动，从而使装具转向克服侧向来流造成的影响，由于装具自身的扶正力矩大，不考虑推进器对存在滚转角的装具扶正控制作用。

图1显示了装具的总体布置，针对其构型及运动特点，对潜水装具的动力学模型建立仿真计算模型，并给出相关模型系数的测定和计算，通过构造动力学数值计算模型，模拟装具在典型海流作用情况下的运动。按照动力学模型将随体坐标系的原点建立在重心处，通过对装具的计算机虚拟总装配完成了各项配件的总装后计算得到了装具的质量属性包括装具的质量和转动惯量分别为m=598 kg，I_xx=159.9 kg·m²，I_yy=165.2 kg·m²，I_zz=59.5 kg·m²，I_xy=17.1 kg·m²，I_xy=55.3 kg·m²。

图1 常压潜水装具的总布置 Fig.1 Configuration of atmospheric diving suit

图选项

对直立行走姿态装具的附加质量阻尼采用Hess-Smith方法求解，对完成装配的装具CAD模型进行适当简化，去掉装具的小尺寸构件如天线、夹具、灯具，对复杂外形进行适当简化，而后在装具的简化CAD外形布置面元。计算得到如下的直立行走状态对应的附加质量阻尼：${X_{\dot u}} = 358{\mathop{\rm kg}\nolimits} ,{Y_{\dot v = 385}}{\rm{kg,}}{Z_{\dot w}} = 146{\rm{kg}},{K_{\dot p}} = 40.9{\rm{kg}} \cdot {{\rm{m}}^2},{M_{\dot q}} = 41.9{\rm{kg}} \cdot {{\rm{m}}^2},{N_{\dot r}} = 2.6{\rm{kg}} \cdot {{\rm{m}}^2}$。

通过浮力材料和搭载设备的优化布置，进行重心、重量和浮心、浮力的计算，得到搭载潜航员的装具的标准水下净浮力5 kg，也可根据水下作业任务和海流环境的不同调整重量使得水下重量略大于浮力，增强装具运动稳定性，此外由于潜航员的四肢活动形成了可动重量恢复力向量是小范围内可变的，在水下运动计算模拟时应考虑潜航员的水下活动形成可动重量，及造成的质量特征矩阵的变化。这里还应当建立质量矩阵、附加质量阻尼和惯性矩与装具姿态的关系，根据装具在水下的四肢运动速度低、运动幅度小，大幅度的运动姿势需要与推进器推力相互配合，克服扶正力矩矢量的作用等特点。对装具进行水下作业活动时的质量矩阵与运动姿势建立如下关系：

式中：m_i表示装具的可动质量，$E{_i} = {\left[ {{x_i}{y_i}{z_i}} \right]^T},\left( {i = 1,2,3, \ldots } \right)$表示可动质量的质心在体轴系中的位置。i表示位于关节处的随体坐标系，伴随装具四肢的运动，可动质量的质心位置随时间而变化，由于装具的活动关节的自由度限定了四肢的运动范围和运动路径，故可动质量的质心在关节衔接处的随体坐标系的空间运动轨迹${E'_i}\left( t \right) = {\left[ {{x_i}\left( t \right){\rm{ }}{y_i}\left( t \right){\rm{ }}{z_i}\left( t \right)} \right]^T}$，可利用体轴系和关节随体坐标系原点矢径\[{R_i} = {\left[ {{x_{0i}}{y_{0i}}{z_{0i}}} \right]^T}\left( {i = 1,2,3,4} \right)\]换算至体轴系中：

决定装具附加质量阻尼的构型，主要包括装具的大体积浮力背包、躯干、头盔、上肢、下肢等主要部件，下肢仅可做小幅度转动，其动力学影响可忽略，对装具上肢的相对运动姿态可采用高效率的Hess-Smith方法自动划分三角形的面元，实时计算附加质量阻尼矩阵，便于实时求解动力学方程组。在装具上肢活动的肩关节和肘关节处对称的建立4个随体坐标。初步设计中装具做直立姿态前进时，浮心与重心的相对位置设定为：x_B=0，y_B=－0.013 m，z_B=0.031 m。装具在水中具有5°前倾角，由于装具低速运动，恢复力矩的值是决定装具运动稳定性和水下活动能力和人员舒适性的关键因素，也涉及到装具的安全性。因此在设计中增加了可调节位置和可抛弃的重块，从而可调节扶正力矩。体轴系中，推进器安装位置为l₂=0.12 m，l₃=0.385 m。

装具的水动力系数采用了水池试验测定^[11]，由于装具的直立行走工况最为常用，以装具直立行走的姿态为水池测试对象，制作1∶1的水池试验模型，进行了拖曳试验和操纵性能试验，通过对试验数据的回归整理，测试获得了相关的水动力系数为X_u=0.032，X_u|u|=0.762，Y_v=0.872，Y_v|v|=1.323，Z_w=0.355，Z_w|w|=0.018 3，K_p=0.023，K_p|p|=0.001，M_q=0.267，M_q|q|=0.019，N_r=0.069，N_r|r|=0.003。

装具的水下运动姿态变化对水动力阻尼系数有较大影响。本研究仅对直立航行姿态进行了测试，还需要对主要部件相对运动形成的水动力阻尼进行

估算，装具的四肢为柱形和锥形，对如双臂相对于装具的主体的运动形成的水动力阻尼力采用圆柱水动力阻尼计算。

D_j中的部件水动力系数取低雷诺数范围内的圆柱运动阻尼系数。对关节随体坐标系中的相对运动的关节在运动方向的投影面积进行计算。与常规潜水器的运动性能和稳定性能的评估方法不同，本研究利用该钝体的水池测试获得流体动力系数以及装具的质量和阻尼特性，展开为上述形式，代入动力学计算模型，计算评估装具的水下直线运动稳定性能，航行方向稳定性能以及姿态等静稳定性能，并评估装具的动稳定性。对装具完成相关的水下作业活动所应当具备的活动能力，如俯身作业、翻身等典型运动进行计算。

装具的水下运动受到潜航员实时控制，在装具的设计阶段对装具水下运动是否可控，是否符合人机工程要求，以及总体布置参数的选择设计是否合理等需要进行验证。选择已有的控制和分析策略对设计参数进行评估显得尤为重要。在装具的初步设计阶段，设定了航速、海流、活动范围等目标量，利用动力学模型评估与之相关的设计参数如推进器布置、恢复力矩、空间活动轨迹的设计是否处于合理的范围，动力学计算流程框图如图2所示。

图2 仿真计算流程 Fig.2 Computational flow chart

图选项

运用Matlab Simulink工具箱编制了程序求解式(1)，在给定装具的初始位置和姿态的情况下，以速度为未知变量时方程组为一阶微分方程组，控制方程组的求解采用了四阶龙格库塔法进行求解。求解过程中需要调用各项系数矩阵和右端项。分别对质量属性矩阵、惯性力矩阵、水动力矩阵、恢复力矩阵、控制力矩阵分别编制了对应的求解子程序，对推进器匹配、面元划分等定义为两个计算模块，计算输出的量包括装具重心的轨迹、姿态角、运动速度等未知变量。

六自由度模型的计算模块包括：

1)质量属性矩阵的计算

根据装具的总体布置可得到装具的质量M₁和惯性矩矩阵M₂，利用式(7)求解质量属性矩阵，包括质量矩阵M_R与附加质量矩阵M_A两部分，质量矩阵含有可动质量，应用式(8)求解可动质量矩阵，通过指定可动质量的质心在关节衔接处的随体坐标系的空间运动轨迹曲线预先给定关节处的局部坐标系下装具在水下的四肢活动姿态随时间的变化轨迹，将质心轨迹的时间历程线性插值，计算给定的时间步长对应的质心位置，应用式(10)通过坐标变换到随体坐标系中获得质量和转动惯量。

2)附加质量矩阵的计算

式(9)包括了所需求解的附加质量，由于装具为钝体外形，利用Hess-smith算法计算得到的附加质量矩阵中非对角线元素的计算结果往往具有较大的误差，本研究仅计算附加质量矩阵对角线的元素，在装具外形发生变化后需要对修改后的装具外形表面划分面元，通过对三维造型软件进行二次开发，在给出了四肢活动轨迹后，可确定某一时刻的装配体姿态，导出装配体的外表面，并对相交曲面进行布尔运算，自动修复后，完成表面面元的划分，输出面元划分文件后代入附加质量计算模块进行计算。

3)水动力阻尼矩阵计算

装具在水下进行低速运动，根据表3给出的水流阻力和力矩系数，计算水流阻尼力矩，利用式(11)、(12)所示的二次函数计算可动部件造成的水流阻力的影响。

4)旋转效应矩阵的计算

包括了相对于初始重心位置处的质量分布形成的质量惯性矩，由于可动质量距离重心较远，形成的惯性矩的变化量较大，通过质心的变化造成的惯性矩可利用造型软件计算后代入式(4)中计算惯性力矩。

5)推进控制模块设计及计算

本设计中需要对布置在腰间的四部推进器发出的推力进行合理的匹配，才能形成对装具的合理控制力和矩，利用式(6)计算所需的推力匹配关系。

6)恢复力矩的设计求解

恢复力矩需要给定装具的重量和浮力，重心和浮心的相对位置，根据式(5)计算恢复力矩设定装配调整参数。

上述各模块的计算流程见图3。

图3 运动模型的求解流程 Fig.3 Solving schemes of kinematics model

图选项

装具近海底以0.5 kn的航速直线航行时，仅开启两只水平推进器发出同等大小的推力，17.25 kg的推力克服水平来流的阻力实现水平直线前进，通过求解动力学方程组，获得了装具在直线前进的过程中航行速度、俯仰角度随航行时间的变化规律。通过求解动力学方程验证装具在直线航行时的运动稳定性能。

腰部安装的两只水平推进器在进入稳定的直线航行状态后，调整两只推进器的推力大小，增加右侧推进器推力为23.5 kg和25.5 kg，降低左侧的推进器推力为11 kg和9 kg，形成的航行轨迹如图6所示，对应的滚转角和俯仰角如图7所示。

图4 装具水下直航轨迹 Fig.4 Straight line walking trajectory of ADS

图选项

图5 装具俯仰角与航行速度 Fig.5 Pitch and cruise velocity of ADS

图选项

图6 装具回转运动轨迹 Fig.6 Full turn trajectory of ADS

图选项

图7 装具回转运动姿态角 Fig.7 Full turn phase angle of ADS

图选项

以直立姿态为基本姿态，取随体坐标系x₁y₁z₁和x₂y₂z₂为左右肩关节的随体坐标系，该随体坐标系原点与体轴系的矢径：|r₀₁|=0.68 m，左右肩关节的随体坐标系原点与体轴x₀的夹角为$\phi $_01，02=67.5°，与体轴y₀的夹角为θ_01，02=13.2°，肘关节相对肩关节附连运动。假定由潜航员体力驱动装具使得双臂绕随体系的原点为不动点做角速度为ω=1°/s的缓慢转动。

装具同时发出推力克服水平来流的阻力做0.5 kn的前进运动。通过求解前述建立的动力学方程组，可知装具仍然具有一定的直线保持性，但装具的姿态角轨迹运动情况如图8所示。由于双臂的微速旋转运动形成的扰动作用力和力矩，对纵倾角的波动有一定的改善。当装具静止于水中时，潜航员仍然做上述运动，那么装具会发生缓慢的自转运动，并伴有增加前倾角度的趋势，姿态角如图9所示。

图8 有航速时对姿态和速度的影响 Fig.8 Effect of arm swing on gesture of cruising ADS at given speed

图选项

图9 无航速时对姿态和速度的影响 Fig.9 Effect of arm swing on gesture of stationary ADS

图选项

结合刚体动力学模型和多体运动学模型建立了用于计算潜水装具的动力学模型，针对装具的水下运动具有多体运动的特性，着重细致的建立了动力学系统中的质量矩阵模型和附加阻尼矩阵模型。并对多体运动情况下的动态附加质量阻尼和动态质量特性进行了计算，通过对直航、回转、关节运动等典型运动状态的建模计算得出如下结论：

1)首次建立包含多体动运动特性模型的完善装具运动动力学模型，对装具的水下水动力预报采用了快速水动力预报模块，应用于装具水下运动性能的设计参数的设计评估预报。

2)建立可动质量沿着装具躯体运动造成的重心、质量特性的变化模型。首次结合多体动力学的数学模型描述装具运动。

3)结合装具的低速活动四肢频繁操作运动 并需精确定位等特点，以及惯性力矩和重力浮力等静力起主要影响的作用的特点，首次对外形的变化引起的附加质量采用CAD曲面处理、网格自动划分Hess-Smith 快速预报方法进行预报附加质量力。

在水池试验和文献给出了详尽的水动力参数、总体预装给出的运动和控制特征参数的基础上,将上述两个运动模型整合，形成实时预报的程序模块，可对装具设计过程中给出总体装配性能参数进行初步的分析评价，对水下直航、回转、自转等典型的运动进行预报，对运动性能建立了评估的平台，给出了典型的仿真计算结果。