CN114077256A - 一种水上无人船路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及路径规划领域，具体涉及一种水上无人船路径规划方法。

背景技术

水面无人船如何能够在复杂多变的海域环境下安全、有效地行驶，根据有效的手段避开潜在的风险，并以最优路径到达终点的研究中，主要研究局部自主避障和全局路径规划的技术路线。在全局路径规划中，环境信息完全己知，这其中包括障碍物位置，形状等，以此为基础建立环境信息模型，再利用相关路径捜索算法求最优解。全局规划是根据环境中的已知信息所做出的规划，而局部规划则是根据外界环境的变化而作出的实时动态规划。局部规划是研究对象通过传感器等设备感知当前所处位置以及外界环境的变化情况后，规划出从起点或者子目标点到下一个子目标点的路径。

目前的研究主要包括以下方法：

①可视图法(Visibility Graph)是在1979年由Michael A.Wesley提出并率先使用的。该算法将起始点与所有障碍物的顶点同终点用直线组合连接起来，同时保证每一条连线不能穿过障碍物，然后搜寻起始点至终点的最优路径。该算法的优点在于，算法本身比较简单，但搜索时间较长且当障碍物繁多时，易出现组合大***。

②自由空间法问世于上世纪80年代，由麻省理工学院lozano-Perez教授所提出，其全称为基于C空间的自由空间法。该算法将环境空间分为自由空间与障碍物空间两部分，然后利用搜索算法在划分的自由空间中搜寻一条路径。自由空间法最大的特点是比较灵活，但其复杂程度也随着障碍物的个数的增加而提升。

③单元分解法被Brooks所提出。该算法将空间划分为各个子模块，并将每个子模块视为一个单元，将每相邻个单元连接起来成为连通图，这样就将路径的搜索简化为针对图的搜索。

④拓扑法是对需要规划的空间进行划分，将其分割成具有拓扑特征子空间，由于空间彼此之间具有连通性，可在此基础上建立拓扑网络，利用建立的拓扑网路捜寻路径，并依据搜寻到的拓扑路径求解出几何路径。拓朴法主要思想是降维，该方法的优点是大大缩小了捜索空间，而且只有障碍物的数目决定算法的复杂性，此外，由于该法不需要机器人的准确位置，因此，也就有了更好的鲁椿性，但是，还存在不足，建立拓朴网络时比较复杂，特别是当障碍物数目增加时，修正己经存在的拓扑网将存在一定的困难。

⑤上世纪90年代，Eberhart和Kennedy两位博士受大自然中鸟类捕食行为的启发，提出了粒子群(PSO)算法。粒子群算法是群智能算法中的一种，可以被视作为现代优化算法。鸟类在觅食时，不了解自己的位置，也不了解食物同自己的距离，此时最有效的策略是靠近离食物最近的鸟的位置，这样寻找到食物的几率就会大大增加。这种机制是一种共享的机制，在不断认知中，个体兼顾自身与其他个体信念，当察觉周围个体信念更优时，个体自身将做出适应性调整，以达到种群最优化。粒子群算法个体与个体之间既有协作又有竞争，进而在复杂的空间中寻求到最优解。

⑥遗传巧法是模拟达尔文生物进化论的一种仿生学算法，它利用了进化论中自然选择和遗传学机理的计算模型，是通过模拟自然进化过程捜索最优解的方法。遗传算法包含染色体的编码、群体的初始化、适应值评价、选择种群、种群的交配及变异这几个问题。它应用于移动机器人路径规划时，采用不确定搜索规则，自适应的调整捜索方向，遗传算法其主要特点是直接对结构对象进行操作，不存在求导和函数连续性的限定，具有内在的隐并行性和更好的全局寻优能为；采用概率化的寻优方法，能自动获取和指导优化的搜索空间，自适应地调整捜索方向，不需要确定的规则。

⑦模拟退火算法源自物理退火的过程，该算法由1983年被Kirkpatrick所提出。模拟退火算法的基本思想为根据随机优化问题的求解过程和热平衡问题在统计力学方面的相似原理，经过人为设定***的最初温度、最初态势以及降温率来控制温度使其不断的下降，并结合了突跳特性，充分的利用了解空间的邻域范围，以便进行随机搜索。模拟退火算法应用于局部路径规划的优势在于可避免局部极值的出现，但却存在着收敛条件苛刻的缺陷，使得其在实际应用中无法得到满足。在计算量有限的条件下，模拟退火算法的收敛性取决于其自身参数，基于这种情况，如何设定参数就成为算法应用中的重要环节。

