一种高精度轨迹跟踪控制方法与流程

本发明涉及本发明属于船舶轨迹跟踪控制领域，具体地说是一种高精度航迹跟踪控制方法。

背景技术：

在船舶运动控制领域，船舶的高精度轨迹跟踪控制一直是重点研究的课题；船舶轨迹跟踪的核心是航向控制；视线法(los)导航算法作为一种经典的目标跟踪方法，是利用了点与点之间简单的几何关系判断可视性；它具有简单直观、参数整定容易、收敛性好的优点。该算法独立于动力学控制器，需要设计的参数少，期望航向的获取只与实际位置和给定轨迹有关，可以高效地获取期望航向角；但仅有los得到的航向角，只能使轨迹收敛于预设轨迹，而无法获得航行时需要的精确指令舵角，不能保证轨迹跟踪的精度。

对于高精度轨迹跟踪，其中最为关键的技术问题是航向控制。当前，控制技术和计算机技术发展迅速，但是在高精度轨迹跟踪方面缺乏相应的研究，在用先进控制方法做控制器实现高精度轨迹跟踪等方面缺乏相应的技术支撑和应用。

技术实现要素：

本发明的目的在于克服以上所述的缺点，提供一种高精度轨迹跟踪控制方法。

为实现上述目的，本发明的具体方案如下：包括如下步骤，

a1、确定船舶的位置信息及舵角信息；

a2、将位置信息及舵角信息赋值于视线瞄准导航算法，通过视线瞄准导航算法得出期望航向角；

a3、包括一gpc控制器，通过确定的船舶位置信息及舵角信息以及期望航向角传输至gpc控制器；

a4、gpc控制器持续跟踪期望航向角得出指令舵角，实现高精度轨迹跟踪控制。

所述视线瞄准导航算法包括：

以坐标原点为起点，指向点(x,y)的射线在坐标平面上与x轴正方向的夹角为los角；定义四象限反正切计算，该角角度为arctan(y,x)∈(-π,π)；地球坐标系下，假设船舶当前的位置为p＝[x,y]，los的位置为plos＝[xlos,ylos]，利用船舶的位置和期望轨迹，计算获得期望航向；

los角具体为:

gpc控制器持续跟踪期望航向角得出指令舵角，具体方法如下：

a、根据已建立的船舶模型中多项式系数a和b,可得到预测输出:

其中ψ为航向,δ为舵角,e为外部白噪声,d+1为系统的纯滞后；

b、由下式递推计算p步航向预测输出,构成向量ψm:

其中k＝1,…,p；p为预测步长；

c、实际操船要求航向近似于一阶惯性系统并逐步逼近设定值ψs；考虑系统的滞后,由下式递推计算p-d步参考轨迹,构成向量ψr,其中α为柔化系数,

d、取目标函数为:

j＝min{(ψr-ψ)^t(ψr-ψ)+λδδ^tδδ}

其中λ为加权系数；极小化目标函数得t时刻的命令舵角为:

δ(k)＝δ(k-1)+δδ

其中，

δδ＝(1,0,…,0)(g^tg+λi)^-1g^t(ψr-ψm)

本发明的有益效果为：该方法是将船舶的位置和舵角分别反馈给视线瞄准导航算法和gpc控制器，视觉计算出期望航向角，gpc控制器跟踪该航向角，得到指令舵角；视觉导航算法可根据设定模仿人的视觉特征，根据距离的远近来跟踪期望的点，再根据自身的位置，可得出期望航向角；本发明利用gpc的多步预测、滚动优化、反馈校正的优势，设计一个gpc控制器，对由视觉输出的航向角进行跟踪，计算出船舶的指令舵角，实现对船舶轨迹的高精度跟踪。

附图说明

图1为本发明的控制过程示意图；

图2为本发明的los导航算法示意图；

图3为本发明的定半径的los导航算法示意图；

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的说明，并不是把本发明的实施范围局限于此。

