基于Serret-Frenet坐标系下的跟踪误差方程
前言
本文引入Serret-Frenet坐标系,并在此坐标系下的建立AUV路径跟踪的误差方程。SF坐标系的引用使得AUV的运动与相关的路径参数之间的关系更加直观,建立的误差动态系统更具灵活性。
一、引入SF坐标系的原因
实际上,是SF坐标系的引用的虚拟向导AUV解决了原本AUV路径跟踪问题中存在的严格初始条件约束问题,即对AUV初始条件的约束:AUV的初始位置误差必须小于路径中存在的最小的曲率半径。
路径上的虚拟参考点(SF坐标系的坐标原点)并非是期望路径上距离AUV最近的点,如此便放宽了控制器设计的初始条件限制,虚拟参考点的选择是根据一个方便定义的控制率演化,进而有效地产生一个额外的控制器设计参数(额外的自由度)。
简单来说,一方面SF坐标系的应用大大简化了AUV控制器的设计;另一方面,虚拟向导AUV的应用很好地避免了传统方法存在的奇异值问题。
二、一些假设
对 AUV 的三维空间运动作出以下说明:
(1)期望的路径为确定的参数化曲线;且虚拟向导设计参数设定为正且存在上确界;
(2)AUV纵向控制性能良好,忽略横摇角和横向速度的影响,将六自由度模型简化为五自由度模型;
(3)欠驱动 AUV 的纵向速度控制良好,且其纵向速度始终为正,并且不存在倒车航行的情况;
三、误差方程推导
误差方程包含两个部分,一是艏向角和纵倾角的误差,二是三个方向上的位置误差。空间路径跟踪示意图如下图所示:
1. 艏向角和纵倾角误差
AUV的航迹角表示为
γ
Q
=
ψ
+
β
{\gamma_Q}=\psi+\beta
γQ=ψ+β,潜浮角为
χ
Q
=
θ
+
α
{\chi_Q}=\theta+\alpha
χQ=θ+α,对的等式两侧进行求导,同时结合AUV的运动学方程:
θ
˙
=
q
\dot{\theta}=q
θ˙=q
ψ
˙
=
q
/
c
o
s
θ
\dot{\psi}=q/cos\theta
ψ˙=q/cosθ
带入可得:
χ
Q
˙
=
q
+
α
˙
\dot{\chi_Q}=q+\dot{\alpha}
χQ˙=q+α˙
γ
Q
˙
=
r
/
c
o
s
θ
+
β
˙
\dot{\gamma_Q}=r/cos\theta+\dot{\beta}
γQ˙=r/cosθ+β˙
ψ
e
\psi_{e}
ψe和
θ
e
\theta_{e}
θe分别表示两个坐标系之间的夹角,具体表达式为:
{
ψ
e
=
γ
Q
−
γ
F
θ
e
=
χ
Q
−
χ
F
\begin{cases}\psi_{e} =\gamma_{Q}-\gamma_{F}&\\\theta_{e}=\chi_{Q} -\chi_{F}& & \end{cases}
{ψe=γQ−γFθe=χQ−χF上式中
γ
\gamma
γ和
χ
\chi
χ分别为AUV在两个坐标系下的航迹角和潜浮角,在
{
F
}
\{F\}
{F}坐标系下的虚拟AUV的期望角速度分别表示为
c
1
(
s
)
s
˙
c_{1}(s)\dot{s}
c1(s)s˙和
c
2
(
s
)
s
˙
c_{2}(s)\dot{s}
c2(s)s˙;
则:
{
ψ
˙
e
=
r
/
c
o
s
θ
+
β
˙
−
c
1
(
s
)
s
˙
θ
˙
e
=
q
+
α
˙
−
c
2
(
s
)
s
˙
\begin{cases}\dot{\psi}_{e}=r/cos\theta+\dot\beta -c_{1}(s)\dot{s}&\\\dot{\theta}_{e}=q+ \dot{\alpha}-c_{2}(s)\dot{s}&&\end{cases}
{ψ˙e=r/cosθ+β˙−c1(s)s˙θ˙e=q+α˙−c2(s)s˙
2. 各方向位置误差
x,y,z三个方向上的误差公式由坐标系 { W } \{W\} {W}和坐标系 { F } \{F\} {F}之间的向量转换公式获得: R ⃗ ( ψ e , θ e ) v ⃗ w = v ⃗ F + d d ⃗ d t + W ⃗ F × d ⃗ \vec{R}(\psi_{e},\theta_{e})\vec{v}_{w}=\vec{v}_{F}+\frac{d\vec{d}}{dt}+\vec{W}_{F}\times \vec{d} R(ψe,θe)vw=vF+dtdd+WF×d
2.1 { W } \{W\} {W}坐标系和 { F } \{F\} {F}坐标系
上述中的
{
W
}
\{W\}
{W}坐标系,是将定系
{
I
}
\{I\}
{I}坐标系同时沿
ζ
\zeta
ζ轴旋转漂角
β
\beta
β,沿
ξ
\xi
ξ轴旋转冲角
α
\alpha
α,并将原点与AUV重心重合得到的,易知
x
W
x_{W}
xW的方向与重心Q的和速度方向一致。
{
F
}
\{F\}
{F}即Serret-Frenet坐标系,
{
F
}
\{F\}
{F}坐标系可视为将定坐标系分别沿
ζ
\zeta
ζ轴旋转
ψ
F
\psi_{F}
ψF,沿轴
η
\eta
η旋转
θ
F
\theta_{F}
θF ,并将定系原点平移至与P点重合得到的。
其中:
2.2转换公式解释
(1)
R
⃗
(
ψ
e
,
θ
e
)
\vec{R}(\psi_{e},\theta_{e})
R(ψe,θe)是坐标系
{
W
}
\{W\}
{W}和坐标系
{
F
}
\{F\}
{F}之间的转换矩阵;
R
⃗
(
ψ
e
,
θ
e
)
\vec{R}(\psi_{e},\theta_{e})
R(ψe,θe)表示为:
R
⃗
(
ψ
e
,
θ
e
)
=
[
c
o
s
ψ
e
c
o
s
θ
e
−
s
i
n
ψ
e
c
o
s
ψ
e
s
i
n
θ
e
s
i
n
ψ
e
c
o
s
θ
e
c
o
s
ψ
e
s
i
n
ψ
e
s
i
n
θ
e
−
s
i
n
θ
e
0
c
o
s
θ
e
]
\vec{R}(\psi_{e},\theta_{e})=\begin{bmatrix}cos\psi_{e}cos\theta_{e}&-sin\psi_{e}&cos\psi_{e}sin\theta_{e}\\sin\psi_{e} cos\theta_{e}& cos\psi_{e}&sin\psi_{e}sin\theta_{e}\\-sin\theta_{e}&0&cos\theta_{e} \end{bmatrix}
R(ψe,θe)=
cosψecosθesinψecosθe−sinθe−sinψecosψe0cosψesinθesinψesinθecosθe
(2)
v
⃗
w
\vec{v}_{w}
vw是AUV在
{
W
}
\{W\}
{W}系下的速度,表示为:
v
⃗
w
=
[
U
B
,
0
,
0
]
T
\vec{v}_{w}=[U_{B},0,0]^{T}
vw=[UB,0,0]T;
(3)
v
⃗
F
\vec{v}_{F}
vF是AUV在
{
F
}
\{F\}
{F}系下的速度,表示为:
v
⃗
F
=
[
s
˙
,
0
,
0
]
T
\vec{v}_{F}=[\dot{s},0,0]^{T}
vF=[s˙,0,0]T;
(4)
d
⃗
\vec{d}
d表示为定系下路径跟踪误差,表示为:
d
⃗
=
[
x
e
,
y
e
,
z
e
]
T
\vec{d}=[x_{e},y_{e},z_{e}]^{T}
d=[xe,ye,ze]T;
(5)
W
⃗
F
\vec{W}_{F}
WF表示
{
F
}
\{F\}
{F}中的旋转角速度,表示为:
W
⃗
F
=
[
0
,
c
2
(
s
)
s
˙
,
c
1
(
s
)
s
˙
]
T
\vec{W}_{F}=[0,c_{2}(s)\dot{s},c_{1}(s)\dot{s}]^{T}
WF=[0,c2(s)s˙,c1(s)s˙]T,定义
c
1
(
s
)
c_{1}(s)
c1(s)与
c
2
(
s
)
c_{2}(s)
c2(s)分别为期望路径曲线的曲率与,它们是关于s连续可导且有界的;
2.3转换公式展开
将转换公式表示为:
[
c
o
s
ψ
e
c
o
s
θ
e
−
s
i
n
ψ
e
c
o
s
ψ
e
s
i
n
θ
e
s
i
n
ψ
e
c
o
s
θ
e
c
o
s
ψ
e
s
i
n
ψ
e
s
i
n
θ
e
−
s
i
n
θ
e
0
c
o
s
θ
e
]
[
U
B
0
0
]
=
∗
[
s
˙
0
0
]
+
[
x
˙
e
y
˙
e
z
˙
e
]
+
[
0
c
˙
2
(
s
)
s
˙
c
˙
1
(
s
)
s
˙
]
×
[
x
e
y
e
z
e
]
\begin{bmatrix}cos\psi_{e}cos\theta_{e}&-sin\psi_{e}&cos\psi_{e}sin\theta_{e}\\sin\psi_{e} cos\theta_{e}& cos\psi_{e}&sin\psi_{e}sin\theta_{e}\\-sin\theta_{e}&0&cos\theta_{e} \end{bmatrix} \begin{bmatrix}U_{B} \\0 \\0\end{bmatrix} =*\begin{bmatrix}\dot{s} \\0 \\0\end{bmatrix} + \begin{bmatrix}\dot{x}_{e} \\\dot{y}_{e} \\\dot{z}_{e}\end{bmatrix} + \begin{bmatrix}0 \\\dot{c}_{2}(s)\dot{s} \\\dot{c}_{1}(s)\dot{s}\end{bmatrix} \times \begin{bmatrix}x_{e} \\y_{e} \\z_{e}\end{bmatrix}
cosψecosθesinψecosθe−sinθe−sinψecosψe0cosψesinθesinψesinθecosθe
UB00
=∗
s˙00
+
x˙ey˙ez˙e
+
0c˙2(s)s˙c˙1(s)s˙
×
xeyeze
展开得到:
{ 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 \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{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˙−s˙y˙e=−xec1(s)s˙+UBsinψecosθez˙e=xec2(s)s˙−UBsinθe
2.4路径跟踪误差
结合艏向角和纵倾角误差以及位置误差,得到AUV的路径跟踪误差方程为:
{
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
ψ
˙
e
=
r
/
c
o
s
θ
+
β
˙
−
c
1
(
s
)
s
˙
θ
˙
e
=
q
+
α
˙
−
c
2
(
s
)
s
˙
\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{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} &\\ \dot{\psi}_{e}=r/cos\theta+\dot\beta -c_{1}(s)\dot{s} &\\ \dot{\theta}_{e}=q+ \dot{\alpha}-c_{2}(s)\dot{s} &&\end{cases}
⎩
⎨
⎧x˙e=yec1(s)s˙+UBcosψecosθe−zec2(s)s˙−s˙y˙e=−xec1(s)s˙+UBsinψecosθez˙e=xec2(s)s˙−UBsinθeψ˙e=r/cosθ+β˙−c1(s)s˙θ˙e=q+α˙−c2(s)s˙
上式即为较为通用的AUV基于SF坐标系的路径跟踪误差方程。