技术-学术-VRX无人艇项目速度控制PID

在之前的博文中，介绍了VRX项目的安装，成功地在虚拟机上跑通了该项目历程。这篇博文主要记录对于无人艇的速度控制的一些公式推导以及仿真过程。
算法是在Matlab-Simulink上编程的，就是一个简单的PID控制器，通过ROS系统连通到Gazebo仿真器进行的3D仿真。内容包括以下：

1. 无人艇的动力学和运动学建模的公式推导；
2. 模型参数在对应项目中的程序代码；
3. 一些思考。

一. 无人艇的动力学和运动学建模公式

参考文献：
[1] Do K D. Practical formation control of multiple underactuated ships with limited sensing ranges[J]. Robotics and Autonomous Systems, 2011, 59(6): 457-471.
[2] Sarda E I, Qu H, Bertaska I R, et al. Station-keeping control of an unmanned surface vehicle exposed to current and wind disturbances[J]. Ocean Engineering, 2016, 127: 305-324.

在[1]中，考虑了无人艇的质心到重心不重合的情况，因此动力学建模相对复杂，在研究横向速度$v$对无人艇运动的影响时会考虑这种，在[2]中则相对简单，认为质心与重心重合，这里我们主要采用[2]中的模型，但会从[1]的模型简化到[2]并指出哪些项目是被简化了。

1.1 运动学&动力学方程-复杂版本

仅考虑$xoy$二维平面上的运动，因此该无人艇是一个三自由度模型。
$$\begin{array}{rl}& \dot{\eta }=J(\eta )v\\ & M\dot{v}+C(v)v+D(v)v=\tau +{\tau }_w\end{array}$$
其中：

1. $\vec{v}=[u,v,r{]}^{\top }$为速度向量，$\eta =[x,y,\psi {]}^{\top }$为状态向量；
2. $\tau$为推力和力矩向量，由无人艇推进器(螺旋桨)产生；${\tau }_w$为风扰力矩向量，无风速仪时，一般无法具体获取(仿真建模在1.2给出)；
3. $J$为非线性齐次矩阵，表征无人艇位置变化率与速度关系；
4. $M$为惯性矩阵，包含无人艇自身质量带来的惯性以及水的黏力带来的附加质量惯性；
5. $C$为科氏力矩阵；
6. $D$为阻尼矩阵，表征水对船的阻力。

以上矩阵具体定义如下：
$$\begin{array}{rl}& J=\left[\begin{array}{ccc}\cos (\psi )& -\sin (\psi )& 0\\ \sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]\\ & M=\left[\begin{array}{ccc}m-X_{\dot{u}}& 0& 0\\ 0& m-Y_{\dot{v}}& m{x}_g-Y_{\dot{r}}\\ 0& m{x}_g-Y_{\dot{r}}& I_z-N_{\dot{r}}\end{array}\right]\\ & C(\vec{v})=\left[\begin{array}{ccc}0& 0& -(m-Y_{\dot{v}})v-(m{x}_g-Y_{\dot{r}})\\ 0& 0& (m-X_{\dot{u}})u\\ -(m-Y_{\dot{v}})v-(m{x}_g-Y_{\dot{r}})& -(m-X_{\dot{u}})u& 0\end{array}\right]\\ & D(\vec{v})=-\left[\begin{array}{ccc}{X}_u+X_{|u|u}|u|& 0& 0\\ 0& Y_v+Y_{|v|v}|v|& -(Y_r+Y_{|v|r}|v|+Y_{|r|r}|r|)\\ 0& -(N_v+N_{|v|v}|v|+N_{|r|v}|r|)& N_r+N_{|r|r}|r|\end{array}\right]\end{array}$$

1.2 运动学&动力学方程-简化版本

简化版本的区别主要在于动力学方程中的各个矩阵，整体的模型还是一样的。
$$\begin{array}{rl}& \dot{\eta }=J(\eta )v\\ & M\dot{v}+C(v)v+D(v)v=\tau +{\tau }_w\end{array}$$
以上矩阵具体定义如下：
$$\begin{array}{rl}& J=\left[\begin{array}{ccc}\cos (\psi )& -\sin (\psi )& 0\\ \sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]\\ & M=\left[\begin{array}{ccc}m-X_{\dot{u}}& 0& 0\\ 0& m-Y_{\dot{v}}& 0\\ 0& 0& I_z-N_{\dot{r}}\end{array}\right]\\ & C(\vec{v})=\left[\begin{array}{ccc}0& 0& -(m-Y_{\dot{v}})v\\ 0& 0& (m-X_{\dot{u}})u\\ (m-Y_{\dot{v}})v& -(m-X_{\dot{u}})u& 0\end{array}\right]\\ & D(\vec{v})=\left[\begin{array}{ccc}{X}_u+X_{|u|u}|u|& 0& 0\\ 0& Y_v+Y_{|v|v}|v|& 0\\ 0& 0& N_r+N_{|r|r}|r|\end{array}\right]\end{array}$$

