文章信息
- 蒲红红, 黄海滨. 2018.
- PU Hong-hong, HUANG Hai-bin. 2018.
- 无人水面航行器全局路径规划方法研究
- Global path planning algorithm for unmanned surface vehicle
- 海洋科学, 42(1): 93-105
- Marine Sciences, 42(1): 93-105.
- http://dx.doi.org/10.11759/hykx20171011008
-
文章历史
- 收稿日期:2017-10-11
- 修回日期:2017-12-20
2. 哈尔滨工业大学(威海) 信息与电气工程学院, 山东 威海 264209;
3. 山东船舶技术研究院, 山东 威海 264209;
4. 海洋通信与组网观测技术威海市重点实验室, 山东 威海 264209
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
随着陆地资源的匮乏, 探索开发并利用占据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运动的参考坐标系有惯性坐标系
本文主要研究三自由度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) |
式中,
$ \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) |
式中,
注意到, 式(2)中仅存在作用在纵荡和艏摇两个运动自由度上的控制力/力矩τu和τr, 横荡运动自由度上不存在控制力/力矩, 系统的控制输入少于运动自由度个数, 故该三自由度USV为典型的欠驱动系统。
1.2 全局路径规划USV的全局路径规划问题可简化为在其活动环境中寻找一系列必经航路点的集合。如图 2所示, 在惯性坐标系O–XY中, start和goal分别表示航行器的起始点和目标点, 黑色填充物体则为活动环境中的障碍物。给定航行器移动步长d, 点序列
极坐标系O′为在惯性坐标系O–XY中建立的新坐标系, 其极点对应惯性坐标系中的当前航路点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) |
式中,
障碍物的判断在以当前航路点pc为圆心, ρ为半径的圆域内进行。若存在障碍物, 则根据航行器移动步长d, 预测下一个航路点, 判断障碍物是否在当前航路点与下一个预测航路点的连线之间。若是, 则调用路径规划算法规划圆域范围内的航路点序列; 否则, 航行器将继续沿着目标点的直线向前航行。
假设当前航路点和下一个预测航路点的连线之间存在障碍物, 则首先对长度ρ进行m等分, 在每个等分线上任取一个点, 可得到序列
定义长度
$ \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) |
式中,
路径规划最终可转变成最小化函数式(4), 即在取值空间内寻找使得长度Lq最小的(xi, yi),
由上述的问题描述和环境建模可知, 若当前航路点和下一个预测航路点的连线之间存在障碍物, 仅在一个以ρ为半径的圆域内调用路径规划算法, 得到少量的点序列即可完成全局路径规划任务, 其搜索量和时空开销很小, 因此可选择参数设置个数较少、实现简单, 且收敛速度快的PSO作为路径规划算法。
2 基于粒子群优化算法的全局路径规划 2.1 粒子群优化算法原理PSO采用速度-位置(v–x)搜索模型, 其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) |
式中,
$ w = {w_{\max }}-D\frac{{\left( {{w_{\max }}-{w_{\min }}} \right)}}{{{D_{\max }}}} $ | (7) |
式中, D为粒子群的当前迭代次数; Dmax为粒子群的最大迭代次数; wmax和wmin分别为最大和最小惯性权重因子, 通常取wmax=0.9, wmin=0.4。由上式可知, w随着迭代次数的增加而逐渐降低, 进而使得PSO拥有前期全局搜索能力强, 后期局部搜索能力较强的特点。
2.2 路径编码和适应度函数应用PSO求解优化问题时, 解的编码和适应度函数的选择是两个关键步骤。考虑到图 2极坐标系中的等分线, 若将序列
对于n个m维粒子, 粒子的每一维坐标为xi,j=θj, θj∈(0,2π)。但考虑到USV自身特性, 一般不会出现倒车运动状况, 而是在当前航向的正负90°范围内航行, 即若航行器在当前航路点
$ \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) |
式中,
USV的PSO全局路径规划求解步骤如下:
步骤1:给定参数。设定USV的起点start, 目标点goal, 粒子个数n, 维度m; 学习因子
步骤2:障碍物判断。首先, 对障碍物进行膨胀处理, 以提高避障安全度。然后, 在以当前航路点pc为圆心, ρ为半径的圆域内判断是否存在障碍物。若存在, 则进一步判断障碍物是否在当前航路点与下一个预测航路点的连线之间。若是, 则转向步骤3;否则, 按照给定步长d沿着目标点的直线继续向前航行。
步骤3:计算粒子位置和速度的边界值
$ v_{i, j}^{\max } = 0.1\left( {x_{i, j}^{\max }-x_{i, j}^{\min }} \right) $ | (9) |
即速度边界值
步骤4:初始化粒子的位置和速度
$ v_{i, j}^0 =-v_{i, j}^{\max } + 2Rv_{i, j}^{\max } $ | (10) |
式中,
相比速度的初始化, 粒子的位置初始化过程涉及是否与障碍物碰撞问题, 即考虑边界值约束的同时, 必须进行障碍物无碰验证。图 3给出了
其中,
得到
步骤5:更新粒子速度和位置
步骤6:转向步骤5进行迭代, 直到达到最大迭代次数
借鉴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) |
式中,
$ {\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给出了该折线路径的几何性质, 且图中各变量均以路径长度
图 6为利用圆弧对图 4中折线航路光顺后的路径, 即Dubins路径。其中, 半径R可选为航行器的最小回转半径
$ {\kappa _{\max }} = \frac{1}{{{R_{\min }}}} $ | (15) |
图 7给出了该Dubins路径的几何性质, 且图中各变量均以路径长度L为参数。可以看出, 此路径仅实现了其方位
$ \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) |
由上式知,
式(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}}_{{\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表示逆时针旋转; 参数θ的取值区域为
参数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) |
式中,
式(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) |
式中,
$ \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]可知, 对于路径跟踪等应用, 要求必须定义路径参数对时间的一阶导数, 即参数路径的速度。给定航行器期望速度
$ \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) |
其中,
在进行路径设计时, 路径长度常作所设计路径的参数。根据直线和圆弧长度的表达式, 可较容易计算得到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) |
且要求
考虑到式(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{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可通过折线航路的方位改变量
$ {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 = \sqrt {\frac{{\sqrt 7 }}{2}-\frac{5}{4}} $ | (46) |
处取得。但是, 当θ的取值区域不包含该点时, 最大曲率在
$ {\theta _{{\kappa _{\max }}}} = \min \left( {{\theta _{{\rm{end}}}}, \sqrt {\frac{{\sqrt 7 }}{2}-\frac{5}{4}} } \right) $ | (47) |
给定最大曲率
$ 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。不失一般性, 令
$ {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)求得了航路的方位改变量, 令
除满足安全性和光滑性外, 最小回转半径约束这一机动性能限制也是进行路径规划时必须要考虑的问题。Dubins路径生成时, 圆弧所在圆的半径可选为航行器最小回转半径; FS光顺折线路径时, FS的设计需给定最大曲率。而在几何上, 最小回转半径即对应最大曲率。因此, 给定航行器最小回旋半径
$ {\kappa _{\max }} = \frac{1}{{{R_{\min }}}} $ | (51) |
即式(15), 并在FS光顺折线航路时利用该最大曲率, 即可满足航行器最小回转半径约束。
4 仿真结果与分析为了更好地验证FS光顺折线路径策略的有效性, 给定一条由更多航路点连接成而的折线航路, 航路点序列为:
根据文献[17]中路径评价准则, 结合直线路径和Dubins路径, 上述仿真实验中FS光滑路径存在如下性质。
(1) 光滑度:该路径的方位和曲率均连续, 即实现了路径的一阶和二阶几何连续性。而在式(24)的路径参数变换下, 可实现该路径的参数连续性。表 1给出了该路径与其他路径的几何性质比较, 其中G表示几何连续性, C表示参数连续性。
(2) 路径长度:由图 6、图 8和图 13可知, 相比Dubins路径, 利用高斯超几何函数计算得到的Dubins路径和折线路径的长度更长。对于本文给定的航路点序列, 折线路径、Dubins路径和FS光滑路径长度分别为128.0、122.1和121.1 m。
(3) 路径偏差:当USV在障碍物环境下航向与作业时, 光顺路径与原折线路径的偏离程度越小越好。FS光滑路径的路径偏差为
$ a = k\sqrt {{\theta _{{\rm{end}}}}} \sin \left( {\rho {\theta _{{\rm{end}}}}} \right) $ | (52) |
而由文献[2]可知, 在折线段路径方位改变量
(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的航行特性, 要求每个航段长度必须大于最短航段长度
图 12为应用PSO路径规划算法得到的全局折线路径, 由航路点序列顺序连接而成, 其中, 图中的PSO优化结果为在
相比文献[11]、[12], 本文仅在极径
在此基础上, 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.
|