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

权利要求书

1.基于改进栅格法和蚁群算法的USV避碰规划方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

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

2.根据权利要求1所述的基于改进栅格法的蚁群算法的USV避碰规划方法，其特征在于，改进蚁群算法具体包括：

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；

S406、若蚂蚁当前位置为终点g_e，则蚂蚁找到完整路径，k＝k+1，转到S404，否则，将本路径点从allowed_i中删除，以防止蚂蚁向回搜索，并基于双矢量角的状态转移规则寻找下一路径点，转到S405；

S407、进化代数计数器G_count＝G_count+1，若本代蚂蚁中最优蚂蚁比历史最优蚂蚁找到的路径更优，更新初始历史最优蚂蚁history_best并且设置历史最优保留代数计数器count＝0，否则，历史最优保留代数计数器count＝count+1；

S408、历史最优保留代数计数器count历史最优最大保留代数Max或者进化代数计数器G_count≥最大进化代数Generation，优化方法停止，否则对蚁群信息素进行全局更新处理，转到S403。

S409、将导航器探测信息及目标点信息输入至基于改进蚁群算法的USV静态避碰规划器，获得USV下一时刻的转艏及速度的调整命令。

3.根据权利要求2所述的基于改进栅格法和蚁群算法的USV避碰规划方法，其特征在于，所述优化窗口包括以下特点：

每次规划，将所有已知障碍信息纳入规划中，为算法增加记忆；

规划采用未知且新发现的障碍触发机制。

4.根据权利要求3所述的基于改进栅格法和蚁群算法的USV避碰规划方法，其特征在于，栅格法的障碍顶点构造具体为：

将障碍顶点分为有效顶点和无效顶点，其中有效顶点满足以下特点：

海图环境边界上的障碍顶点不是有效顶点；

有效顶点所在的四个栅格中，最多只包含一个障碍，即每个有效顶点最多对应一个障碍。

5.根据权利要求4所述的基于改进栅格法和蚁群算法的USV避碰规划方法，其特征在于，将所述有效顶点分为确定性顶点和不确定性顶点，若障碍较小，有效顶点位于当前优化窗口内可由导航器直接确认，改有效顶点定义为确定性顶点；若障碍较大，导航器探测不到障碍的全部，则障碍模型与当前优化窗口边界的交点，该交点定义为不确定性顶点。