1.3 对比

对比二者的$M$矩阵，可以看到，简化版本中质心到重心的距离$x_g=0$，且认为$Y_{\dot{r}}=0$(该参数实际也比较小)，因此简化了非对角项；
对比$C$矩阵，此处简化与上面的对于$M$的简化相同；
对比$D$矩阵，此处简化了以下项目：
$$Y_{|r|v},Y_{|v|r},Y_{|r|r},Y_r,N_v,N_{|v|v},N_{|r|v}$$
这些参数都是侧向速度$v$和角速度$r$的耦合参数项。[1]的研究主要就是针对侧向位移的，因此附带了这些量，而对于一般的无人艇，考虑简单版本的建模就足以设计良好的控制器了。

1.4 推力和力矩建模

一般学术论文中只关心如何产生期望力矩$\tau$，将这个表达式得出来，然后做稳定性分析再给出仿真结果就形成了一篇学术论文。而在实际工程中，还需要关心这个力矩是如何产生的。该节主要参考[2]

1.4.1 油门到推力模型

通过左右桨叶的推力，无人艇将产生力和力矩作用。桨叶的转速则由电机决定。电机控制一般可以简化为对于占空比/油门的控制，因此推力$T$和占空比$x$的关系可以由函数
$f:x\mapsto T$表示，定义域为$[-1,1]\mapsto [T_{min},T_{max}]$.
参考[2]给出的曲线

得到$f$的表达式：
$$T=A+\frac{K-A}{(C+\exp (-B(x-M)){)}^{1/v}}$$
当$x>0$时，$[A,K,B,v,C,M]=[-\mathrm{0.0159.825.00.380.560.28}]$；当$x<0$时$[A,K,B,v,C,M]=[-199.13-\mathrm{0.098.845.340.99}-0.57]$。

$f$是一一映射的，因此通过上式可以得到反函数$f^{-1}$，即通过推力$T$得到$x$：
$$x=-\frac{\ln [(\frac{K-A}{T-A}{)}^v-C]}{B}+M$$

1.4.2 力矩到推力模型

以上得到左右推进器产生的推力$T_p$和$T_s$，那么力矩$\tau$如何由$T_p$和$T_s$合成呢？参考[2]的式(12)~(15)，下图是推力向量示意图(注意wamv无人艇的推进器的方向是可变的，下图展示的是45°方向，我们只考虑0°方向就好了)

$\tau =[T_x,T_y,M_z{]}^{\top }$，其中：
$$\begin{array}{rl}& T_x=(T_p+T_s)\cdot i\\ & T_y=(T_p+T_s)\cdot j\\ & M_z=r_p\times T_p+r_s\times T_s\end{array}$$
以上出现的变量：
$$\begin{array}{rl}& i=[\cos (\psi ),\sin (\psi ),0{]}^{\top }\\ & j=[-\sin (\psi ),\cos (\psi ),0{]}^{\top }\\ & T_p=|T_p|\cdot i;\\ & T_s=|T_s|\cdot i;\\ & r_p=-LCG\cdot i-\frac{B}{2}\cdot j;\\ & r_s=-LCG\cdot i+\frac{B}{2}\cdot j;\end{array}$$
需要注意的是这里的$T_p,T_s$是向量，方向就是推进器产生的力的方向；而$T_x,T_y,M_z$是标量，这里重点说一下转矩$M_z$，由于本人大学物理没好好学，一直没弄懂转动力学。
显然$M_z=r_p\times T_p+r_s\times T_s$，是向量叉乘，产生的是垂直于$xoy$平面的向量$\vec{r}$，称为转矩，而这里的$M_z$其实是转矩$\vec{r}$的模$|\vec{r}|$，这里比较让人疑惑，因为如果$M_z$是三维向量的话，那么矩阵运算的维度就不对了。转矩在转动力学中可以类比于力，公式为：
$$\vec{r}=J\dot{\omega }$$
$J$为转动惯量，$\omega$为转速，转矩$\vec{r}$的指向可以理解为转动轴，如下图，力$F_1$作用于一段为固定旋转关节的木块上，产生了力矩$\vec{r}$，产生的旋转方向如图。

