3维空间目标跟踪的CV,CA,CT动力学模型

类似于二维平面上车辆转动的CV，CA，CT模型。3维空间的CV、CA、CT模型也存在。用于目标跟踪时，需要考虑的角速度、速度、加速度之间的耦合关系更为复杂，所以本博客列举了这些公式。

• 恒定速度模型（Constant Velocity, CV）
• 恒定加速度模型（Constant Acceleration, CA）
• 恒定转弯率和速度幅度模型（Constant Turn Rate and Velocity, CTRV）
• 恒定转率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

本博客突出CT模型，用于空中目标跟踪。

质点运动自然坐标系的分解

考虑质点运动学偏导数与全导数的关系：
$$\begin{gathered} \mathbf{v}=\frac{\mathrm{d}\mathbf{r}}{\mathrm{d}t}=\frac{\partial \mathbf{r}}{\partial t}+\mathbf{\omega }\times \mathbf{r} \\ \mathbf{a}\equiv \frac{\mathrm{d}\mathbf{v}}{\mathrm{d}t}=\frac{\partial \mathbf{v}}{\partial t}+\mathbf{\omega }\times \mathbf{v} \end{gathered}$$

一般的三维平面运动，在自然坐标系下的切向和法向的定义为：

• 速度方向为切向$\mathbf{\tau }$
• 与速度垂直，指向曲线凹陷的一侧为法向$\mathbf{n}$
• 切向与法向张成密切平面

注意这是一个动坐标系，相对惯性系的速度是$\mathbf{v}$，旋转角速度是$\mathbf{w}$（图中为Ω）。

惯性系下的加速度落在密切平面内，可得加速度分解的惠更斯公式
$$\mathbf{a}\equiv \frac{\mathrm{d}\mathbf{v}}{\mathrm{d}t}={\mathbf{a}}_t+{\mathbf{a}}_n$$或者表示为
$$\begin{gathered} {\mathbf{a}}_t=\frac{\partial \mathbf{v}}{\partial t}=a_t\cdot \mathbf{\tau } \\ {\mathbf{a}}_n=\mathbf{\omega }\times \mathbf{v} \end{gathered}$$上图展示了这种现象，也就是在 Span { τ , n } \text{Span}\{\boldsymbol \tau, \boldsymbol n \} Span{τ,n}的正交空间内不存在加速度的分量。切向的单位矢量
$$\mathbf{\tau }={\left[\frac{v_x}{v},\frac{v_y}{v},\frac{v_z}{v}\right]}^{\mathrm{T}},v=\Vert \mathbf{v}\Vert$$

二维平面运动学总结

CT模型，或者一般的二维平面运动，都可写作密切平面内的如下形式：
$$\begin{array}{rl}& \dot{x}(t)=v(t)\cos \varphi (t)\\ & \dot{y}(t)=v(t)\sin \varphi (t)\\ & \dot{v}(t)=a_t(t)\\ & \dot{\varphi }(t)=a_r(t)/v(t)\equiv w\end{array}\text{\hspace{1em}(1)}$$其中$a_n,a_t$分别代表法向加速度与切向加速度，$w$为转弯角速度，$\varphi$为速度方向角$\varphi =\arctan (v_y/v_x)=\arcsin (v_y/v)$。

• $a_n=0,a_t=0$——直线，CV运动
• $a_n=0,a_t\ne 0$——直线，加速运动。若$a_t=C$，CA运动
• $a_n\ne 0,a_t=0$——弧线，匀速率运动。若$a_n=C$，则为CTRV运动
• $a_n\ne 0,a_t\ne 0$——曲线运动。若$a_n=C_1,a_t=C_2$，则为CTRA运动

假设CTRV运动的角速度$w$已知，写到笛卡尔坐标系下，动力学方程成了
$$\begin{array}{rl}& \dot{x}(t)=v_x(t)\\ & \dot{y}(t)=v_y(t)\\ & {\dot{v}}_x(t)=a_x=-w\cdot v_y\\ & {\dot{v}}_y(t)=a_y=+w\cdot v_x\end{array}\text{\hspace{1em}(2)}$$上式和式(1)的关系是：对于CTRV模型，局部加速度$\frac{\partial \mathbf{v}}{\partial t}$为0，则惯性系的加速度$\frac{\mathrm{d}\mathbf{v}}{\mathrm{d}t}=[a_x,a_y]$只由旋转造成，即$\mathbf{a}=\mathbf{\omega }\times \mathbf{v}$，它的大小$a=\sqrt{a_t^2+a_n^2}$.