⑧模糊逻辑算法因为模拟了人类的固有思维方式，所以避免了对其进行建模的麻烦。该算法对传感器的精度要求不是很高，而且对于所研究物体的周围环境的不确定性亦不敏感，这样可以使得所研究对象展现出良好的连续性、一致性以及稳定性。但是因为模糊隶属度以及控制规则均需要认为的进行设定，因此在总结规律时较为困难，除此之外，控制规则一经确立，便很难在线对其进行调整，不能使算法满足情况的变化。所以，怎么样获取隶属度函数、怎样合理制定控制规则、怎样对控制规则进行调整是模糊逻辑算法所要面对的重大问题。面临这些问题，可以将模糊逻辑算法与其他算法相结合的思想，也可以对控制器进行调整，使算法具备学习能力以及自主适应能力。

⑨人工势场APF(Artificial Potential Field)算法于1986年被Khatib提出并被率先引入机器人避障问题中来。人工势场法的基本思想为把研究对象在环境空间中的运动视为受到虚拟势场的作用而产生的运动，目标点对研究对象产生着吸引力，而障碍物对研究对象产生排斥力，使研究对象在一众合力的共同作用下，向目标点运动。人工势场法暴露出来的问题多数是在相对复杂的环境信息中，机器人在特殊运动状态下目标点和障碍物形成特殊位置关系而导致。比如机器人走到相近的障碍物之间难以发现路径，走在狭窄的通道中容易来回摆动，当目标点附近存在障碍物时无法发现目标点等。

发明内容

本发明的目的是针对上述不足，提出了一种局部自主避障拟采用改进的人工势场和模糊逻辑算法，全局路径规划采用优化的粒子群算法的水上无人船路径规划方法。

本发明具体采用如下技术方案：

一种水上无人船路径规划方法，包括以下步骤：

(1)构建水面无人船运动的数学模型；水面无人船的运动模型包括地面坐标系η＝[x,y,ψ]^T和运动坐标系v＝[u,v,r]^T，对这两个坐标系建立对应的关系，通过坐标变换，得到对于水面无人船在水平面上的两个坐标系之间的关系如式(1)、(2)所示：

η＝R(ψ)ν (1)

其中，x，y为水面无人船的位置坐标；u为纵荡速度，v为横荡速度，r为艏摇角速度；R(4)为大地坐标系到运动坐标系的旋转矩阵；ψ为艏摇角度，范围4∈(-π,m)，水面无人船的运动模型如式(3)所示：

M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)

式(3)中各项的表达式如下所示：

τ＝[τ_u 0 τ_r]^T (4)

C₂₃＝-C₃₂＝m₁₁u (8)

其中τ为控制力，R为艏摇旋转矩阵，M为惯性系数矩阵，C为科氏力和向心力矩阵，D为阻尼系数矩阵。

(2)分析水面无人船在路径规划中的优化目标

根据建立的水面无人船运动的数学模型，得到无人船的动力学和运动学模型，描述船舶运动规律，使用改进LOS法进行路径跟踪的控制模型如式(10)-(13)所示：

其中，x_c,y_c，ψ_c是跟踪误差，x_sf,y_sf,ψ_sf表示目标点位姿信息，s＝ν_r,ν_t和ν_r是对目标路径的垂向和切向速度分解，K(s)为参考点曲率，有：

ψ″_ff＝ψ″_B+χ″ (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)仿真验证试验。

优选地，在水面无人船局部区域自主避障策略研究中，采用动态引力势场模型和动态斥力势场模型；

当目标点是运动的，机器人在跟踪障碍物的同时还要躲避障碍物，建立目标点的引力势场模型如式(24)，该模型由两部分组成，基于相对位置势和基于相对速度的势场函数部分：

其中v表示机器人当前的运动速度，v_goal表示目标点当前的速度，二者均为矢量，v-v_goal为机器人和目标点之间的相对速度，通过调整k_attp和k_attv的大小调整相对位置和相对速度在引力总势场中的比例，当k_attv取零值时，就退化成传统的引力场函数；

斥力势场函数引入了相对速度项，如式(26)，综合相对位置的斥力势场函数，选取动态斥力势场模型如式(27)；

是机器人与障碍物之间的基于相对位置产生的斥力势场，

