CN103065323A - 一种基于单应性变换矩阵的分段空间对准方法 - Google Patents

Abstract

本发明公开了一种基于单应性变换矩阵的分段空间对准方法，通过对较大的标定距离进行分段，针对每一分段分别求得摄像机和毫米波雷达坐标系之间的单应性变换矩阵，避免了现有技术中由于用同一个单应性变换矩阵表达两个传感器之间的坐标关系引起的误差，从而能够实现对较大标定距离目标探测的空间对准；通过推导表征摄像机和毫米波雷达的不同坐标系之间的关系，最后采用单应变换矩阵N表征两者的坐标系关系，用两个传感器分别获得的目标数据再解得单应变换矩阵N，避免了求解缩放比例因子、焦距等构成的摄像机内部参数矩阵以及旋转矩阵、平移向量构成的摄像机外部参数矩阵，大大简化了运算过程，节省运算时间。

Description

一种基于单应性变换矩阵的分段空间对准方法

技术领域

本发明涉及无人车多传感器信息融合技术领域，具体涉及一种基于单应性变换矩阵的分段空间对准方法。

背景技术

无人车又称室外智能移动机器人，是一种集环境感知、动态决策与规划、行为控制与执行等多功能为一体的智能化程度很高的装置，其对环境感知的快速性、准确性与多传感器信息融合技术密不可分。多传感器信息融合技术就是计算机充分利用各传感器资源，通过对各种量测信息的合理支配和使用，在空间和时间上把互补与冗余信息依据某种优化准则结合起来，产生对观测环境的一致性解释或描述，同时产生新的融合结果。在环境感知模块，视觉传感器和毫米波雷达是常用的两种传感器。视觉传感器检测范围广、能够获得外界环境中目标的尺寸和轮廓信息，但它易受外界因素影响，存在目标缺失问题。而毫米波雷达分辨率高、抗干扰能力强，可在各种天气环境下准确获得目标的距离、相对速度和方位角信息，但不能识别目标形状和大小，因此，利用这种互补特性融合两者信息，得到更加全面、可靠的环境信息。而空间对准是两者信息融合的前提。空间对准实质是估计摄像机和雷达坐标系间的变换矩阵关系。目前，传统空间对准方法中，在20米的标定距离范围内，在不同的距离点上，随机对目标上的点进行探测，分别得到目标在摄像机中的坐标系表达和雷达坐标系中的表达，根据两个传感器获取的数据，估计由缩放比例因子、焦距等构成的摄像机内部参数矩阵以及旋转矩阵、平移向量构成的摄像机外部参数矩阵，该计算过程比较繁琐，也容易引入误差；另外，根据上述算法对超过20米的标定距离的目标进行求解变换矩阵时，由于范围较大，因此巨大的误差，导致空间对准失败。

发明内容

有鉴于此，本发明提供了一种基于单应性变换矩阵的分段空间对准方法，能够在较大的标定距离范围内实现无人车装载的摄像机和毫米波雷达的空间对准，同时还能简化求解单应性变换矩阵的计算过程。

本发明的一种基于单应性变换矩阵的分段空间对准方法，包括如下步骤：

步骤1：建立摄像机坐标系与毫米波雷达坐标系之间的基于单应性变换矩阵的关系：

定义摄像机的图像坐标系O′uv，其中O′位于摄像机成像平面的左上角；u轴与摄像机扫描行方向平行；v轴垂直于摄像机扫描行方向；

定义O″″ρθ为毫米波雷达极坐标系，O″″为毫米波雷达表面的中心；ρ为目标与毫米波雷达间的直线距离；θ为目标偏离毫米波雷达扫描平面中心线的角度，则摄像机的图像坐标系O′uv与毫米波雷达极坐标系O″″ρθ之间关系表示为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

其中， $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ 定义为单应性变换矩阵；

步骤2：确定无人车与标定目标之间的合适的标定距离：

定义O″″X_rY_rZ_r表示毫米波雷达直角坐标系，O″″为毫米波雷达表面的中心；Y_r轴为毫米波雷达扫描平面中心线，垂直于毫米波雷达表面，指向正前方；X_r轴与Y_r垂直，指向右侧；Z_r轴垂直于X_r、Y_r确定的平面，指向上方；

则毫米波雷达直角坐标系和毫米波雷达极坐标系之间的关系为：