式(2)中只有1项最终的不确定量，加速度表示为有关$w_w$的白噪声，变换可得到。角速度$w$未知，则写成白噪声或一阶Markov过程。

$$\begin{gathered} \dot{\omega }(t)={\text{w}}_w \\ \dot{\omega }(t)=-\frac{1}{{\tau }_w}\omega (t)+{\text{w}}_w \end{gathered}$$上式与公式(1)-(2)可联立，构成5维的状态空间模型，其噪声是$w_w$. 再将加速度大小合二为一，则CTRA模型是：
$$\begin{array}{rl}& \dot{x}(t)=v(t)\cos \varphi (t)\\ & \dot{y}(t)=v(t)\sin \varphi (t)\\ & \dot{v}(t)=a(t)\\ & \dot{\varphi }(t)=\omega (t)\\ & \dot{\omega }(t)=-\frac{1}{{\tau }_w}\omega (t)+{\text{w}}_w\\ & \dot{a}(t)={\text{w}}_a\end{array}\text{\hspace{1em}(3)}$$其中${\text{w}}_w,{\text{w}}_a$代表角速度大小的噪声、加速度大小的噪声；将上式的部分项建模为随机噪声就成了CTRV（1-5行），CA（1-4行），CV（1-3行）模型，每个模型中没有作为状态量变量的都为0，假设为均值为0的随机值${\text{w}}_k$驱动的。
实际上CA模型和CV模型由于前2项在转弯坐标系中式(1)是非线性形式，而在笛卡尔坐标系内式(2)运动模型是线性的，所以类似方程(1)、(3)的形式一般默认为CT模型，而不用来表示CA、CV运动。

作为式(3)的一种补充，考虑到切向加速度与速度单位矢量$\mathbf{\tau }$方向一致，角速度转化成法向加速度$w=a_n/v$，则二维平面的CTRA模型也写作：
$$\begin{array}{rl}& \dot{x}(t)=v_x(t)\\ & \dot{y}(t)=v_y(t)\\ & {\dot{v}}_x(t)=a_t\cdot \frac{v_x}{v}-\frac{a_n}{v}\cdot v_y\\ & {\dot{v}}_y(t)=a_t\cdot \frac{v_y}{v}+\frac{a_n}{v}\cdot v_x\\ & {\dot{a}}_t(t)={\text{w}}_t(t)\\ & {\dot{a}}_n(t)={\text{w}}_n(t)\end{array}\text{\hspace{1em}(2.5)}$$

三维空间运动模型

CV

假设惯性系速度不变，考虑速度为随机噪声扰动的CV模型的状态定义为$\mathbf{x}=[x,y,z,v_x,v_y,v_z{]}^{\mathrm{T}}$
$$\dot{\mathbf{x}}(t)=\left[\begin{array}{ll}{0}_3& {\mathbf{I}}_3\\ 0_3& 0_3\end{array}\right]\mathbf{x}(t)+\left[\begin{array}{l}{0}_3\\ {\mathbf{I}}_3\end{array}\right]\mathbf{w}(t)$$

CA

