基于精确罚函数的无人艇航迹规划和自动避障算法

李清亮，李彬，孙国皓，崔星，毛新涛

1 四川大学 电气工程学院，四川 成都 610065

2 四川大学 空天科学与工程学院，四川 成都 610065

3 北京轩宇空间科技有限公司，北京 100194

0 引 言

近年来，无人驾驶受到日益广泛的关注，在飞行器、智能车、舰船等领域得以快速发展。智能化是未来舰船的发展趋势之一[1]。由于无人艇的制造成本低、制造周期短、环境适应能力强、人力成本低，其在海洋资源勘测、航道测量、环境监测、水域清洁、军事作战等领域具备良好的应用前景。如何快速生成无人艇在复杂环境下的避障航线，是其自主化发展的关键技术之一[2]。

李文华等[3]总结了在工业4.0 背景下世界范围内船舶自主化航行技术的主要发展路径，分析了无人艇航迹规划、智能避障等关键技术。余必秀等[4]通过增加代价函数从而改进了传统的A*算法，可用于规划无人航道测量船的运动路径。Song 等[5]提出了平滑的A*算法来减少冗余的路径点，从而可以提供更连续的路径。Liu 等[6]提出了一种基于快速步进（fast marching，FM）方法的无人艇信息导航路径规划算法，具有计算速度快、计算复杂度低等特点，该算法覆盖了2 个区域（航行区域和避碰区域），可以保证规划轨迹不违反任何禁区。Cho 等[7]在改进现有视线（line-of-sight，LOS）制导算法和速度障碍（velocity obstacle，VO）制导算法的基础上，提出了航点跟踪与避碰算法：即在路径点转换过程中引入额外的控制方案以提高跟踪控制的稳定性，并对传统的VO 算法进行改进，从而有效解决了遇到障碍物时的位置不确定性问题，同时在真实海洋环境下验证了算法的有效性。欧阳子路等[8]将双向搜索 树（bidirectional rapidly-exploring random trees，Bi-RRT）算法与速度障碍法相结合，提出了基于改进Bi-RRT 的无人艇自动避碰算法，并采用并行延伸扩展的2 颗搜索树提高了算法的实时性。受人工智能技术的影响，很多智能算法已在无人艇路径规划领域得以运用，如粒子群算法[9]、蚁群算法[10]、强化学习[11]、人工神经网络[12]等。舒宗玉[9]提出了多目标优化混合粒子群路径规划算法，实现了无人艇路径长度、路径平滑性和安全性的多目标优化。邱晨等[10]基于改进的蚁群算法，规划了无人救生艇的最短无碰撞安全路径。沈海青[11]将A*并行决策动态避障算法与基于深度Q 学习的智能避碰算法相结合，为无人艇提供了多层避碰导航。魏新勇[12]利用卷积神经网络技术识别附近水域的障碍物，并结合模糊数学理论构建了危险系数指标，提出了远程航迹重规划和近程反应式避障方法。

障碍物的存在为航迹规划带来了巨大的挑战，其中障碍物处理的难易程度直接决定了航迹优化问题的复杂程度，而现有文献对障碍物的处理方法普遍比较复杂。为此，本文拟采用圆形包络面和凸四边形包络面对障碍物进行包络处理，并将避障问题转化为在笛卡尔坐标系下的不等式约束问题；通过引入精确罚函数，以简化处理多个障碍物带来的多个强制约束问题。同时，鉴于现有研究成果在航迹规划方面鲜有考虑无人艇的运动特性，本文拟基于无人艇的运动方程，采用控制参数化方法求解时间最优的控制问题；通过结合控制参数化方法与精确罚函数方法，较好地处理无人艇的航迹规划和自动避障问题，并采用仿真验证本文方法的有效性。

1 问题模型

1.1 无人艇运动学建模

为简化处理，本文将无人艇视为仅规划其运动航迹的质点，其运动学坐标系如图1 所示。图中， θ为无人艇的航向角，V为无人艇合速度矢量的模长，Vx和Vy分别为V沿x轴和y轴的速度分量。设定无人艇在t时刻的位置坐标为 (x(t),y(t))，状态量为X(t)=[x(t)y(t) θ(t)V(t)]T， 控制量为u(t)=[γ(t)a(t)]T，则无人艇的动态方程（变量上方的符号“·”表示关于时间的一阶导数）为

式中：V(t)为无人艇在t时刻的合速度矢量的模长；θ(t)为无人艇在t时刻的航向角； γ(t)为无人艇在t时 刻 θ 的 角 速 度；a(t) 为 无 人 艇 在t时 刻V的 加速度。

为便于描述，式（1）可以简记为函数f：

图1 无人艇运动学坐标系Fig. 1 Kinematic coordinate system of USV

1.2 静态障碍物的规避区建模

