一种无人船向量场路径跟踪控制器及设计方法

本发明涉及无人船控制领域，尤其涉及一种无人船向量场路径跟踪控制器及设计方法。

背景技术：

因海洋环境复杂多变且恶劣，海风与洋流等因素对船舶运动影响巨大，无人船的运动控制是无人船舶应用的一个难题，如何获得高效且可靠的无人船的路径跟踪控制器是科研人员致力于解决的问题。从控制器设计方面来看，现有无人船路径控制方法存在以下不足：

第一，在船舶路径跟踪的现有控制方法中未考虑无人船暂态性能，无法在无人船从一个稳定态到另一个稳定态的运动期间进行有效的船舶运动控制，导致跟踪路线不够光滑，在无人船路径跟踪控制中容易产生较大的超调和暂态振荡，致使无人船运动控制效率低下。

第二，在无人船路径跟踪现有的控制方法中如视距制导方法，其在控制无人船路径跟踪中容易产生较大的纵向跟踪误差，导致无人船实际航迹不能有效的跟踪到参考航迹，并且控制无人船跟踪时给定航迹时控制输入的不合理，造成了能源的浪费。

技术实现要素：

本发明提供一种无人船向量场路径跟踪控制器及设计方法，以克服上述技术问题。

本发明一种无人船向量场路径跟踪控制器设计方法，包括：

建立无人船运动学模型；

根据设定的期望路径得到无人船期望路径的参数化信息；

根据设定的期望路径的参数化信息得到制导向量场信息；

根据所述无人船期望路径的给定位置信息与无人船的实际位置信息，得到无人船期望路径的位置与无人船的实际位置之间的路径误差；所述路径误差，包括：横向路径误差和纵向路径误差；

根据所述制导向量场信息与无人船的实际位置信息，得到无人船制导航向；

根据预先设定的期望速度及所述纵向路径误差，得到更新后的路径参数，并根据更新后的路径参数，得到更新后的所述无人船期望路径的位置信息；并根据所述更新后的所述无人船期望路径的位置信息，得到更新后的路径误差和无人船制导航向；

根据所述期望速度和更新后的纵向路径误差，得到无人船制导前向速度；

在向量场作用下，无人船通过所述更新后的无人船制导航向和无人船制导前向速度，沿期望路径运动。

进一步地，所述建立无人船运动学模型，包括：

所述无人船运动学模型，表示为：

式中，分别代表欠驱动无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角；分别代表欠驱动无人船在船体坐标系下的纵荡速度、横漂速度和艏摇角速度。

进一步地，所述根据设定的期望路径得到无人船期望路径的参数化信息，包括：

通过式(2)得到所述无人船期望路径的参数化信息pd，pd＝[xy]^t；

式中，x为欠驱动无人船所要跟踪的参考路径的横坐标，y为欠驱动无人船所要跟踪的参考路径的纵坐标；函数f1与函数f2为将参考路径参数化的函数；θ为路径参数。

进一步地，所述根据所述无人船期望路径的给定位置信息与无人船的实际位置信息得到无人船期望路径的位置与无人船的实际位置之间的路径误差，包括：

由式(3)得到无人船期望路径的位置的切线角度为：

得到旋转矩阵表示为：

无人船的实际位置表示为：

p＝[xy]^t(5)

根据无人船期望路径的位置与无人船的实际位置得到的路径误差的矩阵表示为：

e＝r^t(p-pd)(6)

式中，p为无人船的实际位置，e为路径误差矩阵；f′1(θ)为f1(θ)对θ的偏导数，f′2(θ)为f2(θ)对θ的偏导数，偏导数公式为与r^t为旋转矩阵r的转置；atan2为计算f′1(θ)+f′2(θ)i的辅角。

进一步地，所述根据所述制导向量场信息与无人船的实际位置信息，得到无人船制导航向，包括：

制导向量场表示为：

式中，f为二维向量场函数，φ1(x，y，θ)＝x-f1(θ)，φ2(x，y，θ)＝y-f2(θ)，k1，k2均为正常数，φ1、φ2是向量场里的参数信息，没有具体意义；

通过式(8)计算求得无人船制导航向ψd表示为：

ψd＝atan2(f(2)，f(1))(8)

式中，f(1)代表制导向量场的第一维，f(2)代表制导向量场的第二维。

进一步地，所述根据预先设定的期望速度及所述纵向路径误差，得到更新后的路径参数，包括：

通过式(9)计算路径参数θ的导数表示为：

式中，vs为预先设定的期望速度，k3为正的常数，tanh为双曲正切函数，e(1)为通过误差矩阵得到的第一维路径误差，即纵向路径误差。

进一步地，所述根据所述期望速度和更新后的纵向路径误差，得到无人船制导前向速度，包括：

