CN105628026A - 一种移动物体的定位定姿方法和系统 - Google Patents

Abstract

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

Description

一种移动物体的定位定姿方法和系统

技术领域

本发明涉及惯性导航技术领域，特别涉及一种移动物体的定位定姿方法和系统。

背景技术

在各类移动测量系统中，定姿定姿都是核心任务之一。移动测量中采用的定位定姿系统(POS)一般由全球导航卫星系统(GNSS)和惯性导航系统(INS)组合而成，为移动测量系统及其所搭载的各种任务载荷提供位置、速度和姿态基准。基于全球导航卫星系统和惯性导航系统组合的定位定姿技术有以下优点：利用高精度GNSS信息可以估计出惯性导航系统(INS)的陀螺漂移和加速度计零偏等误差参数，从而抑制其误差随时间的积累；利用INS高采样率和短时高精度的特点，可以为GNSS提供辅助信息，从而使得对GNSS接收机钟差等误差量的估计更加准确，并使得GNSS接收机可保持较低的跟踪带宽，改善其重新俘获卫星信号的能力。

POS的长期绝对精度主要依赖于GNSS。由于GNSS精度受环境影响较大，使得POS长期精度也受环境影响。复杂多变的现实环境给移动测图系统(MMS)应用带来了几大方面的挑战：在GNSS信号较差的环境中，如高楼林立的城区和多路径效应严重的水域，POS的绝对位置精度将会大幅下降至分米级乃至米级，此时已不能满足高精度移动测量要求；在无GNSS信号的地下空间中，单纯依靠INS推算的定位定姿误差会随时间快速发散，经过一段时间后，将超出测量的误差容忍上限。因此，无GNSS信号的地下空间中，如何限制INS的误差发散，保持较高的移动测量精度，是MMS应用面临的另一个挑战。

传统的用于地下空间定位方法主要包括测量机器人定位，即时定位和测图方法(SLAM)，超宽带定位方法(UWB)以及无线射频识别定位方法(RFID)。

测量机器人又称自动全站仪，是一种集自动目标识别、自动照准、自动测角与测距、自动目标跟踪、自动记录于一体的测量平台。测量机器人能自动寻找和对准目标，然后自动量测。测量机器人测量精度高，但寻找目标较为耗时，难以实现高动态连续定位。

即时定位和测图方法(SLAM)利用相机或激光雷达等传感器来推算自身的位置，并构建未知区域的地图或更新已知区域的地图。根据所采用的传感器不同，SLAM算法可以被分为立体视觉、单眼视觉和深度相机、以及基于雷达扫描传感器等。SLAM主要包含数据关联、地图匹配、闭环探测以及全局平差四个步骤。SLAM算法首先通过数据帧与帧之间重叠区域的匹配，获得本帧相对于上一帧的位置增量，然后通过位置增量累加得到本帧相对于初始时刻的位置。同时SLAM算法会利用数据中的多重重叠和轨迹中的闭合环进行全局平差，获得全局最优的轨迹和特征地图数据。当机器人进入到一个完全陌生区域时，SLAM会一边推算自身位置一边绘制出此区域的特征地图。而当机器人进入到一个之前已经行驶过的区域时，SLAM会通过当前数据与特征地图数据的匹配计算当前自身位置，修正推算结果。

