一种水上组合导航控制方法及其控制系统与流程

本发明涉及导航自主控制领域，尤其涉及一种水上组合导航控制方法及其控制系统。

背景技术：

自主运动控制是区别于有人操纵控制的核心技术，现有的导航技术有许多，每种方法都有自己的优点和缺点。组合导航就是将这些导航技术有机地结合起来，优势互补，提供更为可靠的导航信息。其中，ins的数据会随工作时间的延长而累积。与之对比，gnss提供的数据精度却和工作时长无关但却受卫星状态和信号强弱的影响，将卫星导航gnss和惯性导航ins的组合已广泛得到应用。

惯性导航系统ins通过借助惯性传感器来计算用户的加速度,基于所述加速度计算用户的当前速度,并基于所述速度检测用户的当前位置。惯性导航技术具有自主性、连续性和隐蔽性等特点。

现有用于汽车等移动装置的组合导航需要人为参与导航算法的切换，无法自主选择，而且在无人驾驶汽车，无人飞机、无人船艇等装置中，无法智能识别外部条件，也没有人参与，特别地，在外部运动条件下无法准确修正gnss导航引入的运动误差和信号强弱衰减带入的系统误差。因此，特别是在无人参与的导航装置中，会给自主导航带来较大偏差，远离用户既定的设定航线。

技术实现要素：

针对上述缺陷或不足，本发明的目的在于提供一种水上组合导航控制方法及其控制系统。

为达到以上目的，本发明的技术方案为：

一种水上组合导航控制方法，包括：

1)、搜索邻近导航卫星gnss，获得卫星信息，根据卫星信息实时获取精度因子；

2)、当精度因子高于预设阈值，则导航卫星gnss导航定位；当精度因子低于预设阈值时，则开启ins传感器工作，通过ins传感器进行修正，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

所述通过ins传感器进行修正具体包括：

2.1、根据预设的航行路线，将水域水面记为二维平面，并建立坐标系中标注航线；

2.2、获取当前航行航向角以及航行的速度；

2.3、根据所标注的航线，选取修正向量，所述修正向量为航线外一点坐标垂直指向航线的向量；

2.4、根据修正向量，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

所述选取修正向量pc，设向量pc为δ，可得：

其中k为直线lab斜率。设控制向量为ρ，则可：

ρ＝ε+m·δ

其中m为修正系数，其大小决定控制系统对船体航向的修正量；当船体偏离航线越远时修正强度应越强，当船体偏离航线越近时修正强度越弱；m值与δ的模值正比例相关：

m＝n·|δ|

其中航行速度v和航向角θ建立无人艇航行模型n为系数。

根据权利要求2所述的水上组合导航控制方法，其特征在于，所述获取当前航行航向角以及航行的速度为：获取最大航行速度vmax，单位时间最大转角θmax。

当精度因子低于预设阈值时，通过ins传感器获取单位时间航向角偏差率pt＝p/t，其中，p为单位取角度，t为单位时间；当所述航向角偏差率pt＞30角度/秒时，金策间隔为0.5秒；当30角度/秒＞所述航向角偏差率pt＞5角度/秒时，控制间隔为3秒；当所述航向角偏差率pt<5角度/秒时，控制间隔为6秒。

所述精度因子为位置位置精度因子，接收器接收的卫星个数ngnss<4时，采用惯性导航方程；当接收器接收的卫星个数ngnss>＝4时，则开启ins传感器工作。

一种水上组合导航控制系统，包括gnss接收模块、对比模块、以及ins传感器接收模块；

gnss接收模块，用于搜索邻近导航卫星gnss，获得卫星信息，根据卫星信息实时获取精度因子；

对比模块，用于当精度因子高于预设阈值，则导航卫星gnss导航定位；当精度因子低于预设阈值时，则开启ins传感器工作；

ins传感器接收模块，用于通过ins传感器进行修正，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

所述ins传感器接收模块包括：航行拟定模拟模块、参数获取模块、修正选取模块、修正模块；

航行拟定模拟模块，用于根据预设的航行路线，将水域水面记为二维平面，并建立坐标系中标注航线；

参数获取模块，用于获取当前航行航向角以及航行的速度；

修正选取模块，用于根据所标注的航线，选取修正向量，所述修正向量为航线外一点坐标垂直指向航线的向量；

