CN109828566B - 一种水面无人艇自主航行方法 - Google Patents

Abstract

本发明公开的一种水面无人艇自主航行方法，包含以下步骤：获取水面无人艇前进方向的障碍物信息；对获取的障碍物信息进行中值滤波处理，并通过前后数据帧信息结合，构建无人艇周遭水域环境模型；通过VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网络单元，得到当前帧的最优避碰方向；根据最优避碰方向与无人艇行驶方向的差值，结合纯跟踪算法计算无人艇循迹行驶的控制指令，使用简化的阿克曼车辆模型作为纯跟踪模型下无人艇的转角控制量，判断是否需要进行避碰；实时更新位姿信息，判断是否进入目标点的抵达半径，确定下一循环的工作状况；本发明提高无人艇对复杂海域的适应度，满足无人艇自主航行的需求。

Description

一种水面无人艇自主航行方法

技术领域

本发明涉及无人艇智能控制的技术领域，特别涉及一种水面无人艇自主航行方法。

背景技术

在水面无人艇的开发方面，自主航行系统的设计与实现一直都是开发流程中的核心任务。而自主航行算法作为系统中最重要的技术要点，不仅是无人艇能够顺利完成各种任务的前提，也是衡量无人艇智能化水平的重要依据。近年来，世界各国对国家的海洋权益更为重视，无人艇作为未来海洋战争的先驱，其无人避障技术得到了国内外研究机构的重视，并向着更自主化、网络化和集成化的方向发展。因此，研究水面无人艇自主航行新方法，对于提高无人艇智能化水平，实现无人艇军用、民用的普及具有重大意义。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种水面无人艇自主航行方法，针对水面无人艇在复杂海况下的自主航行问题，引入纯跟踪算法实现水面无人艇的循迹航行；采用激光雷达作为传感源，构建水面无人艇环境模型，并以此为基础采用VFH+局部路径规划算法获取当前帧下的最优避碰方向。通过避碰方向与目标行驶方向的差值作为全局循迹和局部避碰的转换点，从而实现水面无人艇在已知和未知环境下的自主航行。

本发明的目的通过以下的技术方案实现：

一种水面无人艇自主航行方法，包含以下步骤：

S1、获取水面无人艇前进方向的障碍物信息；

S2、对获取的障碍物信息进行中值滤波处理，并通过前后数据帧信息结合，构建无人艇周遭水域环境模型；

S3、通过VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网络单元，得到当前帧的最优避碰方向；

S4、根据最优避碰方向与无人艇行驶方向的差值，判断是否需要进行避碰；当角度差值小于阈值K，则无需进行避碰，此时结合纯跟踪算法计算无人艇循迹行驶的控制指令，使用简化的阿克曼车辆模型作为纯跟踪模型下无人艇的转角控制量；当角度差值大于阈值K，则需要进行避碰，使用VFH+算法最优避碰方向作为无人艇转角控制量；

S5、实时更新位姿信息，判断是否进入目标点的抵达半径，确定下一循环的工作状况。

进一步地，所述步骤S1具体为：使用电子海图的全局环境规划无人艇自主航行的目标节点，并通过激光雷达获取水面无人艇周围水域障碍物具体和方位。

进一步地，所述步骤S2具体为：对获取的障碍物信息进行中值滤波处理，剔除噪声数据点，并根据激光雷达前后数据帧信息进行结合，构建无人艇周遭水域环境模型；激光雷达前后数据帧信息指激光雷达经过N个检测过程后，无人艇获得的在时间上存在先后关系的N个相邻的雷达数据帧；

中值滤波处理的过程具体为：激光雷达测距序列中一点的值用该点的一个邻域中各点值的中值代替，计算公式为：

x'＝Med{x_i-n,…x_i-1,x_i,x_i+1,…x_i+n}，

式中，x'为中值滤波之后的数据点，Med{}为中值滤波函数，n为领域窗口的大小，x_i表示激光雷达测得的单个距离值。

进一步地，所述步骤S3具体为：引入VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网格单元，每一个网格单元由其方向和障碍物向量强度构成，具体为：以无人艇的几何中心为圆心，建立随无人艇移动的半径为ω_s的圆形活动窗口，其中ω_s＝d_max，d_max为激光雷达的最大探测范围；圆形活动窗口内每一个障碍物网格对应其障碍物向量C_i,j，C_i,j的方向为活动网格到无人艇几何中心的方向β_i,j：

