CN113436274B - 一种移动机器人的校准方法、装置及设备 - Google Patents

Abstract

本发明公开了一种移动机器人的校准方法、装置及设备，方法包括以下步骤：获取标定平面中3D相机和2D激光传感器的点云数据；建立平面方程，代入点云数据后得到误差方程；使用非线性最小二乘法对误差方程进行求解，得到最终参数；根据最终参数对差速驱动底盘移动机器人进行校准。本发明通过已校准的车载外部2D激光传感器位置，得到3D视觉检测摄像头相对于AGV车体中心点的安装位置，不仅实现了3D视觉检测摄像头安装位置与AGV的2D激光传感器的自动校准，而且精度高。

Description

一种移动机器人的校准方法、装置及设备

技术领域

本发明涉及一种移动机器人的校准方法、装置及设备，属于机器人校准技术领域。

背景技术

随着计算机和机器人技术的发展，移动机器人得到了快速发展和广泛应用，几乎渗透到了社会生活的各个领域，在AGV小车的工作过程中，为了实时获得小车在环境中的位置和姿态信息，以便进行路径规划和路径修正，需要用到多种传感器。

近年来，基于激光雷达定位的传感器是成为移动机器人导航的主要应用之一。激光雷达定位具有精度高、速度快、稳定性强等优点，但只能获取一定高度的大角度场中的信息，为了获取更多的环境信息，现在的移动机器人融合安装在移动机器人上的相机数据，使移动机器人能适应更多的环境，执行更广泛的任务。为了有效地利用相机和激光测距仪的数据，了解它们之间的相对位置和方位是非常重要的，这将影响对其测量结果的几何解释。

发明内容

为了解决上述问题，本发明提出了一种移动机器人的校准方法、装置及设备，能够对摄像头安装位置与AGV中心点进行自动校准。

本发明解决其技术问题采取的技术方案是：

第一方面，本发明实施例提供的一种移动机器人的校准方法，包括以下步骤：

获取标定平面中3D相机和2D激光传感器的点云数据；

建立平面方程，代入点云数据后得到误差方程；

使用非线性最小二乘法对误差方程进行求解，得到最终参数；

根据最终参数对移动机器人进行校准。

作为本实施例一种可能的实现方式，所述获取标定平面中3D相机和外部2D激光传感器的点云数据，包括：

3D相机坐标系用P＝[X,Y,Z]^T表示，以2D激光传感器的原点定义激光坐标系，并且激光扫描平面为平面Y＝0；

从3D相机坐标系到激光坐标系的刚体变换方程为：

P^f＝ΦP+Δ

其中Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量，P^f为3D相机坐标系下的一个点P在激光坐标系下的表示。

作为本实施例一种可能的实现方式，所述建立平面方程，得到误差方程，包括：

标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

使用RANSAC算法对标定平面进行拟合，得到拟合后平面方程；

设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到矩阵方程：

其中，A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数；

将标定板不同姿态下的相机平面与激光数据的误差累计，得到最终的误差方程：

其中，A_i,B_i,C_i,D_i为根据相机点云数据(x_i，y_i)对应的参数。

作为本实施例一种可能的实现方式，所述对误差方程进行求解，得到最终参数，包括：

使用最小二乘法对误差方程求解得到最终参数。

第二方面，本发明实施例提供的一种移动机器人的校准装置，包括：

数据获取模块，用于获取标定平面中3D相机和2D激光传感器的点云数据；

方程模块，用于建立平面方程，代入点云数据后得到误差方程；

方程求解模块，用于使用非线性最小二乘法对误差方程进行求解，得到最终参数；

校准模块，用于根据最终参数对移动机器人进行校准。

作为本实施例一种可能的实现方式，所述数据获取模块，具体用于：

3D相机坐标系用P＝[X,Y,Z]^T表示，以2D激光传感器的原点定义激光坐标系，并且激光扫描平面为平面Y＝0；

从3D相机坐标系到激光坐标系的刚体变换方程为：

P^f＝ΦP+Δ

其中Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量，P^f为3D相机坐标系下的一个点P在激光坐标系下的表示。

作为本实施例一种可能的实现方式，所述方程模块，具体用于：

标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

使用RANSAC算法对标定平面进行拟合，得到拟合后平面方程；

设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到矩阵方程：

其中，A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数；

将标定板不同姿态下的相机平面与激光数据的误差累计，得到最终的误差方程：

其中，A_i,B_i,C_i,D_i为根据相机点云数据(x_i，y_i)对应的参数。

第三方面，本发明实施例提供的一种计算机设备，包括处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当所述装置运行时，所述处理器与所述存储器之间通过总线通信，所述处理器执行所述机器可读指令，以执行时执行如上述任意移动机器人的校准方法的步骤。

