波， 得到平滑的位置、速度和姿态。
[0033] 所述的移动物体的定位定姿系统中，所述惯性扩展卡尔曼滤波器，还用于根据惯 性滤波器输出的位置增量残差，得到惯性测量单元的误差修正量、W及速度误差修正量和 姿态误差修正量，根据速度误差改正向量和滤波周期得到位置误差修正量;将惯性测量单 元的误差修正量反馈到惯性滤波器中；
[0034] 所述惯性滤波器具体用于对惯性测量单元测得的数据进行纯惯性推算，得到惯性 位置、速度和姿态;将惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算 位置增量;惯性位置增量与航位推算位置增量求差，得到位置增量残差，将所述位置增量残 差输出给惯性扩展卡尔曼滤波器;根据惯性扩展卡尔曼滤波器反馈的惯性测量单元的误差 修正量;将位置、速度和姿态误差修正量与惯性位置、速度和姿态相加，得到初始滤波位置、 速度和姿态。
[0035] 所述的移动物体的定位定姿系统中，所述测量控制点为预设的被动式的反射标 祀。
[0036] 相较于现有技术，本发明提供的一种移动物体的定位定姿方法和系统，通过惯性 测量单元、里程计和激光雷达，构建统一的、融合激光雷达控制标祀数据、惯性测量单元数 据W及里程计数据的扩展卡尔曼滤波模型。该模型建立在惯性测量单元动力学模型和误差 模型基础上，通过将激光雷达控制标祀数据带入到卡尔曼滤波方程中，计算IMU/里程计组 合的误差状态向量，限制其误差发散，从而得到高精度位置和姿态。从而实现了在无卫星导 航信号的环境下，对移动物体的高精度定位定姿。
【附图说明】
[0037] 图1为本发明提供的移动物体的定位定姿方法中，小车的结构图。
[0038] 图2为本发明提供的移动物体的定位定姿方法的流程图。
[0039] 图3为本发明提供的移动物体的定位定姿方法中，LiDAR/IMU/里程计组合算法框 图。
[0040] 图4为本发明提供的移动物体的定位定姿方法中，反射标祀的示意图。
[0041] 图5为本发明提供的移动物体的定位定姿方法中，IMU/里程计组合位置方差曲线 示意图。
[0042] 图6为现有的IMU/里程计组合流程图。
[0043] 图7为本发明提供的移动物体的定位定姿方法中，LiDAR/IMU/里程计位置方差曲 线示意图。
[0044] 图8为本发明提供的移动物体的定位定姿系统的结构框图。
【具体实施方式】
[0045] 本发明提供一种移动物体的定位定姿方法和系统。为使本发明的目的、技术方案 及效果更加清楚、明确，W下参照附图并举实施例对本发明进一步详细说明。应当理解，此 处所描述的具体实施例仅用W解释本发明，并不用于限定本发明。
[0046] 请参阅图1，本发明提供一种移动物体的定位定姿方法，所述方法通过设置在移动 物体10内的惯性测量单元（即IMU，图中未示出）、里程计30和激光雷达化iDAR)20对所述移 动物体10进行定位和定姿。所述激光雷达20安装在移动物体10上，工作时扫描设置于场景 内的测量控制点。测量控制点本身的Ξ维坐标用来提供给移动物体10绝对定位。所述惯性 测量单元设置在所述移动物体10内部，接受里程计30的信息，与里程计30-起对移动物体 10进行相对定位定姿。所述里程计30安装在所述移动物体10的轮子上，用来记录小车的线 性行驶距离。所述移动物体10可W是汽车、火车等交通工具，也可W是其他带有轮子的移动 物体，本发明不作限定，在本实施例中，为图1所示的小车。
[0047] 本发明的使用环境为:地下或室内无卫星导航信号环境，场景内有现成的高精度 控制点网或新建立的高精度控制点网。所述高精度控制点网包括多个预先设置好的测量控 制点。所述惯性测量单元、里程计30和激光雷达20在高精度时间同步器所建立时间基准下 测量和获得数据，并通过本发明提供的移动物体的定位定姿方法(算法)来获得移动物体的 高精度位置和姿态。
[0048] 请参阅图2和图3,本发明提供的移动物体的定位定姿方法，具体包括如下步骤：
[0049] S10、惯性测量单元和里程计测量的数据经过惯性滤波器滤波后得到移动物体的 初始位置、初始速度和初始姿态。
[0050] S20、激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始 位置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量 坐标。所述测量控制点为预设的被动式的反射标祀。本实施例中，所述反射标祀的形状如图 4所示，其为方形或矩形，由两条对角线分割成四个Ξ角形区域，其中，两个相对的Ξ角形区 域为深色(黑色），另外两个相对的Ξ角形区域为浅色（白色）。所述反射标祀价格低廉，容易 从点云中提取，其中屯、点坐标事先采用测量机器人测得。为了便于测量，根据具体的场景， 可W调整激光雷达的扫描角度和方向。
[0051] S30、将所述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残 差。
[0052] S40、将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初 始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量;将所述惯性测量 单元的误差反馈到惯性滤波器中。其中，所述惯性测量单元的误差包括加速度计零偏和巧 螺仪漂移。
[0053] S50、通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正 量，对所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态。
[0054] 由此可知，本发明通过惯性测量单元、里程计和激光雷达，构建统一的、融合了Ξ 者的测量数据的扩展卡尔曼滤波模型。该模型建立在惯性测量单元动力学模型和误差模型 基础上，通过将激光雷达控制标祀数据带入到卡尔曼滤波方程中，计算IMU/里程计组合的 误差状态向量，限制其误差发散，从而得到高精度位置和姿态。从而实现了在无卫星导航信 号的环境下，对移动物体的高精度定位定姿。
[0055] 进一步的，所述步骤S50之后，还包括步骤S60:对移动物体的位置、速度、姿态W及 滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。所述滤波信息包括:增益矩阵、状态 转移矩阵、观测矩阵、观测噪声协方差矩阵和系统噪声协方差矩阵。
[0056] 现有技术中，可W通过惯性测量单元(IMU)/里程计组合模型来对移动物体进行定 位定姿，但其误差会随着时间的积累而累积，具体分析如下：
[0057] 不管是IMU推算还是里程计推算，都是一个累计的过程，在此过程中，误差也会随 之累积。决定IMU误差累积速度的两个主要因素分别是加速度计的零偏稳定性和巧螺仪的 漂移。高精度IMU价格昂贵，一般在上百万元，中高精度的IMU的价格为数十万元，较低精度 的IMU价格也为几万元。目前出现的微机械巧螺仪虽然价格较低，从几元到几万元不等，但 其加速度计零偏稳定性差，巧螺漂移大，不适合高精度测量领域。高精度的里程计能达到 0.2m/1000m的精度，远优于中高精度IMU推算里程的精度，且其价格较低，只需数千元。因 此，利用高精度里程计和中高精度的IMU进行无 GNSS环境下的组合定位定姿，无疑是一种既 经济又能保证精度的方案。
[0058] 具体的，在本发明中，首先描述IMU/里程计的组合模型。换而言之，所述步骤S10具 体包括：
[0059] 定义里程计体坐标系与载体坐标系和IMU体坐标系(b系）的轴向保持一致，则里程 计输出的里程增量A D转为b系中的坐标增量Δ 〇b为：
：公式4.1)。
[0060] 公式4.1中，θζ柳X分别为里程计体坐标系相对于b系的方位角修正量和俯仰角修正量， k为里程计刻度因子修正量。在IMU/里程计组合时，需要在IMU误差状态向量的基础上增加 Ξ 维里程计误差状态向量AS=[δk，δθχ，δθz]τ(上标?'表示转置）。因此，组合模型（即，IMU 和里程计的组合模型）状态变量为
（公式 4.2)；
[0061] 其中，Δ Pi为初始Ξ维位置误差改正向量（即，初始位置的误差修正量，其为北东 地坐标系），A Vi为初始速度的误差改正向量(初始速度的误差修正量），Δ 01为初始姿态误 差改正向量(初始姿态的误差修正量），A ai为初始加速计零偏改正向量，Δ结j6为初始加速 计零偏改正向量转化成b系后的量，Aei为巧螺仪漂移改正向量，Aef对应为巧螺仪漂移 改正向量转化成b系后的量，Δ S为里程计误差状态向量。一般认为里程计误差状态向量为 常量，即
：公式4.3)。运里，带原点变量表示原变量对时间的微分。W巧基为 例，吝皮就是化对时间的微分。
[0062] 贝IJIMU/里程计组合的系统状态方程为：
公式4.4)。
[0063] 系数矩阵F'^
(公式4.5);
[0064] 其中，F为惯性系统Φ角系统误差模型中的状态转移矩阵，Omxn表示m行乘η列零矩 阵，即，〇3Χ3和化Χ15分别为3行3列的零矩阵和3行15列的零矩阵。观测模型的位置增量残差模 型:观测量z为:z=ΔP广ΔPs(公式4.6);
[0065] 其中，ΑΡι为一个滤波周期内IMU推算出的位置增量，APs为利用IMU姿态和里程计 推算出的位置增量。
[0066] 观测方程为：Z = Δ ViSt- Δ Ps