CN113296509B - 一种用于无人表面船的自主轨迹追踪融合控制方法 - Google Patents

Abstract

本发明公开了一种用于无人表面船的自主轨迹追踪融合控制方法，该方法具体包括：利用电场模型和快速匹配算法产生的方向趋势和弹性距离来设计控制器的避碰约束和目标函数；重新构建模型预测控制算法的架构，形成新型融合控制方法；采用CasADi方法进行快速优化，实时避碰的同时实现轨迹跟踪目标，在兼顾效率和精度的前提下获得平滑且冗余较少的无碰撞追踪轨迹，并通过仿真对比验证，分析轨迹和状态调整效果，得到了良好的控制性能，对多船环境下的轨迹追踪控制实践具有指导意义。

Description

一种用于无人表面船的自主轨迹追踪融合控制方法

技术领域

本发明涉及轨迹追踪融合控制领域，具体为一种用于无人表面船的自主轨迹追踪融合控制方法。

背景技术

无人表面船的轨迹控制是决定自主导航效果的关键因素，主要衡量轨迹的平滑度、角度可达性、姿态波动、追踪时间等。在实际航行中，还需考虑周围船舶的避碰问题，以便动态调整航行轨迹，适应多船的复杂环境。目前，针对无人表面船在轨迹追踪中遇到的船舶避碰问题，虽然已经出现很多算法，但大多算法在工程中很难实现，因此算法的实用性和可达性受到了越来越多的关注。

现有的文献中大都是习惯将避碰轨迹规划和轨迹追踪控制分为两部分研究，即以全局或局部路径规划为追踪参数，结合适当的控制算法设计控制率，实现轨迹追踪和导航，但由于其控制参数与规划状态之间的弱耦合性和相关性，特别是对于实时避碰场景，增加了目标复杂度，降低了控制的可达性，虽然也有一些关于避碰因素与控制算法的融合方案，然而仅仅是通过设置固定的避碰区域范围来进行设计，缺乏具体的避碰趋势规划。因此，针对轨迹追踪中避碰规划设计与控制算法执行时相互不协调和控制算法可达性的问题，本发明提出了场论规划与模型预测控制算法的融合框架，同时兼顾轨迹平滑性和避碰约束的弹性化，以获得复杂环境下可实现的无碰撞追踪轨迹，实现控制效率和路径损失间的平衡，满足算法的工程应用实现和控制可达性。

发明内容

本发明通过设计基于电场模型的快速行进方法，获得避碰规划的方向趋势，并结合弹性距离，重构模型预测控制方法的算法结构和约束系统，在船舶参数和避碰约束下，生成最优轨迹追踪控制器，此融合框架增强轨迹规划与控制行为间的耦合关系，减少船舶操纵参数的冗余影响，协调参数的状态变化，简化避碰规划的复杂性，实现多船避碰环境下的船舶自主轨迹追踪控制。

为了达到上述目的，本发明通过以下技术方案实现：

本发明公开了一种用于无人表面船的自主轨迹追踪融合控制方法，该方法具体包括：

1)船舶操纵运动模型的建立

根据系统形式，得到模型：

其中，s＝[u,v,r]^T,

τ＝[τ_u,0,τ_r]^T，u,v,r分别为纵向、横向和艏摇速度，x和y分别为船舶的横向、纵向的位置，

是航向角，τ_u是纵向推力，τ_r艏摇力矩，m₁₁,m₂₂,m₃₃为惯性分量，d₁₁,d₂₂,d₃₃为阻尼分量。

上述模型描述了船舶追踪控制的操纵模型，通过对输入量τ的控制，得到对轨迹速度和位置的调整情况，当遇到其他障碍船舶时需将避碰规划得到的调整反映到轨迹控制中。

2)轨迹追踪融合控制框架设计

将上述系统模型离散化，另x＝[s,η]^T，u＝τ,则x(k+1)＝f_d(x(k),u(k))，基于模型预测控制和场模型避碰规划融合的轨迹优化目标函数如下：

表示k时刻状态的第i步预估值，x_r(k+i)表示k时刻的预期追踪状态的第i步预测值。类似地，

是k时刻控制输入的第i步预估值，u_r(k+i)是k时刻预期控制输入的第i步预测值，xta(k+i,j|k)和yta(k+i,j|k)是障碍船舶位置的第i步预测值，N是预测步长，α为常数参量，表示松弛变量，将扩展目标函数的求解空间，减少轨迹追踪中的路径冗余。D是避碰安全距离，用弹性距离来描述，随着船舶间状态的不同而变化，其表达式是：

