CN103942383A - 一种深海作业型水下机器人的动力学和运动学估计方法 - Google Patents

Abstract

本发明涉及深海作业型ROV技术领域，具体地说是一种深海作业型水下机器人的动力学和运动学估计方法。本发明包括：建立定坐标系、随体坐标系和推进器坐标系，估计六自由度坐标转换矩阵；估计作业型水下机器人质量矩阵以及引起的柯氏力和向心力矩阵；估计作业型ROV所受水动力；估计作业型ROV所受静力；估计作业型ROV推力；估计未知干扰项；确定作业型ROV最终的动力学和运动学模型。本发明利用动力学、流体力学和潜艇操纵性等理论进行深海作业型ROV的动力学和运动学建模，针对水下潜器复杂的数学模型，利用对称理论和小量忽略的方法对深海作业型ROV的数学模型进行简化，所建立的模型可以更精确的估计ROV受力情况。

Description

一种深海作业型水下机器人的动力学和运动学估计方法

技术领域

本发明涉及深海作业型ROV技术领域，具体地说是一种深海作业型水下机器人的动力学和运动学估计方法。

背景技术

ROV的动力学和运动学模型体现了ROV的运动规律，是水下机器人对于环境和***信息的一种量化表示，对于水下机器人感知层体系结构中的信息处理技术、环境感知以及指令理解都是重要的参考。研究潜水器在水中受到各种力矩以及力影响下的运动规律，从而建立潜水器的运动方程，是研究其控制***的基础。但是，如果模型过于复杂，则会导致控制***很复杂，甚至不能实现；如果模型过于简单，就不能真实反映***的运动，导致控制性能下降。因此，建立水下机器人空间动力学方程，并适当简化，对于研究水下机器人的控制来说十分重要。

ROV数学模型包括动力学模型和运动学模型，是基于动力学、流体力学和潜艇操纵性等理论建立起来的多变量微分方程组。

1967年美国泰勒海军舰船研究和发展中心(DTNSRDC)发表的《用于潜艇模拟研究的标准运动方程》及1979年修正的潜艇标准运动方程，已经广泛应用于水下潜器建模等方面。但是，方程中含有上百个水动力系数，要获得全部的水动力系数是非常困难的，这就需要对方程进行合理的简化。

作业型ROV普遍使用大功率液压推进***，液压推进***作为执行器，其模型的精确程度对最终的控制性能有着直接的影响。实际工程中，推进器的安装角度、位置以及推进器的推力矢量都存在着误差。而在绝大多数水下运载器建模方法的研究中，均忽略了推进***的安装误差以及推力矢量误差，只是将推进***推力以及力矩作为理想的固定常数处理，这样的处理方式会对ROV的操纵性能以及稳定性控制等都会产生影响。

发明内容

本发明目的在于提供一种用于实现深海作业型ROV动力学和运动学建模，更适用于深海作业型ROV深海作业型水下机器人的动力学和运动学估计方法。

本发明的目的是这样实现的：

（1）建立定坐标系、随体坐标系和推进器坐标系,估计六自由度坐标转换矩阵；

（2）估计作业型水下机器人质量矩阵以及引起的柯氏力和向心力矩阵；

（3）估计作业型ROV所受水动力；

（4）估计作业型ROV所受静力；

（5）估计作业型ROV推力；

（6）估计未知干扰项；

（7）确定作业型ROV最终的动力学和运动学模型。

所述步骤（1）包括：

（1）建立定坐标系：原点E确定在一定点，Eξ轴位于水平面，并以水下机器人主航向为正向；Eη轴位于Eξ轴所在的水平面，按右手法则将Eξ轴顺时针旋转90°，即Eζ轴垂直于ξEη坐标平面，指向地心为正；

（2）建立随体坐标系：纵轴OX平行于潜水器主体基线，指向艏部为正；横轴OY平行于基线面，指向右舷为正；垂轴OZ位于潜水器主体中纵剖面内，指向底部为正；

（3）建立推进器坐标系：设定推进器中心的具***置，作为推进器坐标系的原点，推进器坐标系的纵轴O_iX_i与随动坐标系的纵轴正方向成角度α，根据右手定则O_iZ_i的方向向下，O_iY_i垂直于X_iO_iZ_i面；

（4）计算六自由度坐标转换矩阵：

$J\left({\mathrm{\η}}_2\right)=\left[\begin{array}{cc}{J}_1\left({\mathrm{\η}}_2\right)& 0\\ 0& J_2\left({\mathrm{\η}}_2\right)\end{array}\right],$

其中： $J_1\left({\mathrm{\η}}_2\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right],$

φ为艏向角；θ为纵倾角；ψ为横倾角。

所述步骤（2）包括：

（1）计算作业型水下机器人的质量矩阵：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& -I_{\mathrm{yx}}& I_y& -I_{\mathrm{yz}}\\ -{\mathrm{my}}_G& {\mathrm{mx}}_G& 0& -I_{\mathrm{zx}}& -I_{\mathrm{zy}}& I_z\end{array}\right],$

作业型水下机器人质量引起的柯氏力和向心力矩阵：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& m(y_{G}q+z_{G}r)& -m(x_{G}q-w)& -m(x_{G}r+v)\\ 0& 0& 0& -m(y_{G}p+w)& m(z_{G}r+x_{G}p)& -m(y_{G}r-u)\\ 0& 0& 0& -m(z_{G}p-v)& -m(z_{G}q+u)& m(x_{G}p+y_{G}q)\\ -m(y_{G}q+z_{G}r)& m(y_{G}p+w)& m(z_{G}p-v)& 0& -I_{\mathrm{yz}}q-I_{\mathrm{xz}}p+I_{z}r& I_{\mathrm{yz}}r+I_{\mathrm{xy}}p-I_{y}q\\ m(x_{G}q-w)& -m(z_{G}r+x_{G}p)& m(z_{G}q+u)& I_{\mathrm{yz}}q+I_{\mathrm{xz}}p-I_{z}r& 0& -I_{\mathrm{xz}}r-I_{\mathrm{xy}}q+I_{x}p\\ m(x_{G}r+v)& m(y_{G}r-u)& -m(x_{G}p+y_{G}q)& -I_{\mathrm{yz}}r-I_{\mathrm{xy}}p+I_{y}q& I_{\mathrm{xz}}r+I_{\mathrm{xy}}q-I_{x}p& 0\end{array}\right]$

m为作业型ROV的质量；[x_G y_G z_G]^T为作业型ROV重心坐标；I_x、I_y、I_z为ROV绕X、Y、Z三轴的转动惯量，而I_xy、I_yx、I_xz、I_zx、I_yz、I_zy则为惯性积；

（2）根据对称面原理和小量忽略的方法可将质量矩阵和柯氏力向心力矩阵进行化简作业型水下机器人的质量矩阵：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& 0& 0\\ 0& m& 0& 0& 0& 0\\ 0& 0& m& 0& 0& 0\\ 0& 0& 0& I_x& 0& 0\\ 0& 0& 0& 0& I_y& 0\\ 0& 0& 0& 0& 0& I_z\end{array}\right],$

作业型水下机器人质量引起的柯氏力和向心力矩阵：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& \mathrm{mw}& -\mathrm{mv}\\ 0& 0& 0& -\mathrm{mw}& 0& \mathrm{mu}\\ 0& 0& 0& \mathrm{mv}& -\mathrm{mu}& 0\\ 0& \mathrm{mw}& -\mathrm{mv}& 0& I_{z}r& -I_{y}q\\ -\mathrm{mw}& 0& \mathrm{mu}& -I_{z}r& 0& I_{x}p\\ \mathrm{mv}& -\mathrm{mu}& 0& I_{y}q& -I_{x}p& 0\end{array}\right].$

所述步骤（3）包括：

