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

技术特征：
1.一种基于改进rrt算法的船舶路径规划与跟踪控制方法，包括如下步骤：s1、获取周围环境及周围其他船舶的海况信息；s2、设置参数，采用嵌入dijkstra算法的改进rrt算法对此海况下的本船进行合理规避障碍物的路径规划；s3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示；s4、将此曲线作为船舶运动控制系统的期望输入，利用所设计的神经网络滑膜跟踪控制器跟踪运动学子系统的引导率信号，对船舶规划路径进行跟踪。2.根据权利要求1所述的基于改进rrt算法的船舶路径规划与跟踪控制方法，所述步骤s2中设置参数，采用嵌入dijkstra算法的改进rrt算法对此海况下的本船进行合理规避障碍物的路径规划的具体实现过程如下：s21、建立尺寸为500*500的环境地图；s22、将模拟带有障碍物海域的灰度图转化成二值图，环境地图中黑色区域代表障碍物，设置灰度值为1，表示地图上有障碍物，设置灰度值为0，表示可通行路段；s23、建立欧式坐标系，设置船舶起始点的位置为(10，10)，目标点的位置为(490，490)；s24、设置rrt路径搜索的步长为20，最大尝试次数为10000次；s25、船舶运动学方程的位置指令[x
d y
d
]为x
d
＝t，y
d
为拟合多项式函数；参数设置为k1＝k2＝0.3，k3＝3，η3＝0.50，位置初始值为[0 0 0]，采用线性二阶微分器和船舶动力学模型，针对姿态控制律式的切换项，采用饱和函数代替切换函数，边界层厚度取0.1，微分器参数取r＝100。3.根据权利要求1所述的基于改进rrt算法的船舶路径规划与跟踪控制方法，所述步骤s3中对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示的具体实现过程如下：s31、对规划出的最优路径进行平滑处理，用于改进rrt算法路径为折线，导致不符合航海实际的情况，并作为输入信号进行路径跟踪；使用matlab中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：y＝a1x
n
a2x
n-1
a3x
n-3
... a
n
x a
n-1
上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合；拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当误差最小时，才会出现理想的拟合效果。4.根据权利要求1所述的基于改进rrt算法的船舶路径规划与跟踪控制方法，所述步骤s4中将拟合曲线作为船舶运动控制系统的期望输入，利用所设计的神经网络滑膜跟踪控制器跟踪运动学子系统的引导率信号，对船舶规划路径进行跟踪的具体实现过程如下：s41、基于内外环控制思想，设计船舶运动学控制器；s42、采用神经网络估计动力学模型不确定性与外部海洋环境的扰动方法，设计船舶动力学控制器。5.根据权利要求4所述的基于改进rrt算法的船舶路径规划与跟踪控制方法，所述步骤s41基于内外环控制思想设计船舶运动学控制器的具体实现过程如下：s411、构建船舶平面运动模型；
s412、将船舶的运动学方程表示为：其中，θ表示船舶航向角，[x y]表示船舶在坐标系中的位置，p＝[x y θ]
t
，q表示控制律，q＝[u ω]
t
，u表示船舶实际航行速度，即ω表示船舶的角速度，u和ω表示船舶运动模型的控制输入；s413、船舶航向角θ处于随动状态，因此，得到船舶运动学模型为：s414、设计控制律u，用于实现x跟踪x
d
，y跟踪y
d
，设置理想的跟踪轨迹为[x
d y
d
]，得到跟踪误差方程为：其中，x
e
表示船舶所在位置横坐标与跟踪轨迹横坐标的误差，y
e
表示船舶所在位置纵坐标与跟踪轨迹纵坐标的误差，则x
e
＝x-x
d
，y
e
＝y-y
d
，为了便于计算，取：s415、针对取滑模函数s1＝x
e
，则控制律为：其中，k1＞0；通过以上求解，得到s416、取lyapunov函数则x
e
指数收敛于零；s417、针对取滑模函数s2＝y
e
，得到设计的控制律为：其中，k2＞0；通过以上求解，可以得到s418、取lyapunov函数则y
e
指数收敛于零；s419、由误差方可进一步得到则计算得到满足的理想路径跟踪θ为：
s4110、设理想轨迹的船舶航向角为θ
d
；如果θ＝θ
d
，则理想的轨迹控制律u1和u2可以实现，但是在实际船舶运动学模型中，初始阶段的θ和θ
d
不可能完全一致，导致跟踪系统不稳定。因此，将计算得到的船舶航向角当成理想值：得到实际的位置控制律为：当θ与θ
d
有误差时，造成控制律无法精确实现，通过收敛算法，使θ快速跟踪θ
d
，使得控制率精确实现；s4111、设计姿态控制律ω，实现θ跟踪θ
d
；取θ
e
＝θ-θ
d
，滑模函数为s3＝θ
e
，得到姿态控制律为：其中，k3、η3为函数系数，且k3＞0，η3＞0；、通过求解，得到s3＝-k3s
3-η3sgns3；s4112、采用线性二阶微分器对外环产生的中间信号θ
d
求导；其中，r为微分器参数，n(t)为待微分的输入信号，x1为信号追踪，x2为信号一阶导数的估计，微分器初始值x1(0)＝0，x2(0)＝0；s4113、取lyapunov函数则则实现θ指数收敛于θ
d
。6.根据权利要求4所述的基于改进rrt算法的船舶路径规划与跟踪控制方法，所述步骤s42采用神经网络估计动力学模型不确定性与外部海洋环境的扰动方法，设计船舶动力学控制器的具体实现过程如下：s421、将船舶动力模型表示为：s422、由于u＝ucos(θ)，则得到u和r的动态方程：
s423、采用神经网络用于估计动力学模型不确定性与外部海洋环境的扰动，有界控制律给出控制力与力矩；转化得到如下一阶非线性控制系统：其中，x表示系统状态变量，且u表示控制输入，且u＝[m
u-1
u
u
,m
r-1
u
r
]；f(x)表示非线性函数，且f(x)＝[f1,f2]；d(t)表示有界系统扰动，且d(t)＝[m
u-1
u
dw
,m
r-1
u
rw
]，|d(t)|≤d，f2＝m
r-1
f
r
(u,ν,r)s424、取制导律的参考速度为x
d
＝[u
d
,r
d
]，速度跟踪误差为e＝[e
u
,e
r
]＝x-x
d
，控制目标s425、取滑模函数为：其中，c＞0，则得到：其中，f(x)＝f(x) d(t)；s426、使用rbf神经网络逼近f(x)，网络算法为：f＝w
*t
h(x) ε其中，x为网络输入；j为隐含层第j个节点；h＝[h
j
]
t
为高斯函数输出；w
*
为网络理想权值；ε为网络逼近误差；s427、网络输入x＝[x
1 x2]
t
，则网络输出为：，则网络输出为：s428、定义lyapunov函数为：其中，γ＞0，则得到：
s429、设计控制律为：其中，ηsgn(s)为鲁棒项，则：s4210、取η＞|ε|
max
，自适应律为：得到s4211、当时，s≡0，根据拉萨尔不变性原理，闭环系统趋于稳定，当t
→
∞时，s
→
0。

技术总结
本发明提供一种基于改进RRT算法的船舶路径规划与跟踪控制方法，包括：获取周围环境及周围其他船舶的海况信息；设置参数，采用嵌入Dijkstra算法的改进RRT算法对此海况下的本船进行合理规避障碍物的路径规划；对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示；将此曲线作为船舶运动控制系统的期望输入，利用所设计的神经网络滑膜跟踪控制器跟踪运动学子系统的引导率信号，对船舶规划路径进行跟踪。本发明采用嵌入Dijkstra算法的改进RRT算法进行船舶路径规划，将规划的优化路径作为船舶运动控制系统的期望输入，基于工业界广为使用的内外环控制思想设计船舶运动学跟踪控制率，解决了欠驱动船舶路径跟踪问题。舶路径跟踪问题。舶路径跟踪问题。


技术研发人员：宁君 马昊冉 历志 李伟 李铁山
受保护的技术使用者：大连海事大学
技术研发日：2021.08.24
技术公布日：2022/1/21