CN113311412B - 用于移动机器人的激光雷达传感器六维外参标定方法 - Google Patents

Abstract

本发明公开了一种用于移动机器人的激光雷达传感器六维外参标定方法，包括：在移动机器人设置两个激光雷达和二维码定位模块；利用两个激光雷达扫描当前环境中固定的两个平面，操作在二维码定位模块附近的激光雷达扫描两个平面，记录每一帧的基座相对地图坐标系的位姿，以及激光雷达扫描得到的点云；采用基于随机采样一致方法的直线提取方法处理位姿和点云，求解每一帧交线的交点相对于激光雷达的坐标；求解实际每个交点在世界坐标系中的坐标与两个平面的距离和理论距离的偏差函数，根据偏差函数构建待优化问题模型；采用高斯‑牛顿法求解待优化问题模型，得到最优相对位姿，完成对激光雷达六维外参标定。该标定方法前期准备时间短，且标定误差小。

Description

用于移动机器人的激光雷达传感器六维外参标定方法

技术领域

本发明涉及移动机器人技术领域，特别涉及一种用于移动机器人的激光雷达传感器六维外参标定方法。

背景技术

激光测距雷达传感器具有实时性好、测距稳定性高和抗光照能力强等特点，支撑机器人环境感知、避障和导航等技术发展。激光测距传感器具有高性价比优势，在移动机器人平台上，部署多台套激光测距传感器，由于安装支架、激光测距传感器安装位置和姿态存在误差，使机器人获取数据产生大偏差，甚至造成感知错误现象，所以激光测距雷达外部参数至关重要，通过参数标定与校准，有效提升激光雷达传感器数据一致性，促进机器人环境建模、定位和导航技术发展。但传统激光标定方法均存在准备时间长和标定误差大等缺点。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的目的在于提出一种用于移动机器人的激光雷达传感器六维外参标定方法，该方法前期准备时间短，且标定误差小。

为达到上述目的，本发明实施例提出了用于移动机器人的激光雷达传感器六维外参标定方法，包括以下步骤：步骤S1，在移动机器人设置第一激光雷达、第二激光雷达、中心位置安装二维码定位模块；步骤S2，利用两个激光雷达扫描当前环境中固定的两个竖直平面，生成固定交线，操作在所述二维码定位模块附近的第二激光雷达扫描所述两个竖直平面，记录每一帧的移动机器人的基座相对地图坐标系的位姿以及激光雷达扫描得到的点云/>步骤S3，采用基于随机采样一致方法的直线提取方法处理所述位姿/>和点云/>求解每一帧交线的交点相对于所述第二激光雷达的坐标/>步骤S4，根据所述位姿/>所述点云/>和所述每一帧交线的交点相对于所述第二激光雷达的坐标/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik和理论距离的偏差函数，根据所述偏差函数构建待优化问题模型；步骤S5，采用高斯-牛顿法求解所述待优化问题模型，得到最优相对位姿，完成对激光雷达六维外参标定。

本发明实施例的用于移动机器人的激光雷达传感器六维外参标定方法，通过室内移动机器人在家居、展览馆和车站等场景广泛应用，机器人定位和建图是基础技术之一，为此机器人配置多台激光传感器，解决感知盲区、避障或建图等需求，激光传感器安装位置、姿态需要参数标定，使激光传感器数据统一，实现机器人检测数据高一致性，同时针对激光雷达的场景采用二维码模块与环境平面检测结合方式，实现激光六维外参数标定，该方法具有较强适性，水平、倾斜安装在机器人的激光雷达均适用。

另外，根据本发明上述实施例的用于移动机器人的激光雷达传感器六维外参标定方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述第一激光雷达和所述第二激光雷达的安装姿态为水平、倾斜、俯仰中任意组合方式。

进一步地，在本发明的一个实施例中，所述步骤S4进一步包括：步骤S401，定义激光雷达坐标系相对机器人坐标系的齐次变换矩阵，即六维外参为：

式中，R_lr为三自由度，[x_lr，y_lr，z_lr]三自由度；

步骤S402，定义每个交点在世界坐标系中的坐标为定义所述两个竖直平面的向量为/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik；

