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

说明书

本发明公开了一种水下机器人多推进器耦合控制方法，包括以下步骤：检测推进器状态并获得正常工作的推进器信息；通过姿态传感器获得水下机器人的姿态数据；通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；将控制输出发送给推进器执行；还公开了一种水下机器人多推进器耦合控制系统，可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。

技术领域

本发明涉及水下机器人控制技术领域，尤其涉及一种搭载不少于六个推进器的水下机器人多推进器耦合控制方法及系统。

背景技术

水下机器人无论是在军事，还是在人类生产生活中都有广泛应用，而对其良好的操作方法和运动控制是水下机器人顺利完成任务的重要前提和保障。然而，由于水下机器人在水中运动过程中，复杂多变的水域环境很可能会使水下机器人的某一个推进器处于非正常工作状态。目前水下机器人的多自由度姿态控制多采用多推进器解耦控制的方式来实现，如果某一个推进器出现异常工作，水下机器人姿态控制将会很差，甚至出现失控的情况。传统水下机器人对推进器的异常处理基本只有停机处理。

发明内容

鉴于目前存在的上述不足，本发明提供一种水下机器人多推进器耦合控制方法及系统，可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。

为达到上述目的，本发明的实施例采用如下技术方案：

一种水下机器人多推进器耦合控制方法，所述水下机器人多推进器耦合控制方法包括以下步骤：

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

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

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

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

依照本发明的一个方面，所述姿态传感器包括电子罗盘、加速度计和陀螺仪，姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。

依照本发明的一个方面，所述通过姿态传感器获得水下机器人的姿态数据包括：姿态传感器三维角度输出值分别为航向角、俯仰角和横滚角；三维角速度输出值分别为航向角速度、俯仰角速度和横滚角速度；姿态传感器安装位置为X轴方向指向水下机器人的前进方向。

依照本发明的一个方面，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。

依照本发明的一个方面，所述非线性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控制器的输出。

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

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

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