其中，(x₀，y₀)为无人艇几何中心的坐标，(x_i，y_j)为障碍物网格C_i,j的坐标；

定义C_i,j的障碍物向量强度：

其中，d_i,j为障碍物网格C_i,j到无人艇几何中心的距离，c_i,j为概率函数，当障碍物网格C_i,j被障碍物覆盖时，c_i,j的值设为1，反之，c_i,j的值设为0；

而后通过逐步构建主直方图、二元直方图和掩模直方图压缩环境数据，最后通过对无人艇不同候选方向引入代价函数进行评估，从而获取当前激光雷达帧下的最优避碰方向，代价函数如下：

g(c)＝μ₁Δ(cα,θ_t)+μ₂Δ(cα,θ_i)+μ₃Δ(cα,θ_n,i-1)，

其中，c代表直方图的分区号，α代表激光雷达角度分辨率，Δ(c₁,c₂)则用于计算分区之间的角度差，θ_t为目标方向，θ_i为无人艇当前行进方向，θ_n,i-1为无人艇上一步行进方向；代价函数中系数μ₁表示候选可行方向与目标方向之间的角度差，系数μ₂表示候选可行方向与无人艇当前行进方向之间的角度差，系数μ₃表示候选可行方向与无人艇上一步行进方向之间的角度差；系数μ₁、μ₂、μ₃的选择决定了无人艇最优避碰方向的选择，考虑到无人艇在避障过程中以导向为主，应有μ₁＞μ₂+μ₃。

进一步地，所述步骤S4具体为：根据最优避碰方向与目标行驶方向的差值，判断当前时刻无人艇是否存在避碰需求，角度差值过小则无需进行避碰，采用纯跟踪算法计算无人艇循迹行驶的控制指令；角度差值过大则需要进行避碰，将最优避碰方向转化成控制指令输出；

计算过程如下：

纯跟踪算法以无人艇后轴为切点，纵向艇身为切线，通过控制无人艇转向偏角，使无人艇可以沿着一条经过预瞄点的圆弧行驶；其中R表示无人艇的转向半径，(g_x,g_y)为规划路径上的预瞄点，l_d为预瞄距离，α为无人艇与预瞄点的夹角，根据正弦定理可得：

其中，κ表示圆弧曲率，κ＝1/R；

根据简化的阿克曼车辆模型，无人艇转向控制量可以表示为：

其中，L表示无人艇艇长，α(t)为时变的无人艇与预瞄点夹角；输出的转向控制量σ表示无障碍环境下的偏转角度，通过控制无人艇的横向转角使得无人艇能够循迹航行。

进一步地，所述阈值K设定为[-w*t,w*t]，其中w为无人艇转向角速度，t为算法运行周期。

进一步地，所述步骤S5具体为：

在水面无人艇的一个环境更新周期内，定义激光雷达获得的一帧检测数据为D_i＝{T₁,T₂,…T_i,…T_N-1,T_N}，为此在激光雷达达到N个检测过程后，无人艇获得由N个相邻的雷达数据帧所组成的环境更新周期；基于激光雷达数据帧和水面无人艇在每个数据帧下的位姿信息，将一个环境更新周期内的N个激光雷达数据帧整合到当前帧所在的局部坐标系下，坐标转换公式如下：

其中，τ_im(i＜j)代表T_i到T_j的时间，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，x_ij代表在T_j坐标系下T_i的横坐标值，y_ij代表在T_j坐标系下T_i的纵坐标值，θ_im代表速度

和速度

之间的夹角。(x_im,y_im)为T_i数据帧整合到T_j数据帧的结果。

本发明与现有技术相比，具有如下优点和有益效果：

本发明采用中值滤波和前后帧数据融合处理激光雷达数据信息，能够比较好的剔除噪声数据点，且在不加数据通信时间成本的前提下，丰富了水面无人艇环境模型；根据简化的阿克曼车辆模型作为纯跟踪模型下水面无人艇的转角控制量，使得水面无人艇的回航和航迹保持均有较为可靠的表现。通过避碰方向与目标行驶方向的差值作为全局循迹和局部避碰的转换点，从而实现水面无人艇在已知和未知环境下的自主航行。本发明提出的自主航行算法实现了水面无人艇在已知和位置环境下的自主航行，可广泛适用于各种无人艇的航行控制。

