CN109807911B - 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法 - Google Patents

Abstract

本发明提供一种基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法。具体为：开机检查充电桩与机器人是否连接？若是，机器人初始位置为充电桩位置；若否，则通过GNSS、UWB测得机器人在测量范围内任意栅格出现概率分别为P_g(n)、P_u(n)；码盘、激光雷达在测量范围内任意栅格出现的概率分别为P_o(n)、P_L(n)；设定GNSS、UWB、码盘、激光雷达的定位权重，通过归一化得出机器人出现概率最高的位置坐标；所得位置坐标联合IMU输出的加速度、角速度和角加速度以及码盘输出的实时线速度和角速度，输入EKF滤波器；输出机器人实时位置坐标及实时角速度。本发明提供的联合定位方法能快速高精定位，实现室内室外定位无缝切换，且适用于大地图环境下的自主定位。

Description

基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境 联合定位方法

技术领域

涉本发明涉及导航技术领域，具体涉及一种基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法。

背景技术

现有机器人大部分使用激光雷达、UWB或者使用GNSS进行定位，无法有效完成室内室外自由切换时的定位稳定性问题。使用激光雷达或UWB进行定位时，由于激光雷达和UWB探测距离有限、工作范围较小，无法在室外大环境下使用；而使用GNSS定位时，由于信号遮挡和干扰问题，只能在室外空旷地区使用，无法适用室内、小区、街道等工作环境。室外激光雷达定位大部分采用三维激光雷达来实现定位，不仅需要事先建图，而且对环境适应性差，周围环境改变时会导致误差，需要重新建图，维护成本高，而且三维激光雷达成本高，运算量大，对处理器要求较高，每次开机都需要手动初始化，且无法实现大范围定位。

发明内容

本发明的目的在于克服现有机器人开机无法自动初始化、定位技术无法兼顾室内室外自由切换时的稳定性问题，提供一种定位精度高、稳定性好、实用性强的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法。

一种基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，所述巡逻机器人包括内置EKF滤波器的主控计算单元，以及分别与其连接且协同作用的GNSS模块、UWB系统、激光雷达、IMU惯性测量单元、码盘和充电感应模块，具体包括如下步骤：

S1：开机，检查所述充电桩与机器人是否连接？若是，机器人初始位置等于所述充电桩位置；若否，进入步骤S2；

S2：分别确定所述GNSS模块、所述UWB系统测得机器人在其定位范围内任意栅格出现的概率P_g(n)、P_u(n)；

S3：根据所得概率P_g(n)、P_u(n)确定机器人大致位置，再结合所述IMU惯性测量单元确定机器人初始方向，分别确定所述码盘、所述激光雷达在其定位范围内任意栅格出现的概率P_O(n)、P_L(n)；

S4：设定所述GNSS模块、所述UWB系统、所述码盘、所述激光雷达各自的定位权重，通过归一化得出机器人出现概率最高的位置坐标；

S5：步骤S4所得位置坐标联合所述IMU惯性测量单元输出的加速度、角速度和角加速度以及所述码盘输出的实时线速度和角速度，输入所述EKF滤波器；

S6：输出机器人实时位置坐标及实时角速度。

在本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法的一种较佳实施例中，步骤S2中，所述主控计算单元检查所述UWB系统的基站信号，再确定所述UWB系统测得机器人在其定位范围内任意栅格出现的概率P_u(n)，P_u(n)算法为：

其中，δ为所述UWB系统接收数据的频率，k为所述UWB系统的基站编号，

n_k＝2*π*d_k/r

r为地图分辨率，n_k代表编号为k的基站的定位范围内的栅格数，A、a为常数，dk代表编号为k的基站距离机器人的距离，

为所述UWB系统测距精度误差。

在本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法的一种较佳实施例中，步骤S2中确定所述GNSS模块在其定位范围内任意栅格出现的概率P_g(n)算法为：

P_g(n)＝Φ*exp[-ε²/2η²]+C_mod

其中，Φ、C_mod分别为定位误差ε的权重和定位模式mod的权重，Φ通过所述GNSS模块搜索到的卫星数目及定位模式确定，C_mod由所述GNSS模块的定位模式决定。

在本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法的一种较佳实施例中，步骤S3中所述激光雷达精确测距后，使用自适应蒙特卡洛定位算法，对比地图信息，获得机器人在其定位范围内任意栅格出现的概率P_L(n)。

