CN109708632A - 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法 - Google Patents

Abstract

本发明公开了一种面向移动机器人的激光雷达/INS/地标松组合导航系统及方法，将激光雷达与INS分别测量的位置信息之差作为观测量输入到Kalman滤波器，通过Kalman滤波器进行数据融合，并对移动机器人的位置信息进行预估，最终得到当前时刻移动机器人最优的位置信息。本发明有益效果：通过Kalman滤波器对多种传感器的测量信息进行数据融合，克服了单一导航技术由于自身技术的不足的对导航精度影响。

Description

一种面向移动机器人的激光雷达/INS/地标松组合导航系统 及方法

技术领域

本发明涉及复杂环境下组合定位技术领域，尤其涉及一种面向移动机器人的激光雷达/INS/地标松组合导航系统及方法。

背景技术

近年来，随着科学技术的发展和人民生活水平的提高，移动机器人逐步走进人们的生活。作为移动机器人为人类提供高质量服务的基础，面向移动机器人的导航与定位正逐渐成为该领域的研究热点。然而在室内环境下，移动机器人导航信息获取的准确性和实时性都会受到室内无线电信号微弱、电磁干扰强烈等一系列复杂环境的影响。在室内环境下，如何消除室内复杂导航环境对移动机器人导航信息获取的准确性、实时性及鲁棒性的影响，保证移动机器人在室内环境下的高精度自主导航，具有重要的科学理论意义和实际应用价值。

近年来，针对移动机器人导航与定位问题，国内外研究人员进行了深入研究，并取得了一定研究成果。现有导航定位技术主要有全球卫星导航系统(Global NavigationSatellite System,GNSS)、无线传感器网络(Wireless Sensors Network,WSN)技术、惯性导航系统(Inertial Navigation System,INS)和视觉导航技术等。其中，以全球定位系统(Global Positioning System,GPS)为代表的GNSS导航技术在室内环境下无法克服因信号受遮挡导致定位精度下降甚至失锁的问题，因此完成室内环境下的移动机器人导航。对于WSN技术，目前对这一领域的研究成果众多。例如，现有技术提出基于WiFi的室内移动机器人通信和定位算法；现有技术对基于射频识别(Radio frequency identification,RFID)技术的移动机器人定位算法进行了研究；现有技术提出将超宽带技术(Ultra Wideband，UWB)应用于移动机器人室内导航定位中。发明人发现，虽然上述WSN技术均能实现在室内环境下移动机器人的导航定位，但其信号十分容易受到室内复杂环境的干扰；除此之外，短距离无线定位系统需要预先在室内铺设参考节点，不能实现移动机器人的自主导航。

与GNSS和WSN技术相比，INS技术和视觉导航技术不需要额外的设备辅助导航，因此具有更高的自主性。但是需要指出的是，INS技术的解算误差会随时间累积，因此不适合长时间高精度导航。特别是由于受到移动机器人载体体积的限制，往往采用低成本IMU，传感器的精度远低于高精度IMU，更加剧了误差随时间的累积。近年来随着视觉技术的发展与进步，学者们提出将视觉导航应用于移动机器人的导航与定位。例如：利用RGB-D相机完成对机器人的6D位姿的预估和3D地图的构建，进而实现了移动机器人的自主导航。发明人发现，视觉导航的精度严重依赖于所获取的图像的质量，一旦移动机器人无法获取高质量的图像信息，导航精度会受较大影响。除此之外，视觉导航策略的实时性较差，难以满足移动机器人快速运动的跟踪。通过上述分析可以看出，单一的导航技术难以提供持续稳定的导航信息。

发明内容

为了解决上述问题，本发明提出了一种面向移动机器人的激光雷达/INS/地标松组合导航系统及方法，将激光雷达与INS分别测量的位置信息之差作为观测量输入到Kalman滤波器，通过Kalman滤波器进行数据融合，并对移动机器人的位置信息进行预估，最终得到当前时刻移动机器人最优的位置信息，克服了单一导航技术由于自身技术的不足的对导航精度影响。

为了实现上述目的，本发明采用如下技术方案：