在实际航行环境中，障碍物的形状大多不规则，如果对障碍进行精确建模，势必会显著增加建模的复杂程度和工作量。为简化处理，本文将采用圆形包络面和凸四边形包络面来对障碍物进行包络覆盖。对于长宽比较小的不规则障碍物，可以采用圆形包络面处理，如图2（a）和图2（b）所示。对于长宽比过大的细长型障碍物，如果仍然采用圆形包络面处理，会造成较大的可通航区域浪费（图2（c）），不利于航迹规划。为此，对于细长型障碍物，宜采用凸四边形包络面进行处理，如图2（d）所示。由障碍物包络面和标准禁航区构成的不可航行区域，在下文统称为禁航区（禁止无人艇航行的区域）。

图2 障碍物区域的包络图Fig. 2 Envelope diagram of obstacle area

1.2.1 圆形包络面建模

对于长宽比较小的不规则障碍物，可以采用圆形包络面以简化建模的复杂度。将障碍物的最长端连线作为包络圆的直径，而直径中点作为包络圆的圆心，如图3 所示，圆心记作 (xi,yi)，半径记作ri，其中i为障碍物序号。为保证无人艇的安全性，需引入安全阈值 ρ，其值为船身长度的3 倍。将增加了安全阈值的包络圆作为覆盖障碍物的禁航区域，即图3 中的虚线圆，其半径Ri=ri+ρ。为使无人艇规避障碍物，则其位置应满足：

1.2.2 凸四边形包络面建模

图3 圆形包络面建模Fig. 3 Circular envelope modeling

对于长宽比较大的不规则障碍物，如果继续采用圆形包络面进行建模处理，会浪费大量的可航行区域，增加无人艇的航行距离，甚至出现无可航行区域情况。为此，宜采用凸四边形包络面进行建模处理。如图4 所示，凸四边形包络面由4 条相交的直线lj,1，lj,2，lj,3，lj,4（j为障碍物序号）围合而成。图4 中的实线凸四边形为障碍物的最小面积包络面，虚线凸四边形为考虑了安全阈值 ρ之后的包络面。本文将采用斜截式直线方程对lj,1，lj,2，lj,3，lj,4进行建模，设定kj,1，kj,2，kj,3，kj,4为4 条边的斜率，bj,1，bj,2，bj,3，bj,4为4 条虚线边对应直线在y轴上的截距。

图4 凸四边形包络面建模Fig. 4 Convex quadrilateral envelope modeling

为使无人艇规避凸四边形包络面，其位置应满足下列4 个不等式之一：

当某边的斜率不存在时，所在边的不等式约束形式即变为对x(t)的约束。为简化约束形式，式（4）可以转化为

式中： min(·)为取最小值函数；z=1，2，3，4，为凸四边形包络面的直线序号。

1.3 优化问题建模

航迹规划的目标是：已知无人艇的初始状态，在考虑禁航区域、最小/最大航行速度和终点位置约束的前提下，实现运动总时间的极小化，该问题模型（P1）为：

式中：C0 为控制量约束；C1 为无人艇运动学方程约束；C2 为无人艇的初始状态；C3，C4 为终端位置约束；C5 为无人艇的航行速度约束；C6，C7 分别为圆形禁航区和凸四边形禁航区；T为无人船的最大运动时间；γmax为最大转向角速度；amax为最大航行加速度；X(0)为0 时刻的无人艇状态；X0为无人艇的初始状态；xT，yT为无人艇的终端位置坐标；Vmax是无人艇的最大航行速率；Ni，Nj分别为圆形包络禁航区和凸四边形包络禁航区的障碍物最大数量。时间域 [0,T]是一个变化域，而约束C5～C7 中均包含每个时刻的状态约束，这实际上将进一步产生无数个约束，所以给问题处理带来了巨大的挑战。

2 基于精确罚函数的最优控制算法

2.1 控制参数化

本文将采用时间尺度变换和控制参数化方法来处理上述优化问题。如图5 所示，时间尺度变换将可变时间域t∈[0,T]变 换为固定域s∈[0, 1]，其中s为转换后的时间刻度。由文献[13-14]可知

图5 时间尺度变换Fig. 5 Time scale transformation

为便于优化处理，本文将连续控制量在s∈[0, 1]内等分为K个区间，从而产生K+1 个节点{0,s1,s2,···,sK}，其中sK=1。分段参数化的结果如图6 所示，其函数表达式为[15]

图6 控制参数化Fig. 6 Control parameterization

因此，式（6）可以改写为问题模型（P2）：

式中：S0～S7 为约束C0～C7 的改写形式；x(1)，y(1)为s=1 时刻的位置状态。

2.2 基于精确罚函数的约束处理

（P2）中含有终端等式约束S3 和S4 以及连续状态不等式约束S5～S7，这就为问题求解带来了极大的挑战。为此，本文采用精确罚函数[13,16]来处理约束S3～S7，以便将（P2）转化为仅含控制约束和初始状态约束的优化问题，其优化目标函数J为：

