一种基于改进RRT算法的船舶路径规划与跟踪控制方法与流程

一种基于改进rrt算法的船舶路径规划与跟踪控制方法
技术领域
1.本发明涉及人工智能技术领域，具体而言，尤其涉及一种基于改进rrt 算法的船舶路径规划与跟踪控制方法。


背景技术：

2.近年来，随着物联网、大数据、云计算等技术的快速发展，智能航海受 到广泛关注。面对复杂的海洋通航环境，在满足海上航行安全，保证经济性 的前提下，如何使船舶在航行海域短时间内快速的规划出一条由起始点到终 点并可以安全避开所有障碍物的路径最短航线，并使船舶能够达到良好的路 径跟踪效果，成为学者研究的热点问题。
3.快速搜索随机树(rrt)算法是一种随机采样的算法。它是由控制理论 采用特定随机树的增量方式进行构造且不需要对所规划的空间进行建模，可 以快速地搜索空间空白区域，生成遍历空间的随机树，在一定程度上可以降 低算法的复杂性，缩短算法运行所需要的时间；其可以根据当前环境中的信 息搜索未知区域，对局部空间中的随机点进行碰撞检测，主要用于解决复杂 环境下复杂系统的路径规划问题。不过它的缺点也很明显，它得到的路径一 般质量都不是很好，常常会出现规划的路径不是或远离最优路径，或包含棱 角，不够光滑等问题。且船舶路径规划结果最后必须通过船舶跟踪控制来实 现，从而得到实际的应用，但目前有关此方面研究较少。当前船舶路径规划 及跟踪控制中还存在着算法完善性、路径不够平滑和路径跟踪信号为虚拟信 号等问题。


技术实现要素：