$\left[\begin{array}{c}{X}_r\\ Y_r\\ 1\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)\′$

标定目标与无人车的距离在毫米波雷达直角坐标系纵轴Y_r上的投影称为标定距离；在毫米波雷达的探测范围内，根据无人车的最大运动速度，确定合适的标定距离L；将标定距离L由近到远分为近距范围L1和远距范围L2，在近距范围L1内，将其均分成m1段，在远距范围L2内，将其均分成m2段，且保证L1/m1小于L2/m2；

步骤3：通过无人车中装载的摄像机和毫米波雷达分别采集标定目标的图像和数据信息：

将标定目标分别放置在步骤2中将标定距离L分成的不同分段处，毫米波雷达和摄像机分别对上述m1+m2段距离处的目标进行探测，在探测时，针对每段距离处的目标，将目标沿Y_r轴方向均分成m行，再将每一行沿X_r轴方向均分成h小段，控制毫米波雷达获取每个小段的坐标数据(X^M _rk，Y^M _rk)，控制摄像机拍摄每小段的图像数据f_k ^M，其中M＝1,...,(m1+m2)，k＝1,2,...,mh；

步骤4：针对步骤3中摄像机获得的每一个分段内每一个小段的图像数据f_k ^M，分别计算图像的质心坐标(u_k ^M,v_k ^M)；

步骤5：求解表示毫米波雷达坐标系与摄像机坐标系之间关系的单应性空间变换矩阵：

针对整个标定距离L中分出的每一段距离获得的所有小段对应的毫米波雷达坐标数据(

Y^M _rk)和摄像机的图像数据(u^M _k,v^M _k)组成每个分段内对应的数据集，将每个数据集分别代入到式（7）和(7)′中，得到：

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

和 $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

定义 $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^T，则单应性空间变换矩阵N^M的最小二乘解可表示为： $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ 其中，

和分别为： $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ 和 $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1};$

步骤6：实现视觉传感器和毫米波雷达的空间对准：

根据毫米波雷达扫描的标定目标的实际距离，判断该距离在步骤2中哪个分段内，在步骤5计算得到的m1+m2个结果中查找该距离对应的单应性空间变换矩阵，实现空间对准。

在步骤2中，当标定距离为50米时，0-20米为近距范围，均分成4段；20-50米为远距范围，均分成3段。

所述步骤4中计算标定目标图像中的每一小段图像的质心坐标的方法如下：

S40、手动选取含有标定目标的候选区域；

S41、对候选区域图像进行中值滤波，消除图像中的噪声；

S42、对候选区域图像进行Sobel算子边缘检测，得到二值化的标定目标边缘图像；

S43、在以像素为单位的图像坐标系中，沿u轴寻找标定目标边缘图像中的坐标最小值和最大值的u轴像素点坐标u_min,u_max，沿v轴寻找标定目标边缘图像中的坐标最小值和最大值的v轴像素点坐标v_min,v_max，将上述4点按顺时针或者逆时针方向用直线连接，形成一个四边形区域，在该四边形区域内，利用公式 ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 和 ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 计算标定目标的质心坐标(u_k ^M,v_k ^M)，其中f_k ^M(i,j)表示第M个距离分段上目标的第k个小段对应的四边形区域内像素点(i,j)的灰度值。

本发明具有如下有益效果：

1）通过对较大的标定距离进行分段，针对每一分段分别求得摄像机和毫米波雷达坐标系之间的单应性变换矩阵，避免了现有技术中由于用同一个单应性变换矩阵表达两个传感器之间的坐标关系引起的误差，从而能够实现对较大标定距离目标探测的空间对准；

2）通过推导表征摄像机和毫米波雷达的不同坐标系之间的关系，最后采用单应变换矩阵N表征两者的坐标系关系，用两个传感器分别获得的目标数据再解得单应变换矩阵N，避免了求解缩放比例因子、焦距等构成的摄像机内部参数矩阵以及旋转矩阵、平移向量构成的摄像机外部参数矩阵，大大简化了运算过程，节省运算时间；

3）根据无人车中的传感器对近距和远距目标的关注程度不同，对近距和远距的分段细度不同，在保证空间对准的同时，还能减小计算量；

4）在图像坐标系中，获得了目标的边缘图像后，分别找到两个轴方向的最大值和最小值的像素点，将该4个像素点围成四边形，再找到该四边形的质心，该方法能够快速确定每个目标边缘图像的质心，从而能够简化运算过程，同时还能使得空间对准更加准确。

