主动立体视觉系统标定方法

1.本发明属于服务机器人技术领域，具体涉及一种主动立体视觉系统标定方法。


背景技术：

2.近年来，服务机器人被广泛应用于助老助残、医疗、自动驾驶中，其中机器人通过立体视觉获得场景的深度信息，为环境感知提供坚实的基础。要使用立体视觉感知环境，需要知道立体视觉系统中两个相机之间的空间关系，这就需要对立体视觉系统进行有效的标定以得到立体视觉系统的外参。按照相机之间相对位姿是否可变，立体视觉系统分为固定式立体视觉系统和主动立体视觉系统，其中固定式立体视觉系统的相机之间的相对位姿保持不变，而主动立体视觉系统的相机之间的相对位姿是可变的。相比于固定式立体视觉系统，主动立体视觉系统可以获得更大的工作空间。主动立体视觉系统包含两个相机：第一相机和第二相机，这两个相机大多绕自己的旋转轴旋转。对于固定式立体视觉系统，借助棋盘格标定板、张正友标定法、pnp算法和opencv库已经能对其进行有效的标定；对于采用棋盘格标定板进行标定的方式，基于棋盘格标定板的各角点的2d像素坐标和棋盘格标定板的各角点的3d点坐标，利用pnp算法即可求解出相机之间的相对位姿；但是，这些方法不足以完成主动立体视觉系统的标定。
3.目前的主动立体视觉系统的标定方法可分为两类：基于重定位的标定方法和两阶段的标定方法。基于重定位的标定方法将相机之间相对位姿的估计建模为其中一个相机与另一个相机之间的位姿估计问题，但是该类方法用于主动立体视觉系统的外参更新时较为繁琐，实时性和标定精度较差。两阶段的标定方法考虑了主动立体视觉系统本身的机械结构特性，首先在离线阶段对机械结构的机械参数进行标定，为了得到主动立体视觉系统中各相机的旋转轴的机械参数，可考虑使用rodrigues旋转公式，将每个相机旋转前后的相对位姿转换为该相机的旋转轴对应的旋转向量；然后是在线更新阶段，结合离线阶段标定好的相机的旋转轴机械参数对主动立体视觉系统的外参进行在线更新。两阶段的标定方法对主动立体视觉系统的机械结构进行了建模，从而可以得到实时性好且精度高的系统外参，但是，现有的两阶段标定方法大多需要主动立体视觉系统各相机的光心位于对应的旋转轴上，这限制了其应用范围。如何在主动立体视觉系统中各相机的光心偏离对应的旋转轴时实现高质量标定，仍有待于进一步研究。
4.因此，如何提出一种解决上述问题的方案是本领域技术人员目前需要解决的问题。


技术实现要素：

