一种无人船自主导航方法及装置的制造方法
【技术领域】
[0001] 本发明涉及无人导航技术领域，具体涉及一种无人船自主导航方法及装置。
【背景技术】
[0002] 在水产养殖领域，因为水质状况对水生物的生存、生长影响很大，故水产养殖户对 水质信息的获取较为迫切，但传统的定点监测方法存在监测不全面的问题，要实现多处监 测，还存在要么需要人为调整，要么增加传感器设备从而增加成本等问题。
[0003] 现有技术中，也有使用小型无人船搭载水质监测传感器来实现对水体信息的监 测，通常是基于GPS定位加导航算法来实现自主导航，但这种方案适用于水域范围较大的 场景，大部分的水产养殖池塘的面积不是很大，因此现有的导航方法无法满足无人船自主 导航的精度要求，存在因人为遥控或定位误差因素造成的S型曲线运动的问题，造成导航 系统的不稳定。
【发明内容】
[0004] 本发明的目的在于提出一种无人船自主导航方法及装置，能够对无人船进行更加 精准的导航，并对直线段和曲线段采用不同的预测校正导航方法，解决了人为遥控或定位 误差因素造成的S型曲线运动的问题。
[0005] 为达此目的，本发明采用以下技术方案：
[0006] -种无人船自主导航方法，包括：
[0007] 预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标，并通过所述原始坐 标得到至少两个一级参考点坐标；
[0008] 获取所述无人船的当前位置坐标，并根据所述当前位置坐标判断所述无人船是否 偏离预设的运行轨迹；
[0009] 若所述无人船未偏离预设的运行轨迹，则利用卡尔曼滤波预测出下一个预测点坐 标；
[0010] 判断所述下一个预测点坐标与对应的一级参考点坐标的偏差是否小于预设的第 一阈值，若是，则使所述无人船向所述下一个预测点坐标运行；
[0011] 否则，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一步的运 行方向。
[0012] 优选地，还包括：
[0013] 若所述无人船偏离预设的运行轨迹，则根据所述当前位置坐标与所述一级参考点 坐标确定所述无人船进一步的运行方向。
[0014] 优选地，所述根据所述当前位置坐标及所述一级参考点坐标确定所述无人船进一 步的运行方向具体为：
[0015] 将每间隔预设数量的所述一级参考点坐标标记为二级参考点坐标；
[0016] 根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐标的位置关系 确定所述无人船进一步的运行方向。
[0017] 优选地，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐 标的位置关系确定所述无人船进一步的运行方向具体为：
[0018] 判断所述无人船当前所处轨迹段的轨迹状态；
[0019] 若所述无人船当前所处轨迹段为直线段轨迹，则获取与所述当前位置坐标相邻的 下一个二级参考点坐标，使所述无人船向所述下一个二级参考点坐标运行；
[0020] 若所述无人船当前所处轨迹段为曲线段轨迹，则获取与所述当前位置坐标相邻的 下一个一级参考点坐标，并计算得到所述下一个一级参考点坐标和下一个二级参考点坐标 的中间坐标，使所述无人船向所述中间坐标运行。
[0021] 优选地，所述根据所述当前位置坐标与所述一级参考点坐标、所述二级参考点坐 标的位置关系确定所述无人船进一步的运行方向具体为：
[0022] 获取与所述当前位置坐标相邻的下一个二级参考点坐标，使所述无人船向所述下 一个二级参考点坐标运行。
[0023] 优选地，所述采集无人船运行轨迹上的原始坐标，并通过所述原始坐标得到一级 参考点坐标包括：
[0024] 通过超宽带标签以预设的频率采集所述无人船运行轨迹上的原始坐标数组；
[0025] 从所述原始坐标数组中选取一个原始坐标作为第一个一级参考点坐标，沿着所述 预设的运行轨迹方向，获取与所述第一个一级参考点坐标的距离为预设距离差值的至少两 个原始坐标，计算所获取的所述至少两个原始坐标的平均值，将所述平均值定义为第二个 一级参考点坐标，以此类推，将所有原始坐标数组搜索完毕，即获得一级参考点坐标数组。
[0026] 优选地，所述判断所述无人船当前所处轨迹段的轨迹状态具体为：
[0027] 获取与所述当前位置坐标前后相邻的两个二级参考点坐标；
[0028] 计算所述相邻的两个二级参考点坐标所形成的直线的斜率；
[0029] 根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状态。
[0030] 优选地，所述根据计算所得的斜率值来判断所述无人船当前所处轨迹段的轨迹状 态具体为：
[0031] 当-0. 2 < Rn 1ιη< 0. 2时，确定所述无人船处于X轴直线段轨迹；
[0032] 当（0· 2 < Rn lin< 20&&xn> Xn D I I (-20 < Rn 1ιη< -0· 2&&xn< Xn D 时，确定所述 无人船处于左转弯状态；
[0033] 当（0· 2 < Rn lin< 20&&xn< Xn D I I (-20 < Rn 1ιη< -0· 2&&xn> Xn D 时，确定所述 无人船处于右转弯状态；
[0034] 当（Rn 1ιη> 20) I I (Rn lin< -20时，确定所述无人船处于Y轴直线段轨迹；
[0035] 其中，Rn ^为所述相邻的两个二级参考点坐标所形成直线的斜率值，η为正整数， xn JP X η对应为所述相邻的两个二级参考点坐标的X轴坐标；所述无人船处于X轴直线段 轨迹和处于Y轴直线段轨迹为所述直线段轨迹的两种形式；所述无人船处于左转弯状态和 处于右转弯状态为所述曲线段轨迹的两种形式。
[0036] 优选地，所述利用卡尔曼滤波预测出下一个预测点坐标具体为：
[0037] 1)定义\为卡尔曼滤波器k时刻的状态向量也即所需要预测的无人船的下一个 预测点，；为k时刻的状态先验估计，根据k-1时刻的状态向量Xk i也即无人船的当前坐 标点，计算k时刻的状态先验估计瞧_公式为：Xk = AXk 并计算k时刻的误差协方差先验 估计/V，公式为：Pk = AP k 乂+Q ;
[0038] 2)计算卡尔曼滤波增益Kk，公式为：Kk= P k HT (HPk HT+R) S
[0039] 3)定义Zk为观测向量，也即无人船在k时刻实际获取到的位置坐标，根据Z k计算 得到k时刻的状态向量也即状态后验估计Xk，公式为Xk= X k +Kk (Zk-HXk );
[0040] 4)将 \根据公式 Xk= [px(k)py(k)Vx(k)Vy(k)] TW向量形式展开，其中，px(k)、 PyGO分别是k时刻在x、y轴上的位置分量，vx(k)vy (k)分别是k时刻在x、y轴上的速度分 量，根据Px(k)、py(k)即可获得所需要预测的无人船的下一个预测点的坐标；
[0041] 其中，A为状态转移矩阵，H为观测矩阵
[0042] Q、R为协方差矩阵
[0043] 作为本发明的另一个方面，提供一种无人船自主导航装置，包括：
[0044] 轨迹绘制模块，用于预设无人船的运行轨迹，采集无人船运行轨迹上的原始坐标， 并通过所述原始坐标得到至少两个一级参考点坐标；
[0045] 获取判断模块，用于获取所述无人船的当前位置坐标，并根据所述当前位置坐标 判断所述无人船是否偏离预设的运行轨迹；
[0046] 预测模块，用于当无人船未偏离预设的运行轨迹时，则利用卡尔曼滤波预测出下 一个预测点坐标；
[0047] 偏差判断模块，用于判断所述下一个预测点坐标与对应的一级参考点坐标的偏差 是否小于预设的第一阈值；
[0048] 第一导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差小 于预设的第一阈值时，则使所述无人船向所述下一个预测点坐标运行；
[0049] 第二导航模块，用于当所述下一个预测点坐标与对应的一级参考点坐标的偏差大 于预设的第一阈值时，根据所述当前位置坐标与所述一级参考点坐标确定所述无人船进一 步的运行方向。
[0050] 本发明的有益效果为：一种无人船自主导航方法包括：预设无人船的运行轨迹， 采集无人船运行轨迹上的原始坐标，并通过原始坐标得到一级参考点坐标；获取无人船的 当前位置坐标，并根据当前位置坐标判断无人船是否偏离预设的运行轨迹；若未偏离，则利 用卡尔曼滤波预测出下一个预测点坐标；再判断下一个预测点坐标与对应的一级参考点坐 标的偏差是否小于预设的第一阈值，若是，则使无人船向下一个预测点坐标运行；否则，根 据当前位置坐标与一级参考点坐标确定所述无人船进一步的运行方向。本发明的无人船自 主导航方法解决了人为遥控或定位误差因素造成的S型曲线运动的问题，能更加的精确对 无人船进行导航。
【附图说明】
[0051] 图1是本发明实施例一提供的一种无人船自主导航方法的流程图。
[0052] 图2是本发明实施例一提供的一种坐标轴建立示意图。
[0053] 图3是本发明实施例二提供的一种轨迹绘制方法的流程图。
[0054] 图4是本发明实施例二提供的一种轨迹绘制方法的流程图。
[0055] 图5是本发明实施例二提供的一种轨迹绘制方法的流程图。
[0056] 图6是本发明实施例三提供的一种轨迹绘制方法的流程图。
[0057] 图7是本发明实施例四提供的卡尔曼滤波原理示意图。
[0058] 图8是本发明实施例四提供的一种导航调整示意图。
[0059] 图9是本发明实施例四提供的另一种导航调整示意图。
[0060] 图10是本发明实施例五提供的一种无人船自主导航装置的结构图。
[0061] 图11是图10中第二导航模块的结构图。
[0062] 图12是图11中导航单元的结构图。
[0063] 图13是图10中轨迹绘制模块的结构图。
[0064] 图14是图12中轨迹状态判断单元的