无人船 | 图解推导三自由度USV的运动学和动力学建模

1 参考坐标系定义

USV的数学模型分为运动学和动力学两个部分，其中运动学描述了USV作为质点的几何运动规律；动力学描述了USV在来自螺旋桨、水扰、风扰等力作用下产生的运动规律。为了更好地表达USV的运动学和动力学模型，定义

• 全局坐标系$O_E-X_E{Y}_E{Z}_E$：用于描述USV的位置和姿态。全局坐标系以地球表面某一点为原点，$O_E{X}_E$指向地理正北方向，$O_E{Y}_E$指向地理正东方向，$O_E{Z}_E$指向地心，其中$O_E{Z}_E$是$O_E{X}_E$轴和$O_E{Y}_E$轴构成平面的法线，且平面$O_E{X}_E{Y}_E$与地球表面相切

• 船体坐标系$O_B-X_B{Y}_B{Z}_B$：用于描述USV受力后位置及姿态的瞬时变化，即线速度或角速度。船体坐标系一般以USV船体重心为原点，$O_B{X}_B$平行于海平面并指向船艏方向，$O_B{Y}_B$平行于海平面并指向船右舷方向，$O_B{Z}_B$轴指向地心并与水平面垂直

两个坐标系间的关系如图所示

在水面环境下，USV受到多种方向上的外力，需要用多个自由度去描述其复杂的运动状态。根据造船与轮机工程师学会(SNAME)的有关标准，使用如表所示的定义描述船舶在纵荡、横荡、垂荡、横摇、纵摇、艏摇六个自由度上的运动状态。具体而言，坐标$\left(x,y,z,\varphi ,\theta ,\psi \right)$表示船舶在全局坐标系下的位置和姿态；其一阶导数坐标$\left(u,v,w,p,q,r\right)$表示船舶在船体坐标系下的平移线速度和转动角速度

2 运动学建模

根据上表定义的USV完整六自由度运动学模型为

$$\{\begin{array}{l}\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]=\left[\begin{array}{ccc}\cos \theta \cos \psi & \sin \phi \sin \theta \cos \psi -\cos \phi \sin \psi & \cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi \\ \cos \theta \sin \psi & \sin \phi \sin \theta \sin \psi +\cos \phi \cos \psi & \cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi \\ -\sin \theta & \sin \phi \cos \theta & \cos \phi \cos \theta \end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0& \cos \phi & -\sin \phi \\ 0& \sin \phi /\cos \theta & \cos \phi /\cos \theta \end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}$$

USV在水中虽然存在六个自由度上的运动，但是上述完整的运动学模型过于复杂，对运动分析和控制器设计造成困难。考虑到实际应用中USV在垂荡、纵摇及横摇三个自由度上的运动幅度相对很小，一般忽略这三个自由度上的运动状态，即令

$$w=p=q=z=\phi =\theta =0$$

得到简化的三自由度USV运动学模型

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ r\end{array}\right]\Rightarrow \mathbf{\eta }=\mathbf{Rv}$$

3 动力学建模

Fossen建立了统一的船舶动力学模型，将USV的受力分解为刚体力、流体动力、推进力和干扰力

$$\mathbf{M}\dot{\mathbf{v}}+\mathbf{C}\left(\mathbf{v}\right)\mathbf{v}+\mathbf{D}\left(\mathbf{v}\right)\mathbf{v}+\mathbf{g}\left(\mathbf{\eta }\right)=\mathbf{\tau }+{\mathbf{\tau }}_E$$

上式中$\mathbf{M}$为船体系统的惯性矩阵，由刚体惯性矩阵${\mathbf{M}}_{RB}$和附加质量矩阵${\mathbf{M}}_A$组成

$$\mathbf{M}={\mathbf{M}}_{RB}+{\mathbf{M}}_A=\left[\begin{array}{ccc}{m}_{11}& & \\ & m_{22}& m_{23}\\ & m_{32}& m_{33}\end{array}\right]$$

