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

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

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

1. Constant Velocity (CV)：
   X k + 1 = X k + [ v t cos ⁡ ( θ ) v t sin ⁡ ( θ ) 0 0 0 0 ] T X_{k+1}=X_k+\begin{bmatrix} vt\cos(\theta) & vt\sin(\theta) & 0 & 0 & 0 & 0\end{bmatrix} ^T Xk+1​=Xk​+[vtcos(θ)​vtsin(θ)​0​0​0​0​]T

2. Constant Acceleration (CA)：
   X k + 1 = X k + [ ( v t + 1 / 2 a t 2 ) cos ⁡ ( θ ) ( v t + 1 / 2 a t 2 ) sin ⁡ ( θ ) 0 0 0 0 ] T X_{k+1}=X_k+\begin{bmatrix}(vt+1/2at^2)\cos(\theta) & (vt+1/2at^2)\sin(\theta) & 0 & 0 & 0 & 0\end{bmatrix} ^T Xk+1​=Xk​+[(vt+1/2at2)cos(θ)​(vt+1/2at2)sin(θ)​0​0​0​0​]T

3. Constant Turn Rate and Velocity (CTRV)：
   X k + 1 = X k + [ v w ( sin ⁡ ( θ + w t ) − sin ⁡ ( θ ) ) v w ( cos ⁡ ( θ ) − cos ⁡ ( θ + w t ) ) w t 0 0 0 ] T X_{k+1}=X_k+\begin{bmatrix}\frac{v}{w}(\sin(\theta +wt)-\sin(\theta)) & \frac{v}{w}(\cos(\theta)-\cos(\theta+wt)) & wt & 0 & 0 & 0\end{bmatrix} ^T Xk+1​=Xk​+[wv​(sin(θ+wt)−sin(θ))​wv​(cos(θ)−cos(θ+wt))​wt​0​0​0​]T

4. Constant Turn Rate and Acceleration (CTRA)：
   X k + 1 = X k + [ a [ cos ⁡ ( θ + w t ) − cos ⁡ ( θ ) ] w 2 + ( v + a t ) sin ⁡ ( θ + w t ) − v sin ⁡ ( θ ) w a [ sin ⁡ ( θ + w t ) − sin ⁡ ( θ ) ] w 2 + ( v + a t ) cos ⁡ ( θ + w t ) − v cos ⁡ ( θ ) w w t a t 0 0 ] T X_{k+1}=X_k+\begin{bmatrix} \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{bmatrix} ^T Xk+1​=Xk​+[w2a[cos(θ+wt)−cos(θ)]​+w(v+at)sin(θ+wt)−vsin(θ)​​w2a[sin(θ+wt)−sin(θ)]​+w(v+at)cos(θ+wt)−vcos(θ)​​wt​at​0​0​]T

5. Constant Curvature and Acceleration (CCA)：
   X k + 1 = X k + [ 1 c ( sin ⁡ ( θ + l c ) − sin ⁡ ( θ ) ) 1 c ( cos ⁡ ( θ ) − cos ⁡ ( θ + l c ) ) l c a t a t c 0 ] T l = v t + 1 2 a t 2 X_{k+1}=X_k+\begin{bmatrix} \frac{1}{c}(\sin(\theta+lc)-\sin(\theta)) & \frac{1}{c}(\cos(\theta)-\cos(\theta+lc)) & lc & at & atc & 0 \end{bmatrix} ^T \\ l=vt+\frac{1}{2}at^2 Xk+1​=Xk​+[c1​(sin(θ+lc)−sin(θ))​c1​(cos(θ)−cos(θ+lc))​lc​at​atc​0​]Tl=vt+21​at2
   其中，状态变量为 X k = [ x , y , θ , v , w , a ] T X_k=[x,y,\theta,v,w,a]^T Xk​=[x,y,θ,v,w,a]T。

变道模型

采集车辆变道数据可以发现，变道过程中车辆侧向加速度的变化可以近似为一个正弦函数，如下：
a y = { a 0 + A sin ⁡ 2 π ( t − t s t a r t ) T t s t a r t ≤ t ≤ t s t a r t + T 0 其 他 a_y=\left\{\begin{array}{lc}a_0+A\sin\frac{2\pi(t-t_{start})}{T} & t_{start}\leq t \leq t_{start}+T \\0 & 其他\end{array}\right. ay​={ a0​+AsinT2π(t−tstart​)​0​tstart​≤t≤tstart​+T其他​
对 a y a_y ay​进行两次积分，可以得到侧向位移随时间变化关系：
Y ( t ) = { A T 2 2 π + 1 2 a 0 T 2 t > t s t a r t + T 1 2 a 0 ( t − t s t a r t ) 2 + A T 2 π ( t − t s t a r t ) − A T 2 4 π 2 sin ⁡ ( 2 π T ( t − t s t a r t ) ) t s t a r t ≤ t ≤ t s t a r t + T 0 t < t s t a r t Y(t)=\left\{ \begin{array}{lcl} \frac{AT^2}{2\pi}+\frac{1}{2}a_0T^2 & & t>t_{start}+T \\ \frac{1}{2}a_0(t-t_{start})^2+\frac{AT}{2\pi}(t-t_{start})-\frac{AT^2}{4\pi^2}\sin(\frac{2\pi}{T}(t-t_{start})) & & t_{start} \leq t \leq t_{start}+T \\ 0 & & t<t_{start} \end{array} \right. Y(t)=⎩⎨⎧​2πAT2​+21​a0​T221​a0​(t−tstart​)2+2πAT​(t−tstart​)−4π2AT2​sin(T2π​(t−tstart​))0​​t>tstart​+Ttstart​≤t≤tstart​+Tt<tstart​​
纵向可近似为CA模型：
X ( t ) = v t + 1 2 a t 2 X(t)=vt+\frac{1}{2}at^2 X(t)=vt+21​at2