CN115930948A

CN115930948A - 一种果园机器人融合定位方法

Info

Publication number: CN115930948A
Application number: CN202211647915.2A
Authority: CN
Inventors: 孙瑜; 陈祥龙; 李晨星; 孙艺铭; 李新彤; 姚文广; 黄江州
Original assignee: Nanjing University of Science and Technology
Current assignee: Nanjing University of Science and Technology
Priority date: 2023-02-22
Filing date: 2023-02-22
Publication date: 2023-04-07

Abstract

本发明涉公开了一种果园机器人融合定位方法，包括：分别利用视觉里程计和激光里程计来获取机器人的相对定位信息，并设计SINS/VO和SINS/LiDAR子滤波器，通过改进联邦滤波器的信息分配系数对联邦滤波算法进行优化，再引入GNSS信号对联邦滤波器的全局最优估计进行状态校正，使子滤波器的融合定位信号达到GNSS的定位精度，最后根据GNSS的几何精度因子GDOP以及用户等效测距误差URER设计了GNSS精度评估参数，根据评估参数设计了定位信号切换算法，实现融合定位信号和GNSS信号的无缝切换。该方法有效地降低了里程计的累计误差，保障果园机器人持续高精度的定位需求。

Description

一种果园机器人融合定位方法

技术领域

本发明涉及机器人定位技术领域，特别是一种果园机器人融合定位方法。

背景技术

由于果园移动机器人在定位过程中经常会受到果树树冠干扰以及自身误差累计等客观因素的影响，因此依靠单一的定位传感器很难实现果园机器人的高精度定位与导航任务。为了充分发挥不同定位传感器的优势，提高机器人实时定位精度和稳定性，研究人员提出了将多个定位传感器的输出进行数据融合的定位方法。这种多传感器融合的定位方法通过某种数据融合方式把不同传感器的互补定位信息进行合理分类和利用，将精度更高的融合定位信息作为机器人的位姿估计，以此获得高精度和高稳定性的定位结果。

目前在最常用的融合定位方式有GNSS加惯导系统的松紧融合定位，常用的辅助定位传感器有相机、激光雷达。但是仅仅依靠GNSS/INS或者GNSS/SINS的融合定位系统，会存在较大的定位误差，对于移动范围较小的标准化果园来说并不适用。GNSS结合里程计辅助定位的方式是果园机器人较为常用的定位方式，但是里程计的定位进度较差，无法在GNSS受遮挡时为机器人提供较为精准的定位信号。文献“一种机器人果园高精度实时定位方法”(公开号CN115112115A)中提出了基于双目相机和激光雷达的融合定位方式，可以集中两个传感器的定位优势，一定程度提高融合定位精度，但是其定位结果依旧与GNSS的定位精度相差较大，而且单纯依赖传感器的融合定位系统存在累计误差，工作时间较长会出现较大的误差，系统无法长时间提供高精度的定位信号。因此，需要在此基础上对融合定位系统进一步改进，降低传感器自身的累计误差，提高融合定位系统的精度，使其可以达到GNSS的定位精度要求。

发明内容

本发明的目的在于提供一种果园机器人融合定位方法，在机器人的卫星定位信号丢失或者受到干扰而失准时，仍然可以利用惯导系统与里程计实现融合定位，并有效降低传感器的累计误差，是融合定位系统的定位信号可以有效代替GNSS定位输出。

实现本发明目的的技术解决方案为：

一种果园机器人融合定位方法，通过优化联邦滤波器的信息分配系数对视觉里程计和激光里程计进行联邦滤波信息融合，通过引入GNSS信号对滤波器全局最优估计进行校正，提高定位精度，利用GNSS精度参数设计定位信号切换算法，实现GNSS信号和融合定位信号的无缝切换。所述的方法包括：

步骤1：视觉里程计定位，改进ORB特征提取匹配算法，通过对连续帧图像的特征点进行匹配和位姿转换估计，推算机器人的相对位姿变化。

