[发明专利]三轮无轨AGV定位停车控制方法

权利要求书

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.根据权利要求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)，圆心角的度数为α₁；

圆弧B的圆心C₂坐标为(x_C2，y_C2)，圆心角的度数为α₂；

法向误差的中心位置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的圆心角度数α₁、圆弧B的圆心角度数α₂；

Δ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)：

根据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。