用于移动机器人的多传感器标定方法与流程

1.本发明属于机器人技术领域，具体涉及一种用于移动机器人的多传感器标定方法。


背景技术：

2.大多数商用移动机器人都配备了几个深度摄像头，以感知环境中的三维信息。深度摄像头通过发射和接收红外光束来测量物体在环境中的位置。在实践中，一些传感器用于感知悬挂的障碍物，即桌面，而其他传感器用于感知地面上的低障碍物。已有校准方法需要将机器人固定在夹具上，并使用红外图像标定深度摄像头的姿态，这对标定带来很多不利的因素也对夹具的制作精度与机器人在夹具上的安装精度提出了很高的要求。设计一种使用低成本嵌入式计算单元的移动机器人无夹具深度摄像头校准方法，使得深度摄像头的点云与激光雷达点云进行对齐并保证计算的时间效率是本发明希望解决的技术问题。


技术实现要素：

3.本发明的目的在于提供一种解决或部分解决上述问题的用于移动机器人的多传感器标定方法。
4.为实现上述目的，本发明提供如下技术方案：
5.一种用于移动机器人的多传感器标定方法，移动机器人包括2d激光雷达、上视深度摄像头和下视深度摄像头，其包括以下步骤：
6.s1、设置标定环境：包括地面、垂直于地面的中间标定板、两块垂直于地面的侧标定板；两所述侧标定板均垂直于所述中间标定板；所述中间标定板和两块侧标定板共同围成一u字形结构；
7.s2、设置移动机器人：将所述移动机器人放置在所述地面上，并位于所述中间标定板和两块侧标定板之间，并使得所述2d激光雷达可看到所述中间标定板和两块侧标定板、所述上视深度摄像头和下视深度摄像头均可看到所述中间标定板以及至少一个所述侧标定板；创建以所述移动机器人为原点的机器人坐标系、分别以所述上视深度摄像头和下视深度摄像头为原点的摄像头坐标系；
8.s3、获取原始数据：所述上视深度摄像头和下视深度摄像头拍照以获得原始点云，所述2d激光雷达扫描所述中间标定板和两块侧标定板，并提取其中的轮廓；
9.s4、上视深度摄像头标定，按以下步骤对所述上视深度摄像头进行标定：
10.s4-1、获取标定板点云：对于所述上视深度摄像头获得的原始点云，首先识别其中的所述地面的点云并去除，以得到关于所述中间标定板和侧标定板的标定板点云；
11.s4-2、翻滚角与俯仰角标定：逐次微调所述标定板点云的翻滚角与俯仰角，以生成多个第一样本点云；比较全部所述第一样本点云，以获取其中最垂直于所述地面的所述第一样本点云，作为第一标准样本点云；
12.s4-3、轮廓对齐：逐次微调所述第一标准样本点云的偏航角、以及x轴和y轴的偏移
量，以得到多个第二样本点云，将所述第二样本点云的轮廓与所述2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的所述第二样本点云，以其作为第二标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量。
13.s5、下视深度摄像头标定，按以下步骤对所述上视深度摄像头进行标定：
14.s5-1、获取地面点云：对于所述下视深度摄像头获得的原始点云，首先识别其中的所述地面的点云，以得到地面点云；
15.s5-2、翻滚角与俯仰角标定：逐次微调所述地面点云的翻滚角与俯仰角，以生成多个第三样本点云；求取每个所述第三样本点云的法向量vc，选取其中最接近地面法向量vg的所述第三样本点云作为第三标准样本点云，其中vg＝(0，0，1)
t
，t表示矩阵转置；
16.s5-3、轮廓对齐：逐次微调所述第三标准样本点云的偏航角、以及x轴和y轴的偏移量，以得到多个第四样本点云，将所述第四样本点云的轮廓与所述2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的所述第四样本点云，以其作为第四标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量。
17.优选的，所述步骤s3中，首先在所述机器人坐标系中创建高分辨率网格图，然后在所述高分辨率网格图中描绘所述2d激光雷达扫描得到的所述中间标定板和两块侧标定板的轮廓。
18.优选的，所述步骤s4-2中，首先创建平行于地面的平面网格，然后将所述第一样本点云投影到所述平面网格内，然后采用以下步骤，选择具有所述第一样本点云投影的网格数最少的所述第一样本点云作为所述第一标准样本点云：
19.从所述第一样本点云中提取z分量，并将所述第一样本点云投影到所述平面网格上，然后通过计算占用的网格索引的数量来对所述第一样本点云进行评分；点p＝[x，y]
t
所占据的网格索引ig为：ig＝ceil(y-y0)/res)
×
num
x
+ceil((x-xo)/res)，其中，x方向上的网格数为num
x
，y方向上的栅格数为numy；所述平面网格的原点为originm＝[xo，yo]
t
，res表示分辨率，ceil表示数值运算向上取整。
[0020]
优选的，所述步骤s3中，所述原始点云是从所述上视深度摄像头和下视深度摄像头拍摄的深度图像中获得的，并通过投影变换进行变换：
[0021]
pd＝(xd，yd，zd)
t
，ps＝(xs，ys，1
)
t
[0022]
其中pd是变换到所述上视深度摄像头和下视深度摄像头的摄像头坐标系中的点，d表示该点的深度，f
x
是上视深度摄像头和下视深度摄像头在x轴上的焦距，fy是上视深度摄像头和下视深度摄像头在y轴上的焦距，ps是深度图像中的原始点，xd是相机坐标系内的x轴坐标，yd是相机坐标系内的y轴坐标，zd是相机坐标系内的z轴坐标，t是，xs是深度图片内x轴索引值，ys是深度图片内y轴索引值。
[0023]
优选的，所述步骤s3中，所述上视深度摄像头和下视深度摄像头拍照获得原始点云首先转换为在所述机器人坐标系中，其点云集合为：
[0024]
其中sd为所述上视深度摄像头或下视深度摄像
头，为从所述上视深度摄像头或下视深度摄像头到机器人的齐次变换；为转换到机器人坐标系里面的点。
[0025]
优选的，所述步骤s4-2中、所述第一样本点云包括一个翻滚样本和一个俯仰样本，所述翻滚样本的点云集合s
roll
为：
[0026]sroll
＝{r|r∈[-r
max
，r
max
]}，resr＝2r
max
/nr[0027]
其中，r
max
是翻滚的最大偏移量，resr是样本的分辨率，nr是样本数量，r是一个翻滚样本；
[0028]
所述翻滚样本的旋转矩阵rr为：
[0029]
优选的，所述俯仰样本的点云集合s
pitch
为：
[0030]spitc
h＝{p|p∈[-p
max
，p
max
]}，resp＝2p
max
/n
p
[0031]
其中，p
max
是俯仰的最大偏移量，res
p
是样本的分辨率，n
p
是样本的数量，p是一个俯仰角样本；所述俯仰样本的旋转矩阵r
p
为：
[0032][0033]
优选的，所述翻滚样本和俯仰样本组成所述第一样本点云，所述候选姿态通过左乘变换tc来变换所述机器人坐标系中的点云集合，以获得所述第一样本点云上的点云tc：t＝(0，0，0)
t
；其中，t是点云平移量；
[0034]
所述第一样本点云上的点云集sc为：sc＝{pc|pc＝tc
×
pr，pr∈sr}，其中，pc是sc内点云，pr是传感器坐标系，sr是原始机器人点云集合。
[0035]
优选的，所述步骤s5-2中，通过区域生长方法检测到的最大平面做为所述地面，并求得所述地面的法线向量v
cp
，然后通过计算v
cp
和vg之间的角度差θc来对姿态候选者进行评分，θc越小越接近地面法向量vg，由此得到下视摄像头调整的翻滚角和俯仰角，其中
[0036][0037]
优选的，所述步骤s4-3中，得到所述第一标准样本点云后，将所述第一标准样本点云的点云集合s
rp
调整为垂直于地面；对于点云集合s
rp
中的每个点，移除z分量以创建二维点云；调整二维点云的偏航和位置，以匹配所述高分辨率网格图；
[0038]
一个所述第二样本点云ty由偏航样本和位置样本t
p
＝(x
p
，y
p
)
t
组成，其中，y是偏航角，x
p
是x轴偏移量，y
p
是y轴偏移量；
[0039]
偏航样本集为：s
yaw
＝{y|y∈[-y
max
，y
max
]}，resy＝2y
max
/ny；y
max
是最大偏航角，ny是偏航角样本数量，resy是偏航角样本分辨率；
[0040]
将所述第二样本点云ty变换为新的集合s
yp
：
[0041]syp
＝{p
yp
|p
yp
＝t
yp
×
p
rp
，p
rp
∈s
rp
}，其中，p
yp
是调节航向与平移量的点，t
yp
是调节航向角与平移量齐次变换，p
rp
是s
rp
中的点，s
rp
是第一标准样本点云的点云集合，ry是航向角旋转矩阵，t
p
是平移量；
[0042]
所述第二样本点云ty包含一个旋转样本和平移样本；旋转样本的集合为s
rot
；
[0043]
p
rot
＝[x
rot
，y
rot
]
t
，其中，p
rot
是仅通过旋转的点，x
rot
是p
rot
中的x轴分量，y
rot
是p
rot
中的y轴分量；
[0044]
将s
rot
中的点云投影到所述高分辨率网格图中，对应索引的集合为：
[0045]
其中，是地图内索引值，res是分辨率；
[0046]
显示了所述高分辨率网格图中的一组点云索引；
[0047]
平移样本通过向索引添加一个整数来平移旋转样本的点云；
[0048]
旋转索引的平移集合为：
[0049]
其中，s
trans
是平移集合，p
trans
是平移点，x
offset
是p
trans
x轴分量，y
offset
是p
trans
y轴分量，z是整数集合，x
max
是x轴最大偏置量，y
max
是y轴最大偏置量；
[0050]
平移一个旋转样本索引的集合为：
[0051][0052]
是所述第二样本点云ty在所述高分辨率网格图中对应的索引集；
[0053]
所述第二样本点云ty的得分是在所述高分辨率网格图中投影中点云的对应索引i的灰度值的总和；因此，所述第二样本点云的得分sc为：
[0054]
选择具有最高得分sc的所述第二样本点云作为所述第二标准样本点云。
[0055]
本发明的有益效果是：不依赖夹具的精度和安装精度，来对移动机器人的深度摄像头进行校准，将深度摄像头的点云与激光雷达点云进行对齐并保证计算的时间效率。
附图说明
[0056]
图1是本发明的原理框图；
[0057]
图2是使用高分辨率网格图的原理示意图。
具体实施方式
[0058]
下面结合具体实施方式对本专利的技术方案作进一步详细地说明。
[0059]
在本发明的描述中，需要说明的是，术语“内”、“外”、“上”、“下”、“水平”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0060]
如图1和图2所示，本发明的一种用于移动机器人的多传感器标定方法，移动机器人包括2d激光雷达、上视深度摄像头和下视深度摄像头，其是针对上视深度摄像头和下视深度摄像头的标定方法，其具体的包括以下步骤：
[0061]
s1、设置标定环境：包括地面、垂直于地面的中间标定板、两块垂直于地面的侧标定板；两侧标定板均垂直于中间标定板；中间标定板和两块侧标定板共同围成一u字形结构；
[0062]
s2、设置移动机器人：将移动机器人放置在地面上，并位于中间标定板和两块侧标定板之间，并使得2d激光雷达可看到中间标定板和两块侧标定板、上视深度摄像头和下视深度摄像头均可看到中间标定板以及至少一个侧标定板；创建以移动机器人为原点的机器人坐标系、分别以上视深度摄像头和下视深度摄像头为原点的摄像头坐标系；
[0063]
s3、获取原始数据：上视深度摄像头和下视深度摄像头拍照以获得原始点云，2d激光雷达扫描中间标定板和两块侧标定板，并提取其中的轮廓；
[0064]
s4、上视深度摄像头标定，按以下步骤对上视深度摄像头进行标定：
[0065]
s4-1、获取标定板点云：对于上视深度摄像头获得的原始点云，首先识别其中的地面的点云并去除，以得到关于中间标定板和侧标定板的标定板点云；
[0066]
s4-2、翻滚角与俯仰角标定：逐次微调标定板点云的翻滚角与俯仰角，以生成多个第一样本点云；比较全部第一样本点云，以获取其中最垂直于地面的第一样本点云，作为第一标准样本点云；
[0067]
s4-3、轮廓对齐：逐次微调第一标准样本点云的偏航角、以及x轴和y轴的偏移量，以得到多个第二样本点云，将第二样本点云的轮廓与2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的第二样本点云，以其作为第二标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量。
[0068]
s5、下视深度摄像头标定，按以下步骤对上视深度摄像头进行标定：
[0069]
s5-1、获取地面点云：对于下视深度摄像头获得的原始点云，首先识别其中的地面的点云，以得到地面点云；
[0070]
s5-2、翻滚角与俯仰角标定：逐次微调地面点云的翻滚角与俯仰角，以生成多个第三样本点云；求取每个第三样本点云的法向量vc，选取其中最接近地面法向量vg的第三样本点云作为第三标准样本点云，其中vg＝(0，0，1)
t
，t表示矩阵转置；
[0071]
s5-3、轮廓对齐：逐次微调第三标准样本点云的偏航角、以及x轴和y轴的偏移量，以得到多个第四样本点云，将第四样本点云的轮廓与2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的第四样本点云，以其作为第四标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的
偏移量。
[0072]
步骤s3中，首先在机器人坐标系中创建高分辨率网格图(也称高精地图)，然后在高分辨率网格图中描绘2d激光雷达扫描得到的中间标定板和两块侧标定板的轮廓。高分辨率网格图的分辨率为res(米)/网格。地图原点为origin＝(xo，yo)
t
。激光雷达命中的网格的灰度值为100，其余网格的灰度数值为0。
[0073]
步骤s4-2中，首先创建平行于地面的平面网格，然后将第一样本点云投影到平面网格内，然后采用以下步骤，选择具有第一样本点云投影的网格数最少的第一样本点云作为第一标准样本点云：
[0074]
移动机器人来说，点云信息中的z分量对避障影响不大，点云位置的调整仅限于x轴和y轴，因此，从第一样本点云中提取z分量，并将第一样本点云投影到平面网格上，然后通过计算占用的网格索引的数量来对第一样本点云进行评分；点p＝[x，y]
t
所占据的网格索引ig为：ig＝ceil((y-y0)/res)
×
num
x
+ceil((x-xo)/res)，其中，x方向上的网格数为num
x
，y方向上的栅格数为numy；平面网格的原点为originm＝[xo，yo]
t
，res表示分辨率，ceil表示数值运算向上取整。
[0075]
步骤s3中，原始点云是从上视深度摄像头和下视深度摄像头拍摄的深度图像中获得的，并通过投影变换进行变换：
[0076]
pd＝(xd，yd，zd)
t
，ps＝(xs，ys，1)
t
[0077]
其中pd是变换到上视深度摄像头和下视深度摄像头的摄像头坐标系中的点，d表示该点的深度，f
x
是上视深度摄像头和下视深度摄像头在x轴上的焦距，fy是上视深度摄像头和下视深度摄像头在y轴上的焦距，ps是深度图像中的原始点，xd是相机坐标系内的x轴坐标，yd是相机坐标系内的y轴坐标，zd是相机坐标系内的z轴坐标，t是，xs是深度图片内x轴索引值，ys是深度图片内y轴索引值。
[0078]
步骤s3中，上视深度摄像头和下视深度摄像头拍照获得原始点云首先转换为在机器人坐标系中，其点云集合为：
[0079]
其中sd为上视深度摄像头或下视深度摄像头，为从上视深度摄像头或下视深度摄像头到机器人的齐次变换；为转换到机器人坐标系里面的点。
[0080]
步骤s4-2中、第一样本点云包括一个翻滚样本和一个俯仰样本，翻滚样本的点云集合s
roll
为：
[0081]sroll
＝{r|r∈[-r
max
，r
max
]}，resr＝2r
mmax
/nr[0082]
其中，r
max
是翻滚的最大偏移量，resr是样本的分辨率，nr是样本数量，r是一个翻滚样本；
[0083]
翻滚样本的旋转矩阵rr为：
[0084]
俯仰样本的点云集合s
pitch
为：
[0085]spitch
＝{p|p∈[-p
max
，p
max
]}，res
p
＝2p
max
/n
p
[0086]
其中，p
max
是俯仰的最大偏移量，res
p
是样本的分辨率，n
p
是样本的数量，p是一个俯仰角样本；俯仰样本的旋转矩阵r
p
为：
[0087][0088]
翻滚样本和俯仰样本组成第一样本点云，候选姿态通过左乘变换tc来变换机器人坐标系中的点云集合，以获得第一样本点云上的点云tc：t＝(0，0，0)
t
；其中，t是点云平移量；
[0089]
第一样本点云上的点云集sc为：sc＝{pc|pc＝tc×
pr，pr∈sr}，其中，pc是sc内点云，pr是传感器坐标系，sr是原始机器人点云集合。
[0090]
步骤s5-2中，通过区域生长方法检测到的最大平面做为地面，并求得地面的法线向量v
cp
，然后通过计算v
cp
和vg之间的角度差θc来对姿态候选者进行评分，θc越小越接近地面法向量vg，由此得到下视摄像头调整的翻滚角和俯仰角，其中
[0091]
步骤s4-3中，得到第一标准样本点云后，将第一标准样本点云的点云集合s
rp
调整为垂直于地面；对于点云集合s
rp
中的每个点，移除z分量以创建二维点云；调整二维点云的偏航和位置，以匹配高分辨率网格图；
[0092]
一个第二样本点云ty由偏航样本和位置样本t
p
＝(x
p
，y
p
)
t
组成，其中，y是偏航角，x
p
是x轴偏移量，y
p
是y轴偏移量；
[0093]
偏航样本集为：s
yaw
＝{y|y∈[-y
max
，y
max
]}，resy＝2y
max
/ny；y
max
是最大偏航角，ny是偏航样本数量，resy是偏航角样本分辨率；
[0094]
将第二样本点云ty变换为新的集合s
yp
：
[0095]syp
＝{p
yp
|p
yp
＝t
yp
×
p
rp
，p
rp
∈s
rp
}，其中，p
，p
是调节航向与平移量的点，t
yp
是调节航向角与平移量齐次变换，p
rp
是s
rp
中的点，s
rp
是第一标准样本点云的点云集合，ry是航向角旋转矩阵，t
p
是平移量；
[0096]
第二样本点云ty包含一个旋转样本和平移样本；旋转样本的集合为s
rot
；
[0097]
p
rot
＝[x
rot
，y
rot
]
t
，其中，p
rot
是仅通过旋转的点，x
rot
是p
rot
中的x轴分量，y
rot
是p
ro
t中的y轴分量；
[0098]
将s
rot
中的点云投影到高分辨率网格图中，对应索引的集合为：其中，是地图内索引值，res是分辨率；
[0099]
显示了高分辨率网格图中的一组点云索引；
[0100]
平移样本通过向索引添加一个整数来平移旋转样本的点云，整数的加法在计算上比浮点数的翻译效率高得多；旋转索引的平移集合为：
[0101]
其中，s
trans
是平移集合，p
trans
是平移点，x
offset
是p
trans
x轴分量，y
offset
是p
trans
y轴分量，z是整数集合，x
max
是x轴最大偏置量，y
max
是y轴最大偏置量；
[0102]
平移一个旋转样本索引的集合为：
[0103][0104]
是第二样本点云ty在高分辨率网格图中对应的索引集；
[0105]
第二样本点云ty的得分是在高分辨率网格图中投影中点云的对应索引i的灰度值的总和；因此，第二样本点云的得分sc为：
[0106][0107]
选择具有最高得分sc的第二样本点云作为第二标准样本点云。
[0108]
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。