附图说明

图1为摄像头针孔模型示意图；

图2为毫米波雷达坐标系示意图；

图3图像坐标系和毫米波雷达直角坐标系映射关系示意图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种基于单应性变换矩阵的分段空间对准方法，包括如下步骤：

步骤1：建立摄像机坐标系与毫米波雷达坐标系之间的基于单应性变换矩阵的关系：

如图1所示，OX_cY_cZ_c表示摄像机坐标系，原点O位于摄像机的光心；X_c轴平行于摄像机扫描行方向，指向扫描像素增大的方向；Y_c轴垂直于摄像机扫描行方向，指向扫描行增大的方向；Z_c轴垂直于成像平面，指向摄像头视线方向。O′uv表示以像素为单位的图像坐标系，O′位于成像平面的左上角；u轴与X_c平行；v轴与Y_c平行。O″xy表示以毫米为单位的图像坐标系，O″为成像平面的焦点；x轴与X_c平行；y轴与Y_c平行。f是摄像头的焦距，I表示成像平面。假设点P在坐标系OX_cY_cZ_c、O″xy和O′uv下的坐标分别为(X_c,Y_c,Z_c)，(x,y)和(u,v)，由P点在坐标系OX_cY_cZ_c和坐标系O″xy下的几何比例关系：

将上述关系表示成齐次形式有：

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]---\left(1\right)$

由P点在坐标系O″xy和坐标系O′uv坐标系下的缩放和平移关系：x＝S(u-u₀),y＝S(v-v₀)，表示成齐次形式有：

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=S\left[\begin{array}{ccc}1& 0& -u_0\\ 0& 1& -v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]---\left(2\right)$

其中S为缩放比例因子，(u₀,v₀)为坐标系O″xy原点O″在坐标系O′uv下的坐标。假定世界坐标系O″′X_wY_wZ_w，点P在此坐标系下的坐标为(X_w,Y_w,Z_w)，将两坐标系的关系用齐次形式表示有：

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(3\right)$

其中R和T分别表示旋转矩阵和平移向量。综合式（1）、式（2）和式（3），则有关系：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{\mathrm{\β}}\left[\begin{array}{ccc}1& 0& u_0\\ 0& 1& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(4\right)$

其中，β＝Z_c·S。

如图2所示，O″″X_rY_rZ_r表示毫米波雷达直角坐标系，O″″为毫米波雷达表面的中心；Y_r轴为毫米波雷达扫描平面中心线，垂直于毫米波雷达表面，指向正前方；X_r轴与Y_r垂直，指向右侧；Z_r轴垂直于X_r、Y_r确定的平面，指向上方。O″″ρθ表示毫米波雷达极坐标系，原点与坐标系O″″X_rY_rZ_r的原点重合；ρ为目标与毫米波雷达间的直线距离；θ为目标偏离毫米波雷达扫描平面中心线的角度。点P在O″″ρθ与O″″X_rY_rZ_r下的坐标分别为(ρ,θ)和(X_r,Y_r,Z_r)，由于毫米波雷达的扫描平面为坐标系O″″ρθ下一个二维平面，有Z_r＝0，点P在O″″ρθ和O″″X_rY_rZ_r下的三角关系表示成：

$\left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 0\end{array}\right]---\left(5\right)$

假定毫米波雷达直角坐标系为世界坐标系，借助世界坐标系为中间变量并结合式（4）和式（5），则图像坐标系O′uv和毫米波极坐标系O″″ρθ关系表示为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{\mathrm{\β}}\left[\begin{array}{ccc}1& 0& u_0\\ 0& 1& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 0\\ 1\end{array}\right]---\left(6\right)$

如图3所示，当摄像头和毫米波雷达观测同一目标时，可通过式（6）把毫米波雷达扫描到的目标点投影到摄像头采集的图像中。但需要对缩放因子S、摄像头焦距f等摄像头内部参数以及旋转矩阵R、平移向量T等外部参数进行估计，计算过程较复杂，为简化计算，利用一个等价的变换关系N表示O′uv和O″″ρθ的关系，表示为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

其中， $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ 称为单应性变换矩阵。

步骤2：确定合适的标定距离并对标定距离进行合理分段：