5.为了解决现有技术中的上述问题，即为了解决主动立体视觉系统中各相机的光心偏离对应的旋转轴情况下难以实现高质量标定的问题，本发明提出了一种主动立体视觉系统标定方法，包括以下步骤：
6.步骤s100，获取主动立体视觉系统的第一相机绕其旋转轴旋转前后采集的棋盘格
标定板的图像，分别作为第一图像、第二图像；获取主动立体视觉系统的第二相机绕其旋转轴旋转前后采集的棋盘格标定板的图像，分别作为第三图像、第四图像；将第一相机、第二相机恢复至旋转前的初始位置，并将棋盘格标定板置于第一相机和第二相机的公共视野中，然后利用第一相机、第二相机分别采集棋盘格标定板的图像，作为第五图像、第六图像；
7.步骤s200，结合第一图像和第二图像获取主动立体视觉系统的第一相机旋转前后的相对位姿t
c1
；结合第三图像和第四图像获取主动立体视觉系统的第二相机旋转前后的相对位姿t
c2
；
8.步骤s300，利用rodrigues旋转公式，结合第一相机旋转前后的相对位姿t
c1
获取第一相机的旋转轴对应的旋转向量f1，结合第二相机旋转前后的相对位姿t
c2
获取第二相机的旋转轴对应的旋转向量f2；
9.步骤s400，结合第一相机的旋转轴对应的旋转向量f1以及旋转角度θ，获取第一相机的旋转轴在预定义的第一相机初始位置坐标系o1x1y1z1中的位姿t1；结合第二相机的旋转轴对应的旋转向量f2以及旋转角度θ，获取第二相机的旋转轴在预定义的第二相机初始位置坐标系o2x2y2z2中的位姿t2；
10.步骤s500，结合第五图像、第六图像，获取第一相机和第二相机在初始位置时的相对位姿t
rc
，进而结合t1和t2，得到第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
；
11.步骤s600，根据主动立体视觉系统的第一相机的旋转轴在第一相机初始位置坐标系o1x1y1z1中的位姿t1、第二相机的旋转轴在第二相机初始位置坐标系o2x2y2z2中的位姿t2、第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
，在线更新主动立体视觉系统的外参t
e
。
12.在一些优选的实施方式中，所述棋盘格标定板在第一图像、第二图像、第三图像、第四图像、第五图像和第六图像中所占比例为50％。
13.在一些优选的实施方式中，所述主动立体视觉系统的各相机旋转前后的相对位姿，其计算方法为：
14.通过opencv库的棋盘格角点检测算法分别对第一图像、第二图像进行处理，得到第一图像、第二图像中棋盘格标定板的各个角点的图像坐标p
1(i，j)
、p
2(i，j)
；基于p
1(i，j)
、p
2(i，j)
，结合棋盘格标定板的各个角点的世界坐标w
(i，j)
，利用pnp算法求解出第一相机旋转前后的相对位姿t
c1
；
15.通过opencv库的棋盘格角点检测算法分别对第三图像、第四图像进行处理，得到第三图像、第四图像中棋盘格标定板的各个角点的图像坐标p
3(i，j)
、p
4(i，j)
；基于p
3(i，j)
、p
4(i，j)
，结合棋盘格标定板的各个角点的世界坐标w
(i，j)
，利用pnp算法求解出第二相机旋转前后的相对位姿t
c2
；
16.其中，棋盘格标定板具有r行c列个角点，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1。
17.在一些优选的实施方式中，所述第一相机初始位置坐标系o1x1y1z1为以第一相机在初始位置时的光心作为原点o1，以第一相机在初始位置时的相机图像的横轴方向和纵轴方向分别作为x1轴方向和y1轴方向，以第一相机在初始位置时的相机光轴作为z1轴的坐标系；
18.所述第二相机初始位置坐标系o2x2y2z2为以第二相机在初始位置时的光心作为原
点o2，以第二相机在初始位置时的相机图像的横轴方向和纵轴方向分别作为x2轴方向和y2轴方向，以第二相机在初始位置时的相机光轴作为z2轴的坐标系。
19.在一些优选的实施方式中，所述位姿t1，其获取方法为：
[0020][0021][0022][0023]
f1＝[f
1，0 f
1，1 f
1，2
]
t
[0024]
其中，t1为第一相机旋转前后的相对位姿t
c1
的平移向量，b
1，0
、b
1，1
和b
1，2
分别为b1的第一个分量、第二个分量和第三个分量，f
1，0
、f
1，1
和f
1，2
分别为f1的第一个分量、第二个分量和第三个分量。
[0025]
在一些优选的实施方式中，所述位姿t2，其获取方法为：
[0026][0027][0028][0029]
f2＝[f
2，0 f
2，1 f
2，2
]
t
[0030]
其中，t2为第二相机旋转前后的相对位姿t
c2
的平移向量，b
2，0
、b
2，1
和b
2，2
分别为b2的第一个分量、第二个分量和第三个分量，f
2，0
、f
2，1
和f
2，2
分别为f2的第一个分量、第二个分量和第三个分量。
[0031]
在一些优选的实施方式中，所述第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
，其计算方法为：
[0032]
在一些优选的实施方式中，“在线更新主动立体视觉系统的外参t
e”，其方法为：
[0033][0034]
其中，θ1和θ2分别为第一相机和第二相机当前时刻绕各自对应的旋转轴旋转的角度。
[0035]
本发明的有益效果：
[0036]
本发明解决了主动立体视觉系统中各相机的光心偏离对应的旋转轴情况下难以高质量标定的问题，提升了主动立体视觉系统的标定质量，为服务机器人准确获得场景的
深度信息提供技术支持。
附图说明
[0037]
图1是本发明一种实施例的主动立体视觉系统标定方法的流程示意图。
具体实施方式
[0038]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0039]
下面结合附图和实施例对本技术作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。
[0040]
本发明的一种主动立体视觉系统标定方法，如图1所示，包括以下步骤：
[0041]
步骤s100，获取主动立体视觉系统的第一相机绕其旋转轴旋转前后采集的棋盘格标定板的图像，分别作为第一图像、第二图像；获取主动立体视觉系统的第二相机绕其旋转轴旋转前后采集的棋盘格标定板的图像，分别作为第三图像、第四图像；将第一相机、第二相机恢复至旋转前的初始位置，并将棋盘格标定板置于第一相机和第二相机的公共视野中，然后利用第一相机、第二相机分别采集棋盘格标定板的图像，作为第五图像、第六图像；
[0042]
步骤s200，结合第一图像和第二图像获取主动立体视觉系统的第一相机旋转前后的相对位姿t
c1
；结合第三图像和第四图像获取主动立体视觉系统的第二相机旋转前后的相对位姿t
c2
；
[0043]
步骤s300，利用rodrigues旋转公式，结合第一相机旋转前后的相对位姿t
c1
获取第一相机的旋转轴对应的旋转向量f1，结合第二相机旋转前后的相对位姿t
c2
获取第二相机的旋转轴对应的旋转向量f2；
[0044]
步骤s400，结合第一相机的旋转轴对应的旋转向量f1以及旋转角度θ，获取第一相机的旋转轴在预定义的第一相机初始位置坐标系o1x1y1z1中的位姿t1；结合第二相机的旋转轴对应的旋转向量f2以及旋转角度θ，获取第二相机的旋转轴在预定义的第二相机初始位置坐标系o2x2y2z2中的位姿t2；
[0045]
步骤s500，结合第五图像、第六图像，获取第一相机和第二相机在初始位置时的相对位姿t
rc
，进而结合t1和t2，得到第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
；
[0046]
步骤s600，根据主动立体视觉系统的第一相机的旋转轴在第一相机初始位置坐标系o1x1y1z1中的位姿t1、第二相机的旋转轴在第二相机初始位置坐标系o2x2y2z2中的位姿t2、第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
，在线更新主动立体视觉系统的外参t
e
。
[0047]
为了更清晰地对本发明一种主动立体视觉系统标定方法进行说明，下面对本发明方法一种实施例中各步骤进行展开详述。
[0048]
步骤s100，获取主动立体视觉系统的第一相机绕其旋转轴旋转前后采集的棋盘格
标定板的图像，分别作为第一图像、第二图像；获取主动立体视觉系统的第二相机绕其旋转轴旋转前后采集的棋盘格标定板的图像，分别作为第三图像、第四图像；将第一相机、第二相机恢复至旋转前的初始位置，并将棋盘格标定板置于第一相机和第二相机的公共视野中，然后利用第一相机、第二相机分别采集棋盘格标定板的图像，作为第五图像、第六图像；
[0049]
在本实施例中，将具有r行c列个角点的棋盘格标定板置于环境中，其中水平相邻角点的间隔都为d，竖直相邻角点的间隔也都为d，r、c和d为预设常数，本发明中优选设置分别为8、11和15mm。利用主动立体视觉系统的第一相机在初始位置采集棋盘格标定板的图像，将其作为第一图像；该相机绕对应的旋转轴旋转θ角度后采集棋盘格标定板的图像，将其作为第二图像，其中θ为预设角度，本发明中优选设置为30
°
；同样的，主动立体视觉系统的第二相机在初始位置采集棋盘格标定板的图像，将其作为第三图像；该相机绕对应的旋转轴旋转θ角度后采集棋盘格标定板的图像，将其作为第四图像；主动立体视觉系统的第一相机和第二相机恢复至初始位置，将棋盘格标定板置于主动立体视觉系统的第一相机和第二相机的公共视野中，利用主动立体视觉系统的第一相机采集棋盘格标定板的图像，将其作为第五图像，利用主动立体视觉系统的第二相机采集棋盘格标定板的图像，将其作为第六图像；其中，棋盘格标定板在第一图像、第二图像、第三图像、第四图像、第五图像和第六图像中所占比例为50％。
[0050]
步骤s200，结合第一图像和第二图像获取主动立体视觉系统的第一相机旋转前后的相对位姿t
c1
；结合第三图像和第四图像获取主动立体视觉系统的第二相机旋转前后的相对位姿t
c2
；
[0051]
在本实施例中，通过opencv库的棋盘格角点检测算法对第一图像进行处理，得到第一图像中棋盘格标定板的各个角点的图像坐标p
1(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；通过opencv库的棋盘格角点检测算法对第二图像进行处理，得到第二图像中棋盘格标定板的各个角点的图像坐标p
2(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；另外，棋盘格标定板的各个角点的世界坐标计算如下：w
(i，j
)＝[j
·
d i
·
d 0]
t
，i＝0，1，
…
，r
‑
1，j＝0，1，
…
，c
‑
1；基于p
1(i，j)
、p
2(i，j)
和w
(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1，利用pnp算法求解出第一相机旋转前后的相对位姿t
c1
；
[0052]
通过opencv库的棋盘格角点检测算法对第三图像进行处理，得到第三图像中棋盘格标定板的各个角点的图像坐标p
3(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；通过opencv库的棋盘格角点检测算法对第四图像进行处理，得到第四图像中棋盘格标定板的各个角点的图像坐标p
4(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；基于p
3(i，j)
、p
4(i，j)
和w
(i，j)
，i＝0，1，...，r
‑
1，/＝0，1，...，c
‑
1，利用pnp算法求解出第二相机旋转前后的相对位姿t
c2
。
[0053]
步骤s300，利用rodrigues旋转公式，结合第一相机旋转前后的相对位姿t
c1
获取第一相机的旋转轴对应的旋转向量f1，结合第二相机旋转前后的相对位姿t
c2
获取第二相机的旋转轴对应的旋转向量f2；
[0054]
在本实施例中，利用rodrigues旋转公式，将第一相机旋转前后的相对位姿t
c1
转换为第一相机的旋转轴对应的旋转向量f1＝[f
1，0
f
1，1
f
1，2
]
t
，其中f
1，0
、f
1，1
和f
1，2
分别为f1的第一个分量、第二个分量和第三个分量；利用rodrigues旋转公式，将第二相机旋转前后的相对位姿t
c2
转换为第二相机的旋转轴对应的旋转向量f2＝[f
2，0
f
2，1
f
2，2
]
t
，其中f
2，0
、f
2，1
和f
2，2
分别为f2的第一个分量、第二个分量和第三个分量；定义第一相机初始位置坐标系o1x1y1z1，
以第一相机在初始位置时的光心作为原点o1，以第一相机在初始位置时的相机图像的横轴方向和纵轴方向分别作为x1轴方向和y1轴方向，以第一相机在初始位置时的相机光轴作为z1轴；定义第二相机初始位置坐标系o2x2y2z2，以第二相机在初始位置时的光心作为原点o2，以第二相机在初始位置时的相机图像的横轴方向和纵轴方向分别作为x2轴方向和y2轴方向，以第二相机在初始位置时的相机光轴作为z2轴。
[0055]
步骤s400，结合第一相机的旋转轴对应的旋转向量f1以及旋转角度θ，获取第一相机的旋转轴在预定义的第一相机初始位置坐标系o1x1y1z1中的位姿t1；结合第二相机的旋转轴对应的旋转向量f2以及旋转角度θ，获取第二相机的旋转轴在预定义的第二相机初始位置坐标系o2x2y2z2中的位姿t2；
[0056]
在本实施例中，令为第一中间变量b1，其中t1为第一相机旋转前后的相对位姿t
c1
的平移向量，b
1，0
、b
1，1
和b
1，2
分别为b1的第一个分量、第二个分量和第三个分量，进而结合第一相机的旋转轴对应的旋转向量f1，获取第一相机的旋转轴在第一相机初始位置坐标系o1x1y1z1中的位姿t1如下：
[0057][0058]
其中
[0059]
令为第二中间变量b2，其中t2为第二相机旋转前后的相对位姿t
c2
的平移向量，b
2，0
、b
2，1
和b
2，2
分别为b2的第一个分量、第二个分量和第三个分量，进而结合第二相机的旋转轴对应的旋转向量f2，获取第二相机的旋转轴在第二相机初始位置坐标系o2x2y2z2中的位姿t2如下：
[0060][0061]
其中
[0062]
步骤s500，结合第五图像、第六图像，获取第一相机和第二相机在初始位置时的相对位姿t
rc
，进而结合t1和t2，得到第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
；
[0063]
在本实施例中，通过opencv库的棋盘格角点检测算法对第五图像进行处理，得到第五图像中棋盘格标定板的各个角点的图像坐标p
5(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；通过opencv库的棋盘格角点检测算法对第六图像进行处理，得到第六图像中棋盘格标定板的各个角点的图像坐标p
6(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1；基于p
5(i，j)
、p
6(i，j)
和w
(i，j)
，i＝0，1，...，r
‑
1，j＝0，1，...，c
‑
1，利用pnp算法求解出第一相机和第二相机在初始
位置时的相对位姿t
rc
；
[0064]
结合t
rc
、t1和t2，得到第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
如下：
[0065][0066]
步骤s600，根据主动立体视觉系统的第一相机的旋转轴在第一相机初始位置坐标系o1x1y1z1中的位姿t1、第二相机的旋转轴在第二相机初始位置坐标系o2x2y2z2中的位姿t2、第一相机的旋转轴和第二相机的旋转轴之间的相对位姿t
rl
，在线更新主动立体视觉系统的外参t
e
；
[0067]
在本实施例中，主动立体视觉系统的外参t
e
在线更新如下：
[0068][0069]
其中θ1和θ2分别为第一相机和第二相机当前时刻绕各自对应的旋转轴旋转的角度。
[0070]
采用本发明一种主动立体视觉系统标定方法，服务机器人能够在线更新立体视觉系统的外参t
e
，用以实现更好的环境感知。本发明解决了主动立体视觉系统中各相机的光心偏离对应的旋转轴情况下难以高质量标定的问题，提升了主动立体视觉系统的标定质量，为服务机器人准确获得场景的深度信息提供技术支持。同时，基于标定好外参的主动立体视觉系统对场景的深度测量，机器人可以获得检测到的目标物体的准确空间位置或者对检测到的目标物体进行稠密的三维重建，进而得到包含各目标物体的三维场景地图，在此基础上，机器人可以实现避障导航等任务。其中，检测目标物体、稠密的三维重建、获取包含各目标物体的三维场景地图、避障导航等为现有技术，此处不再一一阐述。
[0071]
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd
‑
rom、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
[0072]
术语“第一”、“第二”、“第三”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。
[0073]
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。