通过式(10)计算无人船制导前向速度ud，表示为：

式中，k4为正的常数。

一种无人船向量场路径跟踪控制器，用于在向量场作用下，得到无人船制导航向和制导前向速度，并发送至无人船，使得无人船沿期望路径运动，包括：参考路径模块、路径误差模块、制导向量场模块、路径参数更新模块及制导前向速度模块；所述参考路径模块的输入端与所述路径参数更新模块相连，输出端分别与所述路径误差模块和制导向量场模块相连；所述路径误差模块的输入端与所述参考路径模块和无人船相连，输出端与所述路径参数更新模块和所述制导前向速度模块相连；所述制导向量场模块的输入端与所述参考路径模块和无人船相连，输出端与无人船相连；所述路径参数更新模块的输入端与路径误差模块的输出端相连，输出端与所述参考路径模块的输入端相连；所述制导前向速度模块的输入端与所述路径误差模块相连，输出端与无人船相连；所述参考路径模块，用于根据所述路径参数更新模块输入的路径参数，得到无人船期望路径的位置信息；所述路径误差模块，用于根据所述无人船期望路径的位置信息与无人船的实际位置信息，得到无人船期望路径的位置与无人船的实际位置之间的路径误差；所述路径误差，包括：横向路径误差和纵向路径误差；所述制导向量场模块，用于根据所述无人船期望路径的位置信息与无人船的实际位置信息，得到无人船制导航向；所述路径参数更新模块，用于根据预先设定的期望速度及所述纵向路径误差，得到更新后的路径参数并发送所述参考路径模块；所述制导前向速度模块，用于通过所述期望速度、纵向路径误差，得到无人船制导前向速度。

本发明采用向量场方法，在期望路径周围生成空间矢量。基于产生的向量场空间矢量方向，给出了无人船制导航向信号。根据无人船位置及路径参数点位置信息，得到了无人船制导前向速度信号。在向量场作用下，无人船能够光滑的沿着场线运动，最终收敛到期望路径。本发明考虑了无人船的暂态控制性能，使无人船路径跟踪路线更加光滑，减小了无人船运动中的风险，降低了执行机构的压力，能够有效的降低船舶执行机构的成本，让船舶的设计、生产更加容易。采用向量场的制导方法，可以得到更小的纵向跟踪误差收敛效果，提高了无人船路径跟踪性能。另外，在路径跟踪控制过程中所需的控制输入更合理，在无人船应用中可以更加节省能源。

附图说明

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

图1为一种无人船向量场路径跟踪控制器结构示意图；

图2为仿真试验中的欠驱动无人船路径跟踪图；

图3为仿真试验中的欠驱动无人船路径跟踪误差图；

图4为仿真试验中的路径跟踪向量场；

图5为仿真试验中的欠驱动无人船路径参数更新图；

图6为仿真试验中的欠驱动无人船航向跟踪效果图；

图7为仿真试验中的欠驱动无人船航速跟踪效果图；

图8为一种无人船向量场路径跟踪控制器设计方法流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图8所示，本实施例提供了一种无人船向量场路径跟踪控制器设计方法，包括：

101、建立无人船运动学模型；

具体而言，无人船运动学模型，表示为：

式中，分别代表欠驱动无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角；分别代表欠驱动无人船在船体坐标系下的纵荡速度、横漂速度和艏摇角速度。

102、根据设定的期望路径得到无人船期望路径的参数化信息；

具体而言，设定的参数如下：

k1＝1，k2＝1，vs＝1，k3＝10，k4＝5

初始路径参数为：

f1(θ)＝0.5θ，f2(θ)＝0.5θ

通过式(2)得到无人船期望路径的参数化信息pd，pd＝[xy]^t；

式中，x为欠驱动无人船所要跟踪的参考路径的横坐标，y为欠驱动无人船所要跟踪的参考路径的纵坐标；函数f1与函数f2为将参考路径参数化的函数；θ为路径参数。

103、根据设定的期望路径的参数化信息得到制导向量场信息；

具体而言，制导向量场表示为：

式中，f为二维向量场函数，φ1(x，y，θ)＝x-f1(θ)，φ2(x，y，θ)＝y-f2(θ)，k1，k2均为正常数，φ1、φ2是向量场里的参数信息，没有具体意义；

104、根据无人船期望路径的给定位置信息与无人船的实际位置信息，得到无人船期望路径的位置与无人船的实际位置之间的路径误差；路径误差，包括：横向路径误差和纵向路径误差；

具体而言，由式(3)得到无人船期望路径的位置的切线角度为：

得到旋转矩阵表示为：

无人船的实际位置表示为：

p＝[xy]^t(6)

