0 引　言

自主靠泊关键技术主要包括环境感知技术、轨迹规划技术、跟踪控制技术^[1]。船舶常见的靠泊方式有很多，根据是否需要外力协助可分为自主靠泊以及拖船协助靠泊，根据是否需要掉头可分为顺靠和掉头靠泊^[2]。

无人艇自主靠泊的轨迹规划主要可以分为两层，前端是从外部水域到达靠泊区的前端路径规划过程，末端是由靠泊区抵达指定泊位的末端轨迹规划过程。从外部水域到靠泊区域前端路径规划的研究成果比较丰富，主要可以分为：一是基于搜索的算法，被较多使用的是A*算法等；二是基于采样的算法，如RRT算法；三是人工势场法；四是蚁群算法；五是曲线法，如Dubins曲线、Reed-Sheep曲线、Bezier曲线等。这些方法各有优劣，如基于搜索或者采样的方法可以实现避障规划，但是实时性一般较差。人工势场法的实现简单、实时性较强，但是会出现局部不可达问题，避障能力较差，比较适用于局部规划。曲线法的实现非常简单，实时性强，但是一般不能实现避障规划，需要人为给定较强的前提条件。

为了克服各种方法的优缺点，科学家们做了大量研究，在2008年提出的混合A*算法将A*算法和运动学约束相结合，被广泛用于解决具有运动学约束的机器人路径规划问题。为了提高基于采样的RRT算法的实时性，Kuffner等^[3]提出了一种双向RRT算法，现已被广泛应用到机器人的路径规划中。Bitar等^[4]优化方法结合RRT和Bezier曲线实现，确保无人艇能够在复杂的环境中完成平稳的自主靠泊。为了解决人工势场法经常遇到的局部最小值导致的目标不可达问题，李晔等^[5]提出一种障碍物补偿方法解决了该问题。Yuan等^[6]提出一种利用Bezier曲线对A*算法所得的路径做路径平滑处理，使得规划路径更符合无人艇的非完整运动学约束。

聚焦于最后的靠泊阶段，李小毛等^[7]提出一种使用单目视觉识别泊位标志物引导小型无人艇完成自主靠泊的方法。Im等^[8]提出一种神经网络方法，根据实验港口的环境训练了一个神经网络，实现了无人艇的自主靠泊，但是这类方法的可移植性不足，对于不同的港口需要训练不同的神经网络来实现。Lee等^[9]参考布罗士马（Brolsma）的靠泊速度曲线，利用机器学习提出了一种评估靠泊速度危险区间的方法。Nguyen等^[10]通过建立3个模糊控制器，分别控制桨、舵和侧推力将船舶稳定到泊位，并通过仿真验证了此方案可以实现船舶靠泊控制。Han等^[11]在靠泊运动控制中考虑国际海事避碰规则，提出了一种基于势场的航迹规划和跟踪方法，利用模糊控制方法生成回避策略，确保与其他船舶的安全距离。Miyauchi等^[12]和WANG等^[13]利用关联船速与船舶领域的方法考虑空间约束，在不同约束条件下对靠泊航迹做了优化。李永正等^[14]提出一种轨线族法，预先给出了6条轨线，根据在进入港口区域时不同的艏向角度选择不同的轨线。胡智焕等^[15]提出一种三次Bezier曲线方法，通过调整符合起始终止位姿要求的中间点，生成三次Bezier曲线作为目标轨迹，但是需要依赖人为给定的条件来实现末端的轨迹规划。

本文参考地面车辆的侧方停车和垂直停车，根据侧方靠泊泊位的常见环境，构造一个可规划区域。在可规划域内，基于三次Bezier曲线的原理构造一个最优化问题，将2个中间点作为优化变量，将轨迹的最小化距离作为最优化问题的目标，将环境障碍物、无人艇的非完整运动学约束作为优化问题的约束条件，通过求解该优化问题得到一条三次Bezier曲线作为末端目标轨迹，从而完成从靠泊区域到最终泊位的末端轨迹规划。最后通过几何方法以及蒙特卡洛的方法验证在规划的可行性，并与其他前端路径规划方法——混合A*算法以及人工势场法完成全局的轨迹规划，同时通过仿真实验验证算法的有效性。

