[发明专利]一种水下机器人路径跟踪方法、设备及存储设备

权利要求书

1.一种水下机器人路径跟踪方法，所述方法由硬件设备实现，其特征在于：包括以下步骤：设定水下机器人期望跟踪的路径；建立水下机器人的坐标系；以所述坐标系建立水下机器人电机布局方式；建立水下机器人的控制系统模型；设定所述控制系统模型中的传递函数及电机模型；进行坐标变换，建立世界坐标系和水下机器人坐标系的联系；完成水下机器人的路径跟踪；

所述水下机器人坐标系包括：移动坐标系，具体为：为x轴、y轴和z轴；旋转坐标系，具体为：l轴、m轴和n轴；

以所述坐标系建立水下机器人电机布局方式具体为：所述电机具体数量为8台电机，记为：M1、M2、M3、M4、M5、M6、M7及M8；所述8台电机的功能具体为：M1及M4用于控制l轴的稳定，M2及M3用于控制m轴的稳定，M5及M6用于控制n轴的稳定，M7及M8用于控制x轴的稳定，M5及M6用于控制y轴的稳定，M1、M2、M3及M4用于控制z轴的稳定；上述电机布局不存在耦合关系，故所述x轴、y轴、z轴、l轴、m轴及n轴的稳定性为分开控制；

所述水下机器人的控制系统模型包括：控制器、电机、水下机器人本体、运动干扰及测量误差；所述设定水下机器人期望跟踪的路径还包括：将路径转化成离散点；

所述完成水下机器人的路径跟踪控制具体步骤包括：向水下机器人发送一个离散点；水下机器人接受到所述一个离散点后，向该离散点移动；设定水下机器人与离散点的距离阈值；判断水下机器人与离散点的距离是否小于所述距离阈值；若不小于，则水下机器人继续接近离散点，并返回判断距离阈值的步骤继续执行；若小于，则判断离散点是否为最后一离散点；若是，则完成路径跟踪；若不是，则向水下机器人再发送一个新离散点；水下机器人向新离散点接近并回到对距离阈值进行判断的步骤继续执行；

所述传递函数包括：水下机器人路径跟踪传递函数及控制器传递函数；所述水下机器人路径跟踪传递函数为：

其中，M为水下机器人的质量，k′为水下机器人在水中执行平动的阻尼系数，T′(t)为水下机器人在水中受到的外力；所述控制器传递函数为：

时域输出表达式为：

其中，K_p为比例系数，K_I为积分系数，K_D为微分系数，所述电机模型具体为：直流电机占空比-输出拉力模型及直流电机占空比-输出推力模型；所述电机模型采用回归拟合方式得到，基本公式为：

其中，A,B及C为拟合参数；

所述进行坐标变换，建立世界坐标系和水下机器人坐标系的联系具体为：

其中，xp及yp为机器人坐标系，x及y为世界坐标系，(a，b)为机器人坐标系原点在世界坐标系中的坐标，θ为旋转角。

2.如权利要求1所述的一种水下机器人路径跟踪方法，其特征在于：所述直流电机占空比-输出拉力模型经过拟合具体为：

所述直流电机占空比-输出推力模型具体为：

3.存储设备，其特征包括：所述存储设备存储指令及数据用于实现权利要求1～2所述的任意一种方法。

4.一种水下机器人路径跟踪设备，其特征在于：包括：处理器及权利要求3中所述存储设备；所述处理器加载并执行权利要求3中所述存储设备中的指令及数据用于实现权利要求1～2所述的任意一种方法。