第4章 基于PSO并融合海事规则的已知动态路径规划方法

水面无人艇局部危险避障算法研究

Local Risk Obstacle Avoidance Algorithm of USV

博主 的硕士毕业论文

第4章 基于PSO并融合海事规则的已知动态路径规划方法

　　水面无人艇在航行期间，由硬件（如AIS、航海雷达等）可探测到在电子海图上没有标示的运动障碍物，并预测其运动信息。无人艇必须对这些已知静态障碍物进行规避，并符合国际海事避碰规则公约相关规则，使其能够快速、安全的到达终点或子目标点。本章将针对障碍信息已知的动态障碍物路径规划问题展开研究，将动态障碍物某一运动时刻转换为相对无人艇瞬时静止的，并实时的进行解算。最后通过仿真实验验证了本章所提出算法的可行性。

4.1 动态已知障碍物避障模型

　　本文所提出的已知动态路径规划方法，是基于粒子群优化算法进行的，与上一章采用PSO算法不同的是：上一章粒子群算法是针对所有静态障碍物而言的，其维度是障碍物所产生的维度圆的个数，随着环境中静态障碍物的变化而改变的，且其优化调整变量为各个维度圆上的路径点极角；而本章粒子群算法则是针对子目标段的运动障碍物而言的，其维度数是固定的二维，分别为无人艇的航行速度和期望航向，且其优化调整变量即为这两个值。

　　与上一章相对比，优化的目标也不尽相同。上一章PSO算法的优化目标为路径最短或能量消耗最少；本章采用的PSO算法优化目标则为，在考虑了海事规则的前提下尽快逃离障碍物的运动区域。

　　本节将对动态已知障碍物避障模型[35]进行分析和数学推导。

　　相比上一章所解决的静态已知障碍物存储结构而言，动态已知障碍物的存储结构还需要在静态已知障碍物上加入运动速度和方向角。



图4-1 避障模型示意图

　　首先考虑建立圆形障碍物几何模型。建立无人艇和障碍物当前位置的几何模型，如图4-1所示。图中无人艇和障碍物都是运动物体，VUSV是无人艇的运动速度，VObz是障碍物的运动速度。其中 α为极轴到VUSV的角度，可表示为α=∠(VUSV,ex)，其中ex表示极轴；同理β=∠(VObs,ex)为极轴到VObz的转角；θ=∠(LRO,ex)为极轴到无人艇与障碍物圆心连线的转角；φ=∠(VUSV,ΔV)为ΔV到VUSV的转角；γ=∠(ΔV,LRO)， μ=∠(LRO,tanL)，tanL代表障碍圆切线。

　　为使无人艇能在下一时刻避开障碍区间， γ应该取在(LRO−μ,LRO+μ)范围之外的角度。下面将通过求解γ，找到无人艇航速和转向需要满足的条件，以实现无人艇避障。



图4-2 相对速度ΔV分解示意图

　　如图4-2所示，将Δν分解为指向障碍物圆心的速度分量Δν0，和其垂直速度分量Δνr。其中Δν0促使无人艇朝向障碍物运动，Δνr促使无人艇逃离障碍物。

