CN110647161B - 基于状态预测补偿的欠驱动uuv水平面轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供的是一种基于状态预测补偿的欠驱动UUV水平面轨迹跟踪控制方法。步骤1:UUV根据当前任务获取位置、姿态信息；步骤2:利用欠驱动UUV的数学模型得出运动坐标下的位置、姿态误差变量；步骤3:通过镇定位姿误差变量，计算虚拟控制律；步骤4:根据UUV的运动学和动力学模型分别构造位姿状态预测器和速度状态预测器；步骤5:根据预测的状态信息构造UUV实际轨迹跟踪控制律。本发明的方法可以在欠驱动UUV执行机构存在时间延迟的情况下，利用本发明所提出的方法设计的UUV轨迹跟踪控制器作用下的UUV，可以实现对平面轨迹的精确跟踪，所设计的控制器更加符合工程实际的要求。

Description

基于状态预测补偿的欠驱动UUV水平面轨迹跟踪控制方法

技术领域

本发明涉及的是一种欠驱动水下无人航行器运动控制方法，具体地说是一种欠驱动UUV在输入时滞下的水平面轨迹跟踪控制方法。

背景技术

地球上海洋面积广阔，广袤的海洋蕴藏着丰富的资源，随着人类文明的进步，资源的重要性越来越重要，因此海洋开发活动拥有非常重要的战略和现实意义。

海洋资源的开发离不开科学技术的支持，而UUV是海洋资源勘探和开发的重要工具，尤其是UUV的轨迹跟踪控制能力是实现水下地形测绘、救生、勘探等任务的重要技术保障。

传统UUV轨迹跟踪控制器的设计均忽略了航行器控制输入具有时滞(时间延迟)的特性，即UUV的舵、螺旋桨等执行机构的控制输入量已经发生改变，其位置和姿态等输出量却并不如阶跃信号一样能立即发生相应改变，而是要延时一段时间后才发生变化。然而，实际的UUV运动控制系统的控制输入存在时间延迟这种固有属性是不可忽略的，主要体现在其舵机上，系统下达指令后，舵机的舵面无法立刻偏转到指定的角度，完成控制力和力矩的分配，从而产生控制输入的时滞。这种时滞是固定的时间延迟，机械传动特性决定了其不可消除，其数量级通常在10^-1秒。时滞对于实时跟踪要求高的欠驱动UUV轨迹跟踪控制系统的跟踪性能以将产生严重的负面影响，甚至破坏整个系统的稳定性。

发明内容

本发明的目的在于提供一种在欠驱动UUV执行机构存在时间延迟的情况下可以实现对平面轨迹的精确跟踪的基于状态预测补偿的欠驱动UUV水平面轨迹跟踪控制方法。

本发明的目的是这样实现的：

步骤1、根据当前任务，将期望轨迹信息X_d(t)＝[x_d(t),y_d(t),ψ_d(t)]^T传给UUV，并通过UUV所搭载的导航设备和传感器采集数据，获得当前的位置信息及航向信息X(t)＝[x(t),y(t),ψ(t)]^T和速度信息V(t)＝[u(t),v(t),r(t)]^T，x(t)、y(t)、ψ(t)分别是纵向位移、横向位移和艏向角，u(t)、v(t)、r(t)分别是纵向速度、横向速度和艏向角速度，x_d(t)、y_d(t)、ψ_d(t)为期望纵向位移、横向位移和艏向角；

步骤2、计算出实际轨迹与期望轨迹之间的位置、姿态跟踪误差，利用欠驱动UUV水平面三自度数学模型，将惯性坐标下的误差信息转换为运动坐标下的误差信息；

步骤3、基于步骤2中计算出的位置误差，采用定义虚拟速度误差变量的方法，将位置跟踪控制转化为速度控制，计算出纵向速度虚拟控制律u_d、横向虚拟控制律v_d和艏向角速度虚拟控制律r_d；

步骤4、分别由UUV的运动学和动力学模型设计位姿预测器和速度预测器，利用当前时刻的状态信息x,y,ψ,u,v,r去预测t+T时刻的状态信息x(t+d),y(t+d),ψ(t+d),u(t+d),v(t+d),r(t+d)；

步骤5、结合步骤2和步骤3中的数据，通过构造Lyapunov函数将位置误差的镇定转化为对虚拟速度误差的镇定，通过镇定虚拟速度误差e_u,e_r设计实际控制τ_u,τ_r，根据步骤4预测的状态信息替换实际控制τ_u,τ_r中当前的状态信息，来构造超前控制律，控制律的超前时刻刚好和执行机构的时滞时间相互抵消。

本发明提供一种在UUV执行机构存在时滞的情况下基于状态预测补偿实现其水平面轨迹跟踪控制的方法。本发明的方法可以在欠驱动UUV执行机构存在时间延迟的情况下，利用本发明所提出的方法设计的UUV轨迹跟踪控制器作用下的UUV，可以实现对平面轨迹的精确跟踪，所设计的控制器更加符合工程实际的要求。