步骤S403，求解所述距离d_ik与理论距离的偏差函数，并根据所述偏差函数构建所述待优化问题模型。

进一步地，在本发明的一个实施例中，所述实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离d_ik为：

其中，为所述两个竖直平面的向量，/>为每个交点在世界坐标系中的坐标。

进一步地，在本发明的一个实施例中，所述偏差函数error为：

其中，d_ik为所述实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离，/>为所述两个竖直平面的向量，/>为所述位姿，T_lr为六维外参，/>为所述第二激光雷达的坐标。

进一步地，在本发明的一个实施例中，所述待优化问题模型为：

其中，为最优相对位姿，/>为所述两个竖直平面的向量，error为所述偏差函数。

进一步地，在本发明的一个实施例中，所述步骤S5进一步包括：

步骤S501，确定所述待优化问题模型中T_lr，的初值/>用于迭代，其中，取每一帧的移动机器人的基座相对地图坐标系的位姿/>取/>

步骤S502，计算errorT_lr，的雅可比矩阵J；

步骤S503，记由迭代公式重复计算逼近优化解，其中，所述迭代公式为：x^k+1＝x^k-(J^T(x^k)J(x^k))^-1J^T(x^k)x^k；

步骤S504，判断x^k+1-x^k是否小于预设停止阈值ε，若是，则得到所述最优相对位姿，否则重复所述步骤S503，继续迭代求解。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一个实施例的用于移动机器人的激光雷达传感器六维外参标定方法流程图；

图2是本发明一个实施例的实验平台机构结构示意图；

图3是本发明一个实施例的标定原理示意图，(a)为主视图，(b)为俯视图；

图4是本发明一个实施例的每一帧的雷达扫描点云示意图；

图5是本发明一个实施例的点云拟合直线效果图；

图6是本发明一个实施例的三维点云重建效果图；

图7是本发明一个实施例的标定前后三维重建效果对比图。

具体实施方式

下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参照附图描述根据本发明实施例提出的用于移动机器人的激光雷达传感器六维外参标定方法。

图1是本发明一个实施例的用于移动机器人的激光雷达传感器六维外参标定方法流程图。

如图1所示，该用于移动机器人的激光雷达传感器六维外参标定方法包括以下步骤：

在步骤S1中，在移动机器人设置第一激光雷达、第二激光雷达、中心位置安装二维码定位模块。

进一步地，在本发明的一个实施例中，第一激光雷达和第二激光雷达的安装姿态为水平、倾斜、俯仰中任意组合方式。

举例而言，如图2所示，将第一激光雷达1水平前置在移动机器人上，将第二激光雷达2倾斜后置在移动机器人上，二维码定位模块3安装在机器人中心位置。

在步骤S2中，利用两个激光雷达扫描当前环境中固定的两个竖直平面，生成固定交线，操作在二维码定位模块附近的第二激光雷达扫描两个竖直平面，记录每一帧的移动机器人的基座相对地图坐标系的位姿以及激光雷达扫描得到的点云/>

由于原理中所需的空间中固定直线理论上没有半径，因此很难找到实际物体替代，所以应当使用两个不平行的平面相交的直线来替代。如图3所示，本发明实施例首先利用纸箱或者木板在环境中作为固定的两个竖直平面生成固定直线，操纵机器人在二维码模块附近位置用激光雷达扫描纸箱，以提升位置精度，并记录每一帧的机器人基座相对地图坐标系位姿

式中，为第i帧机器人基座坐标系相对于世界坐标系中的旋转矩阵，为第i帧机器人基座坐标系相对于世界坐标系的原点坐标；以及在雷达坐标系扫描得到的点云/>如图3与图4所示：

在步骤S3中，采用基于随机采样一致方法的直线提取方法处理位姿和点云/>求解每一帧交线的交点相对于第二激光雷达的坐标/>

具体地，如图3所示，每一帧的点云理论上可以拟合成两条相交的直线，但是激光雷达存在噪点，为了抹除噪点，采用基于随机采样一致方法(RANSAC)的直线提取方法：不断从点云中随机取两点作为待定直线，若在离直线某个范围的点达到要求比例后，则认为这两点拟合得到的模型满足要求。连续使用两次RANSAC算法即可得到雷达扫描面与两个固定平面的交线方程，从而解算得到每一帧交线的交点/>相对于激光雷达的坐标/>如图5所示。

