1. 前言

随着自动驾驶的发展，现代汽车的智能化程度逐步提高，基于视觉系统的自动驾驶也逐步趋于稳定——特斯拉、Mobileye、CommaAI等机构起着推波助澜的作用。

自动驾驶场景或者Adas场景中，当检测出前方车辆后通常需要进行距离估计，为车辆控制提供距离参考信息；而基于视觉的移动机器人也存在该需求。单目摄像头光学图像测距具有低成本和计算快的优点，主要有两种常用的测距方式¹。

• 使用目标物体大小和摄像头焦距
• 使用摄像头高度和俯仰角

本文将介绍第二种测距方式。

2. 几何关系示意图

该部分直接引用自参考文献2

投影关系图和投影平面²如下所示，

平面$ABU$ABU代表路平面，$ABCD$ABCD为摄像头拍摄到的路平面上的梯形区域，$O$O为摄像机$O$O点为摄像机镜头中心点，$OG$OG为摄像机光轴，$G$G 点为摄像机光轴和路平面的交点（同时也是视野梯形的对角线交点），$I$I点为$O$O点在路平面上的垂直投影。在路面坐标系中， 将$G$G点定义为坐标系原点,，车辆前进方向定义为$Y$Y轴方向。

$G、A、B 、C、D$G、A、B、C、D 各点在图像平面内的对应点上图所示，$a、b、c、d$a、b、c、d为像平面矩形的4个端点，$H$H和$W$W分别为像平面的高和宽。定义图像矩形的中点$g$g为像平面坐标系的坐标原点，y 轴代表车辆前进方向。

取路面上一点$P$P，其在路平面坐标系的坐标为$(X_P, Y_P)$(XP​,YP​)，$P$P点在图像平面内的对应点为$p$p, 其在像平面坐标系的坐标为$(x_p, y_p)$(xp​,yp​)。

3. 距离计算

3.1 参数

• 摄像头高度$h$h
• 摄像头水平视场角$2\beta_0$2β0​
• 摄像头垂直视场角$2\alpha_0$2α0​
• 摄像头俯仰角$\gamma_0$γ0​

相关概念请参见博客：相机焦距与视场角

3.2 Y轴方向的成像模型²

摄像头光轴（$OG$OG）所在的与路面相垂直的平面为$OEI$OEI。图中各字符与几何关系示意图中对应字符含义相同。直线$ML$ML与直线$OG$OG垂直，与直线$fF$fF的延长线相交于点$L$L。点$p_y$py​表示像平面上的点$p$p在直线$ef$ef上的投影($y$y轴方向)，$P_y$Py​点是路面上的点$P$P在视野纵向对称线上的投影，Z是直线$p_yP_y$py​Py​与直线$线ML$线ML的交点。

根据三角函数关系和相似三角形可得，
$\begin{aligned} \tan\alpha &= \frac{ZG}{GO} \\ \tan\alpha_0 &= \frac{MG}{GO} \\ \frac{ZG}{MG} &= \frac{p_yg}{eg} \end{aligned}$tanαtanα0​MGZG​​=GOZG​=GOMG​=egpy​g​​
所以，
$\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α​=egpy​g​∗tanα0​=H/2py​g​∗tanα0​=H/2y​∗tanα0​​
那么，
$\begin{aligned} P_yI &= \frac{OI}{\tan(90^{\circ}-\alpha-\gamma_0)}\\ &= \frac{h}{\tan(90^{\circ}-\alpha-\gamma_0)} \end{aligned}$Py​I​=tan(90∘−α−γ0​)OI​=tan(90∘−α−γ0​)h​​

注：计算角度$\alpha = \arctan(\frac{y}{H/2}*\tan\alpha_0)$α=arctan(H/2y​∗tanα0​)时，y为空间点在投影面上的y轴坐标，是有符号的，H为图像高度。

3.3 X轴方向的成像模型

$Px$Px为点P在X轴上的投影点，
在三角形$\Delta OP_yL$ΔOPy​L中，
$\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​​=OPy​Py​P​=OPy​∣Xp​∣​=h2+Py​I2​∣Xp​∣​​
根据P点成像的相似三角形可得，
$\begin{aligned} \frac{OP_y\tan\alpha_1}{OP_y\tan\beta_0} &=\frac{x}{W/2} \\ \end{aligned}$OPy​tanβ0​OPy​tanα1​​​=W/2x​​
综上，
$|X_p| = \frac{\sqrt{h^2+P_yI^2} *x *\tan\beta_0}{W/2}$∣Xp​∣=W/2h2+Py​I2​∗x∗tanβ0​​

x为空间点在投影面上的x轴坐标

4. Show me the code

代码很简单，后续补充上来。

Reference

---

1. Alizadeh, Peyman. Object distance measurement using a single camera for robotic applications. Diss. Laurentian University of Sudbury, 2015. ↩︎

2. 郭磊, 徐友春, *, & 连小珉. (2006). 基于单目视觉的实时测距方法研究 (Doctoral dissertation). ↩︎ ↩︎