附图说明

图1为本发明所述一种水面无人艇自主航行方法的方法流程图；

图2为实施例中纯跟踪算法在预瞄距离内的控制示意图；

图3为实施例中一个环境模型更新周期内水面无人艇的位姿状态示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

一种水面无人艇自主航行方法，如图1所示，包括以下步骤：

第一步、获取水面无人艇前进方向的障碍物信息；具体为：基于电子海图的全局环境规划水面无人艇自主航行的目标节点；通过激光雷达获取水面无人艇周围水域障碍物类型、障碍物距离和障碍物方位信息。

第二步、对获取的障碍物信息进行中值滤波处理，剔除噪声数据点，并根据激光雷达前后数据帧信息进行结合，构建无人艇周遭水域环境模型；激光雷达前后数据帧信息指激光雷达经过N个检测过程后，无人艇获得的在时间上存在先后关系的N个相邻的雷达数据帧；所述障碍物信息包含障碍物类型、障碍物距离和障碍物方位信息；

中值滤波处理的过程具体为：激光雷达测距序列中一点的值用该点的一个邻域中各点值的中值代替，计算公式为：

x'＝Med{x_i-n,…x_i-1,x_i,x_i+1,…x_i+n}，

式中，x'为中值滤波之后的数据点，Med{}为中值滤波函数，n为领域窗口的大小，x_i表示激光雷达测得的单个距离值。

第三步、引入VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网格单元，每一个网格单元由其方向和障碍物向量强度构成，具体为：以无人艇的几何中心为圆心，建立随无人艇移动的半径为ω_s的圆形活动窗口，其中ω_s＝d_max，d_max为激光雷达的最大探测范围；圆形活动窗口内每一个障碍物网格对应其障碍物向量C_i,j，C_i,j的方向为活动网格到无人艇几何中心的方向β_i,j：

其中，(x₀，y₀)为无人艇几何中心的坐标，(x_i，y_j)为障碍物网格C_i,j的坐标；

定义C_i,j的障碍物向量强度：

其中，d_i,j为障碍物网格C_i,j到无人艇几何中心的距离，c_i,j为概率函数，当障碍物网格C_i,j被障碍物覆盖时，c_i,j的值设为1，反之，c_i,j的值设为0；

而后通过逐步构建主直方图、二元直方图和掩模直方图压缩环境数据，最后通过对无人艇不同候选方向引入代价函数进行评估，从而获取当前激光雷达帧下的最优避碰方向，代价函数如下：

g(c)＝μ₁Δ(cα,θ_t)+μ₂Δ(cα,θ_i)+μ₃Δ(cα,θ_n,i-1)，

其中，c代表直方图的分区号，α代表激光雷达角度分辨率，Δ(c₁,c₂)则用于计算分区之间的角度差，θ_t为目标方向，θ_i为无人艇当前行进方向，θ_n,i-1为无人艇上一步行进方向；代价函数中系数μ₁表示候选可行方向与目标方向之间的角度差，系数μ₂表示候选可行方向与无人艇当前行进方向之间的角度差，系数μ₃表示候选可行方向与无人艇上一步行进方向之间的角度差；系数μ₁、μ₂、μ₃的选择决定了无人艇最优避碰方向的选择，考虑到无人艇在避障过程中以导向为主，应有μ₁＞μ₂+μ₃。

第四步、根据最优避碰方向与无人艇行驶方向的差值，结合纯跟踪算法计算无人艇循迹行驶的控制指令，使用简化的阿克曼车辆模型作为纯跟踪模型下无人艇的转角控制量，判断是否需要进行避碰；当角度差值小于阈值K，则无需进行避碰；当角度差值大于阈值，则需要进行避碰；所述阈值K设定为[-w*t,w*t]，其中w为无人艇转向角速度，t为算法运行周期；