（1）计算惯性类水动力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& X_{\stackrel{\·}{v}}& X_{\stackrel{\·}{w}}& X_{\stackrel{\·}{p}}& X_{\stackrel{\·}{q}}& X_{\stackrel{\·}{r}}\\ Y_{\stackrel{\·}{u}}& Y_{\stackrel{\·}{v}}& Y_{\stackrel{\·}{w}}& Y_{\stackrel{\·}{p}}& Y_{\stackrel{\·}{q}}& Y_{\stackrel{\·}{r}}\\ Z_{\stackrel{\·}{u}}& Z_{\stackrel{\·}{v}}& Z_{\stackrel{\·}{w}}& Z_{\stackrel{\·}{p}}& Z_{\stackrel{\·}{q}}& Z_{\stackrel{\·}{r}}\\ K_{\stackrel{\·}{u}}& K_{\stackrel{\·}{v}}& K_{\stackrel{\·}{w}}& K_{\stackrel{\·}{p}}& K_{\stackrel{\·}{q}}& K_{\stackrel{\·}{r}}\\ M_{\stackrel{\·}{u}}& M_{\stackrel{\·}{v}}& M_{\stackrel{\·}{w}}& M_{\stackrel{\·}{p}}& M_{\stackrel{\·}{q}}& M_{\stackrel{\·}{r}}\\ N_{\stackrel{\·}{u}}& N_{\stackrel{\·}{v}}& N_{\stackrel{\·}{w}}& N_{\stackrel{\·}{p}}& N_{\stackrel{\·}{q}}& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\begin{array}{cc}\left[\begin{array}{cccccc}0& 0& 0& 0& -a_3& a_2\\ 0& 0& 0& a_3& 0& -a_1\\ 0& 0& 0& -a_2& a_1& 0\\ 0& -a_3& a_2& 0& -b_3& b_2\\ a_3& 0& -a_1& b_3& 0& -b_1\\ -a_2& a_1& 0& -b_2& b_1& 0\end{array}\right]& \begin{array}{c}{a}_1=X_{\stackrel{\·}{u}}u+X_{\stackrel{\·}{v}}v+X_{\stackrel{\·}{w}}w+X_{\stackrel{\·}{p}}p+X_{\stackrel{\·}{q}}q+X_{\stackrel{\·}{r}}r\\ a_2=X_{\stackrel{\·}{v}}u+Y_{\stackrel{\·}{v}}v+Y_{\stackrel{\·}{w}}w+Y_{\stackrel{\·}{p}}p+Y_{\stackrel{\·}{q}}q+Y_{\stackrel{\·}{r}}r\\ a_3=X_{\stackrel{\·}{w}}u+X_{\stackrel{\·}{w}}v+Z_{\stackrel{\·}{w}}w+Z_{\stackrel{\·}{w}}p+Z_{\stackrel{\·}{q}}q+Z_{\stackrel{\·}{r}}r\\ b_1=X_{\stackrel{\·}{p}}u+Y_{\stackrel{\·}{p}}v+Z_{\stackrel{\·}{p}}w+K_{\stackrel{\·}{p}}p+K_{\stackrel{\·}{q}}q+K_{\stackrel{\·}{r}}r\\ b_2=X_{\stackrel{\·}{q}}u+Y_{\stackrel{\·}{q}}v+Z_{\stackrel{\·}{q}}w+K_{\stackrel{\·}{q}}p+M_{\stackrel{\·}{q}}q+M_{\stackrel{\·}{r}}r\\ b_3={X_{\stackrel{\·}{r}}}_{}u+Y_{\stackrel{\·}{r}}v+Z_{\stackrel{\·}{r}}w+K_{\stackrel{\·}{r}}p+M_{\stackrel{\·}{r}}q+N_{\stackrel{\·}{r}}r\end{array}\end{array}$

化简为：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& 0& 0& 0& 0& 0\\ 0& Y_{\stackrel{\·}{v}}& 0& 0& 0& 0\\ 0& 0& Z_{\stackrel{\·}{w}}& 0& 0& 0\\ 0& 0& 0& K_{\stackrel{\·}{p}}& 0& 0\\ 0& 0& 0& 0& M_{\stackrel{\·}{q}}& 0\\ 0& 0& 0& 0& 0& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v\\ 0& 0& 0& Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u\\ 0& 0& 0& -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0\\ 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v& 0& -N_{\stackrel{\·}{r}}r& M_{\stackrel{\·}{q}}q\\ Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u& N_{\stackrel{\·}{r}}r& 0& -K_{\stackrel{\·}{p}}p\\ -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0& -M_{\stackrel{\·}{q}}q& K_{\stackrel{\·}{p}}p& 0\end{array}\right]$

（2）计算流体粘性力系数矩阵

总的粘性水动力阻尼矩阵D(v)分解为线性水动力阻尼矩阵D_L(v)和非线性水动力阻尼矩阵D_NL(v)之和：

D(v)=D_L(v)+D_NL(v)

$D\left(v\right)=-\left[\begin{array}{cccccc}{X}_u+X_{u\left|u\right|}\left|u\right|& 0& 0& 0& 0& 0\\ 0& Y_v+Y_{v\left|v\right|}\left|v\right|& 0& 0& 0& 0\\ 0& 0& Z_w+Z_{w\left|w\right|}\left|w\right|& 0& 0& 0\\ 0& 0& 0& K_p+K_{p\left|p\right|}\left|p\right|& 0& 0\\ 0& 0& 0& 0& M_q+M_{q\left|q\right|}\left|q\right|& 0\\ 0& 0& 0& 0& 0& N_r+N_{r\left|r\right|}\left|r\right|\end{array}\right]$

所述步骤（4）包括：

计算作业型水下机器人所受重力W=mg；计算作业型ROV所受浮力，ρ为海水密度，g为重力加速度，为ROV排水体积，则静力模型为：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ (y_{G}W-y_{c}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -(x_{G}W-x_{C}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\sin \mathrm{\θ}\\ (x_{G}W-x_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}+(y_{G}W-y_{C}B)\sin \mathrm{\θ}\end{array}\right]$

式中[x_C y_C z_C]^T为ROV浮心坐标，ROV通过配重可以将随动坐标系原点与ROV重心重合，则：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ -y_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -x_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\sin \mathrm{\θ}\\ -x_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}-y_{C}B\sin \mathrm{\θ}\end{array}\right].$

所述的步骤（5）包括：

（1）计算随动坐标系到第i个推进器坐标系的包含误差项的坐标转换矩阵