UWB技术是一种新型的无线通信技术，其频率在3.1～10.6GHz之间，其独特特性在于既能用于通信，也能用于高精度测距。UWB具有对信道衰落不敏感、发射信号功率谱密度低、低截获能力、系统复杂度低以及高测距精度等优点。UWB技术在测距与定位领域的应用具有两方面的优势：1)在理论上可以获得厘米级甚至更高的测距精度，在精确定位应用中极具潜力；2)由于时间分辨率高，UWB具有较强的穿透能力，使其在复杂室内环境中仍能完成测距和定位。UWB测距是基于到达时间(TOA)或双向延时测距来估计来测量UWB移动站到基站之间的距离。TOA测距法根据基站信号达到移动站所用时间(乘以光速c来计算移动站至基站的距离，因此移动站与基站的时间需精确同步。双向延时测距则是移动站向基站的发送请求测距脉冲，基站收到该请求脉冲后返回发送一个应答脉冲，移动站收到应答脉冲后根据发送时刻与接收时刻的时间延时来估计移动站与基站的距离，因此移动站与基站间无需时间精确同步。

UWB技术需要建立基站和移动站，UWB具有对信道衰落不敏感、发射信号功率谱密度低、低截获能力、系统复杂度低等优点。理论上可以获得厘米级甚至更高的测距精度。例如有人在2.5米的范围内得到亚厘米级的精度，在50m的范围内达到0.1-0.15米的精度。但对于地铁这类长线性目标而言，要实现全路段UWB信号覆盖，需要布设非常多的基站，安装成本和维护成本都十分高昂。

一套RFID系统由RFID标签和RFID阅读器组成。RFID定位方式分为两种，一种是标签移动，阅读器固定，另一种是阅读器移动，标签固定。移动标签方式的RFID定位的基本过程是：阅读器坐标已知，RFID标签与多个阅读器建立通信，然后采用接收信号强度检测或信号到达方向或信号到达时间等方法计算RFID到各阅读器之间距离，最后由控制中心计算得到标签的位置。移动阅读器定位方式与移动标签定位方式过程类似，所不同的是在这种方式中，标定固定，坐标已知，而阅读器搭载在载体上，位置待测。

上述方法并不适用于地铁隧道这种长线性空间内的定位。因此，为了保证地下空间移动测量系统的精度，迫切需要一种适用于地下空间移动三维测量的较低成本、动态、高精度定位定姿方法。

因此，现有的技术还有待改进和提高。

发明内容

鉴于上述现有技术的不足之处，本发明的目的在于提供一种移动物体的定位定姿方法和系统，在无卫星导航信号的环境下实现对移动物体的高精度定位定姿。

为了达到上述目的，本发明采取了以下技术方案：

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

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

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

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

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

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

所述的移动物体的定位定姿方法中，所述步骤E之后，还包括步骤：F、对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。

所述的移动物体的定位定姿方法中，所述步骤A具体包括步骤：

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

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

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

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

所述的移动物体的定位定姿方法中，所述测量控制点为预设的被动式的反射标靶。

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

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

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

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

所述的移动物体的定位定姿系统中，所述系统还包括：

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

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

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

所述的移动物体的定位定姿系统中，所述测量控制点为预设的被动式的反射标靶。

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

附图说明

图1为本发明提供的移动物体的定位定姿方法中，小车的结构图。

图2为本发明提供的移动物体的定位定姿方法的流程图。

图3为本发明提供的移动物体的定位定姿方法中，LiDAR/IMU/里程计组合算法框图。

图4为本发明提供的移动物体的定位定姿方法中，反射标靶的示意图。

图5为本发明提供的移动物体的定位定姿方法中，IMU/里程计组合位置方差曲线示意图。

图6为现有的IMU/里程计组合流程图。

图7为本发明提供的移动物体的定位定姿方法中，LiDAR/IMU/里程计位置方差曲线示意图。

图8为本发明提供的移动物体的定位定姿系统的结构框图。

具体实施方式

本发明提供一种移动物体的定位定姿方法和系统。为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

请参阅图1，本发明提供一种移动物体的定位定姿方法，所述方法通过设置在移动物体10内的惯性测量单元(即IMU，图中未示出)、里程计30和激光雷达(LiDAR)20对所述移动物体10进行定位和定姿。所述激光雷达20安装在移动物体10上，工作时扫描设置于场景内的测量控制点。测量控制点本身的三维坐标用来提供给移动物体10绝对定位。所述惯性测量单元设置在所述移动物体10内部，接受里程计30的信息，与里程计30一起对移动物体10进行相对定位定姿。所述里程计30安装在所述移动物体10的轮子上，用来记录小车的线性行驶距离。所述移动物体10可以是汽车、火车等交通工具，也可以是其他带有轮子的移动物体，本发明不作限定，在本实施例中，为图1所示的小车。

本发明的使用环境为：地下或室内无卫星导航信号环境，场景内有现成的高精度控制点网或新建立的高精度控制点网。所述高精度控制点网包括多个预先设置好的测量控制点。所述惯性测量单元、里程计30和激光雷达20在高精度时间同步器所建立时间基准下测量和获得数据，并通过本发明提供的移动物体的定位定姿方法(算法)来获得移动物体的高精度位置和姿态。

请参阅图2和图3，本发明提供的移动物体的定位定姿方法，具体包括如下步骤：

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

S20、激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始位置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标。所述测量控制点为预设的被动式的反射标靶。本实施例中，所述反射标靶的形状如图4所示，其为方形或矩形，由两条对角线分割成四个三角形区域，其中，两个相对的三角形区域为深色(黑色)，另外两个相对的三角形区域为浅色(白色)。所述反射标靶价格低廉，容易从点云中提取，其中心点坐标事先采用测量机器人测得。为了便于测量，根据具体的场景，可以调整激光雷达的扫描角度和方向。

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

S40、将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量；将所述惯性测量单元的误差反馈到惯性滤波器中。其中，所述惯性测量单元的误差包括加速度计零偏和陀螺仪漂移。

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

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

进一步的，所述步骤S50之后，还包括步骤S60：对移动物体的位置、速度、姿态以及滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。所述滤波信息包括：增益矩阵、状态转移矩阵、观测矩阵、观测噪声协方差矩阵和系统噪声协方差矩阵。

现有技术中，可以通过惯性测量单元(IMU)/里程计组合模型来对移动物体进行定位定姿，但其误差会随着时间的积累而累积，具体分析如下：

不管是IMU推算还是里程计推算，都是一个累计的过程，在此过程中，误差也会随之累积。决定IMU误差累积速度的两个主要因素分别是加速度计的零偏稳定性和陀螺仪的漂移。高精度IMU价格昂贵，一般在上百万元，中高精度的IMU的价格为数十万元，较低精度的IMU价格也为几万元。目前出现的微机械陀螺仪虽然价格较低，从几元到几万元不等，但其加速度计零偏稳定性差，陀螺漂移大，不适合高精度测量领域。高精度的里程计能达到0.2m/1000m的精度，远优于中高精度IMU推算里程的精度，且其价格较低，只需数千元。因此，利用高精度里程计和中高精度的IMU进行无GNSS环境下的组合定位定姿，无疑是一种既经济又能保证精度的方案。

具体的，在本发明中，首先描述IMU/里程计的组合模型。换而言之，所述步骤S10具体包括：

定义里程计体坐标系与载体坐标系和IMU体坐标系(b系)的轴向保持一致，则里程计输出的里程增量ΔD转为b系中的坐标增量ΔD^b为： ${\mathrm{\ΔD}}^b=\left[\begin{array}{c}{\mathrm{\θ}}_z\\ 1-k\\ {\mathrm{\θ}}_x\end{array}\right]\mathrm{\Δ}D$ (公式4.1)。

公式4.1中，θz和θx分别为里程计体坐标系相对于b系的方位角修正量和俯仰角修正量，k为里程计刻度因子修正量。在IMU/里程计组合时，需要在IMU误差状态向量的基础上增加三维里程计误差状态向量ΔS＝[δk,δθx,δθz]^T(上标“T”表示转置)。因此，组合模型(即，IMU和里程计的组合模型)状态变量为 $X^{,}={\[{\mathrm{\ΔP}}_1,{\mathrm{\ΔV}}_1,{\mathrm{\Δo}}_1,{\mathrm{\Δa}}_1^b,{\mathrm{\Δe}}_1^b,\mathrm{\Δ}S\]}^T$ (公式4.2)；

其中，ΔP₁为初始三维位置误差改正向量(即，初始位置的误差修正量，其为北东地坐标系)，ΔV₁为初始速度的误差改正向量(初始速度的误差修正量)，Δo₁为初始姿态误差改正向量(初始姿态的误差修正量)，Δa₁为初始加速计零偏改正向量，为初始加速计零偏改正向量转化成b系后的量，Δe₁为陀螺仪漂移改正向量，对应为陀螺仪漂移改正向量转化成b系后的量，ΔS为里程计误差状态向量。一般认为里程计误差状态向量为常量，即： $\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{k}\\ \mathrm{\δ}\stackrel{.}{\mathrm{\θ}x}\\ \mathrm{\δ}\stackrel{.}{\mathrm{\θ}z}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$ (公式4.3)。这里，带原点变量表示原变量对时间的微分。以为例，就是δk对时间的微分。

则IMU/里程计组合的系统状态方程为：(公式4.4)。

系数矩阵F'为 $F^{\′}=\left[\begin{array}{cc}F& 0_{3\×3}\\ 0_{3\×15}& 0_{3\×3}\end{array}\right]$ (公式4.5)；

其中，F为惯性系统Φ角系统误差模型中的状态转移矩阵，0_m×n表示m行乘n列零矩阵，即，0_3×3和0_3×15分别为3行3列的零矩阵和3行15列的零矩阵。观测模型的位置增量残差模型：观测量z为：z＝ΔP_I-ΔP_S(公式4.6)；

其中，ΔP_I为一个滤波周期内IMU推算出的位置增量，ΔP_S为利用IMU姿态和里程计推算出的位置增量。

观测方程为：z＝ΔV₁δt-ΔP_S×MΔS+ξ(公式4.7)；

δt为滤波周期，M为里程计参数误差向量的系数矩阵，ξ为等效观测噪声。M的具体形式为 $M_n={\left[\begin{array}{ccc}{\mathrm{\δS}}_x^b& 0& {\mathrm{\δS}}_y^b\\ {\mathrm{\δS}}_y^b& {\mathrm{\δS}}_x^b& -{\mathrm{\δS}}_x^b\\ {\mathrm{\δS}}_x^b& -{\mathrm{\δS}}_y^b& 0\end{array}\right]}_n$ (公式4.8)， $M={\mathrm{\Σ}}_n{C}_{bn}^n{M}_n$ (公式4.9)；

公式4.8中，为一个里程计采样间隔[n-1,n]内里程计在b系X轴方向上的增量，为Y轴方向上的增量；公式4.9中，为b系至导航坐标系n系的旋转矩阵。

这里取状态向量X'去掉位置误差向量后的子向量X”为IMU/里程计组时的状态向量(F'矩阵中也需要去掉相应的行和列，记为F”)，则观测方程为：z＝HX”+ξ(公式4.10)；