具体为：根据最优避碰方向与目标行驶方向的差值，判断当前时刻无人艇是否存在避碰需求，角度差值过小则无需进行避碰，采用纯跟踪算法计算无人艇循迹行驶的控制指令；角度差值过大则需要进行避碰，将最优避碰方向转化成控制指令输出；图2为纯跟踪算法在一个预瞄距离内的控制示意图；

计算过程如下：

纯跟踪算法以无人艇后轴为切点，纵向艇身为切线，通过控制无人艇转向偏角，使无人艇可以沿着一条经过预瞄点的圆弧行驶；其中R表示无人艇的转向半径，(g_x,g_y)为规划路径上的预瞄点，l_d为预瞄距离，α为无人艇与预瞄点的夹角，根据正弦定理可得：

其中，κ表示圆弧曲率，κ＝1/R；

根据简化的阿克曼车辆模型，无人艇转向控制量可以表示为：

其中，L表示无人艇艇长，α(t)为时变的无人艇与预瞄点夹角；输出的转向控制量σ表示无障碍环境下的偏转角度，通过控制无人艇的横向转角使得无人艇能够循迹航行。

第五步、实时更新位姿信息，判断是否进入目标点的抵达半径，确定下一循环的工作状况；如图3所示；

具体为：在水面无人艇的一个环境更新周期内，定义激光雷达获得的一帧检测数据为D_i＝{T₁,T₂,…T_i,…T_N-1,T_N}，为此在激光雷达达到N个检测过程后，无人艇获得由N个相邻的雷达数据帧所组成的环境更新周期；基于激光雷达数据帧和水面无人艇在每个数据帧下的位姿信息，将一个环境更新周期内的N个激光雷达数据帧整合到当前帧所在的局部坐标系下，坐标转换公式如下：

其中，τ_im(i＜j)代表T_i到T_j的时间，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，x_ij代表在T_j坐标系下T_i的横坐标值，y_ij代表在T_j坐标系下T_i的纵坐标值，θ_im代表速度

和速度

之间的夹角；(x_im,y_im)为T_i数据帧整合到T_j数据帧的结果。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.一种水面无人艇自主航行方法，其特征在于，包括以下步骤：

S1、获取水面无人艇前进方向的障碍物信息；

S2、对获取的障碍物信息进行中值滤波处理，并通过前后数据帧信息结合，构建无人艇周遭水域环境模型；具体为：对获取的障碍物信息进行中值滤波处理，剔除噪声数据点，并根据激光雷达前后数据帧信息进行结合，构建无人艇周遭水域环境模型；激光雷达前后数据帧信息指激光雷达经过N个检测过程后，无人艇获得的在时间上存在先后关系的N个相邻的雷达数据帧；

中值滤波处理的过程具体为：激光雷达测距序列中一点的值用该点的一个邻域中各点值的中值代替，计算公式为：

x'＝Med{x_i-n,…x_i-1,x_i,x_i+1,…x_i+n}，

式中，x'为中值滤波之后的数据点，Med{}为中值滤波函数，n为领域窗口的大小，x_i表示激光雷达测得的单个距离值；

S3、通过VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网络单元，得到当前帧的最优避碰方向；具体为：引入VFH+局部路径规划算法将无人艇周遭水域环境模型分解为一系列具有二值信息的网格单元，每一个网格单元由其方向和障碍物向量强度构成，具体为：以无人艇的几何中心为圆心，建立随无人艇移动的半径为ω_s的圆形活动窗口，其中ω_s＝d_max，d_max为激光雷达的最大探测范围；圆形活动窗口内每一个障碍物网格对应其障碍物向量C_i,j，C_i,j的方向为活动网格到无人艇几何中心的方向β_i,j：

其中，(x₀，y₀)为无人艇几何中心的坐标，(x_i，y_j)为障碍物网格C_i,j的坐标；

定义C_i，j的障碍物向量强度：

其中，d_i,j为障碍物网格C_i,j到无人艇几何中心的距离，c_i,j为概率函数，当障碍物网格C_i,j被障碍物覆盖时，c_i,j的值设为1，反之，c_i,j的值设为0；

而后通过逐步构建主直方图、二元直方图和掩模直方图压缩环境数据，最后通过对无人艇不同候选方向引入代价函数进行评估，从而获取当前激光雷达帧下的最优避碰方向，代价函数如下：