假设惯性系加速度为不变，考虑加速度为一阶Markov噪声的CA模型的状态定义为$\mathbf{x}=[x,y,z,v_x,v_y,v_z,a_x,a_y,a_z{]}^{\mathrm{T}}$
$$\dot{\mathbf{x}}(t)=\left[\begin{array}{lll}{0}_3& {\mathbf{I}}_3& 0_3\\ 0_3& 0_3& {\mathbf{I}}_3\\ 0_3& 0_3& -\frac{{\mathbf{I}}_3}{{\tau }_a}\end{array}\right]\mathbf{x}(t)+\left[\begin{array}{l}{0}_3\\ 0_3\\ {\mathbf{I}}_3\end{array}\right]\mathbf{w}(t)$$若噪声为高斯白噪声，离散化后得到
$$\mathbf{x}(k+1)=\left[\begin{array}{lll}{\mathbf{I}}_3& \Delta t\cdot {\mathbf{I}}_3& \frac{\Delta t^2}{2}{\mathbf{I}}_3\\ 0_3& {\mathbf{I}}_3& \Delta t\cdot {\mathbf{I}}_3\\ 0_3& 0_3& {\mathbf{I}}_3\end{array}\right]\left[\begin{array}{l}\mathbf{r}(k)\\ \mathbf{v}(k)\\ \mathbf{a}(k)\end{array}\right]+\left[\begin{array}{l}\frac{\Delta t^2}{2}{\mathbf{I}}_3\\ \Delta t\cdot {\mathbf{I}}_3\\ {\mathbf{I}}_3\end{array}\right]\mathbf{w}(t)$$
考虑噪声为一阶Markov过程，离散化后得到
$$\mathbf{x}(k+1)=\left[\begin{array}{lllc}{\mathbf{I}}_3& \Delta t\cdot {\mathbf{I}}_3& \frac{\Delta t^2}{2}{\mathbf{I}}_3& 0_3\\ 0_3& {\mathbf{I}}_3& \Delta t\cdot {\mathbf{I}}_3& 0_3\\ 0_3& 0_3& {\mathbf{I}}_3& {\mathbf{I}}_3\\ 0_3& 0_3& 0_3& (1-\frac{1}{{\tau }_a}){\mathbf{I}}_3\end{array}\right]\left[\begin{array}{l}\mathbf{r}(k)\\ \mathbf{v}(k)\\ \mathbf{a}(k)\\ \xi (k)\end{array}\right]+\left[\begin{array}{l}\frac{\Delta t^2}{2}{\mathbf{I}}_3\\ \Delta t\cdot {\mathbf{I}}_3\\ 0_3\\ {\mathbf{I}}_3\end{array}\right]\mathbf{w}(t)$$
CA模型没有考虑加速度与轨迹的关系。

前2个的状态转移矩阵内没有状态，是线性系统。但CT模型都是非线性系统。

三维恒定速率、角速率（CS-CT）运动

(Constant Speed - Constant Turn Rate)此模型假设目标沿空间弧线运动的速率不变即$a_t=0$、角速度$\mathbf{w}$不变，则转弯带来加速度$\mathbf{a}=\mathbf{\omega }\times \mathbf{v}$。状态定义为$\mathbf{x}=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z{]}^{\mathrm{T}}$
$$\dot{\mathbf{x}}(t)=\left[\begin{array}{lll}{0}_3& {\mathbf{I}}_3& 0_3\\ 0_3& \mathbf{\Omega }& 0_3\\ 0_3& 0_3& 0_3\end{array}\right]\mathbf{x}(t)+\left[\begin{array}{l}{0}_3\\ 0_3\\ {\mathbf{I}}_3\end{array}\right]\mathbf{w}(t)\text{\hspace{1em}(4)}$$其中的矩阵
$$\mathbf{\Omega }(\mathbf{w})=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$CT模型由于存在角速度对加速度的牵连加速度项，因而是非线性模型，线性化为离散系统为（假设时间步长为$T$）：
$$\mathbf{x}(k+1)=\mathbf{\Phi }(k+1,k)\mathbf{x}(k)+\mathbf{\Gamma }(k+1,k)\mathbf{w}(k)$$其中的
$$\begin{array}{rl}\mathbf{\Phi }& =\left[\begin{array}{ccc}{\mathbf{I}}_3& \mathbf{B}& 0_3\\ 0_3& {\mathbf{I}}_3+\mathbf{A}& 0_3\\ 0_3& 0_3& {\mathbf{I}}_3\end{array}\right],\mathbf{\Gamma }=\left[\begin{array}{l}{0}_3\\ 0_3\\ {\mathbf{I}}_3\end{array}\right],\mathbf{Q}=\left[\begin{array}{ccc}{0}_3& 0_3& 0_3\\ 0_3& 0_3& 0_3\\ 0_3& 0_3& {\mathbf{I}}_3\end{array}\right]{\sigma }_2^2\\ \mathbf{A}& =\left[\begin{array}{ccc}{c}_1{d}_1& -c_2{\omega }_z-c_1{\omega }_x{\omega }_y& c_2{\omega }_y-c_1{\omega }_x{\omega }_z\\ c_2{\omega }_z-c_1{\omega }_x{\omega }_y& c_1{d}_2& -c_2{\omega }_x-c_1{\omega }_y{\omega }_z\\ -c_2{\omega }_y-c_1{\omega }_x{\omega }_z& c_2{\omega }_x-c_1{\omega }_y{\omega }_z& c_1{d}_3\end{array}\right]\\ \mathbf{B}& =\left[\begin{array}{ccc}{c}_3{d}_1& c_1{\omega }_z-c_3{\omega }_x{\omega }_y& -c_1{\omega }_y-c_3{\omega }_x{\omega }_z\\ -c_1{\omega }_z-c_3{\omega }_x{\omega }_y& c_3{d}_2& c_1{\omega }_x-c_3{\omega }_y{\omega }_z\\ c_1{\omega }_y-c_3{\omega }_x{\omega }_z& -c_1{\omega }_x-c_3{\omega }_y{\omega }_z& c_3{d}_3\end{array}\right]\end{array}$$文献【3】推导出了参数$$\begin{array}{rl}& d_1={\omega }_y^2+{\omega }_z^2,d_2={\omega }_x^2+{\omega }_z^2,d_3={\omega }_x^2+{\omega }_y^2,\\ & c_1=\frac{\cos \Omega T-1}{{\Omega }^2},c_2=\frac{\sin \Omega T}{\Omega },c_3=\frac{1}{{\Omega }^2}\left(\frac{\sin \Omega T}{\Omega }-T\right).\end{array}$$以及$\Omega =\Vert \mathbf{w}\Vert$. 需要注意，这个模型的非线性程度非常高，在使用的时候三轴角速度的估计比较困难。

