轨迹预测

车辆正常行驶的状态可以分成三类：车道保持(Keep Lane, KL)、变道(Change Lane, CL)和转弯(Turn)。车道保持和转弯可以采用运动学模型进行预测，变道过程无法用简单的运动学模型来描述，采用特定的变道模型。

基于运动学模型的轨迹预测

基于运动学模型的轨迹预测方法有：

1. Constant Velocity (CV)：
   $$X_{k+1}=X_k+{\left[\begin{array}{cccccc}vt\cos (\theta )& vt\sin (\theta )& 0& 0& 0& 0\end{array}\right]}^T$$

2. Constant Acceleration (CA)：
   $$X_{k+1}=X_k+{\left[\begin{array}{cccccc}(vt+1/2a{t}^2)\cos (\theta )& (vt+1/2a{t}^2)\sin (\theta )& 0& 0& 0& 0\end{array}\right]}^T$$

3. Constant Turn Rate and Velocity (CTRV)：
   $$X_{k+1}=X_k+{\left[\begin{array}{cccccc}\frac{v}{w}(\sin (\theta +wt)-\sin (\theta ))& \frac{v}{w}(\cos (\theta )-\cos (\theta +wt))& wt& 0& 0& 0\end{array}\right]}^T$$

4. Constant Turn Rate and Acceleration (CTRA)：
   $$X_{k+1}=X_k+{\left[\begin{array}{cccccc}\frac{a[\cos (\theta +wt)-\cos (\theta )]}{w^2}+\frac{(v+at)\sin (\theta +wt)-v\sin (\theta )}{w}& \frac{a[\sin (\theta +wt)-\sin (\theta )]}{w^2}+\frac{(v+at)\cos (\theta +wt)-v\cos (\theta )}{w}& wt& at& 0& 0\end{array}\right]}^T$$

5. Constant Curvature and Acceleration (CCA)：
   $$\begin{gathered} X_{k+1}=X_k+{\left[\begin{array}{cccccc}\frac{1}{c}(\sin (\theta +lc)-\sin (\theta ))& \frac{1}{c}(\cos (\theta )-\cos (\theta +lc))& lc& at& atc& 0\end{array}\right]}^T \\ l=vt+\frac{1}{2}a{t}^2 \end{gathered}$$
   其中，状态变量为$X_k=[x,y,\theta ,v,w,a{]}^T$。

变道模型

采集车辆变道数据可以发现，变道过程中车辆侧向加速度的变化可以近似为一个正弦函数，如下：
$$a_y=\{\begin{array}{lc}{a}_0+A\sin \frac{2\pi (t-t_{start})}{T}& t_{start}\le t\le t_{start}+T\\ 0& 其他\end{array}$$
对$a_y$进行两次积分，可以得到侧向位移随时间变化关系：
$$Y(t)=\{\begin{array}{lcl}\frac{A{T}^2}{2\pi }+\frac{1}{2}{a}_0{T}^2& & t>t_{start}+T\\ \frac{1}{2}{a}_0(t-t_{start}{)}^2+\frac{AT}{2\pi }(t-t_{start})-\frac{A{T}^2}{4{\pi }^2}\sin (\frac{2\pi }{T}(t-t_{start}))& & t_{start}\le t\le t_{start}+T\\ 0& & t<t_{start}\end{array}$$
纵向可近似为CA模型：
$$X(t)=vt+\frac{1}{2}a{t}^2$$