以上式中： ϵ为惩罚参数；X(1)为无人艇的终端状态； α ＞1, β ＞2, γ ＞2, δ,Wi,Wj均 为 正 实 数 参 数；ϵ−α为惩罚权重； δϵβ为惩罚因子； ϵγW为松弛因子，其中W为松弛因子的权重； κ为缩放因子。

对凸四边形包络面而言，当无人艇违反凸四边形包络面的约束时，必将同时违反凸四边形包络面4 条边的约束，经过4 次累乘后，其惩罚值将远大于违反圆形包络面的惩罚值。因此，本文采用了缩放因子 κ来对凸四边形包络面约束进行缩放处理。

在优化初期，由于存在大量违反约束的情况，为了放宽对约束的惩罚程度，可以增加 ϵ以增加惩罚因子，从而减小惩罚权重并增加松弛因子；随着优化的进行，违反约束的情况将逐渐减少，可以减小 ϵ来减小惩罚因子，从而增加惩罚权重并减小松弛因子，最终进一步减少违反约束的情况。当 ϵ趋近0 时，惩罚权重将趋于无穷大，松弛因子趋于0，惩罚因子趋于0，此时，即可认为优化结束后的目标函数J完全满足了约束条件。

需注意的是， δ在理论上为正无穷才能使优化过程中的 ϵ趋近于0。在实际计算中， ϵ小于一定的阈值即可认为其足够小，因此可将 δ设置为一个较大的常数值。对于松弛因子权重W，可以根据不同惩罚项的松弛程度来调整具体数值；关于其他参数的设置，仅需满足理论要求的约束即可[16]。

式（12）是一个标准的最优参数选择问题，为了将其转化为常规的非线性优化问题，需获得目标函数J的梯度方程[15]，即

基于式（17），即可将式（12）转化为一个无状态约束的最优参数选择问题，且其适用于任何有效的分级优化器进行求解。

3 数值仿真

为了验证本文算法的可行性，设定无人艇的初始状态X(0)=[0, 0, 0.75, 5]T， 终点位置为(xT,yT)=(500, 500)， 最大速率Vmax=8 m/s；罚函数的相关参数为： α=1.5, β=3, γ=3, δ=108； κ=10−4；Wi=0.3，其中i=1，2，3，4，5，6；Wj=0.3，其中j=1，2，其他参数如表1 所示。

表1 仿真数据Table 1 Simulated data

为了验证包络面建模对禁航区域的普适性，本文在仿真中设置了不同半径的圆形包络面和凸四边形包络面，其Matlab 仿真结果如图7 所示（蓝色填充区域为实际禁航区，虚线包络区为考虑安全阈值后的禁航区）。根据仿真结果，无人艇从初始位置出发，可以完全规避禁航区域，其理想航迹的总时长为100.6 s。从图7 可以看出，为了极小化航行时间，航迹与1 个禁航区存在相切的情况，在实际航行过程中，某些不确定性因素的影响可能使无人艇存在碰撞危险。不过，本文设置了安全阈值，可以确保航行过程中无人艇与实际禁航区的安全距离。图8 所示为无人艇位置随时间的变化曲线。

图7 无人艇的航迹图Fig. 7 Track map of USV

图8 无人艇位置随时间的变化图Fig. 8 Diagram of USV position changing with time

图9、图10 所示为无人艇的运动速度和控制量随时间的变化曲线，其仿真结果满足相关约束条件。从图9 可以看出，为了实现极小化的航行时间，无人艇在80 s 之前从初始状态加速到了最大航行速度，并维持在最大速度附近；在80 s 之后，由于无人艇存在大角度的航向调整，为了极小化时间，需在转弯半径和转弯速度之间进行权衡，虽然较小的转弯半径可以缩短转弯航程，但需同时减小航行速度。从仿真结果可以看出，为了极小化航行时间，该无人艇的航向角速度正向满舵且航行速度下降，从而匹配了最佳的转弯半径。可见，该仿真结果验证了本文算法的有效性。

图9 无人艇速度随时间的变化曲线Fig. 9 Diagram of USV speed changing with time

图10 输入控制信号Fig. 10 Input control signal

4 结 语

本文采用圆形和凸四边形包络面处理不规则障碍物，将规避不规则障碍物的问题转化为笛卡尔坐标系下的不等式约束条件，从而将无人艇避障航迹规划问题构建为含有连续状态不等式约束和终端约束的时间最优控制问题。通过控制参数化和时间尺度变换，将时间最优控制问题进一步转化为了最优参数选择问题；同时，利用精确罚函数方法，将连续状态不等式约束和终端约束构建为约束惩罚函数并添加到目标函数中，最终转化为一个无状态约束的最优参数选择问题，其适用于任何有效的分级优化技术进行求解。因此，本文所提出的算法可以有效处理无人艇航迹规划中的避障问题。