技术领域
[0001] 本
发明涉及车辆高
精度定位技术领域，特别是一种融合北斗与多传感器的机场行李牵引车定位装置与方法。
背景技术
[0002] 随着“工业4.0时代”的到来，数字信息化、智能化技术、
云计算、
大数据、
人工智能等信息技术谋求发展空间，
支撑行业运行安全、优化
能源消耗、方便旅客出行的理念已逐步成为行业共识。在此背景下，2018年民航局提出实施新时代民航高
质量发展战略，加快推进以“平安机场、绿色机场、智慧机场、人文机场”为核心的“四型机场”建设，着
力打造运营集内在品质和外在品位于一体的现代化民用机场，要求我们从过去注重数量、总量、增量的量优式发展，转向注重质量、效率、效益的质优式发展；其中，智慧机场正是推进“四型机场”建设的关键支撑和实施路径。
[0003] 根据《2018年民航机场生产统计
公报》数据显示：2018年我国机场全年旅客运输量超过12亿人次，比上年增长10.2％，完成货邮运输量1674万吨，比上年增长3.5％。随着航空运输业务量的不断增长，我国大部分枢纽机场和干线机场保障能力面临着越来越大的压力，造成机场时隙资源使用紧张，空中交通拥堵，航班延误严重，服务质量下降。据不完全统计，每年中国航班延误直接损失超过500亿元。影响航班延误的可控因素中，存在着机场地面保障服务不力、机场服务能力急需提升等问题。这主要是由于机场拥有的特种车辆数较少，特种车辆定位精度低，特种车辆服务调度不佳等原因造成的。
[0004]
机车地勤特种车辆的精确调度对提高航班正点率、保障飞行安全、提高民航服务质量和经济效益至关重要。精确定位技术是地勤特种车辆的精确调度的关键技术之一，是实现车辆安全通行的重要保障。虽然不同的特种车辆对定位精度要求不同，但定位的连续性是地勤特种车辆业务安全可靠的必要前提，考虑到环境(遮挡、光线、天气)、成本以及
稳定性等因素，单纯采用某一种定位技术并不能满足地勤特种车辆的精确调度业务的定位需求。通常需要通过多种技术的融合来实现精准定位，包括GNSS、无线电(例如蜂窝网、局域网等)、惯性测量单元(Inertial Measurement Unit,IMU)、传感器以及高精度地图。其中，GNSS或其差分补偿RTK(Real-time Kinematic)，是最基本的定位方法。考虑到GNSS技术在遮挡场景、隧道以及室内的不稳定(或不可用)，其应用场景受限于室外环境。通常，GNSS或传感器等单一技术难以满足现实复杂环境中车辆高精度定位的要求，无法保证行李牵引车定位的连续性和稳定性。
发明内容
[0005] 本发明所要解决的技术问题是克服
现有技术的不足而提供一种北斗与多传感器融合的机场行李牵引车定位装置与方法，实现对行李牵引车的高精度定位。
[0006] 本发明为解决上述技术问题采用以下技术方案：
[0007] 根据本发明提出的一种融合北斗与多传感器的机场行李牵引车定位装置，包括北斗基准站、北斗双天线定位定向系统、惯导测量单元IMU、双目相机测量单元、通讯模
块、地图存储单元和处理器；其中，
[0008] 北斗基准站，安置于室外空旷环境，用于接收北斗卫星
信号，输出差分改正信息通过通讯模块输出至北斗双天线定位定向系统；
[0009] 北斗双天线定位定向系统，用于在室外环境时，将收到的差分改正信息和采集到的北斗
卫星信号处理得到行李牵引车的
位置、航向和
俯仰角度经通讯模块输出至处理器；
[0010] 惯导测量单元，用于将测量出的
加速度值和
角速度值经通讯模块输出至处理器；
[0011] 双目相机测量单元，用于采集图片并处理得到
颜色和距离信息经通讯模块输出至处理器；
[0012] 地图存储单元，用于存储事先得到行李牵引车行驶环境的三维地图，地图存储单元存储于处理器中；
[0013] 处理器，用于将收到的加速度值和角速度值进行处理，从而得到位置与
姿态信息；用于调用地图存储单元的三维地图；用于将收到的颜色和距离信息结合三维地图进行匹配，得到位置信息，并根据实测信息，对地图进行更新；用于接收北斗双天线定位定向系统输出的位置、航向和俯仰角度；用于将接收到的各类信息通过自适应Kalman
滤波器建立函数模型误差的拟合模型，从而得到位置与姿态信息的最优解。
[0014] 作为本发明所述的一种融合北斗与多传感器的机场行李牵引车定位装置进一步优化方案，北斗基准站布设在机场周围空旷、地面
基础设施沉降稳定的环境下，通过前期与IGS
站点联测，并采用GAMIT
软件进行求解，得到坐标；北斗基准站用于接收北斗卫星信号，并根据坐标计算差分改正数；机场周围是指离机场中心距离小于10km。
[0015] 作为本发明所述的一种融合北斗与多传感器的机场行李牵引车定位装置进一步优化方案，北斗双天线定位定向系统为双天线模式，用于实现实时定位和定向功能；在室外环境下，主天线与主接收机利用差分改正数求解得到主天线的位置信息，此位置信息即为行李牵引车的位置信息，副天线与副接收机利用差分改正数求解得到副天线的位置信息，根据主、副天线的向量关系，从而得到行李牵引车航向和俯仰角度。
[0016] 作为本发明所述的一种融合北斗与多传感器的机场行李牵引车定位装置进一步优化方案，地图存储单元包括两类信息：第一类是道路数据，道路数据包括车道线的位置、类型、宽度、坡度和
曲率；第二类是车道周边的固定对象信息，具体包括交通标志、交通信号灯、车道限高、下
水道口、障碍物、高架物体、防护栏、数目、道路边缘类型、路边地标；利用地图匹配将车辆位置定位在车道上。
[0017] 作为本发明所述的一种融合北斗与多传感器的机场行李牵引车定位装置进一步优化方案，处理器用于存储事先测定的三维地图以及物体属性；处理器用于多传感器的数据融合，并解算行李牵引车的位置和姿态信息；室外环境下，室外环境且北斗卫星可见数超过3颗，处理器将选择利用北斗双天线定位定向系统所输出的位置、航向角和俯仰角与IMU所输出的加速度和角速度信息进行松组合定位，经自适应Kalman滤波求解出行李牵引车的最优解，双目相机根据自身位置计算拍摄到的物体在视觉空间中的位置，并结合地图存储单元对地图进行实时更新，并对IMU仪器误差进行校正；室内环境下或北斗卫星可见数小于4颗的室外环境，处理器将选择利用双目相机拍摄到的图像进行得到自身的位置信息与利用IMU所输出的加速度和角速度信息以及地图匹配，并经自适应Kalman滤波求解出行李牵引车的最优解。
[0018] 一种融合北斗与多传感器的机场行李牵引车定位方法，包括如下步骤：
[0019] 步骤1、室外环境下且北斗卫星可见数超过3颗，利用主天线和副天线以及北斗基准站输入的差分改正数进行主天线和副天线的定位，定位方程为：
[0020]
[0021] 式中：λ为
波长， 为载波模糊度，LP、LΦ分别伪距双差、载波宽巷双差与几何距离双差之差，I为单位阵，VP、VΦ分别为伪距和载波残差，ΔX为坐标增量，A为坐标增量的系数矩阵；
[0022] 将 浮点解及其协方差阵代入LAMBDA方法求解到达 固定解，进而得到主天线和副天线的坐标增量参数；
[0023] 将主天线得到的位置信息作为行李牵引车的位置信息，以主天线为原点建立站心
坐标系，根据主天线和副天线的基线向量信息，解算出副天线在该站心坐标系下的坐标(e,n,u)，e、n、u分别为站心坐标系下东、北、天方向上的坐标分量，解算出行李牵引车航向角y和俯仰角p:
[0024] y＝-arctan(e/n)
[0025]
[0026] 步骤2、将北斗双天线定位得到的位置与航向角y和俯仰角p，与IMU所得到的加速度与角速度信息进行松组合；其中，系统状态选取位置误差、速度误差和姿态误差，加速度器件误差和
陀螺仪器件误差也被选取作为增广状态量，用于在线标定和补偿IMU误差；松组合kalman滤波器为：
[0027]
[0028] 式中：X(t)表示系统误差向量， 为X(t)的导数；F(t)表示系统的状态转移矩阵；w(t)表示系统噪声矩阵；G(t)表示系统噪声的驱动阵；
[0029] 系统的观测方程表示为：
[0030] Z(t)＝H(t)X(t)+V(t)
[0031] 式中：Z(t)为观测量，包括IMU通过加速度所得到的位置信息和北斗双天线所得到的位置信息之差，包括IMU通过加速度所得到的速度信息和北斗双天线所得到的位置求导得到的速度信息之差，还包括IMU通过角速度所得到的姿态信息与北斗双天线所得到的姿态信息之差；H(t)为系数矩阵；V(t)为观测噪声矩阵；
[0032] 利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，并根据解算结果，对IMU仪器误差进行校正；
[0033] 步骤3、双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的
视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图；
[0034] 步骤4、双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图；
[0035] 步骤5、室内环境下或北斗卫星可见数小于4颗的室外环境，IMU是配合双目相机，并利用地图进行匹配定位；通过IMU得到的载体
位姿，为图片姿态提供定位，经过传感器间的外部标定，将对应的信息投射到全局三维坐标系；双目相机与地图匹配获取行李牵引车在其运动范围下的全局初始位置，该位置作为初始时刻IMU定位的初始位置，并初始行李牵引车的姿态信息；IMU对所测得的角速度进行积分得到行李牵引车转动的角度，并结合行李牵引车的初始姿态获得运动坐标系下行李牵引车各个方向上的加速度信息；对运动坐标系下行李牵引车各个方向上的加速度进行积分获得行李牵引车的位置变化；将IMU初始位置加上位置位移，获得行李牵引车相邻两时刻之间的实时位置；将当前时刻的视觉定位下行李牵引车的位置信息和IMU定位下行李牵引车的位置信息以及三维地图进行融合，利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，并更新IMU的初始位置；双目相机根据已得到自身位置信息，结合所拍摄的图片，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图。
[0036] 作为本发明所述的一种融合北斗与多传感器的机场行李牵引车定位方法进一步优化方案，双目相机在室外环境下且北斗卫星可见数超过3颗，是利用拍摄的图片对地图进行更新；双目相机在室内环境下或北斗卫星可见数小于4颗的室外环境，是融合IMU和地图匹配进行定位，辅助功能是利用拍摄的图片计算其空间位置，以实现对地图进行更新。
[0037] 本发明采用以上技术方案与现有技术相比，具有以下技术效果：
[0038] (1)相对于现有的技术而言，本发明通过行李牵引车车载的北斗双天线测量其位置和姿态信息，IMU惯导测量单元通过测量的加速度值和角速度值计算行李牵引车的位置和姿态信息，双目相机利用所获得的两幅图像直接计算出物体深度值，并与高精度地图匹配得到行李牵引车的位置信息。在对各传感器函数模型误差充分识别的基础上，通过自适应Kalman滤波器建立函数模型误差的拟合模型，并实时或准实时地
修改原有的函数模型，使其适应行李牵引车特殊场景和相应传感器，得到位置与姿态信息的最优解，从而实现行李牵引车连续、高精度、高可靠性的室内外无缝定位；
[0039] (2)本发明采用车载北斗双天线定位定向、IMU惯性测量单元、双目相机及高精度地图相结合的方式进行行李牵引车的定位定向，最主要的优势在于：①实现了室内、室外的无缝高精度定位与测姿；②IMU数据与其他传感器相结合能提高整体定位精度和连续性；③双目相机和高精度地图的引入，不仅可以解决行李牵引车的定位问题，还可以根据双目相机观测到的物体的位置信息，对高精度地图进行实时更新；
[0040] (3)本发明
专利旨在研究机场行李牵引车室内外无缝高精度定位技术，提出基于北斗+多传感器的车辆高精度定位装置及相应关键技术，为纯室内和纯室外运行的其他车辆，如航站楼内应急车、巡逻车、摆渡车、飞机牵引车、清水车、飞机专用除
冰车、消防救援车等地勤车的精确定位提供技术移植；将为“四型机场”的实现提供基础技术支撑，同时，这也是“智慧机场”、“平安机场”建设的主要组成部分。
附图说明
[0041] 图1是本发明行李牵引车的车载定位装置实时例的结构示意图。
[0042] 图2是本发明行李牵引车的车载定位装置处于工作状态时，车载定位装置的安装示意图。
[0043] 图3是本发明行李牵引车的车载定位
算法流程图。
具体实施方式
[0044] 为了使本发明的目的，技术方案和优点更加清楚，下面结合附图对本发明的技术方案做进一步的详细说明：
[0045] 参照图1，图1是本发明行李牵引车的车载定位装置实时例的结构示意图，包括：北斗基准站、北斗双天线定位定向系统、IMU惯导测量单元、双目相机测量单元、通讯模块、地图存储单元和处理器。
[0046] 其中，北斗基准站，安置于室外空旷环境，用于接收北斗卫星信号，输出差分改正数通过通讯模块输出至北斗双天线定位定向系统；
[0047] 北斗双天线定位定向系统，用于室外环境，将收到的差分改正信息和采集到的北斗卫星
信号处理得到行李牵引车的位置、航向和俯仰角度经通讯模块输出至处理器；
[0048] IMU惯导测量单元，用于将测量出的加速度值和角速度值经通讯模块输出至处理器；
[0049] 双目相机测量单元，用于采集图片并处理得到颜色和距离信息经通讯模块输出至处理器；
[0050] 地图存储单元，用于存储事先得到行李牵引车行驶环境的三维地图，并存储于处理器；
[0051] 处理器，用于将收到的加速度值和角速度值进行处理，从而得到位置与姿态信息；用于调用地图存储单元的三维地图；用于将收到的颜色和距离信息结合三维地图进行匹配，得到位置信息，并根据实测信息，对地图进行更新；用于接收北斗双天线定位定向系统输出的位置和姿态信息；用于将接收到的各类信息通过自适应Kalman滤波器建立函数模型误差的拟合模型，使其适应行李牵引车特殊场景和相应传感器，得到位置与姿态信息的最优解，从而得到连续、高精度、高可靠行李牵引车的位置与姿态信息。
[0052] 参照图2所示，将车载定位装置中的各结构部件正确的安装。北斗基准站通过通讯模块向北斗双天线定位定向系统发送差分改正数，北斗双天线定位定向系统、IMU惯导测量单元、双目相机通过数据线分别与通讯模块相连接，地图存储单元存储于处理器，通讯模块和处理器可实现双向通讯，处理器对各传感器所输出的数据进行融合，最终实现行李牵引车的定位与测姿以及对三维地图的更新；
[0053] 参照图3，图3是本发明行李牵引车的车载定位算法流程图。本发明还公开了一种机场行李牵引车精确定位方法的
实施例。该方法基于行李牵引车精确定位装置，装置包括：北斗基准站、北斗双天线定位定向系统、IMU惯导测量单元、双目相机测量单元、地图存储单元、通讯模块和处理器；该方法得实施包括如下步骤：
[0054] 步骤1：室外环境下(北斗卫星可见数超过3颗)，利用主天线和副天线以及北斗基准站通过通讯模块输入的差分改正数进行主天线和副天线的定位，定位方程为：
[0055]
[0056] 式中：λ为波长； 为载波模糊度；LP、LΦ分别伪距双差、载波宽巷双差与几何距离双差之差；I为单位阵；VP、VΦ分别为伪距和载波残差；ΔX为坐标增量；A为坐标增量的系数矩阵。
[0057] 将 浮点解及其协方差阵代入LAMBDA方法求解到达 固定解，进而得到主天线和副天线的坐标增量参数。
[0058] 将主天线得到的位置信息作为行李牵引车的位置信息，以主天线为原点建立站心坐标系，根据主天线和副天线的基线向量信息，可以解算出副天线在该站心坐标系下的坐标(e,n,u)，e、n、u分别为站心坐标系下东、北、天方向上的坐标分量，可以解算出行李牵引车航向角y和俯仰角p:
[0059] y＝-arctan(e/n)
[0060]
[0061] 步骤2：将北斗双天线定位得到的位置与航向角y和俯仰角p，与IMU所得到的加速度与角速度信息进行松组合。其中，系统状态选取位置误差、速度误差和姿态误差，加速度器件误差和陀螺仪器件误差也被选取作为增广状态量，用于在线标定和补偿IMU误差。松组合kalman滤波器为：
[0062]
[0063] 式中：X(t)表示系统误差向量， 为X(t)的导数；F(t)表示系统的状态转移矩阵；w(t)表示系统噪声矩阵；G(t)表示系统噪声的驱动阵。
[0064] 系统的观测方程可表示为：
[0065] Z(t)＝H(t)X(t)+V(t)
[0066] 式中：Z(t)为观测量，包括IMU通过加速度所得到的位置信息和北斗双天线所得到的位置信息之差，包括IMU通过加速度所得到的速度信息和北斗双天线所得到的位置求导得到的速度信息之差，还包括IMU通过角速度所得到的姿态信息与北斗双天线所得到的姿态信息之差；H(t)为系数矩阵；V(t)为观测噪声矩阵。
[0067] 利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，从而提高车辆定位和定向的精度，并根据解算结果，对IMU仪器误差进行校正。
[0068] 步骤3：双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图。
[0069] 步骤4：双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图。
[0070] 步骤5：室内环境下(也包括北斗卫星可见数小于4颗的室外环境)，IMU惯导测量单元是配合双目相机，并利用地图进行匹配定位。通过IMU惯导测量单元得到的载体位姿，可为图片姿态提供高精度高频定位，经过传感器间的外部标定，将对应的信息投射到全局三维坐标系。双目相机与地图匹配获取行李牵引车在其运动范围下的全局初始位置，该位置作为初始时刻IMU定位的初始位置，并初始行李牵引车的姿态信息；IMU对所测得的角速度进行积分得到行李牵引车转动的角度，并结合行李牵引车的初始姿态可以获得运动坐标系下行李牵引车各个方向上的加速度信息；对运动坐标系下行李牵引车各个方向上的加速度进行积分可以获得行李牵引车的位置变化；把IMU初始位置加上位置位移，就可以获得行李牵引车相邻两时刻之间的实时位置；将当前时刻的视觉定位下行李牵引车的位置信息和IMU定位下行李牵引车的位置信息以及三维地图进行融合，利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，并更新IMU的初始位置。双目相机根据已得到自身位置信息，结合所拍摄的图片，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图。
[0071] 影响本实时例定位精度的主要因素如下：
[0072] 1、北斗接收机
硬件指标，此硬件指标越高，定位精度越高。例如，硬件指标较低的低成本的接收机，在静止状态下，其数据完整率、卫星可见数、
信噪比、多路径效应、周跳发生率以及伪距和载波
相位噪声等均低于硬件指标较高的接收机。
[0073] 2、北斗差分改正数据的接收，此改正数据精度越高，定位精度越高。例如，无改正数据的情况下，北斗伪距单点定位精度为3-10m；只包括伪距差分改正数的情况下，北斗伪距差分定位(RTD)精度为亚米级～分米级；既有伪距差分改正数又有载波差分改正数的情况下，北斗载波差分定位(RTK)精度为厘米级～分米级。
[0074] 3、地图的精度，此地图精度越高，定位精度越高。例如，常规的地图精度为米级，双目相机的定位精度为厘米级，而匹配的定位精度仅能实现米级的定位。
[0075] 4、IMU的测量精度，此测量精度越高，测姿精度越高。姿态不准导致重力影响去除不了，误差不断累积；器件本身漂移不能去除，如
加速度计从静止到运动再到静止，加速度积分得到的速度应该为0，而实际不为0。
[0076] 5、多传感器的时钟同步精度，此精度越高，定位精度越高。高精度定位系统的同步精度每降低3ns就会引入1米左右的测距误差，因此时钟同步性能成为高精度同步技术的关键指标，地面定位网元
节点间的高精度同步技术是这个领域研究的关键。
[0077] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉
本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围内。
