一种水上无人船路径规划方法与流程

技术特征：
1.一种水上无人船路径规划方法，其特征在于，包括以下步骤：(1)构建水面无人船运动的数学模型；水面无人船的运动模型包括地面坐标系η＝[x,y,ψ]
t
和运动坐标系v＝[u,v,r]
t
，对这两个坐标系建立对应的关系，通过坐标变换，得到对于水面无人船在水平面上的两个坐标系之间的关系如式(1)、(2)所示：η＝r(ψ)ν
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)其中，x，y为水面无人船的位置坐标；u为纵荡速度，v为横荡速度，r为艏摇角速度；r(ψ)为大地坐标系到运动坐标系的旋转矩阵；ψ为艏摇角度，范围4∈(-π,m)，水面无人船的运动模型如式(3)所示：m(ν)ν c(ν)ν d(ν)ν＝τ
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)式(3)中各项的表达式如下所示：τ＝[τ
u 0 τ
r
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)(4)(4)c
23
＝-c
32
＝m
11
u
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)其中τ为控制力，r为艏摇旋转矩阵，m为惯性系数矩阵，c为科氏力和向心力矩阵，d为阻尼系数矩阵；(2)分析水面无人船在路径规划中的优化目标根据建立的水面无人船运动的数学模型，得到无人船的动力学和运动学模型，描述船舶运动规律，使用改进los法进行路径跟踪的控制模型如式(10)-(13)所示：(13)所示：(13)所示：
其中，x
c
,y
c
，ψ
c
是跟踪误差，x
sf
,y
sf
,ψ
sf
表示目标点位姿信息，s＝ν
r
,ν
t
和ν
r
是对目标路径的垂向和切向速度分解，k(s)为参考点曲率，有：切向速度分解，k(s)为参考点曲率，有：ψ
″
ff
＝ψ
″
b
χ
”ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)(16)(16)式中ψ
b
是船艏向；水面无人船在航行中对x
c
,y
c
，ψ
c
三个跟踪误差进行控制，对航速进行控制,使航速趋近于期望航速u，因此水面无人船路径跟踪控制的目标是使误差p
c
最小，如式(20)所示；p
c
＝(x
c
,y
c
,ψ
c
,ν
c
)
t
→0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)为使规划的扫海路径便于跟踪，当水面无人船保持航向稳定，进行直线航行时，艏向角的角速度为0，即式(20)将简化为式(21)：p
c
＝(x
c
,y
c
,ν
c
)
t
→0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)当水面无人船保持匀速直线航行时，航速v不变，将进一步简化为式(22)p
c
＝(x
c
,y
c
)
t
→0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)；(3)水面无人船局部区域自主避障策略研究在经典斥力势场函数的基础上进行改进，改进后的斥力势场函数在随着机器人接近目标点的同时，斥力会减小，如式(23)所示：其中，p-p
goal
是一个矢量，表示机器人和目标点之间的欧几里德距离，方向是在机器人和目标点的连线上，由机器人指向目标点；该式与经典势函数中的斥力势场函数比较，增加了p-p
goal
乘子，以此来保证机器人在到达目标点时的势场值是全局最小；(4)水面无人船全局路径规划最优控制研究基于模拟退火的混合粒子群算法进行水面无人船全局路径规划最优控制，基于模拟退火的混合粒子群算法是在基本粒子群算法中加入模拟退火算法的操作，粒子群算法中加入的因素主要包括初始温度、退温方式和以一定概率接受较差解的能力；(5)仿真验证试验；
2.如权利要求1所述的一种水上无人船路径规划方法，其特征在于，在水面无人船局部区域自主避障策略研究中，采用动态引力势场模型和动态斥力势场模型；当目标点是运动的，机器人在跟踪障碍物的同时还要躲避障碍物，建立目标点的引力势场模型如式(24)，该模型由两部分组成，基于相对位置势和基于相对速度的势场函数部分：其中v表示机器人当前的运动速度，v
goal
表示目标点当前的速度，二者均为矢量，v-v
goal
为机器人和目标点之间的相对速度，通过调整k
attp
和k
attv
的大小调整相对位置和相对速度在引力总势场中的比例，当k
attv
取零值时，就退化成传统的引力场函数；斥力势场函数引入了相对速度项，如式(26)，综合相对位置的斥力势场函数，选取动态斥力势场模型如式(27)；斥力势场模型如式(27)；斥力势场模型如式(27)；斥力势场模型如式(27)；是机器人与障碍物之间的基于相对位置产生的斥力势场，是机器人和障碍物之间的基于相对速度产生的斥力势场，k
repp
和k
repv
为斥力场可调常量，p-p
obs
表示机器人和障碍点之间的欧几里德距离，是一个矢量，方向由机器人和障碍物的连线上指向机器人当前位置，e是一个单位向量，其方向是从机器人指向障碍物，v为机器人和障碍物相对速度的分量，其方向在机器人和障碍物的连线上，当v≤0时，障碍物远离机器人运动，当v＞0时，障碍物朝向机器人运动。3.如权利要求1所述的一种水上无人船路径规划方法，其特征在于，在水面无人船全局路径规划最优控制研究中，基于模拟退火的混合粒子群算法具有以下特征：1)增加“初始温度”t0，t0计算式如(28)：t0＝fit(p
g
)/ln 5
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)其中，fit(x)表示适应度函数，p
g
为全局极值；2)增加“退火系数”λ，以控制退温方式，退温方式如式(29)t
k 1
＝λt
k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)3)增加了当前温度下粒子被选择为全局最优的概率计算公式，形式如(30)：其中，t表示当前的温度，n表示粒子群中粒子的个数；
4)模仿模拟退火算法的突跳特性，从所有个体中选取一个p
i
作为全局最优p'
g
来代替真正的全局最优p
g
，即以一定的概率选择较差解作为全局最优值，p'
g
的选取采用轮盘赌法，其中每个个体被选为全局最优的概率由式(30)计算；5)根据式(31)～(33)更新各粒子的速度和位置：ν
i,j
(t 1)＝φ{ν
i,j
(t) c1r1[p
i,j-x
i,j
(t)] c2r2[p
g,j-x
i,j
(t)]}
ꢀꢀꢀꢀꢀꢀꢀꢀ
(31)x
i,j
(t 1)＝x
i,j
(t) ν
i,j
(t 1),j＝1,2,...,d
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(32)其中，φ表示压缩因子，d表示粒子对应的解的维数。

技术总结
本发明公开了一种水上无人船路径规划方法，涉及路径规划领域。水上无人船路径规划方法包括以下步骤：构建水面无人船运动的数学模型；分析水面无人船在路径规划中的优化目标；根据建立的数学模型，得到无人船的动力学和运动学模型，描述船舶运动规律；水面无人船局部区域自主避障策略研究；水面无人船全局路径规划最优控制研究；全局路径规划是指全部了解周围相关环境后，做出的路径规划，全局规划需要掌握运动空间中的所有环境信息，并根据这些信息布置路径规划。研究多种全局路径规划方法，寻求最优控制算法，最后对于上述避障策略、路径规划算法进行仿真验证。径规划算法进行仿真验证。径规划算法进行仿真验证。


技术研发人员：王攀攀
受保护的技术使用者：威海海洋职业学院
技术研发日：2021.12.10
技术公布日：2022/2/23