CN116576868A - 一种多传感器融合精确定位及自主导航方法 - Google Patents

Abstract

本发明公开了一种多传感器融合精确定位及自主导航方法，包括步骤S1：通过多传感器获取点云数据；S2：对多传感器的融合处理并得到全局地图信息；S3：基于构建的三维点云地图进一步进行自主导航系统的定位；S4：进行局部路径规划。以融合轮式里程计与IMU改进Cartographer地图构建算法的方式，能在变电站复杂环境内构建高精度的环境地图。此外，还提升局部路径规划的效果，在不同的窗口模式下根据当前障碍物总密度值与临界条件进行比较，满足条件则在障碍物比较密集的环境采用范围较小的窗口搜索路径，在障碍物稀疏的环境下采用范围较大的窗口搜索路径，使巡检机器人可以快速搜索可行进的方向，解决了背景技术中提出的问题。

Description

一种多传感器融合精确定位及自主导航方法

技术领域

本发明涉及机器人定位与导航技术领域，尤其涉及一种多传感器融合精确定位及自主导航方法。

背景技术

机器人的自主活动在很多领域内如在变电站巡检等都有着广泛应用，针对环境的复杂化和定位及路线规划精度的高准确性，机器人自主活动中单一传感器已经无法获取精确的环境地图信息，针对变电站等环境非结构的特点，传统算法搜索速度慢、路径质量低、获得路径并非最优，智能算法训练成本高、输出不稳定，动态适应性差，但是传统算法相比较而言稳定、成熟。除此之外，当机器人在自主活动中遇到未知障碍物时，目前多为依靠全局路线规划来躲避障碍物，但是面对未知且复杂的障碍物时，单纯依靠全局路线规划会造成无法躲避障碍物。

发明内容

本发明所述的一种多传感器融合精确定位及自主导航方法，解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种多传感器融合精确定位及自主导航方法，包括以下步骤：

S1：通过多传感器获取点云数据，所述多传感器数据包括轮式里程计数据与IMU数据；

S2：对多传感器的融合处理并通过全局SLAM(Simultaneous localization andmapping，即时定位与地图构建)中局部SLAM相结合的方式得到全局地图信息；

S3：基于构建的三维点云地图进一步进行自主导航系统的定位；

S4：基于双窗口VFH(VectorField Histogram)算法进行局部路径规划。

优选的，所述步骤S2具体为：

S201：将轮式里程计数据与IMU数据进行融合处理，通过扩展卡尔曼滤波器，校准里程计打滑带来的误差；

S202：将激光雷达数据经过体素滤波和自适应体素滤波进行点云数据处理；

S203：Cartographer在局部SLAM中通过非线性优化(ceres solver)的方法将融合后的轮式里程计数据和激光数据与子地图进行匹配，生成较好的子地图；

S204：在全局SLAM中将局部SLAM建立的子地图累积误差进行误差消除，将得到的每个子地图以相匹配的位姿进行融合，得到全局地图信息。

优选的，所述步骤S3具体为：

S301：接收里程计发来的里程计位置，并在上一帧位置的基础上进行偏移，获得预测位置Predict_pose；

S302：接收新的点云后，进行预测获得NDT_pose；

S303：融合Predict_pose和NDT_pose，获得当前位置。

优选的，所述步骤S4具体为：

S401：计算当前窗口障碍物密度；

S402：根据条件自动调整窗口范围；

S403：计算扇区个数以及各扇区内障碍物密度值；

S404：通过阈值确定可选择前进方向；

S405：最后通过代价函数确定进行方向；

S406：判断是否抵达目标点，若是则结束进程，若否则继续执行步骤S201-S406。

优选的，所述步骤S401具体如下：

S411：记录初始模式一(W×W)的窗口，W与传感器的扫描距离相关；

S412：:将窗口内的每一个栅格作为障碍物向量，其向量的大小m_i,j和方向为β_i,j，其中a和b的取值满足/>d_max为最远的栅格到巡检机器人中心点的距离，其/>

S413：计算障碍物密度，计算公式为：其中，m_i,j为障碍物向量的大小。

优选的，所述步骤S402具体为：

