[发明专利]融合北斗与多传感器的机场行李牵引车定位装置与方法

权利要求书

1.一种融合北斗与多传感器的机场行李牵引车定位方法，其特征在于，包括如下步骤：

步骤1、室外环境下且北斗卫星可见数超过3颗，利用主天线和副天线以及北斗基准站输入的差分改正数进行主天线和副天线的定位，定位方程为：

式中：λ为波长，为载波模糊度，L_P、L_Ф分别伪距双差、载波宽巷双差与几何距离双差之差，I为单位阵，V_P、V_Ф分别为伪距和载波残差，ΔX为坐标增量，A为坐标增量的系数矩阵；

将浮点解及其协方差阵代入LAMBDA方法求解到达固定解，进而得到主天线和副天线的坐标增量参数；

将主天线得到的位置信息作为行李牵引车的位置信息，以主天线为原点建立站心坐标系，根据主天线和副天线的基线向量信息，解算出副天线在该站心坐标系下的坐标(e,n,u)，e、n、u分别为站心坐标系下东、北、天方向上的坐标分量，解算出行李牵引车航向角y和俯仰角p:

y＝-arctan(e/n)

步骤2、将北斗双天线定位得到的位置与航向角y和俯仰角p，与IMU所得到的加速度与角速度信息进行松组合；其中，系统状态选取位置误差、速度误差和姿态误差，加速度器件误差和陀螺仪器件误差也被选取作为增广状态量，用于在线标定和补偿IMU误差；松组合kalman滤波器为：

式中：X(t)表示系统误差向量，为X(t)的导数；F(t)表示系统的状态转移矩阵；w(t)表示系统噪声矩阵；G(t)表示系统噪声的驱动阵；

系统的观测方程表示为：

Z(t)＝H(t)X(t)+V(t)

式中：Z(t)为观测量，包括IMU通过加速度所得到的位置信息和北斗双天线所得到的位置信息之差，包括IMU通过加速度所得到的速度信息和北斗双天线所得到的位置求导得到的速度信息之差，还包括IMU通过角速度所得到的姿态信息与北斗双天线所得到的姿态信息之差；H(t)为系数矩阵；V(t)为观测噪声矩阵；

利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，并根据解算结果，对IMU仪器误差进行校正；

步骤3、双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图；

步骤4、双目相机根据已得到自身位置信息，对同一物体进行拍摄，然后通过同一物体在不同图像中的视差，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图；

步骤5、室内环境下或北斗卫星可见数小于4颗的室外环境，IMU是配合双目相机，并利用地图进行匹配定位；通过IMU得到的载体位姿，为图片姿态提供定位，经过传感器间的外部标定，将对应的信息投射到全局三维坐标系；双目相机与地图匹配获取行李牵引车在其运动范围下的全局初始位置，该位置作为初始时刻IMU定位的初始位置，并初始行李牵引车的姿态信息；IMU对所测得的角速度进行积分得到行李牵引车转动的角度，并结合行李牵引车的初始姿态获得运动坐标系下行李牵引车各个方向上的加速度信息；对运动坐标系下行李牵引车各个方向上的加速度进行积分获得行李牵引车的位置变化；将IMU初始位置加上位置位移，获得行李牵引车相邻两时刻之间的实时位置；将当前时刻的视觉定位下行李牵引车的位置信息和IMU定位下行李牵引车的位置信息以及三维地图进行融合，利用自适应kalman滤波方法，求解出行李牵引车的位置与姿态信息的最优解，并更新IMU的初始位置；双目相机根据已得到自身位置信息，结合所拍摄的图片，计算出物体在实际空间中的位置，并与地图存储单元相对比，更新地图。

2.根据权利要求1所述的一种融合北斗与多传感器的机场行李牵引车定位方法，其特征在于，双目相机在室外环境下且北斗卫星可见数超过3颗，是利用拍摄的图片对地图进行更新；双目相机在室内环境下或北斗卫星可见数小于4颗的室外环境，是融合IMU和地图匹配进行定位，辅助功能是利用拍摄的图片计算其空间位置，以实现对地图进行更新。