CN113433933B - 一种智能水面保洁无人船的自主巡航方法 - Google Patents

Abstract

本发明提出了一种智能水面保洁无人船的自主巡航方法，自主巡航方法包括固定路线自主巡航，固定路线自主巡航包括以下步骤：S1、拟合巡航路径；S2、初始化无人船起始点经纬度以及被跟踪轨迹点队列Queue[length]；S3、获取无人船的第k个跟踪目标点Queue[k]；S4、计算目标方位角，更新当前无人船航向角；S5、计算航向偏差角度，计算横向距离偏差以及无人船的当前位置与跟踪目标点之间的距离；S7、调用航迹控制算法；S8、无人船的控制中心根据航迹控制算法的输出发指令控制推进器执行单元工作；S9、判断本次巡航结束，具有两种巡航方式，两种巡航作业模式互相配合，并且可以根据场景自动切换，使得无人船巡航更加稳定安全。

Description

一种智能水面保洁无人船的自主巡航方法

技术领域

本发明涉及环保领域，具体涉及一种智能水面保洁无人船的自主巡航方法。

背景技术

随着社会发展，水面上的漂浮物越来越多，严重影响人们的生活质量，但目 前由于没有适合内河、内湖的清理机械，故大多采用人工打捞，但这种方法所占 人力多，劳动强度大，而且清理效果差、效率低，而唯一的一种履带式打捞船， 由于其船体结构庞大，且漂浮物大多聚集于河道岸边上，因此无法满足在内河、 内湖作业的需求。

相对于传统履带式打捞船，无人船艇因有着可在全天候、全水域环境下作业， 体积小而且灵活性好，作业效率高，成本低，没有人员安全隐患等优势而有望被 广泛应用在环境监测与治理、应急救援以及资源勘探等场合。

无人船艇的操作模式主要有远程驾驶、遥控以及自主巡航三种。自主巡航模 式指的是船艇按照船载设备预先规划好的路径进行航行，航行过程中通过船载 GPS模块更新位置、姿态信息，通过激光雷达、毫米波雷达等传感器实时感知环 境信息，如果事先规划好的路径不存在障碍物则按照规划路径行驶，如果出现障 碍物则需要根据传感器检测到的信息进行重规划以避开障碍物。

因此，本发明有必要提出一种用于无人船的自主巡航方法，使其能够有效对 水面尤其是靠近岸边的油污、蓝藻、青苔等细小漂浮物进行打捞清理。

发明内容

为解决现有技术的不足，本发明的目的是提供一种智能水面保洁无人船的自 主巡航方法，其通过船体、打捞部件、激光雷达、GPS、控制单元、推进器等部 件的配合实现无人船的自主巡航，以实现水面尤其是近岸水域的垃圾清理，水面 垃圾清理效率以及效果更佳。

为实现上述技术目的，本发明所采用的技术方案如下。

一种智能水面保洁无人船的自主巡航方法，包括设置于无人船上的激光雷达、GPS，所述自主巡航方法包括固定路线自主巡航，所述固定路线自主巡航包括以 下步骤：

S1、按照实际作业场景要求，拟合巡航路径；

S2、初始化无人船起始点经纬度以及被跟踪轨迹点队列Queue[length]；

S3、获取无人船的第k个跟踪目标点Queue[k]；

S4、计算目标方位角，更新当前无人船航向角；

S5、计算航向偏差角度，计算横向距离偏差以及无人船的当前位置与跟踪目 标点之间的距离；

S7、调用航迹控制算法；

S8、无人船的控制中心根据航迹控制算法的输出发指令控制推进器执行单元 工作；

S9、判断无人船与第k个跟踪目标点之间的距离是否小余事先设定的阈值， 如果小于阈值则认为无人船已经到达第k个目标点，接着判断是否到达最终目的 地，到达则本次巡航结束，如果没有到达则获取下一个跟踪目标点k++,并转到 步骤S3执行；如果执行完步骤S8后距离不小于阈值，则判定还未到达第k个 跟踪目标点，并转到步骤S4执行，直到本次巡航结束。

作为本发明的进一步改进，所述自主巡航方法还包括近岸巡航，所述近岸巡 航包括以下步骤：

