[发明专利]水下机器人多推进器耦合控制方法及系统

权利要求书

1.一种水下机器人多推进器耦合控制方法，其特征在于，所述水下机器人多推进器耦合控制方法包括以下步骤：

检测推进器状态并获得正常工作的推进器信息；

通过姿态传感器获得水下机器人的姿态数据；

通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；

将控制输出发送给推进器执行。

2.根据权利要求1所述的水下机器人多推进器耦合控制方法，其特征在于，所述姿态传感器包括电子罗盘、加速度计和陀螺仪，姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。

3.根据权利要求2所述的水下机器人多推进器耦合控制方法，其特征在于，所述通过姿态传感器获得水下机器人的姿态数据包括：姿态传感器三维角度输出值分别为航向角、俯仰角和横滚角；三维角速度输出值分别为航向角速度、俯仰角速度和横滚角速度；姿态传感器安装位置为X轴方向指向水下机器人的前进方向。

4.根据权利要求1所述的水下机器人多推进器耦合控制方法，其特征在于，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。

5.根据权利要求4所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出K_P[e(t)]、K_I[e(t)]、K_D[e(t)]，得到非线性PID控制模型如式(1.1)所示：

其中：K_P[e(t)]为非线性比例参数，K_I[e(t)]为非线性积分参数，K_D[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。

6.根据权利要求5所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：

首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程；

为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：

将式(1.2)代入(1.1)，则可得到离散的PID表达式：

上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。

7.根据权利要求4所述的水下机器人多推进器耦合控制方法，其特征在于，所述静态坐标系到运动坐标系的转换关系包括：水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向；运动坐标系变换到静止坐标系为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换；首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度；速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：

其中J₁(v_W)是线速度矢量的坐标变换关系，J₂(w_W)是角速度矢量的坐标变换关系；

线速度矢量的坐标变换关系如下：

角速度矢量的坐标变化关系如下：

六自由度下运动坐标系到静止坐标的坐标变换矩阵为：

其中C表示cos(.)，S表示sin(.)，T表示tan(.)，φ表示横滚角，θ表示俯仰角，ψ表示航向角。