基于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 } \{F\} {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 } \{W\} {W}和坐标系 { F } \{F\} {F}之间的向量转换公式获得：$$\vec{R}（{\psi }_e，{\theta }_e）{\vec{v}}_w={\vec{v}}_F+\frac{d\vec{d}}{dt}+{\vec{W}}_F\times \vec{d}$$

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

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

2.2转换公式解释

（1）$\vec{R}（{\psi }_e，{\theta }_e）$是坐标系 { W } \{W\} {W}和坐标系 { F } \{F\} {F}之间的转换矩阵；$\vec{R}（{\psi }_e，{\theta }_e）$表示为：
$$\vec{R}（{\psi }_e，{\theta }_e）=\left[\begin{array}{ccc}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{array}\right]$$
（2）${\vec{v}}_w$是AUV在 { W } \{W\} {W}系下的速度，表示为：${\vec{v}}_w=[U_B，0，0{]}^T$；
（3）${\vec{v}}_F$是AUV在 { F } \{F\} {F}系下的速度，表示为：${\vec{v}}_F=[\dot{s}，0，0{]}^T$；
（4）$\vec{d}$表示为定系下路径跟踪误差，表示为：$\vec{d}=[x_e，y_e，z_e{]}^T$；
（5）${\vec{W}}_F$表示 { F } \{F\} {F}中的旋转角速度，表示为：${\vec{W}}_F=[0，c_2(s)\dot{s}，c_1(s)\dot{s}{]}^T$，定义$c_1(s)$与$c_2(s)$分别为期望路径曲线的曲率与，它们是关于s连续可导且有界的；

2.3转换公式展开

将转换公式表示为:
$$\left[\begin{array}{ccc}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{array}\right]\left[\begin{array}{c}{U}_B\\ 0\\ 0\end{array}\right]=\ast \left[\begin{array}{c}\dot{s}\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{z}}_e\end{array}\right]+\left[\begin{array}{c}0\\ {\dot{c}}_2(s)\dot{s}\\ {\dot{c}}_1(s)\dot{s}\end{array}\right]\times \left[\begin{array}{c}{x}_e\\ y_e\\ z_e\end{array}\right]$$
展开得到：

$$\{\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{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{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坐标系的路径跟踪误差方程。