路径跟踪 神经网络 路径跟踪误差

基于Serret-Frenet坐标系下的跟踪误差方程

• 前言
• 一、引入SF坐标系的原因
• 二、一些假设
• 三、误差方程推导

• 1. 艏向角和纵倾角误差
• 2. 各方向位置误差

• 2.1 { W } \{W\} {W}坐标系和 { F } \{F\} {F}坐标系
• 2.2转换公式解释
• 2.3转换公式展开
• 2.4路径跟踪误差

前言

本文引入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{cases}\psi_{e} =\gamma_{Q}-\gamma_{F}&\\\theta_{e}=\chi_{Q} -\chi_{F}& & \end{cases}$上式中$\gamma$和$\chi$分别为AUV在两个坐标系下的航迹角和潜浮角，在$\{F\}$坐标系下的虚拟AUV的期望角速度分别表示为$c_{1}(s)\dot{s}$和$c_{2}(s)\dot{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}$

2. 各方向位置误差

x,y,z三个方向上的误差公式由坐标系$\{W\}$和坐标系$\{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\}$坐标系和$\{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）$\vec{R}（\psi_{e}，\theta_{e}）$是坐标系$\{W\}$和坐标系$\{F\}$之间的转换矩阵；$\vec{R}（\psi_{e}，\theta_{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}$
（2）$\vec{v}_{w}$是AUV在$\{W\}$系下的速度，表示为：$\vec{v}_{w}=[U_{B}，0，0]^{T}$；
（3）$\vec{v}_{F}$是AUV在$\{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\}$中的旋转角速度，表示为：$\vec{W}_{F}=[0，c_{2}(s)\dot{s}，c_{1}(s)\dot{s}]^{T}$，定义$c_{1}(s)$与$c_{2}(s)$分别为期望路径曲线的曲率与，它们是关于s连续可导且有界的；

2.3转换公式展开

将转换公式表示为:
$\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}$
展开得到：

$\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}$

2.4路径跟踪误差

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