虚拟向导AUV的速度推算

文章目录

  • 前言
  • 一、虚拟向导AUV速度推导
  • 二、李氏函数一阶导负定性分析

前言

针对AUV的路径跟踪控制问题,引入了SF坐标系和虚拟向导,本文基于李雅普诺夫方法进行虚拟向导AUV的速度的推导,得到 s ˙ \dot{s} s˙,进而得到期望路径参数 s s s,进而根据需要生成期望路径。

一、虚拟向导AUV速度推导

首先根据AUV的位置误差,设计的李雅普诺夫能量函数为:
V = 1 2 x e 2 + 1 2 y e 2 + 1 2 z e 2 V=\frac{1}{2}x_{e}^2 +\frac{1}{2}y_{e}^2+\frac{1}{2}z_{e}^2 V=21xe2+21ye2+21ze2求一阶导可得: V ˙ = x e x ˙ e + y e y ˙ e + z e z ˙ e \dot{V}=x_{e}\dot{x}_{e}+y_{e}\dot{y}_{e}+z_{e}\dot{z}_{e} V˙=xex˙e+yey˙e+zez˙e基于SF坐标系的路径跟踪误差方程为: { x ˙ e = y e c 1 ( s ) s ˙ + U B c o s ψ e c o s θ e − z e c 2 ( s ) s ˙ y ˙ e = − x e c 1 ( s ) s ˙ + U B s i n ψ e c o s θ e z ˙ e = x e c 2 ( s ) s ˙ − U B s i n θ e \begin{cases}\dot{x}_{e} =y_{e}c_{1}(s)\dot{s}+U_{B}cos\psi_{e}cos\theta_{e}-z_{e}c_{2}(s)\dot{s}&\\\dot{y}_{e}= -x_{e}c_{1}(s)\dot{s}+U_{B}sin\psi_{e}cos\theta_{e}&\\\dot{z}_{e}=x_{e}c_{2}(s)\dot{s}-U_{B}sin\theta_{e}&&\end{cases} x˙e=yec1(s)s˙+UBcosψecosθezec2(s)s˙y˙e=xec1(s)s˙+UBsinψecosθez˙e=xec2(s)s˙UBsinθe带入可得:
V ˙ = x e ( y e c 1 ( s ) s ˙ + U B c o s ψ e c o s θ e − z e c 2 ( s ) s ˙ − s ˙ ) + y e ( − x e c 1 ( s ) s ˙ + U B s i n ψ e c o s θ e ) + z e ( x e c 2 ( s ) s ˙ − U B s i n θ e ) \dot{V}=x_{e}(y_{e}c_{1}(s)\dot{s}+U_{B}cos\psi_{e} cos\theta_{e}-z_{e}c_{2}(s)\dot{s}-\dot{s}) \\ +y_{e}(-x_{e}c_{1}(s)\dot{s}+U_{B}sin\psi_{e}cos\theta_{e})+z_{e}(x_{e}c_{2}(s)\dot{s}-U_{B}sin\theta_{e}) V˙=xe(yec1(s)s˙+UBcosψecosθezec2(s)s˙s˙)+ye(xec1(s)s˙+UBsinψecosθe)+ze(xec2(s)s˙UBsinθe)化简可得:
V ˙ = x e ( U B c o s ψ e c o s θ e − s ˙ ) + y e U B s i n ψ e c o s θ e − z e U B s i n θ e \dot{V}=x_{e}(U_{B}cos\psi_{e}cos\theta_{e}-\dot{s})+y_{e}U_{B}sin\psi_{e}cos\theta_{e}-z_{e}U_{B}sin\theta_{e} V˙=xe(UBcosψecosθes˙)+yeUBsinψecosθezeUBsinθe设计的虚拟向导AUV的运动速度为:
s ˙ = U B c o s ψ e c o s θ e + k s x e , k s > 0 \dot{s}=U_{B}cos\psi_{e}cos\theta_{e}+k_{s}x_{e},k_{s}>0 s˙=UBcosψecosθe+ksxe,ks>0

二、李氏函数一阶导负定性分析

将设计的AUV虚拟速度带入李氏函数的一阶导得到:
V ˙ = − k s x ˙ e 2 + y e U B s i n ψ e c o s θ e − z e U B s i n θ e , k s > 0 \dot{V}=-k_{s}\dot{x}_{e}^2+y_{e}U_{B}sin\psi_{e}cos\theta_{e}-z_{e}U_{B}sin\theta_{e},k_{s}>0 V˙=ksx˙e2+yeUBsinψecosθezeUBsinθe,ks>0现在结合AUV对上式进行分析:
假设AUV在运动过程中在期望路径的左侧,由右手定则可知 y e ≤ 0 y_{e}\leq0 ye0,此时AUV在水平方向上需沿顺时针转动,因此 ψ e ≤ 0 \psi_{e}\leq0 ψe0,结合 ψ e \psi_{e} ψe的取值范围,知 ψ e ∈ [ − π , 0 ] \psi_{e}\in[-\pi,0] ψe[π,0],则 s i n ψ e ≤ 0 sin\psi_{e}\leq0 sinψe0,因而 y e s i n ψ e ≤ 0 y_{e}sin\psi_{e}\leq0 yesinψe0又知 θ e ∈ ( − π 2 , π 2 ) \theta_{e}\in(-\frac{\pi}{2},\frac{\pi}{2}) θe(2π,2π),得到 c o s θ e > 0 cos\theta_{e}>0 cosθe>0,所以有 y e U B s i n ψ e c o s θ e ≤ 0 y_{e}U_{B}sin\psi_{e}cos\theta_{e}\leq0 yeUBsinψecosθe0;
同理当AUV在期望路径的右侧时,也能得到 y e U B s i n ψ e c o s θ e ≤ 0 y_{e}U_{B}sin\psi_{e}cos\theta_{e}\leq0 yeUBsinψecosθe0,因此此项始终小于等于零。
当AUV运动过程中在期望路径的下侧时, z e ≥ 0 z_{e}\geq0 ze0,此时在垂直方向上需逆时针转动,有 θ e > 0 \theta_{e}>0 θe>0,结合 θ e \theta_{e} θe的取值范围,知 θ e ∈ ( 0 , π 2 ) \theta_{e}\in(0,\frac{\pi}{2}) θe(0,2π),则 s i n θ e > 0 sin\theta_{e}>0 sinθe>0,因而 z e U B s i n θ e ≥ 0 z_{e}U_{B}sin\theta_{e}\geq0 zeUBsinθe0,则加上符号后此项恒小于等于零。
综上得到 V ˙ ≤ 0 \dot{V}\leq0 V˙0

你可能感兴趣的:(AUV,路径跟踪,运动控制,算法)