是机器人和障碍物之间的基于相对速度产生的斥力势场，k_repp和k_repv为斥力场可调常量，p-p_obs表示机器人和障碍点之间的欧几里德距离，是一个矢量，方向由机器人和障碍物的连线上指向机器人当前位置，e是一个单位向量，其方向是从机器人指向障碍物，V为机器人和障碍物相对速度的分量，其方向在机器人和障碍物的连线上，当V≤0时，障碍物远离机器人运动，当V＞0时，障碍物朝向机器人运动。

优选地，在水面无人船全局路径规划最优控制研究中，基于模拟退火的混合粒子群算法具有以下特征：

1)增加“初始温度”t₀，t₀计算式如(28)：

t₀＝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)+c₁r₁[p_i,j-x_i,j(t)]+c₂r₂[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表示粒子对应的解的维数。

本发明具有如下有益效果：

本路径规划方法，利用模拟退火算法的突跳特性来解决，模拟退火算法的突跳特性可以在粒子群算法陷入局部最优解时跳出局部最优，从而增强其全局搜索能力。同时，模拟退火算法和粒子群算法的规则都比较简单，易于实现，且二者的算法效率都较高。

附图说明

图1为局部自主避障技术路线分析示意图；

图2为全局路径规划技术路线分析示意图；

图3为目标不可达问题受力分析图；

图4为目标不可达问题仿真实例示意图；

图5为动态避障仿真实例2第43步示意图；

图6为动态避障仿真实例2第103步示意图；

图7为基于模拟退火的粒子群算法流程；

图8为普通粒子群算法规划示意图；

图9为改进普通粒子群算法规划示意图；

图10为普通粒子群算法迭代次数示意图；

图11为改进普通粒子群算法迭代次数示意图；

图12为普通粒子群算法规划；

图13为改进普通粒子群算法规划；

图14为普通粒子群算法迭代次数；

图15为改进普通粒子群算法迭代次数。

具体实施方式

下面结合附图和具体实施例对本发明的具体实施方式做进一步说明：

结合图1-7，一种水上无人船路径规划方法，包括以下步骤：

(1)构建水面无人船运动的数学模型(数学模型包括动力模型和运动模型)；水面无人船的运动模型包括地面坐标系η＝[x,y,ψ]^T和运动坐标系v＝[u,v,r]^T，对这两个坐标系建立对应的关系，通过坐标变换，得到对于水面无人船在水平面上的两个坐标系之间的关系如式(1)、(2)所示：

η＝R(ψ)ν (1)

其中，x，y为水面无人船的位置坐标；u为纵荡速度，v为横荡速度，r为艏摇角速度；R(4)为大地坐标系到运动坐标系的旋转矩阵；ψ为艏摇角度，范围4∈(-π,m)，水面无人船的运动模型如式(3)所示：

M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)

式(3)中各项的表达式如下所示：

τ＝[τ_u 0 τ_r]^T (4)

C₂₃＝-C₃₂＝m₁₁u (8)

其中τ为控制力，R为艏摇旋转矩阵，M为惯性系数矩阵，C为科氏力和向心力矩阵，D为阻尼系数矩阵。

(2)分析水面无人船在路径规划中的优化目标

根据建立的水面无人船运动的数学模型，得到无人船的动力学和运动学模型，描述船舶运动规律，为保证水面无人船在考虑能耗的路径中航向稳定、航速稳定和路径长度的有机平衡，需要尽可能真实的模拟路径规划中的环境变量，确定优化目标，使用改进LOS法进行路径跟踪的控制模型如式(10)-(13)所示：

其中，x_c,y_c，ψ_c是跟踪误差，x_sf,y_sf,ψ_sf表示目标点位姿信息，s＝ν_r,ν_t和ν_r是对目标路径的垂向和切向速度分解，K(s)为参考点曲率，有：

ψ″_ff＝ψ″_B+χ″ (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)水面无人船局部区域自主避障策略研究

局部规划则是根据外界环境的变化而作出的实时动态规划。局部规划是研究对象通过传感器等设备感知当前所处位置以及外界环境的变化情况后，规划出从起点或者子目标点到下一个子目标点的路径。避障是局部规划研究的重点内容，水面无人船正常行驶时，突遇不明静态或者动态障碍物干扰航线，此时无人船将作出调整，以便及时的规避危险。

经典人工势场法缺陷

人工势场法暴露出来的问题多数是在相对复杂的环境信息中，机器人在特殊运动状态下目标点和障碍物形成特殊位置关系而导致。比如机器人走到相近的障碍物之间难以发现路径，走在狭窄的通道中容易来回摆动，当目标点附近存在障碍物时无法发现目标点等。下面就势场法最突出的几个问题作出研究讨论。

