海洋科学  2018, Vol. 42 Issue (1): 93-105   PDF    
http://dx.doi.org/10.11759/hykx20171011008

文章信息

蒲红红, 黄海滨. 2018.
PU Hong-hong, HUANG Hai-bin. 2018.
无人水面航行器全局路径规划方法研究
Global path planning algorithm for unmanned surface vehicle
海洋科学, 42(1): 93-105
Marina Sciences, 42(1): 93-105.
http://dx.doi.org/10.11759/hykx20171011008

文章历史

收稿日期:2017-10-11
修回日期:2017-12-20
无人水面航行器全局路径规划方法研究
蒲红红1, 黄海滨2,3,4     
1. 哈尔滨工业大学电子与信息工程学院, 黑龙江 哈尔滨 150001;
2. 哈尔滨工业大学(威海) 信息与电气工程学院, 山东 威海 264209;
3. 山东船舶技术研究院, 山东 威海 264209;
4. 海洋通信与组网观测技术威海市重点实验室, 山东 威海 264209
摘要:针对无人水面航行器(unmanned surface vehicle, USV)的路径规划问题, 提出了基于粒子群优化算法(particle swarm optimization, PSO)和费马螺旋曲线(Fermat’s spiral, FS)的全局路径规划算法。首先, 采用PSO全局路径规划算法, 搜索航路点序列并将其顺序连接, 以获得可行、最短且绝对安全的折线路径。其次, 利用FS曲率可从零变化且连续的特点, 设计FS光顺折线路径策略, 使得所规划路径的方位和曲率均为连续函数, 进而可应用于精确的路径跟踪等运动控制中。考虑到USV的机动性能限制, 在FS过渡曲线设计中加入了最小回转半径约束。仿真结果表明, 本文所提方法能够生成一条满足USV自身机动性能限制, 且方位和曲率均为连续函数的可行光滑路径, 从而能够使得USV实现真正意义上的完全自主航行。
关键词无人水面航行器    粒子群优化算法    费马螺旋曲线    最小回转半径约束    
Global path planning algorithm for unmanned surface vehicle
PU Hong-hong1, HUANG Hai-bin2,3,4     
1. School of Electronics and Information Engineering, Harbin Institute of Technology, Harbin 150001, China;
2. School of Information and Electrical Engineering, Harbin Institute of Technology, Weihai, Weihai 264209, China;
3. Shandong Institute of Shipbuilding Technology, Weihai 264209, China;
4. Weihai City Key Laboratory of Ocean Communication and Networking Observation Technology, Weihai 264209, China
Abstract: In this paper, we propose a method for solving the global path planning problem in unmanned surface vehicles (USVs) using particle swarm optimization (PSO) and Fermat's spiral (FS). First, we use the PSO global path planning algorithm to determine and connect a sequence of waypoints to obtain the most feasible, shortest, and safest polyline path. Then, we designed a strategy for constructing a heading and curvature-continuous path using the properties of FS, which has zero curvature at its origin and a continuously varying curvature, so that the obtained path can be applied to motion control to improve the accuracy of path-following systems. Furthermore, taking into account the maneuverability limitation of the USV, we added a minimum-radius-of-rotation constraint to the FS transition curve design. Simulation results show that the proposed algorithm can generate a feasible and smooth path for autonomous USV maneuvers.
Key words: unmanned surface vehicle (USV)    particle swarm optimization (PSO)    Fermat's spiral (FS)    minimum radius of rotation constraint    

随着陆地资源的匮乏, 探索开发并利用占据70%地表面积的丰富海洋资源已成为当下的迫切需求; 近年来不间断的海上冲突, 使得增强海防实力、维护海洋权益以及保护国家海域安全也成为世界各国的重大军事战略任务。而具备体积小、隐蔽性好以及生存能力强等独特优势的无人水面航行器(unmanned surface vehicle, USV)可为之提供新途径, 带来显著的科考效益和军事应用价值。

USV的路径规划属于其任务规划的底层问题。而全局路径规划为路径规划中的一类, 是一种大范围、长航时的路径规划, 可定义为:首先根据电子海图等信息提供的先验知识, 综合考虑航行与作业任务要求和自身航行特点, 确立路径评价标准; 然后通过一定的搜索方式, 在其运动空间中寻找到一条从起点到目标点的安全可行路径[1]

光滑路径的规划生成都是实现精确路径跟踪的前提, 适合无人运载平台行驶的路径必须是满足方位和曲率连续约束的可行路径。考虑到USV的自身机动性能限制, 如最小回转半径约束, 规划出一条满足最小回转半径约束, 且方位和曲率均为连续函数的光滑航路是进行USV全局航路规划时必须要满足的要求。

针对该问题, 常规解决思路是首先利用路径规划算法搜索出一系列航路点, 然后选用合适的曲线将航路点序列顺序连接, 生成一条可行航路。目前存在两类航路点序列连接方式, 一类为直线与弧段拼接; 另一类则直接利用样条曲线[2]

在第一类方法中, 最简单直观的方式是利用圆弧对由航路点序列顺序连接而成的折线航路进行光顺处理, 即生成Dubins路径[3], 满足航行器最小回转半径约束的同时, 保证了航行器前向航行的最短距离。而最短路径意味着最小化航行时间和燃料消耗最少。但是该路径在圆弧与折线段的连接点处曲率不连续, 会造成航行器横荡加速度的跳变, 进而使得USV偏离期望航路。针对这一缺陷, 文献[4]利用Clothoid曲线曲率关于曲线长度线性变化的特点, 将其作为圆弧和折线航路段间的过渡曲线, 并与圆弧和折线段相互连接构成复合曲线, 从而保证所规划路径曲率的连续性。然而, 该方法存在着Clothoid曲线上点的坐标无闭合解, 须通过Fresnel积分才可得到, 计算量大的缺陷。文献[2]则利用费马螺旋线(Fermat’s spiral, FS)曲率连续且计算量小的特点, 将其代替圆弧作为相邻折线段的过渡曲线, 对由航路点序列连接构成的折线航路进行光顺处理, 生成满足最小回转半径约束且路径方位和曲率均为连续函数的FS复合曲线。文献[5]基于Voronoi图, 利用文献[2]中FS复合曲线完成了曲率连续的二维全局路径规划。

在第二类方法中, 各类源于计算机图形学的样条曲线被广泛用作航路点序列的连接曲线。如文献[6]中采用三次Hermit样条拟合航路点序列, 生成精确通过所有航路点且单调的曲线路径, 并针对航路点处曲率不连续对USV路径跟踪控制的影响给出解决方案。文献[7]研究了三次Bezier样条曲线构成的曲率连续路径, 并给出曲率连续变化时样条曲线参数的取值范围。文献[8]则利用非均匀B样条曲线对折线路径的每一个转弯处进行光顺处理, 满足无人机最小转弯半径的同时, 快速构造出方位和曲率均为连续函数的二维路径。

相比第一类复合曲线, 基于样条曲线的路径生成方法更为灵活和美观, 其多项式最小二乘拟合实现了路径方位和曲率的连续, 但难以同时满足机动性能限制中的最小回转半径约束要求。此外, 在运动控制领域, 样条曲线路径的曲率持续变化会使得航行器的执行器(如方向舵)始终处于机动状态; 而用于直线路径跟踪的海流自适应估计方法具有更快的收敛速率和更强的鲁棒性[9]。因此, 第一类直线与弧段复合曲线在USV运动控制中占据更大优势。

对于全局路径规划算法, 即航路点搜索算法, 目前常用的有:启发式图形搜索、人工势场、可视图、神经网络算法和进化算法等。其中, 粒子群优化算法(particle swarm optimization, PSO)是进化算法的一类典型代表。它模仿鸟类的群集行为, 通过种群中粒子的相互合作和竞争完成搜索进化过程。与其他进行算法相比, 它不存在“交叉”和“变异”生物进化操作, 具有所需设置参数个数少、收敛速度较快且算法实现简单等优点, 近年来被广泛应用于各类无人运载平台的全局路径规划问题中[10]。文献[11]应用PSO解决移动机器人的全局路径规划问题, 采用坐标变换将路径的二维编码转化为具有明确物理意义的一维编码, 且所需调整的参数个数少, 算法复杂度较低。文献[12]应用PSO并基于极坐标建模, 解决了足球机器人的全局路径规划问题。该算法在极坐标下对路径进行一维编码, 仅在足球机器人移动步长范围内调用PSO路径规划算法, 路径搜索的时空开销较少。更多基于PSO路径规划算法的文献可参见文献[13-15]。

