采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的制作方法

1.本发明涉及算法和定位系统技术领域，尤其涉及采用多目摄像头免标定外参进行机器人行走轨迹定位的系统。


背景技术：

2.自主移动机器人在各个领域已经普及，比如家用的清洁机器人，商用的送餐机器人等。其中机器人的定位、建图和导航系统是自主移动机器人的基础。测试机器人在移动过程中的实时位置是评估、优化机器人slam(实时定位与建图)的基础。在外部环境搭建一套方便和准确的轨迹追踪系统有利于测试机器人的slam系统，测试清洁机器人的清洁覆盖率，测试机器人的导航规划系统，优化机器人的行走效率等。一个广角摄像头在3米远距离可以覆盖16平米到25平米左右，再大的范围需要用两个或者多个摄像头进行追踪定位；即使范围不大，但是在不同的区域，单个摄像头会有盲区，也需要用多个摄像头进行追踪。通常多个摄像头需要标定摄像头之间的外参，也就是摄像头之间的相对位姿。之后还需要做相机和场地平面做外参标定。外参标定在摄像头位置发生轻微变化就需要重新进行，以保证测量出的精度。外参标定通常比较耗时，只有测量发现问题时才能知道标定的是否准确。给定位追踪系统带来复杂性和造成较大误差。
3.因此，我们提出了采用多目摄像头免标定外参进行机器人行走轨迹定位的系统用于解决上述问题。


技术实现要素：