在本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法的一种较佳实施例中，步骤S3中所述码盘根据运动过程中的积累值确定机器人在其定位范围内任意栅格出现的概率P_O(n)为：

其中，del_d、del_A为计算时间内机器人运动的距离和角度，Q、T分别为码盘线速度、角速度误差权重系数。

在本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法的一种较佳实施例中，步骤S4中，最终机器人处于地图任意栅格的概率P(n)值越大，表明机器人处于该栅格的概率越高，最终P(n)最高的栅格对应的位置即为当前机器位置坐标(x，y)，其计算公式为：

其中，μ、v、

ω分别为P_g、P_u、P_L、P_O的对应定位权重系数且

N_max＝S_地图/S_栅格；

通过所述UWB系统信号强度、基站数目、所述GNSS模块信号强度、卫星数目、所述激光雷达与地图的匹配程度共同综合确定，设置相对信号强度大的对应的权重大。

相较于现有技术，本发明提供的所述基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法创造性通过对不同定位方式进行归一化处理，有助于快速进行数据融合和提高定位精度。另提出了不同定位方式相应的修正误差公式，更进一步提高定位精度和稳定性，实现了各定位方式的无缝切换，实用性强，稳定性好、定位精度高。取得了一系列有益效果，主要表现在：

开机无需人为配置或手动初始化，自动寻找信号，确定机器人本体位置和方向；通过室内、室外，GNSS信号、UWB信号、激光匹配等过渡区域时，自动选择各自权重进行数据融合后输出机器人实时位置坐标及实时角速度，全程定位过程无缝衔接。创造性的融合GNSS、UWB、激光雷达、等可行的机器人定位方案，实现了各定位方式的无缝切换及数据融合，克服全局定位信息受环境影响大、以及惯性器件长时间漂移的缺陷，实用性强，稳定性好，进一步提高巡逻机器人定位定向精度，为精确导航提供基础信息。结合所述IMU惯性测量单元及所述码盘可提供机器人自身以及环境的良好瞬时运动估计，使定位精度进一步提高。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1为本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法工作流程图；

图2为UWB定位示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。

本发明提供的联合定位方法所适用的巡逻机器人包括主控计算单元，以及分别与其连接且协同作用的GNSS模块、UWB系统、激光雷达、IMU惯性测量单元、码盘和充电感应模块。具体实施过程中，所述主控计算单元可以是改良的RK3288主板或者CPU在i3以上的工控机或者PC，用于接收所述GNSS模块、所述UWB系统、所述激光雷达、所述IMU惯性测量单元、所述码盘和所述充电感应模块各自传输的信号，并根据信号情况进行相应的计算和判断后控制定位方式自动切换。所述主控计算单元内置EKF滤波器。

所述GNSS模块可以为单独的GPS信号接收模块或北斗信号接收模块。所述UWB系统包括三个以上基站，用于接收基站发送的信号并作出反馈。所述IMU惯性测量单元属于捷联式惯导，有两个加速度传感器与三个方向的角速率传感器(陀螺)组成。所述充电感应模块为机器人自动充电系统的一部分，可感应机器人是否连接至充电桩，且在充满电后自动脱落充电桩。

请参阅图1，是本发明提供的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法工作流程图。具体包括如下步骤：

步骤S1：开机，检查所述充电桩与机器人是否连接？若是，机器人初始位置等于所述充电桩位置；若否，进入步骤S2；

步骤S2：分别确定机器人在所述GNSS模块、所述UWB系统的定位范围内的任意栅格出现的概率P_g(n)、P_u(n)。

具体的，所述主控计算单元检查所述UWB系统的基站信号，若基站信号正常，则每收到一个基站信号，根据基站编号及其距机器人之间的距离，确定机器人大概范围。如图2所示。当接收到编号为1的基站信号时，基站距机器人的距离为记d₁，那么，以d₁为半径作圆，根据确定的地图分辨率r，可确定该基站的定位栅格数目n₁＝2*π*d₁/r，因此机器人出现在其任意栅格内概率为1/n₁，以此类推。再确定所述UWB系统测得机器人在其定位范围内任意栅格出现的概率P_u(n)算法为：

其中，δ为所述UWB系统接收数据的频率，k为所述UWB系统的基站编号，

n_k＝2*π*d_k/r

r为地图分辨率，将所述UWB系统的基站从1开始编号，即1、2、3.....k；n_k代表编号为k的基站的定位范围内的栅格数，A、a为常数，dk代表编号为k的基站距离机器人的距离，