ξ为观测噪声，观测矩阵H为：H＝[I_3×3δt,-ΔPs×,0_3×6,M](公式4.11)；

其中，I3×3表示三阶单位矩阵，值得注意的是滤波周期δt的选择。如果δt过小，观测噪声会被放大，而如果周期过长，则速度误差不能被视为常量。因此，滤波周期δt可介于0.2s至2s之间，一般采用0.5秒或1秒的滤波周期较佳。

卡尔曼滤波过程时间更新：

X”_k+1|k＝Γ”_kX”_k(公式4.12)；

Γ”_k＝I+F”Δt(公式4.13)；

${D^{\′\′}}_{k+1|k}={{\mathrm{\Γ}}^{\′\′}}_k{D^{\′\′}}_k{{\mathrm{\Γ}}^{\′\′}}_k^T+Q$ (公式4.14)；

状态更新：

X”_k+1＝X”_k+1|k+X_k+1(Z_k-HX”_k+1|k)(公式4.15)；

增益矩阵：K_k+1＝D_k+1|kH^T(HP_k+1|kH^T+R)^-1(公式4.16)；

协方差矩阵：D”_k+1＝(I-H＝K_k+1H)D”_k+1|k(公式4.17)；

式中，Δt为IMU更新周期，R为观测噪声协方差矩阵，Q为等效系统噪声协方差矩阵。得到IMU的速度误差修正量后，采用公式4.19计算位置误差修正量：ΔP₁＝ΔV₁δt(公式4.18)。