1 末端轨迹规划

类似于陆地车辆的侧方停车与倒车入库，水面无人艇的泊位主要有2种：侧方靠泊位与垂直靠泊位，如图1所示。陆地车辆由于其操作的灵便性，可以通过多次前进倒退不断调整车辆的位置和方向来实现精准驶入泊位。而在水面上行驶的无人艇，由于水域会对无人艇产生不确定性的推力，并且倒退的控制相对于前进更难控制，所以为了减低复杂度，不考虑无人艇倒退的情况。本文的研究对象以侧方靠泊为主，基于侧方靠泊环境提出的一种末端轨迹规划方法，这种方法也可以类比运用到更加简单的垂直靠泊中。

1.1 Bezier曲线

Bezier曲线由多个控制点定义，曲线的具体形状取决于这些控制点的位置。Bezier曲线通常以参数形式表示，其中参数$ t $的范围从0到1。当$ t=0 $时，曲线位于第一个控制点，当$ t=1 $时，曲线位于最后一个控制点。Bezier曲线的参数表达式为：

$ B(n,t)=\sum \limits_{i=0}^{n}\left(\begin{matrix}n\\ i\\ \end{matrix}\right){t}^{i}{(1-t)}^{n-i} 。$

(1)

式中：$ t\in (0 , 1) $。为保证所生成的Bezier曲线能够满足无人艇起始位姿和终止位姿的要求并尽可能减少复杂度，本文采取4个控制点控制的三次Bezier曲线来生成末端目标轨迹，其数学方程式为：

$ P\left(t\right)={(1-t)}^{3}{P}_{0\text{}}+3t{(1-t)}^{2}{P}_{1\text{}}+3{t}^{2}(1-t){P}_{2}\text{}+{t}^{3}{P}_{3}。$

(2)

式中：$ {P}_{0} $、$ {P}_{1} $、$ {P}_{2\text{}} $、$ {P}_{3\text{}} $为4个控制点，其中$ {P}_{0} $为起始点，$ {P}_{3\text{}} $为终止点，$ {P}_{1\text{}} $和$ {P}_{2\text{}} $为2个中间控制点。在末端轨迹规划中，$ {P}_{0} $和$ {P}_{3\text{}} $需要满足起始位姿和终止位姿的要求，是固定值，可以通过控制$ {P}_{1} $、$ {P}_{2\text{}} $两个点来实现对Bezier曲线形状的控制。

三次Bezier曲线有一个几何特征，即所规划的曲线会位于4个控制点构成的四边形或者三角形的内部。如图2所示，如果4个控制点是一个凸四边形，那么所规划Bezier曲线就会位于凸四边形内部，如果是一个凹四边形，则位于3个控制点构成的三角形内部。如果在构造的四边形内部没有障碍物，那么所生成的Bezier曲线一定无碰撞。

1.2 可规划域

本文主要以侧方靠泊环境来演示如何去构造一个可规划域，保证能够在该区域内生成一条满足约束的Bezier曲线作为末端轨迹。图3为一个非常经典的侧方泊位环境，可见在一些区域的一些位姿是无法利用三次Bezier曲线来完成轨迹规划的。如果能够找到某个区域，在这个区域内只要无人艇满足指定的位姿要求，即可实现从当前位置到目标泊位的轨迹规划，这个区域就是一个可规划域。

根据侧方靠泊的靠泊环境，构造一个如图4所示的无障碍扇形区域，该区域的半径以及角度范围可以根据泊位的实际宽度以及泊位外的实际环境进行调整。