附图说明

图1输入时滞下欠驱动UUV水平面轨迹跟踪控制流程图；

图2惯性坐标系与运动坐标系示意图；

图3输入时滞下欠驱动UUV水平面轨迹跟踪控制系统结构图；

图4输入时滞下UUV水平面轨迹跟踪控制效果图；

图5UUV水平面轨迹跟踪位置和姿态误差仿真示意图；

图6UUV水平面轨迹跟踪速度响应曲线仿真示意图；

图7UUV水平面轨迹跟踪速度误差响应曲线仿真示意图；

图8UUV水平面轨迹跟踪控制输入响应曲线仿真示意图。

具体实施方式

下面举例对本发明做更详细的描述。

结合图1，一种输入时滞下的UUV水平面轨迹跟踪控制方法包括如下步骤:

步骤1中根据当前任务，将期望轨迹信息X_d(t)＝[x_d(t),y_d(t),ψ_d(t)]^T传给UUV，并通过UUV所搭载的导航设备和传感器采集数据，获得当前的位置信息、航向信息X(t)＝[x(t),y(t),ψ(t)]^T和速度信息V(t)＝[u(t),v(t),r(t)]^T，x(t),y(t),ψ(t)分别是纵向位移，横向位移和艏向角，u(t)、v(t)、r(t)分别是纵向速度、横向速度和艏向角速度，x_d(t),y_d(t),ψ_d(t)为期望纵向位移，横向位移和艏向角，X_d(t)为期望轨迹信息的矢量表示形式。

步骤2中利用步骤1中的信息，计算出实际轨迹与期望轨迹之间的位置、航向误差，利用欠驱动UUV水平面三自度数学模型，将惯性坐标下的误差信息转换为运动坐标下的误差信息。

所涉及的UUV水平面三自度度数学模型信息包括：

运动学模型:

含输入时滞的动力学模型：

式中

d₁₁＝X_u+X_u|u|,d₂₂＝Y_v+Y_v|v|，d₃₃＝N_r+N_r|r|，其中，m为UUV总质量，X_u、X_u|u|、Y_v、Y_v|v|、N_r、N_r|r|均为水动力参数，m₁₁，m₂₂，m₃₃，d₁₁，d₂₂，d₃₃仅是为了方便观察模型，用单个字母来代替公式，并无实际意义，T为控制输入的延迟时间。以上模型为教材中UUV的通用简化模型，非本专利原创。

惯性坐标下的位置和姿态误差：

其中x_d，y_d，为期望位置，

为期望艏向角，x_e，y_e，ψ_e为在惯性坐标系下实际位姿与期望位姿之间的误差。

运动坐标系下的位置和姿态误差：

对(4)式求导可得：

其中，e_x，e_y，e_ψ为运动坐标系下的纵向位移，横向位移，艏向角的误差，

为纵向速度，横向速度，艏向角速度的误差，

为固定坐标下的期望合速度。

步骤3中基于步骤2中计算出的位置误差，采用定义虚拟速度误差变量的方法，将姿态跟踪控制转化为速度控制，计算出纵向速度虚拟控制律u_d、横向虚拟控制律v_d和艏向角速度虚拟控制律r_d。

对于误差(4)式，构造如下Lyapunov函数：

设计纵向虚拟速度控制律u_d和横向虚拟速度控制律v_d为：

其中k₁,k₂为正常数。

将u，v视作虚拟速度，e_u，e_v为虚拟纵向，横向的速度误差。则虚拟速度的误差为：

结合(7)、(8)式对(6)式求导可得：

为镇定虚拟速度误差变量e_v，定义Lyapunov函数：

设计艏向角速度控制律r_d为：

其中k₄为大于零的常数，

为横向虚拟速度控制律v_d的导数。

定义虚拟速度误差变量e_r为：

e_r＝r-r_d (12)

为镇定虚拟速度误差变量e_u,e_r，定义Lyapunov函数：

对V₃求导可得：

设计实际控制输入为：

式中k₃,k₅为大于零的常数，

为实际控制输入。

分别由UUV的运动学和动力学模型设计位姿预测器和速度预测器，利用当前时刻的状态信息x,y,ψ,u,v,r去预测t+T时刻的状态信息x(t+d),y(t+d),ψ(t+d),u(t+d),v(t+d),r(t+d)。

根据预测的状态信息替换

中当前的状态信息，来构造“超前”控制律，控制律的超前时刻刚好和执行机构的时滞时间相互抵消，实现了对输入时滞的抑制。

仿真实验验证与分析

下面通过仿真实验来验证所提出的发明方法的有效性。为了验证所提控制方法的优点和有效性，并从实际工程的角度考虑设计了一种“直线+圆”平面时变轨迹：

设置UUV的起始点为(x,y)＝(3,10)，初始航向和航速均为零，拐点A为(x,y)＝(60,30)，控制增益为k₁＝2,k₂＝1，k₃＝1,k₄＝1,k₅＝1，执行机构的延迟时间设为0.5s。仿真时间设为500s，Simulink仿真实验结果如图4-图8所示。

