基于运动/载体坐标系下的跟踪误差方程

在路径跟踪中，需要跟踪的是虚拟AUV；在轨迹跟踪中，需要跟踪的是位置随时间变化的点。无论是路径跟踪还是轨迹跟踪，实质上都是对一个移动的点进行跟踪，有相同之处。本文进行AUV在动系下的跟踪误差方程的推导，类似于在SF坐标系下的跟踪误差方程，关于在SF系下的跟踪误差方程，见https://blog.csdn.net/dooglocool/article/details/128681600?spm=1001.2014.3001.5501。

1.不同坐标系下的跟踪误差方程推导分析

虽然在不同坐标系下的跟踪误差方程有所不同，但其表达的本质相同。不同坐标系下的误差方程的推导，都有其共同点：就是跟踪误差结合坐标转换矩阵相关知识进行转化。
在SF系下的跟踪误差方程的推导中，用到的是 { W } \{W\} {W}系和 { S F } \{SF\} {SF}系的转换。
而在动系下的跟踪误差方程推导中，用到的是动系和定系的坐标转换（主要），还涉及到动系和SF系的坐标转换。

2.动系下的跟踪误差方程推导

定义目标点在定系下的位置坐标为${\eta }_R=[x_R,y_R,z_R{]}^T$,AUV在定系下的位置坐标为$\eta =[x,y,z{]}^T$
定系下的跟踪误差向量表示为：
$${\eta }_e^n=\left[\begin{array}{c}x-x_R\\ y-y_R\\ z-z_R\end{array}\right]$$
此时在动系下的位置误差向量表示为
$$ϵ=\left[\begin{array}{c}{x}_e\\ y_e\\ z_e\end{array}\right]=J_1({\eta }_2{)}^T{\eta }_e^n$$
$J_1({\eta }_2)$为动系到定系的转换矩阵，上式表示将定系下的跟踪误差向量通过转换矩阵转换为动系下的跟踪误差向量。
对上式进行求导，得到跟踪误差方程
$$\dot{\eta }={\dot{J}}_1^T({\eta }_2){\eta }_e^n+J_1^T({\eta }_2){\dot{\eta }}_e^n$$
下面需要用到旋转矩阵求导的知识，见https://blog.csdn.net/dooglocool/article/details/131954280?spm=1001.2014.3001.5501
即
$${\dot{J}}_1({\eta }_2)=J_1({\eta }_2)S({\omega }_{nb}^b)$$
其中$$S({\omega }_{nb}^b)=\left[\begin{array}{ccc}0& -r& q\\ r& 0& 0\\ -q& 0& 0\end{array}\right]$$
此时的误差方程变为
$$\dot{ϵ}=S^T({\omega }_{nb}^b)J_1^T({\eta }_2){\eta }_e^n+J_1^T({\eta }_2){\dot{\eta }}_e^n$$
进一步化简得到
$$\dot{ϵ}=S^T({\omega }_{nb}^b)ϵ+J_1^T({\eta }_2){\dot{\eta }}_e^n$$
$$\dot{ϵ}=S^T({\omega }_{nb}^b)ϵ+J_1^T({\eta }_2)(\dot{\eta }-{\dot{\eta }}_R)$$
$$\dot{ϵ}=S^T({\omega }_{nb}^b)ϵ+J_1^T({\eta }_2)J_1({\eta }_2)\dot{\eta }-J_1^T({\eta }_2){\dot{\eta }}_R$$
$$\dot{ϵ}=S^T({\omega }_{nb}^b)ϵ+\nu -J_1^T({\eta }_2){\dot{\eta }}_R$$
等式右侧最后一项意为动系下的目标点的速度，等价于将目标点在目标轨迹（SF系）上的速度转换到动系下的速度，而目标点在轨迹上的速度只有纵向速度。表示为
$$\left[\begin{array}{c}{u}_r\\ 0\\ 0\end{array}\right]$$此处的$u_r$也可以理解为目标点在定系下的和速度。
SF系到动系的转换矩阵表示为

则误差方程进一步变为
$$\dot{ϵ}=S^T({\omega }_{nb}^b)ϵ+\nu -J({\psi }_e,{\theta }_e){\nu }_F$$展开得到
$$\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{z}}_e\end{array}\right]=\left[\begin{array}{c}r{y}_e-q{z}_e\\ -r{x}_e\\ q{x}_e\end{array}\right]+\left[\begin{array}{c}u\\ v\\ w\end{array}\right]-J({\psi }_e,{\theta }_e)\left[\begin{array}{c}{u}_r\\ 0\\ 0\end{array}\right]$$展开便得到在动系下的跟踪误差方程