步骤2：激光雷达里程计定位，优化GICP点云匹配算法对相邻点云数据帧进行位姿推算，得到机器人的位姿估计。

步骤3：融合定位，设计改进的联邦卡尔曼滤波器，对视觉里程计和激光雷达里程计的局部位姿结果进行信息融合，得到全局最优位姿估计，即融合定位信号。

步骤4：融合定位信号校准，在GNSS RTK定位状态正常时，将GNSS信号与联邦滤波器的全局最优估计值进行比较，根据比较结果调节信号校正算法参数，对全局最优位姿估计进行校正。

步骤5：GNSS的定位信号和融合定位信号精度评估。

步骤6：定位信号切换，结合GNSS精度参数和融合定位信号精度参数设计定位信号切换算法，当GNSS信号精度评估超过阈值，系统不再使用GNSS的定位结果，而是切换到已经校准的高精度融合定位信号。

与现有技术相比，本发明的有益效果为：

本发明通过双目视觉相机和激光雷达传感器来弥补GNSS定位信号受到干扰时的定位精度，改进了联邦滤波融合算法，对双目视觉相机和激光雷达获取的机器人定位信息进行融合，再利用GNSS RTK定位状态未锁定时的高精度定位信号对该融合定位信号进行校准，使传感器融合定位信号能够达到GNSS的定位精度；本发明设计了GNSS信号和融合定位信号的精度评估参数，当系统检测到GNSS信号受干扰时，将会用校准后的高精度融合定位信息来代替GNSS定位信号，机器人在果园等野外复杂环境下，卫星定位信号频繁受干扰时，该方法可以有效解决定位精度下降问题，而且该方法复杂度低，计算量小，可以快速频繁地实现信号校准和切换，从而为机器人提供高精度实时的定位信息。

附图说明

图1是本发明一种果园机器人融合定位方法的整体流程示意图。

图2是本发明一种果园机器人融合定位方法的双目视觉定位算法流程示意图。

图3是本发明一种果园机器人融合定位方法的激光雷达定位算法流程图。

图4是本发明一种果园机器人融合定位方法的改进后的联邦滤波器结构图。

图5是本发明一种果园机器人融合定位方法的实验轨迹对比图，图5(a)为改进联邦滤波器前后的实验轨迹与GNSS参考轨迹对比图，图5(b)为三维位置对比图。

具体实施方式

本发明通过视觉里程计和激光里程计来提高GNSS定位信号在受到干扰时机器人定位信号的精度，该方法可以实现机器人在果园等复杂环境下拥有高精度实时的定位信号。

机器人在果园树行中行走时，GNSS定位信号会被间隔的果树频繁的遮挡，本方法使用双目视觉相机拍摄机器人周围环境图像，通过对图像的ORB特征提取和匹配，估算机器人位姿变化，同时使用激光雷达扫描周围的环境的三维点云数据，通过对点云数据的配准推算机器人的位子变化，改进联邦滤波器对两个传感器的定位信号进行融合，并利用GNSS正常工作时的定位信号对融合信号进行校准，使精度达到GNSS定位信号的精度，最后设计了定位信号切换算法，在卫星信号再被遮挡时，系统会切换到融合定位信号，以保证机器人的高精度实时定位。

结合图1所示，一种果园机器人融合定位方法，包括以下步骤：

步骤1：双目视觉定位，利用双目视觉相机扫描周围环境，推算出机器人的定位信息。结合图2所示，具体步骤如下：

步骤1.1：利用非极大值抑制算法对相机采集的图像帧数据进行FAST角点检测，特征点引入了方向信息后，需要改进基本的BRIEF描述子，使描述子具有旋转不变特性，进一步提高ORB特征点在不同图像间的鲁棒性，这里可以利用灰度质心算法实现。

步骤1.2：利用余弦相似度算法进行ORB特征点匹配。假设两个匹配点的描述向量为a和b则其余弦值为:

其中，θ为两个匹配点描述向量的夹角。如果余弦值越大，表示两个描述向量的夹角越小，说明这两个匹配点越有可能匹配成功，算法将评估这两个匹配点是正确的匹配点。

