[发明专利]一种移动物体的定位定姿方法和系统

摘要

本发明公开了一种移动物体的定位定姿方法和系统，通过惯性测量单元、里程计和激光雷达，构建统一的、融合激光雷达控制标靶数据、惯性测量单元数据以及里程计数据的扩展卡尔曼滤波模型。该模型建立在惯性测量单元动力学模型和误差模型基础上，通过将激光雷达控制标靶数据带入到卡尔曼滤波方程中，计算IMU/里程计组合的误差状态向量，限制其误差发散，从而得到高精度位置和姿态。从而实现了在无卫星导航信号的环境下，对移动物体的高精度定位定姿。

主权项

1.一种移动物体的定位定姿方法，其特征在于，所述方法通过设置在移动物体内的惯性测量单元、里程计和激光雷达对所述移动物体进行定位和定姿，具体包括如下步骤：A、惯性测量单元和里程计测量的数据经过惯性滤波器滤波后得到移动物体的初始位置、初始速度和初始姿态；B、激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始位置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标；C、将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差；D、将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量；将所述惯性测量单元的误差反馈到惯性滤波器中；E、通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态；所述步骤A具体包括步骤：A1、对惯性测量单元测得的数据进行纯惯性推算，得到惯性位置、速度和姿态；A2、惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置增量；A3、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩展卡尔曼滤波器中，得到惯性测量单元的误差修正量、以及速度误差修正量和姿态误差修正量，根据速度误差修正量和滤波周期得到位置误差修正量；A4、将惯性测量单元的误差修正量反馈到惯性滤波器中，而位置、速度和姿态误差修正量则与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿态。