基于多策略人工蜂群算法在海洋环境下的路径规划方法

专利主权项内容

1.基于多策略人工蜂群算法在海洋环境下的路径规划方法，其特征在于：所述方法具体过程为：步骤1：根据UUV所处海洋环境对洋流流场进行建模，得到洋流的流向与大小信息；根据UUV所处海洋环境，获取障碍物的数量和位置信息；所述UUV为水下无人航行器；步骤2：基于步骤1在满足UUV航迹不同情况的角度约束下，得到连接航迹起点S与航迹终点T的路径，计算路径的代价；步骤3：利用多策略人工蜂群算法对步骤2的路径的代价最小值寻优问题进行求解，输出最优路径；所述步骤1中根据UUV所处海洋环境对洋流流场进行建模，得到洋流的流向与大小信息；根据UUV所处海洋环境，获取障碍物的数量和位置信息；具体过程为：步骤1.1：根据UUV所处海洋环境，利用粘性Lamb涡流叠加的方式对洋流流场进行建模，进而得到洋流的流向与大小信息；单个Lamb涡流数学表达式为：式中：u(r)表示大地坐标系下洋流在X轴方向上的速度大小分量，v(r)表示大地坐标系下洋流在Y轴方向上的速度大小分量，w(r)表示大地坐标系下洋流在Z轴方向上的速度大小分量，r表示UUV质心所在位置即当前UUV的准确位置，k代表洋流的强度大小，r表示洋流半径，r表示洋流中心点坐标，(x, y)表示UUV航迹在大地坐标系XOY下的位置坐标，(x, y)表示洋流在大地坐标系XOY下的位置坐标；oc000步骤1.2：根据UUV所处海洋环境，获取障碍物的数量N和位置信息，将障碍物膨胀为圆以确保UUV航行安全，障碍物通过障碍物中心位置坐标(x, y)和障碍物半径大小r来表征，通过下式表示iiob其中，r为第i个障碍物的半径，(x, y)为UUV航迹在大地坐标系XOY下的位置坐标，(x, y)为大地坐标系XOY下障碍物中心位置坐标；obiii所述步骤2中基于步骤1在满足UUV航迹不同情况的角度约束下，得到连接航迹起点S与航迹终点T的路径，计算路径的代价；具体过程为：步骤2.1：进行坐标变换，将二维路径规划问题的搜索维度降为1维；具体过程为：已知航迹起点S、航迹终点T，UUV从航迹起点S出发，到航迹终点T结束；在大地坐标系XOY下，航迹起点S位置坐标为(x, y)，航迹终点位置坐标为(x, y)；ssTT连接航迹起点S与航迹终点T，直线ST上点的位置坐标为(x, y)；直线ST与大地坐标系X轴夹角为θ；在大地坐标系XOY下，构建新坐标系X′O′Y′，航迹起点S作为新坐标系原点O′，直线ST作为新坐标系X′轴，在原点o′处做垂直X′轴的直线作为新坐标系的Y′轴；在新坐标系X′O′Y′下直线ST上点的位置坐标为(x′, y′)；大地坐标系XOY和新坐标系X′O′Y′的转换关系为：步骤2.2：完成坐标变换后，在新坐标系X′O′Y′下等分直线ST为D段小线段，获得S-1段、1-2段、…、(D-1)-D段，D为航迹终点T；然后在连接等分小线段的等分节点1、2、…、D-1处分别做直线ST的垂线L、L、…、L；12D-1在垂线L、L、…、L上分别随机取一个控制点，得到控制点P、P、…、P，连接航迹起点S与控制点P得到SP，连接控制点P与控制点P得到PP，直至连接航迹终点T；12D-112D-1111212步骤2.3：对步骤2.2得到的UUV航迹(SP, PP, ..., PT)进行优化，使UUV航迹满足不同情况的角度约束；具体过程为：112D-11)、当UUV期望角度在航迹终点T处的y′轴的左侧时UUV没有转角限制，不再额外增加路径节点以满足UUV航向角度限制；2)、当UUV期望角度在航迹终点T处的y′轴时增加一条路径节点以满足UUV航向角度限制；所述增加一条路径节点指在航迹终点T处做直线ST的垂线L，在垂线L上随机取一个控制点P；TTT3)、当UUV期望角度在航迹终点T处的y′轴的右侧时，且UUV期望角度距航迹终点T处的y′轴小于等于45°时，增加两条路径节点以满足UUV航向角度限制；所述增加两条路径节点指：在航迹终点T处做直线ST的垂线L；在S到T方向的延长线上取节点T+1，在节点T+1处做直线ST的垂线L；TT+1所述节点T+1与航迹终点T相邻；所述航迹终点T与节点T+1的距离与航迹起点S到节点1距离相等；在垂线L上随机取一个控制点P，在垂线L上随机取一个控制点P；TTT+1T+14)、当UUV期望角度在航迹终点T处的y′轴的右侧时，且UUV期望角度距航迹终点T处的y′轴大于45°时，增加三条路径节点以满足UUV航向角度限制；所述增加三条路径节点指：在航迹终点T处做直线ST的垂线L；在S到T方向的延长线上取节点T+1，在节点T+1处做直线ST的垂线L；在S到T+1方向的延长线上取节点T+2，在节点T+2处做直线ST的垂线L；TT+1T+2所述节点T+2与航迹终点T+1相邻，节点T+1与航迹终点T相邻；所述节点T+2与节点T+1的距离与航迹起点S到节点1距离相等；在垂线L上随机取一个控制点P，在垂线L上随机取一个控制点P，在垂线L上随机取一个控制点P；TTT+1T+1T+2T+2步骤2.4：将步骤2.3得到的UUV航迹以期望角度进入目标点；具体过程为：针对步骤2.3的1)的情况，控制点P到航迹终点T前先到控制点P′，再由控制点P′到航迹终点T；D-2针对步骤2.3的2)的情况，控制点P到航迹终点T前先到控制点P′，再由控制点P′到航迹终点T；T针对步骤2.3的3)的情况，控制点P到航迹终点T前先到控制点P′，再由控制点P′到航迹终点T；T+1针对步骤2.3的4)的情况，控制点P到航迹终点T前先到控制点P′，再由控制点P′到航迹终点T；T+2其中航迹终点T前最后一个控制点P′的坐标(x, y)按照下式选取；pp其中x为航迹终点T横坐标，y为航迹终点T纵坐标；endendθ为P′T连线与大地坐标系XOY的X轴夹角；expdis为控制点P′的横坐标和航迹终点T横坐标x之间的偏移量；L为垂线间的距离，D为小线段个数；enddis步骤2.5：大地坐标系XOY下连接所有控制点形成连接航迹起点S与航迹终点T的路径；步骤2.6：采用三次准均匀B样条曲线对步骤2.5中路径进行平滑处理，得到平滑后的路径；步骤2.7：计算步骤2.6得到的平滑后的路径的代价。