基于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的航迹角表示为${\gamma }_Q=\psi +\beta$，潜浮角为${\chi }_Q=\theta +\alpha$,对的等式两侧进行求导,同时结合AUV的运动学方程：
$$\dot{\theta }=q$$$$\dot{\psi }=q/cos\theta$$
带入可得：
$$\dot{{\chi }_Q}=q+\dot{\alpha }$$$$\dot{{\gamma }_Q}=r/cos\theta +\dot{\beta }$$
${\psi }_e$和${\theta }_e$分别表示两个坐标系之间的夹角，具体表达式为：
$$\{\begin{array}{llc}{\psi }_e={\gamma }_Q-{\gamma }_F& & \\ {\theta }_e={\chi }_Q-{\chi }_F& & \end{array}$$上式中$\gamma$和$\chi$分别为AUV在两个坐标系下的航迹角和潜浮角，在$\{F\}$坐标系下的虚拟AUV的期望角速度分别表示为$c_1(s)\dot{s}$和$c_2(s)\dot{s}$；
则：$$\{\begin{array}{llc}{\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{array}$$

2. 各方向位置误差

x,y,z三个方向上的误差公式由坐标系$\{W\}$和坐标系$\{F\}$之间的向量转换公式获得：$$R（{\psi }_e，{\theta }_e）v_w=v_F+\frac{dd}{dt}+W_F\times d$$

2.1$\{W\}$坐标系和$\{F\}$坐标系

上述中的$\{W\}$坐标系，是将定系$\{I\}$坐标系同时沿$\zeta$轴旋转漂角$\beta$，沿$\xi$轴旋转冲角$\alpha$，并将原点与AUV重心重合得到的，易知$x_W$的方向与重心Q的和速度方向一致。$\{F\}$即Serret-Frenet坐标系，$\{F\}$坐标系可视为将定坐标系分别沿$\zeta$轴旋转${\psi }_F$，沿轴$\eta$旋转${\theta }_F$ ，并将定系原点平移至与P点重合得到的。
其中：

2.2转换公式解释

（1）$R（{\psi }_e，{\theta }_e）$是坐标系$\{W\}$和坐标系$\{F\}$之间的转换矩阵；$R（{\psi }_e，{\theta }_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ψe​cosθe​sinψe​cosθe​−sinθe​​−sinψe​cosψe​0​cosψe​sinθe​sinψe​sinθe​cosθe​​ ​
（2）$v_w$是AUV在$\{W\}$系下的速度，表示为：$v_w=[U_B，0，0{]}^T$；
（3）$v_F$是AUV在$\{F\}$系下的速度，表示为：$v_F=[\dot{s}，0，0{]}^T$；
（4）$d$表示为定系下路径跟踪误差，表示为：$d=[x_e，y_e，z_e{]}^T$；
（5）$W_F$表示$\{F\}$中的旋转角速度，表示为：$W_F=[0，c_2(s)\dot{s}，c_1(s)\dot{s}{]}^T$，定义$c_1(s)$与$c_2(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ψe​cosθe​sinψe​cosθe​−sinθe​​−sinψe​cosψe​0​cosψe​sinθe​sinψe​sinθe​cosθe​​ ​∗∗ ​UB​00​ ​=∗∗ ​s˙00​ ​+ ​x˙e​y˙​e​z˙e​​ ​+ ​0c˙2​(s)s˙c˙1​(s)s˙​ ​× ​xe​ye​ze​​ ​
展开得到：

$$\{\begin{array}{llc}{\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{array}$$

2.4路径跟踪误差

结合艏向角和纵倾角误差以及位置误差，得到AUV的路径跟踪误差方程为：
$$\{\begin{array}{llc}{\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& & \\ {\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{array}$$
上式即为较为通用的AUV基于SF坐标系的路径跟踪误差方程。