本文提出了一种USV的全局路径规划方法, 该方法将PSO与FS相结合, 使得所生成光滑路径的方位与曲率均为连续函数。通过该方法, USV能够在航行过程中生成合理的航行路径, 保证路径的最优性, 并实现碰撞规避。

1 问题描述 1.1 三自由度数学模型

常用于描述USV运动的参考坐标系有惯性坐标系$O-XYZ$和船体坐标系$o-{x_b}{y_b}{z_b}$两种, 均以右手系表示, 如图 1所示。在惯性坐标系中, O为坐标原点, OX指向地理正北方向, OY指向地理正东方向, OZ指向地心方向; 在船体坐标系中, o为坐标原点, oxb指向船体船艏方向, oyb指向船体右舷方向; ozb垂直于水平面指向地心方向。

图 1 USV参考坐标系 Fig. 1 Reference frame of USV

本文主要研究三自由度USV的自主航行问题, 需对六自由度运动模型进行简化, 建立仅包含纵荡、横荡和艏摇运动自由度的水平面三自由度运动模型, 可描述为

$ \begin{array}{l} \mathit{\boldsymbol{\dot \eta }} = \mathit{\boldsymbol{R}}(\psi )\mathit{\boldsymbol{\upsilon }}\\ \mathit{\boldsymbol{M\dot \upsilon }} + \mathit{\boldsymbol{C}}(\mathit{\boldsymbol{\upsilon }})\mathit{\boldsymbol{\upsilon }} + \mathit{\boldsymbol{D}}(\mathit{\boldsymbol{\upsilon }})\mathit{\boldsymbol{\upsilon }} = \mathit{\boldsymbol{\tau }} \end{array} $ (1)

式中, $\mathit{\boldsymbol{\eta }} = {\kern 1pt} {\left[{x, y, \psi } \right]^{\rm{T}}}$, xy$\psi $分别为纵荡位移、横荡位移和航向; $\mathit{\boldsymbol{\upsilon }} = {\kern 1pt} {\left[{u, v, r} \right]^{\rm{T}}}$, uvr分别为纵荡速度、横荡速度和航向角速度; $\mathit{\boldsymbol{\tau }} = {\left[{{T_u}{\rm{, }}\;{\rm{0, }}\;{T_r}} \right]^{\rm{T}}}$为控制输入向量, Tu为螺旋桨提供的推力, Tr为方向舵提供的偏航力矩; R为旋转变换矩阵, M为包含水动力附加质量的惯性矩阵, C(u)为刚体和水动力的科氏力/向心力矩阵, D(u)为水动力阻尼矩阵。以上矩阵的具体结构形式为

$ \begin{array}{l} \mathit{\boldsymbol{R}}(\psi ) = \left[{\begin{array}{*{20}{c}} {\cos \psi }&{-\sin \psi }&0\\ {\sin \psi }&{\cos \psi }&0\\ 0&0&1 \end{array}} \right], \;\mathit{\boldsymbol{M}} = \left[{\begin{array}{*{20}{c}} {{m_{11}}}&0&0\\ 0&{{m_{22}}}&0\\ 0&0&{{m_{33}}} \end{array}} \right]\\ \mathit{\boldsymbol{C}}(\mathit{\boldsymbol{\upsilon }}) = \left[{\begin{array}{*{20}{c}} 0&0&{{C_{13}}}\\ 0&0&{{C_{23}}}\\ {-{C_{31}}}&{-{C_{23}}}&0 \end{array}} \right], \;\mathit{\boldsymbol{D}}(\mathit{\boldsymbol{\upsilon }}) = \left[{\begin{array}{*{20}{c}} {{d_{11}}}&0&0\\ 0&{{d_{22}}}&0\\ 0&0&{{d_{33}}} \end{array}} \right] \end{array} $ (2)

式中, ${m_{11}} = m-{X_{\dot u}}$, ${m_{22}} = m-{Y_{\dot v}}$, ${m_{33}} = {I_z}-{N_{\dot r}}$; ${C_{13}} = - {m_{22}}v, $; ${C_{23}} = {m_{11}}u$, ${d_{11}} = - {X_u}$, ${d_{22}} = - {X_v}$, ${d_{33}} = - {N_r}$, ${d_{11}} = - {X_u}, {d_{22}} = - {X_v}, {d_{23}} = - {Y_r}$

注意到, 式(2)中仅存在作用在纵荡和艏摇两个运动自由度上的控制力/力矩τuτr, 横荡运动自由度上不存在控制力/力矩, 系统的控制输入少于运动自由度个数, 故该三自由度USV为典型的欠驱动系统。

1.2 全局路径规划

USV的全局路径规划问题可简化为在其活动环境中寻找一系列必经航路点的集合。如图 2所示, 在惯性坐标系OXY中, start和goal分别表示航行器的起始点和目标点, 黑色填充物体则为活动环境中的障碍物。给定航行器移动步长d, 点序列$p = \left\{ {{\rm{start}}, {p_1}, {p_2}, \ldots, {p_p}, {\rm{goal}}} \right\}$为航行器在其活动环境中必经航路点的集合, 且相邻点航路点的连线之间不存在障碍物。

图 2 路径产生过程示意图 Fig. 2 Path generation procedure

极坐标系O′为在惯性坐标系OXY中建立的新坐标系, 其极点对应惯性坐标系中的当前航路点pc=(xc, yc), 极轴与惯性坐标系的X轴方向一致, 最长极径为ρ

对于地图中任一点, 其极坐标与在惯性坐标间的转换方式为