4.根据上述提出现有船舶路径规划及跟踪控制所存在的技术问题，本发明 提供一种基于改进rrt算法的船舶路径规划与跟踪控制方法。本发明采用嵌 入dijkstra算法的改进rrt算法进行船舶路径规划，将规划的优化路径作为 船舶运动控制系统的期望输入，基于工业界广为使用的内外环控制思想设计 船舶运动学跟踪控制率，解决了欠驱动船舶路径跟踪问题。
5.本发明采用的技术手段如下：
6.一种基于改进rrt算法的船舶路径规划与跟踪控制方法，包括如下步骤：
7.s1、获取周围环境及周围其他船舶的海况信息；
8.s2、设置参数，采用嵌入dijkstra算法的改进rrt算法对此海况下的本 船进行合理规避障碍物的路径规划；
9.s3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用 一条函数曲线表示；
10.s4、将此曲线作为船舶运动控制系统的期望输入，利用所设计的神经网 络滑膜跟踪控制器跟踪运动学子系统的引导率信号，对船舶规划路径进行跟 踪。
11.进一步地，所述步骤s2中设置参数，采用嵌入dijkstra算法的改进rrt 算法对此海况下的本船进行合理规避障碍物的路径规划的具体实现过程如 下：
12.s21、建立尺寸为500*500的环境地图；
13.s22、将模拟带有障碍物海域的灰度图转化成二值图，环境地图中黑色区 域代表障碍物，设置灰度值为1，表示地图上有障碍物，设置灰度值为0，表 示可通行路段；
14.s23、建立欧式坐标系，设置船舶起始点的位置为(10，10)，目标点的 位置为(490，490)；
15.s24、设置rrt路径搜索的步长为20，最大尝试次数为10000次；
16.s25、船舶运动学方程的位置指令[x
d yd]为xd＝t，yd为拟合多项式函 数；参数设置为k1＝k2＝0.3，k3＝3，η3＝0.50，位置初始值为[0 0 0]，采 用线性二阶微分器和船舶动力学模型，针对姿态控制律式的切换项，采用饱 和函数代替切换函数，边界层厚度取0.1，微分器参数取r＝100。
[0017]
进一步地，所述步骤s3中对规划的路径作曲线拟合得出优化后的路径， 并将优化后的路径用一条函数曲线表示的具体实现过程如下：
[0018]
s31、对规划出的最优路径进行平滑处理，用于改进rrt算法路径为折 线，导致不符合航海实际的情况，并作为输入信号进行路径跟踪；使用 matlab中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：
[0019]
y＝a1xn a2x
n-1
a3x
n-3
... anx a
n-1
[0020]
上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合；拟 合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当 误差最小时，才会出现理想的拟合效果。
[0021]
进一步地，所述步骤s4中将拟合曲线作为船舶运动控制系统的期望输 入，利用所设计的神经网络滑膜跟踪控制器跟踪运动学子系统的引导率信号， 对船舶规划路径进行跟踪的具体实现过程如下：
[0022]
s41、基于内外环控制思想，设计船舶运动学控制器；
[0023]
s42、采用神经网络估计动力学模型不确定性与外部海洋环境的扰动方 法，设计船舶动力学控制器。
[0024]
进一步地，所述步骤s41基于内外环控制思想设计船舶运动学控制器的 具体实现过程如下：
[0025]
s411、构建船舶平面运动模型；
[0026]
s412、将船舶的运动学方程表示为：
[0027][0028]
其中，θ表示船舶航向角，[x y]表示船舶在坐标系中的位置， p＝[x y θ]
t
，q表示控制律，q＝[u ω]
t
，u表示船舶实际航行速度，即 ω表示船舶的角速度，u和ω表示船舶运动模型的控制输入；
[0029]
s413、船舶航向角θ处于随动状态，因此，得到船舶运动学模型为：
[0030][0031]
s414、设计控制律u，用于实现x跟踪xd，y跟踪yd，设置理想的跟踪 轨迹为[x
d yd]，得到跟踪误差方程为：
[0032][0033]
其中，xe表示船舶所在位置横坐标与跟踪轨迹横坐标的误差，ye表示船舶 所在位置纵坐标与跟踪轨迹纵坐标的误差，则xe＝x-xd，ye＝y-yd，为 了便于计算，取：
[0034][0035]
s415、针对取滑模函数s1＝xe，则控制律为：
[0036][0037]
其中，k1＞0；通过以上求解，得到
[0038]
s416、取lyapunov函数则 xe指数收敛于零；
[0039]
s417、针对取滑模函数s2＝ye，得到设计的控制律为：
[0040][0041]
其中，k2＞0；通过以上求解，可以得到
[0042]
s418、取lyapunov函数则ye指 数收敛于零；
[0043]
s419、由误差方可进一步得到则计算得到满足的理想路径 跟踪θ为：
[0044][0045]
s4110、设理想轨迹的船舶航向角为θd；如果θ＝θd，则理想的轨迹控 制律u1和u2可以实现，但是在实际船舶运动学模型中，初始阶段的θ和θd不 可能完全一致，导致跟踪系统不稳定。因此，将计算得到的船舶航向角当 成理想值：
[0046][0047]
得到实际的位置控制律为：
[0048][0049]
当θ与θd有误差时，造成控制律无法精确实现，通过收敛算法，使θ快速 跟踪θd，使得控制率精确实现；
[0050]
s4111、设计姿态控制律ω，实现θ跟踪θd；取θe＝θ-θd，滑模函数为 s3＝θe，得到姿态控制律为：
[0051][0052]
其中，k3、η3为函数系数，且k3＞0，η3＞0；、通过求解，得到s3＝-k3s
3-η3sgns3；
[0053]
s4112、采用线性二阶微分器对外环产生的中间信号θd求导；
[0054][0055]
其中，r为微分器参数，n(t)为待微分的输入信号，x1为信号追踪，x2为 信号一阶导数的估计，微分器初始值x1(0)＝0，x2(0)＝0；
[0056]
s4113、取lyapunov函数则则实现θ指数收敛于θd。
[0057]
进一步地，所述步骤s42采用神经网络估计动力学模型不确定性与外部 海洋环境的扰动方法，设计船舶动力学控制器的具体实现过程如下：
[0058]
s421、将船舶动力模型表示为：
[0059][0060]
s422、由于u＝u cos(θ)，则得到u和r的动态方程：
[0061][0062]
s423、采用神经网络用于估计动力学模型不确定性与外部海洋环境的扰 动，有界控制律给出控制力与力矩；转化得到如下一阶非线性控制系统：
[0063][0064]
其中，x表示系统状态变量，且u表示控制输入，且u＝[m
u-1uu
,m
r-1
ur]； f(x)表示非线性函数，且f(x)＝[f1,f2]；d(t)表示有界系统扰动，且 d(t)＝[m
u-1udw
,m
r-1urw
]，|d(t)|≤d， f2＝m
r-1fr
(u,ν,r)
[0065]
s424、取制导律的参考速度为xd＝[ud,rd]，速度跟踪误差为 e＝[eu,er]＝x-xd，控制目标
[0066]
s425、取滑模函数为：
[0067][0068]
其中，c＞0，则得到：
[0069][0070]
其中，f(x)＝f(x) d(t)；
[0071]
s426、使用rbf神经网络逼近f(x)，网络算法为：
[0072][0073]
f＝w
*t
h(x) ε
[0074]
其中，x为网络输入；j为隐含层第j个节点；h＝[hj]
t
为高斯函数输出；w
*
为 网络理想权值；ε为网络逼近误差；
[0075]
s427、网络输入x＝[x
1 x2]
t
，则网络输出为：
[0076][0077][0078]
s428、定义lyapunov函数为：
[0079][0080]
其中，γ＞0，则得到：
[0081][0082]
s429、设计控制律为：
[0083][0084]
其中，ηsgn(s)为鲁棒项，则：
[0085][0086]
s4210、取η＞|ε|
max
，自适应律为：
[0087]
[0088]
得到
[0089]
s4211、当时，s≡0，根据拉萨尔不变性原理，闭环系统趋于稳定， 当t
→
∞时，s
→
0。
[0090]
较现有技术相比，本发明具有以下优点：
[0091]
1、本发明提供的基于改进rrt算法的船舶路径规划及跟踪控制方法， 针对船舶路径规划和跟踪控制问题，采用嵌入dijkstra的改进rrt算法对船 舶路径进行规划，改善了传统rrt算法的路径最优问题。
[0092]
2、本发明提供的基于改进rrt算法的船舶路径规划及跟踪控制方法， 针对改进rrt算法的规划航线作为参考跟踪路径，通过多项式拟合，使规划 航线变成了一条光滑连续的曲线，作为船舶运动学控制子系统的参考输入信 号；
[0093]
3、本发明提供的基于改进rrt算法的船舶路径规划及跟踪控制方法， 基于工业界广泛使用的内外双环控制方法，分别设计了船舶运动学控制律， 即合速度引导率，转艏角速度引导率。考虑船舶动力学子系统的特性，研究 了船舶动力学控制问题。通过引入神经网络对船舶动力学子系统的未知不确 定项进行逼近，设计了一种基于径向基神经网络算法的滑模控制器，实现了 对内外扰动及未建模动态的补偿。通过仿真实验，验证了改进rrt算法路径 规划的有效性和船舶对期望航线的路径跟踪的可行性，实现了对运动学控制 子系统引导率的跟踪控制，进而实现了船舶的路径规划与跟踪。
[0094]
基于上述理由本发明可在人工智能等领域广泛推广。
附图说明
[0095]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实 施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下 面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在 不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0096]
图1为本发明方法流程图。
[0097]
图2为本发明实施例提供的船舶运动模型图。
[0098]
图3为本发明实施例提供的具有双环的闭环系统结构图。
[0099]
图4本发明实施例提供的rrt算法及改进rrt算法对比图。
[0100]
图5为发明实施例提供的一种改进rrt算法路径拟合曲线。
[0101]
图6为本发明实施例提供的实际海况船舶路径跟踪航线。
[0102]
图7为本发明实施例提供的一种位置和角度的跟踪实验结果仿真曲线。
[0103]
图8为本发明实施例提供的一种微分器输入输出变化实验结果仿真曲 线。
[0104]
图9为本发明实施例提供的一种输入信号变化实验结果仿真曲线。
[0105]
图10为本发明实施例提供的一种动力学控制力矩实验结果仿真曲线。
[0106]
图11为本发明实施例提供的一种动力学系统控制力实验结果仿真曲线。
具体实施方式
[0107]
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实 施例中
的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然， 所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获 得的所有其他实施例，都应当属于本发明保护的范围。
[0108]
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第 一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后 次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本 发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外， 术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含， 例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于 清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过 程、方法、产品或设备固有的其它步骤或单元。
[0109]
如图1所示，本发明提供了一种基于改进rrt算法的船舶路径规划与跟 踪控制方法，包括如下步骤：
[0110]
s1、获取周围环境及周围其他船舶的海况信息；
[0111]
s2、设置参数，采用嵌入dijkstra算法的改进rrt算法对此海况下的本 船进行合理规避障碍物的路径规划；
[0112]
s3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用 一条函数曲线表示；
[0113]
s4、将此曲线作为船舶运动控制系统的期望输入，利用所设计的神经网 络滑膜跟踪控制器跟踪运动学子系统的引导率信号，对船舶规划路径进行跟 踪。
[0114]
具体实施时，作为本发明优选的实施方式，所述步骤s2中设置参数，采 用嵌入dijkstra算法的改进rrt算法对此海况下的本船进行合理规避障碍物 的路径规划的具体实现过程如下：
[0115]
s21、建立尺寸为500*500的环境地图；
[0116]
s22、将模拟带有障碍物海域的灰度图转化成二值图，环境地图中黑色区 域代表障碍物，设置灰度值为1，表示地图上有障碍物，设置灰度值为0，表 示可通行路段；
[0117]
s23、建立欧式坐标系，设置船舶起始点的位置为(10，10)，目标点的 位置为(490，490)；
[0118]
s24、设置rrt路径搜索的步长为20，最大尝试次数为10000次；
[0119]
s25、船舶运动学方程的位置指令[x
d yd]为xd＝t，yd为拟合多项式函 数；参数设置为k1＝k2＝0.3，k3＝3，η3＝0.50，位置初始值为[0 0 0]，采 用线性二阶微分器和船舶动力学模型，针对姿态控制律式的切换项，采用饱 和函数代替切换函数，边界层厚度取0.1，微分器参数取r＝100。
[0120]
具体实施时，作为本发明优选的实施方式，所述步骤s3中对规划的路径 作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示的具 体实现过程如下：
[0121]
s31、对规划出的最优路径进行平滑处理，用于改进rrt算法路径为折 线，导致不符合航海实际的情况，并作为输入信号进行路径跟踪；使用 matlab中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：
[0122]
y＝a1xn a2x
n-1
a3x
n-3
... anx a
n-1
[0123]
上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合； 拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有 当误差最小时，才会出现理想的拟合效果。本实施例中，拟合函数为10次多 项式。
[0124]
具体实施时，作为本发明优选的实施方式，所述步骤s4中将拟合曲线作 为船舶运动控制系统的期望输入，利用所设计的神经网络滑膜跟踪控制器跟 踪运动学子系统的引导率信号，对船舶规划路径进行跟踪的具体实现过程如 下：
[0125]
s41、基于内外环控制思想，设计船舶运动学控制器；
[0126]
所述步骤s41基于内外环控制思想设计船舶运动学控制器的具体实现过 程如下：
[0127]
s411、构建船舶平面运动模型，如图2所示；
[0128]
s412、将船舶的运动学方程表示为：
[0129][0130]
其中，θ表示船舶航向角，[x y]表示船舶在坐标系中的位置，p＝[x y θ]
t
，q表示控制律，q＝[u ω]
t
，u表示船舶实际航行速度，即 ω表示船舶的角速度，u和ω表示船舶运动模型的控制输入；
[0131]
s413、船舶航向角θ处于随动状态，因此，得到船舶运动学模型为：
[0132][0133]
s414、设计控制律u，用于实现x跟踪xd，y跟踪yd，设置理想的跟踪 轨迹为[x
d yd]，得到跟踪误差方程为：
[0134][0135]
其中，xe表示船舶所在位置横坐标与跟踪轨迹横坐标的误差，ye表示船舶 所在位置纵坐标与跟踪轨迹纵坐标的误差，则xe＝x-xd，ye＝y-yd，为 了便于计算，取：
[0136][0137]
s415、针对取滑模函数s1＝xe，则控制律为：
[0138][0139]
其中，k1＞0；通过以上求解，得到
[0140]
s416、取lyapunov函数则 xe指数收敛于零；
[0141]
s417、针对取滑模函数s2＝ye，得到设计的控制律为：
[0142][0143]
其中，k2＞0；通过以上求解，可以得到
[0144]
s418、取lyapunov函数则ye指 数收敛于零；
[0145]
s419、由误差方可进一步得到则计算得到满足的理想路径 跟踪θ为：
[0146][0147]
s4110、设理想轨迹的船舶航向角为θd；如果θ＝θd，则理想的轨迹控 制律u1和u2可以实现，但是在实际船舶运动学模型中，初始阶段的θ和θd不 可能完全一致，导致跟踪系统不稳定。因此，将计算得到的船舶航向角当 成理想值：
[0148][0149]
得到实际的位置控制律为：
[0150][0151]
当θ与θd有误差时，造成控制律无法精确实现，通过收敛算法，使θ快速 跟踪θd，使得控制率精确实现；
[0152]
s4111、设计姿态控制律ω，实现θ跟踪θd；取θe＝θ-θd，滑模函数为 s3＝θe，得到姿态控制律为：
[0153][0154]
其中，k3、η3为函数系数，且k3＞0，η3＞0；、通过求解，得到s3＝-k3s
3-η3sgns3；
[0155]
s4112、采用线性二阶微分器对外环产生的中间信号θd求导；
[0156][0157]
其中，r为微分器参数，n(t)为待微分的输入信号，x1为信号追踪，x2为 信号一阶导数的估计，微分器初始值x1(0)＝0，x2(0)＝0；由于该微分器具 有积分链式结构，对含有噪声的信号求导时，噪声只含在微分器的最后一 层，通过积分作用，信号一阶导数中的噪声能够被充分抑制；
[0158]
s4113、取lyapunov函数则则实现θ指数收敛于θd。
[0159]
s42、采用神经网络估计动力学模型不确定性与外部海洋环境的扰动方 法，设计
船舶动力学控制器。
[0160]
所述步骤s42采用神经网络估计动力学模型不确定性与外部海洋环境的 扰动方法，设计船舶动力学控制器的具体实现过程如下：
[0161]
s421、将船舶动力模型表示为：
[0162][0163]
s422、由于u＝u cos(θ)，则得到u和r的动态方程：
[0164][0165]
s423、采用神经网络用于估计动力学模型不确定性与外部海洋环境的扰 动，有界控制律给出控制力与力矩；转化得到如下一阶非线性控制系统：
[0166][0167]
其中，x表示系统状态变量，且u表示控制输入，且u＝[m
u-1uu
,m
r-1
ur]； f(x)表示非线性函数，且f(x)＝[f1,f2]；d(t)表示有界系统扰动，且 d(t)＝[m
u-1udw
,m
r-1urw
]，|d(t)|≤d， f2＝m
r-1fr
(u,ν,r)
[0168]
s424、取制导律的参考速度为xd＝[ud,rd]，速度跟踪误差为 e＝[eu,er]＝x-xd，控制目标
[0169]
s425、取滑模函数为：
[0170][0171]
其中，c＞0，则得到：
[0172][0173]
其中，f(x)＝f(x) d(t)；
[0174]
s426、使用rbf神经网络逼近f(x)，网络算法为：
[0175][0176]
f＝w
*t
h(x) ε
[0177]
其中，x为网络输入；j为隐含层第j个节点；h＝[hj]
t
为高斯函数输出；w
*
为 网络理想权值；ε为网络逼近误差；
[0178]
s427、网络输入x＝[x
1 x2]
t
，则网络输出为：
[0179][0180][0181]
s428、定义lyapunov函数为：
[0182][0183]
其中，γ＞0，则得到：
[0184][0185]
s429、设计控制律为：
[0186][0187]
其中，ηsgn(s)为鲁棒项，则：
[0188][0189]
s4210、取η＞|ε|
max
，自适应律为：
[0190][0191]
得到
[0192]
s4211、当时，s≡0，根据拉萨尔不变性原理，闭环系统趋于稳定， 当t
→
∞时，s
→
0。
[0193]
实施例
[0194]
为了验证本发明方案的有效性，本实施例利用matlab进行计算机仿 真研究，参数设置参见步骤s2。
[0195]
采用传统rrt算法与在rrt算法上嵌入dijkstra算法进行二次采样的改 进rrt算法，模拟船舶在海域复杂，障碍物多的情况下的路径规划，并进行 对比分析。
[0196]
表1传统rrt算法与改进rrt算法对比
[0197]
tab.1 comparison between traditional rrt algorithm and improved rrt algorithm
[0198][0199]
实验结果如图4所示，采用传统rrt算法的结果中蓝色为rrt的搜索 路径，红色为
由起始节点到目标节点采用rrt算法规划出来的渐进最优路径。 采用嵌入dijkstra算法的改进rrt算法的结果中蓝色线为rrt算法的最优路 径，红色线表示采用dijkstra算法优化rrt算法的最优路径。对比rrt算法 与dijkstra优化的rrt算法规划出来的路径长度与搜索时间可以看出，在不 碰撞障碍物的前提下，改进的rrt算法所寻找的路径比传统rrt算法的路 径规划更短，规划出来的路径最优；虽然改进rrt算法是在rrt算法的基 础上对船舶航行路径的再规划，所用路径规划时间长于rrt算法，但dijkstra 算法部分时间小于rrt算法时间，因此，dijkstra优化的rrt算法在时间上 是可以接受的，验证了该算法的优越性。
[0200]
如图6-9所示，图中对拟合出来的曲线进行路径跟踪。可以看出，船舶 在跟踪路径时，位置和航向角与规划航线没有较大偏差，船舶在具有双环结 构的闭环控制系统控制下，可以完美地跟踪路径，完成从起始点到目标点的 跟踪任务。并且θd在区间(-π/2,π/2)内，具有良好的跟踪效果。
[0201]
如图10-11所示，设计基于径向基神经网络的船舶动力学控制器，跟踪 运动学制导律给出的期望速度，来解决在真实的海洋环境航行时，船舶容易 受到海洋环境的干扰，系统存在不确定性，对船舶航行造成影响的问题。
[0202]
仿真结果表明，采用嵌入dijkstra的改进rrt算法对船舶路径进行规划， 改善了传统rrt算法的路径最优问题；针对改进rrt算法的规划航线作为 参考跟踪路径，通过多项式拟合，使规划航线变成了一条光滑连续的曲线， 作为船舶运动学控制子系统的参考输入信号；基于工业界广泛使用的内外双 环控制方法，分别设计了船舶运动学控制律，即合速度引导率，转艏角速度 引导率。考虑船舶动力学子系统的特性，研究了船舶动力学控制问题。通过 引入神经网络对船舶动力学子系统的未知不确定项进行逼近，设计了一种基 于径向基神经网络算法的滑模控制器，实现了对内外扰动及未建模动态的补 偿。验证了改进rrt算法路径规划的有效性和船舶对期望航线的路径跟踪的 可行性，实现了对运动学控制子系统引导率的跟踪控制，进而实现了船舶的 路径规划与跟踪。