[發明專利]融合北斗與多傳感器的機場行李牽引車定位裝置與方法

有效

• 申請號: 202010000620.0
• 申請日: 2020-01-02
• 公開（公告）號: CN111077556B
• 公開（公告）日: 2022-04-15
• 發明（設計）人: 王慶;嚴超;張昊;許九靖;張波;劉芬
• 申請（專利權）人: 東南大學
• 主分類號: G01S19/46
• 分類號: G01S19/46;G01S19/47
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 朱楨榮

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和地圖匹配進行定位，輔助功能是利用拍攝的圖片計算其空間位置，以實現對地圖進行更新。