注意到在卡尔曼状态更新中，位置误差方差并没有参与更新。下面进一步考虑IMU/里程计组合时位置误差方差的计算模型。在位置递推过程中，位置误差被建模为一阶马尔科夫过程，其模型如下：

ΔP_i+1＝Γ_p[ΔP_i，ΔV_i]^T+ξ_p(公式4.19)；

$D_{i+1}={\mathrm{\Γ}}_p{D}_{[{\mathrm{\ΔP}}_i,\mathrm{\Δ}{V}_i]}{{\mathrm{\Γ}}_p}^T+Q_p$ (公式4.20)。

其中，Γ_p为系数矩阵，ξ_p为系统模型噪声，Q_p为系统噪声方差矩阵。在滤波时刻，由式(4.19)计算的位置误差修正量被反馈到惯性滤波器，然后被置零。在递推过程中，位置误差保持为零，位置误差方差会呈指数不断增加。考虑IMU/里程计组合的以下三个方面：1)滤波修正后，位置误差不确定性减小，此时方差应该减小；2)IMU/里程计组合系统定位误差不确定性会随着时间增长；3)位置方差的主要作用在于反向平滑时准确地将位置误差改正量分配到其他时刻。因此，在滤波更新时刻，本发明设计了一种随时间平滑增长，但增长速度远小于指数函数的位置方差函数(一种对数模型来计算位置误差的方差)；换而言之，本发明的惯性滤波器在滤波更新时刻，通过对数模型来计算位置误差的方差，具体公式如下：