三维平面运动CTRV模型

在CS-CT模型中尚未考虑平面运动假设，它可以三维螺线运动。

下面的这种是更普遍认同的（Constant Turn Rate and Speed，CTRV）模型，对应于二维CTRV运动。包含了

• 三维恒定速率、
• 恒定角速率、
• 平面运动

假设，平面运动假设即角速度变化率沿着角速度方向
$$\dot{\mathbf{\omega }}=k\cdot \frac{\mathbf{\omega }}{\Vert \mathbf{\omega }\Vert }=-\frac{1}{{\tau }_w}\mathbf{\omega }$$

其中$k(t),{\tau }_w(t)$均为待估计的状态量。如果恒定速率、平面运动同时考虑，则为以下模型
$$\begin{array}{rl}& \dot{x}(t)=v_x(t)\\ & \dot{y}(t)=v_y(t)\\ & \dot{z}(t)=v_z(t)\\ & {\dot{v}}_x(t)=w_y\cdot v_z-w_z\cdot v_y\\ & {\dot{v}}_y(t)=w_z\cdot v_x-w_x\cdot v_z\\ & {\dot{v}}_z(t)=w_x\cdot v_y-w_y\cdot v_x\\ & \dot{\mathbf{\omega }}(t)=-\frac{1}{{\tau }_w}\mathbf{\omega }(t)\\ & \dot{{\tau }_w}=w_{\tau }\end{array}\text{\hspace{1em}(5)}$$其中角速度$\mathbf{w}$方向不变、大小变化，加速度只包含旋转加速度$\mathbf{a}=\mathbf{\omega }\times \mathbf{v}$。此模型需要估计目标的位置、惯性系速度、角速度、角速度变化率，即$\mathbf{x}=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z,{\tau }_w{]}^{\mathrm{T}}$

三维弹道系分解的CTRA模型

在三维空间平面内的CTRA模型为，
$$\begin{array}{rl}& \dot{x}(t)=v_x(t)\\ & \dot{y}(t)=v_y(t)\\ & \dot{z}(t)=v_z(t)\\ & {\dot{v}}_x(t)=a_t\cdot \frac{v_x}{v}+w_y\cdot v_z-w_z\cdot v_y\\ & {\dot{v}}_y(t)=a_t\cdot \frac{v_y}{v}+w_z\cdot v_x-w_x\cdot v_z\\ & {\dot{v}}_z(t)=a_t\cdot \frac{v_z}{v}+w_x\cdot v_y-w_y\cdot v_x\\ & \dot{\mathbf{\omega }}(t)={\mathbf{w}}_w\\ & {\dot{a}}_t(t)={\text{w}}_a\end{array}\text{\hspace{1em}(5)}$$其中$a_t\ne 0$、角速度$\mathbf{w}$不变，则加速度由2部分构成$\mathbf{a}=a_t\cdot \mathbf{\tau }+\mathbf{\omega }\times \mathbf{v}$。CTRA模型需要估计目标的位置、惯性系速度、角速度、切向加速度，即$\mathbf{x}=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z,a_t{]}^{\mathrm{T}}$。但公式(5)比较少见，更常用的是三维空间的CA模型 。更进一步的力学内容，可参考论文【2】.

