[发明专利]基于改进栅格法和蚁群算法的USV避碰规划方法

说明书

本申请提供一种改进栅格法和蚁群算法的USV避碰规划方法，包括以下步骤：构建全局坐标系、船体坐标系和导航器坐标系；设计实时避碰规划的优化窗口；采用栅格法构建障碍模型；采用改进蚁群算法进行避碰规划，其中蚂蚁对于下一路径的选取采用基于双矢量角的状态转移规则。本申请提供的一种改进栅格法和蚁群算法的USV避碰规划方法通过确定障碍顶点中的有效顶点，对障碍物进行有效识别，提高识别效率和准确性，同时能克服传统蚁群算法收敛慢的问题，避免规划陷入局部最优。

技术领域

本公开一般涉及USV避碰规划技术领域，具体涉及基于改进栅格法和蚁群算法的USV避碰规划方法。

背景技术

USV作为海上的只能化工具，USV避碰规划既是自动化的重要标识，也是自主航行的核心，所以USV完成任务的前提是其能够智能化避碰。传统避碰规划方法包括蚁群、粒子群、遗传优化算法，人工势场法，深度学习方法等等。其中蚁群算法由于具有较好的路径规划特点被广泛应用在USV避碰规划中，但是传统的对未知障碍模型进行构建的方法存在障碍物不能被有效识别的缺陷，容易对USV的安全航行造成影响；同时传统蚁群算法存在收敛慢，容易使规划陷入局部最优的问题。

发明内容

鉴于现有技术中的上述缺陷或不足，期望提供一种基于改进栅格法和蚁群算法的USV避碰规划方法以解决上述问题。

本申请提供一种基于改进栅格法和蚁群算法的USV避碰规划方法，包括以下步骤：

S100、构建全局坐标系、船体坐标系和导航器坐标系：

针对USV航行的海图环境建立全局坐标系，以USV为原点USV 的航行方向为X轴建立直角坐标系作为船体坐标系，以导航器为极点， USV的航行方向为极轴建立极坐标系作为导航器坐标系；

S200、设计实时避碰规划的优化窗口：

建立随USV同航行的优化窗口，所述优化窗口的大小设置为导航器的探测距离，每隔设定时间对所述优化窗口进行一次更新，同时对 USV进行一次动态规划；

S300、采用栅格法构建障碍模型：

构造障碍顶点，基于单个栅格，只要其包含障碍物，则将其视为充满障碍物，确定有效顶点，所述有效顶点所在的四个栅格中，最多只有一个栅格包括障碍，将障碍边界膨化成USV长度的一半；

S400、采用改进蚁群算法进行避碰规划；

其中蚂蚁对于下一路径的选取采用基于双矢量角的状态转移规则。

根据本申请实施例提供的技术方案，改进蚁群算法具体包括：

S401、使用基于栅格法统计包括起点和终点在内的所有已经经过膨化的障碍物的有效顶点的可视点，每一个顶点g_i都定义一个allowed_i链表存储其可视点集；

S402、初始化τ_ij(0)，设置历史最优保留代数计数器count＝0、进化代数计数器G_count＝0、历史最优最大保留代数Max、最大进化代数 Generation、初始历史最优蚂蚁history_best的路径代价∞、每代蚂蚁的种群数m；

S403、设置蚂蚁种群编号k＝1；

S404、若km，转到S407，否则把蚂蚁k放置在起始位置g_s上，转到S405；

S405、设此时蚂蚁当前位置为g_i，若g_i的可视点链表allowed_i为空，则这只蚂蚁死亡，k＝k+1，转到S404；