为UWB测距精度误差，对概率进行了修正，满足高斯分布规律，提高了定位精度。

确定所述GNSS模块在其定位范围内任意栅格出现的概率P_g(n)算法为：

P_g(n)＝Φ*exp[-ε²/2η²]+C_mod

其中，Φ、C_mod分别为定位误差ε的权重和定位模式mod的权重，Φ通过所述GNSS模块搜索到的卫星数目及定位模式确定，C_mod由所述GNSS模块的定位模式决定。

具体的，若所述主控计算单元能正常收到所述GNSS模块的信号及差分信号，通过搜索到的所述GNSS模块的卫星数目及其定位模式mod，从而确定此时接收的定位信息误差及权重。如收到的Gnss信号格式如下：

$GPGGA，000001，3112.518576，N，12127.901251，E，4，8，1，0，M，-32，M，3，0*4B

通过解析信号，可得到当前经纬度、定位误差ε、卫星数目k、定位模式mod、时间戳等信息，将经纬度信号转化为机器人可用的笛卡尔坐标系中(x，y)，并转化为对于栅格，即得所述GNSS模块在其定位范围内任意栅格出现的概率P_g(n)。

步骤S3；根据所得概率P_g(n)、P_u(n)确定机器人大致位置，再结合所述IMU惯性测量单元确定机器人初始方向，再分别确定所述码盘、所述激光雷达在其定位范围内任意栅格出现的概率P_O(n)、P_L(n)。

所述激光雷达精确测量机器人距障碍物之间的距离，使用自适应蒙特卡洛定位算法，对比地图信息，获得机器人处其定位范围内任意栅格出现的概率P_L(n)。

步骤S3中根据运动过程中所述码盘积累值确定机器人处在其定位范围内任意栅格出现的概率P_O(n)为：

其中，del_d、del_A为计算时间内机器人运动的距离和角度，Q、T为码盘线速度、角速度误差权重系数。

步骤S4：设定所述GNSS模块、所述UWB系统、所述码盘、所述激光雷达的定位权重，通过归一化得出机器人出现概率最高的位置坐标。

具体的，最终机器人处于地图任意栅格的概率P(n)值越大，表明机器人处于该栅格的概率越高，最终P(n)最高的栅格对应的位置即为当前机器位置坐标(x，y)，其归一化计算公式为：

其中，μ、v、

ω分别为P_g、P_u、P_L、P_O的对应定位权重系数且

N_max＝S_地图/S_栅格；

使用归一化处理大大提高了机器人定位的精度。

通过所述UWB系统信号强度、基站数目、所述GNSS模块信号强度、卫星数目、所述激光雷达与地图的匹配程度共同综合确定，设置相对信号强度大的对应的权重大。

步骤S5：步骤S4所得位置坐标联合所述IMU惯性测量单元输出的加速度、角速度和角加速度以及所述码盘输出的实时线速度和角速度，输入所述EKF滤波器。

步骤S6：输出机器人实时位置坐标及实时角速度。

关于权重：如果机器人通过室外空旷区域时，此时所述GNSS模块信号良好，则所述GNSS模块的权重相对较大；如果室外某一区域如高楼遮挡，导致所述GNSS模块信号弱，此时所述UWB系统的权重相对较大；如果处于室内，如大楼内走廊，大厅，机器人充电室等区域，则通过预先建好的地图，结合所述激光雷达权重相对较大。信号的强弱好坏通过所述主控计算单元进行判定。具体的，所述GNSS模块信号良好的判断标准为收到八个及以上所述卫星信号，并检测出差分信号。所述UWB系统信号正常的判断标准为接收到三个及以上所述基站的信号。

相较于现有技术，本发明提供的所述基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，通过对不同定位方式进行归一化处理，有助于快速进行数据融合和提高定位精度。另提出了不同定位方式相应的修正误差公式，更进一步提高定位精度和稳定性。取得了一系列有益效果：开机无需人为配置或手动初始化，自动寻找信号，确定机器人本体位置和方向；通过室内、室外，GNSS信号、UWB信号、激光匹配等过渡区域时，自动选择各自权重进行数据融合后输出机器人实时位置坐标及实时角速度，全程定位过程无缝衔接。创造性的融合GNSS、UWB、激光雷达、等可行的机器人定位方案，实现了各定位方式的无缝切换及数据融合，克服全局定位信息受环境影响大、以及惯性器件长时间漂移的缺陷，实用性强，稳定性好，进一步提高巡逻机器人定位定向精度，为精确导航提供基础信息。

