本发明涉及移动机器人，特别是移动机器人的多传感器的联合标定方法。
背景技术：
移动机器人自主导航是机器人研究领域的一个热点，移动机器人导航要求机器人通过传感器感知环境和自身状态，实现在有障碍物的环境中自主定位。不同类型传感器之间的信息融合是提高机器人环境理解与认知性能的必备手段，其为移动机器人提供更精确的环境信息。
近年来，有许多系统开始集成使用camera与2DLase-Rangefinder结合的方式，视觉具有探测范围宽，获取信息完整、价格低等优点，但由于其易受外界环境影响如光照，存在目标缺失以及模糊等问题。激光受外部环境变化影响小，但存在数据点稀疏，扫描频率低的缺点，将二者融合，可以弥补各自缺失。
在进行数据融合时，多传感器的标定是必须解决的问题之一。标定一般分为各传感器自身参数的标定以及多传感器的联合标定。各传感器自身参数的标定是为了保证所采集数据的准确性，而多传感器的联合标定是为了多传感器数据的准确匹配,确保数据融合的准确性。在针对camera与Laser传感器的标定中，必须预先获得他们之间的旋转矩阵和平移矩阵。目前的标定方法中较多使用黑白棋盘格标定板的方法进行标定，该方法如(QilongZhang，RobertPless.Extrinsiccalibrationforcameraandlaserrangerfinder(improvescameraintrinsiccalibration).IEEEIROS，pp.2301-2306，2004)(译为：相机与激光雷达外参数的标定(同时优化相机内参数))中所述。但该方法依赖于扫描物质的特性及距离激光的远近，同时激光数据点的稀疏程度也极大地影响标定结果。
另有采用特殊形状的标定物，如四足锥状体标定键，通过分割激光扫描点位于四个足之间各个不同部位的数据和标定键的形状进行比对来获取外参数信息。这种方法存在的主要技术问题就在于对标定物的工艺要求较高；其次，通过标定键形状约束跟分割的数据的比对作为扫描位置的估计的方法其数学变换复杂，且本身就很难精确，再利用估计的结果进行标定计算，精度不高；最后，尽管该方法不需要相机的参与，属于单独标定的方法，但其标定精度不如上述Zhang等人的方法。
综上所述，现有所存在方法的研究工作重点主要集中于怎样估计激光雷达的扫描线上，依赖于外部标定板的较多，限定了标定的外部条件。然后，利用本身有误差的估计结果来进行外参数的计算，使得标定结果的准确度不理想。
技术实现要素：
本发明专利申请的目的在于提供移动机器人的多传感器的联合标定方法，以解决背景技术中的问题。
本发明方法的基本原理如下：
根据针孔摄像机模型描述，对于空间任意一点p，其在相机坐标系的坐标，在2D激光雷达坐标系下的坐标，则点p在相机坐标系下的坐标与在2D激光雷达坐标系下的坐标存在以下关系：
，其中，Rcl为相机坐标系至2D激光雷达坐标系的旋转矩阵，可用列向量表示为，tcl为相机坐标系至2D激光雷达坐标系之间的平移向量，为3维列向量，通过计算Rcl和tcl，即可完成移动机器人的相机和2D激光雷达之间的联合标定。
本发明专利申请的技术方案如下：
一种移动机器人的多传感器的联合标定方法，所述移动机器人包括2D激光雷达和相机，其特征在于，包括以下步骤：
S1：对相机进行内参数标定，得到相机的内参数矩阵；
S2：将相机和2D激光雷达放置在移动机器人的固定位置，使得2D激光雷达和相机在移动机器人运动过程中保持不变；
S3：获取ti时刻相机在世界坐标系下的位置，其中i为正整数；
S4：获取ti时刻2D激光雷达在世界坐标系下的位置；
S5：重复步骤S3、S4直到i不小于4；
S6：根据，得到相机与2D激光雷达的旋转矩阵Rcl以及平移矩阵tcl。
在本发明专利申请的一个具体实施例中，步骤S3包括以下子步骤：
S31：对ti时刻采集到的图片进行特征点提取；
S32：建立以像素为单位的图像坐标系u-v和以物理单位表示的相机坐标系x-y；
S33：通过相机坐标系x-y与世界坐标系之间的变换得到图像坐标系u-v与世界坐标系之间的关系；
S34：构建特征点在世界坐标系坐标与图像坐标系u-v之间的投影关系测量模型；
S35：根据测量模型并结合单目视觉SLAM算法进行计算，得到相机在世界坐标系下的位置。
在本发明专利申请的一个具体实施例中，步骤S4包括以下子步骤：
S41：将2D激光雷达采集到的环境点的极坐标转化为笛卡尔坐标；
S42：对采集到的环境点的笛卡尔坐标进行相关匹配算法。
进一步地，所述步骤S42中的匹配算法为ICP算法。
与现有技术相比，本发明申请具有以下优点：
1、本发明专利申请技术方案完全脱离标定板的限制，可以在多种环境下进行标定；
2、本发明专利申请技术方案可以在使用过程中实时标定，解决了现阶段移动机器人仅出厂标定参数，至后期参数出现波动的造成的定位误差，方便用户在使用过程中进行校正。
附图说明
图1为本发明专利申请较佳实施例的流程图。
图2为图1中步骤S3的子步骤构成流程图。
图3为图1中步骤S4的子步骤构成流程图。
图4为本发明专利申请的针孔成像模型示意图。
图5为图1中的移动机器人标定示意图。
具体实施方式
下面结合附图和具体实施方式对本发明方案进行进一步详细说明。
如附图1至附图5所示，为本发明的一个优选实施例。
S1：对相机进行内参数标定，得到相机的内参数矩阵；
采用针孔摄像机模型不考虑透镜畸变因子（不考虑透镜因子可以使算法相对简单），使用传统的棋盘表格进行相机的内部参数标定，得到相机的内参数矩阵，其中，为相机的焦距，为相机光心。
S2：将相机和2D激光雷达放置在移动机器人的固定位置，使得2D激光雷达和相机在移动机器人运动过程中保持不变；
由于要对相机和2D激光雷达进行联合标定，必定需要求在移动机器人的固定安装，使它们在移动机器人运动过程中保持相对静止，如图5所示。
S3：获取ti时刻相机在世界坐标系下的位置，i为正整数。
在该步骤中，本实施例是通过以下子步骤得到相机在世界坐标系下的坐标的：
S31：对ti时刻采集到的图片进行特征点提取；
移动机器人在运动过程中，相机按一定频率采集图片，然后对在ti时刻采集到的图片进行特征提取，其中，i表示第i次进行图片特征提取。
S32：建立以像素为单位的图像坐标系u-v和以物理单位表示的相机坐标系x-y；
在图像平面内，以左上角为坐标原点，水平方向为u轴，竖直方向为v轴，单位为像素，由于图像坐标系u-v只表示了数字图像中像素所处的行数和列数，而没有用物理单位表示出该像素在图像中的位置，需要再建立以物理单位表示的坐标系，即图像坐标系x-y。在图像坐标系x-y中，原点o1定义在摄像机光轴与图像平面的交点处，o1在坐标系中的坐标为（u0，v0），每一个像素在x轴和y轴方向上的物理尺寸大小为dx，dy，则在图像中每一个像素在两个坐标系下的坐标有以下关系：
（1）
图像坐标系u-v与相机坐标系x-y存在以下关系：
（2）
S33：通过图像坐标系u-v与世界坐标系之间的变换得到图像坐标系x-y与世界坐标系之间的关系；
相机坐标系x-y与世界坐标系可以通过旋转矩阵Rcw和平移向量tcw来描述，空间点p的世界坐标与相机坐标系存在以下关系：
（3）
由公式（1）、（2）、（3）得到图像坐标系与世界坐标系之间的关系：
(4)
S34：构建特征点在世界坐标系坐标与图像坐标系之间的投影关系测量模型；
本实施例中基于卡尔曼滤波器的单目视觉SLAM框架，相机的状态Xcw由世界坐标下相机位置rT、旋转四元数qT、线速度vT、角速度wT组成：
(5)
地图特征状态向量由特征点的世界坐标组成：
(6)
其中，。n代表地图中特征点的数目，因此，相机状态向量X和特征点状态向量组成SLAM系统状态向量：
(7)
本实施例中，SLAM系统采用匀速模型作为过程模型，得到相机状态预测方程：
（8）
其中，表示过程噪声。
S35：根据测量模型并结合单目视觉SLAM算法进行计算，得到相机在世界坐标系下的位置。
利用地图中特征点的三维坐标与其对应成像坐标系的二维坐标之间的投影关系构建测量模型：
（9）
由以上视觉模型并结合相关的单目视觉SLAM算法即可得到相机在世界坐标系的位置：
S4：获取ti时刻2D激光雷达在世界坐标系下的位置；
本实施例中，是通过以下子步骤获取2D激光雷达在世界坐标系下的位置，包括：
S41：将2D激光雷达采集到的环境点的极坐标转化为笛卡尔坐标；
2D激光雷达可以直接获得环境的水平剖面图，测得的环境点的信息是以2D位中心的极坐标，表示为，因而需要将极坐标系下的坐标转换为笛卡尔坐标系下的坐标，表示为：
（10）
其中，r表示2D激光雷达扫描获得的距离值，Ψ为水平扫描角。
S42：对采集到的环境点的笛卡尔坐标进行相关匹配算法。
对采集到的环境点的数据进行相关的匹配算法，如较常使用的ICPSLAM进行相关的处理计算，ICP算法是一种快速、精确的算法，在点集和图像的配准中收到广泛的应用，假设两帧激光获得和两个激光点集，则ICP算法可以表示为：
(11)
其中，mj与pi为两帧待匹配激光点，RMP为旋转矩阵，tMP为平移向量。
由公式（10）、（11）即可得到2D激光雷达在世界坐标系下的位置：
S5：重复步骤S3、S4直到i不小于4；
由于Rcl和tcl分别由3个特征量和3个特征量构成，因此需要至少4组数据才能求得Rcl和tcl，即在移动机器人运动过程中，分别在4个不同时间点ti采集图片信息和2D激光雷达数据，然后重复步骤S3、S4，以得到4组相机在世界坐标下的坐标和2D激光雷达在世界坐标系下的坐标。
S6：根据，得到相机与2D激光雷达的旋转矩阵旋转矩阵Rcl以及平移矩阵tcl。
将步骤5中获得的4组数据代入，通过高斯迭代使总的均方误差收敛，即可得到旋转矩阵Rcl以及平移矩阵tcl，从而完成了移动机器人的相机和2D激光雷达的联合标定。
以上是本发明的较佳实施例的详细说明，不认定本发明只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下所作出的等同替代或明显变形，且性能或用途相同，都应当视为本发明所提交的权利要求书确定的保护范围内。