D₀是距离常数，B是相对方位角，表示本船船速与本船到障碍船间距离的夹角。

总体融合模型优化框架如下：

其中，T为取样时间，

和

分别对应船舶在k时刻的预估状态x、y、u、v，M为障碍船总数，θ是利用基于电场模型的快速匹配算法计算出来的避碰轨迹方向趋势。

在上述优化规划中，利用了θ方向趋势和D距离变化来构建避碰约束，表示了避碰规划路径中方向趋势的融合，且通过弹性距离来限定轨迹位置调整，形成一个融合了避碰动态规划趋势的控制器设计方案。

本发明与现有技术相比具有以下优点：

本发明通过避碰和轨迹追踪控制的融合架构设计，得到优化的轨迹追踪控制器，控制船舶运动朝预期的轨迹航行，从设有多艘障碍船舶的不同场景仿真验证中，得到最终轨迹追踪的轨迹变化状态，以及输入的推力变化情况，和本船的纵向速度、横向速度、艏摇速度、航向的状态变化趋势。与没有避碰场模型规划的模型预测控制算法相对比，从仿真结果中可以得出控制效果，本技术方案有更少的路径冗余和调整幅度，可快速地躲避障碍船舶追踪上预期轨迹。在实时避碰调整的同时实现轨迹追踪目标，兼顾效率和精度，获得平滑且冗余较少的无碰撞轨迹，其具有满足算法工程应用的参数要求和符合实际中不确定因素影响的特点，在多船同时交会的复杂环境下表现出良好的性能。

附图说明

为了更清楚地说明本发明专利实施例的技术方案，下面将对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明专利的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例中船舶模型结构；

图2为本发明一实施例中避碰态势下的船间结构；

图3本发明一实施例中自主轨迹追踪融合控制方法的整体结构图；

图4本发明一实施例中整体算法实施方案流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。需要说明的是，附图采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂，请参阅附图。须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

本发明公开了一种用于无人表面船的自主轨迹追踪融合控制方法，该方法具体包括：

1)船舶操纵运动模型的建立

根据系统形式，得到模型：

其中，s＝[u,v,r]^T,

τ＝[τ_u,0,τ_r]^T，u,v,r分别为纵向、横向和艏摇速度，x和y分别为船舶的横向、纵向的位置，

是航向角，τ_u是纵向推力，τ_r艏摇力矩，m₁₁,m₂₂,m₃₃为惯性分量，d₁₁,d₂₂,d₃₃为阻尼分量。

上述模型描述了船舶追踪控制的操纵模型，通过对输入量τ的控制，得到对轨迹速度和位置的调整情况，当遇到其他障碍船舶时需将避碰规划得到的调整反映到轨迹控制中。

2)轨迹追踪融合控制框架设计

将上述系统模型离散化，另x＝[s,η]^T，u＝τ,则x(k+1)＝f_d(x(k),u(k))，基于模型预测控制和场模型避碰规划融合的轨迹优化目标函数如下：

表示k时刻状态的第i步预估值，x_r(k+i)表示k时刻的预期追踪状态的第i步预测值。类似地，

是k时刻控制输入的第i步预估值，u_r(k+i)是k时刻预期控制输入的第i步预测值，xta(k+i,j|k)和yta(k+i,j|k)是障碍船舶位置的第i步预测值，N是预测步长，α为常数参量，表示松弛变量，将扩展目标函数的求解空间，减少轨迹追踪中的路径冗余。D是避碰安全距离，用弹性距离来描述，随着船舶间状态的不同而变化，其表达式是：

D₀是距离常数，B是相对方位角，表示本船船速与本船到障碍船间距离的夹角。

总体融合模型优化框架如下：

其中，T为取样时间，

和

分别对应船舶在k时刻的预估状态x、y、u、v，M为障碍船总数，θ是利用基于虚拟电场模型的快速匹配算法计算出来的避碰轨迹的方向趋势。

在上述优化规划中，利用了θ方向趋势和D距离变化来构建避碰约束，表示了避碰规划路径中方向趋势的融合，且通过弹性距离来限制轨迹位置调整，形成一个融合了避碰动态规划趋势的控制器设计方案。

为了便于理解上述实施例，下面将以上述实施例的一个具体应用场景中的控制器设计过程为例进行说明：

1)融合控制方法的避碰约束条件设计

利用

