CN105786012A - 基于生物激励模型的uuv虚拟速度控制方法 - Google Patents

Abstract

基于生物激励模型的UUV虚拟速度控制方法，涉及一种UUV速度的控制方法。为了解决UUV速度的控制不稳定的问题。包括：获得虚拟速度；将当前的位置误差输入至生物激励模型进行平滑连续处理，获得新的位置误差，根据所述新的位置误差对虚拟速度进行处理，获得平滑连续的虚拟速度；将虚拟速度、海流速度与UUV当前实际航速相减后获得速度误差作为PID速度控制器的输入，PID速度控制器将输出作用在UUV模型上，得到UUV下一步的实际位置，根据得到实际位置对UUV进行控制；将UUV的实际位置与UUV运动规划输出的期望位置相比较，得到的位置误差项作为下一步生物激励模型的位置误差输入。本发明用于UUV在近水面或浅海区域航行时控制航速。

Description

基于生物激励模型的UUV虚拟速度控制方法

技术领域

本发明涉及一种UUV速度的控制方法，特别涉及一种基于生物激励模型的UUV虚拟速度控制方法。

背景技术

UUV在近水面或浅海区域航行时，海流对于航速的干扰非常强，在实际中使用传统的PID控制器时，出现了许多特有的控制问题，如航速跟踪持续滞后、推进器响应频繁等。致使UUV在航行中噪声增大、续航能力变差、推进器使用寿命减少。

现有一种基于迭代滑模方法设计了UUV航迹跟踪控制器，但并没有对UUV的航速进行必要的控制，仅对位置进行控制，而在实际实验中精确的位置跟踪是建立在稳定的航速基础上的。由于航速的精准与否对UUV整体性能有重要影响，因此对于UUV实际航速的准确稳定控制十分有必要。

而现有采用传统PID控制***进行速度控制也存在不稳定问题。

发明内容

本发明的目的是为了解决UUV速度的控制不稳定的问题，本发明提供一种基于生物激励模型的UUV虚拟速度控制方法。

本发明的基于生物激励模型的UUV虚拟速度控制方法，所述方法包括如下步骤：

步骤一：根据运动学模型建立欠驱动UUV三维空间运动时的速度非线性方程，得到UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系；

步骤二：根据步骤一得到的UUV速度间的关系,获得虚拟速度；

步骤三：将当前的位置误差输入至生物激励模型进行平滑连续处理，获得新的位置误差，根据所述新的位置误差对步骤二中的虚拟速度进行处理，获得平滑连续的虚拟速度；

步骤四：将步骤三生成的虚拟速度、海流速度与UUV当前实际航速相减后获得速度误差作为PID速度控制器的输入，PID速度控制器将输出作用在UUV模型上，得到UUV下一步的实际速度；

步骤五：将步骤四得到的UUV下一步的实际速度从艇体坐标系{B}下转换为固定坐标系{E}下，再积分，得到UUV的下一步的实际位置，根据得到实际位置对UUV进行控制；

步骤六：将UUV的实际位置与UUV运动规划输出的期望位置相比较，得到的位置误差项作为下一步步骤三中生物激励模型的位置误差输入，转入步骤三。

所述步骤一中，UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系为：

其中，u、v和w分别为UUV在艇体坐标系{B}下三个线速度分量；r为UUV在艇体坐标系{B}下与艏向角对应的角速度分量，ψ为艏向角，ψ_d为期望艏向角；ξ、η和ζ分别为UUV在固定坐标系{E}下三个位置分量，UUV当前运动位置η＝[ξηζ]^T，η_d＝[ξ_dη_dζ_d]^T为UUV期望运动位置；UUV的位置误差为e＝[e_ξe_ηe_ζe_ψ]^T，e_ξ、e_η和e_ζ分别表示在ξ、η和ζ三个位置上的误差，e_ψ为艏向角误差，˙表示求导。

所述二中，根据步骤一得到的UUV速度间的关系，利用反步法获得虚拟速度：

