CN116660923B - 一种融合视觉和激光雷达的无人农机机库定位方法和*** - Google Patents

Abstract

本发明公开了一种融合视觉和激光雷达的无人农机机库定位方法和***，属于无人农机机库定位领域，本发明采用融合指示牌，其具有稳定圆形特征，通过视觉定位方法为基于激光点云匹配定位提供初值和重定位，从而在机库内GNSS信号弱且环境信息发生较大变化时能够保持定位的稳定、可靠和高精度。本发明提高无人农机机库内定位的鲁棒性和精度。

Description

一种融合视觉和激光雷达的无人农机机库定位方法和***

技术领域

本发明属于无人农机机库定位领域，具体涉及一种融合视觉和激光雷达的无人农机机库定位方法和***。

背景技术

无人农机是不需人工操控，通过外部传感器和智能计算机***进行环境感知、规划决策和控制的农业装备车辆，具有更安全、便捷的优势，成为当前的研究热点。其中，精准定位是无人农机路径规划和控制的前提，是其具备自动驾驶能力的基础。全球卫星导航***（Global Navigation Satellite System , GNSS）可为载体提供位置、航向等信息，广泛应用于车辆定位中。激光雷达不易受光照干扰，可采集结构丰富的高精度高分辨率环境信息，将三维点云的关键特征与已知地图匹配进行定位。将卫星定位和点云匹配定位结合，一方面GNSS为激光雷达定位提供绝对位置初值，并在误差累积过大时修正位置；另一方面，激光雷达可以充分利用环境特征，并在GNSS位置丢失时保证结果的输出，提高定位的准确度和可靠性。

然而，在室内、隧道和地下等遮挡严重的密闭场所，接收到的GNSS信号弱，质量较差，无法满足精准和稳定的定位需求，无人农机在机库内的定位便面临这种问题。相机具有成本低廉、采集信息丰富、适用场景广等特点，在已知环境中可以不依赖于外界信号，通过提取图像特征估计位姿，弥补GNSS信号缺失的不足。

基于激光雷达的定位技术在机库等结构化环境中保持良好的定位精度，但当周围环境相似或发生某些变化时可能会产生误匹配或与地图匹配不上导致定位丢失，且需要绝对位置给位姿的转换提供基准。视觉传感器可辅助提供重定位信息，当前室内主要通过视觉检测靶标进行重定位，但需要同时检测到多个标志，对其在场所中的布设有着较高要求，且需要定期维护，增加了场地要求和人力成本。

发明内容

针对机库内无GNSS信号辅助定位的问题，本发明提出了一种融合视觉和激光雷达的无人农机机库定位方法和***，将激光雷达的点云匹配定位与特定机库场景可提供的稳定视觉特征进行辅助定位相结合，充分利用环境信息，提高无人农机机库内定位的鲁棒性和精度。

本发明实现其目的所采取的技术方案是：

一种融合视觉和激光雷达的无人农机机库定位方法，包括如下步骤：

步骤（1）车载相机采集含有指示牌的图像，目标检测算法输出所述指示牌的标签进而得到其世界坐标；通过图像处理算法获取视觉特征，通过位姿估计算法获取指示牌相对车载相机的转换关系；

步骤（2）激光雷达扫描区域点云，将实时的区域点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果；

步骤（3）将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，推算载体和激光雷达的位置；其中，相机坐标系是以相机的聚焦中心为原点，以光轴为z轴建立的三维直角坐标系；

步骤（4）在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点，更新当前的位置信息，修正激光定位的累计误差，实现重定位。

本发明还提供一种融合视觉和激光雷达的无人农机机库定位***，包括：

转换关系模块，利用车载相机采集含有指示牌的图像，利用目标检测算法输出所述指示牌的标签进而得到其世界坐标；通过图像处理算法获取视觉特征，通过位姿估计算法获取指示牌相对车载相机的转换关系；

匹配定位模块，通过激光雷达扫描区域点云，将实时的区域点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果；

位置计算模块，将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，推算载体和激光雷达的位置；

重定位模块，在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点，更新当前的位置信息，修正激光定位的累计误差，实现重定位。

本发明的有益效果在于：

