[發明專利]AGV小車的定位方法及定位系統

在審

• 申請號: 201910030711.6
• 申請日: 2019-01-14
• 公開（公告）號: CN109579838A
• 公開（公告）日: 2019-04-05
• 發明（設計）人: 羅世彬;劉海橋;胡茂青;孫方德;王鈺婕
• 申請（專利權）人: 湖南海迅自動化技術有限公司
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種AGV小車的定位方法，其特征在于，包括如下步驟：

(1)系統啟動；

(2)前端采集：工業相機采集圖像數據，IMU采集數據信息，并將圖形數據和IMU數據輸入導航計算機；

(3)數據同步：在圖像數據傳輸進導航計算機后，通過一個標志位，將圖像信息傳輸給IMU，將圖像數據與IMU數據進行同步；

(4)姿態解算；視覺姿態解算和IMU姿態計算；

(6)數據融合：將IMU姿態解算后的結果作為運動量，構建運動方程，將視覺解算出來的姿態信息作為觀測量，來構建觀測方程，構建Kalman濾波方程，進行數據融合，得到最優結果；

(7)判定AGV小車速度：根據判定方法，判定AGV小車處于絕對零速的時候，進入步驟(8)；反之，得到導航相關的定位信息，導航結束；

(8)零速修正：構建速度誤差方程，運用Kalman濾波，對姿態信息進行修正，并將修正后的狀態進行零速更新，達到消除累計誤差的目的，導航結束。

2.根據權利要求1所述的方法，其特征在于，所述步驟(2)中，工業相機將采集的圖像數據通過USB端口傳輸進入導航計算機，IMU通過串口，并運用串口轉換成USB端口，將IMU數據傳輸進入導航計算機。

3.根據權利要求1所述的方法，其特征在于，所述視覺姿態解算為：將圖像數據依次經過視覺跟蹤、圖像通過配置算法、PnP姿態解算算法，得到視覺姿態信息。

4.根據權利要求1所述的方法，其特征在于，所述IMU姿態解算為：IMU數據通過航機推演算法進行姿態解算，并進行姿態更新，得到IMU姿態信息。

5.根據權利要求1所述的方法，其特征在于，所述零速修正的判定方法為：

根據IMU中的加速度計和陀螺儀的輸出來判定零速區間：通過第i時刻加速度計輸出的比力信息f_i＝[f_xi f_yi f_zi]^T，陀螺儀輸出的角速度信息ω_i＝[ω_xi ω_yi ω_zi]^T進行零速檢測。

6.根據權利要求5所述的方法，其特征在于，所述步驟(7)中采用IMU檢測零速，其檢測算法包括三個條件：加速度模值、加速度滑動方差和角速度模值，分別記為Q1，Q2，Q3；

各條件實現方式如下：

a.加速度模值檢測，即當加速度模值大于等于最小閾值且小于等于最小閾值時，條件Q₁(i)＝1；

b.比力滑動方差檢測，即當滑動窗口大小為N時比力方差小于等于給定閾值時，條件Q₂(i)＝1；

其中，為[i-n i+n]時間間隔內采樣得到的2n+1個比力f的均值，即：

c.角速度模值檢測，即當角速度模值小于等于給定閾值th_max時，條件Q₃(i)＝1；

IMU檢測算法為多條件符合檢測方法，即同時滿足以上三個條件時為零速區間：

Q_IMU(i)＝Q₁∧Q₂∧Q₃。