1. 前言
随着自动驾驶的发展,现代汽车的智能化程度逐步提高,基于视觉系统的自动驾驶也逐步趋于稳定——特斯拉、Mobileye、CommaAI等机构起着推波助澜的作用。
自动驾驶场景或者Adas场景中,当检测出前方车辆后通常需要进行距离估计,为车辆控制提供距离参考信息;而基于视觉的移动机器人也存在该需求。单目摄像头光学图像测距具有低成本和计算快的优点,主要有两种常用的测距方式1。
- 使用目标物体大小和摄像头焦距
- 使用摄像头高度和俯仰角
本文将介绍第二种测距方式。
2. 几何关系示意图
该部分直接引用自参考文献2
投影关系图和投影平面2如下所示,
平面
A
B
U
ABU
ABU代表路平面,
A
B
C
D
ABCD
ABCD为摄像头拍摄到的路平面上的梯形区域,
O
O
O为摄像机
O
O
O点为摄像机镜头中心点,
O
G
OG
OG为摄像机光轴,
G
G
G 点为摄像机光轴和路平面的交点(同时也是视野梯形的对角线交点),
I
I
I点为
O
O
O点在路平面上的垂直投影。在路面坐标系中, 将
G
G
G点定义为坐标系原点,,车辆前进方向定义为
Y
Y
Y轴方向。
G
、
A
、
B
、
C
、
D
G、A、B 、C、D
G、A、B、C、D 各点在图像平面内的对应点上图所示,
a
、
b
、
c
、
d
a、b、c、d
a、b、c、d为像平面矩形的4个端点,
H
H
H和
W
W
W分别为像平面的高和宽。定义图像矩形的中点
g
g
g为像平面坐标系的坐标原点,y 轴代表车辆前进方向。
取路面上一点 P P P,其在路平面坐标系的坐标为 ( X P , Y P ) (X_P, Y_P) (XP,YP), P P P点在图像平面内的对应点为 p p p, 其在像平面坐标系的坐标为 ( x p , y p ) (x_p, y_p) (xp,yp)。
3. 距离计算
3.1 参数
- 摄像头高度 h h h
- 摄像头水平视场角 2 β 0 2\beta_0 2β0
- 摄像头垂直视场角 2 α 0 2\alpha_0 2α0
- 摄像头俯仰角 γ 0 \gamma_0 γ0
相关概念请参见博客:相机焦距与视场角
3.2 Y轴方向的成像模型2
摄像头光轴(
O
G
OG
OG)所在的与路面相垂直的平面为
O
E
I
OEI
OEI。图中各字符与几何关系示意图中对应字符含义相同。直线
M
L
ML
ML与直线
O
G
OG
OG垂直,与直线
f
F
fF
fF的延长线相交于点
L
L
L。点
p
y
p_y
py表示像平面上的点
p
p
p在直线
e
f
ef
ef上的投影(
y
y
y轴方向),
P
y
P_y
Py点是路面上的点
P
P
P在视野纵向对称线上的投影,Z是直线
p
y
P
y
p_yP_y
pyPy与直线
线
M
L
线ML
线ML的交点。
根据三角函数关系和相似三角形可得,
tan
α
=
Z
G
G
O
tan
α
0
=
M
G
G
O
Z
G
M
G
=
p
y
g
e
g
\begin{aligned} \tan\alpha &= \frac{ZG}{GO} \\ \tan\alpha_0 &= \frac{MG}{GO} \\ \frac{ZG}{MG} &= \frac{p_yg}{eg} \end{aligned}
tanαtanα0MGZG=GOZG=GOMG=egpyg
所以,
tan
α
=
p
y
g
e
g
∗
tan
α
0
=
p
y
g
H
/
2
∗
tan
α
0
=
y
H
/
2
∗
tan
α
0
\begin{aligned} \tan\alpha &= \frac{p_yg}{eg}*\tan\alpha_0\\ &=\frac{p_yg}{H/2}*\tan\alpha_0\\ &=\frac{y}{H/2}*\tan\alpha_0 \end{aligned}
tanα=egpyg∗tanα0=H/2pyg∗tanα0=H/2y∗tanα0
那么,
P
y
I
=
O
I
tan
(
9
0
∘
−
α
−
γ
0
)
=
h
tan
(
9
0
∘
−
α
−
γ
0
)
\begin{aligned} P_yI &= \frac{OI}{\tan(90^{\circ}-\alpha-\gamma_0)}\\ &= \frac{h}{\tan(90^{\circ}-\alpha-\gamma_0)} \end{aligned}
PyI=tan(90∘−α−γ0)OI=tan(90∘−α−γ0)h
注:计算角度 α = arctan ( y H / 2 ∗ tan α 0 ) \alpha = \arctan(\frac{y}{H/2}*\tan\alpha_0) α=arctan(H/2y∗tanα0)时,y为空间点在投影面上的y轴坐标,是有符号的,H为图像高度。
3.3 X轴方向的成像模型
P
x
Px
Px为点P在X轴上的投影点,
在三角形
Δ
O
P
y
L
\Delta OP_yL
ΔOPyL中,
tan
α
1
=
P
y
P
O
P
y
=
∣
X
p
∣
O
P
y
=
∣
X
p
∣
h
2
+
P
y
I
2
\begin{aligned} \tan \alpha_1 &=\frac{P_yP}{OP_y} \\ &= \frac{|X_p|}{OP_y}\\ &= \frac{|X_p|}{\sqrt{h^2+P_yI^2}} \end{aligned}
tanα1=OPyPyP=OPy∣Xp∣=h2+PyI2∣Xp∣
根据P点成像的相似三角形可得,
O
P
y
tan
α
1
O
P
y
tan
β
0
=
x
W
/
2
\begin{aligned} \frac{OP_y\tan\alpha_1}{OP_y\tan\beta_0} &=\frac{x}{W/2} \\ \end{aligned}
OPytanβ0OPytanα1=W/2x
综上,
∣
X
p
∣
=
h
2
+
P
y
I
2
∗
x
∗
tan
β
0
W
/
2
|X_p| = \frac{\sqrt{h^2+P_yI^2} *x *\tan\beta_0}{W/2}
∣Xp∣=W/2h2+PyI2∗x∗tanβ0
x为空间点在投影面上的x轴坐标
4. Show me the code
代码很简单,后续补充上来。
Reference
Alizadeh, Peyman. Object distance measurement using a single camera for robotic applications. Diss. Laurentian University of Sudbury, 2015. ↩︎
郭磊, 徐友春, 李克强, & 连小珉. (2006). 基于单目视觉的实时测距方法研究 (Doctoral dissertation). ↩︎ ↩︎