$\begin{array}{c}{\mathrm{\Ω}}_O^i=R(X,{\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)R(Y,{\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)R(Z,{\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)\\ =\begin{array}{ccc}\left[\begin{array}{ccc}\cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& -\sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ \sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& \cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ 0& 0& 1\end{array}\right]\·& \left[\begin{array}{ccc}\cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& -\sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\\ 0& 1& 0\\ \sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& \cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\end{array}\right]\·& \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\\ 0& -\sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\end{array}\right]\end{array}\end{array}$

${\stackrel{\→}{p}}_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T$ 为空间任一推进器坐标系原点在动系中的坐标值；

（2）计算推力：

第i个推进器的推力作用在ROV上的力为：

$\begin{array}{cc}{T}_i={\mathrm{\Ω}}_i^O\·\left[\begin{array}{c}{f}_i\\ 0\\ 0\end{array}\right]& {\mathrm{\Ω}}_i^O={\left({\mathrm{\Ω}}_O^i\right)}^{-1}\end{array},$

式中f_i为推进器的实际推力大小；

（3）计算含有误差项的推力力矩：

$\begin{array}{c}{M}_i=\stackrel{\→}{r}\×T_i={\stackrel{\→}{p}}_i\×T_i=\left|\begin{array}{ccc}i& j& k\\ -(x_i+{\mathrm{\Δx}}_i)& -(y_i+{\mathrm{\Δy}}_i)& -(z_i+{\mathrm{\Δz}}_i)\\ T_{\mathrm{ix}}& T_{\mathrm{iy}}& T_{\mathrm{iz}}\end{array}\right|\\ =[T_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)]i+[T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)]j+[T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)]k\\ ={\left[\begin{array}{ccc}{T}_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)& T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)& T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)\end{array}\right]}^T\\ =\begin{array}{cc}\left[\begin{array}{ccc}0& (z_i+{\mathrm{\Δz}}_i)& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& (x_i+{\mathrm{\Δx}}_i)\\ (y_i+{\mathrm{\Δy}}_i)& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·& \left[,\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]\end{array}\end{array}$

式中 $\left[\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]=T_i,$ 为第i个推进器的推力作用在ROV上的力的三个分量；

（4）作业型ROV的推力为：

$T=\left[\begin{array}{c}\underset{i=1}{\overset{7}{\mathrm{\Σ}}}{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\\ \underset{i=1}{\overset{7}{\mathrm{\Σ}}}\left[\begin{array}{ccc}0& z_i+{\mathrm{\Δz}}_i& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& x_i+{\mathrm{\Δx}}_i\\ y_i+{\mathrm{\Δy}}_i& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·[{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\end{array}\right].$

所述步骤（6）：深海作业型ROV所受外界的未知干扰项包括海流的影响、光缆的拖拽力和作业机械手的反冲力，用有界的六自由度力/力矩△f表示。

所述步骤（7）作业型ROV最终的动力学和运动学模型为：

$(M_{\mathrm{RB}}+M_A)\stackrel{\·}{v}+(C_{\mathrm{RB}}\left(v\right)+C_A\left(v\right))v+g\left({\mathrm{\η}}_2\right)=T+\mathrm{\Δf}.$

$\stackrel{\·}{\mathrm{\η}}=J\left({\mathrm{\η}}_2\right)v$

本发明的有益效果在于：

本发明应用动力学、流体力学和潜艇操纵性等理论建立ROV动力学和运动学模型；对ROV的受力分析较为完全，对所受力和力矩的计算较为准确；结构简明，各步骤清晰任务明确，易于理解和便于数学模型的复查、查错；根据相关理论和深海作业型ROV的物理结构特征，利用相关原理和可简化条件，简化了深海作业型ROV的动力学和运动学模型。在可以非常好地表现近似真实的ROV受力情况的基础上，使得深海作业型ROV的模型更加简单明了，降低了模型建立的难度，也为以后的控制设计提供了方便；克服传统ROV建模设计中对推进***安装误差的忽略，将推进***的安装误差纳入到ROV模型中；能够清楚地表示出推进器的所有安装误差，包括角度误差和位置误差，使得模型更加地精确；本发明能够将推进器推力矢量的误差转化成推进器的安装误差，简化了推进***误差模型的建立；本发明的深海作业型ROV模型比较传统深海作业型ROV，尤其在推进***建模方面更加地精确；本发明通用性好，可广泛用于深海作业型ROV动力学和运动学建模中。

附图说明

附图1是本发明流程示意图。

附图2是本发明固定和随动坐标系示意图。

附图3是本发明推进器坐标系示意图。

附图4是本发明推进器安装示意图。

附图5是本发明推进器安装误(角度和位置误差)差示意图。

附图6是本发明推进器推力矢量误差示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

发明流程示意图如图1。本发明实施例是：

(1)分析深海作业型ROV的受力情况。包括水动力、静力、推进器推力、缆力以和海流影响以及机械手的反冲力等。

(2)建立固定坐标系步骤

忽略地球表面加速度相对于低速ROV的影响,则固定坐标系可以被认为是一个惯性坐标系。原点E可选在地球上某一定点，如海面或海中任一点。Eξ轴位于水平面，并以ROV主航向为正向；Eη轴位于Eξ轴所在的水平面，按右手法则将Eξ轴顺时针旋转90°即是；Eζ轴垂直于ξEη坐标平面，指向地心为正。固定坐标系示意图如图2所示。

(3)建立随动坐标系步骤

以固定于潜水器上的右手直角坐标系O-XYZ为随体坐标系。纵轴OX平行于潜水器主体基线，指向艏部为正；横轴OY平行于基线面，指向右舷为正；垂轴OZ位于潜水器主体中纵剖面内，指向底部为正。随动坐标系示意图如图2所示。

(4)建立推进器坐标系步骤

推进器坐标系固连在ROV本体上。首先要人为设定推进器中心在ROV上的具***置，以此作为推进器坐标系的原点，然后根据需要设定推进器坐标系。推进器坐标系的纵轴O_iX_i与随动坐标系的纵轴正方向成一定的角度α，此角度大小由人为规定，根据右手定则O_iZ_i的方向向下，O_iY_i垂直于X_iO_iZ_i面。推进器进行安装时，只要将推进器的中轴线与O_iX_i轴重合，并且将推进器螺旋桨背离O_iX_i的方正向。推进器坐标系示意图如图3所示，推进器安装示意图如图4所示。

(5)计算固定坐标系到随动坐标系的坐标转移矩阵步骤

ROV相对于惯性坐标系的速度为v，v在动系上的投影为u(纵向速度)、v(横向速度)、w(垂向速度)；同理，潜器以角速度Ω在动系上的投影为p(横倾角速度)、q(纵倾角速度)、r(偏航角速度)；ROV所受外力F在动系上的投影为τ_X(纵向力)、τ_Y(横向力)、τ_Z(垂向力)；力矩M的投影为τ_K(横倾力矩)、τ_M(纵倾力矩)、τ_N(偏航力矩)。指向坐标轴的正向为速度和力的分量的正方向，用右手定则来规定角速度和力矩的正负号。

ROV的空间位置和姿态，可用动系原点的地面坐标值[x y z]^T和动系相对于定系的三个姿态角(φ,θ,ψ)来确定。艏向角φ，是主体的对称面XOZ绕铅垂轴Eζ水平旋转，与铅垂面ξEζ的夹角在定系水平面(ξEη)的投影，向右转为正；纵倾角θ是主体的水线面XOY绕OY轴俯仰与定系水平面ξEη的夹角在定系铅垂面的投影，θ向尾倾为正；横倾角ψ是主体的对称面XOZ绕OX轴横倾与定系铅垂面ξEζ的夹角在横滚面ηEζ的投影，向右倾为正。

ROV六自由度位置/姿态向量为：

$\begin{array}{ccc}\mathrm{\η}={\left[\begin{array}{cc}{\mathrm{\η}}_1^T& {\mathrm{\η}}_2^T\end{array}\right]}^T& {\mathrm{\η}}_1={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T& {\mathrm{\η}}_2={\left[\begin{array}{ccc}\mathrm{\φ}& \mathrm{\θ}& \mathrm{\ψ}\end{array}\right]}^T\end{array}$

ROV六自由度速度/角速度向量为：

$\begin{array}{ccc}v={\left[\begin{array}{cc}{v}_1^T& v_2^T\end{array}\right]}^T& v_1={\left[\begin{array}{ccc}u& v& w\end{array}\right]}^T& v_2={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T\end{array}$

ROV六自由度力/力矩向量为：

$\begin{array}{ccc}\mathrm{\τ}={\left[\begin{array}{cc}{\mathrm{\τ}}_1^T& {\mathrm{\τ}}_2^T\end{array}\right]}^T& {\mathrm{\τ}}_1={\left[\begin{array}{ccc}{\mathrm{\τ}}_X& {\mathrm{\τ}}_Y& {\mathrm{\τ}}_Z\end{array}\right]}^T& {\mathrm{\τ}}_2={\left[\begin{array}{ccc}{\mathrm{\τ}}_K& {\mathrm{\τ}}_K& {\mathrm{\τ}}_N\end{array}\right]}^T\end{array}$

描述ROV沿三轴方向直线运动的速度转换方程为

${\stackrel{\·}{\mathrm{\η}}}_1=J_1\left({\mathrm{\η}}_2\right)v_1$

式中 $J_1\left({\mathrm{\η}}_2\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]$ 为速度坐标转换矩阵。

则ROV的速度你转换方程为

$v_1=J_1^{-1}\left({\mathrm{\η}}_2\right){\stackrel{\·}{\mathrm{\η}}}_1$

描述ROV绕三轴转动的角速度转换方程为

${\stackrel{\·}{\mathrm{\η}}}_2=J_2\left({\mathrm{\η}}_2\right)v_2$

式中为角速度坐标转换矩阵。

总的六自由度坐标转换矩阵为

$J\left({\mathrm{\η}}_2\right)=\left[\begin{array}{cc}{J}_1\left({\mathrm{\η}}_2\right)& 0\\ 0& J_2\left({\mathrm{\η}}_2\right)\end{array}\right]$

由以上结果可以得到最终的ROV运动学方程为

$\stackrel{\·}{\mathrm{\η}}=J\left({\mathrm{\η}}_2\right)v$

(6)作业型ROV质量矩阵以及其引起的柯氏力和向心力矩阵的计算步骤

a.大多数作业型ROV通常是具有近似的三个对称面外型为长方体。因为具有三个近似的对称面，所以质量矩阵的非对角元素远远小于对角元素，因此附加质量惯性矩阵中的非对角元素可以进行忽略；

b.作业型ROV通常是通过配重来决定重心和浮心的相对位置。因此ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。这样对ROV模型的简化具有很重要的意义；

c.由于作业型ROV各种设备外形复杂而且布置凌乱，不同的任务需要更换不同的作业设备，因此内部结构较复杂。作业型ROV参数矩阵中的非对角元素也很难精确获得。但事实上，在大多数应用场合，对角假设已经可以非常好地近似真实的ROV受力情况。

则作业型ROV质量矩阵：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& -I_{\mathrm{yx}}& I_y& -I_{\mathrm{yz}}\\ -{\mathrm{my}}_G& {\mathrm{mx}}_G& 0& -I_{\mathrm{zx}}& -I_{\mathrm{zy}}& I_z\end{array}\right]$

m为作业型ROV的质量；[x_G y_G z_G]^T为作业型ROV重心坐标；I_x、I_y、I_z为ROV绕X、Y、Z三轴的转动惯量，而I_xy、I_yx、I_xz、I_zx、I_yz、I_zy则为惯性积。

可以简化为：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& 0& 0\\ 0& m& 0& 0& 0& 0\\ 0& 0& m& 0& 0& 0\\ 0& 0& 0& I_x& 0& 0\\ 0& 0& 0& 0& I_y& 0\\ 0& 0& 0& 0& 0& I_z\end{array}\right]$

作业型ROV质量引起的柯氏力和向心力矩阵：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& m(y_{G}q+z_{G}r)& -m(x_{G}q-w)& -m(x_{G}r+v)\\ 0& 0& 0& -m(y_{G}p+w)& m(z_{G}r+x_{G}p)& -m(y_{G}r-u)\\ 0& 0& 0& -m(z_{G}p-v)& -m(z_{G}q+u)& m(x_{G}p+y_{G}q)\\ -m(y_{G}q+z_{G}r)& m(y_{G}p+w)& m(z_{G}p-v)& 0& -I_{\mathrm{yz}}q-I_{\mathrm{xz}}p+I_{z}r& I_{\mathrm{yz}}r+I_{\mathrm{xy}}p-I_{y}q\\ m(x_{G}q-w)& -m(z_{G}r+x_{G}p)& m(z_{G}q+u)& I_{\mathrm{yz}}q+I_{\mathrm{xz}}p-I_{z}r& 0& -I_{\mathrm{xz}}r-I_{\mathrm{xy}}q+I_{x}p\\ m(x_{G}r+v)& m(y_{G}r-u)& -m(x_{G}p+y_{G}q)& -I_{\mathrm{yz}}r-I_{\mathrm{xy}}p+I_{y}q& I_{\mathrm{xz}}r+I_{\mathrm{xy}}q-I_{x}p& 0\end{array}\right]$

可以化简为：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& \mathrm{mw}& -\mathrm{mv}\\ 0& 0& 0& -\mathrm{mw}& 0& \mathrm{mu}\\ 0& 0& 0& \mathrm{mv}& -\mathrm{mu}& 0\\ 0& \mathrm{mw}& -\mathrm{mv}& 0& I_{z}r& -I_{y}q\\ -\mathrm{mw}& 0& \mathrm{mu}& -I_{z}r& 0& I_{x}p\\ \mathrm{mv}& -\mathrm{mu}& 0& I_{y}q& -I_{x}p& 0\end{array}\right]$

(7)作业型ROV所受水动力建模步骤

1)流体惯性力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵

a.大多数作业型ROV通常是具有近似的三个对称面外型为长方体。因为具有三个近似的对称面，所以质量矩阵的非对角元素远远小于对角元素，因此附加质量惯性矩阵中的非对角元素可以进行忽略；

b.作业型ROV通常是通过配重来决定重心和浮心的相对位置。因此ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。这样对ROV模型的简化具有很重要的意义；

c.由于作业型ROV各种设备外形复杂而且布置凌乱，不同的任务需要更换不同的作业设备，因此内部结构较复杂。作业型ROV参数矩阵中的非对角元素也很难精确获得。但事实上，在大多数应用场合，对角假设已经可以非常好地近似真实的ROV受力情况。

流体惯性力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵为：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& X_{\stackrel{\·}{v}}& X_{\stackrel{\·}{w}}& X_{\stackrel{\·}{p}}& X_{\stackrel{\·}{q}}& X_{\stackrel{\·}{r}}\\ Y_{\stackrel{\·}{u}}& Y_{\stackrel{\·}{v}}& Y_{\stackrel{\·}{w}}& Y_{\stackrel{\·}{p}}& Y_{\stackrel{\·}{q}}& Y_{\stackrel{\·}{r}}\\ Z_{\stackrel{\·}{u}}& Z_{\stackrel{\·}{v}}& Z_{\stackrel{\·}{w}}& Z_{\stackrel{\·}{p}}& Z_{\stackrel{\·}{q}}& Z_{\stackrel{\·}{r}}\\ K_{\stackrel{\·}{u}}& K_{\stackrel{\·}{v}}& K_{\stackrel{\·}{w}}& K_{\stackrel{\·}{p}}& K_{\stackrel{\·}{q}}& K_{\stackrel{\·}{r}}\\ M_{\stackrel{\·}{u}}& M_{\stackrel{\·}{v}}& M_{\stackrel{\·}{w}}& M_{\stackrel{\·}{p}}& M_{\stackrel{\·}{q}}& M_{\stackrel{\·}{r}}\\ N_{\stackrel{\·}{u}}& N_{\stackrel{\·}{v}}& N_{\stackrel{\·}{w}}& N_{\stackrel{\·}{p}}& N_{\stackrel{\·}{q}}& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\begin{array}{cc}\left[\begin{array}{cccccc}0& 0& 0& 0& -a_3& a_2\\ 0& 0& 0& a_3& 0& -a_1\\ 0& 0& 0& -a_2& a_1& 0\\ 0& -a_3& a_2& 0& -b_3& b_2\\ a_3& 0& -a_1& b_3& 0& -b_1\\ -a_2& a_1& 0& -b_2& b_1& 0\end{array}\right]& \begin{array}{c}{a}_1=X_{\stackrel{\·}{u}}u+X_{\stackrel{\·}{v}}v+X_{\stackrel{\·}{w}}w+X_{\stackrel{\·}{p}}p+X_{\stackrel{\·}{q}}q+X_{\stackrel{\·}{r}}r\\ a_2=X_{\stackrel{\·}{v}}u+Y_{\stackrel{\·}{v}}v+Y_{\stackrel{\·}{w}}w+Y_{\stackrel{\·}{p}}p+Y_{\stackrel{\·}{q}}q+Y_{\stackrel{\·}{r}}r\\ a_3=X_{\stackrel{\·}{w}}u+X_{\stackrel{\·}{w}}v+Z_{\stackrel{\·}{w}}w+Z_{\stackrel{\·}{w}}p+Z_{\stackrel{\·}{q}}q+Z_{\stackrel{\·}{r}}r\\ b_1=X_{\stackrel{\·}{p}}u+Y_{\stackrel{\·}{p}}v+Z_{\stackrel{\·}{p}}w+K_{\stackrel{\·}{p}}p+K_{\stackrel{\·}{q}}q+K_{\stackrel{\·}{r}}r\\ b_2=X_{\stackrel{\·}{q}}u+Y_{\stackrel{\·}{q}}v+Z_{\stackrel{\·}{q}}w+K_{\stackrel{\·}{q}}p+M_{\stackrel{\·}{q}}q+M_{\stackrel{\·}{r}}r\\ b_3={X_{\stackrel{\·}{r}}}_{}u+Y_{\stackrel{\·}{r}}v+Z_{\stackrel{\·}{r}}w+K_{\stackrel{\·}{r}}p+M_{\stackrel{\·}{r}}q+N_{\stackrel{\·}{r}}r\end{array}\end{array}$

可以化简为：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& 0& 0& 0& 0& 0\\ 0& Y_{\stackrel{\·}{v}}& 0& 0& 0& 0\\ 0& 0& Z_{\stackrel{\·}{w}}& 0& 0& 0\\ 0& 0& 0& K_{\stackrel{\·}{p}}& 0& 0\\ 0& 0& 0& 0& M_{\stackrel{\·}{q}}& 0\\ 0& 0& 0& 0& 0& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v\\ 0& 0& 0& Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u\\ 0& 0& 0& -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0\\ 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v& 0& -N_{\stackrel{\·}{r}}r& M_{\stackrel{\·}{q}}q\\ Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u& N_{\stackrel{\·}{r}}r& 0& -K_{\stackrel{\·}{p}}p\\ -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0& -M_{\stackrel{\·}{q}}q& K_{\stackrel{\·}{p}}p& 0\end{array}\right]$

2)流体粘性力系数矩阵