在一个或多个实施方式中公开的一种面向移动机器人的激光雷达/INS/地标松组合导航系统，包括：

定位单元、数据处理单元、设置在设定位置的若干地标以及惯性导航系统；所述定位单元和惯性导航系统分别于数据处理单元连接；

所述数据处理单元对定位单元、惯性导航系统以及根据地标获得的移动机器人位置信息进行融合，得到移动机器人的最优位置信息预估。

进一步地，所述定位单元包括：激光雷达，所述激光雷达通过检测移动机器人所处环境的角点位置信息，测量出移动机器人的位置信息。

进一步地，所述惯性导航系统包括：里程计和磁力计；

所述里程计用于测量移动机器人的移动速度；所述磁力计用于测量移动机器人的航向，通过里程计得到在采样周期内移动机器人运行的距离信息，根据所述距离信息与磁力计测量得到的移动机器人航向信息得到移动机器人在导航系坐标下的位置信息。

进一步地，还包括：扫描器，所述扫描器通过扫描预先铺设的地标为激光雷达提供准确的位置信息以及移动机器人的航向角信息。

在一个或多个实施方式中公开的一种面向移动机器人的激光雷达/INS/地标松组合导航方法，包括：

通过激光雷达和惯性导航系统分别获得移动机器人位置信息；

以移动机器人k时刻在东向和北向的位置误差和速度误差作为状态量，以激光雷达与INS分别测量的移动机器人位置信息之差作为观测量输入到Kalman滤波器；

通过Kalman滤波器进行数据融合，并对移动机器人的位置信息进行预估，最终得到当前时刻移动机器人最优的位置信息。

进一步地，激光雷达通过角点扫描、角点位置粗估计以及角点匹配，得到当前时刻移动机器人周边的角点在导航坐标系下的位置以及角点与移动机器人之间的距离，利用所述距离信息，通过最小二乘算法，得到激光雷达测量的移动机器人位置信息。

进一步地，惯性导航系统通过里程计得到在采样周期内移动机器人运行的距离信息，利用所述距离信息与磁力计测量得到的移动机器人航向信息得到移动机器人位置信息。

进一步地，根据扫描器读取到的地标信息，分别对激光雷达和惯性导航系统获得的移动机器人位置信息进行修正。

进一步地，所述Kalman滤波器的状态方程为：