4.本发明的目的是为了解决现有技术中存在外参标定通常比较耗时，只有测量发现问题时才能知道标定的是否准确。给定位追踪系统带来复杂性和造成较大误差的缺点，而提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统。
5.为了实现上述目的，本发明采用了如下技术方案：
6.采用多目摄像头免标定外参进行机器人行走轨迹定位的系统，包括固定硬件模块、内参标定模块、追踪求解模块、拼接模块和测试模块，所述固定硬件模块和内参标定模块连接，且追踪求解模块分别与固定硬件模块和内参标定模块连接，所述追踪求解模块与拼接模块连接，且拼接模块和测试模块连接。
7.优选的，所述固定硬件模块包括多个追踪相机和标记点。
8.优选的，所述追踪相机采用具有图形采集功能的相机。
9.优选的，所述标记点采用二维码或棋盘格，且标记点粘贴在被测试机器人上，并可以被追踪相机完整观察到。
10.优选的，所述内参标定模块用于给多个追踪相机分别做内参标定。
11.优选的，所述内参标定模块采用张正友标定法来标定追踪相机的内参。
12.优选的，所述追踪求解模块用于针对单个追踪相机，分别通过图像追踪外部标记点，并求解出标记点在测试平面内的轨迹，其具体包括以下步骤：
13.s1：通过算法识别出标记点；
14.s2：通过算法求解出标记点在相机平面下的位姿；
15.s3：计算机器中心在相机坐标系下的空间位置；
16.s4：通过算法求解出机器中心在测试平面下的坐标。
17.优选的，所述拼接模块用于通过算法拼接多个追踪相机的测试数据，得到完整轨迹，其具体包括以下步骤：
18.s1：通过时间戳找到多个追踪相机中两两之间的重叠的轨迹点；
19.s2：用icp算法，对齐多个追踪相机中两两之间的重叠区域的轨迹，并找到两段轨迹的r和t；
20.s3：利用找到的r和t把两个追踪相机的所有轨迹点拼接在一起。
21.与现有技术相比，本发明的有益效果是：
22.1、自动化的方案，提升了测试的精度；
23.2、省去了标定外参的步骤(包括标定多个相机之间的外参和相机相对于场地的外参)，节省了时间和成本；
24.3、可以灵活的应用到新环境的测试中；
25.本发明在只标定相机内参的情况下，可以得到稳定可靠的位置定位，不需要标定做相机的外参，也不需要标定相机和场地的外参。即使相机的相对位置发生变化，也可以通过算法进行补偿，不需要重新标定。可以提升系统的追踪定位精度，并极大的简化了系统，提升了系统的灵活性(比如快速的在新测试场地进行部署)。
附图说明
26.图1为现有的测试系统的流程图；
27.图2为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的流程图；
28.图3为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的相机覆盖的区域的示意图；
29.图4为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的对于棋盘格检测得到的角点顺序的示意图；
30.图5为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的对于aruco码检测得到的角点顺序的示意图；
31.图6为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的以pnp算法求解棋盘格/二维码坐标系与相机坐标系之间的变换{r，t}的示意图；
32.图7为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的相机1测试出的追踪轨迹的示意图；
33.图8为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的相机2测试出的追踪轨迹的示意图；
34.图9为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的通过追踪点的时间戳找出两端轨迹中相同时间范围内的点的集合的示意图；
35.图10为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系
统的通过算法把两个时间段重合的点在空间上对齐；并找到这两段轨迹之前的r和t的示意图；
36.图11为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的通过已经找到的r和t，把相机1和2分别追踪到的轨迹拼接成一段完整的轨迹的示意图；
37.图12为本发明提出的采用多目摄像头免标定外参进行机器人行走轨迹定位的系统的实施例二的相机覆盖的区域的示意图。
具体实施方式
38.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
39.实施例一
40.现有的测试系统如下：系统的硬件构成为：多个追踪相机或者其他外部追踪传感器，配合机器人上的标记体，标记体可以是一些标记点，比如二维码或者棋盘格等。参照图1，系统的搭建流程；
41.前解决方案的机器人轨迹定位系统一般安装在固定的场地，比如清洁机器人的国标实验室环境。系统一般用多个摄像头或者其他位置定位系统，安装完毕保证位置固定，标定摄像头的内参，标定多个摄像头的相对位姿，再标定摄像头和场地平面的相对位姿。外参的标定对于轨迹测量的精度影响很大，多个摄像头的相对位姿或者摄像头和场地的相对位姿发生微小变化，就需要重新进行标定。这造成以下不便：外参标定费时费力；外参精度只有到实际测量时才能评估，造成测量的精度较低；相对位姿变化后，需要重新标定。
42.参照图2，本发明在只标定相机内参的情况下，可以得到稳定可靠的位置定位，不需要标定外参。在同一片场地，多个相机和场地的位置不受限制，多个相机和场地平面的位姿发生变化后，不需要做外参标定。可以快速布置到新场地中。参照图2，采用多目摄像头免标定外参进行机器人行走轨迹定位的系统，包括固定硬件模块、内参标定模块、追踪求解模块、拼接模块和测试模块，所述固定硬件模块和内参标定模块连接，且追踪求解模块分别与固定硬件模块和内参标定模块连接，所述追踪求解模块与拼接模块连接，且拼接模块和测试模块连接。
43.本实施例中，所述固定硬件模块包括多个追踪相机和标记点。
44.本实施例中，所述追踪相机采用具有图形采集功能的相机；通过多个相机来覆盖需要测试的场地；需要满足以下条件：相机需要有相同的时间源来获取图像。可以通过把多个相机接到同一台图像采集电脑上实现多个相机的时间戳同源。参照图3，相机覆盖的区域需要有部分重叠，以两个相机为例。
45.参照图3，以两个相机为例，所述标记点一般采用二维码、棋盘格等二维几何图形。标记点粘贴在被测试机器人上，并可以被相机完整观察到。
46.本实施例中，所述内参标定模块用于给多个追踪相机分别做内参标定。
47.本实施例中，所述内参标定模块一般采用张正友标定法来标定追踪相机的内参。
48.本实施例中，所述追踪求解模块用于在测试中，针对单个追踪相机，分别通过图像追踪外部标记点，并求解出标记点在测试平面内的轨迹，其具体包括以下步骤：
49.s1：通过算法识别出标记点；采用开源软件库进行标记点检测，例如opencv软件库。对于棋盘格，使用opencv库中的cv::findchessboardcorners()函数检测棋盘格角点，得到按顺序的一些列点的像素坐标。例如，对于一定4行6列角点的棋盘格，检测得到的角点顺序参照图4；对于aruco码，使用opencv库中的cv::aruco::detectmarkers()函数检测，得到aruco码4个角点的像素坐标，角点的顺序参照图5；s2：通过算法求解出标记点在相机平面下的位姿；在s1中检测图像中棋盘格角点或者二维码角点之后，得到按顺序的一系列角点坐标。同时，可以实际测量出棋盘格角点或者二维码角点的间距，计算出棋盘格各个角点在以0号角点为原点的三维空间坐标系中的坐标，或者二维码各个角点在以0号角点为原点的三维空间坐标系中的坐标；根据图像中检测出的角点的像素坐标以及其对应的以0号角点为原点的三维空间坐标系中的坐标，以pnp算法求解棋盘格/二维码坐标系与相机坐标系之间的变换{r，t}；参照图6；pnp算法有很多具体实现，例如p3p，epnp，upnp，dlt等等，根据使用的标记点不同，可采用相应的算法。例如对于棋盘格，可采用epnp算法，对于aruco码，可采用p3p算法。此处详细介绍dlt算法：
50.对于三维空间点p，齐次坐标坐标为{x,y,z,1}，对应图像上的点齐次坐标为{u,v,1}，内参矩阵为k，满足：
[0051][0052]
消去s得到约束：
[0053]
a1x+a2y+a3z+a
4-uc(a9x+a
10
y+a
11
z+a
12
)＝0
[0054]
a5x+a6y+a7z+a
8-vc(a9x+a
10
y+a
11
z+a
12
)＝0
[0055]
整理得：
[0056][0057]
于n个点，有n个方程，得：
[0058][0059]
这是一个形如ax＝0的方程，可以得到||x||＝1条件下的最小二乘解
argmin||ax||
2；对a进行svd分解：
[0060]
a＝u∑v
t
[0061]
v的对应最小特征值的特征向量就是最小二乘的解：
[0062]
(v
1 v
2 v
3 v
4 v
5 v
6 v
7 v
8 v
9 v
10 v
11 v
12
)
t
[0063]
对于：
[0064][0065]
进行svd分解：
[0066][0067]
令
[0068]
棋盘格/二维码坐标系与相机坐标系之间的变换为：
[0069][0070][0071]
s3：计算机器中心在相机坐标系下的空间位置；对于贴在机身上的标记点(棋盘格/二维码)，容易得到机器中心在棋盘格/二维码坐标系下的坐标(xo,yo,zo)
t
，由第4步计算出棋盘格/二维码坐标系与相机坐标系之间的变换{r，t}后，机器中心在相机坐标系下的坐标(xc,yc,zc)
t
可以由下式计算：
[0072]
(xc,yc,zc)
t
＝r*(xo,yo,zo)
t
+t
[0073]
s4：通过算法求解出机器中心在测试平面下的坐标。随着机器在场地内运动和追踪相机不断采集图像，经过第3～5步得到一系列机器中心在相机坐标系下的坐标：{(xci,yci,zci)
t
|i＝1,2,3,
…
,n}；
[0074]
(1)计算出坐标序列的平均值：
[0075][0076]
(2)所有坐标减去平均值：
[0077]
xci＝xci-x0，yci＝yci-y0，zci＝zci-z0
[0078]
(3)将坐标按矩阵形式排列：
[0079][0080]
(4)对矩阵进行svd分解，得到3个特征值和3个特征向量：
[0081]
σ0＞σ1＞σ2，[v1，v2，v3]
[0082]
(5)将(xci,yci,zci)
t
投影到[v1,v2,v3]坐标系下：(xvi,yvi,zvi)
t
＝[v1,v2,v3]*(xci,yci,zci)
t
；
[0083]
(6)舍弃zvi，得到平面坐标序列：{(xvi,yvi)
t
|i＝1,2,3,
…
,n}，本实施例中，所述拼接模块用于通过算法拼接多个追踪相机的测试数据，得到完整轨迹，其具体包括以下步骤：
[0084]
s1：通过时间戳找到两个相机之间重叠的轨迹点，如果是多个相机，找到两两之间的重叠的轨迹点。参照图7、图8和图9；图中蓝色的点集合是相机1测试到的，红色的点集合是相机2测试到的；
[0085]
s2：用icp算法，对齐相机1和相机2的重叠区域的轨迹，并找到两段轨迹的r和t(位姿变换矩阵)；参照图10；对于使用第3段方法计算得到的两段轨迹点{(xai,yai)
t
}和{(xbi,ybi)
t
}；
[0086]
(1)计算两段轨迹的中心：
[0087][0088][0089]
(2)计算矩阵：
[0090]
(3)对h进行svd分解：h＝u∑v
t
[0091]
(4)得到两段轨迹之间的相对变换：
[0092][0093]
s3：利用找到的r和t把两个追踪相机的所有轨迹点拼接在一起。参照图11，通过已经找到的r和t，把相机1和2分别追踪到的轨迹拼接成一段完整的轨迹。其中蓝色的为相机1测试到的轨迹，红色的为相机2测试到的轨迹；如果是超过两个相机，可以两两重复上述步骤，并最终拼接成一个完整的轨迹；
[0094]
实施例二
[0095]
参照图3，在一个有两个房间的场地中，希望测试清洁机器人的真实运动轨迹。两个房间必须采用至少两个摄像头进行测试以保证没有盲区。测试系统的硬件采用两个等效焦距28mm，分辨率1920*1200的全局曝光相机作为数据采集端；标记点采用aruco二维码。相机固定在距离地面2.8米高的位置，斜朝下对地面进行测试。两个相机分别装在两个房间，但是这两个相机的视野有部分交叠。测试的目标是家庭清洁机器人，二维码粘贴在机器人的背部平面上；
[0096]
再按照下述步骤进行：采用张正友标定法标定相机的内参；运行机器人，机器人在
场地中行走，相机在拍摄的视野中观察到二维码，并测试出分别在相机坐标系下的机器人的运动轨迹；
[0097]
通过算法拼接多个相机的测试数据，得到完整轨迹；
[0098]
实施例三
[0099]
参照图12，在一个有三个房间的场地中，希望测试清洁机器人的真实运动轨迹。必须采用至少三个摄像头进行测试以保证没有盲区。测试系统的硬件采用两个等效焦距28mm，分辨率1920*1200的全局曝光相机作为数据采集端；标记点采用棋盘格。相机固定在距离地面2.8米高的位置，斜朝下对地面进行测试。三个相机分别装在两个房间，但是这三个相机的视野两两有交叠。测试的目标是家庭清洁机器人，棋盘格粘贴在机器人的背部平面上。
[0100]
再按照下述步骤进行：采用张正友标定法标定相机的内参；运行机器人，机器人在场地中行走，相机在拍摄的视野中观察到二维码，测试出分别在相机1，相机2，相机3坐标系下的机器人的运动轨迹；找到相机1和相机2的位姿变换矩阵r12和t12；找到相机2和相机3的位姿变换矩阵r23和t23；把相机1，相机2和相机3的各自的轨迹点拼接成一个完整的轨迹点集合。
[0101]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。