[發明專利]一種對自動導航物體位置坐標及方向偏差的校正方法

在審

• 申請號: 201811059758.7
• 申請日: 2018-09-12
• 公開（公告）號: CN109358616A
• 公開（公告）日: 2019-02-19
• 發明（設計）人: 黃海寧
• 申請（專利權）人: 黃海寧
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廣州市紅荔專利代理有限公司 44214
• 代理人: 吳世民

1.一種對自動導航物體位置坐標及方向偏差的校正方法，在至少具有陀螺儀、測距設備、感應裝置的自動導航物體上使用，其特征在于，包括以下步驟：

S1、首先在具有平行設置的兩反射板BA和BB以及設置于兩反射板之間且垂直于兩反射板的校正觸發裝置SL的校正站內，在自動導航物體的自主參考坐標系下，

a，自動導航物體運動至第0時刻時，自動導航物體上的感應裝置SA感應到其運動至校正觸發裝置SL；通過自動導航物體上的陀螺儀和測距設備獲取自動導航物體第0時刻的當前位置的位置狀態參數集合為AGV_S₀(X₀,Y₀,α₀)，其中(X₀,Y₀)為坐標參數，α₀為方位角參數；

b，自動導航物體上位于同一水平線上的測距設備CA、CB分別測量其與反射板之間的距離L1₀、L2₀；

c，計算自動導航物體的虛擬對稱線AC與反射板之間夾角即自動導航物體偏轉角為θ₀＝arctg((L2₀-L1₀)/D)，其中D為測距設備CA、CB之間的距離；同時計算校正觸發裝置SL所在線在坐標系中的方位角為β₀＝α₀+π/2+θ₀；

S2、自動導航物體運動至第m時刻時，自動導航物體上的感應裝置SA感應到其運動至校正觸發裝置SL；

a，自動導航物體通過其上的陀螺儀和測距設備獲取第m時刻的當前位置的位置狀態參數集合為AGV_S_m(X_m,Y_m,α_m)，其中(X_m,Y_m)為坐標參數，α_m為方位角參數；

b，自動導航物體上位于同一水平線上的測距設備CA、CB分別測量其與反射板之間的距離L1_m、L2_m；

c，計算自動導航物體的虛擬對稱線AC與反射板之間的夾角即自主導航物體偏轉角為θ_m＝arctg((L2_m-L1_m)/D)；計算校正觸發裝置SL所在線在坐標系中的方位角為β_m＝α_m+π/2+θ_m；

S3、計算第m時刻方向角的修正值為α_m'＝α₀-θ₀+θ_m并對自動導航物體的方向角進行修正，修正后的自動導航物體的位置狀態參數集合為AGV_S_m(X_m,Y_m,α_m’)；

S4、a，計算第m時刻自動導航物體的虛擬對稱線AC的垂直線與校正觸發裝置SL所在線的夾角γ_m，γ_m＝θ_m＝arctg((L2_m-L1_m)/D)；

b，計算感應裝置SA到反射板BA的垂直距離D_m，D_m＝L2_m*COS(θ_m)；

c，計算D_m與D₀的差ΔD_m，ΔD_m＝D_m-D₀；其中D₀＝L2₀*COS(θ₀)；

d，計算第m時刻平面坐標的修正值為(X_m’,Y_m’)，并對自動導航物體的平面坐標進行修正，修正后的自動導航物體的位置狀態參數集合為AGV_S_m'(X_m',Y_m',α_m’)。