卡尔曼滤波方程中，计算IMU/里程计组合的误差状态向量，限制惯性 误差发散，从而得到载体高精度修正位置和姿态。
[0115] 本发明解决了全地下/室内环境、完全无卫星定位信号环境内的绝对定位定姿。通 过控制点引入绝对坐标，无需GNSS信号的定位。
[0116] 本发明能提供大范围、大跨度室内/地下场所内的高精度的定位定姿姿态结果，运 是其他传统方法无法相比的。本发明所提出的算法结合相应的传感器，能提供50mm的绝对 定位精度和2mm/10m的相对定位精度。
[0117] 本发明提供的高精度位置和姿态结果可用于高精度的移动测量，是其他传统方法 无法相比的。高精度的定位定姿结果可用于移动测量系统，进行轨道、交通等行业的高精度 测量。运是传统方法无法做到的。
[0118] 本发明的方法无需建立基站和依赖电波信号，因此使用相对灵活、方便、成本低。 所依赖的控制点可W是现有的或重新布置的，是一种无源的、无信号发出和接收的静态标 志。因此，很容易实现，无需接电源和不受电磁信号干扰，使用灵活方便。
[0119] 本发明解决了无卫星定位信号环境内的高精度定位定姿，为相应行业的高精度测 量提供了必要条件。
[0120] 基于上述实施例提供的移动物体的定位定姿方法，本发明还提供一种移动物体的 定位定姿系统。请参阅图8,所述系统包括:如上所述的移动物体、惯性测量单元40、里程计 30和激光雷达20，所述惯性测量单元40、里程计30和激光雷达20均设置在移动物体内；所述 激光雷达20用于对测量控制点进行扫描，得到测量控制点与移动物体的距离和角度;所述 系统还包括:惯性滤波器50、坐标残差计算模块60、惯性扩展卡尔曼滤波器70和RTS平滑滤 波器。
[0121] 所述惯性滤波器50,用于对惯性测量单元40和里程计30测量的数据进行惯性滤 波，得到移动物体的初始位置、初始速度和初始姿态。
[0122] 所述坐标残差计算模块60,用于将激光雷达20测得的测距测角数据与所述初始位 置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐 标;将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差。所述测量 控制点为预设的被动式的反射标祀。
[0123] 所述惯性扩展卡尔曼滤波器70,用于将所述坐标残差通过扩展卡尔曼滤波计算， 计算出惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态 的误差修正量;将所述惯性测量单元的误差反馈到惯性滤波器中；通过初始位置的误差修 正量、初始速度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始 姿态进行修正，得到移动物体的位置、速度和姿态。
[0124] 所述RTS平滑滤波器80,用于对移动物体的位置、速度、姿态W及滤波信息进行平 滑滤波，得到平滑的位置、速度和姿态。
[0125] 所述惯性扩展卡尔曼滤波器70,还用于根据惯性滤波器50输出的位置增量残差， 得到惯性测量单元的误差修正量、W及速度误差修正量和姿态误差修正量，根据速度误差 改正向量和滤波周期得到位置误差修正量;将惯性测量单元的误差修正量反馈到惯性滤波 器50中；
[0126] 所述惯性滤波器50具体用于对惯性测量单元测得的数据进行纯惯性推算，得到惯 性位置、速度和姿态;将惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推 算位置增量;惯性位置增量与航位推算位置增量求差，得到位置增量残差，将所述位置增量 残差输出给惯性扩展卡尔曼滤波器;根据惯性扩展卡尔曼滤波器70反馈的惯性测量单元的 误差修正量;将位置、速度和姿态误差修正量与惯性位置、速度和姿态相加，得到初始滤波 位置、速度和姿态。
[0127] 由于所述移动物体的定位定姿系统的定位定姿原理W及技术特征在上述实施例 中已详细阐述，在此不再寶述。
[0128] 可W理解的是，对本领域普通技术人员来说，可W根据本发明的技术方案及其发 明构思加 W等同替换或改变，而所有运些改变或替换都应属于本发明所附的权利要求的保 护范围。
【主权项】
1. 一种移动物体的定位定姿方法，其特征在于，所述方法通过设置在移动物体内的惯 性测量单元、里程计和激光雷达对所述移动物体进行定位和定姿，具体包括如下步骤： A、 惯性测量单元和里程计测量的数据经过惯性滤波器滤波后得到移动物体的初始位 置、初始速度和初始姿态； B、 激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始位置、 初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标； C、 将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差； D、 将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始位置 的误差修正量、初始速度的误差修正量和初始姿态的误差修正量;将所述惯性测量单元的 误差反馈到惯性滤波器中； E、 通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量，对 所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态。2. 根据权利要求1所述的移动物体的定位定姿方法，其特征在于，所述步骤E之后，还包 括步骤:F、对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到平滑的位置、速 度和姿态。3. 根据权利要求1所述的移动物体的定位定姿方法，其特征在于，所述步骤A具体包括 步骤： A1、对惯性测量单元测得的数据进行纯惯性推算，得到惯性位置、速度和姿态； A2、惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置增量； A3、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩展卡尔 曼滤波器中，得到惯性测量单元的误差修正量、以及速度误差修正量和姿态误差修正量，根 据速度误差改正向量和滤波周期得到位置误差修正量； A4、将惯性测量单元的误差修正量反馈到惯性滤波器中，而位置、速度和姿态误差修正 量则与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿态。4. 根据权利要求1所述的移动物体的定位定姿方法，其特征在于，所述测量控制点为预 设的被动式的反射标靶。5. -种移动物体的定位定姿系统，其特征在于，所述系统包括移动物体、惯性测量单 元、里程计和激光雷达，所述惯性测量单元、里程计和激光雷达均设置在移动物体内；所述 激光雷达用于对测量控制点进行扫描，得到测量控制点与移动物体的距离和角度;所述系 统还包括： 惯性滤波器，用于对惯性测量单元和里程计测量的数据进行惯性滤波，得到移动物体 的初始位置、初始速度和初始姿态； 坐标残差计算模块，用于将激光雷达测得的测距测角数据与所述初始位置、初始姿态 融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标;将所述测 量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差； 惯性扩展卡尔曼滤波器，用于将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性 测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正 量;将所述惯性测量单元的误差反馈到惯性滤波器中；通过初始位置的误差修正量、初始速 度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始姿态进行修 正，得到移动物体的位置、速度和姿态。6. 根据权利要求4所述的移动物体的定位定姿系统，其特征在于，所述系统还包括： RTS平滑滤波器，用于对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到 平滑的位置、速度和姿态。7. 根据权利要求5所述的移动物体的定位定姿系统，其特征在于，所述惯性扩展卡尔曼 滤波器，还用于根据惯性滤波器输出的位置增量残差，得到惯性测量单元的误差修正量、以 及速度误差修正量和姿态误差修正量，根据速度误差改正向量和滤波周期得到位置误差修 正量;将惯性测量单元的误差修正量反馈到惯性滤波器中； 所述惯性滤波器具体用于对惯性测量单元测得的数据进行纯惯性推算，得到惯性位 置、速度和姿态;将惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位 置增量;惯性位置增量与航位推算位置增量求差，得到位置增量残差，将所述位置增量残差 输出给惯性扩展卡尔曼滤波器;根据惯性扩展卡尔曼滤波器反馈的惯性测量单元的误差修 正量;将位置、速度和姿态误差修正量与惯性位置、速度和姿态相加，得到初始滤波位置、速 度和姿态。8. 根据权利要求5所述的移动物体的定位定姿系统，其特征在于，所述测量控制点为预 设的被动式的反射标靶。
【专利摘要】本发明公开了一种移动物体的定位定姿方法和系统，通过惯性测量单元、里程计和激光雷达，构建统一的、融合激光雷达控制标靶数据、惯性测量单元数据以及里程计数据的扩展卡尔曼滤波模型。该模型建立在惯性测量单元动力学模型和误差模型基础上，通过将激光雷达控制标靶数据带入到卡尔曼滤波方程中，计算IMU/里程计组合的误差状态向量，限制其误差发散，从而得到高精度位置和姿态。从而实现了在无卫星导航信号的环境下，对移动物体的高精度定位定姿。
【IPC分类】G01C21/16
【公开号】CN105628026
【申请号】CN201610126054
【发明人】李清泉, 张亮, 毛庆洲, 刘勇, 陈智鹏, 熊智敏
【申请人】深圳大学
【公开日】2016年6月1日
【申请日】2016年3月4日