修正模块，用于根据修正向量，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

所述取修正向量pc，设向量pc为δ，可得：

其中k为直线lab斜率。设控制向量为ρ，则可：

ρ＝ε+m·δ

其中m为修正系数，其大小决定控制系统对船体航向的修正量；当船体偏离航线越远时修正强度应越强，当船体偏离航线越近时修正强度越弱；m值与δ的模值正比例相关：

m＝n·|δ|

其中航行速度v和航向角θ建立无人艇航行模型n为系数。

与现有技术比较，本发明的有益效果为：

本发明提供了一种水上组合导航控制方法及其控制系统，通过将获取的精度因子作为预设的阈值，根据阈值的比较，进行偏正航线的精度的修正工作，将高精度的gnss信息，作为外部测量输入，在运动过程中频繁修正ins，以控制其误差随时间累积；而短时间内高精度的ins定位结果，们可以很好地解决gnss动态环境中信号失锁和周跳问题。ins还可以辅助gnss接收机增强其抗干扰能力，提高铺货和曾总卫星的能力。本发明利用惯导ins的速度信号解决动态跟踪问题，高精度定位则由gnss来实现，提高了无人驾驶船舶的运行精度。

附图说明

图1是本发明控制流程框图；

图2是本发明添加修正向量的航线坐标图；

图3是本发明的系统结构图；

图4是本发明实施例仿真结果图；

图5是本发明实施例在干扰强度下仿真结果图，其中，(a)干扰强度为[0,0.3]，(b)干扰强度为[0,0.5]，(c)干扰强度为[0,0.8]。

具体实施方式

下面将结合附图对本发明做详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示，本发明提供了一种水上组合导航控制方法，包括：

1)、搜索邻近导航卫星gnss(globalnavigationsatellitesystem，全球导航卫星系统)，获得卫星信息，根据卫星信息实时获取精度因子；

示例性的，当精度因子低于预设阈值时，通过ins传感器获取单位时间航向角偏差率pt＝p/t，其中，p为单位取角度，t为单位时间；当所述航向角偏差率pt＞30角度/秒时，金策间隔为0.5秒；当30角度/秒＞所述航向角偏差率pt＞5角度/秒时，控制间隔为3秒；当所述航向角偏差率pt<5角度/秒时，控制间隔为6秒。

所述精度因子为位置位置精度因子，接收器接收的卫星个数ngnss<4时，采用惯性导航方程；当接收器接收的卫星个数ngnss>＝4时，则开启ins传感器工作。

需要说明的是，本发明在具体的设定中，能够根据实际的航行时间和航航行的路程、精度要求进一步的进行设定，本实施例只给出了一种可能想的实施方案。

2)、当精度因子高于预设阈值，则导航卫星gnss导航定位；当精度因子低于预设阈值时，则开启ins(inertialnavigationsystem，惯性导航系统)传感器工作，通过ins传感器进行修正，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

导航控制算法是用于控制无人艇运行方向，使之可以按照预定的航线进行航行的一种算法，合理的设计导航算法可以有效的提升无人艇自主航行的可靠性，本文通过参考其他导航算法，结合实际要求设计了一种基于航线的导航算法。以下是对该导航算法的说明。所述通过ins传感器进行修正具体包括：

2.1、根据预设的航行路线，将水域水面记为二维平面，并建立坐标系中标注航线；

在面积较小的水域中可以将水面看作为一个二维平面，在平面下建立坐标系，设lab为规划的航线，其中a为起始点，b为终点。设p为无人艇实际位置，假设点p与点a不重合。

只考虑无人艇驶向目标航点的情况下，只需要使无人艇以向量pb为方向行驶即可。因此可以按照向量pb来对无人艇进行航行控制，设向量pb为ε。

无人艇的航行可以简化为一下几个属性：1、航行方向；2、航行速度；3、最大航行速度；4、单位时间内最大转角。

2.2、获取当前航行航向角以及航行的速度；

以无人艇的，最大航行速度，单位时间最大转角，在k时刻无人艇的航行模型可以设为，则的迭代式如式：

α＝(vcosθ,vsinθ)

设无人艇单位时间内航行最大速度为vmax，最大调整角为δθmax。βk为k时刻向量ε的方向角。

δθk＝m·(βk-1-θk-1)(δθk≤δθmax)