步骤1.3：利用图像定向対极几何约束改进RANSAC算法，去除ORB特征点错误匹配点对。

在定向几何图形中，假设相机前后各有一个像素点，分别用向量x和-x表示，则场景点X与其对应的像素点x之间的定向关系为

x～pX (2)

其中p为关系系数。

对于相邻两帧的图像，存在一个秩为2的基础矩阵F_3×3，使得匹配的像素点对

满足对极约束：

x′^TFx＝0 (3)

定向极限约束为

e′×x′＝Fx (4)由于任何3×4满秩矩阵P和P′对四维向量X都满足：

e′×(P′X)＝F(PX) (5)

其中F和e′满足：

其中P⁺为P的伪逆矩阵，C由以下公式唯一确定：

上述的两图像间的基础矩阵F可以用标准的八点法计算，根据QR分解得到满足式(6)对极约束的七个特征点对应的3×3基础矩阵，然后利用式(7)的定向约束验证基础矩阵。

步骤1.4：三角测量(Triangulation)是一种利用多相机或多时刻观察同一空间点，并根据空间中该点的投影点，求解该点在空间中的三维位置的方法。通过三角测量的方式可以得到空间中点的深度信息，从而获得空间点的三维坐标，结合空间点在成像平面上的投影，便可以通过求解EPnP问题(3D-2D方法)来估计相机的位姿。

EPnP算法用一组虚拟控制点的加权和来表示某特征点在世界坐标系下的三维坐标，要求这些控制点的数量为4且加权和为1，通过求解这4个不共面的控制点在相机坐标系下的坐标来估计相机位姿。

步骤1.5：重复步骤1.1到步骤1.4，不断利用相邻帧图像推算可以得到某时间段内机器人的三维坐标。

步骤2：激光雷达定位，利用激光雷达扫描周围环境，确定机器人的定位信息。结合图3所示，具体步骤如下：

步骤2.1：利用3D激光雷达传感器扫描得到机器人周围的点云数据。

步骤2.2：利用盒子滤波和体素滤波对第k时刻和第k-1的点云P_k和P_k-1进行预处理，去除点云数据中的离群点和噪声点。

步骤2.3：对相邻点云帧进行点云配准，利用GICP匹配算法计算剔除错误点后的两帧点云的帧间(S2S)相对位姿变换。得到帧间相对位姿变换后，将其作为点云与局部子地图之间匹配的初始化点，将当前帧点云P_k与局部子映射S_k进行匹配。

步骤2.4：利用瞬时点云扫描中的“空间度”调整下一个关键帧选取的平移阈值，将符合阈值的关键帧组成局部子映射集。

步骤2.5：基于GICP算法进行点云帧间匹配。在GICP中采用欧几里得距离来表示对应点之间的关系，并用Kd-Tree搜索邻近点。假设现有源点云

和目标点云

其最优变化模型为

由旋转矩阵R和平移向量t组成，并且已经剔除了不满足距离阈值的匹配点。

步骤2.6：点云数据与局部子映射之间匹配(S2M)，进行全局位姿优化。

步骤2.7：重复步骤2.1到步骤2.6，推算机器人实时相对位姿，最终输出果园机器人k时刻在激光雷达里程计参考坐标系O下的位姿估计。

步骤3：传感器定位信息融合，利用信息融合算法将各个定位模块的定位信号进行信息融合，得到较为精准的定位结果。结合图4所示，具体步骤如下：

步骤3.1：利用捷联惯导系统SINS融合激光雷达定位结果，构建SINS/LiDAR子滤波器。采用姿态、速度和位置误差以及陀螺和加速度计误差作为SINS的状态向量，具体为

将式(8)展开后为

其中，φ_E、φ_N、φ_U为n系(导航坐标系)东、北、天方向失准角；δV_E、δV_N、δV_U为东、北、天方向速度误差；δL、δλ、δh为纬度、经度、高度误差；ε_x、ε_y、ε_z为b系(载体坐标系)x、y、z轴向的陀螺漂移；