Claims (4)

1.一种基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，所述巡逻机器人包括内置EKF滤波器的主控计算单元，以及分别与其连接且协同作用的GNSS模块、UWB系统、激光雷达、IMU惯性测量单元、码盘和充电感应模块，其特征在于：具体包括如下步骤：

S1：开机，检查所述充电桩与机器人是否连接； 若是，机器人初始位置等于所述充电桩位置；若否，进入步骤S2；

S2：分别确定所述GNSS模块、所述UWB系统测得机器人在其定位范围内任意栅格出现的概率P_g(n)、P_u(n)；

S3：根据所得概率P_g(n)、P_u(n)确定机器人大致位置，再结合所述IMU惯性测量单元确定机器人初始方向，分别确定所述码盘、所述激光雷达在其定位范围内任意栅格出现的概率P_O(n)、P_L(n)；

S4：设定所述GNSS模块、所述UWB系统、所述码盘、所述激光雷达各自的定位权重，通过归一化得出机器人出现概率最高的位置坐标；

S5：步骤S4所得位置坐标联合所述IMU惯性测量单元输出的加速度、角速度和角加速度以及所述码盘输出的实时线速度和角速度，输入所述EKF滤波器；

S6：输出机器人实时位置坐标及实时角速度；

所述步骤S2中，所述主控计算单元检查所述UWB系统的基站信号，再确定所述UWB系统测得机器人在其定位范围内任意栅格出现的概率P_u(n)，P_u(n)算法为：

其中，δ为所述UWB系统接收数据的频率，k为所述UWB系统的基站编号，

n_k＝2*π*d_k/r

r为地图分辨率，n_k代表编号为k的基站的定位范围内的栅格数，A、a为常数，d_k代表编号为k的基站距离机器人的距离，

为所述UWB系统测距精度误差；

所述步骤S2中确定所述GNSS模块在其定位范围内任意栅格出现的概率P_g(n)算法为：

P_g(n)＝Φ*exp[-ε²/2η²]+C_mod

其中，Φ、C_mod分别为定位误差ε的权重和定位模式_mod的权重，Φ通过所述GNSS模块搜索到的卫星数目及定位模式确定，C_mod由所述GNSS模块的定位模式决定；

所述定位权重的判别具体如下：如果机器人通过室外空旷区域时，此时所述GNSS模块信号良好，则所述GNSS模块的权重相对较大；如果室外某一区域如高楼遮挡，导致所述GNSS模块信号弱，此时所述UWB系统的权重相对较大；如果处于室内，如大楼内走廊，大厅，机器人充电室等区域，则通过预先建好的地图，结合所述激光雷达权重相对较大；信号的强弱好坏通过所述主控计算单元进行判定；具体的，所述GNSS模块信号良好的判断标准为收到八个及以上所述卫星信号，并检测出差分信号；所述UWB系统信号正常的判断标准为接收到三个及以上所述基站的信号。

2.根据权利要求1所述的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，其特征在于：步骤S3中所述激光雷达精确测量距障碍物之间距离后，使用自适应蒙特卡洛定位算法，对比地图信息，获得机器人在其定位范围内任意栅格出现的概率P_L(n)。

3.根据权利要求1所述的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，其特征在于：步骤S3中所述码盘根据运动过程中的积累值确定机器人在其定位范围内任意栅格出现的概率P_O(n)为：

其中，del_d、del_A为计算时间内机器人运动的距离和角度，Q、T分别为码盘线速度、角速度误差权重系数。

4.根据权利要求1所述的基于GNSS、UWB、IMU、激光雷达、码盘的室外巡逻机器人多环境联合定位方法，其特征在于：步骤S4中，最终机器人处于地图任意栅格的概率p(n)值越大，表明机器人处于该栅格的概率越高，最终P(n)最高的栅格对应的位置即为当前机器位置坐标(x,y)，其计算公式为：

P(n)＝μ*P_u(n)+ν*P_g(n)+l*P_L(n)+ω*P_O(n)，n＝1、2、3....N_max

其中，μ、ν、l、ω分别为P_g、P_u、P_L、P_O的对应定位权重系数且μ+ν+l+ω＝1，N_max＝S_地图/S_栅格；

通过所述UWB系统信号强度、基站数目、所述GNSS模块信号强度、卫星数目、所述激光雷达与地图的匹配程度共同综合确定，设置相对信号强度大的对应的权重大。

Images