一种基于导航误差约束的水面无人艇局部分层路径规划方法技术

本发明专利技术公开了一种基于导航误差约束的水面无人艇局部分层路径规划方法，主要步骤包括：基于导航误差约束并结合遗传算法的局部静态路径规划；融合海事规则的局部动态路径规划。本发明专利技术采用了分层思想，将局部路径规划划分为静态路径规划和动态路径规划两个层面，即解决了静态障碍物规避问题，又解决了动态障碍物规避问题；基于导航误差约束并结合遗传算法的静态路径规划减少了导航误差对路径选择造成的不利影响，提高了规划路径的安全性；融合海事规则的动态路径规划，考虑了水面无人艇自身的动力学约束，提高了规划路径的可行性。

【技术实现步骤摘要】
一种基于导航误差约束的水面无人艇局部分层路径规划方法
本专利技术涉及一种基于导航误差约束的水面无人艇局部分层路径规划方法，属于水面无人艇路径规划

技术介绍
随着科技的飞速发展，海上智能交通作为我国科技战略的重要组成部分，主要用于实现船舶航行的自动化和海上交通管理的智能化。因此，水面无人艇作为海上交通系统的个体船舶，对其航行路径的研究是很有必要的。路径规划是水面无人艇的一个重要研究领域，是水面无人艇能够安全完成巡航任务的前提。近年来，水面无人艇路径规划已经成为海上智能化的研究热点。路径规划的方法有很多，因自身的优缺点而适用于各种不同的情况，大致可分为传统算法、图形法、智能仿生学算法以及其他算法等。传统算法有人工势场法、模糊逻辑算法和模拟退火算法等；图形法有可视图空间法、栅格法和voronoi图法等；智能仿生学算法有蚁群算法、神经网络算法、遗传算法和粒子群算法等；其他算法有A*算法、Dijkstra算法和Floyd算法等。目前，绝大多数路径规划只划分为全局路径规划和局部路径规划两个层面，未考虑局部路径规划的分层问题，具有一定的局限性。此外，绝大多数静态路径规划均是假设在导航信息精确的前提下，水面无人艇能够精准地跟踪规划好的路径。但实际导航系统中存在误差，将会导致水面无人艇与实际障碍物之间的安全区域减少，会造成初始点到目标点的自由路径不存在。即使自由路径仍存在，也很有可能给水面无人艇带来如碰撞等不可预知的风险。因此，在路径规划过程中，导航误差的影响是不可忽视的问题，对水面无人艇的实际工程应用具有重要意义。目前也有部分与本专利技术有关的研究报告，1、复杂海况下水面无人艇分层危险规避方法研究，哈尔滨工程大学，2014，该博士论文中虽然考虑了三个分层层面，但未考虑导航误差对路径规划的影响。2、一种基于导航误差空间的无人水下航行器路径规划方法，兵工学报，2014，35(8)：1243-1250，该文献中虽然考虑了导航误差，但只是针对水下航行器进行了全局路径规划研究，且并未结合遗传算法。
技术实现思路
技术问题：本专利技术提供一种减少导航误差对路径选择造成的不利影响的基于导航误差约束的水面无人艇局部分层路径规划方法，该方法将局部路径规划划分为基于导航误差约束并结合遗传算法的局部静态路径规划以及融合海事规则的局部动态路径规划两个层面，综合考虑导航误差约束、水面无人艇自身动力学约束等限制条件，可以提高规划路径的安全性和可行性。技术方案：本专利技术的基于导航误差约束的水面无人艇局部分层路径规划方法，包括以下步骤：步骤1)根据作业任务、电子海图和传感器得到的静态障碍物信息，采用直角坐标系和栅格法，将环境离散化为二维的基本单元栅格，通过对这些栅格的标示进行环境建模，初始化起点位置Start和目标点位置Goal；步骤2)按照如下方式得到M条染色体：采用随机法在栅格地图上随机选取除起点位置Start和目标点位置Goal外的a个自由栅格点，把起点和目标点分为a+1段，然后采用A*算法连接起点、目标点和a个自由栅格点，使之成为一个可行的初始解，即得到一条有效的染色体；将所述M条染色体作为初始种群，即为第一代种群，其中M为第一代种群的规模；步骤3)确定每条染色体的每个基因点的导航误差，具体为：利用每条染色体中第k个基因点处的期望导航位置μk与第k+1个基因点处的期望导航位置μk+1之间的航行距离D(μk，μk+1)和导航误差变化率Δε，进行乘积运算，再加上第k个基因点的导航误差εk，得到每条染色体第k+1个基因点的导航误差εk+1；然后根据北斗导航误差εBD，若第k+1个基因点的导航误差εk+1＞εBD，则将εBD赋值εk+1；否则，第k+1个基因点的导航误差εk+1为原值；步骤4)根据所述步骤3)得到每条染色体的每个基因点的导航误差，采用高斯模型作为导航误差模型，根据该导航误差模型的导航位置分布密度函数采用蒙特卡洛采样法计算得到每条染色体的路径代价L(s0，sg)，其中所有染色体路径代价中的最小值为该代种群的最优解；步骤5)将有障碍物的危险区域作为碰撞区域A，根据导航误差模型，在碰撞区域A内对导航误差概率密度函数进行积分，求出每条染色体的每个基因点处的碰撞概率，得到每条染色体的起点到目标点，即第0个基因点到第g个基因点之间的全部基因点处的碰撞概率集合{Pc(X0)，Pc(X1)...Pc(Xg)}；步骤6)根据所述步骤5)得到的每条染色体的全部基因点的碰撞概率集合，针对每条染色体，查找该染色体在碰撞概率集合{Pc(X0)，Pc(X1)...Pc(Xg)}中的碰撞概率最大值，然后将其作为该染色体的碰撞概率Pc(X)；步骤7)根据所述步骤4)得到的每条染色体的路径代价和所述步骤6)得到的每条染色体的碰撞概率Pc(X)，按照以下方式计算每条染色体的适应度；判断所述步骤6)得到的每条染色体的碰撞概率Pc(X)是否小于或等于设定值，如是，则根据下式计算每条染色体的适应度E：否则根据下式计算每条染色体的适应度E：其中m为权重值设定值，m＞＞L(s0，sg)；将所有染色体的适应度相加求平均，作为该代种群的平均适应值；步骤8)假设第n代种群代数为x(n)，若种群代数x(n)＜设定值nsetting，则进入步骤9)；否则，按照如下方式处理：若在第n-(nsetting-1)代种群到当前第n代种群的连续nsetting代进化中，种群的最优解均未发生变化，且相邻两代的种群平均适应值变化没有超过1％，则将当前第n代中具有最优解的染色体作为所求解的一条最优无碰路径，并根据组成该染色体的基因点的坐标值得到一系列从起点到目标点的路径节点序列，将该路径节点序列作为局部航路点序列后，进入步骤10)，否则进入步骤9)；步骤9)根据交叉率pc，即设定的交叉操作的概率值，随机选择pc*M条染色体进行交叉操作，对交叉的个体两两随机地交换基因点形成新的染色体，替换适应度最大的pc*M条染色体，然后再根据变异率pm，即设定的变异操作的概率值，随机选择pm*M条染色体进行变异操作，对进行变异的个体随机选择基因点进行实值变异，替换适应度最大的pm*M条染色体；将交叉变异操作获得的种群作为新的种群，即下一代(n+1代)种群代数为x(n+1)＝x(n)+1，返回步骤3)；步骤10)根据所述步骤8)得到的一系列局部航路序列点，将第一个局部航路点作为动态规划起始点，将第二个局部航路点作为动态规划目标点；步骤11)根据传感器获得水面无人艇航行过程中的动态障碍物位置信息及速度信息，求解水面无人艇与障碍物的最近会遇距离DCPA和到达最近会遇点的时间TCPA，然后根据下式，采用DCPA与TCPA加权得到碰撞危险度ρ：ρ＝(ωDDCPA)2+(ωTTCPA)2其中，ωD、ωT分别为加权值；步骤12)判断所述步骤11)得到的碰撞危险度ρ是否小于或等于经验值，如是，则根据融合海事规则的避障策略及水面无人艇的动力学约束条件，调整水面无人艇的航向角或速度大小，否则，保持水面无人艇原有的航向角和速度大小不变；步骤13)根据步骤12)得到的航向角改变量Δα、速度大小改变量ΔVUSV和设定的规划时间ΔT，计算水面无人艇规划一个ΔT时间后的当前位置(X，Y)为：式中，X、Y分别为规划一个ΔT时间后水面无人艇在直角本文档来自技高网...