为b系(载体坐标系)x、y、z轴向的加速度计偏置。

则SINS/LiDAR子滤波器的量测方程为：

其中

分别为子滤波器的位置和速度的量测噪声，Z₁为子滤波器的量测值。

步骤3.2：利用捷联惯导系统SINS融合双目相机定位结果，构建SINS/OV子滤波器。量测方程为：

步骤3.3：改进联邦滤波器信息分配系数，构建改进的联邦滤波器，对步骤3.2获得的双目视觉相机定位信息和步骤3.1得到的激光雷达定位信息进行融合。基于观测性能的信息分配因子A_i为：

其中，G_i为k时刻子系统的观测性能参数，tr(G_i)为观测性能参数矩阵的迹。

首先对N个子系统的观测矩阵进行奇异值分解，计算每个子系统状态矩阵的n个状态分量的信息分配系数，计算公式为：

其中，b_ij为第i个子系统的第j个状态分量的信息分配系数，σ_ij为第i个子系统的第j个状态分量对应的观测矩阵的奇异值。

可观测性矩阵二次分配因子B_i为：

B_i＝diag{b_i1,b_i2,...,b_in} (14)

优化的信息因子为C_i：

C_i＝A_i×B_i(15)

步骤4：融合定位信号校准，在GNSSRTK定位状态正常时，将GNSS的高精度定位信号与融合算法得到的各个传感器的融合定位信号对比，根据比较结果调节信号校正算法参数等。具体步骤如下：

步骤4.1：首先确定GNSSRTK定位为正常定位状态，确保GNSS定位结果的精度。

步骤4.2：将高精度的GNSS定位结果与传感器的融合定位结果相比较，根据比较结果，调节融合定位算法的信息分配数据以及融合定位信号修正因子等校正参数。

利用GNSS对全局最优状态估计进行校正的方法如下：

其中，

为第k时刻主滤波器(联邦滤波器)校正后的对外输出以及对子滤波器的信息分配输出；

为第k时刻主滤波器的最优估计值；c为第k时刻GNSS对主滤波器的最优估计值

的修正值；α为修正因子，默认为1表示开启GNSS校正，当融合定位信号达到预期精度时，α置零，表示关闭GNSS校正；Δx为GNSS状态量与全局最优估计值的相对误差；γ为相对误差阈值，经验取值为10％～25％。

步骤4.3：重复步骤4.1和步骤4.2，不断调整GNSS校正算法的参数，使传感器融合定位结果不断趋近GNSS的定位结果，从而提高融合定位结果的精度和可靠性。

步骤5：GNSS的定位信号和融合定位信号精度评估。具体步骤如下：

步骤5.1：集合GNSS系统中的几何精度因子GDOP和用户等效测距误差UERE，可得GNSS精度评估参数P_GNSS为：

P_GNSS＝GDOP×UERE (16)

步骤5.2：根据融合定位信号修正因子判断传感器融合定位信号精度：修正因子α为0时。表示校正的传感器融合信号可以代替GNSS信号；反之，修正因子α为1时，表示融合定位系统仍需要GNSS校正，无法代替GNSS信号。

步骤6：定位信号切换，设计定位信号切换算法，当GNSS信号精度评估超过阈值，系统不再使用GNSS的定位结果，而是切换到已经校准的高精度融合定位信号。具体步骤如下：

步骤6.1：将GNSS定位信号精度评估参数P_GNSS与设定的阈值比较，判断GNSS信号是否失锁。

步骤6.2：判断融合定位信号精度评估参数α是否为0，可以评估融合定位信号是否已经校正好；

步骤6.3：定位信号切换只涉及GNSS信号和融合定位信号，所以可以设计线性关系式

x_o＝k₁x_GNSS+k₁x_g，k₁、k₂∈{0,1} (17)其中，k₁、k₂为线性系数，且满足互斥关系k₁≠k₂；x_o为定位切换选择的最终定位输出；x_GNSS为GNSS定位输出；x_g为校正后的融合定位信号。选择不同的k₁、k₂值，就可以实现GNSS信号和融合定位信号的切换。

