[发明专利]一种基于导航误差约束的水面无人艇局部分层路径规划方法

摘要

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

主权项

一种基于导航误差约束的水面无人艇局部分层路径规划方法，其特征在于，该方法包括以下步骤：步骤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=1L(s0,sg);]]>否则根据下式计算每条染色体的适应度E：E=1mL(s0,sg);]]>其中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=(VUSV+ΔVUSV)sin(α+Δα)×ΔT+XprevY=(VUSV+ΔVUSV)cos(α+Δα)×ΔT+Yprev]]>式中，X、Y分别为规划一个△T时间后水面无人艇在直角坐标系中的横坐标值和纵坐标值，Xprev、Yprev分别为此次规划前水面无人艇在直角坐标系中的横坐标值和纵坐标值，α、VUSV分别表示水面无人艇的航向角和速度大小；判断水面无人艇的当前位置是否为当前动态规划目标点，如是，则进入步骤14)，否则返回步骤11)；步骤14)根据所述步骤1)得到的目标点位置Goal，判断水面无人艇的当前位置是否为目标点位置Goal，如是，则结束本方法流程，否则将当前动态规划目标点作为下一个动态规划起始点，将下一个局部航路点作为下一个动态规划目标点，返回步骤11)。