在步骤S4中，根据位姿点云/>和每一帧交线的交点相对于第二激光雷达的坐标/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik和理论距离的偏差函数，根据偏差函数构建待优化问题模型。

进一步地，在本发明的一个实施例中，步骤S4进一步包括：

步骤S401，定义激光雷达坐标系相对机器人坐标系的齐次变换矩阵，即六维外参。

步骤S402，定义每个交点在世界坐标系中的坐标为定义两个竖直平面的向量为/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik；

步骤S403，求解距离d_ik与理论距离的偏差函数，并根据偏差函数构建待优化问题模型。

也就是说，步骤S4按照交点到固定平面的平均距离和建立六维外参优化模型，即为对标定进行优化问题建模，具体地，待求变量为激光雷达坐标系相对机器人坐标系的齐次变换矩阵，即六维外参：

式中，R_lr为三自由度，[x_lr，y_lr，z_lr]三自由度；

每个交点在世界坐标系中的坐标为：

两个竖直平面可用方程A_kx+B_ky+1＝0(k＝0，1)表示，为后续推导方便，平面方程使用向量描述。理论上，交点的/>应满足几何约束：

即交点到两个竖直平面的距离均为0，因此实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离d_ik为：

其中，为两个竖直平面的向量，/>为每个交点在世界坐标系中的坐标。

理论上所有的d_ik都应该等于0，因此需要优化的偏差函数error为：

其中，d_ik为实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离，/>为两个竖直平面的向量，/>为位姿，T_lr为六维外参，/>为第二激光雷达的坐标。

则待优化问题模型可以表示为：

其中，为最优相对位姿，/>为两个竖直平面的向量，error为偏差函数。

优化完成后得到的最优即标定好的外参。

在步骤S5中，采用高斯-牛顿法求解待优化问题模型，得到最优相对位姿，完成对激光雷达六维外参标定。

进一步地，在本发明的一个实施例中，步骤S5进一步包括：

步骤S501，确定待优化问题模型中T_lr，的初值/>用于迭代，其中，/>取每一帧的移动机器人的基座相对地图坐标系的位姿/>取/>

步骤S502，计算errorT_lr，的雅可比矩阵J；

步骤S503，记由迭代公式重复计算逼近优化解；

步骤S504，判断x^k+1-x^k是否小于预设停止阈值ε，若是，则得到最优相对位姿，否则重复步骤S503，继续迭代求解。

也就是说，完成步骤S4中对优化问题的建模后，采用高斯-牛顿法对优化问题进行求解，求解得最优的相对位姿，完成对激光雷达外参的标定。

具体地，高斯-牛顿法求解过程如下：

确定errorT_lr，的初值/>用于迭代：/>取三维模型中得到的位姿，取/>

计算errorT_lr，的雅可比矩阵J，其中，T_lr其需要从李群T_lr∈SE(3)转化为李代数∈se(3):

T_lr＝exp(ξ^∧)

ξ^∧＝ln(T_lr)^∨

记由迭代公式重复计算逼近优化解：

x^k+1＝x^k-(J^T(x^k)J(x^k))^-1J^T(x^k)x^k

判断x^k+1-x^k是否小于停止阈值ε，如果是，则得到优化解，否则重复上述步骤S503，继续迭代求解。

因此，通过本发明实施例设计能够方便快捷地对激光雷达外参进行标定，为三维激光雷达外参标定提供了一种可行的原理方案。

经六维外参标定之后的单线激光雷达也可用于三维点云重建，如图6所示。

标定前后三维重建效果对比：标定前外参通过三维模型设计软件计算得到相比(如图7右侧所示)，图a的左图的上方框中的天花板横梁结构重建效果较右图清楚，左侧框中的墙面也较右图稠密一致地重建；图b的左图天花板点云高度一致，可以体现重建精度高；而右图中天花板的点云高度不一致，重建效果差，因此，标定外参后的可以更准确的，更平滑地重建墙面与天花板(如图7左侧所示)。