g(c)＝μ₁Δ(cα,θ_t)+μ₂Δ(cα,θ_i)+μ₃Δ(cα,θ_n,i-1)，

其中，c代表直方图的分区号，α代表激光雷达角度分辨率，Δ(c₁,c₂)则用于计算分区之间的角度差，θ_t为目标方向，θ_i为无人艇当前行进方向，θ_n,i-1为无人艇上一步行进方向；代价函数中系数μ₁表示候选可行方向与目标方向之间的角度差，系数μ₂表示候选可行方向与无人艇当前行进方向之间的角度差，系数μ₃表示候选可行方向与无人艇上一步行进方向之间的角度差；系数μ₁、μ₂、μ₃的选择决定了无人艇最优避碰方向的选择，考虑到无人艇在避障过程中以导向为主，应有μ₁＞μ₂+μ₃；

S4、根据最优避碰方向与无人艇行驶方向的差值，判断是否需要进行避碰；当角度差值小于阈值K，则无需进行避碰，此时结合纯跟踪算法计算无人艇循迹行驶的控制指令，使用简化的阿克曼车辆模型作为纯跟踪模型下无人艇的转角控制量；当角度差值大于阈值K，则需要进行避碰，使用VFH+算法最优避碰方向作为无人艇转角控制量；

S5、实时更新位姿信息，判断是否进入目标点的抵达半径，确定下一循环的工作状况；具体为：

在水面无人艇的一个环境更新周期内，定义激光雷达获得的一帧检测数据为D_i＝{T₁,T₂,…T_i,…T_N-1,T_N}，为此在激光雷达达到N个检测过程后，无人艇获得由N个相邻的雷达数据帧所组成的环境更新周期；基于激光雷达数据帧和水面无人艇在每个数据帧下的位姿信息，将一个环境更新周期内的N个激光雷达数据帧整合到当前帧所在的局部坐标系下，坐标转换公式如下：

其中，τ_im(i＜j)代表T_i到T_j的时间，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，

代表水面无人艇在T_i到T_j在x轴方向的平均移动速度，x_ij代表在T_j坐标系下T_i的横坐标值，y_ij代表在T_j坐标系下T_i的纵坐标值，θ_im代表速度

和速度

之间的夹角；(x_im,y_im)为T_i数据帧整合到T_j数据帧的结果。

2.根据权利要求1所述的一种水面无人艇自主航行方法，其特征在于，所述步骤S1具体为：使用电子海图的全局环境规划无人艇自主航行的目标节点，并通过激光雷达获取无人艇周围水域障碍物具体和方位。

3.根据权利要求1所述的一种水面无人艇自主航行方法，其特征在于，所述障碍物信息包含障碍物类型、障碍物距离、障碍物方位信息；所述障碍物类型包括：海岸线、岛礁、港口、海上标识建筑物。

4.根据权利要求1所述的一种水面无人艇自主航行方法，其特征在于，所述步骤S4具体为：根据最优避碰方向与目标行驶方向的差值，判断当前时刻无人艇是否存在避碰需求，角度差值过小则无需进行避碰，采用纯跟踪算法计算无人艇循迹行驶的控制指令；角度差值过大则需要进行避碰，将最优避碰方向转化成控制指令输出；

计算过程如下：

纯跟踪算法以无人艇后轴为切点，纵向艇身为切线，通过控制无人艇转向偏角，使无人艇可以沿着一条经过预瞄点的圆弧行驶；其中R表示无人艇的转向半径，(g_x,g_y)为规划路径上的预瞄点，l_d为预瞄距离，α为无人艇与预瞄点的夹角，根据正弦定理可得：

其中，κ表示圆弧曲率，κ＝1/R；

根据简化的阿克曼车辆模型，无人艇转向控制量可以表示为：

其中，L表示无人艇艇长，α(t)为时变的无人艇与预瞄点夹角；输出的转向控制量σ表示无障碍环境下的偏转角度，通过控制无人艇的横向转角使得无人艇能够循迹航行。

5.根据权利要求1所述的一种水面无人艇自主航行方法，其特征在于，所述阈值K设定为[-w*t,w*t]，其中w为无人艇转向角速度，t为算法运行周期。

Images