第四方面，本发明实施例提供的一种存储介质，该存储介质上存储有计算机程序，该计算机程序被处理器运行时执行如上述任意移动机器人的校准方法的步骤。

本发明实施例的技术方案可以具有的有益效果如下：

本发明通过已校准的车载外部2D激光传感器位置，得到3D视觉检测摄像头相对于AGV车体中心点的安装位置，不仅实现了3D视觉检测摄像头安装位置与AGV的2D激光传感器的自动校准，而且精度高。

附图说明：

图1是根据一示例性实施例示出的一种移动机器人的校准方法的流程图；

图2是根据一示例性实施例示出的一种移动机器人的校准装置的结构图；

图3是根据一示例性实施例示出的一种获取标定平面中3D相机和2D激光传感器的点云数据的示意图；

图4是根据一示例性实施例示出的一种计算机设备的结构图。

具体实施方式

下面结合附图与实施例对本发明做进一步说明：

为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。

图1是根据一示例性实施例示出的移动机器人的校准方法的流程图。如图1所示，本发明实施例提供的一种移动机器人的校准方法，包括以下步骤：

获取标定平面中3D相机和2D激光传感器的点云数据；

建立平面方程，代入点云数据后得到误差方程；

使用非线性最小二乘法对误差方程进行求解，得到最终参数；

根据最终参数对移动机器人进行校准。

作为本实施例一种可能的实现方式，所述获取标定平面中3D相机和外部2D激光传感器的点云数据，包括：

3D相机坐标系用P＝[X,Y,Z]^T表示，以2D激光传感器的原点定义激光坐标系，并且激光扫描平面为平面Y＝0；

从3D相机坐标系到激光坐标系的刚体变换方程为：

P^f＝ΦP+Δ

其中Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量，P^f为3D相机坐标系下的一个点P在激光坐标系下的表示。

作为本实施例一种可能的实现方式，所述建立平面方程，得到误差方程，包括：

标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

使用RANSAC算法对标定平面进行拟合，得到拟合后平面方程；

设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到矩阵方程：

其中，A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数；

将标定板不同姿态下的相机平面与激光数据的误差累计，得到最终的误差方程：

其中，A_i,B_i,C_i,D_i为根据相机点云数据(x_i，y_i)对应的参数。

作为本实施例一种可能的实现方式，所述对误差方程进行求解，得到最终参数，包括：

使用最小二乘法对误差方程求解得到最终参数。

如图2所示，本发明实施例提供的一种移动机器人的校准装置，包括：

数据获取模块，用于获取标定平面中3D相机和2D激光传感器的点云数据；

方程模块，用于建立平面方程，代入点云数据后得到误差方程；

方程求解模块，用于使用非线性最小二乘法对误差方程进行求解，得到最终参数；

校准模块，用于根据最终参数对移动机器人进行校准。

如图3所示，校准需要使用一块足够大的平板作为标定板，标定板应具有浅色(最好为白色)，返光强度低，不透明的特点，标定板的位置应在相机传感器正方向上，距离在1～2m左右，因为2D激光传感器的扫描角度一般为270或360度，保持标定板在2D激光传感器扫描角度以内即可。

相机坐标系可以用P＝[X,Y,Z]^T表示，实际情况中，相机可能会表现出明显的镜头畸变，可以将其建模为包含径向和切向畸变系数的5维参数，这样，我们就不用考虑相机的镜头畸变了。激光测距仪输出激光测距数据，这些数据是到与地板平行的平面上的点的距离测量值。以激光测距仪的原点定义激光坐标系，并且激光扫描平面为平面Y＝0。假设相机坐标系下的一个点P在激光坐标系下的表示为P^f，从相机坐标系到激光坐标系的刚体变换可描述为：

P^f＝ΦP+Δ 公式1

其中Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量。

本发明的标定方法需要在2D激光传感器和相机前面放置一个标定板，假设标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

3D相机获取到标定板的点云数据，数据格式为：((x₁,y₁,z₁),(x₂,y₂,z₂)Λ(x_n,y_n,z_n))，使用RANSAC算法可以对平面进行拟合，得到平面方程。

由于2D激光传感器获得的点云数据必须位于相机的标定平面上，所以可以得到相机坐标系和激光坐标系之间的刚性转换的几何约束，设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到如下公式：

其中A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数，公式2的结果即为单帧数据的误差，将标定板不同姿态下的相机平面与激光数据的误差累计，即可得到最终的误差方程：

使用最小二乘法对误差方程求解，就得到了最优的参数估计。

RANSAC算法是“RANdom SAmple Consensus(随机抽样一致)”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。

1.首先随机假设一小组局内点为初始值。然后用此局内点拟合一个模型，此模型适应于假设的局内点，所有的未知参数都能从假设的局内点计算得出。

