一种过驱动AUV执行机构切换控制方法与流程

本发明涉及一种过驱动AUV执行机构切换控制方法，属于AUV运动控制领域。

背景技术：

由于海洋资源的探测开发的需要，国内外对AUV技术日益重视。由于AUV在执行不同任务时，对于机器人本身的速度有一定要求，如果需要在不同任务之间转换工作状态，那么就必然会发生高低速之间的切换。因为在低速时推力器效率高，舵效率低，而在高速时，推力器效率下降，舵效率增加，不少AUV尤其是高速运动AUV都采用舵翼和推进器混合控制。一般情况下AUV在低速时采用垂直槽道推力器控制升沉与纵倾，在高速时改用舵翼控制升沉与纵倾的执行机构配置方式。这就存在一个控制方式的切换，若在某一航速，立即关掉推力器，开启舵，由于控制指令的突变，控制会出现一段时间的振荡，速度和纵倾都会出现一定的抖动，纵倾角的变化使得AUV在此过程中作业难度增加。

文献[1]将控制输出设为一个线性函数，而控制器输入由TS形式的FLC给出，以采用平滑函数的VSC控制律设计了一个模糊逻辑控制器，形成了带有平滑函数的模糊控制器的方法。相比于PID控制，该方法减少了超调，增强了AUV控制的鲁棒性。

文献[2]研究了在AUV路径跟踪过程中，针对AUV由全驱动切换为欠驱动时漂角抖动幅度较大的问题，采用了三角函数形式的平滑函数对横移的控制输入进行了约束。仿真结果表明，该方法减小了漂角抖动的最大值，但最小值的绝对值有所增加。

文献[3]针对AUV舵桨联合操纵过程中执行器切换控制时容易造成输出突变的问题，提出了一种切换函数进行平滑切换以达到消除突变的目的。

相关文献表明，在现有技术中，未见效果较好的，能在AUV切换执行机构过程中既能保证切换的平滑过渡，又能使AUV纵倾角不做大幅度改变的方法。

本发明参考的资料如下：

[1]Wang Y,Shen Y,Wang K,et al.Fuzzy controller used smoothing function for depth control of autonomous underwater vehicle[C]//Oceans.IEEE,2016:1-5.

[2]Xiang X,Lapierre L,Jouvencel B.Smooth transition of AUV motion control:From fully-actuated to under-actuated configuration[J].Robotics&Autonomous Systems,2015,67:14-22.

[3]张磊,庞永杰,李晔,等.舵桨联合操纵的自主式水下机器人运动控制[J].北京工业大学学报,2011,37(1):40-46.

技术实现要素：

本发明的目的是为了提供一种过驱动AUV执行机构切换控制方法，能够使过驱动AUV执行机构切换时保持运动状态的平滑过渡。

本发明的目的是这样实现的：步骤1：根据AUV的水动力系数，建立AUV相对于随体坐标系的进退、升沉、俯仰的运动方程；

步骤2：找到主要状态变量，由进退、升沉、俯仰的运动学方程和动力学方程推导成为状态空间模型，并且根据控制器的采样速率将状态空间模型离散化得到增广状态空间模型；

步骤3：根据增广状态空间模型定义二次项形式的代价函数，代价函数是控制器设定值和未来系统轨迹之间的误差与控制输入未来变化幅度之和；

步骤4：分别设定高速与低速的速度区间，并在分界处设定过渡速度区间，将高速时的执行机构设为主推和舵翼，将低速时的执行机构设为主推和垂推，过渡区域执行机构设为主推、垂推和舵翼，在过渡区域为舵翼角度和垂推推力设定一个软约束，使推力和舵角在过渡区间内平滑过渡；

步骤5：使代价函数最小化解得控制输入并作用在系统上，得到的AUV的速度、纵倾、深度信息；

步骤6：步骤5得到的位姿信息作为反馈，判断是否满足控制指令：若满足，控制结束；若不满足，将位姿信息提供给状态空间模型和代价函数，得到预测轨迹，执行步骤5，直至当前控制结束。