我们把毫米波雷达极坐标系下量测的标定目标距离ρ称为标定距离，将标定距离L由近到远分为近距范围L1和远距范围L2，在近距范围L1内，将其均分成m1段，在远距范围L2内，将其均分成m2段，由于在无人车探测过程中，需要对较近的目标进行精确探测，而对于远距离目标只需大致的判断，因此，远距范围分段可以比对近距范围分段大一些，所示L1/m1小于L2/m2；

在本实施例中，无人车规定的最大速度在36km/h，根据无人车在行驶过程中关注的距离区域与其速度大小相关的关系可知，我们确定50m为合适的标定距离。在50m的标定距离内，通过实验得知：利用单一的单应性空间变换矩阵会导致对准失败，因此，根据无人车速度要求以及关注距离区域的不同，在标定距离为0-20m内，间隔5m为一段共4段；在标定距离为20-50m内，间隔10m为一段共3段。

步骤3：通过无人车中装载的摄像机和毫米波雷达分别采集标定目标的图像和数据信息：

为保证空间对准的准确性，在每个分段中，分别在纵向取五行，每行在横向取7组标定目标对应的图像和数据信息。利用安装在无人车上的IEEE1394接口的摄像头和CAN总线接口的毫米波雷达采集同一场景下标定目标的图像和数据信息，并将信息传输到工控机中。

步骤4：针对步骤3中摄像机获得的每一个分段内每一个小段的图像数据f_k ^M，分别计算图像的质心坐标(u_k ^M,v_k ^M)；

一般，毫米波雷达采用的聚类方法是最近领域方法，因此，我们认为毫米波雷达在毫米波雷达坐标系下扫描的标定目标数据信息与图像坐标系下质心所在位置的坐标值相互对应。对数字图像f，f(i,j)表示灰度值，(i,j)表示图像区域中的点。计算图像信息中标定目标的质心坐标，采用如下步骤：

S40、手动选取含有标定目标的候选区域，以减少背景对标定目标质心计算的干扰。

S41、对候选区域进行中值滤波，消除图像中噪声干扰。中值滤波通过表达式f(i,j)＝Median{f(i-k,j-l)实现。其中，(k,l)∈ω，ω为像素的3×3领域。

S42、对候选区域图像进行Sobel算子边缘检测，得到含有标定目标的候选区域边缘图像。Sobel算子采用模版 $\mathrm{mask}=\left[\begin{array}{ccc}-1& -2& -1\\ 0& 0& 0\\ 1& 2& 1\end{array}\right],$ 将候选区域图像与mask作卷积运算，得到候选区域的二值图像。

S43、在以像素为单位的图像坐标系中，沿u轴寻找标定目标边缘图像中的坐标最小值和最大值的u轴像素点坐标u_min,u_max，沿v轴寻找标定目标边缘图像中的坐标最小值和最大值的v轴像素点坐标v_min,v_max，将上述4点按顺时针或者逆时针方向用直线连接，形成一个四边形区域，在该四边形区域内，利用公式 ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 和 ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 计算标定目标的质心坐标(u_k ^M,v_k ^M)，其中f_k ^M(i,j)表示第M个距离分段上目标的第k个小段对应的四边形区域内像素点(i,j)的灰度值；

步骤5：求解表示毫米波雷达坐标系与摄像机坐标系之间关系的单应性空间变换矩阵：

针对整个标定距离L中分出的每一段距离获得的所有小段对应的毫米波雷达坐标数据(

Y^M _rk)和摄像机的图像数据(u^M _k,v^M _k)组成每个分段内对应的数据集，将每个数据集分别代入到式(5)和(7)中，得到：

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

和 $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

定义 $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^T，则单应性空间变换矩阵N^M的最小二乘解可表示为： $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ 其中，

和

分别为： $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ 和 $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1};$

步骤6：实现视觉传感器和毫米波雷达的空间对准：

根据毫米波雷达扫描的标定目标的实际距离，首先，判断该距离在步骤2中哪个分段内，然后，根据步骤5计算得到的该分段内单应性空间变换矩阵的N^M（M＝1,2,...,7），实现空间对准。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于单应性变换矩阵的分段空间对准方法，其特征在于，包括如下步骤：

步骤1：建立摄像机坐标系与毫米波雷达坐标系之间的基于单应性变换矩阵的关系：

定义摄像机的图像坐标系O′uv，其中O′位于摄像机成像平面的左上角；u轴与摄像机扫描行方向平行；v轴垂直于摄像机扫描行方向；

