一种水面无人艇动态规划方法

摘要

本发明涉及一种水面无人艇动态规划方法，所述方法包括：鉴于水面无人艇在海面上碰到的动态障碍物大多为船舶，根据船舶的形状特性，对其建立椭圆模型；在建立椭圆模型基础上，对椭圆模型进行分析，确定船舶动态障碍物的影响范围，并在影响范围内构建椭圆斥力势场提供斥力，从而引导水面无人艇躲避动态障碍物；进一步考虑船舶动态障碍物运动的不稳定性和不可预测性，水面无人艇需要在短时间内避开动态障碍物，故采用变斥力系数方法，增加斥力，改变合力方向，从而达到快速引导水面无人艇避开动态障碍物的效果。

权利要求

1.一种水面无人艇动态规划方法，解决船舶动态障碍物建模困难、快速避障问题，其特征是：

步骤1：

设置人工势场法中引力系数μ和斥力系数η，水面无人艇的半长轴aq、半短轴bq、初始位置qs和目标点位置qg，船舶动态障碍物模型的半长轴ap、半短轴bp、半焦距cp、左焦点位置cp1和右焦点位置cp2，船舶动态障碍物的影响范围半长轴Ap和半短轴Bp需满足如下约束条件：然后进入步骤2；

步骤2：

判断水面无人艇当前位置q是否到达qg，是则结束，否则进入步骤3；

步骤3：

构建引力函数：

Fa(q)＝μ·d(q,qg)    (2)式中：Fa(q)为引力，d(q,qg)是一个矢量，模长为q与qg之间的距离，方向为q指向qg，引力Fa(q)对水面无人艇产生引力，在引力作用下，引导水面无人艇前往目标点；

r＝d(q,cp1)d(q,cp2)   (3)式中：r是一个矢量，r是利用平行四边形法则将d(q,cp1)和d(q,cp2)相加得到，d(q,cp1)是一个矢量，模长为q与cp1之间的距离，方向为cp1指向q，同理，d(q,cp2)也是一个矢量，模长为q与cp2之间的距离，方向为cp2指向q；

构建斥力函数：

式中：Fe(q)为斥力，斥力Fe(q)和r同向，|r|为矢量r的模长，当水面无人艇在船舶动态障碍物的影响范围内，斥力Fe(q)对其产生斥力，让其远离船舶，然后进入步骤4；

步骤4：

判断斥力Fe(q)是否为0，是则进入步骤6，否则进入步骤5；

步骤5：

此时水面无人艇在船舶动态障碍物的影响范围内，为了尽快避开船舶动态障碍物，改变斥力系数η，从而在下一步运动中，增大斥力，斥力系数η变化公式：然后进入步骤6；

步骤6：

构建合力公式：

Ft(q)＝Fa(q)+Fe(q)    (6)式中：Ft(q)为合力，合力方向即水面无人艇下一步的运动方向，然后进入步骤7；

步骤7：

计算下一步运动，然后返回步骤2，循环往复。

说明书

一种水面无人艇动态规划方法

技术领域

[0001] 本发明涉及一种水面无人艇动态规划方法，属于海洋无人器运动操纵及动态规划领域。

背景技术

[0002] 由于海洋防卫等需求不断扩大，各国正在全力搭建海洋无人综合平台，而水面无人艇作为新型无人装备得到全面发展。但是水面无人艇运动操纵的动态规划尚不成熟。

[0003] 常用的动态规划方法有速度斥力场算法、动态窗口算法、视觉算法、人工势场法等。其中人工势场法较其他算法而言，具有运算时间短、实时性强、适应性好等方面的优势。
但是传统人工势场法在水面无人艇运动操纵及动态规划领域的应用有如下问题：

[0004] (1)动态障碍物建模困难问题，由于水面无人艇碰到动态障碍物大多为船舶，传统人工势场法将动态障碍物视为质点，斥力影响范围为圆形，船舶动态障碍物影响范围难以
确定而且所规划路径达不到安全保证；

[0005] (2)快速避障问题，传统人工势场法斥力系数和引力系数固定，产生的斥力是随着水面无人艇与船舶动态障碍物距离的减小而增大，而引力是随着水面无人艇与目标点距离
的减小而减小，水面无人艇的位置时刻变化，同时还需要考虑船舶动态障碍物运动的不稳
定性和不可预测性，系数固定难以实现快速可靠实时的动态规划。