2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点，将局内点扩充。

3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。

4.然后，用所有假设的局内点去重新估计模型，因为此模型仅仅是在初始的假设的局内点估计的，后续有扩充后，需要更新。

5.最后，通过估计局内点与模型的错误率来评估模型。

利用本发明进行3D视觉检测摄像头安装位置与AGV2D激光传感器自动校准的操作过程如下。

1. 3D相机与2D激光传感器连接

首先在移动机器人上固定好3D相机和2D激光传感器的位置，将相机和激光连接到同一计算机，配置相机和2D激光传感器IP地址到计算机同一网段。

2.标定板制作

校准需要使用一块硬质平面板，标定板的宽度需要大于1m，长度大于相机与激光的垂直高度差，并且手动移动时不能产生弯曲。标定板的颜色最好为白色，灰色等淡色，保证激光器和相机成像。

3.初始参数测量

手动测量相机与激光相对位置参数，作为误差方程收敛的初始值，初始值可以加速收敛，减少产生局部最优解的可能性。

4.校准程序

开启校准程序，校准程序与相机和2D激光传感器连接，连接完成后，校准程序可以接收相机和2D激光传感器的数据，并将数据保存在内存中。

5.移动标定板，采集数据

设标定板平面在相机坐标系中沿X、Y、Z轴的旋转的角度分别为(x y z)，初始夹角为[0 0 0]，将标定板旋转到以下角度并保持5s左右时间，采集点云数据，让最终结果收敛。

x	45	45	45	45	-45	-45	-45	-45
									y	45	45	-45	-45	45	45	-45	-45
z	0	45	0	45	0	45	0	45

6.开始校准，输出结果

完成标定板移动后，通过校准程序停止数据采集并选择开始校准，校准程序会计算内存中采集到的数据并进行校准计算，并将最终校准结果：2D激光传感器中心到相机中心的变化矩阵输出至程序控制台界面。

本发明实现了3D视觉检测摄像头安装位置与AGV的2D激光传感器的自动校准，通过已校准的车载外部2D激光传感器位置，可以得到3D视觉检测摄像头相对于AGV车体中心点的安装位置。校准过程中涉及六个参数的估计，分别为相机与2D激光传感器在坐标系中沿三个坐标轴方向的位置偏移以及相机正方向与2D激光传感器正方向沿三个坐标轴的角度偏移，最终参数可以表示为一个4*4的变化矩阵，解释为相机坐标系转化到激光坐标系的变化矩阵。本发明主要用到的技术包括：

一.通过3D相机和外部2D激光传感器获取标定平面的点云数据；

二.根据3D相机点云数据建立平面方程，将2D激光传感器数据代入平面方程，得到误差方程；

三.使用非线性最小二乘法对误差方程进行求解，得到相机与激光的变化矩阵。

本发明校准的两个传感器为3D相机和2D2D激光传感器，3D相机获取的点云数据为相机坐标系下物体每一点的三维坐标，根据三维坐标可以拟合出平面方程的参数。2D2D激光传感器获取的数据为一定高度上激光坐标系下物体指定高度上每一点的二维坐标。

本发明精度可达到10mm，1°以内，需要使用标定板进行指定运动，并涵盖到所有参数即可实现结果收敛。

图4是根据一示例性实施例示出的一种计算机设备的结构图。如图4所示，本发明实施例提供的一种计算机设备，包括处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当所述装置运行时，所述处理器与所述存储器之间通过总线通信，所述处理器执行所述机器可读指令，以执行时执行如上述任意移动机器人的校准方法的步骤。

具体地，上述存储器和处理器能够为通用的存储器和处理器，这里不做具体限定，当处理器运行存储器存储的计算机程序时，能够执行上述移动机器人的校准方法。

本领域技术人员可以理解，图4中示出的计算机设备的结构并不构成对计算机设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者拆分某些部件，或者不同的部件布置。

在一些实施例中，该计算机设备还可以包括触摸屏可用于显示图形用户界面(例如，应用程序的启动界面)和接收用户针对图形用户界面的操作(例如，针对应用程序的启动操作)。具体的触摸屏可包括显示面板和触控面板。其中显示面板可以采用LCD(LiquidCrystal Display，液晶显示器)、OLED(Organic Light-Emitting Diode,有机发光二极管)等形式来配置。触控面板可收集用户在其上或附近的接触或者非接触操作，并生成预先设定的操作指令，例如，用户使用手指、触笔等任何适合的物体或附件在触控面板上或在触控面板附近的操作。另外，触控面板可包括触摸检测装置和触摸控制器两个部分。其中，触摸检测装置检测用户的触摸方位、姿势，并检测触摸操作带来的信号，将信号传送给触摸控制器；触摸控制器从触摸检测装置上接收触摸信息，并将它转换成处理器能够处理的信息，再送给处理器，并能接收处理器发来的命令并加以执行。此外，可以采用电阻式、电容式、红外线以及表面声波等多种类型实现触控面板，也可以采用未来发展的任何技术实现触控面板。进一步的，触控面板可覆盖显示面板，用户可以根据显示面板显示的图形用户界面，在显示面板上覆盖的触控面板上或者附近进行操作，触控面板检测到在其上或附近的操作后，传送给处理器以确定用户输入，随后处理器响应于用户输入在显示面板上提供相应的视觉输出。另外，触控面板与显示面板可以作为两个独立的部件来实现也可以集成而来实现。