定义O″″ρθ为毫米波雷达极坐标系，O″″为毫米波雷达表面的中心；ρ为目标与毫米波雷达间的直线距离；θ为目标偏离毫米波雷达扫描平面中心线的角度，则摄像机的图像坐标系O′uv与毫米波雷达极坐标系O″″ρθ之间关系表示为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

其中， $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ 定义为单应性变换矩阵；

步骤2：确定无人车与标定目标之间的合适的标定距离：

定义O″″X_rY_rZ_r表示毫米波雷达直角坐标系，O″″为毫米波雷达表面的中心；Y_r轴为毫米波雷达扫描平面中心线，垂直于毫米波雷达表面，指向正前方；X_r轴与Y_r垂直，指向右侧；Z_r轴垂直于X_r、Y_r确定的平面，指向上方；

则毫米波雷达直角坐标系和毫米波雷达极坐标系之间的关系为：

$\left[\begin{array}{c}{X}_r\\ Y_r\\ 1\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)\′$

标定目标与无人车的距离在毫米波雷达直角坐标系纵轴Y_r上的投影称为标定距离；在毫米波雷达的探测范围内，根据无人车的最大运动速度，确定合适的标定距离L；将标定距离L由近到远分为近距范围L1和远距范围L2，在近距范围L1内，将其均分成m1段，在远距范围L2内，将其均分成m2段，且保证L1/m1小于L2/m2；

步骤3：通过无人车中装载的摄像机和毫米波雷达分别采集标定目标的图像和数据信息：

将标定目标分别放置在步骤2中将标定距离L分成的不同分段处，毫米波雷达和摄像机分别对上述m1+m2段距离处的目标进行探测，在探测时，针对每段距离处的目标，将目标沿Y_r轴方向均分成m行，再将每一行沿X_r轴方向均分成h小段，控制毫米波雷达获取每个小段的坐标数据(X^M _rk，Y^M _rk)，控制摄像机拍摄每小段的图像数据f_k ^M，其中M＝1,...,(m1+m2)，k＝1,2,...,mh；

步骤4：针对步骤3中摄像机获得的每一个分段内每一个小段的图像数据f_k ^M，分别计算图像的质心坐标(u_k ^M,v_k ^M)；

步骤5：求解表示毫米波雷达坐标系与摄像机坐标系之间关系的单应性空间变换矩阵：

针对整个标定距离L中分出的每一段距离获得的所有小段对应的毫米波雷达坐标数据(

Y^M _rk)和摄像机的图像数据(u^M _k,v^M _k)组成每个分段内对应的数据集，将每个数据集分别代入到式（7）和(7)′中，得到：

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

和 $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

定义 $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^T，则单应性空间变换矩阵N^M的最小二乘解可表示为： $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ 其中，

和

分别为： $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ 和 $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1;}$

步骤6：实现视觉传感器和毫米波雷达的空间对准：

根据毫米波雷达扫描的标定目标的实际距离，判断该距离在步骤2中哪个分段内，在步骤5计算得到的m1+m2个结果中查找该距离对应的单应性空间变换矩阵，实现空间对准。

2.如权利要求1所述的一种基于单应性变换矩阵的分段空间对准方法，其特征在于，当步骤2的标定距离为50米时，0-20米为近距范围，均分成4段；20-50米为远距范围，均分成3段。

3.如权利要求1所述的一种基于单应性变换矩阵的分段空间对准方法，其特征在于，所述步骤4中计算标定目标图像中的每一小段图像的质心坐标的方法如下：

S40、手动选取含有标定目标的候选区域；

S41、对候选区域图像进行中值滤波，消除图像中的噪声；

S42、对候选区域图像进行Sobel算子边缘检测，得到二值化的标定目标边缘图像；

S43、在以像素为单位的图像坐标系中，沿u轴寻找标定目标边缘图像中的坐标最小值和最大值的u轴像素点坐标u_min,u_max，沿v轴寻找标定目标边缘图像中的坐标最小值和最大值的v轴像素点坐标v_min,v_max，将上述4点按顺时针或者逆时针方向用直线连接，形成一个四边形区域，在该四边形区域内，利用公式 ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 和 ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ 计算标定目标的质心坐标(u_k ^M,v_k ^M)，其中f_k ^M(i,j)表示第M个距离分段上目标的第k个小段对应的四边形区域内像素点(i,j)的灰度值。

Images