$ \left\{ \begin{array}{l} {x_i} = {x_c} + {\rho _i}\cos {\theta _i}\\ {y_i} = {y_c} + {\rho _i}sin{\theta _i} \end{array} \right. $ (3)

式中, $\left( {{\rho _i}, {\theta _i}} \right)$对应该点在极坐标系中的坐标, $({x_i}, {y_i})$对应该点在惯性坐标系中的坐标。

障碍物的判断在以当前航路点pc为圆心, ρ为半径的圆域内进行。若存在障碍物, 则根据航行器移动步长d, 预测下一个航路点, 判断障碍物是否在当前航路点与下一个预测航路点的连线之间。若是, 则调用路径规划算法规划圆域范围内的航路点序列; 否则, 航行器将继续沿着目标点的直线向前航行。

假设当前航路点和下一个预测航路点的连线之间存在障碍物, 则首先对长度ρ进行m等分, 在每个等分线上任取一个点, 可得到序列$\left\{ {{q_1}, {q_2}, \ldots, {q_m}} \right\}$, 该序列即为在以当前航路点pc为极点, 极径ρ为半径的圆域范围内调用路径规划算法得到的航路点序列, 且该部分航路点序列必然包括在航路点序列$p = \left\{ {{\rm{start}}, {p_1}, {p_2}, \ldots, {p_p}, {\rm{goal}}} \right\}$中。

定义长度

$ \begin{array}{c} {L_q} = \sqrt {{{\left( {{x_1}-{x_c}} \right)}^2} + {{\left( {{y_1}-{y_c}} \right)}^2}} + \sqrt {{{\left( {{x_m}-{g_x}} \right)}^2} + {{\left( {{y_m} - {g_y}} \right)}^2}} {\rm{ + }}\\ \sum\limits_{i = 1}^{m - 1} {\sqrt {{{\left( {{x_{i + 1}} - {x_i}} \right)}^2} + {{\left( {{y_{i + 1}} - {y_i}} \right)}^2}} } \end{array} $ (4)

式中, $({x_i}, {y_i})$, $i = 1, 2, \cdots, m$, 对应$\left\{ {{q_1}, {q_2}, \ldots, {q_m}} \right\}$在惯性坐标系中的坐标; $\left( {{g_x}, {g_y}} \right)$为目标点goal在惯性坐标系中的坐标。

路径规划最终可转变成最小化函数式(4), 即在取值空间内寻找使得长度Lq最小的(xi, yi), $i = 1, 2, \cdots, m$。值得注意的是, 在(4)的计算过程中包含了障碍物的相关信息, 即对点序列$\left\{ {{q_1}, {q_2}, \ldots, {q_m}} \right\}$进行约束, 相邻点的连线之间不能存在障碍物。

由上述的问题描述和环境建模可知, 若当前航路点和下一个预测航路点的连线之间存在障碍物, 仅在一个以ρ为半径的圆域内调用路径规划算法, 得到少量的点序列即可完成全局路径规划任务, 其搜索量和时空开销很小, 因此可选择参数设置个数较少、实现简单, 且收敛速度快的PSO作为路径规划算法。

2 基于粒子群优化算法的全局路径规划 2.1 粒子群优化算法原理

PSO采用速度-位置(vx)搜索模型, 其n个粒子为解空间中的备选解, 粒子的优劣程度由选定的适应度函数F(x)决定。粒子更新的方向和大小由其更新速度决定, 且跟随在解空间中迭代搜索得到的当前最优粒子。每迭代一次, 都会得到适应度函数极值pBest和gBest, 即分别为粒子自身和种群搜索到的当前最优值, 而粒子则通过这两个极值来更新速度和位置[11]。定义每个粒子的维度m, 则其速度、位置更可更新为

$ v_{i, j}^{t + 1} = wv_{i, j}^t + {c_1}{R_1}(p_{i, j}^t-x_{i, j}^t) + {c_2}{R_2}(g_j^t-x_{i, j}^t) $ (5)
$ x_{i, j}^{t + 1} = x_{i, j}^t + v_{i, j}^{t + 1} $ (6)

式中, $i = 1, 2, \cdots, n$, $j = 1, 2, \cdots, m$; $\left( {v_{i, j}^t, x_{i, j}^t} \right)$t时刻第i个粒子第j维分量的速度和位置; $p_{i, j}^t$为第i个粒子第j维分量在t时刻为止搜索得到的最优位置, 对应${\rm{pBes}}{{\rm{t}}_{i, j}}$; $g_j^t$为种群所有粒子的第j维分量在t时刻为止搜索得到的最优位置, 对应${\rm{gBes}}{{\rm{t}}_j}$; ${R_1}, {R_2}$为(0~1)的随机实数; ${c_1}, {c_2}$为学习因子, 表示每个粒子受到最优位置$p_{i, j}^t$$g_j^t$的影响程度; w为惯性权重因子, 表示前一次迭代速度对当前速度的影响, 均衡粒子的全局和局部搜索能力。当w较大时, 算法的全局搜索能力较强; 当w较小时, 算法的局部搜索能力较强。通过线性递减规律来改变惯性权重因子w, 改善粒子群局部寻优能力差、容易早熟等缺陷。令

$ w = {w_{\max }}-D\frac{{\left( {{w_{\max }}-{w_{\min }}} \right)}}{{{D_{\max }}}} $ (7)

式中, D为粒子群的当前迭代次数; Dmax为粒子群的最大迭代次数; wmaxwmin分别为最大和最小惯性权重因子, 通常取wmax=0.9, wmin=0.4。由上式可知, w随着迭代次数的增加而逐渐降低, 进而使得PSO拥有前期全局搜索能力强, 后期局部搜索能力较强的特点。

2.2 路径编码和适应度函数

应用PSO求解优化问题时, 解的编码和适应度函数的选择是两个关键步骤。考虑到图 2极坐标系中的等分线, 若将序列$\left\{ {{q_1}, {q_2}, \ldots, {q_m}} \right\}$在极坐标系中的极角${\theta _j}, j = 1, 2, \cdots, m$看作是粒子的各维度, 路径的二维编码简化为一维编码, 那么在找到优化问题解空间的同时, 粒子各维度的物理意义也较为明确。

对于nm维粒子, 粒子的每一维坐标为xi,j=θj, θj∈(0,2π)。但考虑到USV自身特性, 一般不会出现倒车运动状况, 而是在当前航向的正负90°范围内航行, 即若航行器在当前航路点${p_c} = \left( {{x_c}, {y_c}} \right)$处的航向为${\psi _c}$, 则${x_{i, j}} \in \left( {{\psi _c}-{\rm{ \mathsf{ π} }}/2, {\rm{ }}{\psi _c} + {\rm{ \mathsf{ π} }}/2} \right)$。则第i个粒子的适应度函数, 即路径评价准则, 可取为

$ \begin{array}{c} {F_i}{\rm{ = }}\sqrt {{{\left( {{x_c} + \cos \left( {{x_1}} \right)-{x_c}} \right)}^2} + {{\left( {{y_c} + \sin \left( {{x_1}} \right)-{y_c}} \right)}^2}} {\rm{ + }}\\ \sqrt {{{\left( {{x_c} + sm\cos \left( {{x_m}} \right)-{g_x}} \right)}^2} + {{\left( {{y_c} + sm\sin \left( {{x_m}} \right) - {g_y}} \right)}^2}} {\rm{ + }}\\ s\sum\limits_{j = 1}^{m - 1} {\sqrt {{{\left( {\left( {j + 1} \right)\cos \left( {{x_{j + 1}}} \right) - j\cos \left( {{x_j}} \right)} \right)}^2} + {{\left( {\left( {j + 1} \right)\sin \left( {{x_{j + 1}}} \right) - j\sin \left( {{x_j}} \right)} \right)}^2}} } \end{array} $ (8)

式中, $i = 1, 2, \cdots, n$, $j = 1, 2, \cdots, m$; $s = \rho /m$为常值; ${x_{i, j}} \in \left( {x_{i, j}^{\min }, {\rm{ }}x_{i, j}^{\max }} \right)$, 其中$x_{i, j}^{\min } = {\psi _c}-{\rm{ \mathsf{ π} }}/2$, $x_{i, j}^{\max } = {\psi _c} + {\rm{ \mathsf{ π} }}/2$, ${\psi _c}$为航行器在当前航路点上的航向, 即当前航路段与惯性坐标系X轴之间的夹角。此外, 由USV的航行特点可知, 其航行机动约束要求每个航线段长度必须大于最短航段长度${l_{\min }}$, 可令$s = {l_{\min }}$

2.3 算法实现

USV的PSO全局路径规划求解步骤如下:

步骤1:给定参数。设定USV的起点start, 目标点goal, 粒子个数n, 维度m; 学习因子${c_1}, {c_2}$, 最大迭代次数Dmax, 航行器移动步长为d, 最短航段长度${l_{\min }}$以及最长极径ρ。令起点start为当前航路点${p_c} = \left( {{x_c}, {y_c}} \right)$, 其航向${\psi _c}$为起点和目标点连线与惯性坐标系X轴间的夹角。

步骤2:障碍物判断。首先, 对障碍物进行膨胀处理, 以提高避障安全度。然后, 在以当前航路点pc为圆心, ρ为半径的圆域内判断是否存在障碍物。若存在, 则进一步判断障碍物是否在当前航路点与下一个预测航路点的连线之间。若是, 则转向步骤3;否则, 按照给定步长d沿着目标点的直线继续向前航行。

步骤3:计算粒子位置和速度的边界值$x_{i, j}^{\min }{\rm{ }}$, $x_{i, j}^{\max }, v_{i, j}^{\min }v_{i, j}^{\max }$。以当前航路点极点pc, ρ为最长极径, 对ρ进行m等分, 计算航行器当前航向${\psi _c}$, 进而得到粒子的位置边界值$x_{i, j}^{\min }, {\rm{ }}x_{i, j}^{\max }$, $i = 1, 2, \cdots, n$, $j = 1, 2, \cdots, m$。考虑到速度边界值$v_{i, j}^{\max }$会对最优解的搜索能力造成影响, 即$v_{i, j}^{\max }$过大会使得粒子掠过最优解, $v_{i, j}^{\max }$过小会使得粒子陷入局部最优值, 取

$ v_{i, j}^{\max } = 0.1\left( {x_{i, j}^{\max }-x_{i, j}^{\min }} \right) $ (9)

即速度边界值$v_{i, j}^{\max }$为位置变化范围的10%, 此外$v_{i, j}^{\min } =-v_{i, j}^{\max }$

步骤4:初始化粒子的位置和速度$x_{i, j}^0, v_{i, j}^0$。考虑速度的边界值约束, 粒子的初始速度可取为

$ v_{i, j}^0 =-v_{i, j}^{\max } + 2Rv_{i, j}^{\max } $ (10)

式中, $i = 1, 2, \cdots, n$, $j = 1, 2, \cdots, m$; $R$为(0~1)的随机实数。

相比速度的初始化, 粒子的位置初始化过程涉及是否与障碍物碰撞问题, 即考虑边界值约束的同时, 必须进行障碍物无碰验证。图 3给出了$x_{i, j}^0$的初始化流程。

图 3 粒子位置的初始化流程图 Fig. 3 Flow chart for the initialization of a particles' position

其中, $q \ge {q_{\max }}$表示第$i$个粒子初始化失败; $k \ge {k_{\max }}$表示第$i$个粒子的第$j$维初始化失败, 需要重新初始化第$i$个粒子; 函数${\rm{Con}}\left( {i, j-1, j} \right)$表示第$i$个粒子的第$j$维和第$j + 1$维分别对应的极坐标中两个航路点连线之间是否存在障碍物, 其中, ${\rm{Con}}\left( {i, 0, 1} \right)$表示第$i$个粒子的第1维与当前航路点${p_c}$的连线之间是否存在障碍物。

得到$x_{i, j}^0, v_{i, j}^0$后, 根据适应度函数式(8), 求出初始最优位置$p_{i, j}^0$$g_j^0$

步骤5:更新粒子速度和位置$v_{i, j}^t$, $x_{i, j}^t$。根据公式(5), 更新粒子速度$v_{i, j}^t$, 并进行边界值约束, 即超出边界$\left( {v_{i, j}^{\min }, v_{i, j}^{\max }} \right)$时, 用边界值将其代替。根据式(6)更新位置$x_{i, j}^t$, 但注意到位置的更新过程中也需要进行障碍物无碰验证, 其$x_{i, j}^0$的初始化流程相似, 故将图 4中的$x_{i, j}^0 = x_{i, j}^{\min } + R\left( {x_{i, j}^{\max }-x_{i, j}^{\min }} \right)$用式(6)和位置边界值约束代替即可。得到$v_{i, j}^t$, $x_{i, j}^t$后, 根据适应度函数式(8), 求出最优位置$p_{i, j}^t$$g_j^t$

图 4 折线航路 Fig. 4 Polygonal path

步骤6:转向步骤5进行迭代, 直到达到最大迭代次数${D_{\max }}$。最后一次迭代得到的种群最优位置为$g_j^{{D_{\max }}}$, $j = 1, 2, \cdots, m$, 即对应序列$\left\{ {{q_1}, {q_2}, \ldots, {q_m}} \right\}$, 通过(3)可实现二者之间的转化。令${q_m}$为当前航路点${p_c}$, 转向步骤2, 直到当前航路点${p_c} = \left( {{x_c}, {y_c}} \right)$与目标点goal$\left( {{g_x}, {g_y}} \right)$之间的距离小于航行器移动步长d, 即${\left( {{x_c}-{g_x}} \right)^2} + {\left( {{y_c}-{g_y}} \right)^2} < {d^2}$

3 基于费马螺旋曲线的折线路径光顺策略

借鉴Dubins路径的生成思想, 即圆弧光顺折线路径, 利用FS曲率可从零开始变化且连续的特点, 给出FS代替圆弧作为折线路段间过渡曲线的光顺折线路径策略, 生成方位和曲率均连续的光滑路径。

3.1 Dubins路径

Dubins路径常用于无人机、无人航行器等无人运载系统的路径规划中, 能够实现前向飞行或者航行的最短距离, 而最短路径意味着最小化飞行或航行时间和燃料消耗最少。

圆弧的参数形式可描述为

$ {P_{{\rm{cir}}}}\left( {\bar w} \right) = \left[\begin{array}{l} {x_{\rm{p}}}\left( {\bar w} \right)\\ {y_{\rm{p}}}\left( {\bar w} \right) \end{array} \right] = \left[\begin{array}{l} {x_{\rm{N}}} + R\cos \left( {{\alpha _0} + \bar w\left( {{\alpha _1}-{\alpha _0}} \right)} \right)\\ {y_{\rm{E}}} + R\sin \left( {{\alpha _0} + \bar w\left( {{\alpha _1}-{\alpha _0}} \right)} \right) \end{array} \right] $ (11)

式中, $\bar w$为圆弧参数; $c = {\left[{{x_{\rm{N}}}, {y_{\rm{E}}}} \right]^{\rm{T}}}$$R$分别为圆弧所在圆的圆心和半径; ${\alpha _0}$${\alpha _1}$分别为圆弧的起始方位和终点方位。其中, 对于任意参数曲线${P_{\rm{p}}}\left( {\bar w} \right) = {\left[{{x_{\rm{p}}}\left( {\bar w} \right), {y_{\rm{p}}}\left( {\bar w} \right)} \right]^{\rm{T}}}$, 曲线的方位可通过

$ {\chi _{\rm{p}}}\left( {\bar w} \right) = a\tan 2\left( {{{y'}_{\rm{p}}}\left( {\bar w} \right), {{x'}_{\rm{p}}}\left( {\bar w} \right)} \right) $ (12)

计算得到。且当

$ \left| {{{P'}_{\rm{p}}}\left( {\bar w} \right)} \right| = \sqrt {{{\left( {{{x'}_{\rm{p}}}\left( {\bar w} \right)} \right)}^2} + {{\left( {{{y'}_{\rm{p}}}\left( {\bar w} \right)} \right)}^2}} \ne 0 $ (13)

时, 该参数曲线为正则曲线。正则曲线的曲率表达式为

$ \kappa = \frac{{{{x'}_{\rm{p}}}\left( {\bar w} \right){{y''}_{\rm{p}}}\left( {\bar w} \right)-{{y'}_{\rm{p}}}\left( {\bar w} \right){{x''}_{\rm{p}}}\left( {\bar w} \right)}}{{{{\left( {{{\left( {{{x'}_{\rm{p}}}\left( {\bar w} \right)} \right)}^2} + {{\left( {{{y'}_{\rm{p}}}\left( {\bar w} \right)} \right)}^2}} \right)}^{3/2}}}} $ (14)

图 4为由航路点序列顺序连接而成的折线路径。图 5给出了该折线路径的几何性质, 且图中各变量均以路径长度$L$为参数。可以看出, 由航路点序列连接而成的折线路径, 其路径方位$\chi $和曲率$\kappa $均不连续。

图 5 折线航路的几何性质 Fig. 5 Geometric properties of polygonal path

图 6为利用圆弧对图 4中折线航路光顺后的路径, 即Dubins路径。其中, 半径R可选为航行器的最小回转半径${R_{\min }}$, 其对应最大曲率

$ {\kappa _{\max }} = \frac{1}{{{R_{\min }}}} $ (15)
图 6 Dubins路径 Fig. 6 Dubins path

图 7给出了该Dubins路径的几何性质, 且图中各变量均以路径长度L为参数。可以看出, 此路径仅实现了其方位$\chi $的连续, 其曲率$\kappa $在航路点处仍间断。而由文献[2]和[6]知, 所跟踪路径的曲率与航行器横荡加速度之间存在关系

图 7 Dubins路径的几何性质 Fig. 7 Geometric properties of Dubins path
$ \left| \mathit{\boldsymbol{\alpha }} \right| = {\left| \mathit{\boldsymbol{u}} \right|^2}\kappa $ (16)

式中, α为横荡加速度矢量, u为航行器的速度矢量。这意味着曲率的不连续通常会造成航向控制器控制输入的不连续, 进而影响跟踪控制性能, 致使航行器偏离期望航路。

3.2 基于费马螺旋曲线的折线路径光顺策略 3.2.1 费马螺旋曲线及其参数曲线

FS, 是阿基米德螺旋曲线的一种, 可表示为

$ r = k{\theta ^{1/2}} $ (17)

式中, r为径向距离; θ为极角; k为比例常数。其曲率为

$ \kappa \left( \theta \right) = \frac{1}{k}\frac{{2\sqrt \theta \left( {3 + 4{\theta ^2}} \right)}}{{{{\left( {1 + 4{\theta ^2}} \right)}^{3/2}}}} $ (18)

由上式知, $\kappa \left( 0 \right) = 0$, 且当$\theta > 0$时, $\kappa \left( \theta \right) > 0$恒成立, 即该曲线的该曲率从零开始变化且连续。

式(17)在惯性坐标系中的参数费马螺旋线, 即参数FS, 可表示为

$ \left[\begin{array}{l} x\\ y \end{array} \right] = \left[\begin{array}{l} r\cos \theta \\ r\sin \theta \end{array} \right] = \left[\begin{array}{l} k\sqrt \theta \cos \theta \\ k\sqrt \theta \sin \theta \end{array} \right] $ (19)

式中, θ为FS的参数。

考虑到不同的初始位置${\mathit{\boldsymbol{p}}_0} = {\left[{{x_0}, {y_0}} \right]^{\rm{T}}}$、旋转方向ρ, 以及初始方位${\chi _0}$, 式(19)可进一步写成

$ {\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( \theta \right) = \left[\begin{array}{l} {x_0} + k\sqrt \theta \cos \left( {\rho \theta + {\chi _0}} \right)\\ {y_0} + k\sqrt \theta \sin \left( {\rho \theta + {\chi _0}} \right) \end{array} \right] $ (20)

式中, ρ=1表示该参数FS顺时针旋转, ρ=-1表示逆时针旋转; 参数θ的取值区域为$\left[{0, {\theta _{{\rm{end}}}}} \right]$, 其中${\theta _{{\rm{end}}}}$为一个对应于该参数FS终点位置的待确定参数, 可通过设置$\theta = \bar w{\theta _{{\rm{end}}}}$将参数$\theta \in \left[{0, {\theta _{{\rm{end}}}}} \right]$转换为参数$\bar w \in \left[{0, 1} \right]$

参数FS的镜像曲线可描述为

$ {\mathit{\boldsymbol{p}}_{{\rm{FS'}}}}\left( \theta \right) = \left[\begin{array}{l} {x_{{\rm{end}}}} + k\sqrt {{\theta _{{\rm{end}}}}-\theta } \cos \left( {\rho \left( {\theta-{\theta _{{\rm{end}}}}} \right) + {\chi _{{\rm{end}}}}} \right)\\ {y_{{\rm{end}}}} + k\sqrt {{\theta _{{\rm{end}}}}-\theta } \sin \left( {\rho \left( {\theta - {\theta _{{\rm{end}}}}} \right) + {\chi _{{\rm{end}}}}} \right) \end{array} \right] $ (21)

式中, ${\mathit{\boldsymbol{p}}_{{\rm{FS'}}}}\left( {{\theta _{{\rm{end}}}}} \right) = {\mathit{\boldsymbol{P}}_{{\rm{end}}}}$, ${\mathit{\boldsymbol{P}}_{{\rm{end}}}} = {\left[{{x_{{\rm{end}}}}, {y_{{\rm{end}}}}} \right]^{\rm{T}}}$为该参数FS的终点位置, ${\chi _{{\rm{end}}}}$为终点方位。

3.2.2 参数费马螺旋曲线的速度和加速度

式(20)的一阶导数为

$ \frac{{\rm{d}}}{{{\rm{d}}\theta }}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( \theta \right) = \frac{k}{{2\sqrt \theta }}\left[\begin{array}{l} \cos \left( {\rho \theta + {\chi _0}} \right)-2\rho \theta \sin \left( {\rho \theta + {\chi _0}} \right)\\ \sin \left( {\rho \theta + {\chi _0}} \right) + 2\rho \theta \cos \left( {\rho \theta + {\chi _0}} \right) \end{array} \right] $ (22)

二阶导数为

$ \begin{array}{l} \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\frac{{{{\rm{d}}^2}}}{{{\rm{d}}{\theta ^2}}}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( \theta \right) = \\ - \frac{k}{{4{\theta ^{3/2}}}}\left[\begin{array}{l} \left( {4{\theta ^2} + 1} \right)\cos \left( {\rho \theta + {\chi _0}} \right) + 4\rho \theta \sin \left( {\rho \theta + {\chi _0}} \right)\\ \left( {4{\theta ^2} + 1} \right)\sin \left( {\rho \theta + {\chi _0}} \right)-4\rho \theta \cos \left( {\rho \theta + {\chi _0}} \right) \end{array} \right] \end{array} $ (23)

注意到, 方程(22)、(23)在θ=0处奇异, 即参数FS这一参数曲线的速度和加速度均在起始位置处无定义。路径跟踪等运动控制要求所跟踪曲线为正则曲线, 故该奇异性质使得参数FS本质上不适用于路径跟踪等运动控制。然而, 由文献[22]可知, 该问题是由曲线参数的不恰当选择造成, 即该奇异性质仅为曲线的参数性质, 而非曲线的几何性质, 可通过变换曲线参数解决。令

$ u = \sqrt \theta, {\rm{ 0}} \le u \le \sqrt {{\theta _{\max }}} $ (24)

则式(20)可改写成

$ {\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( u \right) = \left[\begin{array}{l} {x_0} + ku\cos \left( {\rho {u^2} + {\chi _0}} \right)\\ {y_0} + ku\sin \left( {\rho {u^2} + {\chi _0}} \right) \end{array} \right] $ (25)

进而, 其一阶导数为

$ \frac{{\rm{d}}}{{{\rm{d}}u}}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( u \right) = k\left[\begin{array}{l} \cos \left( {\rho {u^2} + {\chi _0}} \right)-2\rho {u^2}\sin \left( {\rho {u^2} + {\chi _0}} \right)\\ \sin \left( {\rho {u^2} + {\chi _0}} \right) + 2\rho {u^2}\cos \left( {\rho {u^2} + {\chi _0}} \right) \end{array} \right] $ (26)

根据三角函数知识, 上式可改写为

$ \frac{{\rm{d}}}{{{\rm{d}}u}}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( u \right) = k\sqrt {1 + {{\left( {2\rho {u^2}} \right)}^2}} \left[\begin{array}{l} \sin \left( {\rho {u^2} + {\chi _0}-\phi } \right)\\ \cos \left( {\rho {u^2} + {\chi _0} + \phi } \right) \end{array} \right] $ (27)

式中, $\phi = a\tan 2\left( {1, 2\rho {u^2}} \right)$。从而有

$ \left| {\frac{{\rm{d}}}{{{\rm{d}}u}}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( u \right)} \right| = k\sqrt 2 \sqrt {1 + {{\left( {2\rho {u^2}} \right)}^2}} $ (28)

满足正则曲线所要求的条件

$ \left| {\frac{{\rm{d}}}{{{\rm{d}}u}}{\mathit{\boldsymbol{p}}_{{\rm{FS}}}}\left( u \right)} \right| \ne 0 $ (29)

故, 该参数FS可用于路径跟踪等运动控制中。而由文献[23]可知, 对于路径跟踪等应用, 要求必须定义路径参数对时间的一阶导数, 即参数路径的速度。给定航行器期望速度${U_d}\left( t \right)$, 路径参数对时间的一阶导数为

$ \dot u\left( t \right) = \frac{{{U_d}\left( t \right)}}{{\sqrt {{{\left( {{{x'}_p}\left( u \right)} \right)}^2} + {{\left( {{{y'}_p}\left( u \right)} \right)}^2}} }} $ (30)

将式(28)代入上式, 可得

$ \dot u\left( t \right) = \frac{{{U_d}\left( t \right)}}{{k\sqrt 2 \sqrt {1 + 4{u^4}} }} $ (31)

其中, ${\rho ^2} = 1$

3.2.3 费马螺旋曲线的长度

在进行路径设计时, 路径长度常作所设计路径的参数。根据直线和圆弧长度的表达式, 可较容易计算得到Dubins路径长度。但相比Dubins路径, FS长度的计算过程则相对繁琐, 这是由于其参数FS速度, 即式(28)的积分

$ {L_{{\rm{FS}}}} = k\sqrt 2 \int_0^{{u_{\max }}} {\sqrt {1 + {{\left( {2\rho {u^2}} \right)}^2}} } $ (32)

不存在闭合解。由文献[2]可知, 该FS的长度可通过高斯超几何函数计算得到, 即

$ {L_{{\rm{FS}}}}\left( \theta \right) = k\sqrt \theta {}_2{F_1}\left( {-\frac{1}{2}, \frac{1}{4};\frac{5}{4};-4{\theta ^2}} \right) $ (33)

等价于

$ {L_{{\rm{FS}}}}\left( u \right) = ku{}_2{F_1}\left( {-\frac{1}{2}, \frac{1}{4};\frac{5}{4};-4{u^4}} \right) $ (34)

式中,

$ {F_1}\left( {-\frac{1}{2}, \frac{1}{4};\frac{5}{4};-4{u^4}} \right) = \sum\limits_{n = 0}^\infty {\frac{{{{\left( {-\frac{1}{2}} \right)}_n}{{\left( {\frac{1}{4}} \right)}_n}}}{{{{\left( {\frac{5}{4}} \right)}_n}}}} \frac{{{{\left( { - 4{u^4}} \right)}^n}}}{{n!}} $ (35)

且要求$u < 1/2$。而当${\mathop{\rm Re}\nolimits} {\kern 1pt} {\kern 1pt} \left( {{\kern 1pt} 5{\kern 1pt} {\kern 1pt} /{\kern 1pt} {\kern 1pt} {\kern 1pt} 4{\kern 1pt} {\kern 1pt} + {\kern 1pt} {\kern 1pt} {\kern 1pt} 1{\kern 1pt} /{\kern 1pt} {\kern 1pt} 2{\kern 1pt}-{\kern 1pt} {\kern 1pt} {\kern 1pt} 1{\kern 1pt} /{\kern 1pt} {\kern 1pt} 4} \right){\kern 1pt} > {\kern 1pt} 0$时, 式(35)绝对收敛。注意到, 上述对参数$u$的限制并不影响FS的使用, 通过[24]中合适的转换规则, 将参数$u$转换为一个在适当取值区域内变化的参数即可。

3.2.4 费马螺旋曲线的方位

考虑到式(22)可通过转换曲线参数消除其奇异性, 故FS的方位为

$ \chi \left( \theta \right) = \arctan \left( {\frac{{\sin \theta + 2\theta \cos \theta }}{{\cos \theta-2\theta \sin \theta }}} \right) $ (36)

或利用四象限反正切函数求得, 即

$ \chi \left( \theta \right) = \arctan 2\left( {\sin \theta + 2\theta \cos \theta, cos\theta + 2\theta \sin \theta } \right) $ (37)

上述两个方程均连续, 且等价于

$ \chi \left( \theta \right) = \theta + \arctan \left( {2\theta } \right) $ (38)

而式(36)的上界为90°, 式(37)的上界为180°。

3.2.5 基于费马螺旋曲线的折线路径光顺策略

借鉴Dubins路径的生成思想, 可利用FS曲率从零开始变化且连续的特点, 对折线路径进行光顺处理。

不失一般性, 假设原路径是由3个航路点连接而成的折线航路, 其位置矢量分别为${\mathit{\boldsymbol{W}}_{i-1}}, {\mathit{\boldsymbol{W}}_i}, {\mathit{\boldsymbol{W}}_{i + 1}}$, 如图 8所示。平行于该航路两条直线段的单位矢量分别为

图 8 折线航路 Fig. 8 Polygonal path
$ {\mathit{\boldsymbol{V}}_{{\rm{in}}}} = \frac{{{\mathit{\boldsymbol{W}}_i}-{\mathit{\boldsymbol{W}}_{i-1}}}}{{\left\| {{\mathit{\boldsymbol{W}}_i}-{\mathit{\boldsymbol{W}}_{i - 1}}} \right\|}} $ (39)
$ {\mathit{\boldsymbol{V}}_{{\rm{out}}}} = \frac{{{\mathit{\boldsymbol{W}}_{i + 1}}-{\mathit{\boldsymbol{W}}_i}}}{{\left\| {{\mathit{\boldsymbol{W}}_{i + 1}}-{\mathit{\boldsymbol{W}}_i}} \right\|}} $ (40)

折线航路方位改变量的大小和方向分别为

$ \left| {\Delta \chi } \right| = \arccos \left( {{\mathit{\boldsymbol{V}}_{{\rm{in}}}}{\mathit{\boldsymbol{V}}_{{\rm{out}}}}} \right) $ (41)
$ \rho =-{\mathop{\rm sign}\nolimits} \left( {{\mathit{\boldsymbol{V}}_{{\rm{in}}}}_{, y}{\mathit{\boldsymbol{V}}_{{\rm{out}}}}_{, x}-{\mathit{\boldsymbol{V}}_{{\rm{in}}}}_{, x}{\mathit{\boldsymbol{V}}_{{\rm{out}}}}_{, y}} \right) $ (42)

此外, 折线航路的初始方位和终点方位分别为

$ {\chi _0} = a\tan 2\left( {{\mathit{\boldsymbol{V}}_{{\rm{in}}}}_{, y}, {\mathit{\boldsymbol{V}}_{{\rm{in}}}}_{, x}} \right) $ (43)
$ {\chi _{{\rm{end}}}} = a\tan 2\left( {{\mathit{\boldsymbol{V}}_{{\rm{out}}}}_{, y}, {\mathit{\boldsymbol{V}}_{{\rm{out}}}}_{, x}} \right) $ (44)

借鉴圆弧光顺折线路径的思想, 用两条FS代替圆弧作为航路点处的过渡曲线。其中, 第一条FS曲率持续增加, 且起始于折线航路的第一条直线段, 终止于整个过渡曲线的中点; 第二条FS为第一条的镜像曲线, 称为镜像FS, 其曲率持续降低, 且起始于整个过渡曲线的中点, 终止于折线航路的第二条直线段。由于两条FS互为镜像曲线, 故只需要计算其中的一条, 第二条通过镜像操作即可得到。

式(20)中, 参数θ的取值上界θend可通过折线航路的方位改变量$\Delta \chi $计算得到, 比例常数$k$对应于给定的最大曲率${\kappa _{\max }}$。由于式(38)不可逆, 无法求取${\theta _{{\rm{end}}}}$的解析解。而考虑到式(38)即使在$\chi = {180^ \circ }$处也连续可微, 故可通过数值解算方法求取${\theta _{{\rm{end}}}}$。由文献[2]可知, Halley数值解算方法

$ {x_{n + 1}} = {x_n}-\frac{{2f\left( {{x_n}} \right)f'\left( {{x_n}} \right)}}{{2{{\left( {f'\left( {{x_n}} \right)} \right)}^2}-f\left( {{x_n}} \right)f''\left( {{x_n}} \right)}} $ (45)

是最为有效的数值解算方法, 仅需一次迭代即可完成误差精度为10-3的求解。

当求得上界${\theta _{{\rm{end}}}}$后, 即可求取对应于最大曲率${\kappa _{\max }}$的比例常数k。由式(18)可知, FS的最大曲率在

$ \theta = \sqrt {\frac{{\sqrt 7 }}{2}-\frac{5}{4}} $ (46)

处取得。但是, 当θ的取值区域不包含该点时, 最大曲率在${\theta _{{\rm{end}}}}$处取得。因此, 对应于最大曲率处的${\theta _{{\kappa _{\max }}}}$最终可取为

$ {\theta _{{\kappa _{\max }}}} = \min \left( {{\theta _{{\rm{end}}}}, \sqrt {\frac{{\sqrt 7 }}{2}-\frac{5}{4}} } \right) $ (47)

给定最大曲率${\kappa _{\max }}$, 令$\kappa \left( {{\theta _{{\kappa _{\max }}}}} \right) = {\kappa _{\max }}$, 则可由式(18)得到比例常数

$ k = \frac{1}{{{\kappa _{\max }}}}\frac{{2\sqrt {{\theta _{{\kappa _{\max }}}}} \left( {3 + 4{\theta ^2}_{{\kappa _{\max }}}} \right)}}{{{{\left( {1 + 4{\theta ^2}_{{\kappa _{\max }}}} \right)}^{3/2}}}} $ (48)

而FS在折线航路上的投影长度和高度, 决定了过渡曲线的起始点p0。不失一般性, 令${\chi _0} = 0$, 可计算得到长度

$ {l_1} = k\sqrt {{\theta _{{\rm{end}}}}} \cos \left( {\rho {\theta _{{\rm{end}}}}} \right) $ (49)

高度

$ h = k\sqrt {{\theta _{{\rm{end}}}}} \sin \left( {\rho {\theta _{{\rm{end}}}}} \right) $ (50)

至此, 由式(41)、(42)求得了航路的方位改变量, 令$\chi \left( \theta \right) = \left| {\Delta \chi } \right|/2$, 即可通过数值求解式(38)获得参数取值上界${\theta _{{\rm{end}}}}$。给定最大曲率${\kappa _{\max }}$, 继而可通过式(47)、(48)求得比例常数$k$。将折线路径的方位式(43)作为FS的初始方位, 最终可确定起始点p0。类似的, 可通过p0的求取方式求取过渡曲线的终点pend。此外, 起始点p0和终点pend可分别称作该过渡曲线在折线航路中的切入点和切出点。对于由给定的3个航路点构成的折线航路, 利用上述的FS光顺折线路径策略, 可得到如图 9图 10所示的光滑航路。

图 9 光滑航路 Fig. 9 Smooth path

图 10 FS光滑航路 Fig. 10 Smooth path using F

除满足安全性和光滑性外, 最小回转半径约束这一机动性能限制也是进行路径规划时必须要考虑的问题。Dubins路径生成时, 圆弧所在圆的半径可选为航行器最小回转半径; FS光顺折线路径时, FS的设计需给定最大曲率。而在几何上, 最小回转半径即对应最大曲率。因此, 给定航行器最小回旋半径${R_{\min }}$, 将转化成对应的最大曲率${\kappa _{\max }}$

$ {\kappa _{\max }} = \frac{1}{{{R_{\min }}}} $ (51)

即式(15), 并在FS光顺折线航路时利用该最大曲率, 即可满足航行器最小回转半径约束。

4 仿真结果与分析

为了更好地验证FS光顺折线路径策略的有效性, 给定一条由更多航路点连接成而的折线航路, 航路点序列为: $\left( {0, 10} \right), {\rm{ }}\left( {20, 0} \right), {\rm{ }}\left( {0, 10} \right), {\rm{ }}\left( {25, 30} \right)$。应用FS光顺折线路径策略, 对该折线航路进行光顺处理, 得到的光滑航路如图 11所示。图 12为该光滑航路的几何性质, 且各变量均以航路长度L为参数。由于${\theta _{{\rm{end}}}}$是数值方法求取的, 这实际上会致使该光滑航路的曲率并非处处连续, 在其过渡曲线中, 即FS与镜像FS的连接处会存在一个小的间隔。但是, 由于数值解算方法可实现任意精度的解算, 最终会使得该间隔非常小, 进而在实际应用时可忽略该间隔。除此之外, 该光滑航路的任一点方位和曲率均连续。

图 11 FS光滑航路的几何性质 Fig. 11 Geometric properties of smooth path

图 12 基于PSO的USV全局路径规划图 Fig. 12 Global path planning of USV using PSO

根据文献[17]中路径评价准则, 结合直线路径和Dubins路径, 上述仿真实验中FS光滑路径存在如下性质。

(1) 光滑度:该路径的方位和曲率均连续, 即实现了路径的一阶和二阶几何连续性。而在式(24)的路径参数变换下, 可实现该路径的参数连续性。表 1给出了该路径与其他路径的几何性质比较, 其中G表示几何连续性, C表示参数连续性。

表 1 几何性质比较 Tab. 1 Comparison of different paths
路径类型 G0/ C0 G1 G2
折线路径 × ×
Dubins路径 ×
FS光滑路径
注: √和×分别表示是否具有该性质

(2) 路径长度:由图 6图 8图 13可知, 相比Dubins路径, 利用高斯超几何函数计算得到的Dubins路径和折线路径的长度更长。对于本文给定的航路点序列, 折线路径、Dubins路径和FS光滑路径长度分别为128.0、122.1和121.1 m。

图 13 基于PSO和FS的USV全局路径规划图 Fig. 13 Global path planning of USV using PSO and FS

(3) 路径偏差:当USV在障碍物环境下航向与作业时, 光顺路径与原折线路径的偏离程度越小越好。FS光滑路径的路径偏差为

$ a = k\sqrt {{\theta _{{\rm{end}}}}} \sin \left( {\rho {\theta _{{\rm{end}}}}} \right) $ (52)

而由文献[2]可知, 在折线段路径方位改变量$\left| {\Delta \chi } \right| \in \left[{0, {{140}^ \circ }} \right]$时, 相比Dubins路径, FS光滑路径的路径偏差略大, 约在0至0.3 m范围内。

(4) 局部可控性:相比自然样条等曲线路径, 改变一个路径点, 只会造成对FS光滑路径、Dubins路径和折线路径的局部影响。该性质对于USV在障碍物环境中航行时需进行增加或者减少航路点的操作情况非常有利。

(5) 算法复杂度:相比折线路径和Dubins路径, FS光滑路径复杂程度较大。

因此, FS为构建方位和曲率均连续的光滑路径提供一种新策略, 使其光顺后的折线路径可应用于精确的路径跟踪等运动控制中。

选择圆形障碍物对PSO全局路径规划算法进行仿真实例验证。障碍物圆心坐标分别为(130 m, 140 m)、(240 m, 180 m)、(240 m, 280 m)、(350 m, 280 m); 障碍物半径均取为30 m, 膨胀半径取为5 m。根据USV的航行特性, 要求每个航段长度必须大于最短航段长度${L_{\min }}$, 取${L_{\min }} = 15\;{\rm{m}}$。其他仿真参数设置为:最长极径$\rho = 45{\rm{ m}}$, 移动步长为$d = 30{\rm{ m}}$; 粒子个数为$n = 50$, 粒子维度为$m = 3$, 学习因子${c_1} = {c_2} = 1.496\;2$, 最大迭代次数${D_{\max }} = 200$, 粒子位置初始化流程中${q_{\max }}$${k_{\max }}$均取10;起始点为start $\left( {{\rm{50 m, 50 m}}} \right)$, 目标点为goal$\left( {{\rm{400 m, 350 m}}} \right)$

图 12为应用PSO路径规划算法得到的全局折线路径, 由航路点序列顺序连接而成, 其中, 图中的PSO优化结果为在$\rho $为半径的圆域内调用PSO全局路径规划算法得到的3个航路点。可看出, USV在沿目标点的直线前行过程中, 调用了3次PSO全局路径规划算法, 能有效规避障碍物, 并与之保持一定距离。

相比文献[11]、[12], 本文仅在极径$\rho $范围内调用PSO全局路径规划算法, 在极坐标下进行将二维路径编码转化为一维编码, 并考虑不能出现倒车运动状况限制粒子位置的极值大小, 大大减小了搜索范围和盲目搜索性, 其时空开销显然减少, 且获得的全局路径长度减小。因此, 本文所设计的PSO全局路径规划算法, 时空开销小, 得到由航路点序列顺序连接而成的折线路径绝对安全且最短。此外, 该算法从航行器的自身航行特性出发, 考虑了最小移动步长和不能出现倒车运动状况, 本质上简单实现了所规划路径的可行性。

在此基础上, FS光顺折线路径策略, 可得到如图 13所示的光滑路径。由于PSO全局路径规划算法中将最小化路径长度作为适应度函数, 限定粒子位置的极值大小, 且仅在给定半径的圆域内进行, 其规划得到的相邻折线段路径方位改变量幅值较小, 致使FS光顺折线路径策略的有效性不显著, 故在图上对其进行了局部放大, 以便更好地观察。

相比文献[18], 本文所设计算法保证了USV全局路径的方位和曲率均连续, 且路径长度较短。相比文献[19], 本文所设计算法从航行器自身航行特点和机动性能出发, 所规划路径满足可行性的同时, 实现了路径曲率的连续并保证路径长度较短。

因此, 利用本文所设计算法, 即先应用PSO全局路径规划算法得到航路点序列, 将其顺序连接成折线路径, 再利用FS光顺折线路径策略进行光顺处理, 即可得到一条满足航行器最小回转半径约束, 且方位和曲率均连续的安全可行全局路径。

5 结语

本文针对USV的全局路径规划问题, 提出了一种基于PSO全局路径规划算法和FS光顺折线路径策略的USV全局路径规划算法。首先, 从USV的航行特性出发, 基于PSO全局路径规划算法生成一系列航路点连接的可行折线航路; 其次, 给出FS光顺折线路径策略, 保证了光顺后路径上任一点方位和曲率的连续性; 然后, 考虑USV的机动性能限制, 利用FS光顺折线路径策略对PSO路径规划算法规划生成的可行折线航路进行光顺处理, 在满足航行器最小回转半径约束的同时, 也保证了路径方位和曲率函数的连续性, 使得所规划路径可应用于USV的路径跟踪等运动控制中。理论分析和仿真实验验证了本文所设计算法的有效性。

参考文献
[1]
姜言清. 约束条件下欠驱动AUV的路径规划问题研究[D]. 哈尔滨: 哈尔滨工程大学, 2013.
Jiang Yanqing. Research on path planning problem of underactuated AUV considering constraints[D]. Harbin: Harbin Engineering University, 2013.
[2]
Lekkas A M, Dahl A R, Breivik M, et al. Continuous-curvature path generation using Fermat's spiral[J]. Modeling Identication and Control, 2013, 4: 1-15.
[3]
Anderson E P, Beard A W, Mclain T W. Real time dynamic trajectory smoothing[J]. IEEE Transactions on Control Systems Technology, 2005, 13(3): 471-477. DOI:10.1109/TCST.2004.839555
[4]
Fraichard T, Scheuer A. From Reeds and Shepp's to continuous curvature paths[J]. IEEE Transactions on Robotics, 2004, 20(6): 1025-1035. DOI:10.1109/TRO.2004.833789
[5]
Candeloro M, Lekkas A M, S rensen A J, et al. Continuous curvature path planning using Voronoi diagrams and Fermat's spirals[C]// International Federation of Automatic Control. IFAC Proceedings Volumes (46). Holland: Elsevier Ltd, 2013: 132-137.
[6]
Lekkas A M, Fossen T I. Integral LOS path following for curved paths based on a Monotone cubic Hermite spline parametrization[J]. IEEE Transactions on Control Systems Technology, 2014, 22(6): 2287-2301. DOI:10.1109/TCST.2014.2306774
[7]
Walton D J, Meek D S, Ali J M. Planar G2 transition curves composed of cubic Bézier spiral segments[J]. Journal of Computational and Applied Mathematics, 2003, 157(2): 453-476. DOI:10.1016/S0377-0427(03)00435-7
[8]
王振华, 章卫国, 李广文, 等. 基于非均匀B-样条的G2路径平滑方法[J]. 系统工程与电子技术, 2011, 33(7): 1539-1543.
Wang Zhenhua, Zhang Weiguo, Li guangwen, et al. G2 path smoothing using non-uniform B-spline[J]. Systems Engineering and Electronics, 2011, 33(7): 1539-1543.
[9]
Fossen T I, Lekkas A M. Direct and indirect adaptive integral line-of-sight path-following controllers for marine craft exposed to ocean currents[J]. International Journal of Adaptive Control and Signal Processing, 2017, 31(4): 445-463. DOI:10.1002/acs.2550
[10]
张万绪, 张向兰, 李莹. 基于改进粒子群算法的智能机器人路径规划[J]. 计算机应用, 2014, 34(2): 510-513.
Zhang Wanxu, Zhang Xianglan, Li Ying. Path planning for intelligent robots based on improved particle swarm optimization algorithm[J]. Journal of Computer Applications, 2014, 34(2): 510-513.
[11]
孙波, 陈卫东, 席裕庚. 基于粒子群优化算法的移动机器人全局路径规划[J]. 控制与决策, 2005, 20(9): 1052-1055.
Sun Bo, Chen Weidong, Xi Yugeng. Particle swarm optimization based global path planning for mobile robots[J]. Control and Decision, 2005, 20(9): 1052-1055.
[12]
高田田, 张莉, 李炳德, 等. 基于改进粒子群算法的足球机器人路径规划[J]. 西安工程大学学报, 2016, 30(5): 609-615.
Gao Tiantian, Zhang li, Li Bingde, et al. Study on path planning of soccer robot based on improved particle swarm algorithm[J]. Journal of Xi'an Polytechnic University, 2016, 30(5): 609-615.
[13]
Hsieh H T, Chu C H. Improving optimization of tool path planning in 5-axis Flank milling using advanced PSO algorithms[J]. Robotics and Computer-Integrated Manufacturing, 2013, 29(3): 3-11. DOI:10.1016/j.rcim.2012.04.007
[14]
Hsieh H T, Chu C H. Optimization of tool path planning in 5-axis Flank milling of ruled surfaces with improved PSO[J]. International Journal of Precision Engineering and Manufacturing, 2012, 13(1): 77-84. DOI:10.1007/s12541-012-0011-9
[15]
Masehian E, Sedighizadeh D. A multi-objective PSO- based algorithm for robot path planning[C]//IEEE. Proceedings of IEEE International Conference on Industrial Technology. Chile: IEEE, 2010: 465-470.
[16]
Borhaug E, Pavlov A, Pettersen K Y. Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents[C]//IEEE. Proceedings of the 47nd IEEE Conference on Decision and Control. Mexico: IEEE, 2008: 4984-4991.
[17]
Lekkas A, Fossen T I. Advanced in Marine Robotics[M]. Germany: Lap Lambert, 2013: 63-92.
[18]
熊新娟, 范宁军, 吴炎烜. 微小型无人水面航行器的全局避障航迹规划[J]. 系统仿真学报, 2010, 22(a01): 266-268.
Xiong Xinjuan, Fan Ningjun, Wu Yanxuan. Global obstacle-avoiding path planning of micro-miniature unmanned surface vehicle[J]. Journal of System Simulation, 2010, 22(a01): 266-268.
[19]
Song L, Mao Y S, Xiang Z Q, et al. A study on path planning algorithms based upon particle swarm optimization[J]. Journal of Information and Computational Science, 2015, 12(2): 673-680. DOI:10.12733/issn.1548-7741
[20]
Moe S, Pettersen K Y, Fossen T I, et al. Line-of-sight curved path following for underactuated USVs and AUVs in the horizontal plane under the influence of ocean currents[C]//IEEE. Proceedings of 24th Mediterranean Conference on Control and Automation (MED). Greece: IEEE, 2016: 38-45.
[21]
B rhaug E, Pavlov A, Panteley E, et al. Straight line path following for formations of underactuated marine surface vessels[J]. IEEE Transactions on Control Systems Technology, 2011, 19(3): 493-506. DOI:10.1109/TCST.2010.2050889
[22]
Peters J. Changing variables[J]. IEEE Computer Graphics and Applications, 2012, 32(3): 88-93. DOI:10.1109/MCG.2012.51
[23]
Breivik M, Fossen T I. Path following for marine surface vessels[C]// IEEE. OCEANS'04 MTS/IEEE / Techno- Ocea"04 Conference Proceedings(4). Japan: IEEE, 2005: 2282-2289.
[24]
Pearson J. Computation of hypergeometric functions[D]. Oxford: University of Oxford, 2009: 41-43.