技术特征：
1.一种用于移动机器人的多传感器标定方法，移动机器人包括2d激光雷达、上视深度摄像头和下视深度摄像头，其特征在于，其包括以下步骤：s1、设置标定环境：包括地面、垂直于地面的中间标定板、两块垂直于地面的侧标定板；两所述侧标定板均垂直于所述中间标定板；所述中间标定板和两块侧标定板共同围成一u字形结构；s2、设置移动机器人：将所述移动机器人放置在所述地面上，并位于所述中间标定板和两块侧标定板之间，并使得所述2d激光雷达可看到所述中间标定板和两块侧标定板、所述上视深度摄像头和下视深度摄像头均可看到所述中间标定板以及至少一个所述侧标定板；创建以所述移动机器人为原点的机器人坐标系、分别以所述上视深度摄像头和下视深度摄像头为原点的摄像头坐标系；s3、获取原始数据：所述上视深度摄像头和下视深度摄像头拍照以获得原始点云，所述2d激光雷达扫描所述中间标定板和两块侧标定板，并提取其中的轮廓；s4、上视深度摄像头标定，按以下步骤对所述上视深度摄像头进行标定：s4-1、获取标定板点云：对于所述上视深度摄像头获得的原始点云，首先识别其中的所述地面的点云并去除，以得到关于所述中间标定板和侧标定板的标定板点云；s4-2、翻滚角与俯仰角标定：逐次微调所述标定板点云的翻滚角与俯仰角，以生成多个第一样本点云；比较全部所述第一样本点云，以获取其中最垂直于所述地面的所述第一样本点云，作为第一标准样本点云；s4-3、轮廓对齐：逐次微调所述第一标准样本点云的偏航角、以及x轴和y轴的偏移量，以得到多个第二样本点云，将所述第二样本点云的轮廓与所述2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的所述第二样本点云，以其作为第二标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量。s5、下视深度摄像头标定，按以下步骤对所述上视深度摄像头进行标定：s5-1、获取地面点云：对于所述下视深度摄像头获得的原始点云，首先识别其中的所述地面的点云，以得到地面点云；s5-2、翻滚角与俯仰角标定：逐次微调所述地面点云的翻滚角与俯仰角，以生成多个第三样本点云；求取每个所述第三样本点云的法向量v
c
，选取其中最接近地面法向量v
g
的所述第三样本点云作为第三标准样本点云，其中v
g
＝(0，0，1)
t
，t表示矩阵转置；s5-3、轮廓对齐：逐次微调所述第三标准样本点云的偏航角、以及x轴和y轴的偏移量，以得到多个第四样本点云，将所述第四样本点云的轮廓与所述2d激光雷达得到的轮廓进行对齐，选取其中轮廓匹配程度最高的所述第四样本点云，以其作为第四标准样本点云，并以其翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量作为标定的翻滚角、俯仰角、偏航角、以及x轴和y轴的偏移量。2.根据权利要求1所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s3中，首先在所述机器人坐标系中创建高分辨率网格图，然后在所述高分辨率网格图中描绘所述2d激光雷达扫描得到的所述中间标定板和两块侧标定板的轮廓。3.根据权利要求1所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s4-2中，首先创建平行于地面的平面网格，然后将所述第一样本点云投影到所述平面网格
内，然后采用以下步骤,选择具有所述第一样本点云投影的网格数最少的所述第一样本点云作为所述第一标准样本点云:从所述第一样本点云中提取z分量，并将所述第一样本点云投影到所述平面网格上，然后通过计算占用的网格索引的数量来对所述第一样本点云进行评分；点p＝[x，y]
t
所占据的网格索引i
g
为：i
g
＝ceil((y-y0)/res)
×
num
x
+ceil((x-x
o
)/res)，其中，x方向上的网格数为num
x
，y方向上的栅格数为num
y
；所述平面网格的原点为origin
m
＝[x
o
，y
o
]
t
，res表示分辨率，ceil表示数值运算向上取整。4.根据权利要求1所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s3中，所述原始点云是从所述上视深度摄像头和下视深度摄像头拍摄的深度图像中获得的，并通过投影变换进行变换：p
d
＝(x
d
，y
d
，z
d
)
t
，p
s
＝(x
s
，y
s
，1)
t
其中p
d
是变换到所述上视深度摄像头和下视深度摄像头的摄像头坐标系中的点，d表示该点的深度，f
x
是上视深度摄像头和下视深度摄像头在x轴上的焦距，f
y
是上视深度摄像头和下视深度摄像头在y轴上的焦距，p
s
是深度图像中的原始点,x
d
是相机坐标系内的x轴坐标,y
d
是相机坐标系内的y轴坐标,z
d
是相机坐标系内的z轴坐标,t是，x
s
是深度图片内x轴索引值,y
s
是深度图片内y轴索引值。5.根据权利要求4所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s3中，所述上视深度摄像头和下视深度摄像头拍照获得原始点云首先转换为在所述机器人坐标系中，其点云集合为：其中s
d
为所述上视深度摄像头或下视深度摄像头，为从所述上视深度摄像头或下视深度摄像头到机器人的齐次变换；为转换到机器人坐标系里面的点。6.根据权利要求5所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s4-2中、所述第一样本点云包括一个翻滚样本和一个俯仰样本，所述翻滚样本的点云集合s
roll
为：s
roll
＝{r|r∈[-r
max
，r
max
]}，res
r
＝2r
max
/n
r
其中，r
max
是翻滚的最大偏移量，res
r
是样本的分辨率，n
r
是样本数量，r是一个翻滚样本；所述翻滚样本的旋转矩阵r
r
为：7.根据权利要求6所述的用于移动机器人的多传感器标定方法，其特征在于：所述俯仰样本的点云集合s
pitch
为：s
pitch
＝{p|p∈[-p
max
，p
max
]}，res
p
＝2p
max
/n
p
其中，p
max
是俯仰的最大偏移量，res
p
是样本的分辨率，n
p
是样本的数量，p是一个俯仰角样本；所述俯仰样本的旋转矩阵r
p
为：
8.根据权利要求7所述的用于移动机器人的多传感器标定方法，其特征在于：所述翻滚样本和俯仰样本组成所述第一样本点云，所述候选姿态通过左乘变换t
c
来变换所述机器人坐标系中的点云集合，以获得所述第一样本点云上的点云t
c
：t＝(0，0，0)
t
；其中，t是点云平移量；所述第一样本点云上的点云集s
c
为：s
c
＝{p
c
|p
c
＝t
c
×
p
r
，p
r
∈s
r
}，其中，p
c
是s
c
内点云，p
r
是传感器坐标系,s
r
是原始机器人点云集合。9.根据权利要求1所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s5-2中，通过区域生长方法检测到的最大平面做为所述地面，并求得所述地面的法线向量v
cp
，然后通过计算v
cp
和v
g
之间的角度差θ
c
来对姿态候选者进行评分，θ
c
越小越接近地面法向量v
g
，由此得到下视摄像头调整的翻滚角和俯仰角，其中10.根据权利要求1所述的用于移动机器人的多传感器标定方法，其特征在于：所述步骤s4-3中，得到所述第一标准样本点云后，将所述第一标准样本点云的点云集合s
rp
调整为垂直于地面；对于点云集合s
rp
中的每个点，移除z分量以创建二维点云；调整二维点云的偏航和位置，以匹配所述高分辨率网格图；一个所述第二样本点云t
y
由偏航样本和位置样本t
p
＝(x
p
，y
p
)
t
组成，其中，y是偏航角，x
p
是x轴偏移量，y
p
是y轴偏移量；偏航样本集为：s
yaw
＝{y|y∈[-y
max
，y
max
]}，res
y
＝2y
max
/n
y
；y
max
是最大偏航角，n
y
是偏航样本数量，res
y
是偏航分辨率；将所述第二样本点云t
y
变换为新的集合s
yp
：s
yp
＝{p
yp
|p
yp
＝t
yp
×
p
rp
，p
rp
∈s
rp
}，其中，p
yp
是调节航向与平移量的点，t
yp
是调节航向角与平移量齐次变换，p
rp
是s
rp
中的点，s
rp
是第一标准样本点云的点云集合，r
y
是航向角旋转矩阵，t
p
是平移量；所述第二样本点云t
y
包含一个旋转样本和平移样本；旋转样本的集合为s
rot
；p
rot
＝[x
rot
，y
rot
]
t
，其中，p
rot
是仅通过旋转的点，x
rot
是p
rot
中的x轴分量，y
rot
是p
rot
中的y轴分量；将s
rot
中的点云投影到所述高分辨率网格图中，对应索引的集合为：其中，是地图内索引值，res是分辨率；
显示了所述高分辨率网格图中的一组点云索引；平移样本通过向索引添加一个整数来平移旋转样本的点云；旋转索引的平移集合为：其中，s
trans
是平移集合，p
trans
是平移点，x
offset
是p
trans
x轴分量，y
offset
是p
trans
y轴分量，z是整数集合，x
max
是x轴最大偏置量，y
max
是y轴最大偏置量；平移一个旋转样本索引的集合为：平移一个旋转样本索引的集合为：是所述第二样本点云t
y
在所述高分辨率网格图中对应的索引集；所述第二样本点云t
y
的得分是在所述高分辨率网格图中投影中点云的对应索引i的灰度值的总和；因此，所述第二样本点云的得分s
c
为：选择具有最高得分s
c
的所述第二样本点云作为所述第二标准样本点云。

技术总结
本发明公开了一种用于移动机器人的多传感器标定方法，移动机器人包括2D激光雷达、上视深度摄像头和下视深度摄像头，其是针对上视深度摄像头和下视深度摄像头的标定方法，其具体的包括以下步骤：S1、设置标定环境；S2、设置移动机器人；S3、获取原始数据；S4、上视深度摄像头标定；S5、下视深度摄像头标定。本发明的有益效果是：不依赖夹具的精度和安装精度，来对移动机器人的深度摄像头进行校准，将深度摄像头的点云与激光雷达点云进行对齐并保证计算的时间效率。的时间效率。的时间效率。


技术研发人员：袁儒鹏
受保护的技术使用者：袁儒鹏
技术研发日：2023.03.30
技术公布日：2023/9/20