CN109737957A - 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统 - Google Patents

Abstract

本发明公开了一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统，算法采用级联滤波结构，即在激光雷达数据处理部分，将激光雷达采集得到的移动机器人与角点之间的距离作为观测信息，利用EFIR滤波算法对激光雷达测量得到的位置进行预估。在此基础上，在松组合导航数据处理部分，将INS与激光雷达分别测量的位置信息之差作为观测量输入到FIR滤波算法，对INS计算的误差进行预估，最终得到当前时刻移动机器人最优的位置信息。本发明有益效果：通过在激光雷达部分采用级联滤波结构有效提高了激光雷达通过角点距离测量移动机器人位置的精度，同时，FIR滤波算法的使用也提高了算法的鲁棒性。

Description

一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统

技术领域

本发明涉及复杂环境下组合定位技术领域，尤其涉及一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

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

近年来，针对移动机器人导航与定位问题，国内外研究人员进行了深入研究，并取得了一定研究成果。现有导航定位技术主要有全球卫星导航系统(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地图的构建，进而实现了移动机器人的自主导航。但是，发明人发现，视觉导航的精度严重依赖于所获取的图像的质量，一旦移动机器人无法获取高质量的图像信息，导航精度会受较大影响。除此之外，视觉导航策略的实时性较差，难以满足移动机器人快速运动的跟踪。因此，单一的导航技术难以提供持续稳定的导航信息。

发明内容

为了解决上述问题，本发明提出了一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统，采用级联滤波结构，即在激光雷达(激光雷达)数据处理部分，将激光雷达采集得到的移动机器人与角点之间的距离作为观测信息，利用EFIR滤波算法对激光雷达测量得到的位置进行预估。在此基础上，在松组合导航数据处理部分，将INS与激光雷达分别测量的位置信息之差作为观测量输入到FIR滤波算法，对INS计算的误差进行预估，最终得到当前时刻移动机器人最优的位置信息。通过在激光雷达部分采用级联滤波结构有效提高了激光雷达通过角点距离测量移动机器人位置的精度，同时，FIR滤波算法的使用也提高了算法的鲁棒性。

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

在一个或多个实施方式中公开的一种采用级联FIR滤波的INS/LiDAR组合导航系统，包括：定位单元、数据处理单元以及惯性导航系统；

所述激光雷达和惯性导航系统分别与数据处理单元连接；

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

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

进一步地，所述惯性导航系统包括：里程计和电子罗盘；

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

在一个或多个实施方式中公开的一种采用级联FIR滤波的INS/LiDAR组合导航方法，包括：

以移动机器人k时刻在东向和北向的位置和速度为状态量，以激光雷达采集得到的移动机器人与角点位置之间的距离作为观测量，利用EFIR滤波算法对激光雷达测量得到的位置进行预估；

以移动机器人k时刻在东向和北向的位置误差和速度误差做为状态量，以惯性导航系统计算得到的移动机器人在导航系坐标下的位置信息与预估得到的激光雷达测量的位置信息之差作为观测量，利用FIR滤波算法对惯性导航系统计算的位置信息误差进行预估，最终得到当前时刻移动机器人最优的位置信息。

进一步地，所述EFIR滤波算法的状态方程为：

其中，分别为k时刻和k-1时刻移动机器人在东向和北向的位置；分别为k时刻和k-1时刻移动机器人在东向和北向的速度；T为采样时间；为k-1时刻的系统噪声。

进一步地，所述EFIR滤波算法的观测方程为：

其中，为通过激光雷达测量得到的角点C_i在k时刻的位置信息，为k时刻移动机器人在东向和北向的位置；为系统k时刻的观测噪声；代表为激光雷达测量得到的k时刻移动机器人与第i个角特征点之间的距离。

进一步地，所述FIR滤波算法的状态方程为：

其中，(δ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时刻的系统噪声。

进一步地，所述FIR滤波算法的观测方程为：

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

在一个或多个实施方式中公开的一种采用级联FIR滤波的INS/LiDAR组合导航系统，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述的任一种采用级联FIR滤波的INS/LiDAR组合导航方法。

在一个或多个实施方式中公开的一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行上述的任一种采用级联FIR滤波的INS/LiDAR组合导航方法。

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

1、采用INS/LiDAR组合导航的方式，提高了导航稳定性。

2、通过在激光雷达部分采用级联滤波结构有效提高了激光雷达通过角点距离测量移动机器人位置的精度。

3、采用FIR滤波算法提高了组合导航方法的鲁棒性。

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

附图说明

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

图1为实施例一中INS/LiDAR组合导航系统示意图；

图2为实施例二中INS/LiDAR组合导航方法示意图；

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

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

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

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

具体实施方式

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

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

实施例一

在一个或多个实施方式中公开了一种采用级联FIR滤波的INS/LiDAR组合导航系统，如图1所示，包括：惯性导航系统、数据处理单元、激光雷达、里程计和电子罗盘；激光雷达、里程计、电子罗盘均固定在移动机器人上，并且与数据处理单元连接。

其中，

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

INS有低成本里程计和电子罗盘组成，其中：

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

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

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

实施例二

在一个或多个实施方式中公开的一种采用级联FIR滤波的INS/LiDAR组合导航方法，如图2所示，包括：

(1)组合导航方法采用级联是滤波结构，即在激光雷达数据处理部分，将激光雷达采集得到的移动机器人与角点之间的距离作为观测信息，利用EFIR滤波算法对激光雷达测量得到的位置进行预估。在此基础上，在松组合导航数据处理部分，将INS与激光雷达分别测量的位置信息之差作为观测量输入到FIR滤波算法，对INS计算的误差进行预估，最终得到当前时刻移动机器人最优的位置信息；

(2)针对在激光雷达数据处理部分使用的EFIR滤波算法，以移动机器人k时刻在东向和北向的位置和速度为状态量，以激光雷达测量得到的当前时刻角点与移动机器人之间的距离信息作为观测量输入到EFIR滤波算法，得到当前时刻最优的激光雷达测量得到的位置信息；

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

其中，寻找角点的过程如图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)针对在松组合导航数据处理部分使用的FIR滤波算法，以移动机器人k时刻在东向和北向的位置误差和速度误差为状态量，以INS与激光雷达分别测量的位置信息之差作为观测量，通过FIR滤波算法进行数据融合，并对INS的位置误差进行预估，将其与INS测量得到的位置作差，最终得到当前时刻移动机器人最优的位置信息。

(5)针对在激光雷达数据处理部分使用的EFIR滤波算法，以移动机器人k时刻在东向和北向的位置和速度为状态量，以激光雷达测量得到的当前时刻角点与移动机器人之间的距离信息作为观测量输入到EFIR滤波算法，得到当前时刻最优的激光雷达测量得到的位置信息。

EFIR滤波算法的状态方程为：

其中，分别为k时刻和k-1时刻移动机器人在东向和北向的位置；分别为k时刻和k-1时刻移动机器人在东向和北向的速度；T为采样时间；为k-1时刻的系统噪声。

EFIR滤波算法的观测方程为：

其中，为通过激光雷达测量得到的角点C_i在k时刻的位置信息，为系统k时刻的观测噪声。

(6)针对在松组合导航数据处理部分使用的FIR滤波算法，以移动机器人k时刻在东向和北向的位置误差和速度误差为状态量，以INS与激光雷达分别测量的位置信息之差作为观测量，通过FIR滤波算法进行数据融合，并对INS的位置误差进行预估，将其与INS测量得到的位置作差，最终得到当前时刻移动机器人最优的位置信息。

FIR滤波算法的状态方程为：

其中，(δ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时刻的系统噪声。

FIR滤波算法的观测方程为：

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

实施例三

在一个或多个实施方式中公开的一种采用级联FIR滤波的INS/LiDAR组合导航系统，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例二中的采用级联FIR滤波的INS/LiDAR组合导航方法。

实施例四

在一个或多个实施方式中公开的一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行实施例二中的采用级联FIR滤波的INS/LiDAR组合导航方法。

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

Claims (10)

1.一种采用级联FIR滤波的INS/LiDAR组合导航系统，其特征在于，包括：

定位单元、数据处理单元以及惯性导航系统；所述激光雷达和惯性导航系统分别于数据处理单元连接；

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

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

3.如权利要求1所述的一种采用级联FIR滤波的INS/LiDAR组合导航系统，其特征在于，所述惯性导航系统包括：里程计和电子罗盘；

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

4.一种采用级联FIR滤波的INS/LiDAR组合导航方法，其特征在于，包括：

以移动机器人k时刻在东向和北向的位置和速度为状态量，以激光雷达采集得到的移动机器人与角点位置之间的距离作为观测量，利用EFIR滤波算法对激光雷达测量得到的位置进行预估；

以移动机器人k时刻在东向和北向的位置误差和速度误差做为状态量，以惯性导航系统计算得到的移动机器人在导航系坐标下的位置信息与预估得到的激光雷达测量的位置信息之差作为观测量，利用FIR滤波算法对惯性导航系统计算的位置信息误差进行预估，最终得到当前时刻移动机器人最优的位置信息。

5.如权利要求4所述的一种采用级联FIR滤波的INS/LiDAR组合导航方法，其特征在于，所述EFIR滤波算法的状态方程为：

其中，分别为k时刻和k-1时刻移动机器人在东向和北向的位置；分别为k时刻和k-1时刻移动机器人在东向和北向的速度；T为采样时间；为k-1时刻的系统噪声。

6.如权利要求4所述的一种采用级联FIR滤波的INS/LiDAR组合导航方法，其特征在于，所述EFIR滤波算法的观测方程为：

其中，为通过激光雷达测量得到的角点C_i在k时刻的位置信息，为k时刻移动机器人在东向和北向的位置；为系统k时刻的观测噪声；代表为激光雷达测量得到的k时刻移动机器人与第i个角特征点之间的距离。

7.如权利要求4所述的一种采用级联FIR滤波的INS/LiDAR组合导航方法，其特征在于，所述FIR滤波算法的状态方程为：

其中，(δ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时刻的系统噪声。

8.如权利要求4所述的一种采用级联FIR滤波的INS/LiDAR组合导航方法，其特征在于，所述FIR滤波算法的观测方程为：

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

9.一种采用级联FIR滤波的INS/LiDAR组合导航系统，其特征在于，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现权利要求4-8任一项所述的采用级联FIR滤波的INS/LiDAR组合导航方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时执行权利要求4-8任一项所述的采用级联FIR滤波的INS/LiDAR组合导航方法。