本发明结合了机库的稳定圆形特征，在激光点云匹配定位的基础上，融合视觉定位方法为点云定位提供初值和重定位，从而在机库内GNSS信号弱且环境信息发生较大变化时能够保持定位的稳定、可靠和高精度。

附图说明

图1为本发明的一种融合视觉和激光雷达的无人农机机库定位方法流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明的一种融合视觉和激光雷达的无人农机机库定位方法具体包括如下步骤：

步骤（1）车载相机采集含有指示牌的图像，目标检测算法输出该指示牌的标签进而得到其世界坐标。通过图像处理算法获取视觉特征，位姿估计算法获取指示牌相对相机的转换关系；具体包括：

1）机库内竖直安装若干个固定半径为R的圆形的指示牌，每个指示牌上有不同含义的图案或数字。建立指示牌库，训练目标检测深度学***面位于指示牌表面，y轴竖直向上，根据右手坐标系确定x轴的方向。激光雷达坐标系原点为激光脉冲发射点，坐标轴定义方向由激光雷达厂家设定；载体坐标系与无人农机固连，以IMU中心为原点，x轴指向前进方向，z轴垂直向上；指示牌坐标系的原点位于圆心处，z轴垂直指向表面的外侧，x-y平面位于指示牌表面，y轴竖直向上，根据右手坐标系确定x轴的方向；

2）通过直接最小二乘拟合相机成像平面上的指示牌边缘。投影椭圆在像素平面上的一般表达式，方程的系数为待求参数。图像上的待拟合数据点为/>，其中/>为数据点的像素坐标；用最小化离散点和椭圆上点的代数平方和/>以求解参数。

解得的特征值/>和广义特征向量，其中正特征值对应的特征向量为拟合方程的最优解。

其中，，/>。

3）标定得到的车载相机内参矩阵为：，其中/>和/>为焦距，/>和/>为主点坐标值。

令，/>，得到图像坐标系下像素表示的一般方程。

在车载相机坐标系下椭圆和坐标系原点形成椭圆椎面的一般表达式为，其中，/>，/>，/>，，/>，/>。

建立表达式的二次型矩阵，其特征值为/>、/>、/>，即。规定/>、/>，三个特征值对应的特征向量分别为、/>和/>，则/>。

则圆形的指示牌的中心位置和表面法向量/>在车载相机坐标系的表示为：

，

通过一幅图片得到两组解，车载相机获取拟合边缘的近似像素坐标，求取平面上多组两两不平行直线的向量积，将向量积与两组法向量作比较，差值小者即为圆形的指示牌在车载相机中的正确表示结果。R为圆形的指示牌的半径。

步骤（2）激光雷达扫描区域点云，将实时点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果。基于正态分布变换的点云匹配方法为：

1）加载预先建立的点云地图，将其划分为固定大小的正方体网格，计算网格内点的概率密度函数/>，其中，均值向量/>，为该网格内所有点，协方差矩阵/>。

2）当前扫描点云集合为，点/>经过姿态变换参数/>的空间转换函数为/>，采用牛顿法优化最大化目标函数/>以求取最优的姿态变换参数/>，其中/>表示求每个点概率密度函数的乘积，/>为网格内点的概率密度函数。

步骤（3）将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，进而推算载体的位置。

步骤（4）在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点更新当前的位置信息，修正激光定位的累计误差，达到重定位的效果。

本发明还提供一种融合视觉和激光雷达的无人农机机库定位***，包括：

转换关系模块，利用车载相机采集含有指示牌的图像，利用目标检测算法输出所述指示牌的标签进而得到其世界坐标；通过图像处理算法获取视觉特征，通过位姿估计算法获取指示牌相对车载相机的转换关系；

匹配定位模块，通过激光雷达扫描区域点云，将实时点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果；

位置计算模块，将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，推算载体和激光雷达的位置；其中，相机坐标系是以相机的聚焦中心为原点，以光轴为z轴建立的三维直角坐标系；

重定位模块，在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点更新当前的位置信息，修正激光定位的累计误差，达到重定位的效果。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种融合视觉和激光雷达的无人农机机库定位方法，其特征在于，包括如下步骤：

