·346. 智能系统学报 第10卷 Ywo Lvo P 进行下一个规划周期，重复步骤1)~8)，直到目标 √T'oP LvoP-p 到达才结束。 △v0≤m+e,E 开始 Vvo Luo AISAPAR MILP+VO 雷达声呐探测 规划器 √YvoLuo下-pl 计算TCPADCPA 控制方法 △vio≥△yio-e,5 N Yuo Lvo USV P <是否危险 √vuoP LvoP-p2 小 下个 本体呐 会遇局面 △iw≥-T-f话 航速探 划分决萤 航向测 Yuo Lvo P 建模 √T"oP LcoP-pL 障碍物 声呐探测 达到目标 本体运动学 △vo≤△yio+ff 约束 范围约束 动力学约束 (5) 结 式中：e:+f=1,e:和f为二进制变量（取0或 约束建模 1),专为正实数，且远大于式(5)中不等式左侧可 图2在线路径规划流程 取的数值。 Fig.2 Flowchart of online path planning 式(5)确保在同一规划时间段[，：+△t]之 内，有且仅有1组（前2个或后2个）约束成立。此 3 仿真验证 处，为了遵守COLREGs规则，当右交叉相遇和对遇 为了说明此规划方法的有效性，采用实际的 时，e:=0,f=1,式(5)中前2不等式被激活，后2 USV动力学模型进行实验仿真，采用的USV模型是 个不等式失效：当左交叉相遇时，e:=1,f=0,前2 Ribcraft模型1o,其回转速度为l5.8/s,采用PID 个不等式失效，后2个不等式被激活。 控制器对其进行航行控制。下面给出对遇局面和交 2.4算法流程 叉局面的仿真实验结果，如图3和4。图3所示为 总结上述体系结构、并融合COLREGs的在线 对遇局面，其初始环境具体参数见表1。图4所示 路径规划流程图如图2。具体的算法流程如下： 为右交叉局面，其初始环境具体参数见表2。 1)通过APAR雷达获得自身的经纬度和航向， 仿真开发环境在MATLAB环境下。在仿真中 通过AIS接收器获得对方船舶的距离和航向。 用大圆代表USV,小圆代表运动障碍物，最上面的 2)通过公式计算出每艘船舶与USV之间的最 圆代表目标点。USV通过雷达进行探测，最大探测 近会遇时间TCPA和最近会遇距离DCPA。 距离为30nm。图3(a)和4(a)图表示初始状态，图 3)通过危险度判断模型判断是否处于危险状 3(b)和4(b)表示USV进行实时在线路径规划，每 态。如果危险则开启避碰行为进行第4)步，否则不 一个时间段规划出最优的速度矢量，图3(c)和4 实施避碰行为。 (c)表示最终到达目标点，图3(d)和4(d)表示在规 4)进行船舶与USV的会遇局面划分及行为决策。 划中的规划（期望）与USV实际的比较。图3(b)和 5)对本体运动学、动力学、探测范围、障碍物约 4(b)中，USV在此种情况下右转向避碰，符合COL- 束和目标函数进行建模。 REGs规则的规定。通过圆圈稀疏可知当危险解 6)将约束的建模加入MLP规划器中，由规划 除，规划时间变短，并符合COLREGs规定的从后方 器可获得这个规划间隔时间里USV期望的转向角 绕过障碍物的规定。 及速度。 表1对遇局面态势仿真环境 7)将期望的航向角和速度输入到航行控制器 Table 1 Simulation environment of head-on situation 中，通过航行控制器输出发动机的转速和舵角作用 于USV实际模型中，使USV能达到期望的航向和 物体 初始位置/nm初始速度/八(m·s1)半径/m 航速。 USV (0.00.0.00 (0.00.8.00) 12 8)检测是否达到目标，如果达到则结束，否则 运动障碍 (0.00.7.00)】 (-0.00.-5.00) 10－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ £π ＋ ｅｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ⩾ Δγ Ｒ ＵＯ － ｅｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ⩾－ π － ｆ ｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ £Δγ Ｌ ＵＯ ＋ ｆ ｉ ξ ì î í ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï （５） 式中： ｅｉ ＋ ｆ ｉ ＝ １， ｅｉ 和 ｆ ｉ 为二进制变量（ 取 ０ 或 １） ， ξ 为正实数，且远大于式（ ５）中不等式左侧可 取的数值。 式（５）确保在同一规划时间段 ［ｔ ｋ，ｔ ｋ ＋ Δｔ］ 之 内，有且仅有 １ 组（前 ２ 个或后 ２ 个）约束成立。 此 处，为了遵守 ＣＯＬＲＥＧｓ 规则，当右交叉相遇和对遇 时， ｅｉ ＝ ０， ｆ ｉ ＝ １，式（５）中前 ２ 不等式被激活，后 ２ 个不等式失效；当左交叉相遇时， ｅｉ ＝ １， ｆ ｉ ＝ ０，前 ２ 个不等式失效，后 ２ 个不等式被激活。 ２．４ 算法流程 总结上述体系结构、并融合 ＣＯＬＲＥＧｓ 的在线 路径规划流程图如图 ２。 具体的算法流程如下： １）通过 ＡＰＡＲ 雷达获得自身的经纬度和航向， 通过 ＡＩＳ 接收器获得对方船舶的距离和航向。 ２）通过公式计算出每艘船舶与 ＵＳＶ 之间的最 近会遇时间 ＴＣＰＡ 和最近会遇距离 ＤＣＰＡ。 ３）通过危险度判断模型判断是否处于危险状 态。 如果危险则开启避碰行为进行第 ４）步，否则不 实施避碰行为。 ４）进行船舶与 ＵＳＶ 的会遇局面划分及行为决策。 ５）对本体运动学、动力学、探测范围、障碍物约 束和目标函数进行建模。 ６）将约束的建模加入 ＭＩＬＰ 规划器中，由规划 器可获得这个规划间隔时间里 ＵＳＶ 期望的转向角 及速度。 ７）将期望的航向角和速度输入到航行控制器 中，通过航行控制器输出发动机的转速和舵角作用 于 ＵＳＶ 实际模型中，使 ＵＳＶ 能达到期望的航向和 航速。 ８）检测是否达到目标，如果达到则结束，否则 进行下一个规划周期，重复步骤 １） ～ ８），直到目标 到达才结束。 图 ２ 在线路径规划流程 Ｆｉｇ． ２ Ｆｌｏｗｃｈａｒｔ ｏｆ ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ３ 仿真验证 为了说明此规划方法的有效性，采用实际的 ＵＳＶ 动力学模型进行实验仿真，采用的 ＵＳＶ 模型是 Ｒｉｂｃｒａｆｔ 模型［１０］ ，其回转速度为 １５．８ °／ ｓ，采用 ＰＩＤ 控制器对其进行航行控制。 下面给出对遇局面和交 叉局面的仿真实验结果，如图 ３ 和 ４。 图 ３ 所示为 对遇局面，其初始环境具体参数见表 １。 图 ４ 所示 为右交叉局面，其初始环境具体参数见表 ２。 仿真开发环境在 ＭＡＴＬＡＢ 环境下。 在仿真中 用大圆代表 ＵＳＶ，小圆代表运动障碍物，最上面的 圆代表目标点。 ＵＳＶ 通过雷达进行探测，最大探测 距离为 ３０ ｎｍ。 图 ３（ａ）和 ４（ａ）图表示初始状态，图 ３（ｂ）和 ４（ｂ）表示 ＵＳＶ 进行实时在线路径规划，每 一个时间段规划出最优的速度矢量，图 ３（ ｃ） 和 ４ （ｃ）表示最终到达目标点，图 ３（ｄ）和 ４（ｄ）表示在规 划中的规划（期望）与 ＵＳＶ 实际的比较。 图 ３（ｂ）和 ４（ｂ）中，ＵＳＶ 在此种情况下右转向避碰，符合 ＣＯＬ⁃ ＲＥＧｓ 规则的规定。 通过圆圈稀疏可知当危险解 除，规划时间变短，并符合 ＣＯＬＲＥＧｓ 规定的从后方 绕过障碍物的规定。 表 １ 对遇局面态势仿真环境 Ｔａｂｌｅ １ Ｓｉｍｕｌａｔｉｏｎ ｅｎｖｉｒｏｎｍｅｎｔ ｏｆ ｈｅａｄ⁃ｏｎ ｓｉｔｕａｔｉｏｎ 物体 初始位置／ ｎｍ 初始速度／ （ｍ·ｓ －１ ） 半径／ ｍ ＵＳＶ （０．００， ０．００） （０．００， ８．００） １２ 运动障碍 （０．００， ７．００） （－０．００， －５．００） １０ ·３４６· 智 能 系 统 学 报 第 １０ 卷