本发明还包括这样一些结构特征：

1.步骤1具体为：

AUV相对于随体坐标系的进退，升沉、俯仰的运动学方程是：

动力学方程是：

其中：x和z分别是大地坐标系下的纵向速度和垂向速度，θ是AUV的纵倾角，m是AUV的质量，Iy是艇体对于y轴的惯性矩；Xλ、Zλ、Xf、Zf是由u、w速度项引起的流体惯性力项与拖曳力项，Xr与Zr是舵翼产生的拖曳力；Mq、Mh项是AUV水动力产生的旋转力矩与艇体力矩，Mr是舵翼产生的力矩；Xp、ZTf、ZTr项是尾部主推进器、首尾垂直槽道推进器推力；LTf、LTr是首尾垂直槽道推进器推力的力臂；G、▽代表艇体的重力与排水体积；g代表重力加速度；Δz代表他们的中心到Z轴的距离。

2.步骤2中，状态空间模型为：

其中：ρ是水的密度，带角标各项α和β分别是测得的与水动力相关的常数，δ为舵角；

将状态空间模型简化为：

其中：

将状态空间模型离散化得到增广状态模型：

其中，I为单位矩阵。

3.步骤3中：二次项形式的代价函数由增广状态空间模型定义，对于一个给定的预设值r(ki)，在样本时间ki内，在预测空间内,预测控制系统的目标，是将预测的输出端尽可能接近预设值，假设预设值在优化窗口中保持不变，NP与Nc分别为预测步长与控制步长，则以NP为长度的，包含预设值的矢量为：

定义代价函数J以反映控制目标：

式中：U是控制输出，是一个对角矩阵，rw≥0，rw是惩罚因子。

4.步骤4中，考虑到死区和电机转速的限制，对主推推力加以约束：

Xmin≤Xp≤Xmax

ΔXmin≤ΔXp≤ΔXmax

对垂推推力的约束：

Tmin≤Tp≤Tmax

ΔTmin≤ΔTp≤ΔTmax

对舵角的约束：

δmin≤δp≤δmax

Δδmin≤Δδp≤Δδmax

预测输出得到的方法是，利用增广状态空间模型得到预测状态变量：

x(ki+2|ki)＝Ax(ki+1|ki)+BΔu(ki+1)

＝A²x(ki)+ABΔu(ki)+BΔu(ki+1)

其中，

从预测状态变量得到预测输出为：

y(ki+1|ki)＝CAx(ki)+CBΔu(ki)

y(ki+2|ki)＝CA²x(ki)+CABΔu(ki)+CBΔu(ki+1)

y(ki+3|ki)＝CA³x(ki)+CA²BΔu(ki)+CABΔu(ki+1)+CBΔu(ki+2)

其中，C＝[0m I]。

定义向量：

Y＝[y(ki+1|ki) y(ki+2|ki) y(ki+3|ki) ... y(ki+Np|ki)]^T Y表示状态输出，且未来的控制域ΔU，长度为NC，表示为：

ΔU＝[Δu(ki) Δu(ki+1) Δu(ki+2) ... Δu(ki+Nc-1)]^T可得：

其中：

5.步骤5中，为了找到最佳的ΔU使J最小化，将状态输出：

带入代价函数则J为:

函数J的一阶导数为：

使J最小化的条件变为：

与现有技术相比，本发明的有益效果是：1、本发明采用采用模型预测控制方法，根据控制指令解算出主推推力、垂推推力和舵角能自然的处理好多输入多输出(MIMO)控制对象，可以比传统一些控制的方法更好地控制AUV。设计过程中约束可以被系统地纳入，这与传统的控制方法不同，因此该方法不受潜器物理条件的限制。并且MPC有预测的功能，可对未来误差进行预测。因此本发明具有适用性广，控制精度高的优点。2、本发明在高低速切换点处设定一个过渡区域，在过渡区域内，通过对执行机构定义一个软约束，保证纵倾不变的同时，使垂直槽道推进器和舵翼在此区间内平缓变化，能够避免由突然切换AUV执行机构引起的速度以及纵倾产生的较大幅度的抖动，减小控制出现振荡甚至导致控制发散的风险，使AUV保持运动状态的平滑过渡。

