一種agv慣性導航方法
【技術領域】
[0001] 本發明屬于自動控制領域，屬于一種AGV慣性導航方法。
【背景技術】
[0002] 目前AGV自動引導小車已經成為智能制造、先進物流以及數字化工廠中的重要設 備，作為方便工廠運輸、提高生產效率具有非常重要的作用。而所謂自動引導，即是沿著 指定軌跡運行，目前常見的巡線導航有光電傳感器、視覺、激光和磁導航傳感器等等，對于 光電傳感器，配合黑白膠帶整體便宜、簡單，但是信號很容易因為地面不平導致不穩、膠帶 易受灰塵、垃圾影響等，導致巡線不能很穩定；視覺可視范圍大，對線的要求不高，但是很容 易受光線影響，且視頻采集處理卡價格不菲，采樣頻率不高；而目前國內外市面上最常見的 AGV產品大多采用磁條導航。而磁條導航AGV雖然相對價格便宜，且運行也較為可靠，但是 每次運行AGV需要鋪設磁條，特別路徑較長時，鋪設工作量較大，路徑過長時，磁條的成本 會比較高，同時更換路線后，由于磁條底部粘性變弱，不能重復使用，磁條后期在水、碾壓、 磨損等條件下，維護較麻煩，而隨著工業4. 0和智能制造需求，因此市場上對一種使用更靈 活、方便的AGV導航的研發是迫在眉睫。
【發明內容】
[0003] 本發明的目的在于克服現有技術的不足，提供一種AGV慣性導航方法，該方法通 過查詢方式獲取陀螺儀采集的數據，同時能夠有效濾除陀螺儀采集的數據的固定漂移和隨 機誤差，能夠獲得陀螺儀準確的角度數據，精度為±0. 1度，可準確推算出所有采樣時刻的 位置和姿態，精度達到± 5mm，導航精度可達± 10mm。
[0004] 本發明的目的是通過以下技術方案來實現的：一種AGV慣性導航方法，它包括如 下子步驟：
[0005] Sl :導航系統搭建，將陀螺儀設置在AGV小車上，磁傳感器設置在小車車頭底部中 線上，磁釘鋪設在AGV航道上，編碼器、數據處理單元和運動控制單元設置在控制盒內；
[0006] S2 :數據采集，陀螺儀采集車輪的角速度，磁傳感器采集航道上磁釘的位置，編碼 器采集單元根據外部中斷計數編碼器的脈沖，結合電機的極對數得到輪子的轉動周數，根 據輪子直徑和減速比可以計算出里程；
[0007] S3 :采集陀螺儀數據，數據處理單元中的陀螺儀數據采集模塊通過SPI接口定時 發送查詢協議到陀螺儀，獲取陀螺儀測量的原始數據；
[0008] S4:固定漂移處理，數據處理單元中的固定漂移處理模塊將陀螺儀采集的原始數 據，通過離線數據分析得到期望做補償常量，使得數據成標準高斯分布；
[0009] S5 :卡爾曼濾波處理，將去除固定漂移的數據進行卡爾曼濾波，使數據變得平滑； [0010] S6 :角度獲取，將卡爾曼濾波后的數據進行積分得到角度信息；
[0011] S7 :磁釘校準，磁釘校準單元根據磁傳感器采集的磁釘軌跡得到AGV當前運行方 向和實際軌跡方向的偏差角度，如果偏差角度較小，則將陀螺儀采集角度積分初始點校準 為校準角度，否則不校準；如果偏差角度太大，則認為AGV脫軌運行；
[0012] S8:航跡推算，根據編碼器的里程數據、校準后的陀螺儀角度數據形成極坐標關 系，推算出所有采樣時刻的位置和姿態，形成航跡；
[0013] S9 :PID調節，將當前時刻的位置點和給定位置點作為PID調節器的輸入，得出增 量角度作為輸出；
[0014] SlO :運動控制，以增量角度轉為運動輸入，調整AGV的姿態。
[0015] 所述的步驟S5卡爾曼濾波處理采集多組原始數據樣本，分析其固定漂移量、高斯 的期望和方差，并建立陀螺儀的數據模型，建立卡爾曼濾波的參數，
[0016] X (k) =x (k~l) (1)
[0017] 觀測方程就是傳感器的感知值，主要誤差就是來自于測量誤差，所以簡化為
[0018] z (k) =x (k) +V (k) (2)
[0019] z(k)為實際測量，x(k)真實值，v(k)為具有整體便宜的高斯噪聲，這樣就得到了 其狀態更新方程：
[0020] X； = (3)
[0021] Pi =Pi , ⑷
[0022] 同樣得到測量更新方程：
[0026] 雖然卡爾曼濾波為無偏估計，但是由于原始的噪聲使得增量變化不連續，會影響 到控制算法的控制增量，同時也會影響最后角度控制效果，因此需要進行卡爾曼濾波。
[0027] 所述的步驟S6中的角度獲取將卡爾曼濾波后的陀螺儀數據進行積分
，在處理中離散為累計求和方式
這樣求出陀螺儀的角速度。
[0028] 所述的步驟S7磁釘校準原理為：在運行軌跡直線處放置相距L的一對磁釘，磁釘 校準單元，根據磁傳感器分別檢測Ml磁釘、M2磁釘的位置，計算出AGV當前運行方向和實 際軌跡方向的偏差角度Θ = arctan((b_a)/L)，如果該角度在可接受范圍內則將陀螺儀的 角度測量值進行校準，否則則認為AGV脫軌。
[0029] 所述的步驟S8航跡推算根據編碼器的里程數據、校準后的陀螺儀數據形成極坐 標關系，假設k時刻AGV的位置和姿態為（X (k)，y (k)，Θ (k))，則k+Ι時刻姿態為：
[0030] X (k+1) = X (k) + Δ s cos Δ Θ (k)
[0031] y(k+l) = y(k) + A s sin Δ Θ (k)
[0032] θ (k+1) = θ (k) + A θ (k)
[0033] 其中△ s(k)為編碼器獲得增量里程；△ θ (k)采用陀螺儀測量得的增量角度，這 樣通過迭代推算出所有采樣時刻的位置和姿態，從而形成航跡。
[0034] 所述的步驟S9PID調節中如果在k時刻給定點坐標為（xd(k)，yd(k))，而實際位置 和姿態為（X (k)，y (k))，此時輸出偏差采用極坐標方式表達 CN 105180934 A 說明書 3/5 頁
[0036] Δ θ = atari (y (k) -yd (k) /x (k) -xd (k))
[0037] 無偏差輸入則指AGV實際軌跡和給定軌跡重合，也即Δ Θ ;反之，只要Δ Θ不為 零，則存在偏差，糾偏算法仍采用PID控制算法，如果調整后AGV的角度一致，但坐標不一 致，即X不一致，這樣AGV運行一段時間必然和給定坐標重合；但是如果AGV在當前周期調 整還未達到上一時刻的給定值時，AGV偏差輸入以當前時刻的給定值為準。
[0038] 所述的步驟SlO運動控制需根據AGV運動模型控制，若AGV為差動模型則根據角 度偏差控制左右電機差速；若AGV為舵輪則直接控制舵輪角度。
[0039] 本發明的有益效果是：本發明提供了一種AGV慣性導航方法，該方法通過查詢方 式獲取陀螺儀采集的數據，同時能夠有效濾除陀螺儀采集的數據的固定漂移和隨機誤差， 能夠獲得陀螺儀準確的角度數據，精度為±0. 1度，可準確推算出所有采樣時刻的位置和 姿態，精度達到± 5mm，導航精度可達± 10mm。
【附圖說明】
[0040] 圖1為慣性導航方法流程圖；
[0041] 圖2為磁釘校準原理圖A ;
[0042] 圖3為磁釘校準原理圖B ;
[0043] 圖4為軌跡推算示意圖。
【具體實施方式】
[0044] 下面結合附圖進一步詳細描述本發明的技術方案，但本發明的保護范圍不局限于 以下所述。
[0045] 如圖1所示，一種AGV慣性導航方法，它包括如下子步驟：
[0046] Sl :導航系統搭建，將陀螺儀設置在AGV小車上，磁傳感器設置在小車車頭底部中 線上，磁釘鋪設在AGV航道上，編碼器、數據處理單元和運動控制單元設置在控制盒內；
[0047] S2 :數據采集，陀螺儀采集車輪的角速度，磁傳感器采集航道上磁釘的位置，編碼 器采集單元根據外部中斷計數編碼器的脈沖，結合電機的極對數得到輪子的轉動周數，根 據輪子直徑和減速比可以計算出里程；
[0048] S3 :采集陀螺儀數據，數據處理單元中的陀螺儀數據采集模塊通過SPI接口定時 發送查詢協議到陀螺儀，獲取陀螺儀測量的原始數據；
[0049] S4:固定漂移處理，數據處理單元中的固定漂移處理模塊將陀螺儀采集的原始數 據，通過離線數據分析得到期望做補償常量，使得數據成標準高斯分布；
[0050] S5 :卡爾曼濾波處理，將去除固定漂移的數據進行卡爾曼濾波，使數據變得平滑；
[0051] S6 :角度獲取，將卡爾曼濾波后的數據進行積分得到角度信息；
[0052] S7 :磁釘校準，磁釘校準單元根據磁傳感器采集的磁釘軌跡得到AGV當前運行方 向和實際軌跡方向的偏差角度，如果偏差角度較小，則將陀螺儀采集角度積分初始點校準 為校準角度，否則不校準；如果偏差角度太大，則認為AGV脫軌運行；
[0053] S8:航跡推算，根據編碼器的里程數據、校準后的陀螺儀角度數據形成極坐標關 系，推算出所有采樣時刻的位置