综上，本发明实施例提出的用于移动机器人的激光雷达传感器六维外参标定方法，基于二维码模块与平面检测相结合的方法，利用激光雷达扫描在对空间中某一固定的直线进行扫描时，扫描平面将与直线产生交点，通过不同机器人位姿下观测到的此交点位置，建立几何约束方程来求取激光雷达相对于机器人的位姿，使得让载有激光雷达的移动机器人在工作场景低成本方便快捷完成激光雷达标定，且具有较强适性，能够适用于水平、倾斜、俯仰等姿态安装的激光雷达传感器，可以在智慧家居、社区、车站等工作场景完成方便快捷的标定。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (6)

1.一种用于移动机器人的激光雷达传感器六维外参标定方法，包括以下步骤：

步骤S1，在移动机器人设置第一激光雷达、第二激光雷达、中心位置安装二维码定位模块；

步骤S2，利用两个激光雷达扫描当前环境中固定的两个竖直平面，生成固定交线，操作在所述二维码定位模块附近的第二激光雷达扫描所述两个竖直平面，记录每一帧的移动机器人的基座相对地图坐标系的位姿以及激光雷达扫描得到的点云/>

步骤S3，采用基于随机采样一致方法的直线提取方法处理所述位姿和点云/>求解每一帧交线的交点相对于所述第二激光雷达的坐标/>

步骤S4，根据所述位姿所述点云/>和所述每一帧交线的交点相对于所述第二激光雷达的坐标/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik和理论距离的偏差函数，根据所述偏差函数构建待优化问题模型；

步骤S5，采用高斯-牛顿法求解所述待优化问题模型，得到最优相对位姿，完成对激光雷达六维外参标定；

其特征在于，所述步骤S4进一步包括：

步骤S401，定义激光雷达坐标系相对机器人坐标系的齐次变换矩阵，即六维外参为：

式中，R_lr为三自由度，[x_lr,y_lr,z_lr]三自由度；

步骤S402，定义每个交点在世界坐标系中的坐标为定义所述两个竖直平面的向量为/>求解实际每个交点在世界坐标系中的坐标/>与两个竖直平面/>的距离d_ik；

步骤S403，求解所述距离d_ik与理论距离的偏差函数，并根据所述偏差函数构建所述待优化问题模型。

2.根据权利要求1所述的用于移动机器人的激光雷达传感器六维外参标定方法，其特征在于，所述第一激光雷达和所述第二激光雷达的安装姿态为水平、倾斜、俯仰中任意组合方式。

3.根据权利要求1所述的用于移动机器人的激光雷达传感器六维外参标定方法，其特征在于，所述实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离d_ik为：

其中，为所述两个竖直平面的向量，/>为每个交点在世界坐标系中的坐标。

4.根据权利要求1所述的用于移动机器人的激光雷达传感器六维外参标定方法，其特征在于，所述偏差函数error为：

其中，d_ik为所述实际每个交点在世界坐标系中的坐标与两个竖直平面/>的距离，/>为所述两个竖直平面的向量，/>为所述位姿，T_lr为六维外参，/>为所述第二激光雷达的坐标。

5.根据权利要求1所述的用于移动机器人的激光雷达传感器六维外参标定方法，其特征在于，所述待优化问题模型为：

其中，为最优相对位姿，/>为所述两个竖直平面的向量，error为所述偏差函数。

6.根据权利要求1所述的用于移动机器人的激光雷达传感器六维外参标定方法，其特征在于，所述步骤S5进一步包括：

步骤S501，确定所述待优化问题模型中的初值/>用于迭代，其中，/>取每一帧的移动机器人的基座相对地图坐标系的位姿/>取/>

步骤S502，计算的雅可比矩阵J；

步骤S503，记由迭代公式重复计算逼近优化解，其中，所述迭代公式为：x^{k +1}＝x^k-(J^T(x^k)J(x^k))^-1J^T(x^k)x^k；

步骤S504，判断x^k+1-x^k是否小于预设停止阈值ε，若是，则得到所述最优相对位姿，否则重复所述步骤S503，继续迭代求解。