步骤（1）车载相机采集含有指示牌的图像，目标检测算法输出所述指示牌的标签进而得到其世界坐标；通过图像处理算法获取视觉特征，通过位姿估计算法获取指示牌相对车载相机的转换关系；

步骤（2）激光雷达扫描区域点云，将实时的区域点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果；

步骤（3）将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，推算载体和激光雷达的位置；其中，相机坐标系是以相机的聚焦中心为原点，以光轴为z轴建立的三维直角坐标系；

步骤（4）在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点，更新当前的位置信息，修正激光定位的累计误差，实现重定位。

2.根据权利要求1所述的一种融合视觉和激光雷达的无人农机机库定位方法，其特征在于，所述步骤（1）包括：

1）机库内竖直安装若干个固定半径为R的圆形的指示牌，每个圆形的指示牌上有不同含义的图案或数字；建立指示牌库，训练目标检测深度学习模型，通过训练好的目标检测深度学习模型输出圆形的指示牌的标签，进而将其与提前测量的各圆形的指示牌的绝对坐标一一对应，当车载相机检测到某圆形的指示牌时，能确定唯一位置；

2）通过直接最小二乘拟合相机成像平面上的圆形的指示牌边缘；投影椭圆在像素平面上的一般表达式为，方程的系数为待求参数；图像上的待拟合数据点为/>，i=1,2，……，n，其中为待拟合数据点的像素坐标；通过最小化离散点和椭圆上点的代数平方和以求解参数；解得/>的特征值/>和广义特征向量，其中正特征值对应的特征向量为拟合方程的最优解；

其中，，/>；

3）标定得到的相机内参矩阵为：，其中/>和/>为焦距，/>和/>为主点坐标值；

令，/>，得到图像坐标系下像素表示的一般方程为；其中，X，Y为图像坐标系坐标轴，a,b,c,d,e，f为方程系数；

在相机坐标系下，椭圆和坐标系原点形成椭圆椎面的一般表达式为：

，

其中，，/>，/>，/>，/>，/>；

建立表达式的二次型矩阵为，其特征值为/>、/>、/>，即；

规定、/>，三个特征值/>、/>、/>对应的特征向量分别为/>、和/>，则/>；

则圆形的指示牌的中心位置和表面法向量/>在相机坐标系的表示为：

，

其中，R为圆形的指示牌的半径；

车载相机获取拟合的圆形的指示牌边缘的近似像素坐标，求取图像平面上多组两两不平行直线的向量积，将向量积与两组法向量作比较，差值小者即为圆形的指示牌在车载相机中的正确表示结果。

3.根据权利要求2所述的一种融合视觉和激光雷达的无人农机机库定位方法，其特征在于，所述步骤（2）中，激光点云匹配的方法基于正态分布变换，具体包括：

1）加载预先建立的点云地图，将其划分为固定大小的正方体网格，计算正方体网格内的点的概率密度函数/>，其中，均值向量/>，为该正方体网格内所有点，协方差矩阵/>；上标T表示转置；

2）当前扫描点云集合为，集合中的点/>经过姿态变换参数/>进行转换的空间转换函数为/>，采用牛顿法优化最大化目标函数/>以求取最优的姿态变换参数/>，其中/>表示求每个点概率密度函数的乘积，/>为网格内点的概率密度函数。

4.根据权利要求1-3之一所述的一种融合视觉和激光雷达的无人农机机库定位方法的***，其特征在于，包括：

转换关系模块，利用车载相机采集含有指示牌的图像，利用目标检测算法输出所述指示牌的标签进而得到其世界坐标；通过图像处理算法获取视觉特征，通过位姿估计算法获取指示牌相对车载相机的转换关系；

匹配定位模块，通过激光雷达扫描区域点云，将实时的区域点云与预先建立好的点云地图做匹配，进行点云数据的帧间位姿估计，输出激光点云匹配定位结果；

位置计算模块，将相机坐标系、激光雷达坐标系、载体坐标系和指示牌坐标系进行关联，推算载体和激光雷达的位置；

重定位模块，在车辆启动时，利用视觉提供的初始信息为点云定位提供初值；在行驶过程中，定期启动车载相机定位节点，更新当前的位置信息，修正激光定位的累计误差，实现重定位。