可规划域的构造由4个参数确定，分别是左边界与靠岸的夹角$ \textit{α} $，右边界与靠岸的夹角$ \textit{β} $，扇形半径$ \textit{R} $和泊位宽度$ \textit{L} $。其中$ \textit{α} $、$ \textit{β} $、$ \textit{R} $可根据泊位周围的障碍物环境来确定。泊位宽度$ \textit{L} $越宽，可规划区域越宽，边界可规划性会越强，同时为了保证无人艇能以一定速度正常驶入驶出泊位，$ \textit{L} $不能过小，一般要求要大于3倍无人艇的艇长。

对构造的可规划域边界进行可规划讨论如图5所示。区域内部大部分区域的可规划性会比边界的可规划性会更强，并且其可规划性也可以通过下面的方法类推证明。其边界可分为左右直线边界和顶部曲线边界，设置目标位姿为水平向右。总体而言，从右侧的起始位姿比左侧的起始位姿到达终点位姿的航向角需要变化幅度会更大，导致左右边界在规划时会存在不对称性，需要分开讨论。

在处于右区域的双体船位姿$ (x,~y,~\theta ) $满足$ (x,~y) $位于可规划区域内并且$ (\theta ~-~\alpha )\in (0,~{180}^{\circ }) $在不考虑曲率约束的前提下，那么从外部进入可规划区域边界上的位姿都可以构造出内部无障碍物的凸四边形或者三角形，使得所规划的Bzeier曲线位于此无障碍区域中，实现无障碍轨迹规划。

相比于右边界，可以通过Bezier曲线的几何特征对边界进行可规划几何证明，左边界也可以通过类似几何关系来证明，但右区域构造的四边形是$ {P}_{0\text{}}{P}_{1\text{}}{P}_{2}\text{}{P}_{3} $，$ {P}_{3\text{}} $为一个接触障碍物的极限位置。在左区域中，$ {P}_{3\text{}} $为一个非接触障碍物的位置，而是根据实际需要的可调整位置。为保证左右可规划区域的对称性，通过设置$ {\textit{P}}_{2}{\textit{P}}_{3} $关于泊位中心位置对称来完成。

以上几何证明只能表示在可规划域内，符合指定要求的起始位姿可规划一条无碰撞的曲率连续的Bezier曲线到达目标位姿，但是并没有对曲线曲率进行约束。而这些约束将通过构造一个最优化问题，将这些约束作为最优化问题的约束，并通过设置蒙特卡洛实验的方法来验证在非完整约束下能够完成最优的轨迹规划。

1.3 最优化问题

本文目标是规划出一条三次Bezier曲线作为目标轨迹，曲线的形状由4个控制点控制，由于初始点以及目标点已经确定，曲线的形状则由中间2个控制点的位置来控制。因此，以2个中间点的位置作为优化变量，同时以轨迹最短距离作为目标函数，障碍物位置约束、无人艇的非完整约束作为优化问题的约束，构造一个最优化问题，其数学形式表示为：

目标函数：

$ \min (f(P(t)))，$

(3)

约束：

$ \begin{cases} {\psi }_{{{P}_{0}}{{P}_{1}}}={\psi }_{{\mathrm{first}}}，\\ {\psi }_{{{P}_{2}}{{P}_{3}}}={\psi }_{{\mathrm{end}}}，\\ P(t)\cap Obstract=\varnothing，\\ curve(P(t))\leqslant curv{e}_{\max }，\\ t\in [0\quad 1]。\end{cases} $

(4)

式中：$ f(P(t)) $为Bezier曲线的长度；$ {\psi }_{\text{first}} $为起始位姿的航向角；$ {\psi }_{\text{end}} $为终止位姿的航向角；$ {Obstract} $为障碍物区域；$ {curv}{{e}}_{\text{max}} $为给定的最大曲率。通过求解这个最优化问题，可以得到一条满足无人艇非完整约束的、无碰撞的、距离最短的三次Bezier曲线作为目标轨迹。

