[发明专利]一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统

权利要求书

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

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

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

激光雷达采用级联滤波结构，在激光雷达数据处理部分，将激光雷达采集得到的移动机器人与角点之间的距离作为观测信息，利用EFIR滤波算法对激光雷达测量得到的位置进行预估，并在松组合导航数据处理部分，将INS与激光雷达分别测量的位置信息之差作为观测量输入到FIR滤波算法，对INS计算的误差进行预估，得到当前时刻移动机器人最优的位置信息，通过在激光雷达部分采用级联滤波结构提高激光雷达通过角点距离测量移动机器人位置的精度，通过FIR滤波算法提高算法的鲁棒性。

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时刻的系统噪声。