#每周一篇论文4#[感知]毫米波雷达与摄像头联合标定

源代码下载

• 开源付费版:https://download.csdn.net/download/hgz_gs/15546455
• 半开源-体验版:https://download.csdn.net/download/hgz_gs/15546407

系列其他资源

激光雷达外参数自标定
激光雷达标定

一、坐标系说明

下图为车载6个摄像头、1个360度激光雷达、5个毫米雷达、1个GPS及1个惯性测量传感器以及车体base坐标系的坐标系框图。

传感器类型	X轴方向	Y轴方向	Z轴方向	原点
Camera	右	地	前
Radar	前	左	天
Lidar	右	前	天
GPS	东	北	天
IMU	前	左	天
车体base坐标系	右	前	天	车辆后轴中心

1.1 摄像头相关坐标系

1.2 毫米波雷达相关坐标系

1.3 GPS相关坐标系

1.4 车体相关坐标系

1.5 其他坐标系

1.5.1 左右手坐标系

左手坐标系：伸开我们的左手, 掌心向外, 大拇指与食指成90度, 中指、无名指和小指弯曲, 大拇指指向的方向就是X轴正方向, 食指指向的方向就是Y轴正方向, 中指、无名指和小指指向的方向就是Z轴正方向。用左手的大拇指指向轴的正方向，弯曲手指。那么手指所指示的方向即是轴的正旋转方向。

右手坐标系:伸开我们的右手, 掌心向内, 大拇指与食指成90度, 中指、无名指和小指弯曲, 大拇指指向的方向就是X轴正方向, 食指指向的方向就是Y轴正方向, 中指、无名指和小指指向的方向就是Z轴正方向。用右手的大拇指指向轴的正方向，弯曲手指。那么手指所指示的方向即是轴的正旋转方向。

左手坐标系和右手坐标系中, X轴和Y轴的方向是相同的, Z轴的方向相反。在计算机中通常使用的是左手坐标系，而数学中则通常使用右手坐标系。

二、毫米波和摄像机联合标定

传感器类型	X轴方向	Y轴方向	Z轴方向	坐标系
Camera	右	地	前	$O{X}_c{Y}_c{Z}_c$
Radar	前	左	天	$O{X}_r{Y}_r{Z}_r$

2.1 毫米波$O{X}_r{Y}_r{Z}_r$坐标系转摄像机$O{X}_c{Y}_c{Z}_c$坐标系

假设毫米传感器在摄像机坐标系下的坐标为$X_{offset}{Y}_{offset}{Z}_{offset}$；$O{X}_r{Y}_r{Z}_r$坐标系在$O{X}_c{Y}_c{Z}_c$坐标系下的姿态为$(\alpha ,\beta ,\gamma )$(俯仰角pitch、航向角yaw、翻滚角roll)；在$X_c$轴的旋转矩阵为$R_x$, 在$Y_c$轴的旋转矩阵为$R_y$, 在$Z_c$轴的旋转矩阵为$R_z$；则由前左天$O{X}_r{Y}_r{Z}_r$坐标系转右地前$O{X}_c{Y}_c{Z}_c$坐标系：
$$\{\begin{array}{l}{X}_c=R_x(-Y_r)+X_{offset}\\ Y_c=R_y(-Z_r)+Y_{offset}\\ Z_c=R_z(X_r)+Z_{offset}\end{array}$$

写成矩阵的形式：
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left\{\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\alpha )& sin(\alpha )\\ 0& -sin(\alpha )& cos(\alpha )\end{array}\right]\left[\begin{array}{ccc}cos(\beta )& 0& -sin(\beta )\\ 0& 1& 0\\ sin(\beta )& 0& cos(\beta )\end{array}\right]\left[\begin{array}{ccc}cos(\gamma )& sin(\gamma )& 0\\ -sin(\gamma )& cos(\gamma )& 0\\ 0& 0& 1\end{array}\right]\right\}\left\{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right]\ast \left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]\right\}+\left\{\left[\begin{array}{c}{X}_{offset}\\ Y_{offset}\\ Z_{offset}\end{array}\right]\right\}$$
接下来就是相机的标定原理了。

雷达转相机坐标系:
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left\{R|T\right\}\left\{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right]\ast \left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]\right\}\left\{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right]\ast \left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]\right\}+\left\{\left[\begin{array}{c}{X}_{offset}\\ Y_{offset}\\ Z_{offset}\end{array}\right]\right\}$$
因为毫米波雷达的$Z_r$为0，也就时相机坐标系的$Y_c$为0，没有$Y_c$坐标轴。对上式进行化简可得:
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left\{\left[\begin{array}{ccc}cos(\beta )& 0& -sin(\beta )\\ 0& 1& 0\\ sin(\beta )& 0& cos(\beta )\end{array}\right]\right\}\left\{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\\ 1& 0& 0\end{array}\right]\ast \left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]\right\}+\left\{\left[\begin{array}{c}{X}_{offset}\\ Y_{offset}\\ Z_{offset}\end{array}\right]\right\}$$

相机坐标系转像素坐标系:
$$z_{c1}\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left\{\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_0\\ 0& \frac{1}{d_y}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\right\}\left\{\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]\right\}$$

附录A： 旋转矩阵的推导

一般两坐标进行旋转变换时沿逆时针旋转。

二维旋转

法一

有点P(Pa,PYa)[(Xa，Ya)]，当坐标由 x –> y 旋转 b(θ） 度后，求该点在新坐标轴的坐标是多少。

法二

有点P（Xa，Ya），当坐标由 x –> y 旋转 θ 度后，求该点在新坐标轴的坐标是多少。

三维旋转

三维旋转，需要先搞清楚正、负方向（使用的是右手法则，在二维平面增加一维z，它的正方向朝向屏幕外）

绕x轴进行旋转（在yz平面逆时针旋转）:

绕y轴进行旋转（在zx平面逆时针旋转）:

绕z轴进行旋转（在xy平面逆时针旋转）:

参考:https://mathworld.wolfram.com/RotationMatrix.html

附件B:三角公式