2.3、根据所标注的航线，选取修正向量，所述修正向量为航线外一点坐标垂直指向航线的向量；

如图2所示，修正向量选取经过无人艇坐标点p指向航线lab的向量pc。

所述选取修正向量pc，设向量pc为δ，可得：

其中k为直线lab斜率。设控制向量为p，则可：

ρ＝ε+m·δ

其中m为修正系数，其大小决定控制系统对船体航向的修正量；当船体偏离航线越远时修正强度应越强，当船体偏离航线越近时修正强度越弱；m值与δ的模值正比例相关：

m＝n·|δ|

其中航行速度v和航向角θ建立无人艇航行模型n为系数。

将ρ＝ε+m·δ带入δθk＝m·(βk-1-θk-1)(δθk≤δθmax)，进行修正。

2.4、根据修正向量，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

如图3所示，本发明还提供了一种水上组合导航控制系统，包括gnss接收模块1、对比模块2、以及ins传感器接收模块3；

gnss接收模块1，用于搜索邻近导航卫星gnss，获得卫星信息，根据卫星信息实时获取精度因子；

对比模块2，用于当精度因子高于预设阈值，则导航卫星gnss导航定位；当精度因子低于预设阈值时，则开启ins传感器工作；

ins传感器接收模块3，用于通过ins传感器进行修正，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

具体是，所述ins传感器接收模块3包括：航行拟定模拟模块31、参数获取模块32、修正选取模块33、修正模块34；

航行拟定模拟模块31，用于根据预设的航行路线，将水域水面记为二维平面，并建立坐标系中标注航线；

参数获取模块32，用于获取当前航行航向角以及航行的速度；

修正选取模块33，用于根据所标注的航线，选取修正向量，所述修正向量为航线外一点坐标垂直指向航线的向量；

修正模块34，用于根据修正向量，获取修正系数，控制系统对船体航向的修正量，对航行的舵角偏转控制量和油门开度控制量进行操作。

所述取修正向量pc，设向量pc为δ，可得：

其中k为直线lab斜率。设控制向量为ρ，则可：

ρ＝ε+m·δ

其中m为修正系数，其大小决定控制系统对船体航向的修正量；当船体偏离航线越远时修正强度应越强，当船体偏离航线越近时修正强度越弱；m值与δ的模值正比例相关：

m＝n·|δ|

其中航行速度v和航向角θ建立无人艇航行模型n为系数。

实验仿真过程：

对仿真模型设置多个航点，进行连续航行仿真。为无人艇设定三个航点，构成一个闭合三角区域。三个航点分别为：a:(10,10)、b:(10,110)、c:(110,60)，这三个航点构成lab、lbc、lca三条航线。

仿真设置无人艇以最大航行速度2m/s匀速航行，设定无人艇单位时间内的最大转角为24°，开始仿真并记录每个航段的航行距离和航行时间。仿真结果如图4所示。

三条航线航行所用时间、距离等信息如表1所示。

表1航行记录

与路面行驶的车辆不同，无人艇在水中的航行还受到水面的波浪和风的影响。由于波浪与风的方向在一定时间内可以认为恒定，因此本文将波浪与风的影响简化为固定方向的干扰向量加入无人船的仿真模型进行仿真实验。

我们假设由波浪和风引起的干扰向量为σ，其方向角为τ，强度为μ，强度变化范围为[μmin,μmax]，则可得干扰向量的表达式。

σ＝μ·(cosτ,sinτ)(μmin≤μ≤μmax)

将式上式代入，分别设置干扰强度为[0,0.3]、[0,0.5]、[0,0.8]，无人艇以最大航行速度vmax＝2m/s航行，其单位时间最大转向角θmax＝24°，进行仿真可得仿真结果如图5所示。图5(a)图干扰强度为[0,0.3]，图5(b)干扰强度为[0,0.5]，图5(c)干扰强度为[0,0.8]。具体数据如表2所示：

表2，干扰强度分别为[0,0.3]、[0,0.5]、[0,0.8]时仿真数据

对于本领域技术人员而言，显然能了解到上述具体事实例只是本发明的优选方案，因此本领域的技术人员对本发明中的某些部分所可能作出的改进、变动，体现的仍是本发明的原理，实现的仍是本发明的目的，均属于本发明所保护的范围。