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

权利要求书

1.一种移动物体的定位定姿方法，其特征在于，所述方法通过设置在移动物体内的惯性测量单元、里程计和激光雷达对所述移动物体进行定位和定姿，具体包括如下步骤：

A、惯性测量单元和里程计测量的数据经过惯性滤波器滤波后得到移动物体的初始位置、初始速度和初始姿态；

B、激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始位置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标；

C、将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差；

D、将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量；将所述惯性测量单元的误差反馈到惯性滤波器中；

E、通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态；

所述步骤A具体包括步骤：

A1、对惯性测量单元测得的数据进行纯惯性推算，得到惯性位置、速度和姿态；

A2、惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置增量；

A3、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩展卡尔曼滤波器中，得到惯性测量单元的误差修正量、以及速度误差修正量和姿态误差修正量，根据速度误差修正量和滤波周期得到位置误差修正量；

A4、将惯性测量单元的误差修正量反馈到惯性滤波器中，而位置、速度和姿态误差修正量则与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿态。

2.根据权利要求1所述的移动物体的定位定姿方法，其特征在于，所述步骤E之后，还包括步骤：F、对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。

3.根据权利要求1所述的移动物体的定位定姿方法，其特征在于，所述测量控制点为预设的被动式的反射标靶。

4.一种移动物体的定位定姿系统，其特征在于，所述系统包括移动物体、惯性测量单元、里程计和激光雷达，所述惯性测量单元、里程计和激光雷达均设置在移动物体内；所述激光雷达用于对测量控制点进行扫描，得到测量控制点与移动物体的距离和角度；所述系统还包括：

惯性滤波器，用于对惯性测量单元和里程计测量的数据进行惯性滤波，得到移动物体的初始位置、初始速度和初始姿态；

坐标残差计算模块，用于将激光雷达测得的测距测角数据与所述初始位置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标；将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差；

惯性扩展卡尔曼滤波器，用于将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量；将所述惯性测量单元的误差反馈到惯性滤波器中；通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态；

所述惯性扩展卡尔曼滤波器，还用于根据惯性滤波器输出的位置增量残差，得到惯性测量单元的误差修正量、以及速度误差修正量和姿态误差修正量，根据速度误差修正量和滤波周期得到位置误差修正量；将惯性测量单元的误差修正量反馈到惯性滤波器中；

所述惯性滤波器具体用于对惯性测量单元测得的数据进行纯惯性推算，得到惯性位置、速度和姿态；将惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置增量；惯性位置增量与航位推算位置增量求差，得到位置增量残差，将所述位置增量残差输出给惯性扩展卡尔曼滤波器；根据惯性扩展卡尔曼滤波器反馈的惯性测量单元的误差修正量；将位置、速度和姿态误差修正量与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿态。

5.根据权利要求4所述的移动物体的定位定姿系统，其特征在于，所述系统还包括：

RTS平滑滤波器，用于对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。

6.根据权利要求4所述的移动物体的定位定姿系统，其特征在于，所述测量控制点为预设的被动式的反射标靶。