[发明专利]考虑海流与无人艇动力学影响的节能A星路径规划方法

摘要

本发明提出了考虑海流与无人艇动力学影响的节能A星路径规划方法，步骤包括：(1)获取全局海图信息并网格化；(2)获取无人艇的起点和终点位置信息；(3)将当前位置设为起点位置，创建OPEN和CLOSD表；(4)计算在海流影响下的无人艇航速堆；(5)将当前位置存入OPEN表；(6)依次判断当前位置无人艇是否可以向周围八个方向行驶等。本发明在传统A星路径规划算法的基础上，结合海流影响下的无人艇动力学模型，设计考虑海流影响的能耗启发函数E_heurstic，并通过调整该函数的权值，实现对算法节能效率的动态调节，为无人艇在海面上长时间工作提供技术支持。

主权项

1.考虑海流与无人艇动力学影响的节能A星路径规划方法，具体包括如下步骤：(1)获取全局海图信息和全局海流信息，设置网格化分辨率n并将海图和海流图网格化，形成n*n的网格海图和网格海流图；(2)将网格海图中的障碍值设为无限大Inf，其他设为1，网格海流图中包含了每个单位网格内海流的流速Ucurrent(x,y)和流向Dcurrent(x,y)；(3)获取无人艇的起点位置(xstart,ystart)和终点位置(xgoal,ygoal)信息，并令无人艇当前位置(xusv，yusv)等于(xstart,ystart)，创建OPEN表和CLOSED表；(4)U_stack_usv从最下层到最上层依次代表n*n地图上每个单位网格无人艇在海流作用下向E、W、S、N、SE、SW、NW和NE八个方向行驶时需要发动机提供的无人艇速度；计算在海流作用下的无人艇速度堆U_stack_usv；(5)将(xusv，yusv)存入OPEN表；(6)判断无人艇当前位置(xusv,yusv)的周围第i个考察位置(xi,yi)是否为Inf，是则转步骤(7)，否则转步骤(8)；(7)判断i是否等于8，是则转步骤(18)，否则令i＝i+1并返回步骤(6)；(8)根据(xusv，yusv)和(xi,yi)的相对位置计算出无人艇的行驶方向f_c和距离d_c；(9)根据f_c和(xusv，yusv)选择U_stack_usv中所对应的无人艇速度U_c；(10)根据U_c，Uground和d_c，利用海流作用下的无人艇动力学模型计算出无人艇从(xusv，yusv)到(xi,yi)所消耗的能量e_cost(i)；(11)令sofar_cost＝e_cost(i)+sofar_cost，sofar_cost用来保存用起点(xstart,ystart)到(xi,yi)所需的能量，初始值为0；(12)调用启发函数E_heurstic，估算从(xi,yi)到(xgoal,ygoal)需要消耗的能量e_heurstic(i)；(13)判断(xi,yi)是否既不属于CLOSED表又不属于OPEN表，是则将(xi,yi)加入OPEN表，即令OPEN(xi,yi)(1)等于sofar_cost，OPEN(xi,yi)(2)等于e_heurstic(i)并转步骤(17)，否则转步骤(14)；(14)判断(xi,yi)是否已经属于OPEN表，是则转步骤(15)，否则转步骤(16)；(15)判断sofar_cost是否小于OPEN(xi,yi)(1)，是则用(xi,yi)更新OPEN(xi,yi)中对应的值，转步骤(17)；(16)判断sofar_cost是否小于CLOSED(xi,yi)，是则令CLOSED(xi,yi)等于sofar_cost，转步骤(17)；(17)计算e_function(i)＝α·costchart(xi,yi)+β·e_heurstic(i)，其中α,β分别代表代价值和启发值的权重参数，α＞＞β时，节能量越高但规划速度越低；反之规划速度越快但节能量越低，转步骤(7)；(18)将无人艇当前位置(xusv，yusv)从OPEN表中删除，加入CLOSED表；(19)取{e_function}中的最小值所对应的(xi,yi)，令(xusv，yusv)等于(xi,yi)；(20)判断(xusv，yusv)是否等于(xgoal,ygoal)，是则抵达终点规划结束，否则返回步骤(5)。