前言
针对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θe−zec2(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θe−zec2(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θe−s˙)+yeUBsinψecosθe−zeUBsinθ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θe−zeUBsinθe,ks>0现在结合AUV对上式进行分析:
假设AUV在运动过程中在期望路径的左侧,由右手定则可知
y
e
≤
0
y_{e}\leq0
ye≤0,此时AUV在水平方向上需沿顺时针转动,因此
ψ
e
≤
0
\psi_{e}\leq0
ψe≤0,结合
ψ
e
\psi_{e}
ψe的取值范围,知
ψ
e
∈
[
−
π
,
0
]
\psi_{e}\in[-\pi,0]
ψe∈[−π,0],则
s
i
n
ψ
e
≤
0
sin\psi_{e}\leq0
sinψe≤0,因而
y
e
s
i
n
ψ
e
≤
0
y_{e}sin\psi_{e}\leq0
yesinψe≤0又知
θ
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θe≤0;
同理当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θe≤0,因此此项始终小于等于零。
当AUV运动过程中在期望路径的下侧时,
z
e
≥
0
z_{e}\geq0
ze≥0,此时在垂直方向上需逆时针转动,有
θ
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θe≥0,则加上符号后此项恒小于等于零。
综上得到
V
˙
≤
0
\dot{V}\leq0
V˙≤0。