其中_，k、k_ζ和k_ψ均为大于0的常数，U_d＝[u_dv_dw_dr_d]^T为UUV在艇体坐标系{B}下的期望速度。

所述步骤三中，根据所述新的位置误差对步骤二中下一步的虚拟速度进行处理，获得平滑连续的虚拟速度的方法为：

将新的位置误差取代下一步的虚拟速度中的位置误差项，从而得到平滑连续的虚拟速度。

所述生物激励模型为：

其中f(e_Δ)＝max(e_Δ，0),g(e_Δ)＝max(-e_Δ，0)；生物激励模型在任意输入情况下的输出的误差项总是保持在[-D，B]范围内；e_i为生物激励模型的输入，即：当前的位置误差，e_Δ＝0-e_i为当前的位置误差的误差，V_i为生物激励模型的输出，即：新的位置误差，i＝ζ,η,ξ，ψ：

A、B和D分别为生物激励模型中的参数；

平滑连续的虚拟速度U′_c：

本发明的有益效果在于，本发明得到的UUV虚拟速度是基于生物激励模型的，对UUV运动模型进行分析得到UUV位置误差对UUV航行的影响，结合生物激励模型的平滑特性，根据实际控制速度的需求将位置误差结合模型进行平滑处理，最终得到具有平滑连续特性的虚拟速度。本发明针对传统PID***控制产生的速度不稳定问题进行研究，在加入生物激励模型后使UUV航速得以控制，并且在海流干扰下也能够保持稳定性能，降低了噪声，***不会震荡。

附图说明

图1为具体实施方式中UUV垂直面坐标系示意图。

图2为具体实施方式中UUV水平面坐标系示意图。

图3为艇体坐标系{B}的原理示意图。

图4为具体实施方式中基于生物激励模型的UUV虚拟速度控制方法的原理示意图。

具体实施方式

具体实施方式一：本实施方式所述的基于生物激励模型的UUV虚拟速度控制方法，以下将结合图1至图4对本实施方式作进一步详细说明。所述基于生物激励模型的UUV虚拟速度控制方法包括如下步骤：

步骤一：根据运动学模型建立欠驱动UUV三维空间运动时的速度非线性方程，得到UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系；

为便于分析，本实施方式对UUV航行模型做出如下假设：(1)UUV速度为非负值，即不考虑倒行时的速度控制；(2)忽略横滚运动的影响；(3)所考虑的UUV对象左右对称。在UUV运动分析及操纵性试验中，通常分为水平面与垂直面两方面来进行。其中水平面分析主要用于考察UUV航速及回转运动稳定性，而垂直面分析则主要考察UUV定深、变深性能。图1与图2描述了UUV三维空间运动分析时所用到的坐标系。其中，E_ξηζ为大地固定坐标系，以E为大地固定坐标系的中心，ξ、η和ζ分别表示大地固定坐标系下的三个轴对应的坐标。B_XYZ为UUV艇体坐标系，如图3所示，以B为艇体坐标系的中心，p、q和r分别为UUV角速度矢量在艇体坐标系下的三个分量；G为UUV重心，在坐标系{B}下其坐标为R_G＝(x_G，0,0)^T；UUV的合成速度U＝[uvw]^T，χ和γ分别为UUV潜浮角与航迹角；α和β分别为UUV冲角与漂角；ψ为艏向角；由于本实施方式考虑的是近水面UUV航行时，所以忽略横倾角与纵倾角θ，即

UUV艇体坐标系B_XYZ相关参数含义如表1所示。

表1UUV运动变量

UUV空间运动方程利用速度向U＝[uvw]^T表示的形式如下：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=Ucos\mathrm{\γ}cos\mathrm{\χ}\\ \stackrel{\·}{\mathrm{\η}}=Usin\mathrm{\γ}cos\mathrm{\χ}\\ \stackrel{\·}{\mathrm{\ζ}}=Usin\mathrm{\γ}\end{array}\right.---\left(1\right)$

其中，u、v、w分别为UUV在艇体坐标系{B}下3个速度分量。

设固定坐定标系{E}到艇体坐标系{B}之间的坐标旋转矩阵为S，则：