S421：设置两种不同大小的窗口模式，模式一范围为(W×W)，W最大为传感器测量的最远距离，模式二范围为(Q×Q)，窗口模式二的范围的选择与当前总障碍物密度相关；

S423：对2种窗口模式设置不同的跳转条件，当条件符合则切换到另一个窗口模式。

优选的，所述步骤S423的跳转条件如下：

在窗口模式一下：ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二(Q×Q)，其中ω为经验值；

在窗口模式二下：q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一(W×W)，q的计算公式如下：

其中μ为跳转系数，跳转系数由工作人员根据当前环境设置，Q/W为模式二窗口范围和模式一窗口范围之比，ω为经验值。

本发明的有益效果为：

本发明通过步骤S1：通过多传感器获取点云数据，所述多传感器数据包括轮式里程计数据与IMU数据；S2：对多传感器的融合处理并通过全局SLAM中局部SLAM相结合的方式得到全局地图信息；S3：基于构建的三维点云地图进一步进行自主导航系统的定位；S4：基于双窗口VFH(VectorField Histogram)算法进行局部路径规划。以融合轮式里程计与IMU改进Cartographer地图构建算法的方式，能在变电站复杂环境内构建高精度的环境地图，而且构建的高精度三维点云地图被进一步应用于巡检机器人自主导航系统高精度定位。此外，还提升局部路径规划的效果，在不同的窗口模式下根据当前障碍物总密度值与临界条件进行比较，满足条件则在障碍物比较密集的环境采用范围较小的窗口搜索路径，在障碍物稀疏的环境下采用范围较大的窗口搜索路径，使巡检机器人可以快速搜索可行进的方向。

附图说明

图1为本发明全局地图信息的流程图；

图2为本发明进行自主导航系统定位的流程图；

图3为本发明局部路径规划的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

本实施例提供的一种多传感器融合精确定位及自主导航方法，共包含两大板块：基于采集数据的建立高精度地图的定位和自主导航路径规划，这两个板块均需要点云数据为依托。包括以下步骤：

S1：通过多传感器获取点云数据，所述多传感器数据包括轮式里程计数据与IMU数据。

S2：对多传感器的融合处理并通过全局SLAM中局部SLAM相结合的方式得到全局地图信息。这里大致为首先将轮式里程计数据与IMU数据进行融合处理，通过扩展卡尔曼滤波器，校准里程计打滑带来的误差；将激光雷达数据经过体素滤波和自适应体素滤波进行点云数据处理；Cartographer在局部SLAM中通过非线性优化(ceres solver)的方法将融合后的轮式里程计数据和激光数据与子地图进行匹配，生成较好的子地图；在全局SLAM中将局部SLAM建立的子地图累积误差进行误差消除，将得到的每个子地图以相匹配的位姿进行融合，得到全局地图信息。

如图1所示，Cartographer地图构建算法包括两部分Local和Global。其中Local是建立子地图，该部分通过环境点云构建子地图。Global是回环检测，该部分为子地图的匹配和回环检测。Local和Global都是对点云姿态ξ＝(ξ_x,ξ_y,ξ_θ)的约束。激光雷达每扫描一次都能够获得一帧点云，将获得的每帧点云记为c＝{c_k}_k＝1,...,K,c_k∈R²，定义激光点云坐标系到子地图坐标系的位姿变换为T_ξ，则每帧点云在子地图中的位姿变换关系为

子地图采用概率网格的形式表示，计算可以得到每个点云在子地图中概率值，根据新插入的点云在子地图中的位姿更新概率值。

假设t时刻麦克纳姆轮巡检机器人的位姿为(x_t,y_t,θ_t)，使用轮式里程计的估计值作为预测数据，使用IMU的位姿数据作为观测值进行更新。假设轮式里程计提供的控制量为V₀＝(v_x,v_y,w_z)，则根据轮式里程计的运动学模型可以得到t+1时刻的估计值为：

则协方差矩阵为：

其中G_t为对应的雅克比矩阵。根据IMU的(x,y,yaw)得到t+1时刻的观测模型为：

由得到的状态模型和观测模型得到位姿估计的最优值，然后通过不断进行位姿迭代和状态更新，将更新后的巡检机器人状态和系统协方差矩阵作为融合后的信息，为巡检机器人提供精确的位姿信息。