附图说明

图1为本发明的定深定速运动纵向速度仿真结果图；

图2为本发明的定深定速运动深度仿真结果图；

图3为本发明的定深定速运动纵倾角仿真结果图；

图4为本发明的定深定速运动主推推力仿真结果图；

图5为本发明的定深定速运动舵角仿真结果图；

图6为本发明的定深定速运动垂推推力仿真结果图；

图7为本发明的流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

实施例一：如图所示，本发明一种过驱动AUV执行机构切换控制方法包括如下步骤：

步骤1：建立AUV运动方程。根据AUV的水动力系数，建立AUV相对于随体坐标系的进退，升沉，俯仰的运动方程。因为大多数运动的AUV属于弱机动运动，第二和更高阶水动力项对于AUV运动的影响比一阶动力项较小，因此仅考虑一阶水动力，忽略第二阶以上水动力的项，则AUV相对于随体坐标系的进退，升沉，俯仰的运动学方程是：

动力学方程是：

其中，x和z分别是大地坐标系下的纵向速度和垂向速度，θ是AUV的纵倾角，m是AUV的质量，Iy是艇体对于y轴的惯性矩,Xλ、Zλ、Xf、Zf是由u、w速度项引起的流体惯性力项与拖曳力项，Xr与Zr是舵翼产生的拖曳力；Mq、Mh项是AUV水动力产生的旋转力矩与艇体力矩，Mr是舵翼产生的力矩；Xp、ZTf、ZTr项是尾部主推进器、首尾垂直槽道推进器推力；LTf、LTr是首尾垂直槽道推进器推力的力臂；G、代表艇体的重力与排水体积；g代表重力加速度；Δz代表他们的中心到Z轴的距离。

步骤2：推导状态空间模型。找到主要状态变量，由进退，升沉，俯仰的运动学方程和动力学方程推导成为状态空间模型，并且根据控制器的采样速率进一步将状态空间模型离散化得到增广状态空间模型；

对式(1)求导，并把式(3)带入得到：

再把式(4)、(5)考虑进来，式(1)变为：

其中，Xλcosθ，Zλsinθ表示Eξ轴方向上的惯性力，它们之和为：

其中，λξ表示Eξ轴方向上的附加质量。

类似的，Xfcosθ与Zfsinθ表示成：

式(6)可以写成如下形式：

类似的，式(2)与(3)可以表示成：

对于所研究的AUV，纵倾角被限制在θ∈(-15°，15°)，因此：

sinθ≈θ，cosθ≈1

定义：m-λξ＝mx，m-λζ＝mz

所以，所研究AUV在垂直面的运动方程为：

对于方程(14)，可以发现，拖曳力和舵力是纵向速度平方的函数，而阻力系数和舵力系数分别是θ和δ的函数，所以这些项将状态空间模型中近似。

显然，纵向速度变化对纵向速度方程比θ和δ变化影响较大，所以纵向流速作为主要的变量。使用最新的纵向速度表示线性化的拖曳力项，而拖曳力系数和舵力系数计算则使用最新的俯仰角和舵角，然后将方程(14)改写为：

其中：

对于式(15)考虑θ对的影响，因此以θ作为主要状态量，忽略剩余浮力影响,以θ、δ作为主要状态量，得到：

其中:

q和θ是主要变量，改为：

其中：ρ是水的密度，带角标各项α和β分别是测得的与水动力相关的常数，δ为舵角。

综上所述，状态空间表达式为：

得到状态空间模型：

可以表示为：

其中：

控制输出为：

y(t)＝Cmxm(t)

对式(22)、(23)离散化，可得：

xm(k+1)＝Mmxm(k)+Kmum(k) (24)

ym(k+1)＝Nmxm(k+1) (25)