$\left[\begin{array}{c}\mathrm{\ξ}\\ \mathrm{\η}\\ \mathrm{\ζ}\end{array}\right]=S\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\ψ}& -sin\mathrm{\ψ}& 0\\ sin\mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(2\right)$

其中，x、y、z分别为UUV在艇体坐标系{B}下位置，ξ、η、ζ分别为UUV在固定坐定标系{E}下位置。

设在艇体坐标系{B}下：UUV的合成速度U＝[uvw]^T，角速度Ω＝[pqr]^T，p、q、r分别为UUV在艇体坐标系{B}3个角速度分量，r为UUV在艇体坐标系{B}下与艏向角对应的角速度分量。在固定坐定标系{E}下：UUV的绝对航速角速度

对式(2)进行求导可得：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}\\ \stackrel{\·}{\mathrm{\η}}\\ \stackrel{\·}{\mathrm{\ζ}}\end{array}\right]=S\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\ψ}& -sin\mathrm{\ψ}& 0\\ sin\mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]---\left(3\right)$

且有：

定义在固定坐定标系{E}下：近水面运动UUV的位置误差为e＝η_d-η＝[e_ξe_ηe_ζ]^T，对其求导可得：

$\stackrel{\·}{e}={\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-\stackrel{\·}{\mathrm{\ξ}}\\ {\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-\stackrel{\·}{\mathrm{\ζ}}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-ucos\mathrm{\ψ}+vsin\mathrm{\ψ}\\ {\stackrel{\·}{\mathrm{\η}}}_d-usin\mathrm{\ψ}-vcos\mathrm{\ψ}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-w\end{array}\right]---\left(4\right)$

其中，UUV当前运动位置η＝[ξηζ]^T，η_d＝[ξ_dη_dζ_d]^T为UUV期望运动位置，e_ξ、e_η和e_ζ分别表示在ξ、η和ζ三个位置上的误差，e_ψ为艏向角误差，˙表示求导。由图2可得UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=ucos\mathrm{\ψ}-vsin\mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\η}}=usin\mathrm{\ψ}+cos\mathrm{\ψ}\\ \stackrel{\·}{w}=w\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(5\right)$

则有：

$\left[\begin{array}{c}{\stackrel{\·}{e}}_{\mathrm{\ξ}}\\ {\stackrel{\·}{e}}_{\mathrm{\η}}\\ {\stackrel{\·}{e}}_{\mathrm{\ζ}}\\ {\stackrel{\·}{e}}_{\mathrm{\ψ}}\end{array}\right]={\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ξ}}}_d-\stackrel{\·}{\mathrm{\ξ}}\\ {\stackrel{\·}{\mathrm{\η}}}_d-\stackrel{\·}{\mathrm{\η}}\\ {\stackrel{\·}{\mathrm{\ζ}}}_d-\stackrel{\·}{\mathrm{\ζ}}\\ {\stackrel{\·}{\mathrm{\ψ}}}_d-\stackrel{\·}{\mathrm{\ψ}}\end{array}\right]---\left(6\right)$

ψ为艏向角，ψ_d为期望艏向角；

步骤二：根据步骤一得到的UUV速度间的关系,获得虚拟速度：

依据式(5)，根据反步法，获得虚拟速度：

$U_c=\left[\begin{array}{c}k(e_{\mathrm{\ξ}}cos\mathrm{\ψ}+e_{\mathrm{\η}}sin\mathrm{\ψ})+(u_d{\mathrm{cose}}_{\mathrm{\ψ}}-v_d\sin e_{\mathrm{\ψ}})\\ k(-e_{\mathrm{\ξ}}sin\mathrm{\ψ}+e_{\mathrm{\η}}cos\mathrm{\ψ})+(u_d\sin e_{\mathrm{\ψ}}+v_d\cos e_{\mathrm{\ψ}})\\ w_d+k_{\mathrm{\ζ}}{e}_{\mathrm{\ζ}}\\ r_d+k_{\mathrm{\ψ}}{e}_{\mathrm{\ψ}}\end{array}\right]---\left(7\right)$

