[发明专利]水下巡检机器人导航方法、装置、系统及存储介质

权利要求书

1.一种水下巡检机器人导航方法，应用于水下养殖设备，所述水下养殖设备包括支撑桁架和养殖网衣，所述养殖网衣固定于所述支撑桁架上，其特征在于，所述水下巡检机器人导航方法包括以下步骤：

在水下巡检机器人对所述养殖网衣进行巡检的过程中，通过所述水下巡检机器人搭载的定位装置持续获取所述水下巡检机器人的观测航姿信息，并持续获取所述养殖网衣的运动信息；

基于所述观测航姿信息和所述运动信息，通过全局路径规划算法得出所述水下巡检机器人的期望航姿信息；

基于所述观测航姿信息与所述期望航姿信息的偏差，通过卡尔曼滤波算法框架对所述水下巡检机器人的航行参数进行校正。

2.根据权利要求1所述的水下巡检机器人导航方法，其特征在于，所述支撑桁架包括多个边立柱，多个所述边立柱环绕形成环形结构，所述边立柱上设置有竖直方向的固定水尺；所述通过所述水下巡检机器人搭载的定位装置持续获取所述水下巡检机器人的观测航姿信息的步骤，包括：

基于所述水下养殖设备建立非惯性坐标系；其中，所述非惯性坐标系的坐标原点设于所述固定水尺的零点位置，所述非惯性坐标系的第一轴始终指向所述环形结构的水平切线方向，所述非惯性坐标系的第二轴始终指向所述环形结构的中心位置，所述非惯性坐标系的第三轴始终指向所述边立柱的竖直方向；

基于所述非惯性坐标系，通过所述定位装置确定所述水下巡检机器人经过的所述养殖网衣上的网孔数量，以确定所述水下巡检机器人的第一轴向观测坐标、第三轴向观测坐标和观测艏向角，并通过测距装置确定所述水下巡检机器人的第二轴向观测坐标；

以所述第一轴向观测坐标、所述第二轴向观测坐标、所述第三轴向观测坐标和所述观测艏向角作为所述观测航姿信息。

3.根据权利要求2所述的水下巡检机器人导航方法，其特征在于，所述基于所述观测航姿信息和所述运动信息，通过全局路径规划算法得出所述水下巡检机器人的期望航姿信息的步骤，包括：

基于所述观测航姿信息和所述运动信息，将通过全局规划路径算法获得的规划路径映射到所述非惯性坐标系下，得到所述水下巡检机器人在所述非惯性坐标系下的第一轴向期望坐标、第二轴向期望坐标、第三轴向期望坐标和期望艏向角；

以所述第一轴向期望坐标、所述第二轴向期望坐标、所述第三轴向期望坐标和所述期望艏向角作为所述期望航姿信息。

4.根据权利要求3所述的水下巡检机器人导航方法，其特征在于，所述基于所述观测航姿信息与所述期望航姿信息的偏差，通过卡尔曼滤波算法框架对所述水下巡检机器人的航行参数进行校正的步骤，包括：

将所述观测航姿信息、所述期望航姿信息分别解耦至第一平面上，以分别得到第一观测子信息和第一期望子信息；

将所述观测航姿信息、所述期望航姿信息分别解耦至第二平面上，以分别得到第二观测子信息和第二期望子信息；

基于所述第一观测子信息与所述第一期望子信息的偏差，通过卡尔曼滤波算法框架得出第一平面调整参数；

基于所述第二观测子信息与所述第二期望子信息的偏差，通过卡尔曼滤波算法框架得出第二平面调整参数；

基于所述第一平面调整参数和所述第二平面调整参数，对所述水下巡检机器人的航行参数进行校正。

5.根据权利要求4所述的水下巡检机器人导航方法，其特征在于，所述将所述观测航姿信息、所述期望航姿信息分别解耦至第一平面上，以分别得到第一观测子信息和第一期望子信息的步骤，包括：

以所述第一轴向观测坐标、所述第二轴向观测坐标以及所述观测艏向角在所述第一平面上的投影角作为所述第一观测子信息，以所述第一轴向期望坐标、所述第二轴向期望坐标以及所述期望艏向角在所述第一平面上的投影角作为所述第一期望子信息；

所述基于所述第一观测子信息与所述第一期望子信息的偏差，通过卡尔曼滤波算法框架得出第一平面调整参数的步骤，包括：

判断所述第一观测子信息与所述第一期望子信息的偏差量是否大于第一偏差阈值；

若是，则基于所述第一观测子信息与所述第一期望子信息的偏差量，通过卡尔曼滤波算法框架得出可令所述第一观测子信息收敛至所述第一期望子信息的第一增益矩阵。