[0006] 专利《基于椭圆聚类‑碰撞锥推演的无人艇海洋动态避障控制算法》提供的改进方法，存在以下问题：

[0007] (1)该发明对动态船只进行椭圆聚类，按照其尺寸形态进行聚类成动态椭圆障碍物，但是未明确椭圆障碍物的形状，所规划出的路径得不到安全保障；

[0008] (2)该发明需要采集动态障碍物的运动状态，获取信息量大，计算难度高，实时性差。

[0009] 综上所述，如何解决人工势场法在水面无人艇动态环境下的应用已成为亟待解决的难点。

发明内容

[0010] 本发明的目的是提出一种水面无人艇动态规划方法，解决传统人工势场法应用在水面无人艇动态规划过程中的动态障碍物建模困难、快速避障等问题。

[0011] 本发明为了解决上述问题采用以下技术方案：设计了一种水面无人艇动态规划方法，对船舶动态障碍物建立椭圆等效模型，设置满足约束条件的影响范围；根据引力和斥力
关系改变斥力系数，从而在下一次运动中，增大斥力，改变合力，快速避障，实现高效可靠实
时的动态规划。具体包括以下步骤：

[0012] 步骤1：

[0013] 设置人工势场法中引力系数μ和斥力系数η，水面无人艇的半长轴aq、半短轴bq、初始位置qs和目标点位置qg，船舶动态障碍物模型的半长轴ap、半短轴bp、半焦距cp、左焦点位
置cp1和右焦点位置cp2，船舶动态障碍物的影响范围半长轴Ap和半短轴Bp需满足如下约束条
件：

[0014]

[0015] 然后进入步骤2；

[0016] 步骤2：

[0017] 判断水面无人艇当前位置q是否到达qg，是则结束，否则进入步骤3；

[0018] 步骤3：

[0019] 构建引力函数：

[0020] Fa(q)＝μ·d(q,qg)     (2)

[0021] 式中：Fa(q)为引力，d(q,qg)是一个矢量，模长为q与qg之间的距离，方向为q指向qg，引力Fa(q)对水面无人艇产生引力，在引力作用下，引导水面无人艇前往目标点；

[0022] r＝d(q,cp1)+d(q,cp2)       (3)

[0023] 式中：r是一个矢量，r是利用平行四边形法则将d(q,cp1)和d(q,cp2)相加得到，d(q,cp1)是一个矢量，模长为q与cp1之间的距离，方向为cp1指向q，同理，d(q,cp2)也是一个矢
量，模长为q与cp2之间的距离，方向为cp2指向q；

[0024] 构建斥力函数：

[0025]

[0026] 式中：Fe(q)为斥力，斥力Fe(q)和r同向，|r|为矢量r的模长，当水面无人艇在船舶动态障碍物的影响范围内，斥力Fe(q)对其产生斥力，让其远离船舶，然后进入步骤4；

[0027] 步骤4：

[0028] 判断斥力Fe(q)是否为0，是则进入步骤6，否则进入步骤5；

[0029] 步骤5：

[0030] 此时水面无人艇在船舶动态障碍物的影响范围内，为了尽快避开船舶动态障碍物，改变斥力系数η，从而在下一步运动中，增大斥力，斥力系数η变化公式：

[0031]

[0032] 然后进入步骤6；

[0033] 步骤6：

[0034] 构建合力公式：

[0035] Ft(q)＝Fa(q)+Fe(q)      (6)

[0036] 式中：Ft(q)为合力，合力方向即水面无人艇下一步的运动方向，然后进入步骤7；

[0037] 步骤7：

[0038] 计算下一步运动，然后返回步骤2，循环往复。

[0039] 本发明具有如下有益效果：

[0040] 1.本发明所述方法建立船舶动态障碍物椭圆等效模型，确定船舶动态障碍物的影响范围，在影响范围内构建斥力势场，对无人艇产生斥力，保证无人艇能够顺利避开动态障
碍物，达到安全航行的要求；

[0041] 2.本发明所述方法采用变斥力系数方法，改变斥力，进而改变合力，从而达到水面无人艇快速避开船舶动态障碍物的效果，实现高效可靠实时的动态规划；

[0042] 3.本发明所述方法明确动态椭圆障碍物的形状，使建立的模型准确，所规划出的路径安全；

[0043] 4.本发明所诉方法主要是借助位置信息，获取难度低，计算量小，实时性好，能达到快速避障的目的。

附图说明

[0044] 图1为水面无人艇动态规划方法流程图；

