开云优惠体育网页版入口

专利名称：基于改进蚁群算法和改进人工势场法的USV避碰方法

专利类型：实用新型专利

专利申请号：CN202211054163.9

专利申请（专利权）人：天津津航计算技术研究所
权利人地址：天津市东丽区空港经济区保税路357号

专利发明（设计）人：郭峰

专利摘要：本申请提供一种基于改进蚁群算法和改进人工势场法的USV避碰方法，包括以下步骤：构建坐标系，包括全局坐标系和局部坐标系；设计实时避碰规划的优化窗口；采用可视图法构建障碍模型；判断障碍物距离船体的距离选取实时避碰方法：若障碍物距离船体小于设定距离，采用改进人工势场法进行实时避碰，若障碍物距离船体大于设定距离，采用改进蚁群算法进行避碰。本申请提供的一种基于改进蚁群算法和改进人工势场法的USV避碰方法可对未知静态环境下突然出现的障碍物进行紧急避碰，并对下一时刻的路径进行规划，同时避免了传统人工势场法对只能根据距离做出避碰规划的问题，提高了避碰的实时性和有效性。

主权利要求：
1.基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于，包括以下步骤：S100、构建坐标系，包括全局坐标系和局部坐标系；
所述局部坐标系包括船体坐标系和导航器坐标系，所述船体坐标系为以USV为原点，USV航行方向为X轴的直角坐标系，所述导航器坐标系为以USV为原点，USV航行方向为极轴的极坐标系；
S200、设计实时避碰规划的优化窗口：
建立以USV为中心且随USV同航行的优化窗口，所述优化窗口的大小设定位USV导航器探测距离，每隔设定时间对所述优化窗口进行更新，同时对USV进行一次避碰规划；
S300、采用可视图法构建障碍模型：
可视图法包括可视图的膨胀方法和可视图的构建方法，所述膨胀方法采用矢量面积法判断凹凸性，构建方法采用相对位置检测判断法判断可视性；
S400、判断障碍物距离船体的距离选取实时避碰方法：
若障碍物距离船体小于设定距离，采用改进人工势场法进行实时避碰，具体方法为：S411、将势场设置为椭圆形，确定椭圆势场的形状；
S412、计算障碍模型的任意可视顶点 的势值 ；
S413、对 进行判断，若 ≥1，则USV按原路径航行，若 ＜1，计算USV下一时刻的线速度和角速度；
若障碍物距离船体大于设定距离，采用改进蚁群算法进行避碰；
采用改进蚁群算法进行避碰的步骤包括：
S421、使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点 都定义一个 链表存储其可视点集；
S422、初始化 ，设置历史最优保留代数计数器count=0、进化代数计数器G_count=
0、历史最优最大保留代数Max、最大进化代数Generation、初始历史最优蚂蚁history_best的路径代价∞、每代蚂蚁的种群数m；
S423、设置蚂蚁种群编号 ；
S424、若k>m，转到S427，否则把蚂蚁k放置在起始位置 上，转到S425；
S425、设此时蚂蚁当前位置为 ，若 的可视点链表 为空，则这只蚂蚁死亡，k=k+1，转到S424；
S426、若蚂蚁当前位置为终点 ，则蚂蚁找到完整路径，k=k+1，转到S424，否则，将本路径点从 中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到S425；
S427、进化代数计数器G_count=G_count+1，若本代蚂蚁中最优蚂蚁比历史最优蚂蚁找到的路径更优，更新初始历史最优蚂蚁history_best并且设置历史最优保留代数计数器count=0，否则，历史最优保留代数计数器count=count+1；
S428、历史最优保留代数计数器count>历史最优最大保留代数Max或者进化代数计数器G_count≥最大进化代数Generation，优化方法停止，否则基于丛林原则和最大?最小蚂蚁系统对蚁群信息素进行全局更新处理，转到S423；
S429、将导航器探测信息及目标点信息输入至基于改进蚁群算法的USV静态避碰规划器，获得USV下一时刻的转艏及速度的调整命令。
2.根据权利要求1所述的基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于，确定椭圆势场形状具体为：以船体为坐标系原点建立平面直角坐标系，椭圆的四个顶点的坐标分别为 、、 、 ，所述椭圆势场的形状为 。
3.根据权利要求2所述的基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于， 的计算公式如下所示：其中， ， ， ， 。
4.根据权利要求3所述的基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于，USV下一时刻的线速度和角速度的计算公式如下所示：式中， 和 分别为USV规划后得到的角速度和线速度， 和 分别为USV最大转弯角速度和当前转弯角速度， 和 分别为USV当前线速度和最大线速度， 和 为强度因子。
5.根据权利要求4所述的基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于，所述丛林原则的公式如下所示：；
式中，为经过 个时刻，蚂蚁完成一次循环， 表示信息素强度， 为全局信息素挥发系数， 为蚂蚁在本次循环中所走路径的长度， 为本次循环中蚂蚁在路径商的信息素增量， 为蚂蚁的种群数， 为所有蚂蚁在本次循环中所走出的最差路径长度， 为本次循环中最差路径上蚂蚁在路径 上的信息素增量。
6.根据权利要求5所述的基于改进蚁群算法和改进人工势场法的USV避碰方法，其特征在于，所述最大?最小蚂蚁系统的公式如下图所示：式中， 、 为自行设置的信息素浓度上下限。 说明书 : 基于改进蚁群算法和改进人工势场法的USV避碰方法技术领域[0001] 本公开一般涉及水面无人艇避碰规划方法领域，具体涉及基于改进蚁群算法和改进人工势场法的USV避碰方法。背景技术[0002] USV作为海上的智能化工具，USV避碰规划既是自动化的重要标识，也是自主航行的核心，所以USV完成任务的前提是其能够智能化避碰。传统的避碰规划方法包括蚁群算法、粒子群算法、遗传优化算法、人工势场法、深度学习方法等。针对USV智能化避碰，系统方法除了安全度和平滑性之外，实时性也是重要的考核指标，蚁群算法作为避碰的常规方法被广泛应用，但是蚁群算法针对未知静态环境下短距离中突然出现的障碍物没办法做出有效的紧急避碰，同时由于算法本身收敛慢，无法在突发情况下进行紧急避碰，影响USV最佳的安全避碰时间；人工势场法在紧急避碰情况下可以给出较好的反应，但是传统人工势场法仅考虑障碍物与USV的距离，进而判断障碍物与USV之间的斥力大。荒芘卸险习欠穸院叫胁跋，所以避碰方法亟需改进。发明内容[0003] 鉴于现有技术中的上述缺陷或不足，期望提供一种基于改进蚁群算法的改进人工势场法的USV避碰方法以解决上述问题。[0004] 本申请提供一种基于改进蚁群算法的改进人工势场法的USV避碰方法，包括以下步骤：[0005] S100、构建坐标系，包括全局坐标系和局部坐标系；[0006] 所述局部坐标系包括船体坐标系和导航器坐标系，所述船体坐标系为以USV为原点，USV航行方向为X轴的直角坐标系，所述导航器坐标系为以USV为原点，USV航行方向为极轴的极坐标系；[0007] S200、设计实时避碰规划的优化窗口：[0008] 建立以USV为中心且随USV同航行的优化窗口，所述优化窗口的大小设定位USV导航器探测距离，每隔设定时间对所述优化窗口进行更新，同时对USV进行一次避碰规划；[0009] S300、采用可视图法构建障碍模型：[0010] 可视图法包括可视图的膨胀方法和可视图的构建方法，所述膨胀方法采用矢量面积法判断凹凸性，构建方法采用相对位置检测判断法判断可视性；[0011] S400、判断障碍物距离船体的距离选取实时避碰方法：[0012] 若障碍物距离船体小于设定距离，采用改进人工势场法进行实时避碰，具体方法为：[0013] S411、将势场设置为椭圆形，确定椭圆势场的形状；[0014] S412、计算障碍模型的任意可视顶点(x,y)的势值vp；[0015] S413、对vp进行判断，若vp≥1，则USV按原路径航行，若vp＜1，计算USV下一时刻的线速度和角速度；[0016] 若障碍物距离船体大于设定距离，采用改进蚁群算法进行避碰。[0017] 根据本申请实施例提供的技术方案，确定椭圆势场形状具体为：[0018] 以船体为坐标系原点建立平面直角坐标系，椭圆的四个顶点的坐标分别为(0，yr)、(0，yf)、(xf,yxf)、(xf,?yxf)，所述椭圆势场的形状为V(yf,yr,yxf,xf)。[0019] 根据本申请实施例提供的技术方案，vp的计算公式如下所示：[0020] vp＝(y2+(ay+c)x2)/(py+q)[0021] 其中，p＝yf?yr，q＝yf?yr，[0022] 根据本申请实施例提供的技术方案，USV下一时刻的线速度和角速度的计算公式如下所示：[0023] γ＝max(1?vp,0)[0024][0025][0026] 式中，ωr和vl分别为USV规划后得到的角速度和线速度，ωmax和ωpre分别为USV最大转弯角速度和当前转弯角速度，vpre和vmax分别为USV当前线速度和最大线速度，α和β为强度因子。[0027] 根据本申请实施例提供的技术方案，采用改进蚁群算法进行避碰的步骤包括：[0028] S421、使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点gi都定义一个allowedi链表存储其可视点集；[0029] S422、初始化τij(0)，设置历史最优保留代数计数器count＝0、进化代数计数器G_count＝0、历史最优最大保留代数Max、最大进化代数Generation、初始历史最优蚂蚁history_best的路径代价∞、每代蚂蚁的种群数m；[0030] S423、设置蚂蚁种群编号k＝1；[0031] S424、若k>m，转到S427，否则把蚂蚁k放置在起始位置gs上，转到S425；[0032] S425、设此时蚂蚁当前位置为gi，若gi的可视点链表allowedi为空，则这只蚂蚁死亡，k＝k+1，转到S424；[0033] S426、若蚂蚁当前位置为终点ge，则蚂蚁找到完整路径，k＝k+1，转到S424，否则，将本路径点从allowedi中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到S425；[0034] S427、进化代数计数器G_count＝G_count+1，若本代蚂蚁中最优蚂蚁比历史最优蚂蚁找到的路径更优，更新初始历史最优蚂蚁history_best并且设置历史最优保留代数计数器count＝0，否则，历史最优保留代数计数器count＝count+1；[0035] S428、历史最优保留代数计数器count>历史最优最大保留代数Max或者进化代数计数器G_count≥最大进化代数Generation，优化方法停止，否则基于丛林原则和最大?最小蚂蚁系统对蚁群信息素进行全局更新处理，转到S423。[0036] S429、将导航器探测信息及目标点信息输入至基于改进蚁群算法的USV静态避碰规划器，获得USV下一时刻的转艏及速度的调整命令。[0037] 根据本申请实施例提供的技术方案，所述丛林原则的公式如下所示：[0038][0039][0040][0041] 式中，n为经过n个时刻，蚂蚁完成一次循环，Q表示信息素强度，α为全局信息素挥发系数，LALL为蚂蚁在本次循环中所走路径的长度， 为本次循环中蚂蚁在路径(i,j)商的信息素增量，m为蚂蚁的种群数，LBAD为所有蚂蚁在本次循环中所走出的最差路径长度，Δτij(t)为本次循环中最差路径上蚂蚁在路径(i,j)上的信息素增量。[0042] 根据本申请实施例提供的技术方案，所述最大?最小蚂蚁系统的公式如下所示：[0043][0044] 式中，τmax、τmin为自行设置的信息素浓度上下限。[0045] 与现有技术相比，本申请的有益效果在于：本申请结合改进人工势场法和改进蚁群算法对未知静态环境下USV的航行进行避碰规划，通常情况下采用改进蚁群算法进行常规避碰，针对突然出现且对航行产生影响的障碍物采用改进人工势场法进行避碰，同时利用改进蚁群算法弥补人工势场法只能紧急避碰，不能进行路径规划的缺点；通过在不同情况下选择有针对性的方法进行避碰，增强了避碰的实时性，提高了避碰效率；同时通过改进人工势场法，将势场设置为椭圆形，避免了传统人工势场法对只能根据距离做出避碰规划的问题，加入了障碍物是否对航行造成影响的判断条件，提高了避碰的有效性。附图说明[0046] 通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：[0047] 图1为本申请提供的基于改进蚁群算法和改进人工势场法的USV避碰方法流程图；[0048] 图2为本申请的USV坐标示意图；[0049] 图3为本申请的导航器探测未知障碍的示意图；[0050] 图4为本申请的多边形障碍膨化示意图；[0051] 图5为本申请的矢量面积法示意图；[0052] 图6为本申请的膨化原理示意图；[0053] 图7为本申请基于相对位置判断可视性示意图；[0054] 图8为本申请中障碍物对人工势场法的影响示意图；[0055] 图9为本申请基于方位椭圆势场的人工势场模型；[0056] 图10为本申请改进人工势场法势值参数定义示意图；[0057] 图11为本申请的USV避碰规划结果对比图。具体实施方式[0058] 下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。[0059] 需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。[0060] 请参考图1，本申请提供一种基于改进蚁群算法和改进人工势场法的USV避碰方法，包括以下步骤：[0061] S100、构建坐标系，包括全局坐标系和局部坐标系；[0062] 所述局部坐标系包括船体坐标系和导航器坐标系，所述船体坐标系为以USV为原点，USV航行方向为X轴的直角坐标系，所述导航器坐标系为以USV为原点，USV航行方向为极轴的极坐标系；[0063] 具体的，在某一实施例中，所述全局坐标系采用北东坐标系，地图左上角为原点，正东方向为X轴，正北方向为Y轴。[0064] 具体的，如图1所示，USV所在位置的全局坐标为(xu,yu)，αu是USV航行方向与正东方向XG的夹角，导航器安装于船体坐标系XUOUYU下一点(xe,0)处，即USV在航行过程中，导航器坐标系P(di,θi)相对于船体坐标系XUOUYU的位置是固定不变的，其中Qi为被导航器探测到的静态障碍且在导航器坐标系P(di,θi)下的坐标为(di,θi)。[0065] S200、设计实时避碰规划的优化窗口：[0066] 建立以USV为中心且随USV同航行的优化窗口，所述优化窗口的大小设定位USV导航器探测距离，每隔设定时间对所述优化窗口进行更新，同时对USV进行一次避碰规划；[0067] 具体的，避碰规划时仅对当前滚动窗口中的障碍物进行建模，随着环境窗口不断的向前滚动，可对USV不断地进行动态避碰规划；所述优化窗口具有以下特点：[0068] (1)每次规划，将历史已知障碍纳入规划中，为算法增加“记忆”，避免USV避碰规划陷入局部最优；[0069] (2)规划采用未知且新发现的障碍触发机制，一方面可以提高算法对障碍的避碰规划响应速度，另一方面能避免频繁触发规则造成资源浪费。[0070] S300、采用可视图法构建障碍模型：[0071] 导航雷达通过发射电磁波并且接收障碍物反射回来的波对周围环境进行探测，即电磁波与障碍物的表面交汇处为反射点或障碍点，根据反射点到导航雷达的距离和角度，可知个障碍顶点相对于导航雷达的位置信息，并将该位置信息通过坐标变换转化为相对于USV质心的位置信息，从而完成对USV优化窗口中的局部静态未知环境的探测，进而可采用可视图法对优化窗口中的局部静态未知环境进行建模。[0072] 所述坐标变换具体为：[0073] 如图2所示，若已知障碍在所述导航器坐标系XRORYR下坐标为(d,θ)，按照下式将其转换为所述船体坐标系XUOUYU下坐标(x',y')：[0074][0075] 也可按照下式将其转换为全局坐标系XGOGYG下坐标(x,y)：[0076][0077] 同理，若已知障碍在全局坐标系XGOGYG下的坐标为(x,y)，可按照下式将其转换为导航器坐标系XRORYR下坐标(d,θ)：[0078][0079] 可视图法包括可视图的膨胀方法和可视图的构建方法，所述膨胀方法采用矢量面积法判断凹凸性，构建方法采用相对位置检测判断法判断可视性。[0080] 可视图的膨胀方法：[0081] 对于椭圆形障碍物而言，根据实际需要设定安全距离h，将其膨化成同一椭圆圆心且更长的长短半径的椭圆(更长的长短半径是指考虑了安全距离)，运用勾股定理得到膨化后的椭圆的外接矩形。[0082] 对于多边形障碍物而言，一般分为凸多边形和凹多边形障碍，一般方法采用凸多边形障碍的膨化方式对凹多边形进行膨化，如图4(a)所示，这种膨化方式可能遗失避碰规划中的最优路径，通过可视图膨化后的障碍物如图4(b)所示。[0083] 首先，采用矢量面积法去判断多边形顶点是凸还是凹，如图5所示，设多边形ABCDEFG按顺时针顺序形成矢量，其面积公式如下所示：[0084] SABCDEFG＝SABba+SBCcb+SDEed+SCDdc+SEFfe+SGFfg+SAGga[0085] 如图5所示，SABCDEFG和SBCD按照矢量方向均为证，其中多边形ABba面积公式如下：[0086][0087] 若多边形顶点为凹点，则SABCDEFG×SCDE＜0；[0088] 若多边形顶点为凸点，则SABCDEFG×SBCD＞0。[0089] 其次，根据顶点凹凸性设计其膨胀法，如图6(a)所示，A为凸点，根据其相邻点F、B可求出其角平分线与X轴正方向的夹角α如下所示：[0090][0091] 则膨化所得点即为∠BAF角平分线的反向延长线与在多边形ABCDE外侧且与AB(或者AF)相距为h的平行线的交点如下所示：[0092][0093][0094] 同理，如图6(b)所示，F为凹点，其膨化所得点位∠AFE角平分线在多边形ABCDE外侧且与AF(或者FE)相距为h的平行线的交点。[0095] 可视图的构建方法：[0096] 基于相对位置检测的方法首先是求出P1与障碍各个顶点的距离d1、d2、d3、d4、d5，选出距离P1最近的两个顶点，并判断与P1最接近的两个顶点为端点的障碍物的边界线是否相交，如图7所示，即判断P1P2与AE、AB、BC是否相交。显然，若相交则路P1P2与障碍物发生冲突，若不想交则反之，将USV航线P1P2是否与障碍碰撞问题转化为判断P1P2与AE、AB、BC是否相交的问题。[0097] 根据点(x,y)到直线距离公式分别计算出点A、B到P1P2的距离为da和db，点P1、P2到AB的距离为dp1和dp2，其中距离是带有正负号的，公式如下：[0098][0099] 如图7所示，规定若点位于P1P2上方则距离小于0，反之则距离大于0。所以，当da×db＜0且dp1×dp2＜0时，则障碍物的边界线AB与P1P2相交；当da×db＜0且dp1×dp2≥0时，则障碍物的边界线AB与P1P2不相交；当da×db＞0，则障碍物的边界线AB与P1P2不相交；当da和db不同时为0时，说明障碍物的顶点A或者B在P1P2上，又由于P1P2与障碍物边界线AB只相交与一顶点，而障碍物已经膨化处理，从而认为AB与P1P2不相交；当da和db同时为0时，则说明障碍物边界线AB与P1P2在同一直线上，则障碍物边界线AB与P1P2也不相交。同理，可判断出P1P2与AE、BC是否相交，从而确定P1P2与该障碍物是否起冲突。[0100] S400、判断障碍物距离船体的距离选取实时避碰方法：[0101] 通过导航器探测障碍物的距离，若障碍物距离船体小于设定距离，采用改进人工势场法进行实时避碰。[0102] 具体的，传统人工势场法中，仅考虑了障碍距离，如图8所示，障碍A与USV距离大于障碍B，障碍A受到的斥力更。欢导是榭銮∏∠喾，障碍A在USV的航向上，而障碍B在USV右旁，基于方位考虑，障碍A的影响更大些；除此以外，由于改进人工势场法只负责紧急避碰，其重点只需安全避碰障碍即可，因此不考虑目标点的影响。综上，本申请中的改进人工势场模型由于考虑了方位因素，使人工势场法中的势场变成椭圆分布，如图9所示，障碍A和障碍B在同一势场线上，即它们对USV的影响是相同的。[0103] 改进人工势场法的具体方法为：[0104] S411、将势场设置为椭圆形，确定椭圆势场的形状；[0105] S412、计算障碍模型的任意可视顶点(x,y)的势值vp；[0106] S413、对vp进行判断，若vp≥1，则USV按原路径航行，若vp＜1，计算USV下一时刻的线速度和角速度。[0107] 当障碍物距离船体大于设定距离，采用改进蚁群算法进行避碰，具体步骤包括：[0108] S421、使用基于相对位置检测的可视性判断方法来统计包括起点和终点在内的所有已经经过膨化的环境障碍物顶点的可视点，每一个顶点gi都定义一个allowedi链表存储其可视点集；[0109] S422、初始化τij(0)，设置历史最优保留代数计数器count＝0、进化代数计数器G_count＝0、历史最优最大保留代数Max、最大进化代数Generation、初始历史最优蚂蚁history_best的路径代价∞、每代蚂蚁的种群数m；[0110] S423、设置蚂蚁种群编号k＝1；[0111] S424、若k>m，转到S427，否则把蚂蚁k放置在起始位置gs上，转到S425；[0112] S425、设此时蚂蚁当前位置为gi，若gi的可视点链表allowedi为空，则这只蚂蚁死亡，k＝k+1，转到S424；[0113] S426、若蚂蚁当前位置为终点ge，则蚂蚁找到完整路径，k＝k+1，转到S424，否则，将本路径点从allowedi中删除，以防止蚂蚁向回搜索，并按照状态转移规则寻找下一路径点，转到S425；[0114] S427、进化代数计数器G_count＝G_count+1，若本代蚂蚁中最优蚂蚁比历史最优蚂蚁找到的路径更优，更新初始历史最优蚂蚁history_best并且设置历史最优保留代数计数器count＝0，否则，历史最优保留代数计数器count＝count+1；[0115] S428、历史最优保留代数计数器count>历史最优最大保留代数Max或者进化代数计数器G_count≥最大进化代数Generation，优化方法停止，否则基于丛林原则和最大?最小蚂蚁系统对蚁群信息素进行全局更新处理，转到S423。[0116] S429、将导航器探测信息及目标点信息输入至基于改进蚁群算法的USV静态避碰规划器，获得USV下一时刻的转艏及速度的调整命令。[0117] 基于Qt平台，结合改进蚁群算法和改进人工势场法对静态未知环境中USV避碰规划进行仿真，避碰规划过程中对比如图11所示。[0118] 工作原理：本申请结合改进人工势场法和改进蚁群算法对未知静态环境下USV的航行进行避碰规划，通常情况下采用改进蚁群算法进行常规避碰，针对突然出现且对航行产生影响的障碍物采用改进人工势场法进行避碰，同时利用改进蚁群算法弥补人工势场法只能紧急避碰，不能进行路径规划的缺点；通过在不同情况下选择有针对性的方法进行避碰，增强了避碰的实时性，提高了避碰效率；同时通过改进人工势场法，将势场设置为椭圆形，避免了传统人工势场法对只能根据距离做出避碰规划的问题，加入了障碍物是否对航行造成影响的判断条件，提高了避碰的有效性。[0119] 在一优选实施方式中，确定椭圆势场势场形状具体为：[0120] 以船体为坐标原点建立平面直角坐标系，椭圆的四个顶点的坐标分别为(0，yr)、(0，yf)、(xf,yxf)、(xf,?yxf)，所述椭圆势场的形状为V(yf,yr,yxf,xf)，如图10所示。[0121] 在一优选实施方式中，vp的计算公式如下所示：[0122] vp＝(y2+(ay+c)x2)/(py+q)[0123] 其中，p＝yf?yr，q＝yf?yr，[0124] 在一优选实施方式中，USV下一时刻的线速度和角速度的计算公式如下所示：[0125] γ＝max(1?vp,0)[0126][0127][0128] 式中，ωr和vl分别为USV规划后得到的角速度和线速度，ωmax和ωpre分别为USV最大转弯角速度和当前转弯角速度，vpre和vmax分别为USV当前线速度和最大线速度，α和β为强度因子。[0129] 在一优选实施方式中，所述丛林原则的公式如下所示：[0130][0131][0132][0133] 式中，n为经过n个时刻，蚂蚁完成一次循环，Q表示信息素强度，α为全局信息素挥发系数，LALL为蚂蚁在本次循环中所走路径的长度， 为本次循环中蚂蚁在路径(i,j)商的信息素增量，m为蚂蚁的种群数，LBAD为所有蚂蚁在本次循环中所走出的最差路径长度，Δτij(t)为本次循环中最差路径上蚂蚁在路径(i,j)上的信息素增量。[0134] 具体的，首先借鉴丛林原则去除每次循环中局部最差路径上蚂蚁释放的信息素，然后根据下式对每次循环中排在前三分之一的路径(按照从短到长排列)上的信息素做强化处理：[0135] τij(t+n)＝λ×τij(t+n)[0136][0137] 式中，D为起点到终点的欧式距离，Lk为第k只蚂蚁在本次循环中所走的路径长度。[0138] 通过基于丛林原则更新全局信息素克服传统蚁群算法收敛慢的缺点，提高蚁群算法的收敛速度以使得提高避碰规划的实时性。[0139] 在一优选实施方式中，所述最大?最小蚂蚁系统的公式如下所示：[0140][0141] 式中，τmax、τmin为自行设置的信息素浓度上下限。[0142] 通过基于最大?最小蚂蚁系统配合上述丛林原则对全局信息素进行更新，使得进一步提高避碰规划的实时性。[0143] 以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

专利地区：天津

专利申请日期：2022-08-31

专利公开日期：2024-11-29

专利公告号：CN115328147B