基于消失点的相机俯仰角和偏航角的更新方法

首先需要说明的有三点:

  • 针对车载的情况下,外参特指相机的俯仰角和偏航角。由于车载条件下,相机的高度等是固定的,我们做了相应的条件约束。
  • 在辅助驾驶中,车道线的识别是首要的前提条件,所以更新的算法是基于车道线检测的结果。
  • 默认使用的图片是已经解畸变的图片,在计算的过程中没有考虑畸变模型。

具体的方法如下:

如图所示建立图像坐标系$cr$,摄像机坐标系空间直角坐标系$x_c、y_c、z_c$和世界坐标系$x_w、y_w、z_w$。根据内外参矩阵可知图像坐标系和世界坐标系坐标的转换关系,其中外参只考虑了俯仰角和偏航角。

$$ \alpha\left[\begin{array}{l}{c} \\ {r} \\ {1}\end{array}\right]=H_{3 \times 4}\left[\begin{array}{l}{x_{w}} \\ {y_{w}} \\ {z_{w}} \\ {1}\end{array}\right] \tag{1} $$

$$ H_{3 \times 4}=\left[\begin{array}{ccc}{k_{1}} & {0} & {k_{2}} \\ {0} & {k_{3}} & {k_{4}} \\ {0} & {0} & {1}\end{array}\right]\left[\begin{array}{cccc}{\cos \beta} & {-\sin \beta} & {0} & {t_{1}} \\ {\cos \alpha \sin \beta} & {\cos \alpha \cos \beta} & {-\sin \alpha} & {t_{2}} \\ {\sin \alpha \sin \beta} & {\sin \alpha \cos \beta} & {\cos \alpha} & {t_{3}}\end{array}\right] $$

令$z_w = 0$化简得到:

$$ \left[\begin{array}{ll}{H(3,1) c-H(1,1)} & {H(3,2) c-H(1,2)} \\ {H(3,1) r-H(2,1)} & {H(3,2) r-H(1,2)}\end{array}\right]\left[\begin{array}{l}{x_{w}} \\ {y_{w}}\end{array}\right]=\left[\begin{array}{c}{H(1,4)-H(3,4) c} \\ {H(2,4)-H(3,4) r}\end{array}\right] \tag{2} $$

$$ \begin{array}{l}{\left[c \sin \alpha \sin \beta-\left(k_{1} \cos \beta+k_{2} \sin \alpha \sin \beta\right)\right] x_{w}+} \\ {\left[c \sin \alpha \cos \beta-\left(-k_{1} \sin \beta+k_{2} \sin \alpha \cos \beta\right)\right] y_{w}=k_{1} t_{1}+k_{2} t_{3}-t_{3} c}\end{array} \tag{3} $$

$$ \begin{array}{l}{\left[r \sin \alpha \sin \beta-\left(k_{3} \cos \alpha \sin \beta+k_{4} \sin \alpha \sin \beta\right)\right] x_{w}+} \\ {\left[r \sin \alpha \cos \beta-\left(k_{3} \cos \alpha \cos \beta+k_{4} \sin \alpha \cos \beta\right)\right] y_{w}=k_{3} t_{2}+k_{4} t_{3}-t_{3} r}\end{array} \tag{4} $$

等式(3)(4)右侧为有限数,所以当$x_w = 0、y_w \rightarrow \infty$时,

$$ c_{\mathrm{vp}} \sin \alpha \cos \beta-\left(-k_{1} \sin \beta+k_{2} \sin \alpha \cos \beta\right)=0 \tag{5} $$

$$ r_{v p} \sin \alpha \cos \beta-\left(k_{3} \cos \alpha \cos \beta+k_{4} \sin \alpha \cos \beta\right)=0 \tag{6} $$

所以

$$ \alpha=\operatorname{atan}\left(\frac{k_{3}}{r_{i p p}-k_{4}}\right) \tag{7} $$

$$ \beta=\operatorname{atan}\left(\frac{\left(k_{2}-c_{v p}\right) \sin \alpha}{k_{1}}\right) \tag{8} $$

在车道线检测中消失点的坐标是根据直线检测结果不断更新的,由公式(7)(8),根据更新更新的消失点坐标和相机内参数,可以计算出俯仰角和偏航角,从而对外参矩阵进行更新。