(1)目标不可达问题

如果在目标点附近存在障碍物，且机器人同样处在障碍物的影响范围之内，机器人接近障碍物时斥力增大，越是接近就越急剧增大，而引力相对斥力来说很小，就导致机器人最终无法到达目标点。这是人工势场法的目标不可达问题，有些文献中也称作全局最小值问题，意思是说目标点不是全局的总势场最低点。图2所示，机器人和目标点都在障碍物的影响范围以内，p-p_obs值减小，则

值增大，从而

增大；而p-p_goal减小，从而

减小，最后导致机器人被障碍物排斥到远离目标点。

图4是经典势场法的目标不可达问题仿真图。在该仿真中设置了4个障碍物，如图中图形所示，其中在目标点附近有两个障碍物。由仿真图很明显看出机器人快要到达目标点的时候，进入两个障碍物的影响范围内，斥力过大导致机器人在该点处来回兜圈，到达不了目标点。

(2)局部极小值问题

在多障碍物环境中，障碍物的分布可能性有很多，在机器人尚未到达目标之前的某点，目标点对机器人的虚拟引力和障碍物对机器人的虚拟斥力，恰好大小相等方向相反，此时的合力为零，机器人就误以为自己到达了目标点。就在此处停止不前或者徘徊，不知道下一步的运动方向，造成机器人无法到达目标点，这是人工势场法的局部极小值问题。

(3)路径震荡问题

另外当机器人在通过障碍物较多的位置时，会出现局部震荡。震荡的原因是机器人在该位置所受到的引力为零，但是速度并没有为零，所以在惯性作用下机器人继续前进，离开了该点，与此同时又受到了一个相反的斥力作用，使得机器人速度减小到零，又在斥力的作用下向后运动，如此循环往复。由此可见震荡的根本原因同样来自于局部极小值问题。在震荡的过程中，机器人有些时候能够自己走出局部极小值，震荡时浪费很多时间，明显放慢了机器人的速度，但大多数情况下还是不能独自走出局部极小值。

在经典斥力势场函数的基础上进行改进，改进后的斥力势场函数在随着机器人接近目标点的同时，斥力会减小，如式(23)所示：

其中，p-p_goal是一个矢量，表示机器人和目标点之间的欧几里德距离，方向是在机器人和目标点的连线上，由机器人指向目标点；该式与经典势函数中的斥力势场函数比较，增加了p-p_goal乘子，以此来保证机器人在到达目标点时的势场值是全局最小。

在水面无人船局部区域自主避障策略研究中，采用动态引力势场模型和动态斥力势场模型；

当目标点是运动的，机器人在跟踪障碍物的同时还要躲避障碍物，建立目标点的引力势场模型如式(24)，该模型由两部分组成，基于相对位置势和基于相对速度的势场函数部分：

其中v表示机器人当前的运动速度，v_goal表示目标点当前的速度，二者均为矢量，v-v_goal为机器人和目标点之间的相对速度，通过调整k_attp和k_attv的大小调整相对位置和相对速度在引力总势场中的比例，当k_attv取零值时，就退化成传统的引力场函数；

斥力势场函数引入了相对速度项，如式(26)，综合相对位置的斥力势场函数，选取动态斥力势场模型如式(27)；

是机器人与障碍物之间的基于相对位置产生的斥力势场，

是机器人和障碍物之间的基于相对速度产生的斥力势场，k_repp和k_repv为斥力场可调常量，p-p_obs表示机器人和障碍点之间的欧几里德距离，是一个矢量，方向由机器人和障碍物的连线上指向机器人当前位置，e是一个单位向量，其方向是从机器人指向障碍物，V为机器人和障碍物相对速度的分量，其方向在机器人和障碍物的连线上，当V≤0时，障碍物远离机器人运动，当V＞0时，障碍物朝向机器人运动。

(4)水面无人船全局路径规划最优控制研究

基于模拟退火的混合粒子群算法进行水面无人船全局路径规划最优控制，基于模拟退火的混合粒子群算法是在基本粒子群算法中加入模拟退火算法的操作，粒子群算法中加入的因素主要包括初始温度、退温方式和以一定概率接受较差解的能力。

在水面无人船全局路径规划最优控制研究中，基于模拟退火的混合粒子群算法具有以下特征：

1)增加“初始温度”t₀，t₀计算式如(28)：