【技术保护点】
一种基于导航误差约束的水面无人艇局部分层路径规划方法，其特征在于，该方法包括以下步骤：步骤1)根据作业任务、电子海图和传感器得到的静态障碍物信息，采用直角坐标系和栅格法，将环境离散化为二维的基本单元栅格，通过对这些栅格的标示进行环境建模，初始化起点位置Start和目标点位置Goal；步骤2)按照如下方式得到M条染色体：采用随机法在栅格地图上随机选取除起点位置Start和目标点位置Goal外的a个自由栅格点，把起点和目标点分为a+1段，然后采用A*算法连接起点、目标点和a个自由栅格点，使之成为一个可行的初始解，即得到一条有效的染色体；将所述M条染色体作为初始种群，即为第一代种群，其中M为第一代种群的规模；步骤3)确定每条染色体的每个基因点的导航误差，具体为：利用每条染色体中第k个基因点处的期望导航位置μ

【技术特征摘要】
1.一种基于导航误差约束的水面无人艇局部分层路径规划方法，其特征在于，该方法包括以下步骤：步骤1)根据作业任务、电子海图和传感器得到的静态障碍物信息，采用直角坐标系和栅格法，将环境离散化为二维的基本单元栅格，通过对这些栅格的标示进行环境建模，初始化起点位置Start和目标点位置Goal；步骤2)按照如下方式得到M条染色体：采用随机法在栅格地图上随机选取除起点位置Start和目标点位置Goal外的a个自由栅格点，把起点和目标点分为a+1段，然后采用A*算法连接起点、目标点和a个自由栅格点，使之成为一个可行的初始解，即得到一条有效的染色体；将所述M条染色体作为初始种群，即为第一代种群，其中M为第一代种群的规模；步骤3)确定每条染色体的每个基因点的导航误差，具体为：利用每条染色体中第k个基因点处的期望导航位置μk与第k+1个基因点处的期望导航位置μk+1之间的航行距离D(μk,μk+1)和导航误差变化率Δε，进行乘积运算，再加上第k个基因点的导航误差εk，得到每条染色体第k+1个基因点的导航误差εk+1；然后根据北斗导航误差εBD，若第k+1个基因点的导航误差εk+1>εBD，则将εBD赋值εk+1；否则，第k+1个基因点的导航误差εk+1为原值；步骤4)根据所述步骤3)得到每条染色体的每个基因点的导航误差，采用高斯模型作为导航误差模型，根据该导航误差模型的导航位置分布密度函数采用蒙特卡洛采样法计算得到每条染色体的路径代价L(s0,sg)，其中所有染色体路径代价中的最小值为该代种群的最优解；步骤5)将有障碍物的危险区域作为碰撞区域A，根据导航误差模型，在碰撞区域A内对导航误差概率密度函数进行积分，求出每条染色体的每个基因点处的碰撞概率，得到每条染色体的起点到目标点，即第0个基因点到第g个基因点之间的全部基因点处的碰撞概率集合{Pc(X0),Pc(X1)…Pc(Xg)}；步骤6)根据所述步骤5)得到的每条染色体的全部基因点的碰撞概率集合，针对每条染色体，查找该染色体在碰撞概率集合{Pc(X0),Pc(X1)…Pc(Xg)}中的碰撞概率最大值，然后将其作为该染色体的碰撞概率Pc(X)；步骤7)根据所述步骤4)得到的每条染色体的路径代价和所述步骤6)得到的每条染色体的碰撞概率Pc(X)，按照以下方式计算每条染色体的适应度；判断所述步骤6)得到的每条染色体的碰撞概率Pc(X)是否小于或等于设定值，如是，则根据下式计算每条染色体的适应度E：否则根据下式计算每条染色体的适应度E：其中m为权重值设定值，m>>L(s0,sg)；将所有染色体的适应度相加求平均，作为该代种群的平均适应值；步骤8)假设第n代种群代数为x(n)，若种群代数x(n)<设定值nsetting，则进入步骤9)；否则，按照如下方式处理：若在第n-(nsetting-1)代种群到当前第n代种群的连续nsetting代进化中，种群的最优解均未发生变化，且相邻两代的种群平均适应值变化没有超过1％，则将当前第n代中具有最优解的染色体作为所求解的一条最优无碰路径，并根据组成该染色体的基因点的坐标值得到一系列从起点到目标点的路径节点序列，将该路径节点序列作为局部航路点序列后，进入步骤10)，否则进入步骤9)；步骤9)根据交叉率pc，即设定的交叉操作的概率值，随机选择pc*M条染色体进行交叉操作，对交叉的个体两两随机地交换基因点形成新的染色体，替换适应度最大的pc*M条染色体，然后再根据变异率pm，即设定的变异操作的概率值，随机选择pm*M条染色体进行变异操作，对进行变异的个体随机选择基因点进行实值变异，替换适应度最大的pm*M条染色体；将交叉变异操作获得的种群作为新的种群，即下一代(n+1代)种群代数为x(n+1)＝x(n)+1，返回步骤3)；步骤10)根据所述步骤8)得到的一系列局部航路序列点，将第一个局部航路点作为动态规划起始点，将第二个局部航路点作为动态规划目标点；步骤11)根据传感器获得水面无人艇航行过程中的动态障碍物位置信息及速度信息，求解水面无人艇与障碍物的最近会遇距离DCPA和到达最近会遇点的时间TCPA，然后根据下式，采用DCPA与TCPA加权得到碰撞危险度ρ：ρ＝(ωDDCPA)2+(ωTTCPA)2其中，ωD、ωT分别为加权值；步骤12)判断所述步骤11)得到的碰撞危险度ρ是否小于或等于经验值，如是，则融合海事规则的避障策略及水面无人艇的动力学约束条件，调整水面无人艇的航向角或速度大小，否则，保持水面无人艇原有的航向角和速度大小不变；步骤13)根据步骤12)...

【专利技术属性】
技术研发人员：程向红，刘钰，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32