一种移动物体的定位定姿方法和系统的制作方法
【技术领域】
[0001] 本发明设及惯性导航技术领域，特别设及一种移动物体的定位定姿方法和系统。
【背景技术】
[0002] 在各类移动测量系统中，定姿定姿都是核屯、任务之一。移动测量中采用的定位定 姿系统(P0S)-般由全球导航卫星系统(GNSS)和惯性导航系统（INS)组合而成，为移动测量 系统及其所搭载的各种任务载荷提供位置、速度和姿态基准。基于全球导航卫星系统和惯 性导航系统组合的定位定姿技术有W下优点：利用高精度GNSS信息可W估计出惯性导航系 统（INS)的巧螺漂移和加速度计零偏等误差参数，从而抑制其误差随时间的积累；利用INS 高采样率和短时高精度的特点，可W为GNSS提供辅助信息，从而使得对GNSS接收机钟差等 误差量的估计更加准确，并使得GNSS接收机可保持较低的跟踪带宽，改善其重新俘获卫星 信号的能力。
[0003] P0S的长期绝对精度主要依赖于GNSS。由于GNSS精度受环境影响较大，使得P0S长 期精度也受环境影响。复杂多变的现实环境给移动测图系统(MMS)应用带来了几大方面的 挑战:在GNSS信号较差的环境中，如高楼林立的城区和多路径效应严重的水域，P0S的绝对 位置精度将会大幅下降至分米级乃至米级，此时已不能满足高精度移动测量要求；在无 GNSS信号的地下空间中，单纯依靠 INS推算的定位定姿误差会随时间快速发散，经过一段时 间后，将超出测量的误差容忍上限。因此，无 GNSS信号的地下空间中，如何限制INS的误差发 散，保持较高的移动测量精度，是MMS应用面临的另一个挑战。
[0004] 传统的用于地下空间定位方法主要包括测量机器人定位，即时定位和测图方法 (SLAM)，超宽带定位方法(UWB似及无线射频识别定位方法(RFID)。
[0005] 测量机器人又称自动全站仪，是一种集自动目标识别、自动照准、自动测角与测 距、自动目标跟踪、自动记录于一体的测量平台。现慢机器人能自动寻找和对准目标，然后 自动量测。测量机器人测量精度高，但寻找目标较为耗时，难W实现高动态连续定位。
[0006] 即时定位和测图方法(SLAM)利用相机或激光雷达等传感器来推算自身的位置，并 构建未知区域的地图或更新已知区域的地图。根据所采用的传感器不同，SLAM算法可W被 分为立体视觉、单眼视觉和深度相机、W及基于雷达扫描传感器等。SLAM主要包含数据关 联、地图匹配、闭环探测W及全局平差四个步骤。SLAM算法首先通过数据帖与帖之间重叠区 域的匹配，获得本帖相对于上一帖的位置增量，然后通过位置增量累加得到本帖相对于初 始时刻的位置。同时SLAM算法会利用数据中的多重重叠和轨迹中的闭合环进行全局平差， 获得全局最优的轨迹和特征地图数据。当机器人进入到一个完全陌生区域时，SLAM会一边 推算自身位置一边绘制出此区域的特征地图。而当机器人进入到一个之前已经行驶过的区 域时，SLAM会通过当前数据与特征地图数据的匹配计算当前自身位置，修正推算结果。
[0007] UWB技术是一种新型的无线通信技术，其频率在3.1~10.6G化之间，其独特特性在 于既能用于通信，也能用于高精度测距。UWB具有对信道衰落不敏感、发射信号功率谱密度 低、低截获能力、系统复杂度低W及高测距精度等优点。UWB技术在测距与定位领域的应用 具有两方面的优势：1)在理论上可W获得厘米级甚至更高的测距精度，在精确定位应用中 极具潜力；2)由于时间分辨率高，UWB具有较强的穿透能力，使其在复杂室内环境中仍能完 成测距和定位。UWB测距是基于到达时间（TOA)或双向延时测距来估计来测量UWB移动站到 基站之间的距离。TOA测距法根据基站信号达到移动站所用时间（乘W光速C来计算移动站 至基站的距离，因此移动站与基站的时间需精确同步。双向延时测距则是移动站向基站的 发送请求测距脉冲，基站收到该请求脉冲后返回发送一个应答脉冲，移动站收到应答脉冲 后根据发送时刻与接收时刻的时间延时来估计移动站与基站的距离，因此移动站与基站间 无需时间精确同步。
[000引 UWB技术需要建立基站和移动站，UWB具有对信道衰落不敏感、发射信号功率谱密 度低、低截获能力、系统复杂度低等优点。理论上可W获得厘米级甚至更高的测距精度。例 如有人在2.5米的范围内得到亚厘米级的精度，在50m的范围内达到0.1-0.15米的精度。但 对于地铁运类长线性目标而言，要实现全路段UWB信号覆盖，需要布设非常多的基站，安装 成本和维护成本都十分高昂。
[0009] -套RFID系统由RFID标签和RFID阅读器组成。RFID定位方式分为两种，一种是标 签移动，阅读器固定，另一种是阅读器移动，标签固定。移动标签方式的RFID定位的基本过 程是：阅读器坐标已知，RFID标签与多个阅读器建立通信，然后采用接收信号强度检测或信 号到达方向或信号到达时间等方法计算RFID到各阅读器之间距离，最后由控制中屯、计算得 到标签的位置。移动阅读器定位方式与移动标签定位方式过程类似，所不同的是在运种方 式中，标定固定，坐标已知，而阅读器搭载在载体上，位置待测。
[0010] 上述方法并不适用于地铁隧道运种长线性空间内的定位。因此，为了保证地下空 间移动测量系统的精度，迫切需要一种适用于地下空间移动Ξ维测量的较低成本、动态、高 精度定位定姿方法。
[0011] 因此，现有的技术还有待改进和提高。
【发明内容】
[0012] 鉴于上述现有技术的不足之处，本发明的目的在于提供一种移动物体的定位定姿 方法和系统，在无卫星导航信号的环境下实现对移动物体的高精度定位定姿。
[001 ；3]为了达至化述目的，本发明采取了 W下技术方案：
[0014] -种移动物体的定位定姿方法，所述方法通过设置在移动物体内的惯性测量单 元、里程计和激光雷达对所述移动物体进行定位和定姿，具体包括如下步骤：
[0015] A、惯性测量单元和里程计测量的数据经过惯性滤波器滤波后得到移动物体的初 始位置、初始速度和初始姿态；
[0016] B、激光雷达对测量控制点进行扫描，将激光雷达测得的距离和角度与所述初始位 置、初始姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐 标；
[0017] C、将所述ii量控制点已知的实际坐标与所述初始ii量坐标求差，得到坐标残差； [0018] D、将所述坐标残差通过扩展卡尔曼滤波计算，计算出惯性测量单元的误差、初始 位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正量;将所述惯性测量单 元的误差反馈到惯性滤波器中；
[0019] E、通过初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差修正 量，对所述初始位置、初始速度、初始姿态进行修正，得到移动物体的位置、速度和姿态。
[0020] 所述的移动物体的定位定姿方法中，所述步骤E之后，还包括步骤:F、对移动物体 的位置、速度、姿态W及滤波信息进行平滑滤波，得到平滑的位置、速度和姿态。
[0021 ]所述的移动物体的定位定姿方法中，所述步骤A具体包括步骤：
[0022] A1、对惯性测量单元测得的数据进行纯惯性推算，得到惯性位置、速度和姿态；
[0023] A2、惯性位置和姿态数据转换得到旋转矩阵，将里程计增量转为航位推算位置增 量；
[0024] A3、惯性位置增量与航位推算位置增量求差，得到位置增量残差，带入到惯性扩展 卡尔曼滤波器中，得到惯性测量单元的误差修正量、W及速度误差修正量和姿态误差修正 量，根据速度误差改正向量和滤波周期得到位置误差修正量；
[0025] A4、将惯性测量单元的误差修正量反馈到惯性滤波器中，而位置、速度和姿态误差 修正量则与惯性位置、速度和姿态相加，得到初始滤波位置、速度和姿态。
[0026] 所述的移动物体的定位定姿方法中，所述测量控制点为预设的被动式的反射标 祀。
[0027] -种移动物体的定位定姿系统，所述系统包括移动物体、惯性测量单元、里程计和 激光雷达，所述惯性测量单元、里程计和激光雷达均设置在移动物体内；所述激光雷达用于 对测量控制点进行扫描，得到测量控制点与移动物体的距离和角度;所述系统还包括：
[0028] 惯性滤波器，用于对惯性测量单元和里程计测量的数据进行惯性滤波，得到移动 物体的初始位置、初始速度和初始姿态；
[0029] 坐标残差计算模块，用于将激光雷达测得的测距测角数据与所述初始位置、初始 姿态融合后，得到初始激光点云，从初始激光点云中提取测量控制点的初始测量坐标;将所 述测量控制点已知的实际坐标与所述初始测量坐标求差，得到坐标残差；
[0030] 惯性扩展卡尔曼滤波器，用于将所述坐标残差通过扩展卡尔曼滤波计算，计算出 惯性测量单元的误差、初始位置的误差修正量、初始速度的误差修正量和初始姿态的误差 修正量;将所述惯性测量单元的误差反馈到惯性滤波器中；通过初始位置的误差修正量、初 始速度的误差修正量和初始姿态的误差修正量，对所述初始位置、初始速度、初始姿态进行 修正，得到移动物体的位置、速度和姿态。
[0031] 所述的移动物体的定位定姿系统中，所述系统还包括：
[0032] RTS平滑滤波器，用于对移动物体的位置、速度、姿态W及滤波信息进行平滑滤