0 引　言

轨迹规划是无人船自主航线的核心模块，依据感知模块获得的周边环境信息，目标是生成一条时空平滑、保证行驶安全、可执行的行驶轨迹。关于轨迹规划问题，快速扩展随机树搜索算法(RRT)^[1]是路径规划中一种基于采样的规划算法，RRT改进方法 Goal-biasing^[2]是将一定比例的目标点补充作为扩展节点以提高算法的求解速度。然而，RRT算法包含随机种子，导致生成的路径具有一定的随机性，算法规划的效率难以保证。A*算法是基于栅格地图搜索的规划算法，它将规划空间离散，通过设计启发值对节点的扩展进行引导^[3-4]。文献[5-6]提出一种 Anytime D*算法以进行增量式轨迹规划。Field D*算法^[7-8]和Theta*算法^[9-10]等是继承A*演化而来的算法。这些算法需要对规划空间进行离散，建立栅格地图，因此规划得到的路线曲率并不连续。针对环境周围存在静态障碍物的场景，上述各种基于搜索的规划方法可以快速找到一个可行的航行轨迹，但劣势是生成的轨迹曲率不连续，后续需要利用其他方法进行平滑性拟合。相比采样和搜索的方法，数值曲线计算简单且易于构造最优目标函数。Chu^[12]基于数值离散优化思想，提出以3次多项式作为无人车运动参考线的算法，能够安全有效地避开静态障碍物，但没有考虑多车道场景和动态障碍物。Werling,Ziegler等^[13]以5次多项式作为无人车运动参考线，同时采用最优控制的方法进行规划，能够避开静态和低速行驶的障碍物，但无人车行驶时会频繁的转动方向盘。采样数值优化的轨迹规划方法，在建立并求解模型时，通常会面临非线性优化问题，导致计算复杂度太高结果难以满足模型求解实时性要求。

无人船自主航行的2种典型应用场景，一种是在狭窄内河的航道中，航道线通过电子海图通常已给出；另一种是在广袤的海洋中，一些区域需按航道线自主航行，一些区域无航道线可参考。本文针对以上2种有无航道线参考的应用场景，提出一种统一且可实施的规划路径生成方法，作为轨迹规划的指引线。

为了解决轨迹规划问题，特别是为了降低计算的复杂度和提高实时性，将轨迹规划问题分解成轨迹的路线规划和速度规划，首先确定轨迹的路线，也就是轨迹的几何形状，然后在已经得到路线的基础上，计算速度分配。通过顺序计算路线及此路线上的速度，将一个高维度的规划问题，转换成2个低维度规划问题。

然后，基于路径的光滑度、路径与参考线的横向偏移距离、与障碍物的碰撞风险等以构建约束，通过构建和求解优化约束函数实现各种复杂环境中无人船的轨迹规划。

1 指引线生成

在轨迹规划中，借助弗莱纳坐标系(Frenet)^[13]。在弗莱纳坐标系中，通常计算和使用航道的中心线作为参考线，在此基础上，采用参考线的法线向量 $ l $ 与切线向量 $ s $ 建立一个直角坐标系，坐标系以无人船自身位置为坐标原点，两坐标轴互相垂直，分为纵向和横向2个方向。建立和使用Frenet坐标系的前提条件是具有一条光滑的指引线。通常情况下，将航道中心线理解为指引线。弗莱纳坐标系如图1所示， $ (x,y,\theta ,\kappa ,v,a) $ 表示无人船的运动状态，式中 $ x $ , $ y $ 表示无人船在地图坐标系中的坐标 $ (x,y) $ ， $ \theta $ 表示无人船的首向角， $ \kappa $ 表示无人船的首向角速度。 $ v $ ， $ a $ 分别表示无人船的速度和加速度。 $ (s,\dot s,\ddot s) $ 表示无人船沿着参考线方向(纵向)的位置，速度，加速度。 $ (l,\dot l,\ddot l) $ 表示无人船在参考线当前的法向(横向)的位置、速度、加速度。