其中，(δP_East,k,δP_North,k)、(δP_East,k-1,δP_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的位置误差；(δV_East,k,δV_North,k)、(δV_East,k-1,δV_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的速度误差；T为采样时间；w_k-1为k-1时刻的系统噪声。

进一步地，所述Kalman滤波器的观测方程为：

其中，分别为通过激光雷达和INS测量得到的移动机器人在k时刻的位置信息，v_k为系统k时刻的观测噪声。

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

1、Kalman滤波器以移动机器人在k时刻在东向和北向的位置误差和速度误差作为状态量，激光雷达与INS分别测量的位置信息之差作为观测量数据滤波模型进行数据融合，克服了单一导航技术由于自身技术的不足的对导航精度影响。

2、可用于室内环境下的移动机器人的中高精度定位。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为实施例一中面向移动机器人的激光雷达/INS/地标松组合导航系统示意图；

图2为实施例二中面向移动机器人的激光雷达/INS/地标松组合导航数据融合方法示意图。

图3为实施例二中寻找角点流程图；

图4为实施例二中通过雷达获取周围环境信息示意图；

图5为实施例二中使用IEPF方法进行线段分割寻找角点示意图；

图6为实施例二中关联角点流程图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

在一个或多个实施方式中公开了一种面向移动机器人的激光雷达/INS/地标松组合导航系统，如图1所示，包括：惯性导航系统、数据处理单元、激光雷达、地标、二维码扫描器、里程计和磁力计；地标固定在移动机器人导航环境的地上的任意位置，激光雷达、二维码扫描器、里程计、磁力计均固定在移动机器人上，并且与数据处理单元连接。

其中，

激光雷达：用于检测移动机器人所处环境角点位置信息；

二维码扫描器：通过检测预先铺设的地标为激光雷达提供准确的位置信息以及移动机器人的航偏信息；地标中含有位置和航向信息，可以从二维码中直接读出；因为地标是预先铺设的，所有他的位置和航向可以预先测定，我们认为是准确的。

INS有低成本里程计和磁力计组成，其中：

里程计：用于测量移动机器人移动速度；

磁力计：用于测量移动机器人航向，以及与码盘合作共同计算移动机器人在导航系坐标下的位置信息；

数据处理单元：用于对采集到的传感器数据进行数据融合。

实施例二

在一个或多个实施方式中公开的一种面向移动机器人的激光雷达/INS/地标松组合导航数据融合方法，如图2所示，包括：

(1)以移动机器人在k时刻在东向和北向的位置误差和速度误差作为状态量，以激光雷达和INS分别测量得到的移动机器人的位置之差作为系统观测量，构建组合定位模型；

(2)激光雷达通过寻找角点和角点关联等步骤得到当前时刻移动机器人周边的角点在导航坐标系下的位置以及角点与移动机器人之间的距离，利用距离信息，通过最小二乘算法，得到激光雷达测量的移动机器人位置信息；

其中，寻找角点的过程如图3所示，具体为：

1.通过雷达获取周围环境信息，包括距离信息和角度信息如图4；

2.对步骤1获取的环境位置信息进行预处理，删除跳变点。

3.对经预处理后的t时刻环境位置信息点集A_t{i₁…i_n}进行区域分割，设置阈值D₁＝200mm，计算连续两点i_k和i_k+1之间距离S_k,如果S_k>D₁则将原点集A_t{i₁…i_n}分割成两个区域和

4.对新生成的区域重复进行步骤3，直到每个区域内连续两点均满足S_k<D₁。

5.通过步骤4我们可以获得环境位置信息每个区域的点集对每个区域使用IEPF方法进行线段分割寻找角点，对如图5区域AD做如下操作：

首先连接区域起点和终点，获得线段AD，计算区域AD内到线段AD距离最大的点C，计算点C到线段AD距离S，并与自适应阈值β作比较，如果一个点到该线段的距离大于β，则为角点，得到角点位置信息(阈值β为线段AD长度的10分之一)。

6.角点特征C将区域AD分割为AC和CD两条新的线段，对AC和CD重复步骤5直到每条线段内的点均小于自适应阈值β。

角点关联的过程如图6所示，具体为：

1.t时刻已知电子罗盘提供的航向角与第一时刻航向角做差获得t时下载体航向角变化t时刻载体位置P_t|Nx,P_t|Ny，雷达测得的载体到达角点距离雷达安装方向到角点位置的夹角为θ_t|n，载体坐标的航向角为θ_t|Y，则t时刻角点i_t|n的全局坐标为：

2.将已知角点位置出现新角点特征时将角点位置与预先确定的电子地图中角点位置做比较，确定当前角点在全局地图中位置。

3.已知t时刻存在的角点集合为{i_t|1...i_t|n}，对t时刻第k个角点i_t|k求到t-1时刻角点集合{i_t-1|1...i_t-1|n}，每个角点的欧氏距离

两个时刻的角点夹角差δθ_k|1～n，角点距离差δd_k|1～n求取在满足条件下D_k|min，第i点和上一时刻第一点被视作同一角点，对{i_t|1...i_t|n}依次寻找其对应角点，如果存在角点特征未能关联，则当做新角点特征跳转步骤2。

(3)惯性导航系统通过里程计得到在采样周期内移动机器人运行的距离信息，利用该距离信息与磁力计测量得到的移动机器人航向信息共同计算惯性导航系统测量的移动机器人位置信息；

(4)移动机器人根据读取到的地标信息，对激光雷达角点位置粗估计进行修正，同时对移动机器人航向角信息进行修正；

(5)将激光雷达与惯性导航系统分别测量的位置信息之差作为观测量输入到Kalman滤波器，通过Kalman滤波器进行数据融合，并对移动机器人的位置信息进行预估，最终得到当前时刻移动机器人最优的位置信息。

Kalman滤波器的状态方程为：

其中，(δP_East,k,δP_North,k)、(δP_East,k-1,δP_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的位置误差；(δV_East,k,δV_North,k)、(δV_East,k-1,δV_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的速度误差；T为采样时间；w_k-1为k-1时刻的系统噪声。

Kalman滤波器的观测方程为：

其中，分别为通过激光雷达和INS测量得到的移动机器人在k时刻的位置信息，v_k为系统k时刻的观测噪声。

作为状态量移动机器人k时刻和k-1时刻的东向和北向位置误差的初值根据实际需要自行设定。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种面向移动机器人的激光雷达/INS/地标松组合导航系统，其特征在于，包括：

定位单元、数据处理单元、设置在设定位置的若干地标以及惯性导航系统；所述定位单元和惯性导航系统分别于数据处理单元连接；

所述数据处理单元对定位单元、惯性导航系统以及根据地标获得的移动机器人位置信息进行融合，得到移动机器人的最优位置信息预估。

2.如权利要求1所述的一种面向移动机器人的激光雷达/INS/地标松组合导航系统，其特征在于，所述定位单元包括：激光雷达，所述激光雷达通过检测移动机器人所处环境的角点位置信息，测量出移动机器人的位置信息。

3.如权利要求1所述的一种面向移动机器人的激光雷达/INS/地标松组合导航系统，其特征在于，所述惯性导航系统包括：里程计和磁力计；

所述里程计用于测量移动机器人的移动速度；所述磁力计用于测量移动机器人的航向，通过里程计得到在采样周期内移动机器人运行的距离信息，根据所述距离信息与磁力计测量得到的移动机器人航向信息得到移动机器人在导航系坐标下的位置信息。

4.如权利要求1所述的一种面向移动机器人的激光雷达/INS/地标松组合导航系统，其特征在于，还包括：扫描器，所述扫描器通过扫描预先铺设的地标为激光雷达提供准确的位置信息以及移动机器人的航向角信息；其中，所述地标上设有包含位置信息和航向角信息的二维码。

5.一种面向移动机器人的激光雷达/INS/地标松组合导航方法，其特征在于，包括：

通过激光雷达和惯性导航系统分别获得移动机器人位置信息；

以移动机器人k时刻在东向和北向的位置误差和速度误差作为状态量，以激光雷达与INS分别测量的移动机器人位置信息之差作为观测量输入到Kalman滤波器；

通过Kalman滤波器进行数据融合，并对移动机器人的位置信息进行预估，最终得到当前时刻移动机器人最优的位置信息。

6.如权利要求5所述的一种面向移动机器人的激光雷达/INS/地标松组合导航方法，其特征在于，激光雷达通过角点扫描、角点位置粗估计以及角点匹配，得到当前时刻移动机器人周边的角点在导航坐标系下的位置以及角点与移动机器人之间的距离，利用所述距离信息，通过最小二乘算法，得到激光雷达测量的移动机器人位置信息。

7.如权利要求5所述的一种面向移动机器人的激光雷达/INS/地标松组合导航方法，其特征在于，惯性导航系统通过里程计得到在采样周期内移动机器人运行的距离信息，利用所述距离信息与磁力计测量得到的移动机器人航向信息得到移动机器人位置信息。

8.如权利要求5所述的一种面向移动机器人的激光雷达/INS/地标松组合导航方法，其特征在于，根据扫描器读取到的地标信息，分别对激光雷达和惯性导航系统获得的移动机器人位置信息进行修正。

9.如权利要求5所述的一种面向移动机器人的激光雷达/INS/地标松组合导航方法，其特征在于，所述Kalman滤波器的状态方程为：

其中，(δP_East,k,δP_North,k)、(δP_East,k-1,δP_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的位置误差；(δV_East,k,δV_North,k)、(δV_East,k-1,δV_North,k-1)分别为k时刻和k-1时刻移动机器人在东向和北向的速度误差；T为采样时间；w_k-1为k-1时刻的系统噪声。

10.如权利要求5所述的一种面向移动机器人的激光雷达/INS/地标松组合导航数据融合方法，其特征在于，所述Kalman滤波器的观测方程为：

其中，分别为通过激光雷达和INS测量得到的移动机器人在k时刻的位置信息，v_k为系统k时刻的观测噪声。