ROV在运动时，受到线性和非线性的粘性类水动力。总的粘性水动力阻尼矩阵D(v)可以分解为线性水动力阻尼矩阵D_L(v)和非线性水动力阻尼矩阵D_NL(v)之和：

D(v)=D_L(v)+D_NL(v)

对于高速行进的六自由度水下运载器，其将受到高度耦合且非线性的水动力阻尼力。但对于低速运动的具有近似的三个对称面ROV，可以忽略耦合作用，并忽略高于二阶的阻尼项。则粘性水动力阻尼矩阵将为仅包括主运动的一次、二次水动力系数的对角阵，即

$D\left(v\right)=-\left[\begin{array}{cccccc}{X}_u+X_{u\left|u\right|}\left|u\right|& 0& 0& 0& 0& 0\\ 0& Y_v+Y_{v\left|v\right|}\left|v\right|& 0& 0& 0& 0\\ 0& 0& Z_w+Z_{w\left|w\right|}\left|w\right|& 0& 0& 0\\ 0& 0& 0& K_p+K_{p\left|p\right|}\left|p\right|& 0& 0\\ 0& 0& 0& 0& M_q+M_{q\left|q\right|}\left|q\right|& 0\\ 0& 0& 0& 0& 0& N_r+N_{r\left|r\right|}\left|r\right|\end{array}\right]$

式中X_u、Y_v等被称为一阶水动系数，其主符号代表运动产生的水动力/力矩的方向，下标则表示运动的方向；w|w|和v|v|等表示该项的大小与w²、v²成正比，而符号随来流方向而变。

(8)作业型ROV所受静力建模步骤

