軌跡預測

車輛正常行駛的狀態可以分成三類：車道保持(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