T1、根据具体水域作业场景，设定无人船距离堤岸的阈值d₀，并记录此时 无人船沿激光雷达探测光束方向到堤岸边的距离s_l，计算激光雷达探测光束与堤 岸的夹角θ₀，

T2、根据GPS返回的航向角以及激光雷达数据自动调整初始航向直至与堤 岸平行，控制单元发指令控制推进器驱动无人船按照预先设定的动力巡航；

T3、读取激光雷达数据，即无人船与堤岸边的斜向距离s，并计算无人船与 堤岸间的垂直距离d，同时判断是否有障碍物，有则转入避障模式；

T4、调用航迹控制算法；

T5、判断无人船与堤岸边距离是否满足阈值距离要求，满足阈值要求则转 步骤T2执行，不满足距离要求则转步骤T3执行；直至巡航作业结束。

作为本发明的进一步改进，所述固定路线自主巡航方法中的巡航路径可以通 过手机、平板、电脑等便携式智能终端的APP、网页或者客户端根据实际作业需 求实时在线规划，也可以通过遥控器方式调用预先训练并存储在船载工控机或者 控制板中的巡航路径。

作为本发明的进一步改进，所述无人船的当前位置与跟踪目标点之间的距离 计算公式如下：

其中，curr(lat,lnt)、pre(lat1,lnt1)和dest(lat2,lnt2)分别是无人船的当前位置经纬度，上一目标点和下一目标点经纬度，lat为纬度值，lnt为经度值，l 为无人船的当前位置与下一跟踪目标点间的距离，R为赤道半径。

作为本发明的进一步改进，所述目标方位角为ψ_r，所述当前无人船航向角为

以无人船的当前位置为原点建立真北坐标系，其计算方法如下：

令

a、当目标点在X轴上时，即目标点与此时无人船处于同一纬度，当目标点 位于X轴正半轴时，ψ_r＝90°，当目标点位于X轴负半轴上时，ψ_r＝270°；

b、当目标点在第一象限或者Y轴正半轴上时，ψ_r＝ψ；

c、当目标点在第二象限上时，ψ_r＝ψ+360°；

d、当目标点在第三、四象限或者Y轴负半轴上时，ψ_r＝ψ+180°。

作为本发明的进一步改进，所述航向偏差角度的计算公式如下：

其中，ΔAngle为航向偏差角，ψ_r为目标方向角，

为当前无 人船的航向角，可以通过船载GPS获取。

作为本发明的进一步改进，所述横向距离偏差d_e指的是无人船的当前位置到 第k-1个和第k个跟踪目标点连线的垂直距离，其计算公式如下所示：

作为本发明的进一步改进，所述航迹控制步骤采用增量式PID控制算法， 其表达式如下所示：

Δu_k＝K_P(e_k-e_k-1)+K_Ie_k+K_D(e_k-2e_k-1+e_k-2)

u_k＝u_k-1+Δu_k

分别采用无人船航向控制PID控制器和横向距离PID控制器单独计算控制 量，PID控制器的最终输出，即为两个单独PID控制器的加权和，具体计算公式 如下所示：

Δu_k＝ρΔψ_r+(1-ρ)Δd_r

其中S是无人船上一个目标点pre与下一个目标点dest之间的距离；s是无 人船当前时刻与下一个跟踪目标点dest之间的实时距离；

Δu_k为PID控制器最终输出，ρ(0≤ρ≤1)为平衡因子，Δψ_r为航向PID控 制器的输出值，也即航向偏差调节值，K_Pa、K_Ia、K_Da分别为航向PID控制器的 比例常数、积分常数和微分常数，Δψ为航向偏差角，是航向PID控制器的输入 值，即Δψ＝ΔAngle；Δd_r为横向距离PID控制器的输出值，K_Pr、K_Ir、K_Dr分别 为横向距离PID控制器的比例常数、积分常数和微分常数，Δd为控制器的输入 值，与横向距离偏差相等，即Δd＝d_e。