计算作业型ROV所受重力W=mg；计算作业型ROV所受浮力。ρ为海水密度，g为重力加速度，为ROV排水体积。

则静力模型为：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ (y_{G}W-y_{c}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -(x_{G}W-x_{C}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\sin \mathrm{\θ}\\ (x_{G}W-x_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}+(y_{G}W-y_{C}B)\sin \mathrm{\θ}\end{array}\right]$

式中[x_C y_C z_C]^T为ROV浮心坐标。

ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。则模型可以简化为：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ -y_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -x_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\sin \mathrm{\θ}\\ -x_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}-y_{C}B\sin \mathrm{\θ}\end{array}\right].$

(9)作业型ROV推力建模步骤

因为推进器坐标系是固连在ROV主体上的，所以推进器坐标系相对动坐标系是相对静止的。动坐标系到空间内任一坐标系的位姿转换矩阵为：

$T_O^i=\left[\begin{array}{c}\begin{array}{cc}{\mathrm{\Ω}}_O^i& {\stackrel{\→}{p}}_i\end{array}\\ \begin{array}{cccc}0& 0& 0& 1\end{array}\end{array}\right]$

此位姿转换矩阵的含义是：定坐标系首先按照向量 ${\stackrel{\→}{p}}_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T$ 平移，使坐标原点与空间内任一坐标系原点相重合，再根据旋转矩阵旋转坐标系就能够使随动坐标系和空间内任一坐标系相重合。

则对于人为确定好的推进器坐标系，规定逆时针旋转为正方向，将平移后坐标原点和推进器坐标系原点相重合的随动坐标系绕X、Y、Z轴分别旋转γ_i、β_i、α_i，使得随动坐标系与推进器坐标系重合。但是实际工程中推进器存在安装误差(包括角度误差和位置误差见图5)、推进器存在推力矢量误差，推力矢量误差是指：理想情况下，推进器的推力方向是沿着推进器的中轴的，而大多数情况下我们也是认为推进器的推力方向是沿着推进器的中轴的。但是实际情况中推力方向和中轴线是由偏差的，见图6。则定义误差项△γ_i、△β_i、△α_i其中包含了推进器存在的安装角度误差和推力矢量误差。

作业型ROV的推进***一般都包含多个推进器，则就要相应建立多个推进器坐标系。则随动坐标系到第i个推进器的坐标转换矩阵可以表示为：