t₀＝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)+c₁r₁[p_i,j-x_i,j(t)]+c₂r₂[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表示粒子对应的解的维数。

(5)仿真验证试验，结合图8-15，图8中，环境地图实例1，普通粒子群算法下，设定环境尺寸为20*20，路径规划情况。左上角方框为出发点，右下角星状为目的点。经过普通粒子群算法路径规划，可以由出发点到达目的点。

图9中，环境地图实例1，改进粒子群算法下，设定环境尺寸为20*20，路径规划情况。左上角方框为出发点，右下角星状为目的点。经过改进粒子群算法路径规划，可以由出发点到达目的点，并且改进粒子群算法路径规划的路径比普通粒子群算法更短。

图10和图11分别是环境地图实例1中，普通粒子群算法和改进粒子群算法下，路径规划迭代次数。由图10和图11所示可知，普通粒子群算法(图10)和改进粒子群算法(图11)在迭代5次后均达到各自的局部最优解，在这之后，改进的粒子群算法在经过7次迭代后发现了比之前更优的解，并稳定在最优解，直至搜索结束；普通粒子群算法在经过54次迭代后，到达更优解，直至搜索结束。由此可见，在收敛速度方面上，根据实验结果，比较明显的发现改进粒子群算法快于普通粒子群算法；而在规划路径长度方面，改进粒子群算法的路径长度也小于普通粒子群算法。

图12为环境地图实例2，普通粒子群算法下，设定环境尺寸为20*20，路径规划情况。左上角方框为出发点，右下角星状为目的点。经过普通粒子群算法路径规划，并没有规划出从出发点到目的地的路径，而是陷入了局部最优值，未能到达终点。

图13为环境地图实例2，改进粒子群算法下，设定环境尺寸为20*20，路径规划情况。左上角方框为出发点，右下角星状为目的点。经过改进粒子群算法路径规划，可以由出发点到达目的点，跳出了图12中的局部最优值。

图14和图15分别是环境地图实例2中，普通粒子群算法和改进粒子群算法下，路径规划迭代次数。由图14和图15所示可知，普通粒子群算法(图14)和改进粒子群算法(图15)在迭代3次后均达到各自的局部最优解，在这之后，改进的粒子群算法在经过7次迭代后发现了比之前更优的解，并稳定在最优解，直至搜索结束；普通粒子群算法在经过10次迭代后，到达局部最优位置，但却无法从中“挣脱”出来继续搜索，使最终搜索结果滯留在局部值7100附近。由此可见，改进粒子群算法能够克服普通粒子群算法的先天不足，能够突破局部最优值障碍，使规划结果最终达到全局最优解。

应用MATLAB仿真软件，对于上述避障策略、路径规划算法进行仿真验证。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (3)

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)

C₂₃＝-C₃₂＝m₁₁u (8)

其中τ为控制力，R为艏摇旋转矩阵，M为惯性系数矩阵，C为科氏力和向心力矩阵，D为阻尼系数矩阵；

(2)分析水面无人船在路径规划中的优化目标

根据建立的水面无人船运动的数学模型，得到无人船的动力学和运动学模型，描述船舶运动规律，使用改进LOS法进行路径跟踪的控制模型如式(10)-(13)所示：

其中，x_c,y_c，ψ_c是跟踪误差，x_sf,y_sf,ψ_sf表示目标点位姿信息，s＝ν_r,ν_t和ν_r是对目标路径的垂向和切向速度分解，K(s)为参考点曲率，有：

ψ″_ff＝ψ″_B+χ” (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)；

是机器人与障碍物之间的基于相对位置产生的斥力势场，

是机器人和障碍物之间的基于相对速度产生的斥力势场，k_repp和k_repv为斥力场可调常量，p-p_obs表示机器人和障碍点之间的欧几里德距离，是一个矢量，方向由机器人和障碍物的连线上指向机器人当前位置，e是一个单位向量，其方向是从机器人指向障碍物，V为机器人和障碍物相对速度的分量，其方向在机器人和障碍物的连线上，当V≤0时，障碍物远离机器人运动，当V＞0时，障碍物朝向机器人运动。

3.如权利要求1所述的一种水上无人船路径规划方法，其特征在于，在水面无人船全局路径规划最优控制研究中，基于模拟退火的混合粒子群算法具有以下特征：

1)增加“初始温度”t₀，t₀计算式如(28)：

t₀＝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)+c₁r₁[p_i,j-x_i,j(t)]+c₂r₂[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表示粒子对应的解的维数。

Images