作为本发明的进一步改进，所述推进器接收来自船载控制中心的指令并通过 差速调节方式实现无人船的航向调整，从而达到航迹跟踪的目的，根据航向偏差 以及PID控制器的输出Δu_k来确定下一时刻推进器的控制量，当航向偏差 -360°<ΔAngle<-180°或者0°<ΔAngle<180°时，船载控制单元发出左转指 令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推进器控制量u_kr＝ u_(k-1)r+K_rΔu_k；当航向偏差-180°≤ΔAngle≤0°或者180°≤ΔAngle≤360°时， 船载控制单元发出右转指令，右转时左推进器控制量u_kl＝u_(k-1)l+K_lΔu_k，右 推进器控制量u_kr＝u_(k-1)r-K_rΔu_k，其中K_l和K_r为左右推进器的增量控制系数， 当K_l＝K_r时左右推进器的增量大小相等。

作为本发明的进一步改进，所述近岸巡航中的航迹控制步骤采用增量式PID 控制算法，其表达式如下所示：

Δu_k＝K_P(e_k-e_k-1)+K_Ie_k+K_D(e_k-2e_k-1+e_k-2)

u_k＝u_k-1+Δu_k

以激光雷达探测到的距离s作为控制器的输入，当s>s_l时，无人船远离岸 边，此时控制器输出Δu_k使得无人船向右偏转靠近岸边，船载控制单元发出右转 指令，右转时左推进器控制量u_kl＝u_(k-1)l+K_lΔu_k，右推进器控制量 u_kr＝u_(k-1)r-K_rΔu_k，直到s＝s_l；当s<s_l时，无人船靠近岸边，为了防止撞 上岸边，此时控制器输出Δu_k使得无人船向左偏转远离岸边，进而船载控制单元 发出左转指令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推进器控制量 u_kr＝u_(k-1)r+K_rΔu_k，直到s＝s_l。

作为本发明的进一步改进，所述近岸巡航中无人船根据航迹控制算法调整完 阈值距离后会先按照GPS返回的航向角以及激光雷达数据自动调整当前航向直 至与堤岸再次平行，然后再按设定动力继续行驶。

作为本发明的进一步改进，其特征在于，所述固定路线自主巡航、所述近岸 巡航根据场景自动切换，在进行保洁作业时，无人船实时进行障碍物探测，若出 现障碍物则暂时停止当前目标点跟踪，直至绕过静态障碍物或者动态障碍物消失 后继续向当前跟踪点行驶。

本发明与现有技术相比的有益效果在于：

1、本发明将河道固定路线巡航模式(GPS航迹模式)模式的巡航轨迹点放 入队列中，方便读取；在航向计算时采用类似平面直角坐标系法求解目标方向角， 并将可能的4种情况列举出来，使得计算更加简单；

2、设计航向和轨迹PID控制器，并引入平衡因子ρ，以调节两个控制器的 权重，令ρ＝1，即可实现航向保持功能，当既要考虑航向又要跟踪轨迹时，可 以根据无人船的当前位置自动调节ρ，以实现航迹自动保持功能；同时，本发明 通过引入控制器增量系数K_l和K_r使得无人船的差速控制效果更佳；

3、针对现有保洁无人船无法靠近岸边作业，本发明利用激光雷达提出一种 简便可靠的近岸巡航模式(溜边模式)，使得智能保洁水面无人船可以实现岸边 作业；

4、两种巡航作业模式互相配合，并且可以根据场景自动切换；不仅可以实 现水域固定路线自主保洁作业，还能够实现近岸或者靠岸垃圾清理，而且打捞过 程简单易操作，使得水面垃圾清理效率以及效果更佳。

附图说明

图1为本发明GPS航迹模式算法流程图；

图2为本发明的GPS航迹模式自主巡航路径规划界面示意图；

图3为本发明的GPS航迹模式控制示意图；

图4为本发明的PID控制器设计示意图；

图5为本发明的溜边模式控制示意图；

图6为本发明的无人船作业流程图；

图7为本发明实施例1无人船结构示意图。

图中标号为：

1、船体；2、前舱盖；3、GPS天线；4、后舱盖；5、摄像头；6、激光雷达。

具体实施方式

为了便于理解本发明，下面将结合具体的实施例对本发明进行更全面的描述。

一种智能水面保洁无人船的自主巡航方法，具体巡航形式可以分为河道固定 路线巡航方式和近岸巡航方式，其结构主要包括船体、打捞部件、激光雷达、 GPS、控制单元、推进器等；

无人船的自主巡航步骤如下：

(一)、河道固定路线巡航模式

S1：按照实际作业场景要求，拟合巡航路径；

S2：初始化无人船起始点经纬度以及被跟踪轨迹点队列Queue[length]；

S3：获取无人船的第k个跟踪目标点Queue[k]；；

S4：计算目标方位角，更新当前无人船航向角；

S5：计算航向偏差角度，计算横向距离偏差以及无人船的当前位置与跟踪目 标点之间的距离；

S7：调用航迹控制算法；

S8：无人船的控制中心根据航迹控制算法的输出发指令控制推进器执行单元 工作；

S9：判断无人船与第k个跟踪目标点之间的距离是否小余事先设定的阈值， 如果小于阈值则认为无人船已经到达第k个目标点，接着判断是否到达最终目的 地，到达则本次巡航结束，如果没有到达则获取下一个跟踪目标点k++,并转到 步骤S3执行；如果执行完步骤S8后距离不小于阈值，则判定还未到达第k个 跟踪目标点，并转到步骤S4执行，直到本次巡航结束。

如图1-2所示，无人船自主巡航方法中的巡航路径可以通过手机、平板、电 脑等便携式智能终端的APP、网页或者客户端根据实际作业需求实时在线规划， 也可以通过遥控器方式调用预先训练并存储在船载工控机或者控制板中的巡航 路径.

无人船的当前位置与跟踪目标点之间的距离计算公式如下：

其中，curr(lat,lnt)、pre(lat1,lnt1)和dest(lat2,lnt2)分别是无人船的当前位置经纬度，上一目标点和下一目标点经纬度，lat为纬度值，lnt为经度值，l 为无人船的当前位置与下一跟踪目标点间的距离，R为赤道半径。

目标方位角ψ_r采用类似平面直角坐标系法求解，其计算方法如下：

以无人船的当前位置为原点建立真北坐标系，本发明将目标方位角的计算分 为以下4种情况：

a、当目标点在X轴上时，即目标点与此时无人船处于同一纬度，无法使用 上述公式计算，因此规定当目标点位于X轴正半轴时，ψ_r＝90°，当目标点 dest(lat2,lnt2)位于X轴负半轴上时，ψ_r＝270°；

b、当目标点在第一象限或者Y轴正半轴上(此时无人船的当前位置与目标 点的经度相同)时，ψ_r＝ψ；

c、当目标点在第二象限上时，ψ_r＝ψ+360°；

d、当目标点在第三、四象限或者Y轴负半轴上(此时无人船的当前位置与 目标点的经度相同)时，ψ_r＝ψ+180°。

进一步的，航向偏差角度的计算公式如下：

其中，ΔAngle为航向偏差角，ψ_r为目标方向角，

为当前无 人船的航向角，可以通过船载GPS获取。

如图3所示，横向距离偏差d_e指的是无人船的当前位置到第k-1个和第k个 跟踪目标点连线的垂直距离，其计算公式如下所示：

进一步的，航迹控制步骤采用增量式PID控制算法，其表达式如下所示：

Δu_k＝K_P(e_k-e_k-1)+K_Ie_k+K_D(e_k-2e_k-1+e_k-2)

u_k＝u_k-1+Δu_k

实际控制时分别采用无人船航向控制PID控制器和横向距离PID控制器单 独计算控制量，PID控制器的最终输出，即为两个单独PID控制器的加权和，具 体计算公式如下所示：

Δu_k＝ρΔψ_r+(1-ρ)Δd_r

其中，S是无人船上一个目标点pre与下一个目标点dest之间的距离；s是 无人船当前时刻与下一个跟踪目标点dest之间的实时距离；

当S/s<1，说明此时距离目标越远，此时优先考虑距离，距离误差的权重大 于航向误差的权重；

其他情况，即当S/s>1时，ρ设置为1，说明此时距离目标较近，距离误 差在可控范围内，只考虑调整航向误差；

Δu_k为PID控制器最终输出，ρ(0≤ρ≤1)为平衡因子，Δψ_r为航向PID控 制器的输出值，也即航向偏差调节值，K_Pa、K_Ia、K_Da分别为航向PID控制器的 比例常数、积分常数和微分常数，Δψ为航向PID控制器的输入值，在本发明中 为航向偏差角，即Δψ＝ΔAngle；Δd_r为横向距离PID控制器的输出值，KP_r、KI_r、K_Dr分别为横向距离PID控制器的比例常数、积分常数和微分常数，Δd为控制器 的输入值，在本发明中为横向距离偏差，即Δd＝d_e。

航迹控制步骤同时考虑了航向控制与轨迹跟踪，并通过平衡因子 ρ(0≤ρ≤1)调节两个PID控制器的控制权重，ρ越大航向控制PID控制效果明 显增强，轨迹控制减弱；ρ越小航向控制PID控制效果减弱，轨迹控制效果增强。 当作业场景不需要考虑轨迹跟踪时可以将ρ设为1，此时仅执行航向控制。

推进器接收来自船载控制中心的指令并通过差速调节方式实现无人船的航 向调整，从而达到航迹跟踪的目的，推进器具体逻辑控制关系如下所示：

根据航向偏差以及PID控制器的输出Δu_k来确定下一时刻推进器的控制量， 具体的当航向偏差-360°<ΔAngle<-180°或者0°<ΔAngle<180°时，船载控 制单元发出左转指令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推进器 控制量u_kr＝u_(k-1)r+K_rΔu_k；当航向偏差-180°≤ΔAngle≤0°或者180°≤ ΔAngle≤360°时，船载控制单元发出右转指令，右转时左推进器控制量 u_kl＝u_(k-1)l+K_lΔu_k，右推进器控制量u_kr＝u_(k-1)r-K_rΔu_k，其中K_l和K_r为左 右推进器的增量控制系数，当K_l＝K_r时左右推进器的增量大小相等。

如图6所示，无人船工作在河道固定路线巡航模式(GPS航迹模式)时，不 断重复上述步骤并实时根据激光雷达的扫描结果进行障碍物判断，若出现障碍物 则暂时停止当前目标点跟踪，直至绕过静态障碍物或者动态障碍物消失后继续向 当前跟踪点行驶，如果没有则继续航行，从而达到目标点跟踪，航迹保持的效果。

无人船在河道固定路线巡航模式(GPS航迹模式)时，可以通过远程控制执 行装置(例如打捞机构)的启停，以实现特定的作业功能(例如水面清洁)。

(二)、近岸巡航模式(溜边模式)

由于风和水面波浪等因素的影响，水面中的垃圾很大一部分会沉积在岸边， 无人船的河道固定路线巡航模式(GPS航迹模式)无法完成岸边垃圾的清理。因 此，本发明提出一种近岸巡航模式(溜边模式)，主要实现无人船沿着岸边行驶 并完成清扫作业的功能，近岸巡航模式(溜边模式)主要配合激光雷达实现，其 具体实现步骤如下：

S1：根据具体水域作业场景，设定无人船距离堤岸的阈值d₀(例如1-2m)， 并记录此时无人船沿激光雷达探测光束方向到堤岸边的距离s_l，计算激光雷达探 测光束与堤岸的夹角θ₀，

S2：根据GPS返回的航向角以及激光雷达数据自动调整初始航向直至与堤 岸平行，控制单元发指令控制推进器驱动无人船按照预先设定的动力巡航；

S3：读取激光雷达数据，即无人船与堤岸边的斜向距离s，并计算无人船与 堤岸间的垂直距离d，同时判断是否有障碍物，有则转入避障模式；

S4：调用航迹控制算法；

S5：判断无人船与堤岸边距离是否满足阈值距离要求，满足阈值要求则转步 骤S2执行，不满足距离要求则转步骤S3执行；直至巡航作业结束。

进一步的，航迹控制算法采用增量式PID控制算法，其表达式如下所示：

Δu_k＝K_P(e_k-e_k-1)+K_Ie_k+K_D(e_k-2e_k-1+e_k-2)

u_k＝u_k-1+Δu_k

实际控制中以激光雷达探测到的距离s作为控制器的输入，当s>s_l时，说 明无人船远离岸边，此时控制器输出Δu_k使得无人船向右偏转靠近岸边，船载控 制单元发出右转指令，右转时左推进器控制量u_kl＝u_(k-1)l+K_lΔu_k，右推进器 控制量u_kr＝u_(k-1)r-K_rΔu_k，直到s＝s_l；当s<s_l时，说明无人船靠近岸边， 为了防止撞上岸边，此时控制器输出Δu_k使得无人船向左偏转远离岸边，进而船 载控制单元发出左转指令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推 进器控制量u_kr＝u_(k-1)r+K_rΔu_k，直到s＝s_l。

更进一步的，无人船根据航迹控制算法调整完阈值距离后会先按照GPS返 回的航向角以及激光雷达数据自动调整当前航向直至与堤岸再次平行，然后再按 设定动力继续行驶。

实施例1

如图1所示，一种智能保洁水面无人船，包括船体1以及设置于船体1内的 垃圾收集舱14，船体1为双体船，船体1前后两端均设有与垃圾收集舱14连通 的开口，船体1两侧均设有密封舱，船体1中部为贯通结构，垃圾收集舱14为 两侧密封舱之间，垃圾收集舱14上方设有与船体1转动连接的前舱盖2，船体1 前端设有固态激光雷达6、GPS天线7，船体1上设有摄像头9，摄像头9位于 垃圾收集舱14上方，由于设置有固态激光雷达6，当无人驾驶时，船体前端的 固态激光雷达用于探测水面障碍物和识别水岸分界线，进一步通过水岸界线和激 光点云计算船体与岸边的距离和夹角，用于船体无人模式下的自主避障和路线优 化，同时用于船体溜边作业时精确的保持船体与岸边的相对距离。

如图6所示，无人船的自主巡航方法有两种工作模式，分别是河道固定路线 巡航方式(GPS航迹模式)和近岸巡航方式(溜边模式)。操作人员根据作业场 景自动设置无人船的作业模式。

当选择GPS航迹模式时，根据水面垃圾位置自动规划自主巡航路径，并生 成巡航路径点队列Queue[length]，自主巡航时依次从路径队列中选择一个点作为 本次目标跟踪点，根据无人船的当前位置获取当前航向角并计算目标跟踪点的方 位角，进而计算航向偏差角以及横向距离偏差，然后将航向偏差角和横向距离偏 差作为自适应PID控制器的输入，训练得到推进器的控制量，具体的说是根据 航向偏差以及PID控制器的输出来控制无人船的航向，例如当航向偏差 -360°<ΔAngle<-180°或者0°<ΔAngle<180°时，船载控制单元发出左转指 令；当航向偏差-180°≤ΔAngle≤0°或者180°≤ΔAngle≤360°时，船载控制单 元发出右转指令，然后按照当前调整航向行驶，行驶过程中根据PID控制器的 输出不断改变航向，并根据激光雷达检测结果判断是否执行避障，若出现障碍物 则暂时停止当前目标点跟踪，直至绕过静态障碍物或者动态障碍物消失后继续向 当前跟踪点行驶，直到无人船到达指定目标点或者巡航作业结束。

如图5所示，当工作在溜边模式时，首先根据河道情况设定与岸边的最小阈 值距离d₀，无人船起步逼近河道并保持阈值距离，记录无人船沿激光雷达探测光 束方向到堤岸边的距离s_l，计算激光雷达探测光束与堤岸的夹角θ₀。溜边行驶时 必须保证船的中心轴线方向与堤岸平行，所以无人船会根据GPS返回的航向角 以及激光雷达数据自动调整初始航向，航向调整完毕后设定推进器初始值，推进 器驱动无人船行驶，行驶过程中不断读取激光雷达数据，即无人船与堤岸边的斜 向距离s，并计算无人船与堤岸间的垂直距离d，判断d是否满足阈值距离d₀要 求，若不满足则调用航迹控制算法，航迹控制算法以斜向距离s作为PID控制器 的输入，推进器控制量增量为输出，当d<d₀时，说明距离岸边过近，可能产生搁浅危险，因此无人船向左偏转；同理，当d>d₀时，即如图5状态3所示，说 明距离岸边过远，此时无法完成清理岸边垃圾功能，因此无人船向右偏转；优选 的当无人船完成岸边距离调整后必须再次根据GPS返回的航向角以及激光雷达 数据自动调整航向直到与岸边平行，以上流程循环往复进行直到作业任务完成。

以上所述实施例仅表达了本发明的实施方式，其描述较为具体和详细，但并 不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技 术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些 都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (3)

1.一种智能水面保洁无人船的自主巡航方法，其特征在于，包括设置于无人船上的激光雷达、GPS，所述自主巡航方法包括固定路线自主巡航，所述固定路线自主巡航包括以下步骤：

S1、按照实际作业场景要求，拟合巡航路径，所述固定路线自主巡航方法中的巡航路径通过手机、平板、电脑便携式智能终端的APP、网页或者客户端根据实际作业需求实时在线规划，或通过遥控器方式调用预先训练并存储在船载工控机或者控制板中的巡航路径；

S2、初始化无人船起始点经纬度以及被跟踪轨迹点队列Queue[length]；

S3、获取无人船的第k个跟踪目标点Queue[k]；

S4、计算目标方位角，更新当前无人船航向角，所述目标方位角为ψ_r，所述当前无人船航向角为

以无人船的当前位置为原点建立真北坐标系，其计算方法如下：

令

a、当目标点在X轴上时，即目标点与此时无人船处于同一纬度，当目标点位于X轴正半轴时，ψ_r＝90°，当目标点位于X轴负半轴上时，ψ_r＝270°；

b、当目标点在第一象限或者Y轴正半轴上时，ψ_r＝ψ；

c、当目标点在第二象限上时，ψ_r＝ψ+360°；

d、当目标点在第三、四象限或者Y轴负半轴上时，ψ_r＝ψ+180°；

S5、计算航向偏差角度，计算横向距离偏差以及无人船的当前位置与跟踪目标点之间的距离，所述航向偏差角度的计算公式如下：

其中，ΔAngle为航向偏差角，ψ_r为目标方向角，

为当前无人船的航向角，

的取值范围为

可以通过船载GPS获取；

所述无人船的当前位置与跟踪目标点之间的距离计算公式如下：

其中，curr(lat,lnt)、pre(lat1,lnt1)和dest(lat2,lnt2)分别是无人船的当前位置经纬度，上一目标点和下一目标点经纬度，lat为纬度值，lnt为经度值，l为无人船的当前位置与下一跟踪目标点间的距离，R为赤道半径；

S7、调用航迹控制算法；

S8、无人船的控制中心根据航迹控制算法的输出指令控制推进器执行单元工作；

S9、判断无人船与第k个跟踪目标点之间的距离是否小于事先设定的阈值，如果小于阈值则认为无人船已经到达第k个目标点，接着判断是否到达最终目的地，到达则本次巡航结束，如果没有到达则获取下一个跟踪目标点k++,并转到步骤S3执行；如果执行完步骤S8后距离不小于阈值，则判定还未到达第k个跟踪目标点，并转到步骤S4执行，直到本次巡航结束；

所述横向距离偏差d_e指的是无人船的当前位置到第k-1个和第k个跟踪目标点连线的垂直距离，其计算公式如下：

所述自主巡航方法还包括近岸巡航，所述近岸巡航包括以下步骤：

T1、根据具体水域作业场景，设定无人船距离堤岸的阈值d₀，并记录此时无人船沿激光雷达探测光束方向到堤岸边的距离s_l，计算激光雷达探测光束与堤岸的夹角θ₀，

T2、根据GPS返回的航向角以及激光雷达数据自动调整初始航向直至与堤岸平行，控制单元发指令控制推进器驱动无人船按照预先设定的动力巡航；

T3、读取激光雷达数据，即无人船与堤岸边的斜向距离s，并计算无人船与堤岸间的垂直距离d，同时判断是否有障碍物，有则转入避障模式；

T4、调用航迹控制算法；

T5、判断无人船与堤岸边距离是否满足阈值距离要求，满足阈值要求则转步骤T2执行，不满足距离要求则转步骤T3执行；直至巡航作业结束；

所述航迹控制步骤采用增量式PID控制算法，其表达式如下：

Δu_k＝K_P(e_k-e_k-1)+K_Ie_k+K_D(e_k-2e_k-1+e_k-2)

u_k＝u_k-1+Δu_k

采用无人船航向控制PID控制器和横向距离PID控制器作为控制器输入量或采用激光雷达探测到的距离s作为控制器的输入量；

以无人船航向控制PID控制器和横向距离PID控制器作为控制器输入量时，PID控制器的最终输出，即为两个单独PID控制器的加权和，具体计算公式如下：

Δu_k＝ρΔψ_r+(1-ρ)Δd_r

其中S是无人船上一个目标点pre与下一个目标点dest之间的距离；s是无人船当前时刻与下一个跟踪目标点dest之间的实时距离；

Δu_k为PID控制器最终输出，ρ为平衡因子，ρ的取值范围为0≤ρ≤1，Δψ_r为航向控制PID控制器的输出值，也即航向偏差调节值，K_Pa、K_Ia、K_Da分别为航向控制PID控制器的比例常数、积分常数和微分常数，Δψ为航向偏差角，是航向控制PID控制器的输入值，即Δψ＝ΔAngle；Δd_r为横向距离PID控制器的输出值，K_Pr、K_Ir、K_Dr分别为横向距离PID控制器的比例常数、积分常数和微分常数，Δd为控制器的输入值，与横向距离偏差相等，即Δd＝d_e；

所述推进器接收来自船载控制单元的指令并通过差速调节方式实现无人船的航向调整，从而达到航迹跟踪的目的，根据航向偏差以及PID控制器的输出Δu_k来确定下一时刻推进器的控制量，当航向偏差-360°<ΔAngle<-180°或者0°<ΔAngle<180°时，船载控制单元发出左转指令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推进器控制量u_kr＝u_(k-1)r+K_rΔu_k；当航向偏差-180°≤ΔAngle≤0°或者180°≤ΔAngle≤360°时，船载控制单元发出右转指令，右转时左推进器控制量u_kl＝u_(k-1)l+K_lΔu_k，右推进器控制量u_kr＝u_(k-1)r-K_rΔu_k，其中K_l和K_r为左右推进器的增量控制系数，当K_l＝K_r时左右推进器的增量大小相等；

以激光雷达探测到的距离s作为控制器的输入量时，当s＞s_l时，无人船远离岸边，此时控制器输出Δu_k使得无人船向右偏转靠近岸边，船载控制单元发出右转指令，右转时左推进器控制量u_kl＝u_(k-1)l+K_lΔu_k，右推进器控制量u_kr＝u_(k-1)r-K_rΔu_k，直到s＝s_l；当s<s_l时，无人船靠近岸边，为了防止撞上岸边，此时控制器输出Δu_k使得无人船向左偏转远离岸边，进而船载控制单元发出左转指令，左转时左推进器控制量u_kl＝u_(k-1)l-K_lΔu_k，右推进器控制量u_kr＝u_(k-1)r+K_rΔu_k，直到s＝s_l；

其中s_l为无人船沿激光雷达探测光束方向到堤岸边的距离。

2.根据权利要求1所述的一种智能水面保洁无人船的自主巡航方法，其特征在于，所述近岸巡航中无人船根据航迹控制算法调整完阈值距离后会先按照GPS返回的航向角以及激光雷达数据自动调整当前航向直至与堤岸再次平行，然后再按设定动力继续行驶。

3.根据权利要求1所述的一种智能水面保洁无人船的自主巡航方法，其特征在于，所述固定路线自主巡航、所述近岸巡航根据场景自动切换，在进行保洁作业时，无人船实时进行障碍物探测，若出现障碍物则暂时停止当前目标点跟踪，直至绕过静态障碍物或者动态障碍物消失后继续向当前跟踪点行驶。

Images