一种适用于内陆和近海无人船的路径规划方法与流程
本发明属于无人船自主决策装置，涉及无人船技术，路径规划算法领域，具体为一种适用于内陆和近海无人船的路径规划方法。
背景技术
伴随世界人口快速增长，社会高速发展和人类活动日益频繁，过多追求经济效益导致环境污染日趋加深，这其中水资源的污染尤为严重。为保障人类社会能够继续高速可持续化发展，近海和内陆地表水体的测绘和监测成为不可或缺的重要工作。根据我国环保部现行规定，不同等级水库需按时定点采集水样作水质分析，以保障居民生活用水；根据我国水利资源部的现行规定，针对我国内湖内河需定时进行水文信息采集，以便监测收集水体信息供绘制地图和水利资源管理。而人工测量采集费时费力，人力物力成本较高，且在一定区域，载人船只由于重量较大，吃水较深，无法航行至待测水面的全部区域，所以在当下各个行业都以无人设备代替人工的大趋势下，无人船成为了取代人工进行水文勘探测绘和水质采样工作的优质选择。无人船是继无人机和无人车之后无人设备的又一大新研究领域，是一种现代无人技术与多领域技术相结合的产物，具有一定的自主规划、自主航行、自主调节能力。无人船在工作过程中可以根据卫星地图以及实际的工作环境中所感知的信息，相对于传统船只具有明显优势。而目前，无人机、无人车已经成功在摄影、快递分拣、玩具、家居、港口集装箱运输、消防、反恐打击、国防军事等等行业成熟应用并且实现了商业化，相比之下对于无人船领域的研究起步较晚，但发展前景良好，潜在发展空间巨大。而在水文测绘和水质监测此类领域中更是少有类似自主航行无人船的应用，其路径规划方法也多用于其他领域，在无人船领域少有应用，所以本发明提出一种适用于此类情形下的无人船及其路径规划方法。
现有公开技术：
专利申请号cn107816999a的专利《一种基于蚁群算法的无人船航行路径自主》中介绍了一种基于蚁群算法的无人船航行路径自主规划方法；
专利申请号cn106774341a的专利《无人船运动控制系统及运动控制方法》中发布了一种涉及无人船运动控制系统及控制方法；
现有技术在进行水文测绘和水质采集时往往耗时费力，效率低下，占用大量人工成本的同时且路线规划结果很差。而且，载人船只重量大，吃水深，船体尺寸大，在小型水域不够灵活；噪音大，严重影响自然生态文明建设。而现有少量算法在无人船领域的应用不够完善，得出的规划路线不够精确圆滑，且运算耗时过长，导致整体规划效果往往很差、效率不高。
为减少人力资源占用，降低经济成本，覆盖率更高地测量水文水体信息，考虑采取无人船代替人工进行水文测绘和定时定点水质采样工作。无人船尺寸小巧，重量轻便，吃水较浅。使用无人船进行水文勘探和水质采样工作，可以到达的水面区域更广，水面覆盖率更高。另外，为克服现有无人船路径规划算法不足，综合算法快速性和精确性，提出一种基于快速拓展随机树的启发式搜索算法，以改善原有算法速度慢、效果差、效率低等缺点。
技术实现要素：
本发明的目的在于提供一种适用于内陆和近海无人船的路径规划方法，该路径规划方法以传统快速拓展随机树算法(rrt)为主要基础，再辅以人工设定势场函数拓展和目标引力函数扩展相结合的扩展策略，并采用节点拓展概率准则及多步贪婪准则，自动求解出无人船的航行路径；大大改善了单一传统快速拓展随机树随机性太高，求解不稳定，容易陷入局部最优解的缺点。
为实现上述目的，本发明的技术方案是：一种适用于内陆和近海无人船的路径规划方法，包括如下步骤：
步骤s1、对于已知的地图环境，将环境地图进行锐化处理后输入无人船核心处理器；
步骤s2、无人船核心处理器接收环境地图图像信息，并对其进行再次处理，转化为无人船核心处理器能够识别的环境地图电子数据信息；
步骤s3、针对环境地图电子数据信息，综合考虑无人船起点，途径点，终点，用经改进的快速拓展随机树rrt算法介入进行运算，得出整体基本规划路线；
步骤s4、船载核心处理器对步骤s3得出的整体基本规划路线进行b样条曲线圆滑处理，进一步优化路线；
步骤s5、将路径规划路线图传送给无人船船体控制系统，再由无人船船体控制系统对船体进行控制，使其沿步骤s4优化后的路线进行航行。
在本发明一实施例中，所述步骤s1具体实现如下：
首先确定一个包含环境地图信息和船体运动状态模型的坐标系；将环境地图进行锐化，空白部分为可行驶区域，阴影部分为障碍物，环境地图信息就可以以横纵坐标系表示，无人船核心处理器就能够读取和识别地图信息；
再将船体状态进行坐标化表示，在状态空间x中船体状态
包含的非完整性约束可由微分方程x＝f(x,u)表示，状态转移方程：
输入控制ui∈u，由
在本发明一实施例中，所述步骤s2具体实现为：通过船上传感器来感知附近环境信息，结合无人船核心处理器已经获取的环境地图信息，将整个环境地图空间划分为障碍物空间xobs和无障碍物空间xfree，障碍物以阴影部分来表示。
在本发明一实施例中，所述步骤s3，针对环境地图电子数据信息，利用无人船核心处理器，导入快速拓展随机树rrt算法对其进行路径规划的计算；针对无人船当时的工作场景和所要执行的具体任务，综合考虑到无人船的起始位置，起始状态，路径中所要途径的点以及终点，导入改进后的快速拓展随机树rrt算法进行运算；运算的过程又分为如下步骤：
步骤s31、无人船核心处理器接收环境地图电子数据信息，确定起始点状态和目标点状态；
步骤s32、通过改进后的快速拓展随机树rrt算法进行快速拓展随机树路线拓展；
步骤s33、若遇到障碍物，则提升人工势场函数权重，使无人船避开障碍物；
步骤s34、若距离目标点越来越远，则提升目标引力函数，使无人船向着靠近目标点的方向前进；若无人船距离目标点越来越近，则执行下一步骤；
步骤s35、若算法陷入局部最优点，则说明目标引力函数所占权重过高，此时则需要提升rrt算法权重以便能够跳出局部最优点，继续寻找目标点；若算法没有陷入局部最优点，则执行下一步骤；
步骤s36、判断是否已经到达目标点位置状态，若没有到达目标点位置状态，则降低rrt算法权重，回到步骤s32，继续进行拓展；
步骤s37、若已到达目标点位置状态，则对规划路线进行b样条曲线处理，继续优化路线；
步骤s38、得到最终规划路线，算法结束，引导无人船行驶。
在本发明一实施例中，所述改进后的快速拓展随机树rrt算法，即对快速拓展随机树rrt算法，加入设定人工势场和与目标引力函数扩展相结合的扩展策略，具体过程如下：
假设无人船所在点相对于目标点的引力势场为u1，其引力势场的一般形式可表现为
式中，x是无人船当前所在位置，xg是目标点所在位置，k是引力势场的增益系数；
无人船在该点受到来自目标点的引力为f1，即其所受引力势场的梯度，方向由其当前所在点位置指向目标点位置，公式如下所示
f1(x)＝-▽u1(x)＝k(x-xg)(4)
无人船所受引力随其到目标点距离的增加而减小，反之则增加，当无人船到达或无限接近目标点时，其所受引力为0；
在假设无人船在此点受到斥力势场为u2，来自于障碍物，其公式如下
其中，xb是障碍物的位置坐标，m是斥力势场的增益系数，r为障碍物的影响半径；
因而，无人船受到的斥力f2(x)为其所受斥力势场的负梯度，公式如下
由此可得，无人船受到的合力作用为f，公式如下
f＝f1+f2(7)
无人船在合力f的作用下，躲避障碍，朝着目标点移动；若无人船、目标点和障碍物三者处于同一条直线上，且障碍物位于无人船与目标点之间，目标点对无人船的引力和障碍物对无人船的斥力的合力为零，无人船则陷入局部最小值点，在该局部小范围内反复移动而无法到达目的点；如果遇到这种情形，系统将自动切换为快速拓展随机树进行拓展，跳出此局部最优点。
相较于现有技术，本发明具有以下有益效果：本发明在快速拓展随机树(rrt)算法的基础上引入了人工势场函数和目标引力函数两个函数，克服了扩展随机树随机性高，避免了随机树向目标点相反的方向拓展生长的情况，加入目标引力函数改进了快速扩展随机树缺乏确定性的问题，加入人工势场函数则有效提升了无人船在具体水面环境中的避碰能力，总体提高了无人船在水面路径规划方面的安全性和效率。
附图说明
图1是无人船路径规划流程图。
图2是船体运动控制状态简化结构模型。
图3是经改进的基于rrt的加入人工势场函数和目标引力函数的改进后算法的算法结构简图。
具体实施方式
下面结合附图，对本发明的技术方案进行具体说明。
如图1所示，本发明提供了一种适用于内陆和近海无人船的路径规划方法，包括如下步骤：
步骤s1、对于已知的地图环境，将环境地图进行锐化处理后输入无人船核心处理器；
步骤s2、无人船核心处理器接收环境地图图像信息，并对其进行再次处理，转化为无人船核心处理器能够识别的环境地图电子数据信息；
步骤s3、针对环境地图电子数据信息，综合考虑无人船起点，途径点，终点，用经改进的快速拓展随机树rrt算法介入进行运算，得出整体基本规划路线；
步骤s4、船载核心处理器对步骤s3得出的整体基本规划路线进行b样条曲线圆滑处理，进一步优化路线；
步骤s5、将路径规划路线图传送给无人船船体控制系统，再由无人船船体控制系统对船体进行控制，使其沿步骤s4优化后的路线进行航行。
所述步骤s1具体实现如下：
首先确定一个包含环境地图信息和船体运动状态模型的坐标系；将环境地图进行锐化，空白部分为可行驶区域，阴影部分为障碍物，环境地图信息就可以以横纵坐标系表示，无人船核心处理器就能够读取和识别地图信息；
再将船体状态进行坐标化表示，船体运动控制状态简化结构模型如图2所示，在状态空间x中船体状态
包含的非完整性约束可由微分方程x＝f(x,u)表示，状态转移方程：
输入控制ui∈u，由
所述步骤s2具体实现为：通过船上传感器来感知附近环境信息，结合无人船核心处理器已经获取的环境地图信息，将整个环境地图空间划分为障碍物空间xobs和无障碍物空间xfree，障碍物以阴影部分来表示。
所述步骤s3，针对环境地图电子数据信息，利用无人船核心处理器，导入快速拓展随机树rrt算法对其进行路径规划的计算；针对无人船当时的工作场景和所要执行的具体任务，综合考虑到无人船的起始位置，起始状态，路径中所要途径的点以及终点，导入改进后的快速拓展随机树rrt算法进行运算；运算的过程又分为如下步骤：
步骤s31、无人船核心处理器接收环境地图电子数据信息，确定起始点状态和目标点状态；
步骤s32、通过改进后的快速拓展随机树rrt算法进行快速拓展随机树路线拓展；
步骤s33、若遇到障碍物，则提升人工势场函数权重，使无人船避开障碍物；
步骤s34、若距离目标点越来越远，则提升目标引力函数，使无人船向着靠近目标点的方向前进；若无人船距离目标点越来越近，则执行下一步骤；
步骤s35、若算法陷入局部最优点，则说明目标引力函数所占权重过高，此时则需要提升rrt算法权重以便能够跳出局部最优点，继续寻找目标点；若算法没有陷入局部最优点，则执行下一步骤；
步骤s36、判断是否已经到达目标点位置状态，若没有到达目标点位置状态，则降低rrt算法权重，回到步骤s32，继续进行拓展；
步骤s37、若已到达目标点位置状态，则对规划路线进行b样条曲线处理，继续优化路线；
步骤s38、得到最终规划路线，算法结束，引导无人船行驶。
所述改进后的快速拓展随机树rrt算法，即对快速拓展随机树rrt算法，加入设定人工势场和与目标引力函数扩展相结合的扩展策略，具体过程如下：
假设无人船所在点相对于目标点的引力势场为u1，其引力势场的一般形式可表现为
式中，x是无人船当前所在位置，xg是目标点所在位置，k是引力势场的增益系数；
无人船在该点受到来自目标点的引力为f1，即其所受引力势场的梯度，方向由其当前所在点位置指向目标点位置，公式如下所示
f1(x)＝-▽u1(x)＝k(x-xg)(4)
无人船所受引力随其到目标点距离的增加而减小，反之则增加，当无人船到达或无限接近目标点时，其所受引力为0；
在假设无人船在此点受到斥力势场为u2，来自于障碍物，其公式如下
其中，xb是障碍物的位置坐标，m是斥力势场的增益系数，r为障碍物的影响半径；
因而，无人船受到的斥力f2(x)为其所受斥力势场的负梯度，公式如下
由此可得，无人船受到的合力作用为f，公式如下
f＝f1+f2(7)
无人船在合力f的作用下，躲避障碍，朝着目标点移动；若无人船、目标点和障碍物三者处于同一条直线上，且障碍物位于无人船与目标点之间，目标点对无人船的引力和障碍物对无人船的斥力的合力为零，无人船则陷入局部最小值点，在该局部小范围内反复移动而无法到达目的点；如果遇到这种情形，系统将自动切换为快速拓展随机树进行拓展，跳出此局部最优点。
以下为本发明具体实施实例。
针对rrt搜索算法有时会触碰障碍物和随机性太高的缺点，以及其容易陷入局部最优解的情况，通过分别加入预设的人工势场和目标引力来改进rrt搜索算法。
众所周知，无人船在执行任务时，一旦触碰到礁石、岸基以及其他船只，势必会影响无人船的正常工作，甚至会损伤无人船船体，重则报废，造成更加严重的后果。引入人工势场法，避免无人船触碰障碍物这一点尤其重要。而无人船在执行rrt算法时，因其较高的的随机性，导致其工作效率太过低下，此时就需要引进目标引力函数对其进行大方向上的引导，用以提高它的工作效率。在rrt算法中引入目标引力函数，目标引力函数使随机树朝着目标的位置方向生长，这样不仅避免了随机树对全局空间进行搜索，大大减少了运算量，提高了实时性，而且使规划出的路径尽可能的接近最优路径，并改进了路径的光滑性。
通过对rrt搜索算法进行改进，加入设定人工势场和与目标引力函数扩展相结合的扩展策略，加入势场的过程如下：
假设无人船所在点相对于目标点的引力势场为u1，其引力势场的一般形式可表现为
式中，x是无人船当前所在位置，xg是目标点所在位置，k是引力势场的增益系数。
无人船在该点受到来自目标点的引力为f1，即其所受引力势场的梯度，方向由其当前所在点位置指向目标点位置，公式如下所示
f1(x)＝-▽u1(x)＝k(x-xg)(4)
无人船所受引力随其到目标点距离的增加而减小，反之则增加，当无人船到达或无限接近目标点时，其所受引力为0；
在假设无人船在此点受到斥力势场为u2，来自于障碍物，其公式如下
其中，xb是障碍物的位置坐标，m是斥力势场的增益系数，r为障碍物的影响半径；
因而，无人船受到的斥力f2(x)为其所受斥力势场的负梯度，公式如下
由此可得，无人船受到的合力作用为f，公式如下
f＝f1+f2(7)
无人船在合力f的作用下，躲避障碍，朝着目标点移动；若无人船、目标点和障碍物三者处于同一条直线上，且障碍物位于无人船与目标点之间，目标点对无人船的引力和障碍物对无人船的斥力的合力为零，无人船则陷入局部最小值点，在该局部小范围内反复移动而无法到达目的点；如果遇到这种情形，系统将自动切换为快速拓展随机树进行拓展，跳出此局部最优点。
经改进后的算法运算流程结构简图如图3所示。其运算步骤如下：
步骤1，无人船船载处理器接收地图信息，确定起始点状态和目标点状态；通过本发明所介绍的方法建立环境坐标系，并将船体位置状态参数化，由船载传感器自动感知，并将船体状态输入船载核心处理器。
步骤2，通过改进后的快速随机拓展树算法进行路线拓展。令gk为一个有k个节点的快速随机拓展树，且gk∈xfree，xfree为无人船在地图中的无障碍区域时的状态空间，同时xobs为无人船在地图中的障碍区域的状态空间。x为gk的节点，x∈gk。定义xinit为初始状态，即起点，xgoal为目标状态即终点，xgoal∈xfree为目标区域。令xrand为空间中一个随机选取的位置状态，同样xrand∈xfree，然后再找出gk中距离xrand最近的节点xnear，令d(p,q)表示两个位置状态之间的几何距离，则xrand和xnear的关系可以表示为d(xrand,xnear)≤d(x,xrand)。在xnear与xnear的连接线上求xnew，xnew必须满足的条件是xnew∈xfree，且d(xrand,xnear)≤ρ(其中ρ>0)。每当一个新的xnew出现，则gk就会增加一个新的节点gk+1，随机选择一个树g，将x_(init)添加到树g上，再选择一个构型空间xrand，使xrand∈xfree。以固定的步长ρ计算新的节点xnew，计算公式为：
其中，||xrand-xnear||表示欧拉范数定义的xrand和xnear之间距离。
不断重复搜索过程，即可在相近两点间得到一条路径。
步骤3，若遇到障碍物，则提升人工势场函数权重，使无人船避开障碍物。当无人船在行驶过程中接近障碍物时，即当||xrand-xobs||趋近于我们设定的某一数值时，及时调整各算法函数权重比例，使人工势场函数权重比例上升，避免无人船触碰到障碍物，从而保证无人船安全平稳行驶。
步骤4，若距离目标点越来越远，则提升目标引力函数，使无人船向着靠近目标点的方向前进。若无人船距离目标点越来越近，则执行下一步骤；当||xrand-xgoal||增大时，及时调整各个算法所占权重，提升目标引力函数权重，降低快速拓展随机树算法权重，使无人船驶向更加靠近目标点的方向。
步骤5，若算法陷入局部最优点，则说明目标引力函数所占权重过高，此时则需要提升rrt算法权重以便能够跳出局部最优点，继续寻找目标点。若算法没有陷入局部最优点，则执行下一步骤。判断无人船是否行驶至局部最优点时，只需判断||xrand-xgoal||的数值是否变化，若保持不变，切不为0，则可以判定其已行驶至局部最优点。此时要跳出局部最优点，就要增加快速拓展随机树的权重比例，利用随机树树枝跳出局部最优点，继续向目标点行驶。
步骤6，判断是否已经到达目标点位置状态，若没有到达目标点位置状态，则降低rrt算法权重，回到步骤2，继续进行拓展。若无人船已行驶至目标点，则||xrand-xgoal||数值为0保持不变。若数值不为0，则重新执行步骤2。
步骤7，若已到达目标点位置状态，则对规划路线进行b样条曲线处理，继续优化路线。利用b样条曲线对现有路线进行圆滑化优化处理，可以帮助无人船节省更多的能量，使其拥有更高的续航能力。
步骤8，得到最终规划路线，算法结束，引导无人船行驶。
本发明综合了快速拓展随机树(rrt)和人工势场函数与目标引力函数，在降低快速拓展随机树的随机性的同时，增大跳出局部最优点的能力，使无人船路径规划算法更加完善。
本发明中，最后算法运算到达目标点后，再用b样条曲线对规划路线进行曲线圆滑处理，对规划所得路线进行平滑化处理。使用b样条曲线对其进行处理，可以使无人船在实际航行中减少急停、急加速和急转弯，使其导航路线更加优化，得到无人船最终的执行路线。
以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。
- 还没有人留言评论。精彩留言会获得点赞！