狭窄内河的航道中，通过提取电子海图中的信息，得到航道线，再计算得到航道中心线，可作为弗莱纳坐标系的指引线，也就是轨迹规划的指引线，如图2所示。在广袤的海洋中，可根据无人船自主航行规划的路径点连线作为弗莱纳坐标系的指引线，这样2种典型应用场景下，提出一种统一且可实施的规划路径生成方法，作为轨迹规划的指引线，如图3所示。

2 轨迹规划

无人船轨迹规划的定义如下：

计算得到一个以时间 $ t $ 为参数的函数 $ f(t) $ ，对于定义域 $ \left[ {0,{t_{\max }}} \right] $ 的每一个时间 $ t $ ，都有满足条件的无人船运动状态 $ s_{t}=(x, y, \theta,K, v, a) $ ，同时满足无人船的防碰撞约束、运动学约束，以及无人船的物理极限。

$ f(t):t \to {s_t} = (x,y,\theta ,\kappa ,v,a),\forall t \in [0,{t_{\max }}] 。$

(1)

无人船轨迹规划是一个复杂且困难的问题，首先，轨迹规划需要考虑并加入无人船的状态、时间和速度的信息，增加了轨迹规划的维度。其次，由于无人船自主航线运动学模型是非线性系统，需要满足特殊的运动学约束条件。由无人船运动学模型可知，典型无人船不能独立地横向运动，需要纵向运动的同时才能获得横向运动。无人船自主航线时如何规避障碍物，尤其是高速动态障碍物，也是一个非常困难的问题。规划出的轨迹必须做到平滑，加速度、向心加速度等参数，也必须保持在能够容忍的范围内。另外，为了应对多变的环境，轨迹规划算法需要以短周期高频率运行，这对规划算法的运算效率也提出了严格要求。

在无人船平台中，为了解决轨迹规划问题，特别是为了降低计算的复杂度和提高实时性，将轨迹规划问题分解成轨迹的路线规划和速度规划。首先确定轨迹的路线，也就是轨迹的几何形状，然后在已经得到的路线的基础上，计算速度分配，也就是此路线上各点的速度。使用这种分解策略，通过顺序计算路线及此路线上的速度，将一个高维度的规划问题，转换成2个低维度规划问题：路线规划，在弗莱纳坐标系中，沿着坐标系纵向，路线的累计长度 $ s $ ，作为中间变量，先求解 $ s $ 映射到几何形状 $ (x,y,\theta ,\kappa ) $ 的路线函数；速度规划，求解时间 $ t $ 映射到 $ v,a $ 和中间变量 $ s $ 的速度函数 $ (s,v,a) $ 。式中 $ x $ , $ y $ 表示无人船在地图坐标系中的坐标 $ (x,y) $ ， $ \theta $ 表示无人船的首向角， $ \kappa $ 表示无人船的首向角速度； $ v $ ， $ a $ 分别表示无人船的速度和加速度。

2.1 路线规划

借助于弗莱纳坐标系，在之前得到的平滑指引线基础上，按照无人船位置，将其投影到指引线上。以投射点为坐标系原地，将地图坐标系下当前无人船的运动状态 $ (x,y,\theta ,\kappa ,v,a) $ 进行分解，获得无人船沿着指引线方向的位置、速度、加速度。同理将无人船周边环境中的静态障碍物从地图坐标系都映射到依据指引线的弗莱纳坐标系。如图4所示，由于指引线是通过航道的中心线得到的，所以在弗莱纳坐标系下，航道边界是上下对称的。

沿着坐标轴 $ s $ 轴方向，使用 $ \Delta s $ 将 $ s $ 方向进行等距离离散化。依据感知系统获得的各障碍物的信息，航道的边界，通过计算得到每个离散点 $ i $ 的可通行区域 $ (\begin{array}{*{20}{c}} {l_{\min }^i}&{l_{\max }^i} \end{array}) $ 。为了描述方便，将无人船简化成一个点。同时从安全行考虑，为了避免发生碰撞，在获得可通行区域时需要预留适当的缓存区间。

