潜航器路径规划方法及控制系统 |
|||||||
申请号 | CN202410081166.4 | 申请日 | 2024-01-19 | 公开(公告)号 | CN117908568A | 公开(公告)日 | 2024-04-19 |
申请人 | 苏州桑泰海洋仪器研发有限责任公司; | 发明人 | 刘飞飞; 汤羽昌; 马腾飞; 杨成伟; | ||||
摘要 | 本 发明 涉及潜航器路径规划方法及控制系统,在潜航器作业空间内随机选取路径点,保存不在障碍物内的路径点,并根据这部分路径点中,路径点之间的连线不经过障碍物的路径点,得到路径网络图;确定起点和终点,分别连接距离起点和终点最近的路径点,并在路径网络图中搜索路径,确定起点和终点之间的路径,经后处理得到最优路径;其中,后处理包括,基于适应度函数从路径网络图中选取路径点并遗传至一下次 迭代 中;每次迭代过程中,以每个路径点与终点之间的欧几里得距离为最小,在两次连续迭代过程中潜航器的移动 角 度为最小。 | ||||||
权利要求 | 1.潜航器路径规划方法,其特征在于,包括以下步骤: |
||||||
说明书全文 | 潜航器路径规划方法及控制系统技术领域[0001] 本发明涉及潜航器控制技术领域,具体为潜航器路径规划方法及控制系统。 背景技术[0002] 本部分的陈述仅仅是提供了与本发明相关的背景技术信息,不必然构成在先技术。 [0003] 海域监测、水下搜救等领域中的潜航器在水下作业时,存在运动状态的障碍物,需要控制潜航器寻找一条最优的路径,保证潜航器能够以最小的能耗来规避障碍物并到达指定目标。针对此类路径规划问题,目前常用到的路径规划方法主要有人工势场法、栅格法等,这些方法虽然计算的实时性很强,便于机器人的动力学优化控制和实时路径规划,但却难以保证能够寻求到全局最优的路径。 [0004] 遗传算法由于其具有优良的全局寻优能力和隐含的并行计算特性,是一个优秀路径规划方法,但由于常规遗传算法本身所存在着的一些缺陷(如解的早熟现象、局部寻优能力差等),难以保证对路径规划的计算效率和可靠性的要求。 [0005] 其次,对于潜航器而言,每一个具体观察目标(水域)和周围可能存在一个或多个障碍物,这些障碍物既有静态的(如海底的礁石、海沟、禁航区等),也有动态障碍物(如其它潜水器、移动的冰山等)。目前的路径规划控制方法难以确保潜航器能够找到最优的全局路径,使得其路径的寻优能力不理想。 发明内容[0006] 为了解决上述背景技术中存在的技术问题,本发明提供潜航器路径规划方法及控制系统,利用遗传算法对潜航器进行多目标路径规划,能够对全局路径进行搜索,查找出最优的全局路径,在遗传算法的适应度函数中引入了平滑度,确保潜航器行驶过程中转弯尽可能的小,减少了潜航器的能耗。 [0007] 为了实现上述目的,本发明采用如下技术方案: [0008] 本发明的第一个方面提供潜航器路径规划方法,包括以下步骤: [0009] 在潜航器作业空间内随机选取路径点,保存不在障碍物内的路径点,并根据这部分路径点中,路径点之间的连线不经过障碍物的路径点,得到路径网络图; [0010] 确定起点和终点,分别连接距离起点和终点最近的路径点,并在路径网络图中搜索路径,确定起点和终点之间的路径,经后处理得到最优路径; [0012] 进一步的,在潜航器作业空间内随机选取路径点,保存不在障碍物内的路径点,具体为: [0013] 创建采样点集x,在作业空间中采用均匀采样随机采取一点ni,当采集到足够的采样点时退出; [0014] 若ni在障碍物内,则舍弃该点; [0015] 若ni不在障碍物内将ni加入到点集x。 [0016] 进一步的,根据不在障碍物内的路径点中,路径点之间的连线不经过障碍物的路径点,得到路径网络图,具体为: [0017] 创建路径集f,分别依次连接点集x中的两点xinit,xgoal; [0018] 若两点之间的连线碰撞到障碍物,则舍弃该路线; [0019] 若两点之间的连线未碰撞到障碍物,则将两点之间路线加入路径集f,得到路径网络图。 [0020] 进一步的,在路径网络图中搜索路径,确定起点和终点之间的路径,具体为: [0021] 种群初始化、选择适应度最高的路径点; [0022] 根据路径距离和路径平滑度,建立适应度函数; [0023] 通过交叉和变异形成新的路径点。 [0024] 进一步的,选择,具体为:从多个路径点中选取一个适应度最高的路径点并遗传到下一代群体中。 [0025] 进一步的,交叉,具体为:对两个相互配对的路径点互相代替和重组形成新的路径点。 [0026] 进一步的,变异,具体为:随机选择路径中的两个栅格,采用种群初始化的方法在两个栅格之间产生新的路径。 [0027] 进一步的,适应度函数包括路径距离函数和路径平滑度函数,路径距离函数为:每次迭代过程中,每个路径点与终点之间的欧几里得距离;路径平滑度函数为:迭代过程中目标路径点与潜航器两个连续的位置连接而成的两条假想直线的夹角。 [0028] 进一步的,适应度函数,如下式所示: [0029] [0030] [0031] [0032] fit=λ1×fit1+λ2×fit2; [0033] [0034] 其中,d为路径的平滑度,fit1为路径的最小距离,fit2为两次连续迭代过程中机器人的最小移动角度,fit为适应度函数,pi为权重,xi,yi均为种群样本数值,a,b,c分别为三个连续种群位置的三角距离。 [0035] 本发明的第二个方面提供基于上述路径规划方法的潜航器控制系统,包括: [0036] 水面单元,与浮出水面的水下单元通讯连接,接收并显示水下单元发送的潜航器运行数据,并向水下单元发送用于控制潜航器的指令; [0037] 水下单元,潜入水下时与水面单元断开通讯,根据接收到的指令控制潜航器执行对应的动作。 [0038] 与现有技术相比,以上一个或多个技术方案存在以下有益效果: [0039] 利用遗传算法对潜航器进行多目标路径规划,能够对全局路径进行搜索,查找出最优的全局路径,在遗传算法的适应度函数中引入了平滑度,确保潜航器行驶过程中的转弯尽可能的小,减少了潜航器的能耗。附图说明 [0041] 图1是本发明一个或多个实施例提供的遗传算法构造过程示意图; [0042] 图2是本发明一个或多个实施例提供的连续三个位置之间的夹角示意图; [0043] 图3是本发明一个或多个实施例提供的遗传算法的选择过程示意图; [0044] 图4是本发明一个或多个实施例提供的遗传算法的交叉过程示意图; [0045] 图5是本发明一个或多个实施例提供的遗传算法中变异前的示意图; [0046] 图6是本发明一个或多个实施例提供的遗传算法中变异后的示意图; [0047] 图7是本发明一个或多个实施例提供的算法验证时改进前的轨迹示意图; [0048] 图8是本发明一个或多个实施例提供的改进前算法路径长度与迭代次数示意图; [0049] 图9是本发明一个或多个实施例提供的遗传算法的选择过程改进后算法路径长度与迭代次数示意图; [0050] 图10是本发明一个或多个实施例提供的算法验证时改进后的轨迹示意图; [0051] 图11是本发明一个或多个实施例提供的算法学习阶段的采样示意图; [0052] 图12是本发明一个或多个实施例提供的算法学习阶段的网络路径示意图; [0053] 图13是本发明一个或多个实施例提供的算法的PRM查询阶段示意图; [0054] 图14是本发明一个或多个实施例提供的算法的PRM查询阶段(显示目标点)示意图; [0055] 图15是本发明一个或多个实施例提供的采样点为50的地图优化示意图; [0056] 图16是本发明一个或多个实施例提供的采样点为1000的地图优化示意图; [0057] 图17是本发明一个或多个实施例提供的地图优化前的示意图; [0058] 图18是本发明一个或多个实施例提供的地图优化后的示意图; [0059] 图19是本发明一个或多个实施例提供的模糊PID控制结构示意图; [0060] 图20是本发明一个或多个实施例提供的自适应模糊PID算法流程示意图; 具体实施方式[0062] 下面结合附图与实施例对本发明作进一步说明。 [0063] 应该指出,以下详细说明都是示例性的,旨在对本发明提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。 [0064] PID控制器,(比例(proportion)、积分(integral)、微分(differential))控制器,根据给定值和实际输出值构成控制偏差,将偏差按比例、积分和微分通过线性组合构成控制量,对被控对象进行控制,是一种常见的线性控制器。 [0065] 模糊控制器,以模糊集理论、模糊语言变量和模糊逻辑推理为基础的一种智能控制方法,是从行为上模仿人的模糊推理和决策过程的一种智能控制方法。首先将操作人员或专家经验编成模糊规则,然后将来自传感器的实时信号模糊化,将模糊化后的信号作为模糊规则的输入,完成模糊推理,将推理后得到的输出量加到执行器上。 [0066] 以下实施例给出潜航器路径规划方法及控制系统,利用遗传算法对潜航器进行多目标路径规划,能够对全局路径进行搜索,查找出最优的全局路径,在遗传算法的适应度函数中引入了平滑度,确保潜航器行驶过程中转弯尽可能的小,减少了潜航器的能耗。 [0067] 实施例一: [0068] 潜航器路径规划方法,包括以下步骤: [0069] 在潜航器作业空间内随机选取路径点,保存不在障碍物内的路径点,根据这部分路径点中,路径点之间的连线不经过障碍物的路径点,得到路径网络图; [0070] 确定起点和终点,分别连接距离起点和终点最近的路径点,并在路径网络图中搜索路径,确定起点和终点之间的路径,经后处理得到最优路径; [0071] 其中,后处理包括,基于适应度函数从路径网络图中选取路径点并遗传至一下次迭代中;每次迭代过程中,以每个路径点与终点之间的欧几里得距离为最小,在两次连续迭代过程中潜航器的移动角度为最小。 [0072] 本实施例中,遗传算法构造过程如图1所示。 [0073] a)种群初始化,种群初始化公式如公式1所示: [0074] D=max{abs(xi+1‑xi),abs(yi+1‑yi)} (1) [0075] 式中,D为初始种群,abs求绝对值。xi,yi为种群样本序列。 [0076] b)选择,遗传算法需对个体进行择优操作,本实施例采用随机联赛选择模型,其核心思想是每次从多个个体中选取一个适应度最高的个体并把它遗传到下一代群体中,在遗传到下一代群体的过程中,只要对多个个体适应度进行大小比较,如图2和图3所示。 [0077] c)建立适应度函数。潜航器在实际海洋工作中,过多的转弯会使潜航器能耗增大,移动时间也会增加,因此最优路径应满足最短性和平滑性约束条件。本实施例提出一个新的适应度函数,路径的距离计算方式为每次迭代过程中,每个粒子与目标点之间的欧几里得距离,路径的平滑度判定则是迭代过程中目标点与机器人两个连续的位置连接而成的两条假想直线的夹角,迭代公式如公式(2),对所有个体组成的路径求一个最小的距离,如公式(3)所示,选择路径越短适应度函数越大。这里平滑度表示为在两次连续迭代过程中机器人最小移动角度,如公式(4)求得每次转弯的角度,最后的适应度函数如公式(5),对适应度函数求一个权重,权重的大小可以通过具体的实验来求得,如公式(6)所示。 [0078] [0079] [0080] [0081] fit=λ1×fit1+λ2×fit2 (5) [0082] [0083] 其中,d为路径的平滑度,fit1为路径的最小距离,fit2为两次连续迭代过程中机器人的最小移动角度,fit为适应度函数,pi为权重,xi,yi均为种群样本数值,a,b,c分别为三个连续种群位置的三角距离。 [0084] d)交叉,交叉操作是指对两个相互配对的染色体的部分结构互相代替和重组形成新个体的操作,如图4所示,交叉的操作是两个个体之间前后路径的交换。本实施例产生新个体的方式是基于浮点数编码方式下的整体算术交叉方法:首先生产m个随机数αi∈(0,1),i=1,2,...,m,执行交叉操作后产生子代为: [0085] [0086] [0087] 其中, [0088] [0089] [0090] 上式中 均为交叉染色体数,xα,xβ均为交叉染色体组。 [0091] e)变异,变异操作是指通过将个体编码串中的某些基因座上的基因值用该基因座的其他等位基因值来替换而形成一个新个体的操作,如图5所示。本实施例采用的方式是先随机选择路径中的两个栅格,如图5和图6所示,然后采用种群初始化的方法在两个栅格之间产生新的路径,最终达到变异的目的。 [0092] 仿真验证。为了验证算法的正确性,本实施例设置了一个20×20单位边界的二维正方形搜索空间,如图7和图9所示。通过反复实验确定最终参数,具体设置如下:种群规模NP=200,最大迭代次数max_gen=50,交叉概率pc=0.8,变异概率pm=0.2,经过实验发现当适应度函数中λ1=1,λ2=1时,有一个比较好的适应。起点位置是(0,0),终点位置是(20,20),图中黑色多边形是障碍物,每次迭代后的最优路径会用红色的线表示出来。实验中环境中的障碍物如图9所示,是可以动态变化的,个数位置都可以自行设置,不同的障碍物个数,该算法都能找到符合要求的路径,拐弯的次数不多,个体路径都能够避开障碍物,收敛速度很快,迭代5次以内就趋于平缓,找到了最优解。 [0093] 在环境中随机放置障碍物,生成的路径能够避免所有障碍物,最终个体的位置也不在障碍物边界内。改进后的遗传算法的最优路径长度和达到平稳的迭代次数如图9所示,需要10代到20代,收敛速度较慢。轨迹如图8所示,会有交叉的部分,不符合需求。改进后的遗传算法效率高,收敛速度快,在3代开始趋于平缓,并且得到的最优解,图中未有交叉路线,说明已经非常接近最好的路线,或者已经达到了最好的路线。 [0094] 针对改进的遗传算法有退火算法和基于PRM算法等,本实施例采用后者。通过在环境中建立路径网络图,将连续的空间转化为离散的空间,然后再进行路径规划,使算法的复杂程度只依赖于搜索路径的复杂度,从而使复杂程度降低。PRM算法也可以用在多自由度的移动机器人路径规划中,而且可以有效地克服局部极小值的缺点,在解决高维空间和复杂障碍物的情况下有计算量小的优势。 [0095] PRM算法的基本思想是用概率图来表示移动机器人所处环境的自由空间。在自由空间内进行随机采样,把采样点加入到图集中,利用局部规划器把各个采样点进行连接,构成路径网络图,然后在这个随机网络中使用搜索算法来找到移动机器人的可行路径。该算法可以分两个阶段完成,即离线学习阶段和查询阶段。 [0096] (1)学习阶段。在自由空间中进行尽可能随机且均匀的采样,然后连接各个点,构建出一幅路径网络图。具体步骤如下: [0097] 创建采样点集x,在地图中采用均匀采样随机采取一点ni,当采集到足够的采样点时则跳出; [0098] 若ni在障碍物内,则舍弃该点; [0099] 若ni不在障碍物内将ni加入到点集x。 [0100] 创建路径集f,分别依次连接点集x中的两点xinit,xgoal; [0101] 若两点之间的连线碰撞到障碍物,则舍弃该路线; [0102] 若两点之间的连线未碰撞到障碍物,则将两点之间路线加入路径集f。 [0103] 建立无向网络图G(x,f),若G(x,f)为空,则不存在路径;若G(x,f)不为空,则查找最优路径。 [0104] PRM算法学习阶段是在地图内进行随机采样,并对采样点进行碰撞检测,从而保证采样点不会落在地图障碍物内,使采样点随机均匀地分布在自由无障碍空间内。采样完成后通过连接各个点,构建路径网络图。构建网络图时,采用从一个点开始,向周围点扩散的方法,如果两个点之间可以连接,并且没有碰到障碍物,就把路线添加到网络图中,反之则舍弃,从而构建出整幅路径网络图,如图11和如图12所示。其中:左上角圆点代表起始点;右下角圆点代表终点;黑色为障碍物;小点为采样点;蓝色细线为路径网络图中路径。 [0105] (2)查询阶段。利用学习阶段建立好的无向路径网络图,在无向网络图中设置好起点和终点,根据设置好的起点和终点搜索出一条符合需求的路径,具体包括以下步骤: [0106] 分别连接地图中离起始点和目标点最近的两个点; [0107] 在网络图中搜索路径,找到起始点到目标点的路径; [0108] 找到路径后,通过平滑处理,优化路径。 [0109] 最终结果如图13和图14所示,红线为规划出的路线。 [0110] PRM算法仿真。当采样点N=50时和采样点N=1000时的对比,图15和图16为采样点为50和1000时的路径网络图。如图17和图18所示,算法能够规划出路径,并且经优化后,路径网络图中的路径更加的平滑,而收敛时间会随着采样点数量的增长而增加,但最短距离的求解更加的优化。 [0111] 表1采样点数对收敛时间和路径长度的影响 [0112] [0113] 如表1所示,采样点个数可以决定能否规划出路径,采样点数量越多,成功规划出路线的概率越大,随着采样点的增加运行时间会增长最优路径长度也会变短,最后逐渐趋于一个稳定的区间,采样点是随机取样的,因此如果采样点过少,则可能规划不出最优路径,导致任务失败,而采样点过多运行时间会增大,精度可以得到保证。 [0114] 模糊PID控制原理。模糊PID控制器的结构是基于模糊逻辑和传统PI D控制技术相结合的。模糊逻辑与PID控制技术所采用的搭配方式不同,所构成的模糊PID控制也不同,一般来说有以下三种方式: [0116] 二、混合型模糊PID控制:将PID控制器和模糊控制器并联组成新的控制形式。 [0118] 模糊逻辑控制器(FLC)可以被应用到数学模型很难获得的非线性系统,可被设计为应用反映人类专家经验的启发式规则。相关的输入和输出语言变量的从属关系函数(MFS)根据系统的非线性,一般预定义常规的PID控制器提供高灵敏度且倾向于增加总体反馈控制系统的稳定性。此外,PI D控制器可以减小过冲,并通过添加阻尼系统允许使用较大的增益。采用微分是因为它在可以降低干扰并保持所需设定值。 [0119] 在创建模糊PID控制器的“FLC”时,最为重要任务是结构与参数的设计。对结构的布局即为确立控制器的体系结构,控制器的输入/输出变量,模糊控制规则的格式与规则的数量。而参数化构造则表示明确模糊控制器最佳参数。 [0120] 为成功设计模糊PID控制器的“PID”部分,需要正确选择最佳的输入和输出比例因子(SFs),比例因子能放大或缩小整个论域。由于其对控制性能和鲁棒性的全论域效应,输入和输出的SF在模糊PID控制器起到关键作用,它们在调整和优化方面有着最高优先级。 [0121] 按照PID控制的具体特点,对于不同e(k)与ec(k),相应地kp、ki、kd的取值也不尽相同: [0122] (1)若出现|e(k)|很大的情况,则需要快速消除误差,加快响应速度,此时kp的取值应该较大。而以防发生超调情况,一般令ki、kd取值为零。 [0123] (2)当|e(k)|较小时,为继续消除偏差并防止超调过大而产生震荡,kp应减小,ki取较小值,kd的值则视|ec(k)|而定。 [0124] (3)而若e(k)与ec(k)恰好同号,此时被控量正向远离设定值的方向变化;而若二者符号相异,则此时被控量正向靠近设定值的方向变化。从中可看出,若被控量逐渐向设定值靠近的情况发展,相反符号的比例环节起着约束积分的用处,从而可以防止由于积分引起的超调与因其发生的震荡,然而若被控量偏离设定值距离较大,却正逐渐向设定值靠近,这时因比例与积分这两个环节的符号相反,其对整个系统的控制会缓慢下来。 [0125] (4)若出现|ec(k)|很大的情况,则需要对kp取较小的值,而ki却要取较大的值。 [0126] (5)微分环节主要是用来控制偏差变化ec(k),减小超调,克服振荡。 [0127] 运用模糊数学的基本原理和方法,把规则的条件、操作用模糊集表示,然后将这些模糊规则等信息(如初始PID参数等)以知识的形式存入计算知识库里,然后计算机根据控制系统的实际响应情况,运用模糊推理,就可以自动实现对PID参数的最佳调整,实现自适应模糊PID控制。 [0128] 自适应模糊PID控制器以误差e及其变化ec作为输入,可以满足不同时刻的e与ec对PID参数自整定的要求。运用模糊控制规则在线对PID参数进行修改,便构成了自适应模糊。PID控制器PID参数模糊自整定就是要找到PID三个参数与e和ec之间的模糊关系。 [0129] 将e和ec作为模糊控制器的输入,将PID的三个参数当作控制器的输出。模糊控制器设计的焦点是总结工程设计人员的技术知识和实际操作经验,建立合适的模糊规则表,得到针对PID三个参数分别整定的控制表。 [0130] 模糊PID控制器分为两部分:PID控制和模糊化模块。本实施例所采用的方式是PID参数模糊自整定的方法,形成了自适应模糊。本实施例将潜航器的运动分解到三个方向,通过三个自适应模糊PID控制器来分别控制潜航器这三个方向的运动。 [0131] 通过将偏差e以及其变化ec当作输入,找出PID控制器的三个参数和偏差e及其变化量ec之间的模糊关系,并在整个系统进行的过程中不停地检测e及ec量,运用模糊控制器对PID控制器的各个相关参数及时地调整,以确保系统达到最佳响应状态。其结构框图如图19所示。 [0132] 模糊PID控制算法流程。模糊PID控制器通过模糊控制器找出使系统处于最佳状态的PID控制器的比例、积分、微分系数这三个参数值。在整个系统进行中,通过对模糊推理结果进行处理、查表和运算,实现对PID控制器三个参数的实时自调整,最终得到合适的参数值,使系统达到最佳响应。其算法程序流程如图20所示。 [0133] 上述方法能够对全局路径进行搜索,查找出最优的全局路径,在遗传算法的适应度函数中引入了平滑度,确保潜航器行驶过程中转弯尽可能的小,减少了潜航器的能耗。 [0134] 实施例二: [0135] 基于实施例一所述路径规划方法的潜航器控制系统,包括: [0136] 水面单元,与浮出水面的水下单元通讯连接,接收并显示水下单元发送的潜航器运行数据,并向水下单元发送用于控制潜航器的指令; [0137] 水下单元,潜入水下时与水面单元断开通讯,根据接收到的指令控制潜航器执行对应的动作。 [0138] 潜航器控制系统硬件架构可分为两部分:水面硬件单元和水下硬件单元,如图21所示。水面硬件系统由水面计算机、手操杆、显示器、键盘、鼠标、无线网卡和无线路由器组成。水下硬件系统由各个执行机构、传感器、水下计算机以及功能模块组成。 [0139] 水面硬件单元中,手操杆、键盘、鼠标和无线网卡通过USB与水面计算机相连,主要用于人机交互。当潜航器浮于水面时,水面计算机可以实时读取各传感器数值信息、执行机构状态以及报警信息,并通过水面GUI界面显示。同时,水面计算机可通过水面GUI界面将设置的舵角偏转、螺旋桨转速等开环指令或者航行速度、航行艏向、航行深度等闭环指令发送至潜航器从而进行远程控制。 [0140] 当潜航器潜入水下后,水面计算机与水下计算机的无线通讯自动停止。潜航器按照水下计算机先前读取的闭环控制指令进行自动控制。当潜航器由水下上浮至水面后,无线通讯重新建立。水面计算机可读取潜航器在水下自动运行时的各传感器数据信息及执行机构自动控制指令信息。 [0141] 水下硬件单元主要由三部分组成:水下计算机、水下执行机构以及传感器。水下计算机是水下硬件单元的核心,需要对深度传感器、惯性导航、多普勒流速仪等传感器上传的数据进行解码处理,并根据算法程序向主推进器、垂直舵机、前水平舵机、后水平舵机以及压载水舱中的电磁阀和水泵等执行机构输出指令,实现潜航器的运动控制。同时,还需要根据漏水检测器和电压检测器的数据进行健康管理,保证潜航器在水面和水下处于正常的工作状态。 [0142] 以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。 |