根据无人船期望路径的位置与无人船的实际位置得到的路径误差的矩阵表示为：

e＝r^t(p-pd)(7)

式中，p为无人船的实际位置，e为路径误差矩阵；f′1(θ)为f1(θ)对θ的偏导数，f′2(θ)为f2(θ)对θ的偏导数，偏导数公式为与r^t为旋转矩阵r的转置；atan2为计算f′1(θ)+f′2(θ)i的辅角。路径误差的矩阵包括二维数据，即横向路径误差和纵向路径误差。

105、根据制导向量场信息与无人船的实际位置信息，得到无人船制导航向；

具体而言，通过式(8)计算求得无人船制导航向ψd表示为：

ψd＝atan2(f(2)，f(1))(8)

式中，f(1)代表制导向量场的第一维，f(2)代表制导向量场的第二维。

106、根据预先设定的期望速度及纵向路径误差，得到更新后的路径参数，并根据更新后的路径参数，得到更新后的无人船期望路径的位置信息；并根据更新后的无人船期望路径的位置信息，得到更新后的路径误差和无人船制导航向；

具体而言，通过式(9)计算路径参数θ的导数表示为：

式中，vs为预先设定的期望速度，k3为正的常数，tanh为双曲正切函数，e(1)为通过误差矩阵得到的第一维路径误差，即纵向路径误差。

得到路径参数的导数后，将更新后的路径参数θ代入式(2)中重新计算无人船给定路径的位置信息，然后再通过式(3)-(8)计算得到更新后的路径误差和无人船制导航向。

107、根据期望速度和更新后的纵向路径误差，得到无人船制导前向速度；

具体而言，通过式(10)计算无人船前向速度ud，表示为：

式中，k4为正的常数。

108、在向量场作用下，无人船通过更新后的无人船制导航向和无人船制导前向速度，沿期望路径运动。

如图1所示，本实施例提供了一种无人船向量场路径跟踪控制器，用于在向量场作用下，得到无人船制导航向和前向速度，并发送至无人船，使得无人船沿期望路径运动，包括：参考路径模块、路径误差模块、制导向量场模块、路径参数更新模块及制导前向速度模块；参考路径模块的输入端与路径参数更新模块相连，输出端分别与路径误差模块和制导向量场模块相连；路径误差模块的输入端与参考路径模块和无人船相连，输出端与路径参数更新模块和制导前向速度模块相连；制导向量场模块的输入端与参考路径模块和无人船相连，输出端与无人船相连；路径参数更新模块的输入端与误差模块的输出端相连，输出端与参考路径模块的输入端相连；制导前向速度模块的输入端与路径误差模块相连，输出端与无人船相连；参考路径模块，用于根据路径参数更新模块输入的路径参数，得到无人船期望路径的位置信息；路径误差模块，用于根据无人船期望路径的位置信息与无人船的实际位置信息，得到无人船期望路径的位置与无人船的实际位置之间的路径误差；路径误差，包括：横向路径误差和纵向路径误差；制导向量场模块，用于根据无人船期望路径的位置信息与无人船的实际位置信息，得到无人船制导航向；路径参数更新模块，用于根据预先设定的期望速度及纵向路径误差，得到更新后的路径参数并发送参考路径模块；制导前向速度模块，用于通过期望速度、纵向路径误差，得到无人船制导前向速度。

仿真试验：

仿真结果如图2-图7所示。图2是无人船在向量场路径跟踪中的运动轨迹，图2中的细虚线为给定的参考路径，粗实线为无人船的实际运动轨迹。从图2中可以看出无人船在一定距离以后跟踪上了给定的参数化路径，并在一定时间后有着较好的跟踪效果。图3是无人船在向量场路径跟踪中的路径跟踪误差图，图3中的粗实线为横坐标y的误差，细虚线为纵坐标x的误差，图3中可以看出跟踪误差快速收敛至零并保持，仅在船舶初始调整位置时跟踪误差会有一些波动。图4是无人船在向量场路径跟踪中的向量场图，图4中的粗虚线为虚拟参考路径，图4中的黑色箭头为向量场方向，图4中为虚拟参考点在(2.52.5)处时的向量场图。图5显示的是无人船在向量场路径跟踪中的路径参数更新图，图5中可以看出随着时间的路径参数θ更新平稳。图6是无人船在向量场路径跟踪中的航向跟踪效果图，图6中粗虚线为参考航向，细实线为实际航向，可以看出无人船舶的航向可以短时间内很好的跟踪到给定航向。图7是无人船在向量场路径跟踪中的航速跟踪效果图，图7中粗虚线为参考航速，细实线为实际航速，可以看出无人船舶的航速可以在短时间内很好的跟踪到给定航速。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。