对于沿着坐标系s轴方向的每一个离散点 $ i $ ，选取该点的横向偏移 $ {l_i} $ ， $ {l'_i} $ 和 $ {l''_i} $ 作为优化变量。 $ {l'_i} $ 和 $ {l''_i} $ 为横向偏移 $ {l_i} $ 相对于 $ s $ 的一阶和二阶导数。 $ {l'_i} $ 和 $ {l''_i} $ 可以大致理解为无人船横向移动的“速度”与“加速度”， $ {l'_i} $ 和 $ {l''_i} $ 的大小决定了决定了无人船靠近与偏离指引线的趋势。

选取和设定优化目标函数。首先，对于沿着坐标系s轴方向的每一个离散点 $ i $ 的横向偏移 $ {l_i} $ ，在不考虑周边环境因素影响的情况下，尽可能贴近指引线。对于横向偏移 $ {l_i} $ 的一阶导数 $ {l'_i} $ 和二阶导数 $ {l''_i} $ ，尽可能小，这样可以减少横向运动的加速度，避免频繁打舵，节约能量。按以上分析目标函数定义为:

$ f(l) = {w_l}\sum\limits_{i = 0}^{n - 1} {l_i^2} + {w_{l'}}\sum\limits_{i = 0}^{n - 1} {l'}_i^2 + {w_{l''}}\sum\limits_{i = 0}^{n - 1} {{l''}_i^2} 。$

(2)

为了保证路径的连续性，横向距离 $ {l_i} $ 的相关导数由每个离散点进行差分近似，即

$ {l'_i} \approx \frac{{{l_{i + 1}} - {l_i}}}{{\Delta s}}，$

(3)

$ {l''_i} \approx \frac{{{l_{i + 2}} - 2{l_{i + 1}} + {l_i}}}{{\Delta {s^2}}} 。$

(4)

路径规划及优化后的效果如图5所示。

2.2 速度规划

在已知路线上的速度分配需要具有机动灵活性，能够规避周边复杂环境中的静止和移动障碍物。同时速度分配需要满足无人船的动力学的限制条件，包括无人船最大速度、最大加速度、转弯半径等。确保规化出来的轨迹无人船自主航行能够执行。规划出来的速度分配需要满足船舶海上避碰规则的限制，需要在无人船最大速度的限制范围内进行规划。规划的速度分配需要完成在指定时间 $ (t) $ 到达指定位置 $ (s) $ 的任务。并且在满足运动学约束的限制条件下，完成时间尽可能短。

在确定了路线的条件下，综合考虑周边环境中动静态障碍物。路径-时间障碍物图非常适合此场景的多障碍物规避问题，其基本思想是：首先通过环境感知模块获得周围障碍物的形状及预测轨迹，然后将各障碍物的轨迹投影到已经确定的无人船路线上，如图6所示。各障碍物代表着在何时 $ (t) $ 与何地 $ (s) $ 无人船会与障碍物发生碰撞。

在路线-时间障碍物图的坐标系下，沿着时间轴 $ t $ 和路线的累计距离 $ s $ 进行离散化，形成等时间、等距离的栅格地图，然后根据规划需求，考虑各种环境因素并使用不同的权重。建立一个通用统一的代价函数。栅格地图中相邻栅格节点会赋予一个代价，建立栅格地图后，整个求解过程相当与一个典型的路径规划的图搜索过程，搜索结果是一个全局最优的的路径，搜索算法采样典型的 $ {A^*} $ 算法^[12]。由 $ {A^*} $ 的结果可知，可以获得一组的路线累计距离 $ s $ 与时间 $ t $ 的序列，此序列中每个点代表何时 $ (t) $ ，期望无人船到达何地 $ (s) $ 。

搜索结果由多条直线段依序连接而成，不符合无人船自主航行平滑性要求，也不满足运动学约束，需对结果进行优化。首先计算时间戳 $ {t_i} $ 对应的纵向距离 $ {s_i} $ ，通过固定时间戳 $ {t_i} $ 把 $S = {[\begin{array}{*{20}{c}} {{s_0},} {{s_1},} {\cdots}, {{s_n}} \end{array}]^{\rm{T}}}$ 作为优化的决策变量，按以上分析优化问题的目标函数定义如下:

$ f(s) = {w_h}\cdot\sum\limits_{i = 0}^{n - 1} {{{({s_i} - s_i^h)}^2}} + {w_s}\cdot\sum\limits_{i = 0}^{n - 1} {({{s''}_i}} {)^2}。$

(5)

第1项是为了使优化的结果尽可能贴合原始的搜索结果，另一方面也保证速度优化能够高效快速的收敛到全局最优；第2项是对纵向距离 $ {s_i} $ 加速度的惩罚约束，是为了保证生成的速度曲线更加平滑。

建立避免碰撞约束关系时，通过对路径-时间障碍物图进行处理，将搜索结果 $ {s_i} $ 各点可行域变成一个凸集，约束关系为：

$ {s_i} \leqslant {s_{i + 1}},{s_{d,i}} \leqslant {s_i} \leqslant {s_{h,i}}(i = 0,1, \cdots ,{N_t} - 1)。$

(6)

为了保证速度分配的连续性， $ {s_i} $ 相关导数由每个离散点进行差分近似，即

$ {s'_i} \approx \frac{{{s_{i + 1}} - {s_i}}}{{\Delta t}}，$

(7)

$ {s''_i} \approx \frac{{{{s'}_{i + 1}} - {{s'}_i}}}{{\Delta t}}。$

(8)

上述各约束都是线性约束，决策变量 $ S = [ {{s_0},} {{s_1},} {\cdots,} {{s_n}} ]^{\rm{T}} $ 的可行域通过避障约束关系已经变成一个凸集，优化问题的目标函数是二次凸函数，速度优化变成一个二次规划问题，运用二次规划求解器可快速求解，优化结果如图7 所示。

3 仿真结果与分析

为验证本算法的有效性，在试验中设置若干移动或静止的障碍物，针对静态障碍物和动态障碍物2种典型的场景进行测试。在试验场景下，通过对比 $ A^{{\text{*}}}$ 算法与本文提出方法的避障效果，对2种不同算法规划出的轨迹进行分析。在仿真器中，设置无人船最大速度为18 $\;{\rm{m}} / {\rm{s}}$ ，最大加速度为2.5 $\;{\rm{m}} / {\rm{s}}^{2}$ 。

3.1 静态避障场景

此场景在航道上随机布置多个静止障碍物，要求无人船能同时避让多个障碍物。图8和图9分别为2种不同规划方法生产的避障轨迹和轨迹段上的横向加速度。2种轨迹规划算法得到的轨迹虽然大体一致，但也存在可辨别的差异， $ A^{{\text{*}}} $ 算法更加靠近障碍物，避障过程曲线更加陡峭，本文算法避障过程更加平缓。同时，本文的轨迹生成算法较 $ A^{{\text{*}}}$ 算法具有较低的横向加速度，也就是横向冲击更加小。

3.2 动态避障场景

此场景布置无人船与动态目标分别以10 kn的速度相向行驶，图10和图11针对此场景，进行2种不同轨迹规划算法的仿真结果对比， $A^{{\text{*}}} $ 算法规避动态目标时的轨迹过渡生涩，轨迹的横向冲击变化剧烈，存在较大的横向冲击峰值，本文算法表现更好，规划的轨迹更加平顺，同时横向冲击变化平稳。

4 结　语

本文针对2种有无航道线参考的应用场景，提出一种统一且可实施的规划路径生成方法，作为轨迹规划的指引线。为了解决规划问题，特别是为了降低计算的复杂度和提高实时性，将轨迹规划问题分解成轨迹的路线规划和速度规划。然后基于路径的光滑度、路径与参考线的横向偏移距离、与障碍物的碰撞风险等以构建约束，通过构建和求解优化约束函数实现各种复杂场景中无人船的轨迹规划。在2种试验场景下，对比 $A^{{\text{*}}}$ 算法与本文算法的避障规划效果,本文算法能够处理各种试验场景下的轨迹规划问题，算法生成的轨迹更加平滑，横向加速度小。