在将获得的laser-scan插入到子地图之前，通过Ceres-solve计算点云在子地图中的位姿，使得该位姿在子地图中获得的概率最大化。将计算位姿之间的关系转化为一个求解非线性最小二乘的问题，得到点云在子地图中概率值如下：

其中，M_smooth采用三次插值函数。

针对小地图累计误差很小，因此在处理较大的空间时，通过创建许多子地图进行处理，然后再回环检测中进行误差的消除。回环检测的过程中，首先检测回环，使用了帧匹配算法(CSM)的回环检测方式，将当前获得的点云插入到子地图中，通过下式求得所有位姿的最高分，当位姿匹配分数高于设定的阈值则认为已经检测到了回环。

在检测到回环之后进行位姿之间的优化，将位姿之间的差值处理为最小二乘法问题，通过Ceres-solve对最小二乘法问题进行求解。

S3：基于构建的三维点云地图进一步进行自主导航系统的定位；该步骤主要为构建的高精度三维点云地图被进一步应用于巡检机器人自主导航系统高精度定位。接收里程计发来的里程计位置，并在上一帧位置的基础上进行偏移，获得预测位置Predict_pose；接收新的点云后，进行预测获得NDT_pose；按照一定的规则融合Predict_pose和NDT_pose，可获得当前位置。

S4：基于双窗口VFH(VectorField Histogram)算法进行局部路径规划。具体如下：S401：计算当前窗口障碍物密度；

S402：根据条件自动调整窗口范围；

S403：计算扇区个数以及各扇区内障碍物密度值；

S404：通过阈值确定可选择前进方向；

S405：最后通过代价函数确定进行方向；

S406：判断是否抵达目标点，若是则结束进程，若否则继续执行步骤S201-S406。

如图3所示，初始为模式一为(W×W)的窗口，W与传感器的扫描距离相关。将窗口内的每一个栅格作为障碍物向量，其向量的大小m_i,j和方向为β_i,j

式中针对传统算法距离较远的障碍物也能产生较大的障碍物密度问题，使用缩小距离较远的障碍物对密度的影响，a和b的取值满足/>d_max为最远的栅格到巡检机器人中心点的距离，其/>

式中：

(x_vcp,y_vcp)——巡检机器人的中心点

(x_i,y_i)——障碍物栅格位置。

窗口内障碍物总密度值p_all的计算公式如下：

然后是根据障碍物密度选择窗口模式。

巡检机器人在运动过程中按照给定的步长行进，在路径规划的过程中，地图采用栅格地图，设置2种不同大小的窗口模式，构建窗口数学模型：

设巡检机器人从一个栅格到另一个栅格距离为d_r，巡检机器人在栅格地图中障碍物密度最大为巡检机器人周围8个栅格均有障碍物，其最大障碍物总密度为：

模式一范围为(W×W)，W最大为传感器测量的最远距离，模式二范围为(Q×Q)，窗口模式二的范围的选择与当前总障碍物密度相关，其计算为：

式中：

p₁——模式一下的障碍物总密度值，该值表明模式一下的窗口总障碍物密度越大，表示巡检机器人扫描的障碍物越多，窗口模式二的范围就越小，从而使巡检机器人快速适应当前环境，可以在不同的环境下寻找可行进的路径。

对2种窗口模式设置不同的跳转条件，其跳转条件如下所示：

在窗口模式一下：

ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二(Q×Q)，其中ω为经验值。

在窗口模式二下：

q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一(W×W)。

为了避免固定临界条件适用性较低和窗口范围较小的情况下q值过大，导致在巡检机器人一直处于大窗口模式的问题。保证巡检机器人可以在条件满足时切换不同的窗口扫描范围，从而快速搜索可行进的路径。q值计算公式如下：

式中：

μ——跳转系数，确保在巡检机器人切换小窗口模式时临界q值适应当前环境。

Q/W——模式二窗口范围和模式一窗口范围之比，表明窗口扫描范围的变化幅度，Q越小，巡检机器人所扫描的范围越小，其q的值也就越小。Q越大，巡检机器人所扫描的范围越大，q的值越也相应增大。