这是一个带碰撞约束的低维连续控制点优化场景，本文选择选内点法作为优化求解的方法。因为内点法原生支持非线性约束，能保障迭代在可行域内实现避障，并且收敛稳定性强，优于易震荡发散的梯度下降方法与结果波动的PSO和GA，而且低维下收敛高效，比单步易绕路的梯度下降、后期疲软的PSO及高开销的GA更适配场景对“快且稳”的需求。

1.4 蒙特卡洛实验

基于以上末端轨迹方案以及蒙特卡洛实验的设计思想，设计蒙特卡洛实验检验在可规划域边界的可规划性。实验设计如下：首先将边界障碍膨胀，使得双体无人艇能够使用质点模型进行轨迹规划实验而不与障碍物发生碰撞。本实验使用的无人艇外观模型的长度为1.5 m，总宽为1.0 m，为保证无人艇侧方靠泊在不用考虑倒退的情况下可以正常驶入泊位并从泊位驶出，设置膨胀后的泊位宽度为无人艇长度的4倍，即6 m。为保证无人艇靠泊的效率，假设无人艇在最后减速阶段之前的最小速度为1 m/s，而无人艇在静水测试中最大角速度可达45°/s，根据计算可得最小转弯半径约为1.27 m，为保留一定的裕度，设置无人艇在最小转弯半径为1.5 m。

左右边界：从与底部障碍区域的交点开始，设置间隔相同的30个点，每个点设置与边界的夹角角度分别为30°、90°、150°的3个方向，左右边界各有90个初始位姿，终点位姿在泊位内，利用上述优化求解方法得到每一个起始位姿到终点位姿的目标轨迹，如图6所示。

圆弧边界：引入如图7的辅助中心点$ ({x}_{0},~{y}_{0}) $。

为保证圆弧左右2个极限点也能够实现规划，辅助点为圆弧与左右边界直线的角平分线的交点。边界上的位姿满足如下要求即可利用优化求解Bezier曲线的方式完成末端轨迹规划。由于泊位的宽度设置原因，左右边界与膨胀障碍物边界的夹角为45°。为避免从圆弧进入可规划域的无人艇又从左右边界离开可规划域，设定对于圆弧边界上的航向角要求为航向角与从无人艇初始坐标到辅助中心点的坐标的角度的差值小于45°，即小于圆弧与左右边界夹角的一半。基于以上要求，在圆弧上均匀设置30个点，每个点设置3个航向角，航向角与无人艇所在坐标指向辅助点坐标线段的夹角分别为（−45°，0，45°），如图8所示。

在上述边界的给定位姿中，均可实现从边界的指定位姿到最终泊位位姿的末端轨迹规划，由此验证在可规划域的边界的可规划性，并验证了所提出的末端轨迹规划方法的有效性。

2 分层轨迹规划

前面提出一种基于构造可规划域和优化求解Bezier曲线的末端轨迹规划方法，这只是全局路径规划的一部分。下面将讨论2种路径规划方法：人工势场法以及混合A*算法。分别利用这2种算法完成前端的路径规划，通过设置辅助终点引导其到达可规划区域的边界，使得其位姿符合可规划条件，再利用上述提到的末端轨迹规划方法完成末端轨迹规划，最终完成全部的路径规划。

2.1 混合A*算法与末端轨迹规划方法

在可规划区域内找到一个辅助中心点，以这个点作为混合A*算法的路径规划目标点，使得无人艇可以在辅助目标点的引导下以满足指定位姿要求的方式进入可规划域。当系统检测到它进入可规划域之后，标志着前端路径规划已经完成，以前端路径规划的最后位姿作为末端规划的初始位姿，通过以上提出的优化求解方法完成末端路径规划，最终完成自主靠泊的全部轨迹规划。