本发明采用MPC算法来预测未来系统的轨迹，系统的输出是一个函数当前状态变化的向量，而不是输入向量。

这里使用的MPC算法嵌入到模型中，使控制器能处理不精确的模型。将U与ΔU之和作为系统的输入，ΔU是当前和以前的状态之间的差值。

差分状态方程可以表示为:

Δxm(k+1)＝MmΔxm(k)+KmΔum(k) (26)

由方程(25)得

y(k+1)-y(k)＝Nm(xm(k+1)-xm(k))＝NmΔxm(k+1)＝NmMmΔxm(k)+NmKmΔu(k) (27)

组合方程(24)与(25)可以得到扩展状态方程:

步骤3：定义二次项形式的代价函数。二次项形式的代价函数由增广状态空间模型定义,是控制器设定值和未来系统轨迹之间的误差与控制输入未来变化值之和，对于一个给定的预设值r(ki)，在样本时间ki内，在预测空间内，预测控制系统的目标，是将预测的输出端尽可能接近预设值，在这里我们假设预设值在优化窗口中保持不变。NP与Nc分别为预测步长与控制步长，则以NP为长度的，包含预设值的矢量为：

我们定义代价函数J以反映控制目标：

第一项与最小化预测输出同预设值之间误差的目的相关,第二项反映的是当目标函数J尽可能地小时，Δu的大小。

是一个对角矩阵，

其中，rw是惩罚因子。当rw＝0时，代价函数J，表示这样的情况：我们不关注ΔU的大小，我们的唯一目标是使第一项误差尽可能的小。

而当rw>0时，代价函数需同时考虑ΔU的大小和第二项误差。

步骤4：设定约束。分别设定高速与低速的速度区间，并将高速时的执行机构设为主推和舵翼，将低速时的执行机构设为主推和垂推，而过渡区域执行机构设为主推、垂推和舵翼，但是在过渡区域为舵翼角度和垂推推力设定一个软约束，使推力和舵角在过渡区间内平滑过渡；

考虑到死区和电机转速的限制，对主推推力加以约束：

Xmin≤Xp≤Xmax

ΔXmin≤ΔXp≤ΔXmax (32)

对垂推推力的约束：

Tmin≤Tp≤Tmax

ΔTmin≤ΔTp≤ΔTmax (33)

对舵角的约束：

δmin≤δp≤δmax

Δδmin≤Δδp≤Δδmax (34)

步骤5：设定软约束。对于垂推推力和舵翼，因为在低速时推力器效率高，舵效率低，而在高速时，推力器效率下降，舵效率增加，过驱动AUV在低速时采用垂直槽道推力器控制升沉与纵倾，在高速时改用舵翼控制升沉与纵倾的执行机构配置方式，这就存在一个控制方式的切换。由低速向高速切换时，本方法没有在某一航速立即关掉推力器开启舵，而是分界处设定过渡速度区间，平滑地将控制指令从推力器切换到舵，使每一个执行器的控制指令是总保持平缓的改变。

这里v是AUV的纵向速度，[a1,a2]是软约束作用的速度过渡区间，速度a1、a2要根据实际推力器及舵的效率与航速关系来确定，T表示垂推推力，δ表示舵角，P表示主推推力，则约束如下：

