一种全景环带相机与激光雷达联合标定的方法与流程

1.本发明涉及车载全景感知与移动机器人的技术领域，尤其涉及一种全景环带相机与激光雷达联合标定的方法。


背景技术：

2.激光雷达作为各类家用智能机器人、服务机器人、无人小车和车载自动驾驶系统的传感器，具有高精度的测距能力，可以准确快速地获取环境物体的相对空间坐标。全景环带相机有一颗全景环形透镜(panoramic annular lens，pal)的二次反射镜头，能够以单目的方式实现水平360
°
光学信息的记录，可以以一次成像的方式获取环境物体的颜色信息。360
°
激光雷达提供全向的距离信息但是信息度单一，分辨率不高；全景环带相机提供rgb图像，分辨率高，但是没有距离信息。将二者结合，发挥各自传感能力的优势，将三维点精确映射到全景环带图像上，形成全景环带与激光雷达融合系统，可以输出更高维度的传感器信息，用以解决三维建图、物体识别等各类环境感知问题。
3.现有技术中，申请号为201010223750.7的专利文献公开了一种单线激光雷达和ccd相机相互关系的标定方法，基于ccd相机可以弱成像单线激光雷达使用的红外光源的这一条件。首先，在立方体标定键辅助下提取位于扫描面内的虚拟控制点；然后，使用红外滤光片滤掉可见光，仅使红外光成像，然后对有扫描线信息的红外图像进行增强、二值化处理再进行hough变换，提取两条激光扫描线，扫描线交点即为虚拟控制点在图像中的像坐标。
4.公开号为cn106556825a的专利公开了一种全景视觉成像系统的联合标定方法。该联合标定方法利用多光束激光雷达和视觉相机针对同一目标景物所采集图像的法向量在空间内平行的原理，建立两幅图像的之间的映射关系，从而实现多光束激光雷达点云图像和视觉相机图像间的联合标定。虽然上述方法可以实现相机和雷达之间的联合标定，但是上述专利采用多个相机，其需要将激光雷达轴线和相机光心垂直，且还要转到完全一样的角度，存在操作复杂，很难在实际中应用的问题。


技术实现要素：

