过滤算法
-
根据地图匹配算法，可以得到路口区域内驶向交叉口的车辆，从而过滤掉驶出路口的车辆；
-
以交叉口中心为原点，和该点距离大于 d 0 d_0 d0的滤除， d d d的计算公式如下：
d 0 = v 0 ⋅ t 0 d_0=v_0\cdot t_0 d0=v0⋅t0
式中， v 0 v_0 v0为该路口区域的限制的最高速度；一般情况下，提前 3 s 3 \rm s 3s预警是既满足安全要求又不至于干扰驾驶员驾驶，考虑到可能有超速行驶的车辆，取 t 0 = 5 s t_0=5 {\rm s} t0=5s。
预警算法
假设如下：
- 驾驶员遵守交通规则，在接近路口时，会提前开启转向灯。打左转灯代表将左转，右转灯代表将右转，不打转向灯代表将直行；
- 车基本沿着车道中心线行驶。
当HV从南边向上驶入交叉口时，同样驶向交叉路口的RV可能来自HV所在位置的左侧、对侧或者右侧，需要分情况讨论不同工况下碰撞点的位置。下述情况中的坐标系均以交叉口中心为原点，正东为 x x x轴，正北为 y y y轴。
HV直行：
从图2可以看出，HV直行时，可能发生碰撞的情况有以下6种（下面列举3种）：
一、左侧来车
- 直行：此时冲突点如图3所示。
由图可知，冲突点为
Z
Z
Z点。已知点
O
(
x
1
,
y
1
)
,
Q
(
x
2
,
y
2
)
O(x_1,y_1),Q(x_2,y_2)
O(x1,y1),Q(x2,y2)，
l
O
W
l_{OW}
lOW和
l
Q
U
l_{QU}
lQU与
x
x
x轴的夹角为
θ
L
1
,
θ
L
2
\theta_{L1}, \theta_{L2}
θL1,θL2，因此可以得到两条直线的表达式，联立得到以下方程组：
{
y
−
y
1
=
tan
(
θ
L
1
)
(
x
−
x
1
)
y
−
y
2
=
tan
(
θ
L
2
(
x
−
x
2
)
)
⟹
[
tan
(
θ
L
1
)
−
1
tan
(
θ
L
2
)
−
1
]
[
x
y
]
=
[
tan
(
θ
L
1
)
⋅
x
1
−
y
1
tan
(
θ
L
2
)
⋅
x
2
−
y
2
]
\left\{\begin{matrix} y-y_1=\tan(\theta_{L1})(x-x_1) \\ y-y_2=\tan(\theta_{L2}(x-x_2)) \end{matrix}\right. \Longrightarrow \begin{bmatrix} \tan(\theta_{L1}) & -1 \\ \tan(\theta_{L2}) & -1 \end{bmatrix} \begin{bmatrix}x \\ y\end{bmatrix} = \begin{bmatrix} \tan(\theta_{L1})\cdot x_1-y_1 \\ \tan(\theta_{L2})\cdot x_2-y_2\end{bmatrix}
{y−y1=tan(θL1)(x−x1)y−y2=tan(θL2(x−x2))⟹[tan(θL1)tan(θL2)−1−1][xy]=[tan(θL1)⋅x1−y1tan(θL2)⋅x2−y2]
解该方程组得冲突点
Z
Z
Z的坐标为：
{
x
c
=
−
y
1
−
y
2
−
k
1
x
1
+
k
2
x
2
k
1
−
k
2
y
c
=
k
1
y
2
−
k
2
y
1
+
k
1
k
2
x
1
−
k
1
k
2
x
2
k
1
−
k
2
\left\{\begin{array}{l} x_c=-\frac{y_1 -y_2 -k_1 \,x_1 +k_2 \,x_2 }{k_1 -k_2 }\\ y_c=\frac{k_1 \,y_2 -k_2 \,y_1 +k_1 \,k_2 \,x_1 -k_1 \,k_2 \,x_2 }{k_1 -k_2 } \end{array}\right.
{xc=−k1−k2y1−y2−k1x1+k2x2yc=k1−k2k1y2−k2y1+k1k2x1−k1k2x2
式中，
k
1
=
tan
(
θ
L
1
)
,
k
2
=
tan
(
θ
L
2
)
k_1=\tan(\theta_{L1}), k_2={\tan(\theta_{L2})}
k1=tan(θL1),k2=tan(θL2).
得到碰撞点后需要判断车是否驶入了路口和是否经过了碰撞点。设HV和
O
O
O点的连线的角度为
α
\alpha
α，HV的航向角为
θ
H
V
\theta_{HV}
θHV，定义符号函数如下：
s
i
g
n
=
{
1
∣
α
−
θ
H
V
∣
≥
π
2
0
∣
α
−
θ
H
V
∣
<
π
2
sign = \left\{ \begin{array}{lc} 1 & |\alpha-\theta_{HV}|\geq\frac{\pi}{2} \\ 0 & |\alpha - \theta_{HV}|<\frac{\pi}{2}\end{array} \right.
sign={10∣α−θHV∣≥2π∣α−θHV∣<2π
s
i
g
n
=
1
sign=1
sign=1表示HV已经经过
O
O
O点，
s
i
g
n
=
0
sign=0
sign=0表示HV还没有经过
O
O
O点。同理，也判断HV是否经过
Z
Z
Z点或者RV是否经过
Q
Q
Q点和
Z
Z
Z点。
判断完HV和RV的位置后，需要分段计算HV和RV到碰撞点的距离，设碰撞点坐标为
Z
(
x
c
,
y
c
)
Z(x_c, y_c)
Z(xc,yc)。
s
H
V
=
{
(
x
H
V
−
x
1
)
2
+
(
y
H
V
−
y
1
)
2
+
(
x
1
−
x
c
)
2
+
(
y
1
−
y
c
)
2
未
经
过
O
点
(
x
H
V
−
x
c
)
2
+
(
y
H
V
−
y
c
)
2
经
过
O
点
，
未
经
过
碰
撞
点
s_{HV}=\left\{ \begin{array}{lc} \sqrt{(x_{HV}-x_1)^2+(y_{HV}-y_1)^2}+\sqrt{(x_1-x_c)^2+(y_1-y_c)^2} & 未经过O点 \\ \sqrt{(x_{HV}-x_c)^2+(y_{HV}-y_c)^2} & 经过O点，未经过碰撞点 \end{array}\right.
sHV={(xHV−x1)2+(yHV−y1)2
+(x1−xc)2+(y1−yc)2
(xHV−xc)2+(yHV−yc)2
未经过O点经过O点，未经过碰撞点
s R V = { ( x R V − x 2 ) 2 + ( y R V − y 2 ) 2 + ( x 2 − x c ) 2 + ( y 2 − y c ) 2 未 经 过 Q 点 ( x R V − x c ) 2 + ( y R V − y c ) 2 经 过 Q 点 ， 未 经 过 碰 撞 点 s_{RV}=\left\{ \begin{array}{lc} \sqrt{(x_{RV}-x_2)^2+(y_{RV}-y_2)^2}+\sqrt{(x_2-x_c)^2+(y_2-y_c)^2} & 未经过Q点 \\ \sqrt{(x_{RV}-x_c)^2+(y_{RV}-y_c)^2} & 经过Q点，未经过碰撞点 \end{array}\right. sRV={(xRV−x2)2+(yRV−y2)2 +(x2−xc)2+(y2−yc)2 (xRV−xc)2+(yRV−yc)2 未经过Q点经过Q点，未经过碰撞点
计算TTC：
Δ
t
=
−
v
+
v
2
+
2
a
s
a
\Delta t=\frac{-v+\sqrt{v^2+2as}}{a}
Δt=a−v+v2+2as
如果
∣
Δ
t
H
V
−
Δ
t
R
V
∣
<
t
0
|\Delta t_{HV} - \Delta t_{RV}|<t_0
∣ΔtHV−ΔtRV∣<t0则认为存在碰撞风险，
t
0
=
v
8
+
1
,
v
=
v
H
V
t_0=\frac{v}{8}+1, v = v_{HV}
t0=8v+1,v=vHV.
- 左转：此时冲突点如图4所示。
由图可知，冲突点为
W
(
x
c
,
y
c
)
W(x_c,y_c)
W(xc,yc)。HV仍未直行，
s
H
V
s_{HV}
sHV计算公式不变。RV的路径为圆弧，
s
R
V
s_{RV}
sRV计算公式如下：
s
R
V
=
{
(
x
R
V
−
x
2
)
2
+
(
y
R
V
−
y
2
)
2
+
π
2
×
(
1.5
l
w
)
未
经
过
Q
点
2
×
arcsin
(
(
x
R
V
−
x
c
)
2
+
(
y
R
V
−
y
c
)
2
2
×
(
1.5
l
w
)
)
⋅
(
1.5
l
w
)
经
过
Q
点
，
未
经
过
碰
撞
点
s_{RV}=\left\{ \begin{array}{lc} \sqrt{(x_{RV}-x_2)^2+(y_{RV}-y_2)^2}+\frac{\pi}{2}\times(1.5lw) & 未经过Q点 \\ 2\times\arcsin(\frac{\sqrt{(x_{RV}-x_c)^2+(y_{RV}-y_c)^2}}{2\times(1.5lw)})\cdot (1.5lw) & 经过Q点，未经过碰撞点 \end{array}\right.
sRV={(xRV−x2)2+(yRV−y2)2
+2π×(1.5lw)2×arcsin(2×(1.5lw)(xRV−xc)2+(yRV−yc)2
)⋅(1.5lw)未经过Q点经过Q点，未经过碰撞点
二、对侧来车
- 左转：此时冲突点如图5所示。
由图可知，冲突点为
B
1
B_1
B1点，为了简化计算，可以用
Z
Z
Z点近似替代
B
1
B_1
B1点。HV仍未直行，
s
H
V
s_{HV}
sHV计算公式不变。RV的路径为圆弧，设
A
1
A_1
A1的坐标为
A
1
(
x
3
,
y
3
)
A_1(x_3,y_3)
A1(x3,y3)则，
s
R
V
s_{RV}
sRV的计算公式如下：
s
R
V
=
{
(
x
R
V
−
x
3
)
2
+
(
y
R
V
−
y
3
)
2
+
arccos
(
1
3
)
×
(
1.5
l
w
)
未
经
过
A
1
点
2
×
arcsin
(
(
x
R
V
−
x
c
)
2
+
(
y
R
V
−
y
c
)
2
2
×
(
1.5
l
w
)
)
⋅
(
1.5
l
w
)
经
过
A
1
点
，
未
经
过
碰
撞
点
s_{RV}=\left\{ \begin{array}{lc} \sqrt{(x_{RV}-x_3)^2+(y_{RV}-y_3)^2}+\arccos(\frac{1}{3})\times(1.5lw) & 未经过A_1点 \\ 2 \times \arcsin(\frac{\sqrt{(x_{RV}-x_c)^2+(y_{RV}-y_c)^2}}{2\times(1.5lw)})\cdot (1.5lw) & 经过A_1点，未经过碰撞点 \end{array}\right.
sRV={(xRV−x3)2+(yRV−y3)2
+arccos(31)×(1.5lw)2×arcsin(2×(1.5lw)(xRV−xc)2+(yRV−yc)2
)⋅(1.5lw)未经过A1点经过A1点，未经过碰撞点
剩余11种情况类似，读者可以参照以上例子自己思考。