构建船舶运动状态的虚拟电场模型，结合快速匹配算法，得到避碰规划的轨迹调整状况，进而得到变化角度θ，构建出避碰约束表达式

其中，

2)融合控制方法的整体目标函数设计

在原来模型预测控制的目标函数中

引入松弛变量因素，则整体目标函数如下：

3)融合控制方法进行轨迹追踪控制

将上述的目标函数和避碰约束合成形成非线性优化问题，利用CasADi工具进行快速求解最优控制器，进而实现预期轨迹追踪。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (6)

1.一种用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，包含：

建立船舶操纵运动模型；

设计轨迹追踪融合控制方案中的避碰约束；

设计轨迹追踪融合控制方案中的目标函数；

船舶操纵运动模型的建立，根据系统形式，得到模型：

其中，s＝[u，v，r]^T，

τ＝[τ_u，0，τ_r]^T，u，v，r分别为纵向、横向和艏摇速度，x和y分别为船舶的横向、纵向的位置，

是航向角，t_u是纵向推力，τ_r艏摇力矩，m₁₁，m₂₂，m₃₃为惯性分量，d₁₁，d₂₂，d₃₃为阻尼分量；

融合控制方法的避碰约束条件设计，利用

构建船舶运动状态的虚拟电场模型，结合快速匹配算法，得到避碰规划的轨迹调整状况，进而得到变化角度θ，构建出避碰约束表达式；

i＝0，1，...N-1，j＝1，...M

其中，

其中，E为虚拟电场模型的场能，Q为船舶质量，k₀为常量参数，r为周边点到船舶中心点的距离，T为取样时间，

和

分别对应船舶在k时刻的预估状态x、y、u、v，M为障碍船总数，N是预测步长，θ是利用基于虚拟电场模型的快速匹配算法求解出来的避碰轨迹的方向趋势，D是避碰安全距离；

融合控制方法的整体目标函数设计，在原来模型预测控制的目标函数中

引入松弛变量因素，则整体目标函数如下：

i＝0，1，...N-1，j＝1，...M；

其中，

表示k时刻状态的第i步预估值，x_r(k+i)表示k时刻的预期追踪状态的第i步预测值；类似地，

是k时刻控制输入的第i步预估值，u_r(k+i)是k时刻预期控制输入的第i步预测值，xta(k+i，j|k)和yta(k+i，j|k)是障碍船舶位置的第i步预测值，α为常数参量，表示松弛变量。

2.如权利要求1所述的用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，设计轨迹追踪融合控制方案中的避碰约束，具体包括：

利用虚拟电场模型构建船舶航行场景；

结合快速匹配算法进行避碰轨迹规划，得出避碰规划的方向趋势；

根据方向趋势和弹性安全距离构建避碰约束。

3.如权利要求1所述的用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，设计轨迹追踪融合控制方案中的目标函数，具体包括：

给出模型预测控制算法的目标函数模型；

引入松弛变量，形成新的目标函数模型。

4.如权利要求1所述的用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，建立船舶操纵运动模型，通过如下公式进行计算：

其中，s＝[u，v，r]^T，

τ＝[τ_u，0，τ_r]^T，u，v，r分别为纵向、横向和艏摇速度，x和y分别为船舶的横向、纵向的位置，

是航向角，τ_u是纵向推力，τ_r艏摇力矩，m₁₁，m₂₂，m₃₃为惯性分量，d₁₁，d₂₂，d₃₃为阻尼分量。

5.如权利要求2所述的用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，根据方向趋势和弹性安全距离构建避碰约束，通过如下公式进行计算：

j＝1，...M，i＝0，1，……N-1

其中，T为取样时间，

和

分别对应船舶在k时刻的预估状态x、y、u、v，M为障碍船总数，N是预测步长，θ是利用基于虚拟电场模型的快速匹配算法求解出来的避碰轨迹的方向趋势，D是避碰安全距离。

6.如权利要求3所述的用于无人表面船的自主轨迹追踪融合控制方法，其特征在于，引入松弛变量，形成新的目标函数模型，具体通过如下公式进行计算：

j＝1，...M，i＝0，1，……N-1

其中，

表示k时刻状态的第i步预估值，x_r(k+i)表示k时刻的预期追踪状态的第i步预测值；类似地，

是k时刻控制输入的第i步预估值，u_r(k+i)是k时刻预期控制输入的第i步预测值，xta(k+i，j|k)和yta(k+i，j|k)是障碍船舶位置的第i步预测值，α为常数参量，表示松弛变量。

Images