5.本发明的目的是针对现有技术的缺陷，提供了一种全景环带相机与激光雷达联合标定的方法。
6.为了实现以上目的，本发明采用以下技术方案：
7.一种全景环带相机与激光雷达联合标定的方法，包括全景环带相机、激光雷达、棋盘格的标定板，所述全景环带相机置于激光雷达上方，且全景环带相机的光轴与激光雷达的旋转轴重合；标定板置于全景环带相机与激光雷达的共视区域；所述标定板上设有编号；
8.标定方法包括：
9.s1.获取激光雷达的点云数据以及全景环带相机的全景环带图像数据；
10.s2.根据全景环带图像数据标定全景环带相机，得到全景环带相机的相机参数；
11.s3.提取激光雷达的点云数据中所有标定板上的点云数据，并对提取出的点云数
据使用最小二乘法得到每块标定板在激光雷达坐标系下的平面方程；
12.s4.计算每块标定板在激光雷达坐标系下的平面方程的法向量，并对计算得到的激光雷达点云数据的法向量进行归一化处理；
13.s5.计算每块标定板在全景环带相机坐标系下的法向量，并对计算得到的全景环带相机图像数据的法向量进行归一化处理；
14.s6.将归一化处理后的标定板点云数据法向量和全景环带图像数据法向量根据标定结果投影至全景环带图像上，并记录标定板在全景环带图像、点云数据下对应法向量的各自编号及像素坐标；
15.s7.基于记录的编号，采用最小二乘法对归一化处理后标定板的点云数据法向量和归一化处理后标定板的全景环带图像数据法向量进行拟合，得到拟合后的标定板点云数据法向量和全景环带图像数据法向量的像素坐标；
16.s8.计算步骤s7中拟合后的点云数据法向量的像素坐标与步骤s6中点云数据法向量的像素坐标之间的旋转矩阵与平移矩阵，完成全景环带相机与激光雷达的联合标定。
17.进一步的，所述步骤s3中得到每块标定板在激光雷达坐标系下的平面方程，表示为：
18.z＝ax+by+c
ꢀꢀꢀ
(1)
19.其中，x、y、z表示空间坐标；a、b、c表示平面方程系数。
20.进一步的，所述步骤s3中还包括对每块标定板在激光雷达坐标系下的平面方程进行求解，得到最小二乘解；具体为：
21.平面方程的代价函数表示为：
22.s＝∑(ax
i
+by
i
+c
‑
z
i
)2ꢀꢀꢀ
(2)
23.其中，s表示代价函数；
24.对a、b、c分别求偏导并整理，表示为：
[0025][0026]
其中，n表示平面拟合点的总数；
[0027]
将激光雷达的点云数据相对应的坐标(x
i
,y
i
,z
i
)代入公式(3)，得到平面方程的a、b、c，即a、b、c为最小二乘解。
[0028]
进一步的，所述步骤s3中得到每块标定板在激光雷达坐标系下的平面方程，表示为：
[0029]
ax+by+cz+d＝0
ꢀꢀꢀ
(4)
[0030]
其中，a、b、c、d表示平面方程系数；x、y、z表示空间坐标。
[0031]
进一步的，所述步骤s4中对计算得到的激光雷达法向量进行归一化处理，法向量表示为：
[0032][0033]
其中，a、b、c表示平面方程系数。
[0034]
进一步的，所述步骤s6中根据标定结果投影至全景环带图像上中的投影数据为：
[0035][0036]
其中，c
x
，c
y
表示全景环带相机镜头中心的像素坐标值；π表示投影函数；p(u,v)表示全景环带图像的像素坐标；p(x,y,z)表示世界坐标；θ、ρ1(θ)分别表示为：
[0037][0038]
ρ1(θ)＝a0+a2θ2+a3θ3+
…
+a
n
θ
n
[0039]
其中，a
n
表示多项式系数；n表示项数；
[0040]
反投影表示为：
[0041][0042]
其中，π
‑1表示反投影函数，d
p
表示三维深度；r、ρ2(r)分别表示为：
[0043][0044]
ρ2(r)＝b0+b2r2+b3r3+
…
+b
n
r
n
[0045]
其中，b
n
表示多项式系数，n表示项数。
[0046]
进一步的，所述步骤s7具体为：
[0047]
设全景环带图像法向量的像素坐标为(u
ci
,v
ci
)，点云数据法向量的像素坐标为(u
li
,v
ii
)，其中，i为标定板的编号；
[0048]
以(0,0)为原点指向(u
li
,v
li
)建立向量ζ
li
，则每个ζ
li
(u
li
,v
li
)(i＞1)均采用ζ
l1
(u
l1
,v
l1
)表示：
[0049][0050]
其中，θ
li
表示ζ
li
与ζ
l1
夹角；t
ul1
、t
vl1
表示平移量；
[0051]
[0052]
因为(u
li
,v
li
)、(u
l1
,v
l1
)、θ
li
为已知，则可得到：
[0053][0054]
则：
[0055][0056][0057]
其中，r
li
、t
li
表示ζ
li
到ζ
l1
的旋转和平移矩阵；
[0058]
若点云数据法向量与全景环带图像法向量拟合后的坐标为(u
′
li
,v
′
li
)；
[0059]
对u
′
li
与v
′
li
分别求偏导并等于0，表示为：
[0060][0061][0062]
其中，s表示代价函数；
[0063]
s＝∑((u
ci
‑
u
′
li
)2+(v
ci
‑
v
′
li
)2)
[0064]
各个(u
′
li
,v
′
li
)与(u
li
,v
li
)对应点相对位置关系不变，即，使用(u
′
l1
,v
′
l1
)表示(u
′
li
,v
′
li
)时，对应的r
′
li
＝r
li
，t
′
li
＝t
li
；
[0065]
令(u
′
l1
,v
′
l1
)表示(u
′
li
,v
′
li
)，将所有(u
ci
,v
ci
)代入公式(9)、(10)，得到拟合后的点云数据法向量的像素坐标(u
′
l1
,v
′
l1
)。
[0066]
进一步的，所述步骤s8中计算步骤s7中拟合后的点云数据法向量的像素坐标与步骤s6中点云数据法向量的像素坐标之间的旋转矩阵与平移矩阵，表示为：
[0067][0068]
其中，θ表示ζ
′
l1
与ζ
l1
夹角；t
u
、t
v
表示平移量；
[0069][0070][0071]
记：
[0072][0073][0074]
其中，r、t表示旋转矩阵与平移矩阵。
[0075]
进一步的，所述步骤s8中完成全景环带相机与激光雷达的联合标定具体为：将激
光雷达坐标投影至全景环带图像上后，通过旋转和平移的变换，将点云数据与全景环带图像数据对齐，全景环带相机与激光雷达的联合标定完成。
[0076]
进一步的，所述步骤s3中提取激光雷达的点云数据中所有标定板上的点云数据中的提取方法包括聚类、分割、手动识别、滤波。
[0077]
与现有技术相比，本发明使用的标定方法弥补了当前全景环带相机与激光雷达联合标定的不足，且标定板不苛求摆放位置，激光雷达为水平360
°
采集，非单线即可，对高低精度的激光雷达均可进行标定，标定后的全景环带相机与激光雷达模组可以实现像素级的融合效果，在全景环境感知领域与移动机器人领域可以提供高维度的输出，具有广阔的应用前景。
附图说明
[0078]
图1是实施例一提供的全景环带
‑
激光雷达融合装置结构图；
[0079]
图2是实施例一提供的一种全景环带相机与激光雷达联合标定的方法流程图。
具体实施方式
[0080]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0081]
本发明的目的是针对现有技术的缺陷，提供了一种全景环带相机与激光雷达联合标定的方法。
[0082]
实施例一
[0083]
本实施例提供一种全景环带相机与激光雷达联合标定的方法，如图1所示，包括全景环带相机1、激光雷达2、棋盘格的标定板，全景环带相机1置于激光雷达2上方，且全景环带相机1的光轴与激光雷达2的旋转轴重合；标定板置于全景环带相机1与激光雷达2的共视区域；标定板上设有编号。
[0084]
本发明实施例采用fov为(
‑
30
°
～+30
°
)
×
360
°
的全景环带相机，搭配fov为(
‑
25
°
～+15
°
)
×
360
°
的32线激光雷达，全景环带相机与激光雷达刚性连接，全景环带镜头的光轴与激光雷达的旋转轴平行或重合，使两者相对位姿不得发生旋转与平移，进而形成一体化的全景环带
‑
激光雷达融合装置，本实施例的标定板为普通硬板上粘贴打印棋盘格，本应用实例使用8块标定板，对每块做好编号。
[0085]
需要指出的是，标定板可使用高反射率透明板，在远离激光雷达的方向朝向激光雷达粘贴棋盘格，或是直接使用高反射率的棋盘格硬板，以提升激光雷达接收到的强度信息。
[0086]
在进行联合标定时，将全景环带
‑
激光雷达融合装置固定在实行标定的场地中，标定板的棋盘格面朝相机，移动标定板使棋盘格尽量充满全景环带图像的径向视场。
[0087]
在本实施例中，由于采用的是全景环带相机，则相机需要设置于雷达的上方，不然雷达会挡住相机的视场角。
[0088]
本实施例的联合标定方法如图2所示，包括：
[0089]
步骤s1.获取激光雷达的点云数据以及全景环带相机的全景环带图像数据；
[0090]
将标定板置于全景环带相机与激光雷达的共视区域，并同时存储激光雷达点云数据与全景环带图像数据，其中标定板在全景环带图像中尽可能多地占据径向视场。
[0091]
步骤s2.根据全景环带图像数据标定全景环带相机，得到全景环带相机的相机参数；
[0092]
对全景环带相机进行标定的目的是为了求出相机的参数，且这个求解参数的过程就称之为相机标定，求出的解即为相机标定结果。
[0093]
本实施例通过泰勒级数描述相机模型的工具箱，且设全景环带图像的像素坐标为p(u,v)，世界坐标为p(x,y,z)，则全景环带图像的像素坐标和世界坐标相互投影的关系如下：
[0094]
将世界坐标转换为像素坐标，表示为：
[0095][0096]
其中，c
x
，c
y
表示全景环带相机镜头中心的像素坐标值；π表示投影函数；θ、ρ1(θ)由下式给出：
[0097][0098]
ρ1(θ)＝a0+a2θ2+a3θ3+
…
+a
n
θ
n
[0099]
其中，a
n
表示多项式系数；n表示项数；
[0100]
反投影，即将像素坐标转换为世界坐标，表示为：
[0101][0102]
其中，π
‑1表示反投影函数，d
p
表示三维深度，对于本实施例可置为1；r、ρ2(r)由下式给出：
[0103][0104]
ρ2(r)＝b0+b2r2+b3r3+
…
+b
n
r
n
[0105]
其中，b
n
表示多项式系数，n表示项数。
[0106]
根据标定图像可以求解出多项式系数a0,a2,a3,
…
,a
n
和b0,b2,b3,
…
,b
n
。
[0107]
步骤s3.提取激光雷达的点云数据中所有标定板上的点云数据，并对提取出的点云数据使用最小二乘法得到每块标定板在激光雷达坐标系下的平面方程；
[0108]
提取方法可使用聚类、分割、手动识别等方法，根据接收的激光强度不同还可使用滤波；本实施例以手动提取为例。
[0109]
根据步骤s1中获得的点云数据手动提取出所有标定板上的点云数据，对提取到的点云数据使用最小二乘法拟合出每块标定板在激光雷达坐标系下的平面方程。其中对于每块标定板的平面方程表示为：
[0110]
z＝ax+by+c
[0111]
其中，x、y、z表示空间坐标；a、b、c表示平面方程系数。
[0112]
对每块标定板在激光雷达坐标系下的平面方程进行求解，得到最小二乘解；具体为：
[0113]
平面方程的代价函数表示为：
[0114]
s＝∑(ax
i
+by
i
+c
‑
z
i
)2[0115]
其中，s表示代价函数；
[0116]
对a、b、c分别求偏导并整理，表示为：
[0117][0118]
其中，n表示平面拟合点的总数；
[0119]
将激光雷达的点云数据相对应的坐标(x
i
,y
i
,z
i
)代入上述线性方程组的公式中，得到平面方程的a、b、c，即a、b、c为最小二乘解。
[0120]
在本实施例中，每块标定板在激光雷达坐标系下的平面方程，还可以表示为：
[0121]
ax+by+cz+d＝0
[0122]
其中，a、b、c、d表示平面方程系数；x、y、z表示空间坐标。
[0123]
步骤s4.计算每块标定板在激光雷达坐标系下的平面方程的法向量，并对计算得到的激光雷达点云数据的法向量进行归一化处理；
[0124]
对计算得到的激光雷达法向量进行归一化处理，法向量表示为：
[0125][0126]
需要说明的是，若平面方程采用z＝ax+by+c，则对应的法向量采用的形式。
[0127]
若平面方程采用ax+by+cz+d＝0，则对应的法向量采用的形式。
[0128]
步骤s5.计算每块标定板在全景环带相机坐标系下的法向量，并对计算得到的全
景环带相机图像数据的法向量进行归一化处理；
[0129]
步骤s2标定完成后，工具箱会返回每个标定板所有角点的坐标，且
[0130]
与步骤s3相似的，对于每块标定板的平面方程表示为：
[0131]
zax+by+c
[0132]
其中，x、y、z表示空间坐标；a、b、c表示平面方程系数。
[0133]
然后对每块标定板在激光雷达坐标系下的平面方程进行求解，得到最小二乘解；具体为：
[0134]
平面方程的代价函数表示为：
[0135]
s＝∑(ax
i
+by
i
+c
‑
z
i
)2[0136]
其中，s表示代价函数；
[0137]
对a、b、c分别求偏导并整理，表示为：
[0138][0139]
其中，n表示平面拟合点的总数；
[0140]
将全景环带图像数据上各标定板相对应的角点坐标(x
i
,y
i
,z
i
)代入上述线性方程组的公式中，得到平面方程的a、b、c，即a、b、c为最小二乘解。
[0141]
步骤s6.将归一化处理后的标定板点云数据法向量和全景环带图像数据法向量根据标定结果投影至全景环带图像上，并记录标定板在全景环带图像、点云数据下对应法向量的各自编号及像素坐标；
[0142]
根据步骤s2的投影关系将步骤s4、s5得到的各个标定板的法向量投影至全景环带图像的像素平面上，并记录标定板在全景环带图像、点云数据下对应的法向量的各自编号与像素坐标。
[0143]
本实施例将法向量转到图像处理，即采用二维的方式进行处理，减少了计算量，且计算能力加快。
[0144]
步骤s7.基于记录的编号，采用最小二乘法对归一化处理后标定板的点云数据法向量和归一化处理后标定板的全景环带图像数据法向量进行拟合，得到拟合后的标定板点云数据法向量和全景环带图像数据法向量的像素坐标；
[0145]
将激光雷达点云数据和全景环带图像算得的法向量各自视为一个整体，根据对应编号计算二者的最小二乘拟合，记录拟合后的激光雷达数据法向量坐标。
[0146]
设全景环带图像法向量的像素坐标为(u
ci
,v
ci
)，激光雷达点云数据法向量的像素坐标为(u
li
,v
li
)，其中i为标定板编号。
[0147]
以(0,0)为原点指向(u
li
,v
li
)建立向量ζ
li
，则每个ζ
li
(u
li
,v
li
)(i＞1)均可采用ζ
l1
(u
l1
,v
l1
)表示：
[0148]
[0149]
其中，θ
li
表示ζ
li
与ζ
l1
夹角；t
ul1
、t
vl1
表示平移量；
[0150][0151]
因为(u
li
,v
li
)、(u
l1
,v
l1
)、θ
li
都已知，可算得：
[0152][0153]
记：
[0154][0155][0156]
其中，r
li
、t
li
表示ζ
li
到ζ
l1
的旋转和平移矩阵。
[0157]
采用最小二乘法对点云数据法向量与全景环带图像法向量进行拟合，设拟合后的坐标为(u
′
li
,v
′
li
)(i＞0)；
[0158]
拟合中涉及的代价函数s表示为：
[0159]
s＝∑((u
ci
‑
u
′
li
)2+(v
ci
‑
v
′
li
)2)
[0160]
对u
l
l
i
与v
′
li
分别求偏导并等于0，表示为：
[0161][0162][0163]
各个(u
′
li
,v
′
li
)与(u
li
,v
li
)对应点相对位置关系不变，即，使用(u
′
l1
,v
′
l1
)表示(u
′
li
,v
′
li
)时，对应的r
′
li
＝r
li
，t
′
li
＝t
li
。
[0164]
令(u
′
l1
,v
′
l1
)表示(u
′
li
,v
′
li
)，将所有(u
ci
,v
ci
)代入上述两个公式中，得到拟合后的点云数据法向量的像素坐标(u
′
l1
,v
′
′
i1
)。
[0165]
步骤s8.计算步骤s7中拟合后的点云数据法向量的像素坐标与步骤s6中点云数据法向量的像素坐标之间的旋转矩阵与平移矩阵，完成全景环带相机与激光雷达的联合标定。
[0166]
计算步骤s7中拟合后的标定板点云数据法向量的像素坐标与步骤s6中标定板点云数据法向量的像素坐标之间的旋转矩阵与平移矩阵，表示为：
[0167][0168]
其中，θ表示ζ
′
l1
与ζ
l1
夹角；t
u
、t
v
表示平移量；
[0169]
[0170][0171]
记：
[0172][0173][0174]
其中，r、t表示旋转矩阵与平移矩阵。
[0175]
将激光雷达坐标投影至全景环带图像上后，通过上述旋转和平移的变换，可将点云数据与全景环带图像对齐，全景环带相机与激光雷达的联合标定完成。
[0176]
与现有技术相比，本实施例使用的标定方法弥补了当前全景环带相机与激光雷达联合标定的不足，且标定板不苛求摆放位置，激光雷达为水平360
°
采集，非单线即可，对高低精度的激光雷达均可进行标定，标定后的全景环带相机与激光雷达模组可以实现像素级的融合效果，在全景环境感知领域与移动机器人领域可以提供高维度的输出，具有广阔的应用前景。
[0177]
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。