[发明专利]未知静态障碍环境下USV基于改进蚁群优化的避碰规划方法

权利要求书

1.未知静态障碍环境下USV基于改进蚁群优化的避碰规划方法，其特征在于，包括：

(1)构建全局坐标系和局部坐标系，建立导航雷达仿真模型；

全局坐标系采用北东坐标系，地图左上角为原点，正东方向为X轴，正北方向为Y轴；局部坐标系分为艇载坐标系和传感器坐标系，其中艇载坐标系是以USV为原点所建立的直角坐标系，传感器坐标系是以导航雷达为极点、USV前进方向作为极轴所建立的极坐标系；

(2)设计用于实时避碰规划的滚动优化窗口；

滚动优化窗口法为建立一个随USV一起运动的环境窗口，每隔固定时间对USV进行一次动态避碰规划，避碰规划时仅对当前滚动窗口中的障碍物进行建模，每次避碰规划，将所有已知障碍物信息纳入避碰规划中；避碰规划采用新的未知环境信息触发机制；

(3)采用可视图法构建环境模型；

可视图法包括可视图的膨胀方法和可视图的构建方法，其中膨胀方法采用矢量面积法判断凹凸性，构建方法采用相对位置检测法判断可视性；

(4)设计用于实时避碰规划的改进蚁群优化方法；

改进蚁群优化方法具体步骤为：

(4.1)使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点g_i都定义一个allowed_i链表存储其可视点集；

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

(4.3)设置蚂蚁种群编号k＝1；

(4.4)若k＞m，转到步骤4.7，否则把蚂蚁k放置在起始位置g_s上，转到步骤(4.5)；

(4.5)设此时蚂蚁当前位置为g_i，若g_i的可视点链表allowed_i为空，则这只蚂蚁死亡，k＝k+1，转到步骤(4.4)；

(4.6)若蚂蚁当前位置为终点g_e，则蚂蚁找到完整路径，k＝k+1，转到步骤(4.4)，否则，将本路径点从allowed_i中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到步骤(4.5)；

(4.7)G_count＝G_count+1；若本代蚂蚁中的最优蚂蚁比历史最优蚂蚁找到的路径更优，更新history_best并且count＝0，否则count＝count+1；

(4.8)count＞Max或G_count≥Generation，优化方法停止；否则对蚁群信息素进行全局更新处理，转到步骤(4.3)；

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

2.根据权利要求1所述的未知静态障碍环境下USV基于改进蚁群优化的避碰规划方法，其特征在于：所述步骤(1)中导航雷达仿真模型探测距离为50m～10km，探测范围为0°～360°，方位分辨率为5°，角度测量精度为1°。