步骤6：状态变量和输出变量的预测。在每一个釆样时刻ki，状态变量xi(k)可通过测量获得，状态值xi(k)提供对象的当前信息，未来增量记为Δu(ki),Δu(ki+1),…,Δu(ki+NC-1)，未来的状态变量为：x(ki+1|ki,x(ki+2|ki),…,x(ki+Np|ki)，未来状态向量可表示为：

其中，

从预测状态变量得到预测输出为：

y(ki+1|ki)＝CAx(ki)+CBΔu(ki) (36)

y(ki+2|ki)＝CA²x(ki)+CABΔu(ki)+CBΔu(ki+1)

y(ki+3|ki)＝CA³x(ki)+CA²BΔu(ki)+CABΔu(ki+1)+CBΔu(ki+2)

...

其中，C＝[0m I]

定义向量：

Y＝[y(ki+1|ki) y(ki+2|ki) y(ki+3|ki) ... y(ki+Np|ki)]^T (38)

Y表示状态输出,并且未来的控制域长度为NC，ΔU可以表示为：

ΔU＝[Δu(ki) Δu(ki+1) Δu(ki+2) ... Δu(ki+Nc-1)]^T (39)

可得：

其中，

步骤7：解算控制输入。使代价函数最小化解得控制输入，作用在系统上，得到的AUV的速度、纵倾、深度信息；为了找到最佳的ΔU使J最小化，将状态输出：

带入代价函数把J表示成：

函数J的一阶导数为：

使J最小化的条件变为：

那么，可以得到：

步骤8：步骤6得到的位姿信息作为反馈，判断是否满足控制指令：若满足，控制结束；若不满足，将位姿信息提供给状态空间模型和代价函数，得到预测轨迹，执行步骤5至7，直至当前控制结束。

实施例二：本发明的步骤如下：

步骤1：根据AUV的水动力系数，建立AUV相对于随体坐标系的进退，升沉，俯仰的运动方程。因为大多数运动的AUV属于弱机动运动，第二和更高阶水动力项对于AUV运动的影响比一阶动力项较小，因此仅考虑一阶水动力，忽略第二阶以上水动力的项；

步骤2：找到主要状态变量，由进退，升沉，俯仰的运动学方程和动力学方程推导成为状态空间模型，并且根据控制器的采样速率进一步将状态空间模型离散化得到增广状态空间模型；

步骤3：根据增广状态空间模型定义二次项形式的代价函数，代价函数是控制器设定值和未来系统轨迹之间的误差与控制输入未来变化幅度之和；

步骤4：分别设定高速与低速的速度区间，并在分界处设定过渡速度区间，将高速时的执行机构设为主推和舵翼，将低速时的执行机构设为主推和垂推，而过渡区域执行机构设为主推、垂推和舵翼，但是在过渡区域为舵翼角度和垂推推力设定一个软约束，使推力和舵角在过渡区间内平滑过渡；

步骤5：使代价函数最小化解得控制输入，作用在系统上，得到的AUV的速度、纵倾、深度信息；

步骤6：步骤5得到的位姿信息作为反馈，判断是否满足控制指令，若不满足，将位姿信息提供给状态空间模型和代价函数，得到预测轨迹，执行步骤5，直至当前控制结束。

步骤1中，忽略2阶以上水动力后，AUV相对于随体坐标系的进退，升沉，俯仰的运动学方程是：

动力学方程：

其中，x和z分别是大地坐标系下的纵向速度和垂向速度，θ是AUV的纵倾角，m是AUV的质量，Iy是艇体对于y轴的惯性矩,Xλ、Zλ、Xf、Zf是由u、w速度项引起的流体惯性力项与拖曳力项，Xr与Zr是舵翼产生的拖曳力。Mq、Mh项是AUV水动力产生的旋转力矩与艇体力矩，Mr是舵翼产生的力矩。Xp、ZTf、ZTr项是尾部主推进器、首尾垂直槽道推进器推力。LTf、LTr是首尾垂直槽道推进器推力的力臂。G、代表艇体的重力与排水体积；g代表重力加速度；Δz代表他们的中心到Z轴的距离。

表1运动的参数以及力的动坐标的分量

步骤2中，状态空间模型为：

其中，ρ是水的密度，带角标各项α和β分别是测得的与水动力相关的常数，δ为舵角。

状态空间模型可以简写为以下形式：

其中:

步骤2中，将状态空间模型离散化得到增广状态模型:

I为单位矩阵。

步骤3中，二次项形式的代价函数由增广状态空间模型定义,对于一个给定的预设值r(ki)，在样本时间ki内，在预测空间内,预测控制系统的目标，是将预测的输出端尽可能接近预设值，在这里我们假设预设值在优化窗口中保持不变。NP与Nc分别为预测步长与控制步长，则以NP为长度的，包含预设值的矢量为：

我们定义代价函数J以反映控制目标：

第一项与最小化预测输出同预设值之间误差的目的相关,第二项反映的是当目标函数J尽可能地小时,Δu的大小。

U是控制输出，是一个对角矩阵，

其中，rw是惩罚因子。当rw＝0时，代价函数J，表示这样的情况：我们不关注ΔU的大小，我们的唯一目标是使第一项误差尽可能的小。

而当rw>0时，代价函数需同时考虑ΔU的大小和第二项误差。

预测输出得到的方法是，利用增广状态空间模型得到预测状态变量：

x(ki+2|ki)＝Ax(ki+1|ki)+BΔu(ki+1)

＝A²x(ki)+ABΔu(ki)+BΔu(ki+1)

其中，

从预测状态变量得到预测输出为：

y(ki+1|ki)＝CAx(ki)+CBΔu(ki)

y(ki+2|ki)＝CA²x(ki)+CABΔu(ki)+CBΔu(ki+1)

y(ki+3|ki)＝CA³x(ki)+CA²BΔu(ki)+CABΔu(ki+1)+CBΔu(ki+2)

其中，C＝[0m I]

定义向量：

Y＝[y(ki+1|ki) y(ki+2|ki) y(ki+3|ki) ... y(ki+Np|ki)]^T

Y表示状态输出,并且未来的控制域ΔU，长度为NC，可以表示为：

ΔU＝[Δu(ki) Δu(ki+1) Δu(ki+2) ... Δu(ki+Nc-1)]^T

可得:

其中，

步骤4中，考虑到死区和电机转速的限制，对主推推力加以约束：

Xmin≤Xp≤Xmax

ΔXmin≤ΔXp≤ΔXmax

对垂推推力的约束：

Tmin≤Tp≤Tmax

ΔTmin≤ΔTp≤ΔTmax

对舵角的约束：

δmin≤δp≤δmax

Δδmin≤Δδp≤Δδmax

步骤4中，对于垂推推力和舵翼，因为在低速时推力器效率高，舵效率低，而在高速时，推力器效率下降，舵效率增加，过驱动AUV在低速时采用垂直槽道推力器控制升沉与纵倾，在高速时改用舵翼控制升沉与纵倾的执行机构配置方式，这就存在一个控制方式的切换。由低速向高速切换时，本方法没有在某一航速立即关掉推力器开启舵，而是设定了一个中间阶段，缓慢地将控制指令从推力器切换到舵，由高速向低速切换时同理，也就是说每一个执行器的控制指令是总保持平缓的改变。

这里v是AUV的纵向速度，[a1,a2]是软约束作用的速度过渡区间，速度a1、a2要根据实际推力器及舵的效率与航速关系来确定，T表示垂推推力，δ表示舵角，P表示主推推力，则约束如下：

步骤5中，为了找到最佳的ΔU使J最小化，将状态输出：

带入代价函数把J表示成:

函数J的一阶导数为：

使J最小化的条件变为：

综上，本发明提供的是一种过驱动智能水下机器人(Autonomous Underwater Vehicle,AUV)切换执行机构时的控制方法。本发明包括：建立AUV垂直面动力学方程；建立AUV垂直面运动状态空间模型；低速时执行机构采用主推和垂直槽道推进器，高速时采用主推和舵翼；在高低速切换点处设定一个过渡区域，在过渡区域内，通过对执行机构定义一个软约束，使垂直槽道推进器和舵翼在此区间内平缓变化，并保持纵倾角不变；采用模型预测控制方法，根据控制指令解算出主推推力、垂推推力和舵角；将控制输入带入模型得到AUV位姿信息。本发明能够避免由突然切换AUV执行机构引起的速度以及纵倾产生的较大幅度的抖动，减小控制出现的振荡幅度，保证AUV在切换执行机构的同时保持运动状态的平滑过渡，而对于纵倾的控制可以保证AUV在切换过程中的作业能力不受影响。