其中，k、k_ζ和k_ψ均为大于0的常数，U_d＝[u_dv_dw_dr_d]^T为UUV在艇体坐标系{B}下的期望速度，u_dcose_ψ-v_dsine_ψ，u_dsine_ψ+v_dcose_ψ为期望速度坐标到实际速度坐标的转换。

步骤三：将当前的位置误差输入至生物激励模型进行平滑连续处理，获得新的位置误差，根据所述新的位置误差对步骤二中的虚拟速度进行处理，获得平滑连续的虚拟速度：本实施方式中虚拟速度设计模块应用了生物激励模型，是本实施方式中最重要的特征。生物激励模型具有平滑响应，抑制控制输出上限及下限的作用，且具有全局一致渐进稳定的特性。

本发明应用如下简化的生物激励模型：

$\stackrel{\·}{V}=-AV+(B-V)S{\left(t\right)}^{+}-(D+V)S{\left(t\right)}^{-}---\left(8\right)$

其中，V为神经元的兴奋度，A,B及D分别为神经元兴奋度的衰减率、上限及下限。S⁺与S-为神经元兴奋性及抑制性输入。

进一步将式(8)表示为多路神经元的形式：

${\stackrel{\·}{V}}_i=-{\mathrm{AV}}_i+(B-V_i)f\left(e_{\mathrm{\Δ}}\right)-(D+V_i)g\left(e_{\mathrm{\Δ}}\right)---\left(9\right)$

其中f(e_Δ)＝max(e_Δ，0),g(e_Δ)＝max(-e_Δ，0)；生物激励模型在任意输入情况下的输出的误差项总是保持在[-D，B]范围内；e_i为生物激励模型的输入，即：当前的位置误差，e_Δ＝0-e_i为当前的位置误差的误差，V_i为生物激励模型的输出，即：新的位置误差，i＝ζ,η,ξ，ψ：

调整参数A、B和D共同作用决定UUV误差收敛的速度，即UUV跟踪轨迹收敛的速度。A值越小，UUV渐进期望轨迹用的时间越短，速度变化的范围相对越大；B和D与A有相似影响，但是值B和D的值越小，UUV渐进期望轨迹用的时间越长，速度变化的范围相对越大；

UUV虚拟速度控制策略生成的虚拟速度中的位置误差项得到平滑连续处理：。根据所述新的位置误差对步骤二中下一步的虚拟速度进行处理，获得平滑连续的虚拟速度的方法为：将新的位置误差取代下一步的虚拟速度中的位置误差项，从而得到平滑连续的虚拟速度。平滑连续的虚拟速度为：

$U_c^{\′}=\left[\begin{array}{c}k(V_{\mathrm{\ξ}}cos\mathrm{\ψ}+V_{\mathrm{\η}}sin\mathrm{\ψ})+(u_d{\mathrm{cosV}}_{\mathrm{\ψ}}-v_d\sin V_{\mathrm{\ψ}})\\ k(-V_{\mathrm{\ξ}}sin\mathrm{\ψ}+V_{\mathrm{\η}}cos\mathrm{\ψ})+(u_d\sin V_{\mathrm{\ψ}}+v_d\cos V_{\mathrm{\ψ}})\\ w_d+k_z{V}_{\mathrm{\ζ}}\\ r_d+k_{\mathrm{\ψ}}{V}_{\mathrm{\ψ}}\end{array}\right]---\left(10\right)$

UUV根据运动规划与实际位置误差e，在速度控制策略基础上设计UUV虚拟速度，由于虚拟速度受到误差影响，在海流干扰下该虚拟速度会在一定范围内波动，造成PID速度控制器频繁响应，从而导致UUV实际航速出现高频率波动，增加了UUV自身噪声，减少了推进器的使用寿命。为解决该问题，在虚拟速度设计中引入生物激励模型，利用该模型的连续平滑且有界的输出特性设计出平滑连续的虚拟速度U′_c；