结合图5，以GNSS轨迹为参考轨迹，传统的联邦滤波算法得到的组合定位轨迹在实验前期路段的逐渐贴合参考轨迹，但是由于存在传感器累计误差，在实验后期路段，传统联邦滤波器的定位轨迹与GNSS参考轨迹出现了明显偏差。改进后的联邦滤波器由于优化了信息分配并加入了GNSS高精度位置信号的校正，有效地抑制了惯导和两个里程计的累计误差，使得全局最优估计值的轨迹基本符合GNSS参考轨迹。可见，改进的果园机器人组合定位方法在长时间高精度定位方面具有很大的优势。

需要指出的，传感器不仅仅限制于双目视觉和激光雷达传感器，还可以选择其他传感器作为融合定位的定位信号源。

出于说明的目的，上述的具体实施方式只是示例性的，是为了更好的使本领域技术人员能够理解本专利，不能理解为是对本专利包括范围的限制；凡是采用等同替换或等效变换的方式获得的技术方案均落在本发明的保护范围内。

Claims

1.一种果园机器人融合定位方法，其特征在于，包括：

步骤1：视觉里程计定位，采用ORB特征提取匹配算法，通过对连续帧图像的特征点进行匹配和位姿转换估计，推算机器人的相对位姿变化；

步骤2：激光雷达里程计定位，通过GICP点云匹配算法对相邻点云数据帧进行位姿推算，得到机器人的位姿估计；

步骤3：融合定位，通过联邦卡尔曼滤波器，对视觉里程计和激光雷达里程计的局部位姿结果进行信息融合，得到全局最优位姿估计，即融合定位信号；

步骤4：融合定位信号校准，在GNSS RTK定位状态正常时，将GNSS信号与联邦滤波器的全局最优估计值进行比较，根据比较结果调节信号校正算法参数，对全局最优位姿估计进行校正；

步骤5：GNSS的定位信号和融合定位信号精度评估；

步骤6：定位信号切换，结合GNSS精度和融合定位信号精度设计定位信号切换算法，当GNSS信号精度评估超过阈值，系统不再使用GNSS的定位结果，切换到已经校准的融合定位信号。

2.根据权利要求1所述的果园机器人融合定位方法，其特征在于，所述步骤1具体包括：

步骤1.1：利用非极大值抑制算法对相机采集的图像帧数据进行FAST角点检测，同时利用灰度质心算法使BRIEF二进制描述子具有旋转不变性；

步骤1.2：利用余弦相似度算法进行ORB特征点匹配；

步骤1.3：利用图像定向対极几何约束RANSAC算法，去除ORB特征点错误匹配点对；

步骤1.4：根据三角测量原理求得空间点的深度信息，再利用EPnP算法对机器人位姿变化进行估计；

步骤1.5：重复步骤1.1到步骤1.4，不断利用相邻帧图像推算得到某时间段内机器人的三维坐标。

3.根据权利要求2所述的果园机器人融合定位方法，其特征在于，所述步骤1.3中利用图像定向対极几何约束RANSAC算法，去除ORB特征点错误匹配点对具体包括：

在定向几何图形中，假设相机前后各有一个像素点，分别用向量x和-x表示，

则场景点X与其对应的像素点x之间的定向关系为：

x～pX

其中p为关系系数；

满足对极约束：

x′^TFx＝0

定向极限约束为：

e′×x′＝Fx

由于任何3×4满秩矩阵P和P′对四维向量X都满足：

e′×(P′X)＝F(PX)

其中F和e′满足：

其中P⁺为P的伪逆矩阵，C由以下公式唯一确定：

上述的两图像间的基础矩阵F用标准的八点法计算，根据QR分解得到满足对极约束的七个特征点对应的3×3基础矩阵。

4.根据权利要求1所述的果园机器人融合定位方法，其特征在于，所述步骤2具体包括：

