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

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

引用

CSDN

1.

https://blog.csdn.net/FRIGIDWINTER/article/details/144957766

无人船（USV）的运动学和动力学建模是实现其自主控制的基础。本文将详细介绍三自由度USV的运动学和动力学模型，包括参考坐标系的定义、运动学方程的推导以及动力学方程的各个组成部分。

1 参考坐标系定义

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

• 全局坐标系O_E−X_EY_EZ_E：用于描述USV的位置和姿态。全局坐标系以地球表面某一点为原点，O_EX_E指向地理正北方向，O_EY_E指向地理正东方向，O_EZ_E指向地心，其中O_EZ_E是O_EX_E轴和O_EY_E轴构成平面的法线，且平面O_EX_EY_E与地球表面相切。

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

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

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

2 运动学建模

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

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

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

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

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

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

3 动力学建模

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

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

上式中M为船体系统的惯性矩阵，由刚体惯性矩阵M_RB和附加质量矩阵M_A组成：

$$
\boldsymbol{M}=\boldsymbol{M}{RB}+\boldsymbol{M}A=\left[ \begin{matrix}
m{11}& & \
& m{22}& m_{23}\
& m_{32}& m_{33}\\end{matrix} \right]
$$

其中船舶质量为m，(x_g,y_g,z_g)是船舶重心在船体坐标系下的坐标，I_z是绕O_BZ_B转动惯量：

$$
\boldsymbol{M}{RB}=\left[ \begin{matrix}
m& & -my_g\
& m& mx_g\
-my_g& mx_g& I_z\\end{matrix} \right] , \boldsymbol{M}A=-\left[ \begin{matrix}
X{\dot{u}}& & X{\dot{r}}\
& Y_{\dot{v}}& Y_{\dot{r}}\
N_{\dot{u}}& N_{\dot{v}}& N_{\dot{r}}\\end{matrix} \right]
$$

C(v)为科氏力-向心力矩阵，由刚体科氏力-向心力矩阵C_RB(v)和附加科氏力-向心力矩阵C_A(v)组成：

$$
\boldsymbol{C}\left( \boldsymbol{v} \right) =\boldsymbol{C}{RB}\left( \boldsymbol{v} \right) +\boldsymbol{C}A\left( \boldsymbol{v} \right) =\left[ \begin{matrix}
0& 0& c{13}\
0& 0& c{23}\
c_{31}& c_{32}& 0\\end{matrix} \right]
$$

其中：

$$
\boldsymbol{C}_{RB}\left( \boldsymbol{v} \right) =\left[ \begin{matrix}
0& 0& -m\left( x_gr+v \right)\
0& 0& -m\left( y_gr-u \right)\
m\left( x_gr+v \right)& m\left( y_gr-u \right)& 0\\end{matrix} \right]
$$

$$
\boldsymbol{C}A\left( \boldsymbol{v} \right) =\left[ \begin{matrix}
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{matrix} \right]
$$

D(v)为阻尼矩阵，由线性阻尼矩阵D和非线性阻尼矩阵D_n(v)组成：

$$
\boldsymbol{D}\left( \boldsymbol{v} \right) =\boldsymbol{D}+\boldsymbol{D}n\left( \boldsymbol{v} \right) =\left[ \begin{matrix}
d{11}& & \
& d_{22}& d_{23}\
& d_{32}& d_{33}\\end{matrix} \right]
$$

其中：

$$
\boldsymbol{D}=-\left[ \begin{matrix}
X_u& & \
& Y_v& Y_r\
& N_v& N_r\\end{matrix} \right] , \boldsymbol{D}n\left( \boldsymbol{v} \right) =-\left[ \begin{matrix}
X{\left| u \right|u}\left| u \right|& & \
& Y_{\left| v \right|v}\left| v \right|+Y_{\left| r \right|v}\left| r \right|& Y_{\left| v \right|r}\left| v \right|+Y_{\left| r \right|r}\left| r \right|\
& N_{\left| v \right|v}\left| v \right|+N_{\left| r \right|v}\left| r \right|& N_{\left| v \right|r}\left| v \right|+N_{\left| r \right|r}\left| r \right|\\end{matrix} \right]
$$

g(η)是由重力和浮力产生的恢复力和力矩。控制力τ = [τ_u τ_v τ_r]^T，其中τ_u、τ_v、τ_r分别表示由螺旋桨产生的前进力、横漂力和艏摇力矩。

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

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