对应于上述应用程序的启动方法，本发明实施例还提供了一种存储介质，该存储介质上存储有计算机程序，该计算机程序被处理器运行时执行如上述任意移动机器人的校准方法的步骤。

本申请实施例所提供的应用程序的启动装置可以为设备上的特定硬件或者安装于设备上的软件或固件等。本申请实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，前述描述的系统、装置和单元的具体工作过程，均可以参考上述方法实施例中的对应过程，在此不再赘述。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个模块或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。

作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

另外，在本申请提供的实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (7)

1.一种移动机器人的校准方法，其特征是，包括以下步骤：

获取标定平面中3D相机和2D激光传感器的点云数据；

建立平面方程，代入点云数据后得到误差方程；

使用非线性最小二乘法对误差方程进行求解，得到最终参数；

根据最终参数对移动机器人进行校准；

所述建立平面方程，得到误差方程，包括：

标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

使用RANSAC算法对标定平面进行拟合，得到拟合后平面方程；

设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到矩阵方程：

其中，A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数，Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量；

将标定板不同姿态下的相机平面与激光数据的误差累计，得到最终的误差方程：

其中，A_i,B_i,C_i,D_i为根据相机点云数据(x_i，y_i)对应的参数。

2.根据权利要求1所述的移动机器人的校准方法，其特征是，所述获取标定平面中3D相机和外部2D激光传感器的点云数据，包括：

3D相机坐标系用P＝[X,Y,Z]^T表示，以2D激光传感器的原点定义激光坐标系，并且激光扫描平面为平面Y＝0；

从3D相机坐标系到激光坐标系的刚体变换方程为：

P^f＝ΦP+Δ

其中，P^f为3D相机坐标系下的一个点P在激光坐标系下的表示。

3.根据权利要求1所述的移动机器人的校准方法，其特征是，所述对误差方程进行求解，得到最终参数，包括：

使用最小二乘法对误差方程求解得到最终参数。

4.一种移动机器人的校准装置，其特征是，包括：

数据获取模块，用于获取标定平面中3D相机和2D激光传感器的点云数据；

方程模块，用于建立平面方程，代入点云数据后得到误差方程；

方程求解模块，用于使用非线性最小二乘法对误差方程进行求解，得到最终参数；

校准模块，用于根据最终参数对移动机器人进行校准；

所述方程模块，具体用于：

标定平面在相机坐标系P中的平面方程为：

Ax+By+Cz+D＝0

使用RANSAC算法对标定平面进行拟合，得到拟合后平面方程；

设2D激光传感器获取的点云数据为((x₁,y₁),(x₂,y₂)Λ(x_i,y_i))，将2D激光传感器数据代入平面方程，得到矩阵方程：

其中，A,B,C,D为根据相机点云数据求出的已知参数，Φ和Δ为待求解参数，Φ表示相机相对2D激光传感器的方向的3*3的正交矩阵，Δ表示对应的相对位置的3维向量；

将标定板不同姿态下的相机平面与激光数据的误差累计，得到最终的误差方程：

其中，A_i,B_i,C_i,D_i为根据相机点云数据(x_i，y_i)对应的参数。

5.根据权利要求4所述的移动机器人的校准装置，其特征是，所述数据获取模块，具体用于：

3D相机坐标系用P＝[X,Y,Z]^T表示，以2D激光传感器的原点定义激光坐标系，并且激光扫描平面为平面Y＝0；

从3D相机坐标系到激光坐标系的刚体变换方程为：

P^f＝ΦP+Δ

其中，P^f为3D相机坐标系下的一个点P在激光坐标系下的表示。

6.一种计算机设备，其特征是，包括处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当所述计算机设备运行时，所述处理器与所述存储器之间通过总线通信，所述处理器执行所述机器可读指令，以执行时执行如权利要求1-3任一所述的移动机器人的校准方法的步骤。

7.一种存储介质，其特征是，该存储介质上存储有计算机程序，该计算机程序被处理器运行时执行如权利要求1-3任一所述的移动机器人的校准方法的步骤。

Images