$\begin{array}{c}{\mathrm{\Ω}}_O^i=R(X,{\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)R(Y,{\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)R(Z,{\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)\\ =\begin{array}{ccc}\left[\begin{array}{ccc}\cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& -\sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ \sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& \cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ 0& 0& 1\end{array}\right]\·& \left[\begin{array}{ccc}\cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& -\sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\\ 0& 1& 0\\ \sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& \cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\end{array}\right]\·& \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\\ 0& -\sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\end{array}\right]\end{array}\end{array}$

${\stackrel{\→}{p}}_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T$ 为空间任一坐标系原点在动系中的坐标值，随动坐标系按照此向量将坐标原点移动到空间任一坐标系原点，在这里为第i个推进器坐标系原点在随动坐标系中的坐标值。

则第i个推进器的推力作用在ROV上的力可以表示为：

$\begin{array}{cc}{T}_i={\mathrm{\Ω}}_i^O\·\left[\begin{array}{c}{f}_i\\ 0\\ 0\end{array}\right]& {\mathrm{\Ω}}_i^O={\left({\mathrm{\Ω}}_O^i\right)}^{-1}\end{array}$

式中f_i为推进器的实际推力大小。

推进器存在的安装位置误差对ROV所受的推理力矩产生影响。第i个推进器的位置用坐标[x_i y_i z_i]^T表示，[x_i y_i z_i]^T为推进器中心点在动坐标系中的坐标值。

如果存在位置误差，即矢径存在误差，如图5。设矢径误差为△x_i、△y_i、△z_i。则

$\begin{array}{c}{M}_i=\stackrel{\→}{r}\×T_i={\stackrel{\→}{p}}_i\×T_i=\left|\begin{array}{ccc}i& j& k\\ -(x_i+{\mathrm{\Δx}}_i)& -(y_i+{\mathrm{\Δy}}_i)& -(z_i+{\mathrm{\Δz}}_i)\\ T_{\mathrm{ix}}& T_{\mathrm{iy}}& T_{\mathrm{iz}}\end{array}\right|\\ =[T_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)]i+[T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)]j+[T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)]k\\ ={\left[\begin{array}{ccc}{T}_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)& T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)& T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)\end{array}\right]}^T\\ =\begin{array}{cc}\left[\begin{array}{ccc}0& (z_i+{\mathrm{\Δz}}_i)& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& (x_i+{\mathrm{\Δx}}_i)\\ (y_i+{\mathrm{\Δy}}_i)& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·& \left[,\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]\end{array}\end{array}$

式中 $\left[\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]=T_i,$ 为第i个推进器的推力作用在ROV上的力的三个分量。

则作业型ROV的推力模型为：

$T=\left[\begin{array}{c}\underset{i=1}{\overset{7}{\mathrm{\Σ}}}{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\\ \underset{i=1}{\overset{7}{\mathrm{\Σ}}}\left[\begin{array}{ccc}0& z_i+{\mathrm{\Δz}}_i& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& x_i+{\mathrm{\Δx}}_i\\ y_i+{\mathrm{\Δy}}_i& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·[{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\end{array}\right].$

(10)未知干扰项的建模步骤

深海作业型ROV所受外界的未知干扰项包括海流的影响、光缆的拖拽力和作业机械手的反冲力等，它们可以用有界的六自由度力/力矩△f来表示。

(11)作业型ROV最终的动力学和运动学模型步骤

根据以上的建模步骤，深海作业型ROV最终的动力学和运动学模型可以表示为：

$(M_{\mathrm{RB}}+M_A)\stackrel{\·}{v}+(C_{\mathrm{RB}}\left(v\right)+C_A\left(v\right))v+g\left({\mathrm{\η}}_2\right)=T+\mathrm{\Δf}.$

$\stackrel{\·}{\mathrm{\η}}=J\left({\mathrm{\η}}_2\right)v$

本发明目的针对现有水下机器人动力学模型和运动学模型设计的不足：模型简化和推进***建模不精确方面，提供一种深海作业型ROV动力学和运动学建模的方法，用于实现深海作业型ROV动力学和运动学建模，更适用于深海作业型ROV。发明流程示意图如图1。

本发明具体实施方式是：

(1)分析深海作业型ROV的受力情况。包括水动力、静力、推进器推力、缆力以和海流影响以及机械手的反冲力等。

(2)建立固定坐标系步骤

忽略地球表面加速度相对于低速ROV的影响,则固定坐标系可以被认为是一个惯性坐标系。原点E可选在地球上某一定点，如海面或海中任一点。Eξ轴位于水平面，并以ROV主航向为正向；Eη轴位于Eξ轴所在的水平面，按右手法则将Eξ轴顺时针旋转90°即是；Eζ轴垂直于ξEη坐标平面，指向地心为正。固定坐标系示意图如图2所示。

(3)建立随动坐标系步骤

以固定于潜水器上的右手直角坐标系O-XYZ为随体坐标系。纵轴OX平行于潜水器主体基线，指向艏部为正；横轴OY平行于基线面，指向右舷为正；垂轴OZ位于潜水器主体中纵剖面内，指向底部为正。随动坐标系示意图如图2所示。

(4)建立推进器坐标系步骤

推进器坐标系固连在ROV本体上。首先要人为设定推进器中心在ROV上的具***置，以此作为推进器坐标系的原点，然后根据需要设定推进器坐标系。推进器坐标系的纵轴O_iX_i与随动坐标系的纵轴正方向成一定的角度α，此角度大小由人为规定，根据右手定则O_iZ_i的方向向下，O_iY_i垂直于X_iO_iZ_i面。推进器进行安装时，只要将推进器的中轴线与O_iX_i轴重合，并且将推进器螺旋桨背离O_iX_i的方正向。推进器坐标系示意图如图3所示，推进器安装示意图如图4所示。

(5)计算固定坐标系到随动坐标系的坐标转移矩阵步骤

ROV相对于惯性坐标系的速度为v，v在动系上的投影为u(纵向速度)、v(横向速度)、w(垂向速度)；同理，潜器以角速度Ω在动系上的投影为p(横倾角速度)、q(纵倾角速度)、r(偏航角速度)；ROV所受外力F在动系上的投影为τ_X(纵向力)、τ_Y(横向力)、τ_Z(垂向力)；力矩M的投影为τ_K(横倾力矩)、τ_M(纵倾力矩)、τ_N(偏航力矩)。指向坐标轴的正向为速度和力的分量的正方向，用右手定则来规定角速度和力矩的正负号。

ROV的空间位置和姿态，可用动系原点的地面坐标值[x y z]^T和动系相对于定系的三个姿态角(φ,θ,ψ)来确定。艏向角φ，是主体的对称面XOZ绕铅垂轴Eζ水平旋转，与铅垂面ξEζ的夹角在定系水平面(ξEη)的投影，向右转为正；纵倾角θ是主体的水线面XOY绕OY轴俯仰与定系水平面ξEη的夹角在定系铅垂面的投影，θ向尾倾为正；横倾角ψ是主体的对称面XOZ绕OX轴横倾与定系铅垂面ξEζ的夹角在横滚面ηEζ的投影，向右倾为正。

ROV六自由度位置/姿态向量为：

$\begin{array}{ccc}\mathrm{\η}={\left[\begin{array}{cc}{\mathrm{\η}}_1^T& {\mathrm{\η}}_2^T\end{array}\right]}^T& {\mathrm{\η}}_1={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T& {\mathrm{\η}}_2={\left[\begin{array}{ccc}\mathrm{\φ}& \mathrm{\θ}& \mathrm{\ψ}\end{array}\right]}^T\end{array}$

ROV六自由度速度/角速度向量为：

$\begin{array}{ccc}v={\left[\begin{array}{cc}{v}_1^T& v_2^T\end{array}\right]}^T& v_1={\left[\begin{array}{ccc}u& v& w\end{array}\right]}^T& v_2={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T\end{array}$

ROV六自由度力/力矩向量为：

$\begin{array}{ccc}\mathrm{\τ}={\left[\begin{array}{cc}{\mathrm{\τ}}_1^T& {\mathrm{\τ}}_2^T\end{array}\right]}^T& {\mathrm{\τ}}_1={\left[\begin{array}{ccc}{\mathrm{\τ}}_X& {\mathrm{\τ}}_Y& {\mathrm{\τ}}_Z\end{array}\right]}^T& {\mathrm{\τ}}_2={\left[\begin{array}{ccc}{\mathrm{\τ}}_K& {\mathrm{\τ}}_K& {\mathrm{\τ}}_N\end{array}\right]}^T\end{array}$

描述ROV沿三轴方向直线运动的速度转换方程为

${\stackrel{\·}{\mathrm{\η}}}_1=J_1\left({\mathrm{\η}}_2\right)v_1$

式中 $J_1\left({\mathrm{\η}}_2\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]$ 为速度坐标转

换矩阵。

则ROV的速度你转换方程为

$v_1=J_1^{-1}\left({\mathrm{\η}}_2\right){\stackrel{\·}{\mathrm{\η}}}_1$

描述ROV绕三轴转动的角速度转换方程为

${\stackrel{\·}{\mathrm{\η}}}_2=J_2\left({\mathrm{\η}}_2\right)v_2$

式中为角速度坐标转换矩阵。

总的六自由度坐标转换矩阵为

$J\left({\mathrm{\η}}_2\right)=\left[\begin{array}{cc}{J}_1\left({\mathrm{\η}}_2\right)& 0\\ 0& J_2\left({\mathrm{\η}}_2\right)\end{array}\right]$

由以上结果可以得到最终的ROV运动学方程为

$\stackrel{\·}{\mathrm{\η}}=J\left({\mathrm{\η}}_2\right)v$

(6)作业型ROV质量矩阵以及其引起的柯氏力和向心力矩阵的计算步骤

a.大多数作业型ROV通常是具有近似的三个对称面外型为长方体。因为具有三个近似的对称面，所以质量矩阵的非对角元素远远小于对角元素，因此附加质量惯性矩阵中的非对角元素可以进行忽略；

b.作业型ROV通常是通过配重来决定重心和浮心的相对位置。因此ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。这样对ROV模型的简化具有很重要的意义；

c.由于作业型ROV各种设备外形复杂而且布置凌乱，不同的任务需要更换不同的作业设备，因此内部结构较复杂。作业型ROV参数矩阵中的非对角元素也很难精确获得。但事实上，在大多数应用场合，对角假设已经可以非常好地近似真实的ROV受力情况。

则作业型ROV质量矩阵：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& -I_{\mathrm{yx}}& I_y& -I_{\mathrm{yz}}\\ -{\mathrm{my}}_G& {\mathrm{mx}}_G& 0& -I_{\mathrm{zx}}& -I_{\mathrm{zy}}& I_z\end{array}\right]$

m为作业型ROV的质量；[x_G y_G z_G]^T为作业型ROV重心坐标；I_x、I_y、I_z为ROV绕X、Y、Z三轴的转动惯量，而I_xy、I_yx、I_xz、I_zx、I_yz、I_zy则为惯性积。

可以简化为：

$M_{\mathrm{RB}}=\left[\begin{array}{cccccc}m& 0& 0& 0& 0& 0\\ 0& m& 0& 0& 0& 0\\ 0& 0& m& 0& 0& 0\\ 0& 0& 0& I_x& 0& 0\\ 0& 0& 0& 0& I_y& 0\\ 0& 0& 0& 0& 0& I_z\end{array}\right]$

作业型ROV质量引起的柯氏力和向心力矩阵：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& m(y_{G}q+z_{G}r)& -m(x_{G}q-w)& -m(x_{G}r+v)\\ 0& 0& 0& -m(y_{G}p+w)& m(z_{G}r+x_{G}p)& -m(y_{G}r-u)\\ 0& 0& 0& -m(z_{G}p-v)& -m(z_{G}q+u)& m(x_{G}p+y_{G}q)\\ -m(y_{G}q+z_{G}r)& m(y_{G}p+w)& m(z_{G}p-v)& 0& -I_{\mathrm{yz}}q-I_{\mathrm{xz}}p+I_{z}r& I_{\mathrm{yz}}r+I_{\mathrm{xy}}p-I_{y}q\\ m(x_{G}q-w)& -m(z_{G}r+x_{G}p)& m(z_{G}q+u)& I_{\mathrm{yz}}q+I_{\mathrm{xz}}p-I_{z}r& 0& -I_{\mathrm{xz}}r-I_{\mathrm{xy}}q+I_{x}p\\ m(x_{G}r+v)& m(y_{G}r-u)& -m(x_{G}p+y_{G}q)& -I_{\mathrm{yz}}r-I_{\mathrm{xy}}p+I_{y}q& I_{\mathrm{xz}}r+I_{\mathrm{xy}}q-I_{x}p& 0\end{array}\right]$

可以化简为：

$C_{\mathrm{RB}}\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& \mathrm{mw}& -\mathrm{mv}\\ 0& 0& 0& -\mathrm{mw}& 0& \mathrm{mu}\\ 0& 0& 0& \mathrm{mv}& -\mathrm{mu}& 0\\ 0& \mathrm{mw}& -\mathrm{mv}& 0& I_{z}r& -I_{y}q\\ -\mathrm{mw}& 0& \mathrm{mu}& -I_{z}r& 0& I_{x}p\\ \mathrm{mv}& -\mathrm{mu}& 0& I_{y}q& -I_{x}p& 0\end{array}\right]$

(7)作业型ROV所受水动力建模步骤

1)流体惯性力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵

a.大多数作业型ROV通常是具有近似的三个对称面外型为长方体。因为具有三个近似的对称面，所以质量矩阵的非对角元素远远小于对角元素，因此附加质量惯性矩阵中的非对角元素可以进行忽略；

b.作业型ROV通常是通过配重来决定重心和浮心的相对位置。因此ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。这样对ROV模型的简化具有很重要的意义；

c.由于作业型ROV各种设备外形复杂而且布置凌乱，不同的任务需要更换不同的作业设备，因此内部结构较复杂。作业型ROV参数矩阵中的非对角元素也很难精确获得。但事实上，在大多数应用场合，对角假设已经可以非常好地近似真实的ROV受力情况。

流体惯性力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵为：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& X_{\stackrel{\·}{v}}& X_{\stackrel{\·}{w}}& X_{\stackrel{\·}{p}}& X_{\stackrel{\·}{q}}& X_{\stackrel{\·}{r}}\\ Y_{\stackrel{\·}{u}}& Y_{\stackrel{\·}{v}}& Y_{\stackrel{\·}{w}}& Y_{\stackrel{\·}{p}}& Y_{\stackrel{\·}{q}}& Y_{\stackrel{\·}{r}}\\ Z_{\stackrel{\·}{u}}& Z_{\stackrel{\·}{v}}& Z_{\stackrel{\·}{w}}& Z_{\stackrel{\·}{p}}& Z_{\stackrel{\·}{q}}& Z_{\stackrel{\·}{r}}\\ K_{\stackrel{\·}{u}}& K_{\stackrel{\·}{v}}& K_{\stackrel{\·}{w}}& K_{\stackrel{\·}{p}}& K_{\stackrel{\·}{q}}& K_{\stackrel{\·}{r}}\\ M_{\stackrel{\·}{u}}& M_{\stackrel{\·}{v}}& M_{\stackrel{\·}{w}}& M_{\stackrel{\·}{p}}& M_{\stackrel{\·}{q}}& M_{\stackrel{\·}{r}}\\ N_{\stackrel{\·}{u}}& N_{\stackrel{\·}{v}}& N_{\stackrel{\·}{w}}& N_{\stackrel{\·}{p}}& N_{\stackrel{\·}{q}}& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\begin{array}{cc}\left[\begin{array}{cccccc}0& 0& 0& 0& -a_3& a_2\\ 0& 0& 0& a_3& 0& -a_1\\ 0& 0& 0& -a_2& a_1& 0\\ 0& -a_3& a_2& 0& -b_3& b_2\\ a_3& 0& -a_1& b_3& 0& -b_1\\ -a_2& a_1& 0& -b_2& b_1& 0\end{array}\right]& \begin{array}{c}{a}_1=X_{\stackrel{\·}{u}}u+X_{\stackrel{\·}{v}}v+X_{\stackrel{\·}{w}}w+X_{\stackrel{\·}{p}}p+X_{\stackrel{\·}{q}}q+X_{\stackrel{\·}{r}}r\\ a_2=X_{\stackrel{\·}{v}}u+Y_{\stackrel{\·}{v}}v+Y_{\stackrel{\·}{w}}w+Y_{\stackrel{\·}{p}}p+Y_{\stackrel{\·}{q}}q+Y_{\stackrel{\·}{r}}r\\ a_3=X_{\stackrel{\·}{w}}u+X_{\stackrel{\·}{w}}v+Z_{\stackrel{\·}{w}}w+Z_{\stackrel{\·}{w}}p+Z_{\stackrel{\·}{q}}q+Z_{\stackrel{\·}{r}}r\\ b_1=X_{\stackrel{\·}{p}}u+Y_{\stackrel{\·}{p}}v+Z_{\stackrel{\·}{p}}w+K_{\stackrel{\·}{p}}p+K_{\stackrel{\·}{q}}q+K_{\stackrel{\·}{r}}r\\ b_2=X_{\stackrel{\·}{q}}u+Y_{\stackrel{\·}{q}}v+Z_{\stackrel{\·}{q}}w+K_{\stackrel{\·}{q}}p+M_{\stackrel{\·}{q}}q+M_{\stackrel{\·}{r}}r\\ b_3={X_{\stackrel{\·}{r}}}_{}u+Y_{\stackrel{\·}{r}}v+Z_{\stackrel{\·}{r}}w+K_{\stackrel{\·}{r}}p+M_{\stackrel{\·}{r}}q+N_{\stackrel{\·}{r}}r\end{array}\end{array}$

可以化简为：

$M_A=-\left[\begin{array}{cccccc}{X}_{\stackrel{\·}{u}}& 0& 0& 0& 0& 0\\ 0& Y_{\stackrel{\·}{v}}& 0& 0& 0& 0\\ 0& 0& Z_{\stackrel{\·}{w}}& 0& 0& 0\\ 0& 0& 0& K_{\stackrel{\·}{p}}& 0& 0\\ 0& 0& 0& 0& M_{\stackrel{\·}{q}}& 0\\ 0& 0& 0& 0& 0& N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v\\ 0& 0& 0& Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u\\ 0& 0& 0& -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0\\ 0& -Z_{\stackrel{\·}{w}}w& Y_{\stackrel{\·}{v}}v& 0& -N_{\stackrel{\·}{r}}r& M_{\stackrel{\·}{q}}q\\ Z_{\stackrel{\·}{w}}w& 0& -X_{\stackrel{\·}{u}}u& N_{\stackrel{\·}{r}}r& 0& -K_{\stackrel{\·}{p}}p\\ -Y_{\stackrel{\·}{v}}v& X_{\stackrel{\·}{u}}u& 0& -M_{\stackrel{\·}{q}}q& K_{\stackrel{\·}{p}}p& 0\end{array}\right]$

2)流体粘性力系数矩阵

ROV在运动时，受到线性和非线性的粘性类水动力。总的粘性水动力阻尼矩阵D(v)可以分解为线性水动力阻尼矩阵D_L(v)和非线性水动力阻尼矩阵D_NL(v)之和：

D(v)=D_L(v)+D_NL(v)

对于高速行进的六自由度水下运载器，其将受到高度耦合且非线性的水动力阻尼力。但对于低速运动的具有近似的三个对称面ROV，可以忽略耦合作用，并忽略高于二阶的阻尼项。则粘性水动力阻尼矩阵将为仅包括主运动的一次、二次水动力系数的对角阵，即

$D\left(v\right)=-\left[\begin{array}{cccccc}{X}_u+X_{u\left|u\right|}\left|u\right|& 0& 0& 0& 0& 0\\ 0& Y_v+Y_{v\left|v\right|}\left|v\right|& 0& 0& 0& 0\\ 0& 0& Z_w+Z_{w\left|w\right|}\left|w\right|& 0& 0& 0\\ 0& 0& 0& K_p+K_{p\left|p\right|}\left|p\right|& 0& 0\\ 0& 0& 0& 0& M_q+M_{q\left|q\right|}\left|q\right|& 0\\ 0& 0& 0& 0& 0& N_r+N_{r\left|r\right|}\left|r\right|\end{array}\right]$

式中X_u、Y_v等被称为一阶水动系数，其主符号代表运动产生的水动力/力矩的方向，下标则表示运动的方向；w|w|和v|v|等表示该项的大小与w²、v²成正比，而符号随来流方向而变。

(8)作业型ROV所受静力建模步骤

计算作业型ROV所受重力W=mg；计算作业型ROV所受浮力。ρ为海水密度，g为重力加速度，为ROV排水体积。

则静力模型为：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ (y_{G}W-y_{c}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -(x_{G}W-x_{C}B)\cos \mathrm{\θ}\cos \mathrm{\φ}-(z_{G}W-z_{C}B)\sin \mathrm{\θ}\\ (x_{G}W-x_{C}B)\cos \mathrm{\θ}\sin \mathrm{\φ}+(y_{G}W-y_{C}B)\sin \mathrm{\θ}\end{array}\right]$

式中[x_C y_C z_C]^T为ROV浮心坐标。

ROV可以将重心位置通过配重移到浮心的正下方，调节初始的纵倾和横倾角度；通过配重可以将随动坐标系原点与ROV重心重合，同时艇体坐标系与ROV的惯性主轴重合。则模型可以简化为：

$g\left({\mathrm{\η}}_2\right)=\left[\begin{array}{c}-(W-B)\sin \mathrm{\θ}\\ (W-B)\cos \mathrm{\θ}\sin \mathrm{\φ}\\ (W-B)\cos \mathrm{\θ}\cos \mathrm{\φ}\\ -y_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -x_{C}B\cos \mathrm{\θ}\cos \mathrm{\φ}+z_{C}B\sin \mathrm{\θ}\\ -x_{C}B\cos \mathrm{\θ}\sin \mathrm{\φ}-y_{C}B\sin \mathrm{\θ}\end{array}\right].$

(9)作业型ROV推力建模步骤

因为推进器坐标系是固连在ROV主体上的，所以推进器坐标系相对动坐标系是相对静止的。动坐标系到空间内任一坐标系的位姿转换矩阵为：

$T_O^i=\left[\begin{array}{c}\begin{array}{cc}{\mathrm{\Ω}}_O^i& {\stackrel{\→}{p}}_i\end{array}\\ \begin{array}{cccc}0& 0& 0& 1\end{array}\end{array}\right]$

此位姿转换矩阵的含义是：定坐标系首先按照向量 ${\stackrel{\→}{p}}_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T$ 平移，使坐标原点与空间内任一坐标系原点相重合，再根据旋转矩阵旋转坐标系就能够使随动坐标系和空间内任一坐标系相重合。

则对于人为确定好的推进器坐标系，规定逆时针旋转为正方向，将平移后坐标原点和推进器坐标系原点相重合的随动坐标系绕X、Y、Z轴分别旋转γ_i、β_i、α_i，使得随动坐标系与推进器坐标系重合。但是实际工程中推进器存在安装误差(包括角度误差和位置误差见图5)、推进器存在推力矢量误差，推力矢量误差是指：理想情况下，推进器的推力方向是沿着推进器的中轴的，而大多数情况下我们也是认为推进器的推力方向是沿着推进器的中轴的。但是实际情况中推力方向和中轴线是由偏差的，见图6。则定义误差项△γ_i、△β_i、△α_i其中包含了推进器存在的安装角度误差和推力矢量误差。

作业型ROV的推进***一般都包含多个推进器，则就要相应建立多个推进器坐标系。则随动坐标系到第i个推进器的坐标转换矩阵可以表示为：

$\begin{array}{c}{\mathrm{\Ω}}_O^i=R(X,{\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)R(Y,{\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)R(Z,{\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)\\ =\begin{array}{ccc}\left[\begin{array}{ccc}\cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& -\sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ \sin ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& \cos ({\mathrm{\α}}_i+{\mathrm{\Δ\α}}_i)& 0\\ 0& 0& 1\end{array}\right]\·& \left[\begin{array}{ccc}\cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& -\sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\\ 0& 1& 0\\ \sin ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)& 0& \cos ({\mathrm{\β}}_i+{\mathrm{\Δ\β}}_i)\end{array}\right]\·& \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\\ 0& -\sin ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)& \cos ({\mathrm{\γ}}_i+{\mathrm{\Δ\γ}}_i)\end{array}\right]\end{array}\end{array}$

${\stackrel{\→}{p}}_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T$ 为空间任一坐标系原点在动系中的坐标值，随动坐标系按照此向量将坐标原点移动到空间任一坐标系原点，在这里为第i个推进器坐标系原点在随动坐标系中的坐标值。

则第i个推进器的推力作用在ROV上的力可以表示为：

$\begin{array}{cc}{T}_i={\mathrm{\Ω}}_i^O\·\left[\begin{array}{c}{f}_i\\ 0\\ 0\end{array}\right]& {\mathrm{\Ω}}_i^O={\left({\mathrm{\Ω}}_O^i\right)}^{-1}\end{array}$

式中f_i为推进器的实际推力大小。

推进器存在的安装位置误差对ROV所受的推理力矩产生影响。第i个推进器的位置用坐标[x_i y_i z_i]^T表示，[x_i y_i z_i]^T为推进器中心点在动坐标系中的坐标值。

如果存在位置误差，即矢径存在误差，如图5。设矢径误差为△x_i、△y_i、△z_i。则

$\begin{array}{c}{M}_i=\stackrel{\→}{r}\×T_i={\stackrel{\→}{p}}_i\×T_i=\left|\begin{array}{ccc}i& j& k\\ -(x_i+{\mathrm{\Δx}}_i)& -(y_i+{\mathrm{\Δy}}_i)& -(z_i+{\mathrm{\Δz}}_i)\\ T_{\mathrm{ix}}& T_{\mathrm{iy}}& T_{\mathrm{iz}}\end{array}\right|\\ =[T_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)]i+[T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)]j+[T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)]k\\ ={\left[\begin{array}{ccc}{T}_{\mathrm{iy}}(z_i+{\mathrm{\Δz}}_i)-T_{\mathrm{iz}}(y_i+{\mathrm{\Δy}}_i)& T_{\mathrm{iz}}(x_i+{\mathrm{\Δx}}_i)-T_{\mathrm{ix}}(z_i+{\mathrm{\Δz}}_i)& T_{\mathrm{ix}}(y_i+{\mathrm{\Δy}}_i)-T_{\mathrm{iy}}(x_i+{\mathrm{\Δx}}_i)\end{array}\right]}^T\\ =\begin{array}{cc}\left[\begin{array}{ccc}0& (z_i+{\mathrm{\Δz}}_i)& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& (x_i+{\mathrm{\Δx}}_i)\\ (y_i+{\mathrm{\Δy}}_i)& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·& \left[,\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]\end{array}\end{array}$

式中 $\left[\begin{array}{c}{T}_{\mathrm{ix}}\\ T_{\mathrm{iy}}\\ T_{\mathrm{iz}}\end{array}\right]=T_i,$ 为第i个推进器的推力作用在ROV上的力的三个分量。

则作业型ROV的推力模型为：

$T=\left[\begin{array}{c}\underset{i=1}{\overset{7}{\mathrm{\Σ}}}{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\\ \underset{i=1}{\overset{7}{\mathrm{\Σ}}}\left[\begin{array}{ccc}0& z_i+{\mathrm{\Δz}}_i& -(y_i+{\mathrm{\Δy}}_i)\\ -(z_i+{\mathrm{\Δz}}_i)& 0& x_i+{\mathrm{\Δx}}_i\\ y_i+{\mathrm{\Δy}}_i& -(x_i+{\mathrm{\Δx}}_i)& 0\end{array}\right]\·[{\left({\mathrm{\Ω}}_O^i\right)}^{-1}{\left[\begin{array}{ccc}{f}_i& 0& 0\end{array}\right]}^T\end{array}\right]$

(10)未知干扰项的建模步骤

深海作业型ROV所受外界的未知干扰项包括海流的影响、光缆的拖拽力和作业机械手的反冲力等，它们可以用有界的六自由度力/力矩△f来表示。

(11)作业型ROV最终的动力学和运动学模型步骤

根据以上的建模步骤，深海作业型ROV最终的动力学和运动学模型可以表示为：

$(M_{\mathrm{RB}}+M_A)\stackrel{\·}{v}+(C_{\mathrm{RB}}\left(v\right)+C_A\left(v\right))v+g\left({\mathrm{\η}}_2\right)=T+\mathrm{\Δf}.$

$\stackrel{\·}{\mathrm{\η}}=J\left({\mathrm{\η}}_2\right)v$

Claims (8)

1.一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于： 

（1）建立定坐标系、随体坐标系和推进器坐标系,估计六自由度坐标转换矩阵； 

（2）估计作业型水下机器人质量矩阵以及引起的柯氏力和向心力矩阵； 

（3）估计作业型ROV所受水动力； 

（4）估计作业型ROV所受静力； 

（5）估计作业型ROV推力； 

（6）估计未知干扰项； 

（7）确定作业型ROV最终的动力学和运动学模型。 

2.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（1）包括： 

（1）建立定坐标系：原点E确定在一定点，Eξ轴位于水平面，并以水下机器人主航向为正向；Eη轴位于Eξ轴所在的水平面，按右手法则将Eξ轴顺时针旋转90°，即Eζ轴垂直于ξEη坐标平面，指向地心为正； 

（2）建立随体坐标系：纵轴OX平行于潜水器主体基线，指向艏部为正；横轴OY平行于基线面，指向右舷为正；垂轴OZ位于潜水器主体中纵剖面内，指向底部为正； 

（3）建立推进器坐标系：设定推进器中心的具***置，作为推进器坐标系的原点，推进器坐标系的纵轴O_iX_i与随动坐标系的纵轴正方向成角度α，根据右手定则O_iZ_i的方向向下，O_iY_i垂直于X_iO_iZ_i面； 

（4）计算六自由度坐标转换矩阵： 

其中：

φ为艏向角；θ为纵倾角；ψ为横倾角。 

3.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（2）包括： 

（1）计算作业型水下机器人的质量矩阵： 

作业型水下机器人质量引起的柯氏力和向心力矩阵： 

m为作业型ROV的质量；[x_G y_G z_G]^T为作业型ROV重心坐标；I_x、I_y、I_z为ROV绕X、Y、Z三轴的转动惯量，而I_xy、I_yx、I_xz、I_zx、I_yz、I_zy则为惯性积； 

（2）根据对称面原理和小量忽略的方法可将质量矩阵和柯氏力向心力矩阵进行化简作业型水下机器人的质量矩阵： 

作业型水下机器人质量引起的柯氏力和向心力矩阵： 

4.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（3）包括： 

（1）计算惯性类水动力系数矩阵以及流体惯性力引起的柯氏力和向心力矩阵： 

化简为： 

（2）计算流体粘性力系数矩阵 

总的粘性水动力阻尼矩阵D(v)分解为线性水动力阻尼矩阵D_L(v)和非线性水动力阻尼矩阵D_NL(v)之和： 

D(v)=D_L(v)+D_NL(v) 

。

5.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（4）包括： 

计算作业型水下机器人所受重力W=mg；计算作业型ROV所受浮力，ρ为海水密度，g为重力加速度，为ROV排水体积，则静力模型为： 

式中[x_C y_C z_C]^T为ROV浮心坐标，ROV通过配重可以将随动坐标系原点与ROV重心重合，则： 

6.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述的步骤（5）包括： 

（1）计算随动坐标系到第i个推进器坐标系的包含误差项的坐标转换矩阵

为空间任一推进器坐标系原点在动系中的坐标值； 

（2）计算推力： 

第i个推进器的推力作用在ROV上的力为： 

式中f_i为推进器的实际推力大小； 

（3）计算含有误差项的推力力矩： 

式中为第i个推进器的推力作用在ROV上的力的三个分量； 

（4）作业型ROV的推力为： 

7.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（6）：深海作业型ROV所受外界的未知干扰项包括海流的影响、光缆的拖拽力和作业机械手的反冲力，用有界的六自由度力/力矩△f表示。 

8.根据权利要求1所述的一种深海作业型水下机器人的动力学和运动学估计方法，其特征在于，所述步骤（7）作业型ROV最终的动力学和运动学模型为：