其中船舶质量为$m$，$\left(x_g,y_g,z_g\right)$是船舶重心在船体坐标系下的坐标，$I_z$是绕$O_B{Z}_B$转动惯量

$${\mathbf{M}}_{RB}=\left[\begin{array}{ccc}m& & -m{y}_g\\ & m& m{x}_g\\ -m{y}_g& m{x}_g& I_z\end{array}\right],{\mathbf{M}}_A=-\left[\begin{array}{ccc}{X}_{\dot{u}}& & X_{\dot{r}}\\ & Y_{\dot{v}}& Y_{\dot{r}}\\ N_{\dot{u}}& N_{\dot{v}}& N_{\dot{r}}\end{array}\right]$$

$\mathbf{C}\left(\mathbf{v}\right)$为科氏力-向心力矩阵，由刚体科氏力-向心力矩阵${\mathbf{C}}_{RB}\left(\mathbf{v}\right)$和附加科氏力-向心力矩阵${\mathbf{C}}_A\left(\mathbf{v}\right)$组成

$$\mathbf{C}\left(\mathbf{v}\right)={\mathbf{C}}_{RB}\left(\mathbf{v}\right)+{\mathbf{C}}_A\left(\mathbf{v}\right)=\left[\begin{array}{ccc}0& 0& c_{13}\\ 0& 0& c_{23}\\ c_{31}& c_{32}& 0\end{array}\right]$$

其中

$$\begin{gathered} {\mathbf{C}}_{RB}\left(\mathbf{v}\right)=\left[\begin{array}{ccc}0& 0& -m\left(x_{g}r+v\right)\\ 0& 0& -m\left(y_{g}r-u\right)\\ m\left(x_{g}r+v\right)& m\left(y_{g}r-u\right)& 0\end{array}\right] \\ {\mathbf{C}}_A\left(\mathbf{v}\right)=\left[\begin{array}{ccc}0& 0& Y_{\dot{v}}v+Y_{\dot{r}}r\\ 0& 0& -X_{\dot{u}}u\\ -Y_{\dot{v}}v-Y_{\dot{r}}r& X_{\dot{u}}u& 0\end{array}\right] \end{gathered}$$

$\mathbf{D}\left(\mathbf{v}\right)$为阻尼矩阵，由线性阻尼矩阵$\mathbf{D}$和非线性阻尼矩阵${\mathbf{D}}_n\left(\mathbf{v}\right)$组成

$$\mathbf{D}\left(\mathbf{v}\right)=\mathbf{D}+{\mathbf{D}}_n\left(\mathbf{v}\right)=\left[\begin{array}{ccc}{d}_{11}& & \\ & d_{22}& d_{23}\\ & d_{32}& d_{33}\end{array}\right]$$

其中

$$\mathbf{D}=-\left[\begin{array}{ccc}{X}_u& & \\ & Y_v& Y_r\\ & N_v& N_r\end{array}\right],{\mathbf{D}}_n\left(\mathbf{v}\right)=-\left[\begin{array}{ccc}{X}_{|u|u}\left|u\right|& & \\ & Y_{|v|v}\left|v\right|+Y_{|r|v}\left|r\right|& Y_{|v|r}\left|v\right|+Y_{|r|r}\left|r\right|\\ & N_{|v|v}\left|v\right|+N_{|r|v}\left|r\right|& N_{|v|r}\left|v\right|+N_{|r|r}\left|r\right|\end{array}\right]$$

$\mathbf{g}\left(\mathbf{\eta }\right)$是由重力和浮力产生的恢复力和力矩。控制力$\mathbf{\tau }={\left[\begin{array}{ccc}{\tau }_u& {\tau }_v& {\tau }_r\end{array}\right]}^T$，其中${\tau }_u$、${\tau }_v$、${\tau }_r$分别表示由螺旋桨产生的前进力、横漂力和艏摇力矩

上面涉及到的水动力系数的含义如下所示

至此，USV的运动学和动力学建模完成，本专栏的后续章节都将以本节推导的模型展开规划、控制算法设计

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …