CN111142527B - 无人船任意路径的追踪控制方法 - Google Patents

Abstract

本发明公开了一种无人船任意路径的追踪控制方法，包括以下步骤：遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径；对期望路径进行滤波处理，得到最终路径信息；对最终路径信息计算得到控制信息；根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息；将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动。用户可以通过操作端，控制无人船任意行驶一段曲线路径，后台自动记录并提取所行驶的路径信息，同时可以根据无人船的实时位置自动计算出所需的控制指令，使无人船实现对任意曲线的跟踪，达到任意曲线路径自主导航的目的。

Description

无人船任意路径的追踪控制方法

技术领域

本发明涉及无人船自动控制领域，更具体地说是一种无人船任意路径的追踪控制方法。

背景技术

近年来，随着无人化智能化深入推进到水面清洁领域，无人船的自主导航与控制越来越受到重视。现有的无人船自主导航采用的是通过在操作端标点形成直线段路径，进而实现点与点之间直线行驶。如公开号为CN201725222U的实用新型专利公开的基于单个目标点进行自主导航控制的无人船导航算法，该算法根据船只当前位置与目标点的位置的偏差，计算得到船只的控制指令，进而达到直线行驶的效果。

在实际场景中，由于河流湖泊等水面垃圾经常积聚在堤岸附近，这里形状崎岖变化，直线行驶效率低下，且难以较好的贴合堤岸形状。同时在狭窄河道中，直线行驶标定复杂且实现困难。因此无人船对任意曲线路径的追踪控制具有十分重要的意义。

发明内容

本发明的目的在于克服现有技术的不足，提供一种无人船任意路径的追踪控制方法。

为实现上述目的，本发明采用以下技术方案：一种无人船任意路径的追踪控制方法，包括以下步骤：

遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径；

对期望路径进行滤波处理，得到最终路径信息；

对最终路径信息计算得到控制信息；

根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息；

将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动。

其进一步技术方案为：所述遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径的步骤，包括：

每隔一定时间依次记录储存无人船当前位置点N_i的位置坐标，形成期望路径的点队列

其进一步技术方案为：所述对期望路径坐标进行滤波处理，得到最终路径信息的步骤，包括：

删去期望路径中的重复点，形成路径点队列

删去路径点队列中的临近点，形成路径点队列/>

检测路径点队列有效性，删除偏离点，将剩下的路径点组成路径点

队列

用插值的方法进行无人船路径点补充，形成路径点队列

其进一步技术方案为：所述对最终路径信息计算得到控制信息的步骤，包括：

计算无人船路径切线方向角，任选路径点队列中的相邻两点N_i和N_i+1，点N_i处的切线方向角等于点N_i和N_i+1所在直线的方向角α_i，形成切线方向角序列α＝{α₁,α₂,α₃......α_n}；

计算无人船目标方向角，在点队列中向后搜索m个点，点N_i处的目标方向角为这(m+1)个点的切线方向角的加权平均，形成目标方向角序列β＝{β₁,β₂,β₃......β_n；}

其进一步技术方案为：所述根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息的步骤，包括：

更新无人船的目标点，在点队列中向后搜索r个点形成点队列子集计算无人船与点队列子集/>中每个点的距离，将距离最近的点作为无人船的目标点；

计算无人船与期望路径的垂直距离；

判断无人船偏离期望路径的区域；

计算无人船的目标法向角；

计算无人船的控制方向角；

计算无人船的转弯控制量；

预判无人船未来路径曲率；

计算无人船的前进控制量。

其进一步技术方案为：所述计算无人船与期望路径的垂直距离的步骤，包括：

定义当前时刻无人船的目标位置点为；

计算无人船当前位置点N_i和前一个位置点N_i-1相连所成直线的垂直距离，并将计算出的距离作为无人船与期望路径的垂直距离d。

其进一步技术方案为：所述判断无人船偏离期望路径的区域步骤，包括：

定义沿期望路径方向右侧为内侧区域，沿期望路径方向左侧为外侧区域；

根据无人船当前位置坐标，判定判断无人船所在区域。

其进一步技术方案为：所述计算无人船的目标法向角的步骤，包括：

定义当前时刻无人船的目标位置点为N_i，此时无人船的切线方向角为α_i；

根据无人船所在区域，若无人船在内侧区域，则无人船的目标法向角为α_i+90°；

若无人船在外侧区域，则无人船的目标法向角为α_i-90°。

其进一步技术方案为：所述计算无人船的控制方向角的步骤，包括：

根据无人船的目标方向角和目标法向角，将目标方向角和目标法向角加权平均得到无人船的控制方向角。

其进一步技术方案为：所述计算无人船的转弯控制量的步骤，包括：

根据无人船的控制方向角，与无人船实际的方向角做差得到误差方向角ε；

根据误差方向角ε并通过PID控制器得到转弯控制量值dire。

其进一步技术方案为：所述预判无人船未来路径曲率的步骤，包括：

定义当前时刻无人船的目标位置点为N_i；

向后搜索k个点形成无人船点队列子集

根据无人船切线方向角，计算点队列子集对应的切线角序列α_k＝{α_i,α_i+1,α_i+2......α_i+k}的峰峰值；

若峰峰值大于设定的阈值，判定为无人船未来路径出现拐弯；

若峰峰值小于或等于设定的阈值，判定为无人船未来路径没有拐弯。

其进一步技术方案为：所述计算无人船的前进控制量的步骤，包括：

根据无人船与期望路径的垂直距离、误差方向角和是否出现转弯，通过控制系统计算出无人船前进控制量thrust。

其进一步技术方案为：所述将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动的步骤，包括：

将无人船转弯控制量dire和前进控制量thrust发送给无人船的执行机构，使无人船按指令行驶。

本发明与现有技术相比的有益效果是：本发明提供的一种无人船任意路径的追踪控制方法，用户可以通过操作端，控制无人船任意行驶一段曲线路径，后台自动记录并提取所行驶的路径信息，同时可以根据无人船的实时位置自动计算出所需的控制指令，使无人船实现对任意曲线的跟踪，达到任意曲线路径自主导航的目的。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明技术手段，可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征及优点能够更明显易懂，以下特举较佳实施例，详细说明如下。

附图说明

图1为本发明无人船任意路径的追踪控制方法流程图；

图2为对期望路径坐标进行滤波处理，得到最终路径信息的步骤流程图；

图3为对最终路径信息计算得到控制信息的步骤流程图；

图4为根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息的步骤流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，下面结合附图和具体实施方式对本发明作进一步详细说明。

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

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

如图1所示，一种无人船任意路径的追踪控制方法，包括以下步骤：

S11、遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径。

S12、对期望路径坐标进行滤波处理，得到最终路径信息。

S13、对最终路径信息计算得到控制信息。

S14、根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息。

S15、将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动。用户可以通过操作端，控制无人船任意行驶一段曲线路径，后台自动记录并提取所行驶的路径信息，同时可以根据无人船的实时位置自动计算出所需的控制指令，使无人船实现对任意曲线的跟踪，达到任意曲线路径自主导航的目的。

具体地，遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径的步骤，包括：

在期望路径的起点，用户通过操作端，发送开始记录指令，船只开始记录并不断存储自身位置坐标。然后用户通过操作端控制船只行驶所期望的任意一段曲线路径。当船只达到期望路径终点时，用户通过操作端，发送停止记录指令。每隔一定时间依次记录储存无人船当前位置点N_i的位置坐标，形成期望路径的点队列

具体地，用户的操作端可以为遥控器、智能手机、平板、电脑等一切包含通讯功能的设备。在操作端通过控制无人船行驶一段任意曲线路径，获取期望路径曲线的坐标信息。

具体地，如图2所示，对期望路径坐标进行滤波处理，得到最终路径信息的步骤，包括：

S12a、删去期望路径中的重复点，后台根据路径点队列中每个点的坐标信息，删去路径点中两两相邻坐标相同的重复点，形成路径点队列/>

S12b、删去期望路径中的临近点，后台根据路径点队列中每个点的坐标信息，计算两两相邻点之间的距离，删去距离过小的点，形成路径点队列/>

S12c、检测路径点有效性，当路径点中出现单个点偏离期望路线时，删除该偏离点，针对路径点队列重复步骤检测路径点有效性直至路径中没有偏离路径的单个点，形成路径点队列/>

S12d、用插值的方法进行路径点补充，计算路径点队列中两两相邻点之间的距离。当距离过大时，采用插值的方法进行路径点补充，重复路径点补充直至任意相邻路径点之间距离适当，形成路径点队列/>

具体地，如图3所示，对最终路径信息计算得到控制信息的步骤，包括：

S13a、计算路径切线方向角，任选路径点队列中的相邻两点N_i和N_i+1，点N_i处的切线方向角等于点N_i和N_i+1所在直线的方向角α_i，形成切线方向角序列α＝{α₁,α₂,α₃......α_n；}

S13b、计算目标方向角，对于路径点列中的任意点N_i，在点队列中向后搜索m个点，点N_i处的目标方向角为这(m+1)个点的切线方向角的加权平均，形成目标方向角序列β＝{β₁,β₂,β₃......β_n}。

具体地，如图4所示，对根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息的步骤，包括：

S14a、更新无人船的目标点，若上一时刻无人船的目标点为点队列中的N_i点，向后搜索r个点形成点队列子集计算无人船与点队列子集/>中每个点的距离，将距离最近的点作为无人船的目标点；其中搜索长度r与船速成正比。

S14b、计算无人船与期望路线的垂直距离；

S14c、判断无人船偏离期望路线的区域；

S14d、计算无人船的目标法向角；

S14e、计算无人船的控制方向角；

S14f、计算无人船的转弯控制量；

S14g、预判无人船未来路径曲率；

S14h、计算无人船的前进控制量。

具体地，计算无人船与期望路线的垂直距离的步骤，包括：

定义当前时刻无人船的目标点为N_i；

计算无人船当前位置点N_i和前一个点N_i-1相连所成直线的垂直距离；将计算出的距离作为无人船与期望路线的垂直距离d。

具体地，判断无人船偏离期望路线的区域步骤，包括：

期望路径将整个坐标平面划分为两个区域，定义沿期望路径方向右侧为内侧区域，沿期望路径方向左侧为外侧区域；

根据无人船当前位置坐标，判定判断无人船所在区域。

具体地，计算无人船的目标法向角的步骤，包括：

定义当前时刻无人船的目标点为N_i；

此时无人船的目标切向角为α_i；

根据无人船所在区域，若无人船在内侧区域，则无人船的目标法向角为α_i+90°；

若无人船在外侧区域，则无人船的目标法向角为α_i-90°。

具体地，计算无人船的控制方向角的步骤，包括：

根据目标方向角和目标法向角，加权平均得到无人船的控制方向角。

具体地，计算无人船的转弯控制量的步骤，包括：

根据无人船的控制方向角，与船实际的方向角做差得到误差方向角ε；误差方向角通过PID控制器得到转弯控制量值dire。

具体地，未来路径曲率的预判的步骤，包括：

定义当前时刻无人船的目标点为N_i；

向后搜索k个点形成点队列子集

根据步骤切线方向角，计算点队列子集对应的切线角序列α_k＝{α_i,α_i+1,α_i+2......α_i+k}的峰峰值；

若峰峰值大于设定的阈值，判定为未来路径出现拐弯，反之未来路径没有拐弯。

具体地，计算无人船的前进控制量的步骤，包括：

根据无人船与期望路线的垂直距离、误差方向角和是否出现转弯，通过控制系统计算出前进控制量thrust。

具体地，将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动的步骤，包括：

将转弯控制量dire和前进控制量thrust发送给无人船的执行机构，使无人船按指令行驶。实现用户可以通过操作端，控制无人船任意行驶一段曲线路径，后台自动记录并提取所行驶的路径信息，同时可以根据无人船的实时位置自动计算出所需的控制指令，使无人船实现对任意曲线的跟踪，达到任意曲线路径自主导航的目的。

与现有技术相比，本发明提供的一种无人船任意路径的追踪控制方法，用户可以通过操作端，控制无人船任意行驶一段曲线路径，后台自动记录并提取所行驶的路径信息，同时可以根据无人船的实时位置自动计算出所需的控制指令，使无人船实现对任意曲线的跟踪，达到任意曲线路径自主导航的目的。

上述仅以实施例来进一步说明本发明的技术内容，以便于读者更容易理解，但不代表本发明的实施方式仅限于此，任何依本发明所做的技术延伸或再创造，均受本发明的保护。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种无人船任意路径的追踪控制方法，其特征在于，包括以下步骤：

遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径；

对期望路径进行滤波处理，得到最终路径信息；

对最终路径信息计算得到控制信息；

根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息；

将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动；

所述对最终路径信息计算得到控制信息的步骤，包括：

计算无人船路径切线方向角，任选路径点队列中的相邻两点N_i和N_i+1，点Ni处的切线方向角等于点N_i和N_i+1所在直线的方向角α_i，形成切线方向角序列α＝{α₁,α₂,α₃......α_n}；

计算无人船目标方向角，在点队列中向后搜索m个点，点N_i处的目标方向角为这(m+1)个点的切线方向角的加权平均，形成目标方向角序列β＝{β₁,β₂,β₃......β_n}；

所述计算无人船的目标法向角的步骤，包括：

定义当前时刻无人船的目标位置点为N_i，此时无人船的切线方向角为α_i；

根据无人船所在区域，若无人船在内侧区域，则无人船的目标法向角为α_i+90°；

若无人船在外侧区域，则无人船的目标法向角为α_i-90°；所述计算无人船的控制方向角的步骤，包括：

根据无人船的目标方向角和目标法向角，将目标方向角和目标法向角加权平均得到无人船的控制方向角；所述计算无人船的转弯控制量的步骤，包括：

根据无人船的控制方向角，与无人船实际的方向角做差得到误差方向角ε；

根据误差方向角ε并通过PID控制器得到转弯控制量值dire。

2.根据权利要求1所述的无人船任意路径的追踪控制方法，其特征在于，所述遥控无人船行驶任意一段路径，并自动存储该段路径的坐标点作为期望路径的步骤，包括：

每隔一定时间依次记录储存无人船当前位置点N_i的位置坐标，形成期望路径的点队列

3.根据权利要求2所述的无人船任意路径的追踪控制方法，其特征在于，所述对期望路径进行滤波处理，得到最终路径信息的步骤，包括：

删去期望路径中的重复点，形成路径点队列

删去路径点队列中的临近点，形成路径点队列/>

检测路径点队列有效性，删除偏离点，将剩下的路径点组成路径点队列

用插值的方法进行无人船路径点补充，形成路径点队列

4.根据权利要求1所述的无人船任意路径的追踪控制方法，其特征在于，所述根据控制信息和无人船当前位置坐标，计算得到无人船实际运动信息的步骤，包括：

更新无人船的目标点，在点队列中向后搜索r个点形成点队列子集计算无人船与点队列子集/>中每个点的距离，将距离最近的点作为无人船的目标点；

计算无人船与期望路径的垂直距离；

判断无人船偏离期望路径的区域；

计算无人船的目标法向角；

计算无人船的控制方向角；

计算无人船的转弯控制量；

预判无人船未来路径曲率；

计算无人船的前进控制量。

5.根据权利要求4所述的无人船任意路径的追踪控制方法，其特征在于，所述计算无人船与期望路径的垂直距离的步骤，包括：

定义当前时刻无人船的目标位置点为Ni；

计算无人船当前位置点Ni和前一个位置点Ni-1相连所成直线的垂直距离，并将计算出的距离作为无人船与期望路径的垂直距离d。

6.根据权利要求5所述的无人船任意路径的追踪控制方法，其特征在于，所述判断无人船偏离期望路径的区域步骤，包括：

定义沿期望路径方向右侧为内侧区域，沿期望路径方向左侧为外侧区域；

根据无人船当前位置坐标，判定判断无人船所在区域。

7.根据权利要求4所述的无人船任意路径的追踪控制方法，其特征在于，所述预判无人船未来路径曲率的步骤，包括：

定义当前时刻无人船的目标位置点为N_i；

向后搜索k个点形成无人船点队列子集

根据无人船切线方向角，计算点队列子集对应的切线角序列α_k＝{α_i,α_i+1,α_i+2......α_i+k}的峰峰值；

若峰峰值大于设定的阈值，判定为无人船未来路径出现拐弯；

若峰峰值小于或等于设定的阈值，判定为无人船未来路径没有拐弯。

8.根据权利要求7所述的无人船任意路径的追踪控制方法，其特征在于，所述计算无人船的前进控制量的步骤，包括：

根据无人船与期望路径的垂直距离、误差方向角和是否出现转弯，通过控制系统计算出无人船前进控制量thrust。

9.根据权利要求8所述的无人船任意路径的追踪控制方法，其特征在于，所述将无人船实际运动信息发送至无人船，控制无人船按照期望路径运动的步骤，包括：

将无人船转弯控制量dire和前进控制量thrust发送给无人船的执行机构，使无人船按指令行驶。