　　{Δνr=νUSVsin(α−θ)−νObssin(β−θ)Δνo=νUSVcos(α−θ)−νObscos(β−θ)　　　(4-1)

　　

　　　tgγ=νUSVsin(α−θ)−νObssin(β−θ)νUSVcos(α−θ)−νObscos(β−θ)　　(4-2)

　　　

　　　γ=tg−1(νUSVsin(α−θ)−νObssin(β−θ)νUSVcos(α−θ)−νObscos(β−θ))　　(4-3)

　　

　　dγ=d{tg−1(νUSVsin(α−θ)−νObssin(β−θ)νUSVcos(α−θ)−νObscos(β−θ))}=d(tg−1f(νUSV,α,νObs,β))=11+f2df　　　(4-4)

其中：

　　11+f2=11+(νUSVsin(α−θ)−νObssin(β−θ)νUSVcos(α−θ)−νObscos(β−θ))2=(νUSVcos(α−θ)−νObscos(β−θ))2(νUSVcos(α−θ)−νObscos(β−θ))2+(νUSVsin(α−θ)−νObssin(β−θ))2=(νUSVcos(α−θ)−νObscos(β−θ))2νUSV2+νObs2−2νUSVνObscos(α−β)　　　(4-5)

　　

　　　df=df(νusv,α,νObs,β)=∂f∂νusvdνusv+∂f∂αdα+∂f∂νObsdνObs+∂f∂βdβ　　(4-6)

　　　

　　在式(4-6)中，假设圆形障碍物运动不发生突变，其运动速度νObs和运动方向β在很短的时间变化很小，可以忽略不计，即dνObs=0，dβ=0 ，则有：

　　

　　∂f∂νUSVdνUSV=νObssin(β−α)(νUSVcos(α−θ)−νObscos(β−θ))2dνUSV　　　(4-7)

　　　　　

　　∂f∂αdα=νUSV2−νObsνUSVcos(α−β)(νUSVcos(α−θ)−νObscos(β−θ))2dα　　　(4-8)

　　　　　

　　　df=∂f∂νUSVdνUSV+∂f∂αdα=νObssin(β−α)(νUSVcos(α−θ)−νObscos(β−θ))2dνUSV+νUSV2−νObsνUSVcos(α−β)(νUSVcos(α−θ)−νObscos(β−θ))2dα　　(4-9)

　　　　　

　　所以式(4-6)可以转化为：

　　dγ=νObssin(β−α)νusv2+νObs2−2νusvνObscos(α−β)dνusv+νusv2−νObsνusvcos(α−β)νusv2+νObs2−2νusvνObscos(α−β)dα　　　(4-10)

　　也可以写成：

　　Δγ=νObssin(β−α)νusv2+νObs2−2νusvνObscos(α−β)Δνusv+νusv2−νObsνusvcos(α−β)νusv2+νObs2−2νusvνObscos(α−β)Δα　　　(4-11)

　　　　　

　　在νUSV 、νObs 、Δν 构成的速度三角形（如图4-2）中，存在(4-12)关系式，其中φ为νUSV 、νObs夹角，将其带入(4-11)，可得到式(4-13)。

　　　⎧⎩⎨⎪⎪νObssin(β−α)=ΔνsinφνUSV−νObscos(β−α)=ΔνcosφνUSV2+νObs2−2νUSVνObscos(β−α)=Δν2　　(4-12)

　　　　　

　　　Δγ=−sinφΔνΔνUSV+νUSVcosφΔνΔα　　(4-13)

　　　　　

　　至此， Δγ已经求出，要使无人艇逃离危险区域(LRO−μ,LRO+μ)范围， Δγ需要满足下面的不等式：

　　{γ≥μ−r,　　if　　γ>0γ≤−(μ+r),　　if　　γ<0　　　(4-14)

　　　　　

　　式(4-13)说明了Δγ同无人艇的航速改变量ΔνUSV和航向改变量Δα存在关联，于是避障任务就转化成对无人艇此两值的求解。调整无人艇的航速ΔνUSV和航向Δα，分别对应了无人艇的两种有效避障行为，若仅仅采取其中的一个基本就能完成避障任务。

　　以上为建立圆形障碍物几何模型，及求解避障约束条件的过程；矩形障碍物几何模型的建立，及求解过程同圆形障碍物，在此不再赘述。

4.2 国际海上避碰规则公约的约束

　　水面无人艇在海面航行过程中，为了避免对海上其它船只、人员的人身和财产安全，及无人艇自身的安全造成威胁，在进行避障的过程中必须满足一定的规则。国际海上避碰规则公约(International Regulations for Preventing Collisions At Sea，1972 / COLREGS)，是由国际海事组织制定，为了防止、避免海事船舶相撞而制定的海上交通规则。但现在还没有专门的法律法规对水面无人艇的航行行为做出明确的指示，较为合理的解决方法就是使无人艇遵守国际海上避碰规则公约[56]。

　　国际海上避碰规则公约的13、14、15条，分别对无人艇在航行过程中可能出现的碰撞局面：追越、正面相遇和交叉相遇，做出了规定。然而，这些规则只是针对船舶操作的一种行为约束，并没有明确给出在实际应用中的角度和范围。有关学者已经根据经验和大量测试，给定了相关的参数[57]。下面将针对无人艇的实际情况，给出本文对国际海上避碰规则公约相关条例的具体说明，其中海事规则冲突情景定义见图4-3。



图4-3 海事规则冲突情景定义



（1）追越避障模型示意图 （2）正面相遇避障模型示意图

（3）右舷交叉相遇避障模型示意图 （4）左舷交叉相遇避障模型示意图

图4-4 避障模型示意图

　　图4-4避障模型示意图罗列了四种情况的避障模型，图中红色矩形障碍物为无人艇在起始点“Start”处探测到该障碍物的位置，箭头为其运动方向的指向，紫色障碍物则为无人艇对该障碍物避障结束时，障碍物所处的位置。

　　以下无人艇和运动障碍物之间的航向角度差，均以无人艇航向方向为基准（即航向角为0°）进行描述。

　　追越：若无人艇高速航行与障碍物航向角度差在[ 315°，360°)之间时，接近危险距离，则障碍物应该在无人艇的右舷，即无人艇左侧航行；而角度差在[ 0°，45°]之间时，若无人艇速度大于障碍物速度为追越，则左侧通行（见图4-4(1)所示），否则右侧通行。

　　正面相遇：若夹角在[ 165°，195°]之间，接近危险距离，则无人艇同障碍物相遇碰撞，此时无人艇应右侧通行（见图4-4(2)所示）。

　　交叉相遇：若夹角在（45°，165°）之间，接近危险距离，则障碍物在无人艇右侧交叉相遇，此时无人艇应右侧通行（见图4-4(3)所示）；若夹角在（195°，315°）之间，接近危险距离，则障碍物在无人艇左侧交叉相遇，此时无人艇应左侧通行（见图4-4(4)所示）。

　　

4.3 基于PSO算法的已知动态路径规划算法

4.3.1 基于PSO算法的适应度函数的确定

　　无人艇在航行过程中，较多的使用的是不改变航速，仅改变航向的策略；但在某些情况下还需要同时改变航速和航向的策略，进行避障。该策略下是同时求解两个变量的最优值，无人艇路径规划问题就可以表达成一个多条件下的目标优化问题，如式(4-14)所示：

　　⎧⎩⎨⎪⎪⎪⎪⎪⎪f(ΔνUSV,Δα)=m1|ΔνUSV|+m2|Δα|−sinφΔνΔνUSV+νUSVcosφΔνΔα>=μ−γ(γ>=0)−sinφΔνΔνUSV+νUSVcosφΔνΔα<μ−γ(γ<0)　　　(4-15)

　　　　　

　　式中φ、μ 、γ 、Δν 已知， f(ΔνUSV,Δα)为要实现最优的目标，即为粒子群优化算法中的适应度函数。且计算得到最优解后还需判断是否在可行域内，即需要满足国际海上避碰规则公约的约束的同时，还需要满足最优解不与已知静态障碍物相交。若该段路径中存在多个已知动态障碍物，则只需对每个障碍物加入式(4-15)中第二、三个约束即可。

　　此粒子群优化算法适应度函数f(ΔνUSV,Δα)的选取，是考虑到尽可能少地改变已知静态环境下规划的最优路径和运动状态（包括航行速度和航向）。因已知静态环境下规划的最优路径已经包含了全局优化信息，所以已知动态算法就没有过多的考虑全局优化目标了。

4.3.2 避障结束点的确定

　　上述的算法只得到了实现避障后的最优航速和航向，对于避障路径点的选取，还需要确定无人艇实现成功规避障碍物时刻的避障结束点。

　　若障碍物是朝向无人艇运行，即无人艇和障碍物各自前进的途中，在未到达避障结束点之前，障碍物在全局中的极径在不断缩小，如图4-5(1)所示。图中ν¯usv为经过解算优化后的无人艇速度、Δν¯为解算优化后无人艇和障碍物的合速度。该情况下避障结束点的选取，可根据在避障模型中，将障碍物视为静止障碍物，以合速度 到达障碍物最远端（图中1/4圆弧段处）所耗费的时间t，作为避障结束所耗费的时间；进而转换到全局坐标系下，以此时间段t计算避障结束点的极径值和极角。



（1）交叉相遇情况 （2）追越情况

图4-5 避障结束点示意图

　　若障碍物正远离起始点运行，即无人艇和障碍物各自前进的途中，在未到达避障结束点之前，障碍物在全局中的极径在不断增大，如图4-5(2)所示。此情况下若还按上述方法进行处理，无疑会造成避障结束点远离本段子目标点。所以在此情况下，设置避障结束点即为本段起始点时障碍物所处位置的极径处为避障结束点处的极径。

　　综合以上两种求避障结束点的情形，在计算时选取两者极径较小那一点作为避障结束点。

　　以上为针对已知动态圆形障碍物的避障结束点选取方法，已知动态矩形障碍物的避障结束点选取方法同上。

4.3.3 算法描述

　　结合本章对基于PSO并融合海事规则的已知动态路径规划方法，给出用于该算法的具体过程，其流程图见图4-6所示：



图4-6 基于PSO并融合海事规则的已知动态路径规划方法流程图

　　在粒子群优化算法中，可行域判断是指要同时满足对静态已知障碍物的规避，和对符合海事规则约束的动态已知障碍物的规避，以及对无人艇最小回转直径的限制。还需要说明的是，在粒子群优化算法中，对无人艇采取海事规则约束时，需设定一个海事规则判断最大次数，若大于此值，就放弃采用海事规则约束，以免陷入局部最优，从而达到降低解算时间，实时快速避障的目的。

4.4 仿真实验结果及分析

　　本节将针对基于PSO并融合海事规则的已知动态路径规划方法在Visual C++平台上进行仿真分析。根据真实的船舶参数建立已知动态环境，并在此环境下进行仿真实验。

　　在本节仿真实验中，假定水面无人艇的全局最优路径为正东方，“Start”为起始点，“Goal”为终止点，始末点相距3000m；无人艇运行速度为40Kn，最大航速为50Kn。粒子群优化算法中惯性权重ω采用线型递减策略，且ωmax=0.95， ωmin=0.2；c1=c2=2；m1=1，m2=70；设置粒子群迭代次数50，粒子数50。



图4-7 基于PSO的已知动态路径规划方法仿真分析示意图

　　该环境下包括一个已知静态圆形障碍物SOC1（相关参数见表4-1），和一个已知静态矩形障碍物SOR1（相关参数见表4-2）。经过第一层基于PSO的已知静态路径规划方法解算，得到已知静态环境规划路径，路径节点见表4-3所示。

表 4-1 已知静态圆形障碍物SOC参数

序号	极径(m)	极角(rad)	直径(m)
1	1000	0.000	400

表 4-2 已知静态矩形障碍物SOR参数

序号	中心点极径(m)	中心点极角(rad)	半长(m)	半宽(m)	方向角(°)
1	1800	0.000	400	80	90

表 4-3 已知静态环境规划路径节点

路径点	极径(m)	极角(rad)
1	1000.00	0.259
2	1748.20	0.259
3	1950.64	0.242

　　该环境下还包括一个已知动态圆形障碍物DOC1（相关参数见表4-4），和三个已知动态矩形障碍物DOR1、DOR2、DOR3（相关参数见表4-5）。三个已知动态矩形障碍物，分别选取为上海外高桥制造的85,000立方米液化气船，18,000箱级集装箱船和106K DWT阿芙拉型原油轮。该环境下，为了方便计算，将动态障碍物的起始时刻位置均定为无人艇起始点时刻的位置。动态障碍物起始时刻的位置用粉红色进行表示，避障结束点时刻障碍物的位置用紫色进行表示，箭头方向即为障碍物运动方向。

　　经过第二层基于PSO并融合海事规则的已知动态路径规划方法解算，得到已知动态环境规划路径信息，路径节点相对于已知静态路径的改变量见表4-6所示，路径节点及无人艇航速和解算该段路径的耗时见表4-7所示。下面将对该环境下的已知动态避障进行简要的说明。

　　无人艇在起始时刻时，探测到已知动态矩形障碍物DOR1，并与无人艇呈185.17°相向运行，即夹角在[ 165°，195°]之间，为海事规则中的正面相遇情形。若无人艇不做改变继续向前行进，会同障碍物相遇碰撞，所以需要无人艇作出相应的调整，从障碍物左舷即无人艇右转向通行。所以无人艇在起始时刻就进行已知动态障碍物避障，通过该算法解算，耗时0.081s，得到的避障方法为向右舷转向11.34°，航速不变行进。无人艇运行到避障结束点后，就已成功完成了对障碍物DOR1的规避，此时便转向朝向本段的子目标点Path1行进。

　　无人艇继续运行到Path1位置时，探测到已知动态矩形障碍物DOR2，和已知动态圆形障碍物DOC1，并分别从无人艇两侧驶来，角度分别为75.17°和254.17°，为海事规则中的交叉相遇情形，需要分别从两个障碍物的后方通过，即对DOC1而言为右侧通行，对DOR2为左侧通行。无人艇在Path1时刻进行已知动态障碍物避障，通过该算法解算，耗时0.119s，得到的避障方法为航向不进行改变，航速从40Kn减少到27.14Kn行进。无人艇运行到避障结束点后，就已成功完成了对障碍物DOR2和DOC1的规避，此时便逐渐加速到40Kn继续朝向本段的子目标点Path2行进。

　　无人艇继续运行到Path3位置时，探测到已知动态矩形障碍物DOR3，并与无人艇呈32.06°运行，即角度差在[ 0°，45°]之间时，且无人艇速度（40Kn）大于障碍物速度（15Kn），为海事规则中的追越情形。此时需要无人艇作出相应的调整，从障碍物左舷即无人艇左转向通行。无人艇在Path3时刻进行已知动态障碍物避障，通过该算法解算，耗时0.078s，得到的避障方法为向左舷转向7.62°，航速增加到40.2Kn行进。无人艇运行到避障结束点后，就已成功完成了对障碍物DOR3的规避，此时便转向朝向本段的子目标点即终止点行进。

表 4-4 已知圆形动态障碍物DOC参数

序号	极径(m)	极角(rad)	直径(m)	方向角(°)	速度(Kn)
1	1379.28	5.989	80	90	20

表 4-5 已知矩形动态障碍物DOR参数

序号	中心点极径(m)	中心点极角(rad)	半长(m)	半宽(m)	方向角(°)	速度(Kn)
1	620.97	0.261	112	20	200	17
2	1786.17	0.594	200	28	269	25
3	2189.77	0.450	120	20	305	15

表 4-6 已知动态环境避障路径节点改变量

路径点	航向(°)	航速(Kn)
0	-11.34	0.00
1	0.000	-12.86
3	+7.62	+2.00

表 4-7 已知动态环境避障路径节点

路径点 极径(m)	极角(rad)	航速(Kn)	解算时间(s)
0	554.76	0.061	40
1	1541.84	0.259	27.14
3	2447.56	0.132	42

　　为了验证本章所设计的基于PSO并融合海事规则的已知动态路径规划方法，设立了上面的避障环境。该避障环境包括了海事规则中正面相遇、交叉相遇、追越的三种情形，以及多障碍路径段的避障。仿真实验结果及分析表明，该算法能够很好的完成对已知动态障碍物的规避，具有较强的环境适应能力、较快的解算速度、较好的避障效果。

　　本章的研究完善了水面无人艇对已知环境下障碍物的规避，从而通过两层粒子群优化算法实现了对已知静态和动态障碍物的规避。

博主 的硕士毕业论文

===========文档信息============

版权声明：非商用自由转载-保持署名-注明出处

署名(BY) ：dkjkls（dkj卡洛斯）

文章出处：http://blog.csdn.net/dkjkls