综上所述，本实施例通过步骤S1：通过多传感器获取点云数据，所述多传感器数据包括轮式里程计数据与IMU数据；S2：对多传感器的融合处理并通过全局SLAM中局部SLAM相结合的方式得到全局地图信息；S3：基于构建的三维点云地图进一步进行自主导航系统的定位；S4：基于双窗口VFH(VectorField Histogram)算法进行局部路径规划。以融合轮式里程计与IMU改进Cartographer地图构建算法的方式，能在变电站复杂环境内构建高精度的环境地图，而且构建的高精度三维点云地图被进一步应用于巡检机器人自主导航系统高精度定位。此外，还提升局部路径规划的效果，在不同的窗口模式下根据当前障碍物总密度值与临界条件进行比较，满足条件则在障碍物比较密集的环境采用范围较小的窗口搜索路径，在障碍物稀疏的环境下采用范围较大的窗口搜索路径，使巡检机器人可以快速搜索可行进的方向，解决了背景技术中提出的问题。

以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (9)

1.一种多传感器融合精确定位及自主导航方法，其特征在于，包括以下步骤：

S1：通过多传感器获取点云数据，所述多传感器数据包括轮式里程计数据与IMU数据；

S2：对多传感器的融合处理并通过全局SLAM中局部SLAM相结合的方式得到全局地图信息；

S3：基于构建的三维点云地图进一步进行自主导航系统的定位；

S4：基于双窗口VFH(VectorField Histogram)算法进行局部路径规划。

2.根据权利要求1所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S2具体为：

S201：将轮式里程计数据与IMU数据进行融合处理，通过扩展卡尔曼滤波器，校准里程计打滑带来的误差；

S202：将激光雷达数据经过体素滤波和自适应体素滤波进行点云数据处理；

S203：Cartographer在局部SLAM中通过非线性优化(ceres solver)的方法将融合后的轮式里程计数据和激光数据与子地图进行匹配，生成较好的子地图；

S204：在全局SLAM中将局部SLAM建立的子地图累积误差进行误差消除，将得到的每个子地图以相匹配的位姿进行融合，得到全局地图信息。

3.根据权利要求1所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S3具体为：

S301：接收里程计发来的里程计位置，并在上一帧位置的基础上进行偏移，获得预测位置Predict_pose；

S302：接收新的点云后，进行预测获得NDT_pose；

S303：融合Predict_pose和NDT_pose，获得当前位置。

4.根据权利要求1所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S4具体为：

S401：计算当前窗口障碍物密度；

S402：根据条件自动调整窗口范围；

S403：计算扇区个数以及各扇区内障碍物密度值；

S404：通过阈值确定可选择前进方向；

S405：最后通过代价函数确定进行方向；

S406：判断是否抵达目标点，若是则结束进程，若否则继续执行步骤S201-S406。

5.根据权利要求4所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S401具体如下：

S411：记录初始模式一(W×W)的窗口，W与传感器的扫描距离相关；

S412：:将窗口内的每一个栅格作为障碍物向量，其向量的大小m_i,j和方向为β_i,j，其中 a和b的取值满足/>d_max为最远的栅格到巡检机器人中心点的距离，其/>

S413：计算障碍物密度，计算公式为：其中，m_i,j为障碍物向量的大小。

6.根据权利要求4所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S402具体为：

S421：设置两种不同大小的窗口模式，模式一范围为(W×W)，W最大为传感器测量的最远距离，模式二范围为(Q×Q)，窗口模式二的范围的选择与当前总障碍物密度相关；

S423：对2种窗口模式设置不同的跳转条件，当条件符合则切换到另一个窗口模式。

7.根据权利要求6所述的一种多传感器融合精确定位及自主导航方法，其特征在于，所述步骤S423的跳转条件如下：

在窗口模式一下：ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二(Q×Q)，其中ω为经验值；

在窗口模式二下：q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一(W×W)，q的计算公式如下：

其中μ为跳转系数，跳转系数由工作人员根据当前环境设置，Q/W为模式二窗口范围和模式一窗口范围之比，ω为经验值。

8.一种电子设备，包括：处理器和存储装置；所述存储装置上存储有程序，所述程序被所述处理器执行，实现如权利要求1-7中任意一所述的方法。

9.一种计算机介质，其上存储有计算机程序，其中，所述程序被处理器执行时实现如权利要求1-7中任意一所述的方法。