平面ACT模型

假设目标的运动轨迹基本上在一个平面内，那么它的角速度朝向是始终不变的，这样是三维CT模型的一个更常用的形式。如果恒定速率、平面运动同时考虑，则为以下模型
$$\begin{array}{rl}& \dot{x}(t)=v_x(t)\\ & \dot{y}(t)=v_y(t)\\ & {\dot{v}}_x(t)=w_z\cdot v_y\\ & {\dot{v}}_y(t)=-w_z\cdot v_x\\ & \dot{w_z}={\xi }_w(t)\end{array}\text{\hspace{1em}(5)}$$其中角速度$\omega$受有色噪声或白噪声驱动，加速度只包含旋转加速度$\mathbf{a}=\mathbf{\omega }\times \mathbf{v}$。模型中包含转弯角速度，这种模型被称为扩维的转弯模型(Augmented Constant Turn, ACT)。此模型需要估计目标的平面位置、速度、转动角速度，即$\mathbf{x}=[x,y,v_x,v_y,w_z{]}^{\mathrm{T}}$，此时方程的离散形式为：
$$\left[\begin{array}{c}x(k+1)\\ y(k+1)\\ v_x(k+1)\\ v_y(k+1)\\ \omega (k+1)\end{array}\right]=\left[\begin{array}{c}x+\frac{1}{\omega }\sin (\omega T)v_x-\frac{1}{\omega }(1-\cos (\omega T))v_y\\ y+\frac{1}{\omega }(1-\cos (\omega T))v_x+\frac{1}{\omega }(\sin (\omega T))v_y\\ v_x\cos (\omega T)-v_y\sin (\omega T)\\ v_x\sin (\omega T)+v_y\cos (\omega T)\\ 0\end{array}\right]+\Gamma \mathbf{w}$$考虑角速度为常值且由白噪声驱动。在这之中，ACT模型的转动角速度直接决定跟踪精度，所以如果为了收敛更快，会把加速度、角速度中均包含噪声项$\mathbf{w}=[w_{ax},w_{ay},w_w{]}^{\mathrm{T}}$：
$$\Gamma \mathbf{w}=\left[\begin{array}{cc}\frac{\Delta t^2}{2}\cdot {\mathbf{I}}_2& 0_{2\times 1}\\ \Delta t\cdot {\mathbf{I}}_2& 0_{2\times 1}\\ 0_{1\times 2}& 1\end{array}\right]\left[\begin{array}{c}{w}_{ax}\\ w_{ay}\\ w_w\end{array}\right]$$

如果角速度由一阶Markov过程噪声驱动，那么它的表达式为：
$$\dot{\omega }=-\frac{1}{{\tau }_w}\omega +w_w$$其中${\tau }_w$为相关噪声的时间相关系数，与时间同量纲。这个式子的离散形式为：
$$\omega (k+1)=\exp (\frac{T}{{\tau }_w})\omega (k)+w_w(k)$$由于系数${\tau }_w$是人为设定的，因此这个式子也写作
$$\omega (k+1)=\alpha \cdot \omega (k)+w_w(k)$$其中$\alpha \in (0,1)$

参考

• CSDN博客3月16日 CV,CA,CTRV等运动模型，EKF，UKF在运动模型下的分析与实践
• Li, X. R., & Jilkov, V. P. (2003). Survey of maneuvering target tracking. Part I. Dynamic models. IEEE Transactions on Aerospace and Electronic Systems, 39(4), 1333-1364. doi:10.1109/TAES.2003.1261132
• Ronghui, Z., & Wan, J. (2006). Passive maneuvering target tracking using 3D constant-turn model. 2006 IEEE Conference on Radar. doi:10.1109/RADAR.2006.1631832