图4为欠驱动UUV对期望轨迹跟踪效果图。UUV在初始误差为x_e＝3m，y_e＝10m，ψ_e＝(-arctan2)rad的情况下实现了对期望轨迹的精确跟踪，说明本发明所提方法很好地抑制了控制输入存在时滞的问题。

图5是位置和航向跟踪误差，从图中可以看出系统稳定后跟踪误差很小，几乎为零。图6、图7分别为UUV速度和速度误差响应曲线，从中可以看出即使在仿真时间150s时角度突变的情况下，速度变化依然较缓，说明所设计的控制器控制性能好。

图8为UUV线速度和角速度响应曲线，从图中可以看出，控制器作用下UUV的纵向推力和艏向力矩的输出曲线略有抖动，目的是为了通过调整输入来快速跟踪上期望轨迹，因为仿真开始150s时期望轨迹有一个不平滑的过渡点A，所以两个控制输入的响应曲线都在150s时图像上出现了一个“毛刺”，之后二者都趋于一个稳定值，控制输入响应曲线比较光滑，符合工程实际的需求。

从图4-图8可以看出本发明所设计的欠驱动轨迹跟踪控制器能够实现在执行机构存在时间延迟的情况下对期望时变轨迹的精确跟踪。

Claims (2)

1.一种基于状态预测补偿的欠驱动UUV水平面轨迹跟踪控制方法，其特征是：

步骤1、根据当前任务，将期望轨迹信息X_d(t)＝[x_d(t)，y_d(t)，ψ_d(t)]^T传给UUV，并通过UUV所搭载的导航设备和传感器采集数据，获得当前的位置信息及航向信息X(t)＝[x(t)，y(t)，ψ(t)]^T和速度信息V(t)＝[u(t)，v(t)，r(t)]^T，x(t)、y(t)、ψ(t)分别是纵向位移、横向位移和艏向角，u(t)、v(t)、r(t)分别是纵向速度、横向速度和艏向角速度，x_d(t)、y_d(t)、ψ_d(t)为期望纵向位移、横向位移和艏向角；

步骤2、计算出实际轨迹与期望轨迹之间的位置、姿态跟踪误差，利用欠驱动UUV水平面三自度数学模型，将惯性坐标系下的误差信息转换为运动坐标系下的误差信息；

步骤3、基于步骤2中计算出的位置误差，采用定义虚拟速度误差变量的方法，将位置跟踪控制转化为速度控制，计算出纵向速度虚拟控制律u_d、横向虚拟控制律v_d和艏向角速度虚拟控制律r_d；

步骤4、分别由UUV的运动学和动力学模型设计位姿预测器和速度预测器，利用当前时刻的状态信息x，y，ψ，u，v，r去预测t+T时刻的状态信息x(t+d)，y(t+d)，ψ(t+d)，u(t+d)，v(t+d)，r(t+d)；

步骤5、结合步骤2和步骤3中的数据，通过构造Lyapunov函数将位置误差的镇定转化为对虚拟速度误差的镇定，通过镇定虚拟速度误差e_u，e_r设计实际控制τ_u，τ_r，根据步骤4预测的状态信息替换实际控制τ_u，τ_r中当前的状态信息，来构造超前控制律，控制律的超前时刻刚好和执行机构的时滞时间相互抵消；

对于运动坐标系下的位置和姿态误差

构造如下Lyapunov函数：

设计纵向虚拟速度控制律u_d和横向虚拟速度控制律v_d为：

其中k₁，k₂为正常数，

将u，v视作虚拟速度，e_u，e_v为虚拟纵向、横向的速度误差，则虚拟速度的误差为：

结合

式对

求导得：

为镇定虚拟速度误差变量e_v，定义Lyapunov函数：

设计艏向角速度控制律r_d为：

其中k₄为大于零的常数，

为横向虚拟速度控制律v_d的导数；

定义虚拟速度误差变量e_r为：

e_r＝r-r_d

为镇定虚拟速度误差变量e_u，e_r，定义Lyapunov函数：

对V₃求导得：

设计实际控制输入为：

式中k₃，k₅为大于零的常数，

为实际控制输入；

分别由UUV的运动学和动力学模型设计位姿预测器和速度预测器，利用当前时刻的状态信息x，y，ψ，u，v，r去预测t+T时刻的状态信息x(t+d)，y(t+d)，ψ(t+d)，u(t+d)，v(t+d)，r(t+d)，

根据预测的状态信息替换

中当前的状态信息，来构造超前控制律，控制律的超前时刻刚好和执行机构的时滞时间相互抵消，实现了对输入时滞的抑制，

2.根据权利要求1所述的基于状态预测补偿的欠驱动UUV水平面轨迹跟踪控制方法，其特征是：所述的欠驱动UUV水平面三自度数学模型包括运动学模型、含输入时滞的动力学模型、惯性坐标系下的位姿误差、运动坐标系下的位姿误差。

Images