东方向位置误差协方差矩阵 $\begin{array}{cc}{D}_{P_E}={\mathrm{\σ}}_E^{2}In& (e+t)\end{array}$

北方向位置误差协方差矩阵 $\begin{array}{cc}{D}_{P_N}={\mathrm{\σ}}_N^{2}In& (e+t)\end{array}$

高程误差协方差矩阵 $\begin{array}{cc}{D}_{P_H}={\mathrm{\σ}}_H^{2}In& (e+t)\end{array}$ (公式4.21)。

其中，为东方向方差基值，为北方向方差基值，为高程方差基值。因此，IMU/里程计组合定位方差整体趋势将如图5所示：在一个滤波周期δt内，位置方差随着时间快速增长，在里程计数据修正时刻，位置误差不确定降低，方差也随之降低，但仍比上个滤波时刻要大，既总体上仍呈增长趋势。

最终，IMU/里程计组合的整体流程如图6所示，即，所述步骤S10具体包括：

S110、对惯性测量单元(IMU)测得的数据进行纯惯性推算，得到惯性位置、速度和姿态。

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

S130、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩展卡尔曼滤波器中，得到惯性测量单元的误差修正量(加速度计零偏和陀螺仪漂移)、以及速度误差修正量和姿态误差修正量，根据速度误差改正向量ΔV₁和滤波周期δt得到位置误差修正量ΔP₁，即，利用公式4.18计算得到位置误差修正量ΔP₁。

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

而本发明提供的则是LiDAR/IMU/里程计组合模型。如果没有激光雷达的辅助，误差累积速度会随时间增长。经过一段时间后，误差可能发散到超过测量要求。为了获得长期高精度轨迹，需要引入外部高精度位置信息对累积误差进行修正。因此，所引入的绝对位置精度将决定整个定位定姿系统的绝对精度。本发明提供的移动物体的定位定姿方法，通过在地下空间的测量场地中布设LiDAR控制标靶(测量控制点)，然后利用激光雷达对这些LiDAR控制标靶进行观测，将观测值与真值之间的残差观测值带入到卡尔曼滤波方差中，从而修正IMU/里程计系统的累积误差。