如图1至图3所示，本实施例所述的一种高精度轨迹跟踪控制方法。

为实现上述目的，本发明的具体方案如下：包括如下步骤，

a1、确定船舶的位置信息及舵角信息；

a2、将位置信息及舵角信息赋值于视线瞄准导航算法，通过视线瞄准导航算法得出期望航向角；

a3、包括一gpc控制器，通过确定的船舶位置信息及舵角信息以及期望航向角传输至gpc控制器；

a4、gpc控制器持续跟踪期望航向角得出指令舵角，实现高精度轨迹跟踪控制。

视线瞄准导航算法包括，即los算法：

以坐标原点为起点，指向点(x,y)的射线在坐标平面上与x轴正方向的夹角为los角，定义四象限反正切计算，该角角度为arctan(y,x)∈(-π,π)；地球坐标系下，假设船舶当前的位置为p＝[x,y]，los的位置为plos＝[xlos,ylos]，利用船舶的位置和期望轨迹，计算获得期望航向；

los角具体为:

若los的位置位于前一个路点和后一个路点的连线间，如图3所示，图中(xk,yk)，(xk+1,yk+1)代表两个相邻的航路点，(x,y)代表船舶的当前位置。以(x,y)为圆心，r为半径(r＝nlpp，lpp为船长，n＝2～5)画一条圆弧；当船舶靠近(xk,yk)和(xk+1,yk+1)之间的轨迹段时，圆弧将与轨迹线相交于两点ab(ti),af(ti)，船舶位置(x,y)与af(ti)之间的连线即是los，los与水平坐标x之间的夹角即是所求的船舶期望航向ψlos；按照期望航向跟踪虚拟点可以使船舶轨迹逐渐收敛至给定轨迹。在航向控制器的作用下，船舶以给定航速沿期望航向逐渐收敛至期望轨迹。

船舶沿当前轨迹跟踪完毕，需要一定的更新原则将期望轨迹切换至下一段轨迹，以达到连续跟踪给定轨迹的目的。当被控船舶的位置(x,y)，满足如下关系时：

(xk+1-x)²+(yk+1-y)²≤r²

则需要将跟踪的当前期望航向点转换成下一个期望航向点，即：取k＝k+1。利用船舶的位置和下一个期望航向点的数据计算出新的los角。

gpc控制器持续跟踪期望航向角得出指令舵角，具体方法如下：

a、根据已建立的船舶模型中多项式系数a和b,可得到预测输出:

其中ψ为航向,δ为舵角,e为外部白噪声,d+1为系统的纯滞后；

b、由下式递推计算p步航向预测输出,构成向量ψm:

其中k＝1,…,p；p为预测步长；

c、实际操船要求航向近似于一阶惯性系统并逐步逼近设定值ψs；考虑系统的滞后,由下式递推计算p-d步参考轨迹,构成向量ψr,其中α为柔化系数,

d、取目标函数为:

j＝min{(ψr-ψ)^t(ψr-ψ)+λδδ^tδδ}

其中λ为加权系数；极小化目标函数得t时刻的命令舵角为:

δ(k)＝δ(k-1)+δδ

其中，

δδ＝(1,0,…,0)(g^tg+λi)^-1g^t(ψr-ψm)

本发明的有益效果为，该方法是将船舶的位置和舵角分别反馈给视线瞄准导航算法和gpc控制器，视觉计算出期望航向角，gpc控制器跟踪该航向角，得到指令舵角；视觉导航算法可根据设定模仿人的视觉特征，根据距离的远近来跟踪期望的点，再根据自身的位置，可得出期望航向角；本发明利用gpc的多步预测、滚动优化、反馈校正的优势，设计一个gpc控制器，对由视觉输出的航向角进行跟踪，计算出船舶的指令舵角，实现对船舶轨迹的高精度跟踪。

以上所述仅是本发明的一个较佳实施例，故凡依本发明专利申请范围所述的构造、特征及原理所做的等效变化或修饰，包含在本发明专利申请的保护范围内。