步骤四：将步骤三生成的虚拟速度、海流速度与UUV当前实际航速相减后获得速度误差作为PID速度控制器的输入，PID速度控制器将输出作用在UUV模型上，得到UUV下一步的实际速度；

步骤五：将步骤四得到的UUV下一步的实际速度从艇体坐标系{B}下转换为固定坐标系{E}下，再积分，得到UUV的下一步的实际位置，根据得到实际位置对UUV进行控制；

步骤六：将UUV的实际位置与UUV运动规划输出的期望位置相比较，得到的位置误差项作为下一步步骤三中生物激励模型的位置误差输入，转入步骤三。

Claims (5)

1.一种基于生物激励模型的UUV虚拟速度控制方法，其特征在于，所述方法包括如下步骤：

步骤一：根据运动学模型建立欠驱动UUV三维空间运动时的速度非线性方程，得到UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系；

步骤二：根据步骤一得到的UUV速度间的关系,获得虚拟速度；

步骤三：将当前的位置误差输入至生物激励模型进行平滑连续处理，获得新的位置误差，根据所述新的位置误差对步骤二中的虚拟速度进行处理，获得平滑连续的虚拟速度；

步骤四：将步骤三生成的虚拟速度、海流速度与UUV当前实际航速相减后获得速度误差作为PID速度控制器的输入，PID速度控制器将输出作用在UUV模型上，得到UUV下一步的实际速度；

步骤五：将步骤四得到的UUV下一步的实际速度从艇体坐标系{B}下转换为固定坐标系{E}下，再积分，得到UUV的下一步的实际位置，根据得到实际位置对UUV进行控制；

步骤六：将UUV的实际位置与UUV运动规划输出的期望位置相比较，得到的位置误差项作为下一步步骤三中生物激励模型的位置误差输入，转入步骤三。

2.根据权利要求1所述的基于生物激励模型的UUV虚拟速度控制方法，其特征在于，所述步骤一中，UUV在固定坐标系{E}下与艇体坐标系{B}下速度间的关系为：

其中，u、v和w分别为UUV在艇体坐标系{B}下三个线速度分量；r为UUV在艇体坐标系{B}下与艏向角对应的角速度分量，ψ为艏向角，ψ_d为期望艏向角；ξ、η和ζ分别为UUV在固定坐标系{E}下三个位置分量，UUV当前运动位置η＝[ξηζ]^T，η_d＝[ξ_dη_dζ_d]^T为UUV期望运动位置；UUV的位置误差为e＝[e_ξe_ηe_ζe_ψ]^T，e_ξ、e_η和e_ζ分别表示在ξ、η和ζ三个位置上的误差，e_ψ为艏向角误差，˙表示求导。

3.根据权利要求2所述的基于生物激励模型的UUV虚拟速度控制方法，其特征在于，所述二中，根据步骤一得到的UUV速度间的关系，利用反步法获得虚拟速度：

其中，k、k_ζ和k_ψ均为大于0的常数，U_d＝[u_dv_dw_dr_d]^T为UUV在艇体坐标系{B}下的期望速度。

4.根据权利要求1所述的基于生物激励模型的UUV虚拟速度控制方法，其特征在于，所述步骤三中，根据所述新的位置误差对步骤二中下一步的虚拟速度进行处理，获得平滑连续的虚拟速度的方法为：

将新的位置误差取代下一步的虚拟速度中的位置误差项，从而得到平滑连续的虚拟速度。

5.根据权利要求1或4所述的基于生物激励模型的UUV虚拟速度控制方法，其特征在于，所述生物激励模型为：

其中f(e_Δ)＝max(e_Δ,0),g(e_Δ)＝max(-e_Δ,0)；生物激励模型在任意输入情况下的输出的误差项总是保持在[-D，B]范围内；e_i为生物激励模型的输入，即：当前的位置误差，e_Δ＝0-e_i为当前的位置误差的误差，V_i为生物激励模型的输出，即：新的位置误差，i＝ζ,η,ξ，ψ：

A、B和D分别为生物激励模型中的参数；

平滑连续的虚拟速度U′_c：