步骤2.1：通过3D激光雷达传感器扫描得到机器人周围的点云数据；

步骤2.2：利用点云滤波算法对点云数据进行预处理，去除点云数据中的离群点和噪声点；

步骤2.3：利用GICP算法对相邻点云帧进行点云配准；

步骤2.4：利用瞬时点云扫描中的空间度调整下一个关键帧选取的平移阈值，将符合阈值的关键帧组成局部子映射集；

步骤2.5：基于GICP算法进行点云帧间匹配；

步骤2.6：点云数据与局部子映射之间匹配，进行全局位姿优化；

步骤2.7：重复步骤2.1到步骤2.6，推算机器人实时相对位姿。

5.根据权利要求4所述的果园机器人融合定位方法，其特征在于，所述步骤2.5的GICP算法中采用欧几里得距离来表示对应点之间的关系，并用Kd-Tree搜索邻近点。

6.根据权利要求1所述的果园机器人融合定位方法，其特征在于，所述步骤3具体包括：

步骤3.1：利用捷联惯导系统SINS融合激光雷达定位结果，构建SINS/LiDAR子滤波器；

步骤3.2：利用捷联惯导系统SINS融合双目相机定位结果，构建SINS/OV子滤波器；

步骤3.3：改进联邦滤波器信息分配系数，构建联邦滤波器，对SINS/LiDAR子滤波器和SINS/OV子滤波器的输出进行融合。

7.根据权利要求6所述的果园机器人融合定位方法，其特征在于，所述步骤3.1具体包括：

采用姿态、速度和位置误差以及陀螺和加速度计误差作为SINS的状态向量，具体为

其中，φ_E、φ_N、φ_U为导航坐标系东、北、天方向失准角；δV_E、δV_N、δV_U为东、北、天方向速度误差；δL、δλ、δh为纬度、经度、高度误差；ε_x、ε_y、ε_z为载体坐标系x、y、z轴向的陀螺漂移；

为载体坐标系x、y、z轴向的加速度计偏置；

则SINS/LiDAR子滤波器的量测方程为：

其中

分别为子滤波器的位置和速度的量测噪声，Z₁为子滤波器的量测值；

同理，确定步骤3.2中SINS/OV子滤波器的量测方程。

8.根据权利要求6所述的果园机器人融合定位方法，其特征在于，所述步骤3.3改进联邦滤波器信息分配系数具体包括：

基于观测性能的信息分配因子A_i为：

其中，G_i为k时刻子系统的观测性能参数，tr(G_i)为观测性能参数矩阵的迹；

对N个子系统的观测矩阵进行奇异值分解，计算每个子系统状态矩阵的n个状态分量的信息分配系数，计算公式为：

其中，b_ij为第i个子系统的第j个状态分量的信息分配系数，σ_ij为第i个子系统的第j个状态分量对应的观测矩阵的奇异值；

可观测性矩阵二次分配因子B_i为：

B_i＝diag{b_i1,b_i2,...,b_in}

优化的信息因子为C_i：

C_i＝A_i×B_i

9.根据权利要求1所述的果园机器人融合定位方法，其特征在于，所述根据比较结果调节信号校正算法参数，对全局最优位姿估计进行校正具体为：

其中，

为第k时刻联邦滤波器校正后的对外输出以及对子滤波器的信息分配输出；

为第k时刻联邦滤波器的最优估计值；c为第k时刻GNSS对联邦滤波器的最优估计值

10.根据权利要求9所述的果园机器人融合定位方法，其特征在于，所述GNSS的定位信号精度评估为：

P_GNSS＝GDOP×UERE

其中，GDOP为GNSS系统中的几何精度因子，UERE为等效测距误差；

所述融合定位信号精度评估为：根据融合定位信号修正因子判断传感器融合定位信号精度为：修正因子α为0时，表示校正的传感器融合信号可以代替GNSS信号；反之，修正因子α为1时，表示融合定位系统仍需要GNSS校正，无法代替GNSS信号。