所述步骤S20具体包括：根据公式4.22，可以将激光雷达测量的距离和角度与IMU/里程计组合定位定姿计算出的初始滤波位置与初始滤波姿态数据融合得到初始激光点云。

$X^g=T^g+R_b^g\·(R_l^b\·X^l+T_l^b)$ 公式(4.22)

这里，上标g，b，l分别表示地理空间坐标系、IMU坐标系和激光扫描坐标系，X^g是地理空间坐标系坐标下LiDAR点的坐标向量，T^g是地理空间坐标系下导航中心的坐标向量，是从IMU坐标系到地理空间坐标系的旋转矩阵，由IMU计算出得姿态悈计算得到。X^l是激光扫描观测值在激光坐标系下的的坐标向量，是从激光扫描仪坐标系到IMU坐标系的的平移向量。是从激光扫描仪到IMU坐标系到旋转矩阵，由标定得到的空间同步姿态角计算得到。

所述步骤S30具体包括：利用其它高精度测量手段测得激光雷达控制标靶高精度坐标，用X_L表示，将该值作为真实值，激光雷达控制标靶高精度坐标X_L就是测量控制点已知的实际坐标。X_L与初始激光点云中对应标靶观测量X^g相减得到坐标残差观测量ΔX_G，即ΔX_G＝X_L-X^g公式(4.23)。

所述步骤S40具体包括：一方面，考虑到经过里程计修正后，IMU/里程计组合解算的姿态精度较高；另一方面，从点云中提取控制标靶中心点时，可能会存在较小的偏差，而姿态误差的影响可能被控制点提取误差的影响所掩盖。因此，控制点残差观测值将主要由于定位误差造成。从而，我们得到ΔX_G与IMU/里程计定位定姿误差ΔP₂,Δo₂之间的关系ΔX_G＝ΔP₂+ξ(公式4.24)。

令观测值z₂＝ΔX_G，系统状态向量X为IMU误差状态向量，即：

$X={\left[\begin{array}{ccccc}{\mathrm{\ΔP}}_2& {\mathrm{\ΔV}}_2& {\mathrm{\Δo}}_2& {\mathrm{\Δa}}_2^b& {\mathrm{\Δe}}_2^b\end{array}\right]}^T$ (公式4.25)；

则观测方程为：

z₂＝H₂X+ξ

H₂＝[I_3×3,0_3×12](公式4.26)；

状态方程为： $\stackrel{\·}{X}=\mathrm{FX}+\mathrm{\ξ}$ (公式4.27)。

控制点残差观测值的噪声主要来自于控制点坐标误差以及从点云中提取控制点时的提取误差。控制点误差和提取误差分别服从高斯分布和高斯分布则观测值噪声的分布为在实际作业中，我们可以利用测量机器人将控制点坐标精度控制在0.02m，利用可视化软件将提取精度控制在0.01m，因此，控制点噪声的方程为0.0005m²。

所述步骤S50具体包括：LiDAR/IMU滤波过程与公式4.12～4.15类似。如果LiDAR/IMU滤波时间间隔不等长的，在采用控制点改正后，里程计推算量应清零，重新开始IMU/里程计的滤波周期。否则会出现错误的速度改正量，造成滤波扰乱。图7为LiDAR/IMU/里程计组合时的位置方差曲线示意图：在一个LiDAR/IMU/里程计滤波周期内，位置方差随着时间而增长，在滤波时刻，累积位置误差得到修正，位置方差随之减小。

