基于快速移动萤火虫算法的无人船舶局部路径规划方法与流程
本发明涉及智能无人船舶航行控制领域，尤其涉及一种基于快速移动萤火虫算法的无人船舶局部路径规划方法。
背景技术：
智能无人船舶融合了通信、自动化、机器人控制、远程监控、网络化系统等技术，能够实现自主导航、智能避障等功能，保证了无人船舶的自主航行；与有人驾驶船相比智能无人船舶具有安全系数高，经济环保，绿色节能的优点。
然而，现有的智能无人船舶在实现自主航行过程中需要进行路径规划，针对大型无人船舶的路径规划，业内技术人员提出了使用粒子群算法、蚁群算法、遗传算法等多种仿生智能算法进行船舶路径的规划。
进一步地，为更好的实现对船舶路径的规划，业内人士提出使用萤火虫算法实现无人船舶的自主导航，而在实际中，将传统萤火虫算法与无人船舶路径规划相结合存在许多问题，例如：(1)萤火虫算法在初始化阶段针对的是整个环境空间进行初始化，导致获取的路径点较分散；(2)萤火虫算法针对大范围搜索寻优时收敛速度慢；(3)应用萤火虫算法不能综合考虑海洋环境的复杂性、船舶操纵性能及能耗，缺乏实际航海应用的可行性。
技术实现要素：
(一)要解决的技术问题
为了解决现有技术中传统萤火虫算法搜索范围大、收敛速度慢，且规划出的路径实用性差的问题，本发明提供一种基于快速移动萤火虫算法的无人船舶局部路径规划方法。
(二)技术方案
为了达到上述目的，本发明采用的主要技术方案包括：
s1、根据电子海图获取不可航行区域s和目标点；
s2、获取无人船舶当前位置(x0,y0)，利用所述无人船舶当前位置(x0,y0)和目标点获取预测路径点(x1,y1)；
s3、判断预测路径点(x1,y1)是否属于不可航行区域s和所述预测路径点(x1,y1)是否为目标点，若所述预测路径点(x1,y1)不属于不可航行区域s且不是目标点，则无人船舶行驶至预测路径点，并返回步骤s2；
s4、若步骤s3中所述预测路径点(x1,y1)属于不可航行区域s且不是目标点，则初始化迭代次数k等于1，并利用预设范围限定参数θ随机生成预设值n_fires个待规划路径点，获取引导每一待规划路径点向接近目标点且远离不可航行区域s移动的目标函数值lightn(i)，将目标函数值lightn(i)最大值对应的待规划路径点作为引导路径点(xl_max,yl_max)；
s5、利用前一次迭代确定引导路径点与每一待规划路径点的欧式距离d更新每一待规划路径点，将迭代次数k增加预设值1并采用步骤s4的方式获取更新迭代后的引导路径点；
s6、直至迭代次数k等于预设最大迭代次数gen，将最大迭代次数gen对应的引导路径点作为最终的引导路径点；
s7、无人船舶行驶至最终的引导路径点并返回步骤s2。
可选地，在步骤s1前还包括：
s0、预先初始化快速移动萤火虫算法的参数，包括：萤火虫的种群大小n_fires，避碰系数k1，最短路径系数k2，最大迭代次数gen、步长参数α，最大吸引力β0和光强吸收率γ。
可选地，在步骤s1中，包括获取障碍物中心位置坐标集合obstacles、障碍物个数size_obstacles，每个障碍物的半径、无人船舶的船长l和目标点，并利用所述障碍物中心位置坐标集合obstacles、每个障碍物的半径和无人船舶的船长l获取不可航行区域s。
可选地，在步骤s2中获取预测路径点(x1,y1)包括：
s21、根据所述无人船舶当前位置坐标(x0,y0)和目标点利用公式一获取夹角
公式一：
s22、根据预设移动步长l、所述夹角
公式二：
公式三：
其中，x0和y0分别为无人船舶当前位置的横坐标和纵坐标，targetx和targety分别为目标点的横坐标和纵坐标。
可选地，在步骤s4中包括：
根据所述无人船舶当前位置(x0,y0)为中心，利用公式四和公式五分别获取每一初始化的待规划路径点的横坐标xn(i)和纵坐标yn(i)；
公式四：xn(i)＝-2*rand*l+l+x0+l*cos(θ)；
公式五：yn(i)＝-2*rand*l+l+y0+l*sin(θ)；
其中，rand为0～1之间的随机数，l为预设移动步长，θ为预设范围限定参数，x0和y0分别为无人船舶当前位置的横坐标和纵坐标，1≤i≤n_fires。
可选地，在步骤s4中获取目标函数值lightn(i)包括：
利用公式六获取每个待规划路径点与每个障碍物距离的第一目标函数值dfo(i,j)，利用公式七获取每个待规划路径点与目标点距离的第二目标函数值dfg(i)，利用公式八至公式十获取引导无人船舶接近目标点且远离不可航行区域s的第三目标函数值lightn(i)；
公式六：
公式七：
公式八：
公式九：
公式十：
其中，xn(i)和yn(i)为待规划路径点的横坐标和纵坐标，obstaclesx(j)和obstaclesy(j)分别为障碍的横坐标和纵坐标，targetx和targety分别为目标点的横坐标和纵坐标，min_dfo为第一目标函数值dfo(i,j)中的最小值，k1为预设避碰系数，k2为预设最短路径系数，i为路径点的索引，j为障碍物的索引，1≤j≤size_obstacles，size_obstacles为障碍物个数。
可选地，在步骤s5中包括利用公式十一至公式十三更新每一待规划路径点的横坐标xn'(i)和纵坐标yn'(i)；
公式十一：
公式十二：xn'(i)＝xn(i)×(1-β×d)+xl_max×β×d+d×α×(rand-0.5)；
公式十三：yn'(i)＝yn(i)×(1-β×d)+yl_max×β×d+d×α×(rand-0.5)；
其中，xn(i)和yn(i)分别为待规划路径点横坐标和纵坐标，xl_max和yl_max分别为引导路径点的横坐标和纵坐标，d为引导路径点与待规划路径点的欧式距离，γ为预设光强吸收率，β0为预设光源最大吸引力，α为预设步长参数，rand为0～1之间的随机数。
(三)有益效果
本发明的有益效果是：本发明的方法中使用快速移动萤火虫算法在初始化随机路径点时限定了范围，避免了出现大跨度路径点，有效的提高了传统萤火虫算法的收敛速度，进而节省了计算时间；
此外，本发明中采用目标函数能够自适应调整避障系数和最短路径系数，适宜海上复杂的环境状况，且规划的路径适宜无人船舶实际操作且减少无人船舶的能源消耗，具有较佳的实用性；
进一步地，在本发明应用中，还在结合了无人船舶的船身长度，由此，可以应用大型无人船舶。
附图说明
图1为本发明一实施例提供的一种基于快速移动萤火虫算法的无人船舶局部路径规划方法流程图；
图2为本发明一实施例提供的采用快速移动萤火虫算法规划的无人船舶仿真航行路径。
具体实施方式
为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。
如图1所示，本实施例提供了一种基于快速移动萤火虫算法的无人船舶局部路径规划方法，具体包括以下步骤：
s0、预先初始化快速移动萤火虫算法的参数，包括：萤火虫的种群大小n_fires，避碰系数k1，最短路径系数k2，最大迭代次数gen、步长参数α，具体的举例来说，α为变的常数；最大吸引力β0和光强吸收率γ。
s1、根据电子海图获取不可航行区域s和目标点；
具体地，在本实施例中步骤s1中，包括获取障碍物中心位置坐标集合obstacles、障碍物个数size_obstacles，每个障碍物的半径、无人船舶的船长l和目标点，并利用所述障碍物中心位置坐标集合obstacles、每个障碍物的半径和无人船舶的船长l获取不可航行区域s；在本实施例中通过电子海图获取数据使无人船舶可以快速获取环境信息；其次，在获取不可航行区域时不仅考虑了障碍物的大小，还考虑了无人船舶的船身长度，使规划的路径适宜无人船舶的实际操作。
s2、获取无人船舶当前位置(x0,y0)，利用所述无人船舶当前位置(x0,y0)和目标点获取预测路径点(x1,y1)；
举例来说，如图2所示，在本实施例中获取预测路径点(x1,y1)包括以下步骤：
s21、根据所述无人船舶当前位置坐标(x0,y0)和目标点利用公式1获取夹角
公式1：
s22、根据预设移动步长l、所述夹角
公式2：
公式3：
其中，x0和y0分别为无人船舶当前位置的横坐标和纵坐标，targetx和targety分别为目标点的横坐标和纵坐标。
s3、判断预测路径点(x1,y1)是否属于不可航行区域s和所述预测路径点(x1,y1)是否为目标点，若所述预测路径点(x1,y1)不属于不可航行区域s且不是目标点，则无人船舶行驶至预测路径点，并返回步骤s2；具体的举例来说，在本实施例中没有障碍物时沿当前位置与目标点方向行驶，能够获得最短路径。
s4、若步骤s3中所述预测路径点(x1,y1)属于不可航行区域s且不是目标点，则初始化迭代次数k等于1，并利用预设范围限定参数θ随机生成预设值n_fires个待规划路径点，获取引导每一待规划路径点向接近目标点且远离不可航行区域s移动的目标函数值lightn(i)，将目标函数值lightn(i)最大值对应的待规划路径点作为引导路径点(xl_max,yl_max)；
在本实施例中存在障碍物时则启动快速移动萤火虫算法避开障碍物，且限制了随机生成的待规划路径点的局部位置，由此生成的路径不会存在较大偏离，且避免了存在大跨度路径而造成不必要的能源消耗。
举例来说，本实施例根据所述无人船舶当前位置(x0,y0)为中心，利用公式4和公式5分别获取每一初始化的待规划路径点的横坐标xn(i)和纵坐标yn(i)；
公式4：xn(i)＝-2*rand*l+l+x0+l*cos(θ)；
公式5：yn(i)＝-2*rand*l+l+y0+l*sin(θ)；
其中，rand为0～1之间的随机数，l为预设移动步长，θ为预设范围限定参数，x0和y0分别为无人船舶当前位置的横坐标和纵坐标，1≤i≤n_fires。
举例来说，在步骤s4中获取目标函数值lightn(i)包括：
利用公式6获取每个待规划路径点与每个障碍物距离的第一目标函数值dfo(i,j)，利用公式7获取每个待规划路径点与目标点距离的第二目标函数值dfg(i)，利用公式8至公式10获取引导无人船舶接近目标点且远离不可航行区域s的第三目标函数值lightn(i)；
公式6：
公式7：
公式8：
公式9：
公式10：
其中，xn(i)和yn(i)为待规划路径点的横坐标和纵坐标，obstaclesx(j)和obstaclesy(j)分别为障碍的横坐标和纵坐标，targetx和targety分别为目标点的横坐标和纵坐标，min_dfo为第一目标函数值dfo(i,j)中的最小值，k1为预设避碰系数，k2为预设最短路径系数，i为路径点的索引，j为障碍物的索引，1≤j≤size_obstacles，size_obstacles为障碍物个数。
s5、利用前一次迭代确定引导路径点与每一待规划路径点的欧式距离d更新每一待规划路径点，将迭代次数k增加预设值1并采用步骤s4的方式获取更新迭代后的引导路径点；在具体实施过程中，步骤s4中利用目标函数值更新待规划点的方式多次迭代，并在每次迭代后都获取一个引导路径点以引导无人船舶向靠近目标点，且向远离不可航行区域的位置靠近。
举例来说，在步骤s5中包括利用公式11至公式13更新每一待规划路径点的横坐标xn'(i)和纵坐标yn'(i)；
公式11：
公式12：xn'(i)＝xn(i)×(1-β×d)+xl_max×β×d+d×α×(rand-0.5)；
公式13：yn'(i)＝yn(i)×(1-β×d)+yl_max×β×d+d×α×(rand-0.5)；
其中，xn(i)和yn(i)分别为待规划路径点横坐标和纵坐标，xl_max和yl_max分别为引导路径点的横坐标和纵坐标，d为引导路径点与待规划路径点的欧式距离，γ为预设光强吸收率，β0为预设光源最大吸引力，α为预设步长参数，rand为0～1之间的随机数。
s6、依次采用步骤s5的方式获取引导路径点，直至迭代次数k等于预设最大迭代次数gen，将最大迭代次数gen对应的引导路径点作为最终的引导路径点；
s7、无人船舶行驶至最终的引导路径点并返回步骤s2。
一方面使用上述位置更新公式和目标函数提高了快速移动萤火虫算法的收敛速度，减少优化计算的时间；另一方面通过调整快速移动萤火虫算法中的避障系数和最短路径系数能够自适应变化，适宜海上复杂的环境变化并保证无人船舶稳定高效航行。
最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
- 还没有人留言评论。精彩留言会获得点赞！