[0045] 图2为船舶动态障碍物椭圆模型；

[0046] 图3为水面无人艇受力分析图；

[0047] 图4为t＝1s时水面无人艇动态规划图；

[0048] 图5为t＝14s时水面无人艇动态规划图；

[0049] 图6为t＝28s时水面无人艇动态规划图；

[0050] 图7为t＝47s时水面无人艇动态规划图；

[0051] 图8为t＝62s时水面无人艇动态规划图；

[0052] 图9为t＝68s时水面无人艇动态规划图；

[0053] 图10为t＝69s时水面无人艇动态规划图；

[0054] 图11为t＝81s时水面无人艇动态规划图；

[0055] 图12为t＝97s时水面无人艇动态规划图；

[0056] 图13为t＝122s时水面无人艇动态规划图。

具体实施方式

[0057] 图1为本发明水面无人艇动态规划方法流程图，包括以下步骤：

[0058] 步骤1：

[0059] 设置人工势场法中引力系数μ和斥力系数η，水面无人艇的半长轴aq、半短轴bq、初始位置qs和目标点位置qg，船舶动态障碍物模型的半长轴ap、半短轴bp、半焦距cp、左焦点位
置cp1和右焦点位置cp2，船舶动态障碍物的影响范围半长轴Ap和半短轴Bp需满足如下约束条
件：

[0060]

[0061] 船舶动态障碍物模型及其影响范围如图2所示，然后进入步骤2；

[0062] 步骤2：

[0063] 判断水面无人艇当前位置q是否到达qg，是则结束，否则进入步骤3；

[0064] 步骤3：

[0065] 构建引力函数：

[0066] Fa(q)＝μ·d(q,qg)     (2)

[0067] 式中：Fa(q)为引力，d(q,qg)是一个矢量，模长为q与qg之间的距离，方向为q指向qg，引力Fa(q)对水面无人艇产生引力，在引力作用下，引导水面无人艇前往目标点；

[0068] r＝d(q,cp1)+d(q,cp2)     (3)

[0069] 式中：r是一个矢量，r是利用平行四边形法则将d(q,cp1)和d(q,cp2)相加得到，d(q,cp1)是一个矢量，模长为q与cp1之间的距离，方向为cp1指向q，同理，d(q,cp2)也是一个矢
量，模长为q与cp2之间的距离，方向为cp2指向q。

[0070] 构建斥力函数：

[0071]

[0072] 式中：Fe(q)为斥力，斥力Fe(q)和r同向，|r|为矢量r的模长，当水面无人艇在船舶动态障碍物的影响范围内，斥力Fe(q)对其产生斥力，让其远离船舶，然后进入步骤4；

[0073] 步骤4：

[0074] 判断斥力Fe(q)是否为0，是则进入步骤6，否则进入步骤5；

[0075] 步骤5：

[0076] 此时水面无人艇在船舶动态障碍物的影响范围内，为了尽快避开船舶动态障碍物，改变斥力系数η，从而在下一步运动中，增大斥力，斥力系数η变化公式：

[0077]

[0078] 然后进入步骤6；

[0079] 步骤6：

[0080] 构建合力公式：

[0081] Ft(q)＝Fa(q)+Fe(q)       (6)

[0082] 式中：Ft(q)为合力，合力方向即水面无人艇下一步的运动方向，如图3所示，q为无人艇当前位置，qg为目标点，cp1和cp2为船舶椭圆模型的左右焦点，Fa(q)为引力，Fe(q)为斥
力，利用平行四边形法则合成合力Ft(q)，然后进入步骤7；

[0083] 步骤7：

[0084] 计算下一步运动，然后返回步骤2，循环往复。

[0085] 仿真结果如图4‑图13所示，其中实线为无人艇路径，点画线为船舶路径，箭头表示各自运动方向。t＝1s时如图4所示，无人艇朝着目标点出发；t＝14s时如图5所示，无人艇进
入船舶椭圆模型的影响范围；图6、图7、图8、图9为无人艇在船舶动态障碍物的影响范围内
合力作用下的运动，快速离开船舶动态障碍物的影响范围；图10为无人艇离开船舶动态障
碍物影响范围后，继续朝着目标点前进；图11为无人艇在引力作用下，又进入船舶动态障碍
物影响范围，再次快速离开船舶动态障碍物影响范围；图12为无人艇朝着目标点前进；图13
为无人艇最终到达目标点。