所述步骤S60具体包括：无论是IMU/里程计组合还是LiDAR/IMU/里程计组合，在滤波修正处位置、速度和姿态都会出现跳动，造成他们在时间上的不连续。对于实时定位导航而言，这种跳动可以被忽略。而对于移动测图而言，这些跳动则需要被消除，以保证轨迹的时间连续性，即需要对轨迹进行平滑处理。平滑的另一个目的在于利用滤波。

如步骤S60所述，无论是IMU/里程计组合还是LiDAR/IMU/里程计组合，在滤波修正处位置、速度和姿态都会出现跳动，造成他们在时间上的不连续。对于实时定位导航而言，这种跳动可以被忽略。而对于移动测图而言，这些跳动则需要被消除，以保证轨迹的时间连续性，即需要对轨迹进行平滑处理。平滑的另一个目的在于利用滤波信息计算其他时刻的位置、速度和姿态误差改正量，从而提高整体轨迹精度。常用的平滑算法有正反向滤波和固定滞后区间反向平滑。这里采用固定区间反向平滑R-T-S算法，其数学模型为：

${\hat{X}}_k^S={\hat{X}}_k+K_k^S({\hat{X}}_{k+1}^S-{\mathrm{\Γ}}_{k+1|k}{\hat{X}}_k)$ (公式4.28)；

D_kS＝D_k+K_kS(D_kS-D_k+1|k)(K_kS)^T(公式4.29)；

$K_k^S=D_k{\mathrm{\Γ}}_{k+1|k}^T{D}_{k+1|k}^{-1}$ (公式4.30)；

其中，为k时刻平滑改正量，为滤波改正量，K_kS平滑增益矩阵，D_kS为平滑后的协方差矩阵。

由此可知，本发明构建了统一的融合LiDAR控制标靶数据、IMU数据以及里程计数据的扩展卡尔曼滤波模型。该模型建立在IMU动力学模型和误差模型基础上，通过将LiDAR控制标靶数据带入到卡尔曼滤波方程中，计算IMU/里程计组合的误差状态向量，限制惯性误差发散，从而得到载体高精度修正位置和姿态。

本发明解决了全地下/室内环境、完全无卫星定位信号环境内的绝对定位定姿。通过控制点引入绝对坐标，无需GNSS信号的定位。

本发明能提供大范围、大跨度室内/地下场所内的高精度的定位定姿姿态结果，这是其他传统方法无法相比的。本发明所提出的算法结合相应的传感器，能提供50mm的绝对定位精度和2mm/10m的相对定位精度。

本发明提供的高精度位置和姿态结果可用于高精度的移动测量，是其他传统方法无法相比的。高精度的定位定姿结果可用于移动测量系统，进行轨道、交通等行业的高精度测量。这是传统方法无法做到的。

本发明的方法无需建立基站和依赖电波信号，因此使用相对灵活、方便、成本低。所依赖的控制点可以是现有的或重新布置的，是一种无源的、无信号发出和接收的静态标志。因此，很容易实现，无需接电源和不受电磁信号干扰，使用灵活方便。

本发明解决了无卫星定位信号环境内的高精度定位定姿，为相应行业的高精度测量提供了必要条件。

基于上述实施例提供的移动物体的定位定姿方法，本发明还提供一种移动物体的定位定姿系统。请参阅图8，所述系统包括：如上所述的移动物体、惯性测量单元40、里程计30和激光雷达20，所述惯性测量单元40、里程计30和激光雷达20均设置在移动物体内；所述激光雷达20用于对测量控制点进行扫描，得到测量控制点与移动物体的距离和角度；所述系统还包括：惯性滤波器50、坐标残差计算模块60、惯性扩展卡尔曼滤波器70和RTS平滑滤波器。

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

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

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

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

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

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

由于所述移动物体的定位定姿系统的定位定姿原理以及技术特征在上述实施例中已详细阐述，在此不再赘述。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims (8)

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所述的移动物体的定位定姿系统，其特征在于，所述测量控制点为预设的被动式的反射标靶。