[发明专利]一种水下机器人移动式对接回收的路径规划方法及装置

权利要求书

1.一种水下机器人移动式对接回收的路径规划方法，其特征在于，包括：

获取待回收水下机器人的第一定位信息、移动坞站的第二定位信息以及所述待回收水下机器人的水动力参数；

所述第一定位信息包括所述待回收水下机器人的重心，根据所述第一定位信息和所述水动力参数，建立所述待回收水下机器人的空间六自由度运动模型，包括：根据所述水动力参数,确定六个自由度上的静水力和动水力的合外力；将所述水动力参数和所述合外力代入所述六自由度运动模型等式中，以所述重心为原点，建立所述空间六自由度运动模型；

根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力,其中，所述位置引力点用于划分距离范围，所述姿态引力点用于划分角度范围；

根据所述虚拟引力，确定所述待回收水下机器人的推力、垂直舵力以及水平舵力；

将所述推力、所述垂直舵力以及所述水平舵力带入所述空间六自由度运动模型，确定所述待回收水下机器人的下一时刻位姿，并获取所述移动坞站的下一时刻位姿；

判断所述待回收水下机器人的下一时刻位姿是否与所述移动坞站的下一时刻位姿达到一致，若达到，则结束路径规划任务。

2.如权利要求1所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述虚拟引力包括主动力和主动力矩，所述根据所述第二定位信息设置位置引力点和姿态引力点，根据所述位置引力点和所述姿态引力点确定虚拟引力，包括：

根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定所述主动力，其中，所述主动力包括纵向引力、横向引力和垂向引力；

根据所述第二定位信息设置所述姿态引力点，再根据所述姿态引力点确定所述主动力矩，其中，所述主动力矩包括艏向引力矩、俯仰引力矩。

3.如权利要求2所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述位置引力点包括距离控制点，所述根据所述第二定位信息设置所述位置引力点，再根据所述位置引力点确定所述主动力，包括：

根据所述第二定位信息，通过坐标转换公式得到所述待回收水下机器人和所述坞站在随体坐标系下的相对矢量距离和相对标量距离；

根据所述第二定位信息，设置所述距离控制点、最大限制吸引力和第一常数；

根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定所述纵向引力、所述横向引力以及所述垂向引力。

4.如权利要求3所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述根据所述距离控制点、所述最大限制吸引力、所述第一常数、所述相对矢量距离和所述相对标量距离，确定纵向引力、横向引力以及垂向引力，包括：

根据所述距离控制点、所述相对标量距离和第一常数，确定第一权重；

判断所述距离控制点和所述相对标量距离是否满足第一预设条件；

若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力；

根据所述总矢量引力确定所述纵向引力、所述横向引力和所述垂向引力，其中，所述总矢量引力包括沿Y轴方向的第一引力、沿X轴方向的第二引力和沿Z轴方向的第三引力，所述纵向引力为所述第一引力，所述横向引力为所述第二引力，所述垂向引力为所述第三引力。

5.如权利要求4所述的水下机器人移动式对接回收的路径规划方法，其特征在于，所述若满足所述第一预设条件，则根据所述最大限制吸引力、所述第一权重、所述相对矢量距离确定总矢量引力，包括：

当所述相对标量距离大于所述距离控制点，则根据所述最大限制吸引力和所述相对矢量距离的乘积确定所述总矢量引力；

当所述相对标量距离小于或等于所述距离控制点，则根据所述最大限制吸引力、所述相对矢量距离和所述第一权重的乘积确定所述总矢量引力。