一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统
1.本发明涉及无人船运动控制技术领域，具体而言，尤其涉及一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统。
背景技术：
2.21世纪是海洋的世纪，世界各国围绕海洋权益的争夺日益升温，开发和利用海洋资源成为当今热潮。无人船作为常见的自主航行器，由于其自身的小型化、智能化和具备多用途海洋运载平台被广泛用来执行具有危险性和重复性特点的工作。实际的海洋环境十分复杂，存在如礁石、沉船以及正在航行的船只等多种静态和动态障碍物，给无人船的安全航行带来巨大挑战。为降低无人船对岸端操作人员的依赖性，无人船应具备自主决策和智能避障的能力，无人船需根据外部传感器感知环境信息，实时自主地完成避障过程。
3.目前，在无人船在运动控制领域中，国内外学者已经对避障问题进行了相关研究，并提出了许多避障算法，如速度障碍法、人工势场法、基于强化学习避碰避障算法和基于向量场的避障避碰算法等。其中速度障碍法通过分析自身位置和障碍物之间的几何关系，计算避障所需的速度和航向，但当速度受限的情况下时，不能保证避障后的路径最优，降低任务效率。基于人工势场法的避障系统若已知障碍物分布，可先获取人工势场，即可计算出最优路径，当未知障碍物的分布时，可根据自身装载传感器检测周围环境信息实时构造周围势场，但获取局部最优路径时可能陷入局部极小值，需进行改进。而强化学习是将动态规划与监督学习相结合的一种新型学习方法，但当运行场景发生变化时，往往需要重新对模型进行训练，代价相对较高。
4.基于向量场避碰避障算法是一种通过排斥向量场来定义几乎全局反馈的避障算法，根据反馈控制律，迫使系统沿矢量场流动收敛至目标路径。该算法不需要定义碰撞避免目标的类李雅普诺夫函数或势能函数，并且不会出现远离目标点的情况。总体而言，现有的无人船运动控制依然存在如下问题：
5.一、现有的基于路径跟踪视距制导的方法中，一般不考虑避碰问题，只考虑无人船跟踪指定路径，但如果在实际航行中遇到的未知障碍物时，不能保证无人船的安全航行。
6.二、现有的基于向量场规避障碍物的方法中，多数采用混合场来进行规避障碍物，即吸引场制导与排斥场避障相结合，但设计过程较为复杂，需根据不同运动场景设计不同向量场。
7.三、现有的基于需要参数调试的规避障碍物的方法中，如强化学习算法，其避碰参数需要依赖奖励函数通过不断的数据训练获得，更换环境后需要重新进行训练，不能保证路径最优而表现出局限性。
技术实现要素：
8.根据上述提出的技术问题，提供一种规避动静态障碍物的无人船路径跟踪视距制
导方法及系统。本发明将视距制导与基于向量场避碰避障方法中排斥场理论相结合，提出一种规避动静态障碍物的无人船路径跟踪视距制导方法，通过视距制导产生的制导向量和排斥场产生的排斥向量相结合，得到用于控制欠驱动无人船的安全制导角，保证了欠驱动无人船在路径跟踪过程中规避动静态障碍物以保证航行的安全性。
9.本发明采用的技术手段如下：
10.一种规避动静态障碍物的无人船路径跟踪视距制导方法，包括：
11.构建欠驱动无人船运动学模型；
12.根据构建的欠驱动无人船运动学模型，计算得到视距制导角；
13.根据得到的视距制导角，计算得到向量场引导角；
14.根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；
15.根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。
16.进一步地，所述构建欠驱动无人船运动学模型，包括：
17.将无人船的运动学模型描述为：
[0018][0019]
其中，x，y，ψ分别表示无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角信息，u，v，r分别表示无人船在船体坐标系下的纵荡速度、横漂速度以及艏摇角速度；
[0020]
在欠驱动无人船运动规划中，假设横漂速度v为0，将无人船的运动学模型进一步表示为：
[0021][0022]
进一步地，所述根据构建的欠驱动无人船运动学模型，计算得到视距制导角，包括：
[0023]
根据当前无人船的位置信息和航向信息x，y，ψ，以及需要跟踪的路径点信息，将两个相邻路径点表示为：
[0024]
p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，pi＝[xi，yi]
t
[0025]
其中，下角标i表示路径点位次，pi表示第i个路径点的位置信息，xi，yi分别表示目标点在地球坐标系下的x轴、y轴上的位置；
[0026]
定义路径切向角ψd，将ψd表示为：
[0027]
ψd＝atan2(y
(i-1)-yi，x
(i-1)-xi)
[0028]
将无人船在航行过程中的位置跟踪误差表示为：
[0029][0030]
其中，s和e分别表示为路径坐标系下的横向跟踪误差和纵向跟踪误差，r(ψd)为旋转矩阵，表示为：
[0031][0032]
设计视距制导的引导角：
[0033][0034]
其中，δ为一个正的参数；
[0035]
将设计的视距制导的引导角表示为一个向量v
los
，如下所示：
[0036]vlos
＝[cosψ
los
，sinψ
los
]
t
。
[0037]
进一步地，所述根据得到的视距制导角，计算得到向量场引导角，包括：
[0038]
基于欠驱动无人艇当前的位置和航向信息x，y，ψ，和障碍物位置信息po以及路径点信息，根据向量场定义，引入向量场f(pe)：
[0039][0040]
其中，κ表示一个参数，α＝[α
x
，αy]
t
为确定向量场指向的向量，pe＝p-po，p＝[x，y]
t
为当前无人船位置信息，po＝[xo，yo]
t
为障碍物位置信息；
[0041]
定义障碍物周围排斥场，根据引入的向量场f(pe)的公式，得到向量场分量的解析形式：
[0042][0043]
其中，[α
x
，αy]
t
＝[cos α,sin α]
t
，α＝atan2(y
o-yi，x
o-xi)，κ设计为：
[0044][0045]
其中，φ＝atan2(y
i-yo，x
i-xo)+π；
[0046]
计算向量场引导矢量角ψ
gvf
，如下：
[0047][0048]
其中，||
·
||表示向量的范数；
[0049]
将排斥场引导角表示为向量fo，如下：
[0050]fo
＝[cosψ
gvf
，sinψ
gvf
]
t
。
[0051]
进一步地，所述根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角，包括：
[0052]
根据视距制导角v
los
和排斥场引导角fo，为了实现安全路径跟踪任务，设计控制律，如下：
[0053]fg
＝(1-λ)fo+λv
los
＝[f
gx
，f
gy
]
t
[0054]
其中，λ是一个平滑过渡函数，它在欠驱动无人船远离障碍物时值取为1，当欠驱动无人船接近障碍物时减小，直到在障碍物的边界时取值为0；λ设计如下：
[0055][0056]
其中，ro为障碍物半径，r
max
，r
min
为用户选择的碰撞避免参数；
[0057]
为满足连续性，设计系数如下：
[0058][0059][0060]
设计合成向量最终制导角ψg，如下：
[0061]
ψg＝atan2(f
gy
，f
gx
)。
[0062]
进一步地，所述根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制，包括：
[0063]
基于无人船实际航向信息ψ和向量最终制导角ψg，考虑无人船行驶过程中假设纵荡速度u自由变化，设计角速度控制器为：
[0064]
rg＝-k tanh(ψ-ψg)
[0065]
其中，k为正的参数，tanh(
·
)为饱和函数。
[0066]
本发明还提供了一种基于上述规避动静态障碍物的无人船路径跟踪视距制导方法的无人船路径跟踪视距制导系统，包括：
[0067]
欠驱动无人船运动学模型构建模块，用于构建欠驱动无人船运动学模型；
[0068]
视距制导模块，用于根据构建的欠驱动无人船运动学模型，计算得到视距制导角；
[0069]
排斥向量场模块，用于根据得到的视距制导角，计算得到向量场引导角；
[0070]
向量合成模块，用于根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；
[0071]
运动学制导模块，用于根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。
[0072]
进一步地，所述视距制导模块、排斥向量场模块、向量合成模块以及运动学制导模块的连接关系如下：
[0073]
所述视距制导模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及外部输入即当前需要跟踪的路径点p
(i-1)
，pi相连，所述视距制导模块的输出端与向量合成模块的输入端相连；
[0074]
所述排斥向量场模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及障碍物位置信息po相连，所述排斥向量场模块的输出端与向量合成模块的输入端相连；
[0075]
所述向量合成模块的输入端与所述视距制导模块的输出端信号视距引导角向量vlos
和所述排斥向量场模块的输出端信号排斥场引导向量fo相连，所述向量合成模块的输出端与所述运动学制导模块的输入端相连；
[0076]
所述运动学制导模块的输入端与所述合成向量模块的输出信号最终制导角ψg和无人船实际航向信息ψ相连，所述运动学制导模块的输出端信号制导角速度rg与欠驱动无人船相连。
[0077]
较现有技术相比，本发明具有以下优点：
[0078]
1、本发明提供的规避动静态障碍物的无人船路径跟踪视距制导方法，与现有的基于路径跟踪视距制导的方法相比，本发明在路径跟踪的基础上，考虑了欠驱动无人船航行过程中出现静态障碍物和动态障碍物情况，保证了欠驱动无人船航行过程中的安全性。
[0079]
2、本发明提供的规避动静态障碍物的无人船路径跟踪视距制导方法，与现有基于向量场规避障碍物的方法相比，本发明采用视距制导和向量场引导相结合的方式，根据欠驱动无人艇与障碍物之间的距离获取权重分配，有效地解决了规避算法设计的复杂问题。
[0080]
3、本发明提供的规避动静态障碍物的无人船路径跟踪视距制导方法，与现有需要参数调试的规避障碍物的方法相比，本发明仅需提供障碍物大小和避碰半径等外部信息，不需要进行复杂的参数调节过程，避免了算法应用过程中调整参数困难的问题。
[0081]
基于上述理由本发明可在无人船运动控制等领域广泛推广。
附图说明
[0082]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0083]
图1为本发明规避动静态障碍物的无人船路径跟踪视距制导系统结构示意图。
[0084]
图2为本发明实施例提供的障碍物周围排斥场示意图。
[0085]
图3为本发明实施例提供的欠驱动无人船轨迹图。
[0086]
图4为本发明实施例提供的路径跟踪过程中欠驱动无人船与障碍物之间的距离变化曲线。
[0087]
图5为本发明实施例提供的路径跟踪过程中欠驱动无人船的横向跟踪误差曲线。
[0088]
图6为本发明实施例提供的路径跟踪过程中欠驱动无人船的纵向跟踪误差曲线。
具体实施方式
[0089]
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
[0090]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0091]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0092]
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当清楚，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员己知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任向具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
[0093]
在本发明的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、“横向、竖向、垂直、水平”和“顶、底”等所指示的方位或位置关系通常是基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，在未作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有特定的方位或者以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制：方位词“内、外”是指相对于各部件本身的轮廓的内外。
[0094]
为了便于描述，在这里可以使用空间相对术语，如“在
……
之上”、“在
……
上方”、“在
……
上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其位器件或构造之下”。因而，示例性术语“在
……
上方”可以包括“在
……
上方”和“在
……
下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。
[0095]
此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。
[0096]
本发明提供了一种规避动静态障碍物的无人船路径跟踪视距制导方法，包括：
[0097]
s1、构建欠驱动无人船运动学模型；
[0098]
s2、根据构建的欠驱动无人船运动学模型，计算得到视距制导角；
[0099]
s3、根据得到的视距制导角，计算得到向量场引导角；
[0100]
s4、根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；
[0101]
s5、根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。
[0102]
具体实施时，作为本发明优选的实施方式，所述步骤s1具体包括：
[0103]
s11、将无人船的运动学模型描述为：
[0104][0105]
其中，x，y，ψ分别表示无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角信息，u，v，r分别表示无人船在船体坐标系下的纵荡速度、横漂速度以及艏摇角速度；
[0106]
s12、在欠驱动无人船运动规划中，假设横漂速度v为0，将无人船的运动学模型进一步表示为：
[0107][0108]
具体实施时，作为本发明优选的实施方式，所述步骤s2具体包括：
[0109]
s21、根据当前无人船的位置信息和航向信息x，y，ψ，以及需要跟踪的路径点信息，将两个相邻路径点表示为：
[0110]
p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，pi＝[xi，yi]
t
[0111]
其中，下角标i表示路径点位次，pi表示第i个路径点的位置信息，xi，yi分别表示目标点在地球坐标系下的x轴、y轴上的位置；
[0112]
s22、定义路径切向角ψd，将ψd表示为：
[0113]
ψd＝atan2(y
(i-1)-yi，x
(i-1)-xi)
[0114]
s23、将无人船在航行过程中的位置跟踪误差表示为：
[0115][0116]
其中，s和e分别表示为路径坐标系下的横向跟踪误差和纵向跟踪误差，r(ψd)为旋转矩阵，表示为：
[0117][0118]
s24、设计视距制导的引导角：
[0119][0120]
其中，δ为一个正的参数，在本实施例中，前视距离参数设计为δ＝7。
[0121]
s25、将设计的视距制导的引导角表示为一个向量v
los
，如下所示：
[0122]vlos
＝[cosψ
los
，sinψ
los
]
t
。
[0123]
具体实施时，作为本发明优选的实施方式，所述步骤s3具体包括：
[0124]
s31、基于欠驱动无人艇当前的位置和航向信息x，y，ψ，和障碍物位置信息po以及路径点信息，根据向量场定义，引入向量场f(pe)：
[0125][0126]
其中，κ表示一个参数，α＝[α
x
，αy]
t
为确定向量场指向的向量，pe＝p-po，p＝[x，y]
t
为当前无人船位置信息，po＝[xo，yo]
t
为障碍物位置信息；
[0127]
s32、定义障碍物周围排斥场，根据引入的向量场f(pe)的公式，得到向量场分量的解析形式：
[0128][0129]
其中，[α
x
，αy]
t
＝[cosα，sinα]
t
，α＝atan2(y
o-yi，x
o-xi)，κ设计为：
[0130][0131]
其中，φ＝atan2(y
i-yo，x
i-xo)+π；
[0132]
s33、计算向量场引导矢量角ψ
gvf
，如下：
[0133][0134]
其中，||
·
||表示向量的范数；
[0135]
s34、将排斥场引导角表示为向量fo，如下：
[0136]fo
＝[cosψ
gvf
，sinψ
gvf
]
t
。
[0137]
具体实施时，作为本发明优选的实施方式，所述步骤s4具体包括：
[0138]
s41、根据视距制导角v
los
和排斥场引导角fo，为了实现安全路径跟踪任务，设计控制律，如下：
[0139]fg
＝(1-λ)fo+λv
los
＝[f
gx
，f
gy
]
t
[0140]
其中，λ是一个平滑过渡函数，它在欠驱动无人船远离障碍物时值取为1，当欠驱动无人船接近障碍物时减小，直到在障碍物的边界时取值为0；λ设计如下：
[0141][0142]
其中，ro为障碍物半径，r
max
，r
min
为用户选择的碰撞避免参数；在本实施例中，为实现不同避碰效果所引入参数：图中ro为用户设定障碍物半径，一般大于真实具体障碍物，为验证算法的有效性，设计静态障碍物ro＝10，动态障碍物ro＝5，r
max
，r
min
为用户选择的碰撞避免参数。
[0143]
s42、为满足连续性，设计系数如下：
[0144][0145]
[0146]
s43、设计合成向量最终制导角ψg，如下：
[0147]
ψg＝atan2(f
gy
，f
gx
)。
[0148]
具体实施时，作为本发明优选的实施方式，所述步骤s5具体包括：
[0149]
基于无人船实际航向信息ψ和向量最终制导角ψg，考虑无人船行驶过程中假设纵荡速度u自由变化，设计角速度控制器为：
[0150]
rg＝-ktanh(ψ-ψg)
[0151]
其中，k为正的参数，tanh(
·
)为饱和函数。在本实施例中，为实现角速度控制所引入的控制参数：k＝0.5。
[0152]
本发明实施例还提供了一种基于上述规避动静态障碍物的无人船路径跟踪视距制导方法的无人船路径跟踪视距制导系统，包括：
[0153]
欠驱动无人船运动学模型构建模块，用于构建欠驱动无人船运动学模型；
[0154]
视距制导模块，用于根据构建的欠驱动无人船运动学模型，计算得到视距制导角；
[0155]
排斥向量场模块，用于根据得到的视距制导角，计算得到向量场引导角；
[0156]
向量合成模块，用于根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；
[0157]
运动学制导模块，用于根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。
[0158]
具体实施时，作为本发明优选的实施方式，所述视距制导模块、排斥向量场模块、向量合成模块以及运动学制导模块的连接关系如下：
[0159]
所述视距制导模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及外部输入即当前需要跟踪的路径点p
(i-1)
，pi相连，所述视距制导模块的输出端与向量合成模块的输入端相连；
[0160]
所述排斥向量场模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及障碍物位置信息po相连，所述排斥向量场模块的输出端与向量合成模块的输入端相连；
[0161]
所述向量合成模块的输入端与所述视距制导模块的输出端信号视距引导角向量v
los
和所述排斥向量场模块的输出端信号排斥场引导向量fo相连，所述向量合成模块的输出端与所述运动学制导模块的输入端相连；
[0162]
所述运动学制导模块的输入端与所述合成向量模块的输出信号最终制导角ψg和无人船实际航向信息ψ相连，所述运动学制导模块的输出端信号制导角速度rg与欠驱动无人船相连。
[0163]
如图1所示，根据欠驱动无人船运动学模型，在运动学层次，计算得到视距制导角，进一步将视距制导角表示为一个向量。根据向量场分量的解析形式定义障碍物周围排斥场，获得向量场引导角，并将获得的向量场引导角表示为向量形式。为了实现安全路径跟踪任务，根据权重λ取值得到角度制导律。考虑欠驱动无人船行驶过程中纵荡速度u自由变化，运动学控制只需设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。
[0164]
实施例
[0165]
为了验证本发明方法的有效性，进行了仿真实验，根据仿真结果可知，图3为欠驱动无人船路径跟踪过程中运动轨迹图，图中虚线表示无人船参考轨迹即需要跟踪的路径信息，实线表示无人船实际轨迹，图中大的圆形障碍物为静态障碍物，图中小的障碍物为动态
障碍物并最终移动到图右侧虚线圆位置，从图中可以看出欠驱动无人船跟踪上给定的参考路径，并绕开了环境中的静态障碍物和动态障碍物。图4为路径跟踪过程中欠驱动无人船与静态障碍物和动态障碍物之间的距离变化曲线，从图中可看出欠驱动无人船航行时与三个静态障碍物和一个动态障碍物之间的距离始终大于设定的最小安全距离。图5-6为欠驱动无人船跟踪参考路径时的横向跟踪误差和纵向跟踪误向图像，在规避障碍物时发生动态变化，并均在可接受范围内，符合控制要求。
[0166]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
技术特征：
1.一种规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，包括：构建欠驱动无人船运动学模型；根据构建的欠驱动无人船运动学模型，计算得到视距制导角；根据得到的视距制导角，计算得到向量场引导角；根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。2.根据权利要求1所述的规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，所述构建欠驱动无人船运动学模型，包括：将无人船的运动学模型描述为：其中，x，y，ψ分别表示无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角信息，u，v，r分别表示无人船在船体坐标系下的纵荡速度、横漂速度以及艏摇角速度；在欠驱动无人船运动规划中，假设横漂速度v为0，将无人船的运动学模型进一步表示为：3.根据权利要求1所述的规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，所述根据构建的欠驱动无人船运动学模型，计算得到视距制导角，包括：根据当前无人船的位置信息和航向信息x，y，ψ，以及需要跟踪的路径点信息，将两个相邻路径点表示为：p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，p
i
＝[x
i
，y
i
]
t
其中，下角标i表示路径点位次，p
i
表示第i个路径点的位置信息，x
i
，y
i
分别表示目标点在地球坐标系下的x轴、y轴上的位置；定义路径切向角ψ
d
，将ψ
d
表示为：ψ
d
＝atan2(y
(i-1)-y
i
，x
(i-1)-x
i
)将无人船在航行过程中的位置跟踪误差表示为：其中，s和e分别表示为路径坐标系下的横向跟踪误差和纵向跟踪误差，r(ψ
d
)为旋转矩阵，表示为：设计视距制导的引导角：
其中，δ为一个正的参数；将设计的视距制导的引导角表示为一个向量v
los
，如下所示：v
los
＝[cosψ
los
，sinψ
los
]
t
。4.根据权利要求1所述的规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，所述根据得到的视距制导角，计算得到向量场引导角，包括：基于欠驱动无人艇当前的位置和航向信息x，y，ψ，和障碍物位置信息p
o
以及路径点信息，根据向量场定义，引入向量场f(p
e
)：其中，κ表示一个参数，α＝[α
x
，α
y
]
t
为确定向量场指向的向量，p
e
＝p-p
o
，p＝[x，y]
t
为当前无人船位置信息，p
o
＝[x
o
，y
o
]
t
为障碍物位置信息；定义障碍物周围排斥场，根据引入的向量场f(p
e
)的公式，得到向量场分量的解析形式：其中，[α
x
，α
y
]
t
＝[cosα，sinα]
t
，α＝atan2(y
o-y
i
，x
o-x
i
)，k设计为：其中，φ＝atan2(y
i-y
o
，x
i-x
o
)+π；计算向量场引导矢量角ψ
gvf
，如下：其中，||
·
||表示向量的范数；将排斥场引导角表示为向量f
o
，如下：f
o
＝[cosψ
gvf
，sinψ
gvf
]
t
。5.根据权利要求1所述的规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，所述根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角，包括：根据视距制导角v
los
和排斥场引导角f
o
，为了实现安全路径跟踪任务，设计控制律，如下：f
g
＝(1-λ)f
o
+λv
los
＝[f
gx
，f
gy
]
t
其中，λ是一个平滑过渡函数，它在欠驱动无人船远离障碍物时值取为1，当欠驱动无人船接近障碍物时减小，直到在障碍物的边界时取值为0；λ设计如下：
其中，r
o
为障碍物半径，r
max
，r
min
为用户选择的碰撞避免参数；为满足连续性，设计系数如下：为满足连续性，设计系数如下：设计合成向量最终制导角ψ
g
，如下：ψ
g
＝atan2(f
gy
，f
gx
)。6.根据权利要求1所述的规避动静态障碍物的无人船路径跟踪视距制导方法，其特征在于，所述根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制，包括：基于无人船实际航向信息ψ和向量最终制导角ψ
g
，考虑无人船行驶过程中假设纵荡速度u自由变化，设计角速度控制器为：r
g
＝-k tanh(ψ-ψ
g
)其中，k为正的参数，tanh(
·
)为饱和函数。7.一种基于权利要求1-6中任意一项权利要求所述规避动静态障碍物的无人船路径跟踪视距制导方法的无人船路径跟踪视距制导系统，其特征在于，包括：欠驱动无人船运动学模型构建模块，用于构建欠驱动无人船运动学模型；视距制导模块，用于根据构建的欠驱动无人船运动学模型，计算得到视距制导角；排斥向量场模块，用于根据得到的视距制导角，计算得到向量场引导角；向量合成模块，用于根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；运动学制导模块，用于根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。8.根据权利要求7所述的规避动静态障碍物的无人船路径跟踪视距制导系统，其特征在于，所述视距制导模块、排斥向量场模块、向量合成模块以及运动学制导模块的连接关系如下：所述视距制导模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及外部输入即当前需要跟踪的路径点p
(i-1)
，p
i
相连，所述视距制导模块的输出端与向量合成模块的输入端相连；所述排斥向量场模块的输入端与欠驱动无人船输出的位置信息x，y、航向信息ψ以及障碍物位置信息p
o
相连，所述排斥向量场模块的输出端与向量合成模块的输入端相连；所述向量合成模块的输入端与所述视距制导模块的输出端信号视距引导角向量v
los
和所述排斥向量场模块的输出端信号排斥场引导向量f
o
相连，所述向量合成模块的输出端与所述运动学制导模块的输入端相连；
所述运动学制导模块的输入端与所述合成向量模块的输出信号最终制导角ψ
g
和无人船实际航向信息ψ相连，所述运动学制导模块的输出端信号制导角速度r
g
与欠驱动无人船相连。
技术总结
本发明提供一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统，属于无人船运动控制技术领域，本发明方法，包括：构建欠驱动无人船运动学模型；根据构建的欠驱动无人船运动学模型，计算得到视距制导角；根据得到的视距制导角，计算得到向量场引导角；根据得到的视距制导角和向量场引导角，计算得到合成向量最终制导角；根据得到的合成向量最终制导角和无人船实际航向信息，设计角速度制导律，实现欠驱动无人船运动学闭环安全控制。本发明通过视距制导产生的制导向量和排斥场产生的排斥向量相结合，得到用于控制欠驱动无人船的安全制导角，保证了欠驱动无人船在路径跟踪过程中规避动静态障碍物以保证航行的安全性。规避动静态障碍物以保证航行的安全性。规避动静态障碍物以保证航行的安全性。
