技术领域
[0001] 本
发明属于水下机器人路径规划领域，涉及一种水下机器人路径规划方法，尤其涉及一种Dubins曲线和遗传
算法的水下机器人路径规划方法。
背景技术
[0002] 水下机器人路径规划是指AUV能够在水下环境保证安全无碰撞的前提下，同时以AUV的速度、最小
转弯半径和离障碍物的安全距离为约束，自主规划出一条最优路径。
[0003] 近年来，国内外针对AUV路径规划为题也有很多解决方法，主要有A*、
粒子群优化法、人工势场法、神经网络等。这些算法虽然能够在一定程度上解决AUV路径规划问题，但由于小型AUV具有许多约束条件，如有限的导航距离、严格的非完整运动约束、无方向反转等。这些算法未考虑AUV自身的运动学约束，AUV无法沿着算法搜寻到的路径航行，这样的路径也不能直接用在AUV上。AUV路径规划问题不仅仅要考虑起始点目标点以及障碍物的
位置坐标，还要考虑到AUV自身的约束条件。
发明内容
[0004] 针对上述
现有技术，本发明要解决的技术问题是提供一种在考虑AUV自身的约束条件的情况下规划出相对最优路径的水下机器人路径规划方法。
[0005] 为解决上述技术问题，本发明的一种水下机器人路径规划方法，包括以下步骤：
[0006] 步骤1：根据AUV的相关参数以及已知的障碍物信息构建直
角坐标系下的环境模型，包括：AUV起始点以及目标点坐标、
最小转弯半径、AUV与障碍物间需要保持的最小安全距离；障碍物中心点坐标以及障碍物半径；
[0007] 步骤2：将障碍物按照在直角坐标系下的位置进行排序编码；
[0008] 步骤3：按照三进制编码规则随机生成
染色体群体，种群数量为100，用三进制染色体序列代替AUV路径，进行后续
遗传算法的操作；
[0009] 步骤4：结合Dubins曲线将三进制染色体序列解码，转化为AUV真实的路径，解码得到的AUV真实路径由圆弧—直线—圆弧组成，并通过检测算子判断AUV真实路径与危险区域是否有交点，具体为：判断两个相切的危险区域中成对的切点，用直线连接后，与其他危险区域有没有交点。
[0010] 步骤5：求出每个染色体对应路径的长度，包括AUV直线路径长度和AUV圆弧路径长度，根据适应度函数计算染色体群体中每个个体的适应度值；
[0011] 步骤6：根据适应度值选择当前阶段最佳的染色体，并判断适应度值是否大于设定值或是否达到最大
迭代次数，如果适应度值大于设定值或达到最大迭代次数，则输出AUV路径，否则继续步骤7；
[0012] 步骤7：通过交叉算子丰富染色体构成，得到新一代染色体；
[0013] 步骤8：在交叉算子得到的染色体中，随机选择一个基因，并以一定的概率将其替换为与原基因不同的基因，得到新一代染色体；
[0014] 步骤9：迭代次数加1，返回步骤步骤4进行下一次迭代。
[0015] 本发明还包括：
[0016] 1.步骤1中当最小转弯半径大于障碍物半径与安全距离的和时，使安全距离＝最小转弯半径-障碍物半径。
[0017] 2.步骤2所述的排序编码具体为：由危险区域圆心做起始点到目标点连线的垂线，根据垂足到起始点的距离大小给危险区域进行排序编码，危险区域为以各障碍物为圆心，以各障碍物半径和AUV距离障碍物的安全距离之和为半径的圆形区域。
[0018] 3.步骤3所述三进制编码规则具体为：路径经过危险区域有三种方式：路径与危险区域右切；路径与危险区域左切；路径与危险区域不相切，这三种情况分别用2，1，0来表示，染色体中基因个数＝障碍物个数+1，且最后两个非0基因一定相同，每个基因代表路径经过下一个危险区域的方式，用这样的染色体序列代替真实的AUV路径。
[0019] 4.步骤4所述结合Dubins曲线将三进制染色体序列解码，转化为AUV真实的路径具体为：
[0020] 步骤4a：根据染色体中非0的基因，判断与路径相切的危险区域，并计算每对危险区域相切存在的4对切点；
[0021] 步骤4b：从4对切点中选出符合当前的非0基因以及上一个非0基因的唯一一对切点；
[0022] 步骤4c：用直线连接起始点与第一个切点；同一个危险区域上相邻的切点用以该危险区域中心为圆心，危险区域半径为半径的圆弧连接；将步骤4b中选择的切点用直线连接，最后一个切点与目标点也用直线进行连接；
[0023] 步骤4b:根据检测算子，判断AUV真实路径与危险区域是否有交点；
[0024] 步骤4a中计算每对危险区域相切存在的4对切点具体为：
[0025]
[0026] 相应的切点坐标(xT_1,yT_1)、(xT_2,yT_2)，可用极坐标进行表示：其中τ和λ为该切点所对应的极坐标角度，(x1,y1)、(x2,y2)分别为两危险区域中心坐标，R、r分别为两危险区域半径。
[0027] 5.步骤5所述AUV直线路径长度计算方法为：
[0028]
[0029] 其中(xa,ya)、(xb,yb)分别为步骤4b中选择的一对切点。
[0030] 6.步骤5所述AUV圆弧路径长度计算方法为：
[0031]
[0032] 从起始点至目标点共有N个危险区域，ri为第i个危险区域半径，θi为第i个危险区域上两个切点对应的圆弧角，0＜θi＜π，若后一对切点中靠近起始点的切点比前一对切点中靠近目标点的切点更靠近起始点，则θi＝θi+2π。
[0033] 7.步骤5所述根据适应度函数计算染色体群体中每个个体的适应度值具体为：
[0034]
[0035] 其中d为起始点到目标点的距离，L为AUV路径长度。
[0036] 8.步骤7所述交叉算子过程：
[0037] 随机选择两个进行交叉的个体Ind1和Ind2，分别代表从起始点到目标点的路径P1(s-f)和P2(s-f)，s代表起始点，f代表目标点，其中P1和P2至少有一个公共中间
节点，表示为mid，Ind1定义为P1(s-f)＝P1(s-mid)+P1(mid-f)，Ind2定义为P2(s-f)＝P2(s-mid)+P2(mid-f)；通过交叉得到两个个体，分别为Ind3和Ind4，Ind3表示路径P3(s-f)＝P1(s-mid)+P2(mid-f)，Ind4表示路径P4(s-f)＝P2(s-mid)+P1(mid-f)。
[0038] 本发明的有益效果：本发明通过Dubins曲线和遗传算法的结合，实现了AUV全局路径规划，考虑到AUV自身存在运动学约束，最小转弯半径大的特点，可以在满足AUV运动学约束的前提下，规划出满足需求相对最优的路径。
附图说明
[0039] 图1为本发明一种基于Dubins曲线和遗传算法的水下机器人路径规划方法示意图；
[0040] 图2为本发明
实施例提供的障碍物排序方法示意图；
[0041] 图3为本发明实施例提供的染色体编码示意图；
[0042] 图4(a)为本发明实施例提供切点坐标求取示意图；
[0043] 图4(b)为本发明实施例提供切点坐标求取示意图；
[0044] 图5为本发明在危险区域有重叠情况下的仿真结果示意图；
[0045] 图6为本发明在危险区域无重叠情况下的仿真结果示意图；
[0046] 图7为本发明一种基于Dubins曲线和遗传算法的水下机器人路径规划方法方法
流程图。
具体实施方式
[0047] 下面结合图以及实例对本发明作进详细描述。
[0048] 本发明设计一种基于Dubins曲线和遗传算法的水下机器人(AUV)路径规划方法。该方法包括：根据AUV的相关参数以及已知的障碍物信息构建直角坐标系下的环境模型；将障碍物按照在直角坐标系下的位置进行排序；按照三进制编码规则随机生成染色体群体；
对染色体进行解码，求出每个染色体对应的路径及其长度，并通过检测算子判断与危险区域是否有交点；根据适应度函数计算染色体群体中各个个体的适应度值；根据适应度值选择当前阶段最佳的染色体；通过交叉算子、变异算子增加染色体种群的多样性，补充新的染色体作为下次迭代种群的一部分；经多次迭代输出规划路径。上述方法考虑到AUV自身的运动学约束，通过Dubins曲线和遗传算法的结合，实现了AUV的路径规划。
[0049] 如图1所示，根据本发明一种基于Dubins曲线和遗传算法的水下机器人路径规划方法，具体地，包括下述步骤：
[0050] 步骤1、根据AUV的相关参数以及已知的障碍物信息构建直角坐标系下的环境模型。
[0051] AUV以及障碍物的相关参数包括：AUV起始点以及目标点坐标、最小转弯半径、AUV与障碍物间需要保持的最小安全距离；障碍物中心点坐标以及障碍物半径，初始化迭代次数n＝0。
[0052] 根据AUV搭载的
传感器获取AUV起始点位置坐标；根据AUV的性能参数、
推进器类型以及技术手册获取AUV的最小转弯半径、最大航行距离以及与障碍物需要保持的安全距离；根据任务要求获取目标点位置以及所在环境的危险区域。建立直角坐标系下的环境模型。
[0053] 当最小转弯半径大于障碍物半径与安全距离的和时，提高安全距离，使安全距离＝最小转弯半径-障碍物半径。
[0054] 步骤2、将障碍物按照在直角坐标系下的位置按照进行排序编码。
[0055] 如图2所示，由危险区域圆心做起始点到目标点连线的垂线，计算垂足到起始点的距离，按照垂足到起始点的距离从小到大的顺序给危险区域编码。为后续步骤做准备。其中Rz为障碍物半径，Ls为安全距离。
[0056] 步骤3、按照三进制编码规则随机生成染色体群体(问题的可行解)，种群数量为100，用三进制染色体序列代替AUV路径，进行后续遗传算法的操作。
[0057] 三进制编码规则如下：
[0058] 路径经过危险区域有三种方式：路径与危险区域右切；路径与危险区域左切；路径与危险区域不相切。这三种情况分别用2，1，0来表示。染色体中基因个数＝障碍物个数+1，且最后两个非0基因一定相同，每个基因代表路径经过下一个危险区域的方式。用这样的染色体序列代替真实的AUV路径。有5个障碍物的染色体实例如图3所示，该染色体为212022，代表路径与第一个危险区域右切，与第二个危险区域左切，与第三个危险区域右切，与第四个危险区域不相切，在与第五个危险区域右切后最终到达目标点。
[0059] 步骤4、结合Dubins曲线对染色体进行解码，将三进制染色体序列转化为AUV路径，并通过检测算子判断与危险区域是否有交点。
[0060] 步骤4a、结合Dubins曲线将三进制染色体序列解码，转化为AUV真实的路径；
[0061] 该解码方法得到的AUV路径属于Dubins路径，转化后的AUV路径由圆弧-直线-圆弧组成；
[0062] 步骤4a1、根据染色体中非0的基因，判断与路径相切的危险区域，并计算路径与危险区域相切时存在的切点，其中起始点与第一个危险区域相切有两个切点，两个危险区域相切有4对切点，实例如图4(a)和图4(b)所示，(x1,y1)、(x2,y2)分别为两危险区域中心坐标，R、r分别为两危险区域半径，l为两危险区域中心坐标间的距离，α、θ为切点与圆心所在直线与两危险区域中心坐标所在直线的夹角，β为两危险区域中心坐标与x轴的夹角。
[0063] 切点坐标计算公式如下：
[0064]
[0065]
[0066]
[0067]
[0068] 当x2≥x1且y2＞y1时，若x2＝x1，β＝90；
[0069]
[0070]
[0071] 当x2≤x1且y2＜y1时，若x2＝x1，β＝90；
[0072]
[0073]
[0074] 当x2＜x1且y2≥y1时，若y2＝y1，β＝-180；
[0075]
[0076]
[0077] 当x2＞x1且y2≤y1时，若y2＝y1，β＝-180；
[0078]
[0079]
[0080] 其中τi，λi(i＝1,2,3,4)为每种情况下切点对应的极坐标角度。
[0081]
[0082] 相应的一对切点坐标(xT_1,yT_1)、(xT_2,yT_2)，可用极坐标进行表示：其中τ和λ为该切点所对应的极坐标角度；
[0083] 步骤4a2、从4对切点中选出符合当前非0基因以及上一个非0基因的唯一一对切点；
[0084] 由起始点出发的路径与下一个危险区域相切只有左切、右切两种方式，每种方式对应一个切点，Dubins路径左切或者右切到达其中一个危险区域，再与下一个危险区域相切时同样也只有左切、右切两种方式，且这一对切点唯一可以计算得到，上述危险区域编号是由染色体中非0基因在染色体中的所在位置决定的。
[0085] 步骤4a3、用直线连接起始点与第一个切点；同一个危险区域上相邻的切点用以该危险区域中心为圆心，危险区域半径为半径的圆弧连接；将步骤4a2中选择的切点用直线连接，最后一个切点与目标点也用直线进行连接。
[0086] 步骤4b、根据检测算子，判断AUV真实路径与危险区域有没有交点。
[0087] 判断两个相切的危险区域中成对的切点，用直线连接后，与这两个切点之间的危险区域有没有交点，也就是染色体中基因为0的危险区域。di为两切点所在直线到两个切点之间的危险区域中心坐标的距离，Ri为危险区域的半径，Si为安全距离，当di＞Ri+Si时，AUV真实路径与危险区域有没有交点。
[0088] 步骤5、求出每个染色体对应Dubins路径的长度，根据适应度函数计算染色体群体中每个个体的适应度值。
[0089] 步骤5a、求AUV直线路径长度的公式如下：
[0090]
[0091] 其中(xa,ya)、(xb,yb)分别为步骤4a2中选择的一对切点；
[0092] 步骤5b、求AUV圆弧路径长度如下：
[0093]
[0094] 从起始点至目标点共有N个危险区域，其中ri为第i个危险区域半径，θi为第i个危险区域上两个切点对应的圆弧角，0＜θi＜π，若后一对切点中靠近起始点的切点比前一对切点中靠近目标点的切点更靠近起始点，则θi＝θi+2π；
[0095] 步骤5c、AUV总路径长度＝AUV直线路径长度+AUV圆弧路径长度；
[0096] 步骤5d、根据AUV路径长度以及适应度函数求取适应度值。
[0097] 适应度函数如下：
[0098]
[0099] 其中d为起始点到目标点的距离，L为AUV路径长度。
[0100] 步骤6、根据适应度值选择当前阶段适应度值最大的染色体，并保证这条染色体会一直存在于下次的迭代过程中。
[0101] 在得到适应度值最大的染色体后，将这条染色体储存起来。并判断这条染色体的适应度值是否大于设定值或是否达到最大迭代次数，如果适应度值大于设定值或达到最大迭代次数，则输出AUV路径，否则继续步骤步骤7；
[0102] 步骤7、通过交叉算子丰富染色体构成，得到新一代染色体。
[0103] 将交叉概率设置为0.7，随机生成一个0，1之间的随机数当这个随机数大于交叉概率(0.7)时，进行交叉，否则不进行交叉。
[0104] 所述交叉算子包括：随机选择两个进行交叉的个体，如Ind1和Ind2，分别代表路径P1(s-f)和P2(s-f)。其中P1和P2至少有一个公共中间节点(用mid表示)，Ind1可以定义为P1(s-f)＝P1(s-mid)+P1(mid-f)，同样Ind2也可以定义为P2(s-f)＝P2(s-mid)+P2(mid-f)。通过交叉得到两个个体(分别Ind3和Ind4表示)。Ind3表示路径P3(s-f)＝P1(s-mid)+P2(mid-f)，Ind4表示路径P4(s-f)＝P2(s-mid)+P1(mid-f)。
[0105] 步骤8、在交叉算子得到的染色体中，随机选择一个基因，并以一定的概率将其替换为与原基因不同的基因，得到新一代染色体，此时迭代次数n＝n+1。
[0106] 将突变概率设置为0.1，随机生成一个0，1之间的随机数，当这个数大于突变概率(0.1)时，将相应的基因替换为与原基因不同的基因以得到新一代染色体。
[0107] 将步骤6步骤中储存起来的染色体加入到新一代染色体中，防止经过交叉、变异丢掉上一代适应度值最大(最优的)染色体。
[0108] 步骤9、迭代次数+1，返回步骤步骤4进行下一次迭代。
[0109] 考虑到AUV自身的运动学约束，本算法中，遗传算法编码、解码部分借助Dubins曲线作为解码方式，将相对最优的AUV路径定义为与部分危险区域相切的直线与圆弧组成的路径，在遗传算法中引入检测算子，使能方便通过适应度函数计算AUV路径的适应度值，通过多次迭代得到了能够避开危险区域且满足AUV运动学约束的Dubins路径。
[0110] 本领域内的技术人员将会认识到，本发明并不局限于上文所诉并在图中示出的特定配置，为了简明起见这里省略了对已知方法的详细陈述，尽管参照前述实施例对本发明进行了详细的说明，以上所述的实施例是为了帮助读者理解本发明的技术方案。本发明可以有各种更改和变化，凡在本发明的精神和原则之内，所做出的任何
修改、同等替换、改进等，均应包含在本发明的
权利要求范围之内。