实验设计：构建一个包含障碍物的地图，把可规划域的中心位置作为混合A*算法的虚拟目标点，从起始点开始使用混合A*算法向给定的目标点进行避障路径搜索。当检测到有扩展点进入到可规划域内时，结束搜索，完成前端规划。以最后的扩展点作为三次Bezier曲线的起始点，利用本文提出的优化求解方法完成末端轨迹规划。

所得实验结果如图9所示。前端的混合A*算法能够规划出一条满足无人艇非完整运动学约束的、避障的路径，并能在可规划域的边界上与末端的Bezier轨迹平滑连接，最终完成从起始点到泊位的轨迹规划，验证了这种方案的可行性。

2.2 人工势场法与末端轨迹规划方法

与混合A*算法通过在可规划域内设置一个虚拟目标点相似，人工势场法可通过在可规划域内设置一个吸引域，引导前端人工势场规划算法以符合可规划位姿要求的位姿进入可规划域中。当检测到前端路径规划进入到可规划域时，即利用优化求解三次Bezier曲线的方法完成末端轨迹规划，从而完成全部的轨迹规划。

所得实验结果如图10所示。可见，无人艇能够在障碍物的斥力作用下以及终点的引力作用下进入可规划域，然后利用上述提出的末端轨迹规划方法完成末端轨迹规划，最终完成完整的轨迹规划。

3 仿真实验

为了验证所提末端轨迹规划方法的可行性，建立无人艇的数学模型，并搭建仿真平台，将所提末端轨迹规划方法与人工势场法结合作为第一种分层轨迹规划方案，完成仿真实验1。将所提末端轨迹规划方法与混合A*算法结合作为第二种分层轨迹规划方案，完成仿真实验2。同时，利用经典混合A*算法作为第三种轨迹规划方案，在仿真平台上完成与仿真实验2的对比实验。通过以上3个仿真实验验证所提末端轨迹规划方法的可行性、适应性和优越性。

3.1 无人艇的数学模型

无人艇的运动是一个包含惯性坐标系下三自由度运动和艇体坐标系下三自由度运动的六自由度的全方位运动，它既可以在惯性坐标系下使用位置向量的导数$ \left(\begin{matrix}{\dot{x} }_{{0}} & {\dot{y} }_{{0}} & {\dot{z} }_{{0}}\\ \end{matrix}\right) $和欧拉角的导数$ \left(\begin{matrix}\dot{\psi } & \dot{\theta } & \dot{\varphi }\\ \end{matrix}\right) $，也可以在艇体坐标系下用速度向量$ \left(\begin{matrix}\textit{u} & \textit{v} & \textit{w}\\ \end{matrix}\right) $和角速度向量$ \left(\begin{matrix}\textit{p} & \textit{q} & \textit{r}\\ \end{matrix}\right) $来表示。但是在实际情况中一般只考虑无人艇的航向角以及航行轨迹$ {x}_{0} $、$ {\textit{y}}_{{0}} $，忽略起伏运动、横倾运动和纵倾运动，从而将无人艇的运动简化成三自由度平面运动。所以将无人艇的运动状态向量定义为$ \textit{U =}~\left[\begin{matrix}{\textit{x}}_{{0}} & {\textit{y}}_{{0}} & \textit{ψ}\\ \end{matrix}\right] $。其三自由度平面运动方程：

$ \left[\begin{matrix}{\dot{x} }_{{0}}\\ {\dot{y} }_{{0}}\\ \dot{\psi }\text{}\\ \end{matrix}\right]\textit=\left[\begin{matrix}\text{cos}\psi & \text{-sin}\psi & {0}\\ \text{sin}\psi & \text{cos}\psi & {0}\\ {0} & {0} & {1}\\ \end{matrix}\right]\left[\begin{matrix}\textit{u}\\ v\\ \textit{r}\\ \end{matrix}\right]。$

(5)

参考文献[16]无人艇数学模型的建立过程，沿用Abkowitz^[17]的整体建模方法，采用整体法建立水面无人艇的线性化数学模型：

