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

基于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 的纵向速度控制良好,且其纵向速度始终为正,并且不存在倒车航行的情况;

三、误差方程推导

误差方程包含两个部分,一是艏向角和纵倾角的误差,二是三个方向上的位置误差。空间路径跟踪示意图如下图所示:
基于Serret-Frenet坐标系下的跟踪误差方程_第1张图片

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θev w=v F+dtdd +W F×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θesinθesinψecosψe0cosψesinθesinψesinθecosθe
(2) v ⃗ w \vec{v}_{w} v w是AUV在 { W } \{W\} {W}系下的速度,表示为: v ⃗ w = [ U B , 0 , 0 ] T \vec{v}_{w}=[U_{B},0,0]^{T} v w=[UB00]T
(3) v ⃗ F \vec{v}_{F} v F是AUV在 { F } \{F\} {F}系下的速度,表示为: v ⃗ F = [ s ˙ , 0 , 0 ] T \vec{v}_{F}=[\dot{s},0,0]^{T} v F=[s˙00]T
(4) d ⃗ \vec{d} d 表示为定系下路径跟踪误差,表示为: d ⃗ = [ x e , y e , z e ] T \vec{d}=[x_{e},y_{e},z_{e}]^{T} d =[xeyeze]T
(5) W ⃗ F \vec{W}_{F} W F表示 { 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} W F=[0c2(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θesinθesinψ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 ˙ 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θezec2(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 ˙ 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{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θezec2(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坐标系的路径跟踪误差方程。

你可能感兴趣的:(路径跟踪,AUV,运动控制,算法)