[發(fā)明專利]三輪無軌AGV定位停車控制方法

有效

• 申請?zhí)枺?/td>: 201110095722.6
• 申請日: 2011-04-15
• 公開（公告）號: CN102231083A
• 公開（公告）日: 2011-11-02
• 發(fā)明（設計）人: 吳焱明;趙韓;王軍;王秋杰;尹曉紅
• 申請（專利權）人: 合肥工業(yè)大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 安徽省合肥新安專利代理有限責任公司 34101
• 代理人: 何梅生

1.一種三輪無軌AGV定位停車控制方法，其特征是所述控制方法是按以下方式使AGV沿定位停車路徑L行走，消除起點位姿P和預定停車位姿P′之間的位置誤差(Δx，Δy)和方位角誤差Δα；

首先，使AGV轉過角度α₁，車體到達法向誤差的中心位置P_m，消除法向位置誤差Δd的1/2；

然后，將AGV向相反的方向轉過角度α₂，使AGV運行到與預定停車方位角相同的第二位置P_e，又一次消除法向位置誤差Δd的1/2，剩余為切向位置誤差Δt；

最后，通過判斷預定停車位姿P′與第二位置P_e的位置關系，使AGV前進或后退以消除切向位置誤差Δt，到達預定停車位姿P′。

2.根據(jù)權利要求1所述的三輪無軌AGV定位停車控制方法，其特征是：

所述定位停車路徑L由圓弧A、圓弧B和直線l共三段路徑組成；所述圓弧A與圓弧B相切于AGV法向誤差的中心位置P_m；所述圓弧B與直線l相切于第二位置P_e；所述圓弧A與圓弧B的半徑相等均為r，且其圓心分別為C₁和C₂；

所述AGV的定位停車路徑L、法向誤差的中心位置P_m和第二位置P_e按照以下步驟求得：

在AGV運動所在平面的直角坐標系中，將AGV的位姿表示為(x，y，α)，其中(x，y)為AGV在所述直角坐標系中的位置坐標，α為所述AGV的方位角，所述方位角α為AGV的中心軸線與x軸正向的夾角，范圍為[-180°，180°)；

在直角坐標系中，AGV的起點位姿P由位置傳感器檢測得到為(x₀，y₀，α₀)，AGV的預定停車位姿P′設置為(x′，y′，α′)；

定義圓弧A的圓心C₁坐標為(x_C1，y_C1)，圓心角的度數(shù)為α₁；

圓弧B的圓心C₂坐標為(x_C2，y_C2)，圓心角的度數(shù)為α₂；

法向誤差的中心位置P_m的坐標為(x_m，y_m，α_m)；

第二位置P_e的坐標為(x_e，y_e，α_e)；

AGV的起點位姿P點所在的直線在y軸上的截距為b₁，在x軸上的截距為b₁₁；

AGV的預定停車位姿P′點所在的直線在y軸上的截距為b₂，在x軸上的截距為b₂₂；

連接P點和圓心C₁，過P_m點做垂直于PC₁的直線P_mF，垂足為F；

連接P_e點和圓心C₂，過P_m點做垂直于P_eC₂的直線P_mE，垂足為E；

直線P_mF和P_mP之間的夾角為θ₁；

直線P_mE和P_mP_e之間的夾角為θ₂；

有向線段與x軸正向之間的夾角為

有向線段與x軸正向之間的夾角為

有向線段與x軸正向之間的夾角為

有向線段與x軸正向之間的夾角為

標志符n₁＝1或-1；

a、計算AGV起點位姿和目標位姿之間的位姿誤差(Δx，Δy，Δα)和法向位置誤差Δd，圓弧A的圓心角度數(shù)α₁、圓弧B的圓心角度數(shù)α₂；

Δx=x′-x0Δy=y′-y0Δα=α′-α0---(1)]]>

Δd=|Δxsinα0-Δycosα0||Δxsinα0-ΔycosΔα+(Δxcosα0+Δysinα0)tanΔα|]]>當Δα＝0

????????????????????????????????(2)

??????當Δα≠0

α₁＝arccos(1-Δd/2r)(3)

α2=α1=arccos(1-Δd/2r)α1+n1Δα=arccos(1-Δd/2r)+n1Δα]]>當Δα＝0

????????????????????????????????(4)

??????當Δα≠0

b、計算圓弧A的圓心C₁(x_C1，y_C1)、法向誤差的中心位置P_m(x_m，y_m，α_m)、圓弧B的圓心C₂(x_C2，y_C2)和第二位置P_e(x_e，y_e，α_e)：

根據(jù)AGV的起點位姿P(x₀，y₀，α₀)得到圓心C₁坐標值為：

法向誤差的中心位置P_m的坐標為：

由AGV的預定停車位姿P′(x′，y′，α′)和法向誤差的中心位置P_m得到P_e的坐標為：

由第二位置P_e(x_e，y_e，α_e)得到圓心C₂的坐標為：

各線段的長度通過其幾何關系求出，如下所示：

|PC1→|=|PeC2→|=r;---(9)]]>

|PPm→|=|PF→|2+|FPm→|2=(r-rcosα1)2+(rsinα1)2---(10)]]>

|PmPe→|=|PmE→|2+|EPe→|2=(rsinα2)2+(r-rcosα2)2---(11)]]>

角度和標識符n₁的值如下所示：

當α₀＝±90°或α‘＝±90°時，令b₁＝b₁₁，b₂＝b₂₂

1)當0°＜|α₀|＜90°，α₀＝0°或-90°時：

當b₁＜b₂時

當b₁＞b₂時

2)當90°＜|α₀|＜180°，α₀＝90°或-180°時：

當b₁＜b₂時

當b₁＞b₂時

其中θ1=arcsin|PF→||PPm→|=|r-rcosα1|(rsinα1)2+(r-rcosα1)2]]>

θ2=arcsin|EPe→||PmPe→|=|r-rcosα2|(rsinα2)2+(r-rcosα2)2]]>

c、確定以圓弧A、圓弧B及直線l表征的定位停車路徑L

由圓心C₁(x_C1，y_C1)、AGV起點位姿P(x₀，y₀，α₀)、法向誤差的中心位置P_m(x_m，y_m，α_m)、半徑r確定圓弧A，由圓心C₂(x_C2，y_C2)、法向誤差的中心位置P_m(x_m，y_m，α_m)、第二位置P_e(x_e，y_e，α_e)、半徑r確定圓弧B，由第二位置P_e(x_e，y_e，α_e)、AGV預定停車位姿P′(x′，y′，α′)確定直線l。