$ \left[\begin{matrix}\dot{\nu }\text{}\\ \dot{r} \\ \dot{\psi }\\ \end{matrix}\right]=\left[\begin{matrix}{{m}}_{{11}} & {{m}}_{{12}} & {0}\\ {{m}}_{{21}} & {{m}}_{{22}} & {0}\\ {0} & {1} & {0}\\ \end{matrix}\right]\left[\begin{matrix}{\nu }\\ {r} \\ {\psi }\\ \end{matrix}\right]+\left[\begin{matrix}{{n}}_{{11}}\\ {{n}}_{{21}}\\ {0}\\ \end{matrix}\right]{\delta}。$

(6)

以无人艇航向的改变量$ {\psi }_{\Delta } $作为输出，得到输出方程：

$ {\psi }_{\Delta }=\left[\begin{matrix}0 & 0& 1 \end{matrix}\right]\left[\begin{matrix}\textit{v}\\ \textit{r}\\ {\psi }_{\Delta }\\ \end{matrix}\right]。$

(7)

由上述数学模型，舵角δ 与水面无人艇偏航角速度r 的传递模型，并经过降阶得到Nomoto模型：

$ G(s)=\frac{(V/V_0)K}{s[(V/V_0){T}_0s+1]}。$

(8)

其中：参数$ {\textit{m}}_{\textit{xy}} $、$ {\textit{n}}_{\textit{xy}} $、K、T₀由艇长 L、艇宽B、吃水T、方形系数C_b、尾舵面积δ、艇排水量m、重心与几何中心距离x和艇速决定。

3.2 仿真实验1

利用人工势场法完成前端路径规划，本文所提方法完成末端轨迹规划，并利用构建好的无人艇数学模型完成仿真实验。仿真结果如图11所示。

最终航向角与目标航向角偏差为7.2°，位置偏差为(−0.16 m，0.08 m)。

3.3 仿真实验2

利用混合A*算法完成前端路径规划，本文所提方法完成末端轨迹规划，并利用前面构建好的无人艇模型完成仿真实验。仿真结果如图12所示。

完成轨迹规划耗时为5.738 s，最终其航向角与目标航向角偏差为5.6°，位置偏差为（−0.11 m，0.07 m）。

3.4 对比仿真实验3

将混合A*算法作为以一种全局路径规划方案，以混合A*算法作为目标轨迹，并利用前面构建好的无人艇模型完成仿真实验。仿真结果如图13所示。

完成轨迹规划耗时为7.037 s，最终其航向角与目标航向角偏差为3.36°，位置偏差为（−0.012 m，0.29 m）。

仿真实验1和仿真实验2以指定的方法完成了轨迹规划，并在仿真环境下完成了对期望轨迹的无碰撞跟踪，以较小的靠泊误差完成了自主靠泊任务，验证了本文所提轨迹规划方法的可行性和适应性。将仿真实验2与仿真实验3进行对比，可见仿真实验2的规划时间比混合A*算法提升了18.46%，且仿真实验2的规划轨迹更加平滑，更易于无人艇跟踪，由此验证了所提规划方法的具有一定的优越性。

4 结　语

本文提出了一种基于构造可规划域及优化求解三次Bezier曲线的末端轨迹规划方法。以常见的侧方靠泊的泊位环境作为研究对象，基于该泊位环境构造了一个扇形的可规划域，利用蒙特卡洛实验验证了在这个扇形可规划域边界的可规划性，并将所提出的末端轨迹规划方法与其他前端路径规划方法——混合A*算法和人工势场法结合完成全局轨迹规划，并完成了仿真实验，以较小误差实现了无人艇的自主靠泊，最终验证了所提的末端轨迹规划的可行性和适应性。同时通过对比仿真实验，验证了所提末端轨迹规划方法相比于经典的算法在规划效率和平滑性有了一定的提升，验证了所提方法具有一定的优越性。