在无人艇中，旋转关节可以视为无人艇的重心，单个推进器的推力因此可以建模。

1.4.3 风力和力矩模型

在考虑风扰的时候，需要对风力矩进行建模，这里参考[2]中的式(31):
$$\begin{array}{rl}& {\tau }_w=\left[\begin{array}{c}{C}_x\cdot V_{Rx}\cdot |V_{Rx}|\\ C_y\cdot V_{Ry}\cdot |V_{Ry}|\\ -2\cdot C_n\cdot V_{Rx}\cdot V_{Ry}\end{array}\right]\\ & [C_x,C_y,C_n]=[0.5,0.5,0.33]\\ & V_{wind}=|V_{wind}|\cdot [\cos ({\theta }_{wind}),\sin ({\theta }_{wind}){]}^{\top }\\ & V_{Rx}=V_{wind}^{\top }\cdot [\cos (\psi ),\sin (\psi ){]}^{\top }\\ & V_{Ry}=V_{wind}^{\top }\cdot [-\sin (\psi ),\cos (\psi ){]}^{\top }\end{array}$$

二. 在项目文件中查询参数

这里的动力学参数分散在项目中的各个地方，这里一一指出：

1. 在~/vrx_ws/src/vrx/wamv_gazebo/urdf/dynamics/wamv_gazebo_dynamics_plugin.xacro中，存有水动力系数：

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="usv_dynamics_gazebo" params="name width:=2.4 length:=4.9">
    <!--Gazebo Plugin for simulating WAM-V dynamics-->
    <gazebo>
      <plugin name="usv_dynamics_${name}" filename="libusv_gazebo_dynamics_plugin.so">
        <bodyName>${namespace}/base_link</bodyName>
        <!-- Must be same as the ocean model!-->
        <waterLevel>0</waterLevel>
        <waterDensity>997.8</waterDensity>
        <!-- Added mass -->
        <xDotU>0.0</xDotU>
        <yDotV>0.0</yDotV>
        <nDotR>0.0</nDotR>
        <!-- Linear and quadratic drag -->
        <xU>51.3</xU>
        <xUU>72.4</xUU>
        <yV>40.0</yV>
        <yVV>0.0</yVV>
        <zW>500.0</zW>
        <kP>50.0</kP>
        <mQ>50.0</mQ>
        <nR>400.0</nR>
        <nRR>0.0</nRR>
        <!-- General dimensions -->
        <!--<boatArea>2.2</boatArea>-->
        <hullRadius>0.213</hullRadius>
        <boatWidth>${width}</boatWidth>
        <boatLength>${length}</boatLength>
        <!-- Length discretization, AKA, "N" -->
        <length_n>2</length_n>
        <!-- Wave model -->
        <wave_model>ocean_waves</wave_model>
      </plugin>
    </gazebo>
  </xacro:macro>
</robot>

2. 在~/vrx_ws/src/vrx/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro中，存有推进器的力建模

<?xml version="1.0"?>
<plugin xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro  name="wamv_gazebo_thruster_config" params="name enable_angle:=true">
    <thruster>
      <!-- Required Parameters -->
      <linkName>${namespace}/${name}_propeller_link</linkName>
      <propJointName>${namespace}/${name}_engine_propeller_joint</propJointName>
      <engineJointName>${namespace}/${name}_chasis_engine_joint</engineJointName>
      <cmdTopic>${thruster_namespace}${name}_thrust_cmd</cmdTopic>
      <angleTopic>${thruster_namespace}${name}_thrust_angle</angleTopic>
      <enableAngle>${enable_angle}</enableAngle>

      <!-- Optional Parameters -->
      <mappingType>1</mappingType>
      <maxCmd>1.0</maxCmd>
      <maxForceFwd>250.0</maxForceFwd>
      <maxForceRev>-100.0</maxForceRev>
      <maxAngle>${pi/2}</maxAngle>
    </thruster>
  </xacro:macro>
</plugin>

其实这些参数并不足以模拟出理想的动力学模型，必要的时候还是需要自行调整。

三. PID速度控制实验

实验目的是设计PID控制参数，使得Gazebo中的无人艇能够以期望速度运行：
这里给出实验文件模型，模型是用simulink搭的，通过ROS对无人艇的桨叶发布控制命令，范围是$[-1,1]$。以下为其仿真图。