CN102231083B - 三轮无轨agv定位停车控制方法 - Google Patents

Abstract

本发明公开了一种三轮无轨AGV定位停车控制方法，其特征是首先使AGV转过角度α₁，车体到达法向误差的中心位置p_m，消除法向位置误差Δd的1/2；然后将AGV向相反的方向转过角度α₂，使AGV运行到与预定停车方位角相同的第二位置p_e，又一次消除法向位置误差Δd的1/2，剩余为切向位置误差Δt；最后通过判断预定停车位姿P′与第二位置P_e的位置关系，使AGV前进或后退以消除切向位置误差Δt，到达预定停车位置P′。本发明能够更好地保证三轮无轨AGV的控制精度以及实现快速到达目标位姿的目的。

Description

三轮无轨AGV定位停车控制方法

技术领域

本发明属于机器人技术领域，特别涉及一种作业于自动化车间的智能机器人的定位停车控制方法。

背景技术

目前，AGV已经广泛应用于数字化制造系统中，通常是与其他设备配合使用，所以AGV的定位精度非常重要。AGV运行在数字化车间，作为自动运输工具，其精确定位性能直接影响到整个车间的正常运行。只有AGV准确停靠在指定的位姿，才能很好地实现和其他设备的配合，完成运输工作。目前，绝大部分研究都是直接通过路径跟踪控制来保证AGV的定位精度，当AGV距离目标位置的误差较小时，可以快速调整实现目标，但当AGV与目标位置相差较大，且方位角误差较小时用这种控制方法耗时长，影响AGV的运行效率。另外，针对这种情况，有研究学者提出了一种两轮驱动型AGV的定位停车方法，通过两段圆弧路径消除AGV的法向位置误差，方位角误差通过车体的原地转向完成，但此种方法仅适用于两轮驱动型AGV的定位停车控制，而无法应用在三轮无轨AGV中。

发明目的

本发明是为避免上述现有技术所存在的不足之处，提供一种三轮无轨AGV定位停车控制方法，以期更好地保证三轮无轨AGV的控制精度以及实现快速到达目标位姿的目的。

本发明为解决技术问题采用如下技术方案：

本发明三轮无轨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′。

本发明三轮无轨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的圆心角度数α₂；

$\left\{\begin{array}{c}\mathrm{\Δx}=x^{\′}-x_0\\ \mathrm{\Δy}=y^{\′}-y_0\\ \mathrm{\Δ\α}={\mathrm{\α}}^{\′}-{\mathrm{\α}}_0\end{array}\right.---\left(1\right)$

$\mathrm{\Δd}=\left\{\begin{array}{c}|\mathrm{\Δ}x\sin {\mathrm{\α}}_0-\mathrm{\Δ}y\cos {\mathrm{\α}}_0|\\ |\mathrm{\Δ}x\sin {\mathrm{\α}}_0-\mathrm{\Δ}y\cos \mathrm{\Δ\α}+(\mathrm{\Δ}x\cos {\mathrm{\α}}_0+\mathrm{\Δ}y\sin {\mathrm{\α}}_0)\tan \mathrm{\Δ\α}|\end{array}\right.$ 当Δα＝0

                         (2)

当Δα≠0

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

${\mathrm{\α}}_2=\left\{\begin{array}{c}{\mathrm{\α}}_1=\arccos (1-\mathrm{\Δd}/2r)\\ {\mathrm{\α}}_1+n_1\mathrm{\Δ\α}=\arccos (1-\mathrm{\Δd}/2r)+n_1\mathrm{\Δ\α}\end{array}\right.$

当Δα＝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₂的坐标为：

各线段的长度通过其几何关系求出，如下所示：

$\left|\stackrel{\→}{{\mathrm{PC}}_1}\right|=\left|\stackrel{\→}{P_e{C}_2}\right|=r;---\left(9\right)$

$\left|\stackrel{\→}{{\mathrm{PP}}_m}\right|=\sqrt{{\left|\stackrel{\→}{\mathrm{PF}}\right|}^2+{\left|\stackrel{\→}{{\mathrm{FP}}_m}\right|}^2}=\sqrt{{(r-r\cos {\mathrm{\α}}_1)}^2+{\left(r\sin {\mathrm{\α}}_1\right)}^2}---\left(10\right)$

$\left|\stackrel{\→}{P_m{P}_e}\right|=\sqrt{{\left|\stackrel{\→}{P_{m}E}\right|}^2+{\left|\stackrel{\→}{{\mathrm{EP}}_e}\right|}^2}=\sqrt{{\left(r\sin {\mathrm{\α}}_2\right)}^2+{(r-r{\cos \mathrm{\α}}_2)}^2}---\left(11\right)$

角度和标识符n₁的值如下所示：

当α₀＝±90°或α‘＝±90°时，令b₁＝b₁₁，b₂＝b₂₂

1)当0°＜|α₀|＜90°，α₀＝0°或-90°时：

2)当90°＜|α₀|＜180°，α₀＝90°或-180°时：

其中 ${\mathrm{\θ}}_1=\arcsin \frac{\left|\stackrel{\→}{\mathrm{PF}}\right|}{\left|\stackrel{\→}{{\mathrm{PP}}_m}\right|}=\frac{|r-r\cos {\mathrm{\α}}_1|}{\sqrt{{\left(r\sin {\mathrm{\α}}_1\right)}^2+{(r-r\cos {\mathrm{\α}}_1)}^2}}$

${\mathrm{\θ}}_2=\arcsin \frac{\left|\stackrel{\→}{{\mathrm{EP}}_e}\right|}{\left|\stackrel{\→}{P_m{P}_e}\right|}=\frac{|r-r\cos {\mathrm{\α}}_2|}{\sqrt{{\left(r\sin {\mathrm{\α}}_2\right)}^2+{(r-r\cos {\mathrm{\α}}_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。

与已有技术相比，本发明有益效果体现在：

1、本发明方法中将AGV的位置误差转换为AGV运行时的切向位置误差Δt、法向位置误差Δd和方位角误差Δα，其中法向位置误差和方位角误差通过两段圆弧路径消除，切向位置误差通过AGV的前进或者后退消除。此种控制方法可以解决当AGV出现法向位置误差较大而方位角误差较小情况时的误差消除问题，定位精度高，可以很好地实现精确停车的要求。

2、本发明方法不仅仅适用于三轮无轨AGV的定位停车控制，而且也可以实现两轮驱动型AGV的定位停车需求，具有很好的通用性。

附图说明

图1为本发明实施例中所应用的激光导引型三轮无轨AGV系统结构框图；

图2为本发明中AGV的定位停车误差示意图；

图3为本发明中AGV的定位停车路径规划示意图；

图4为本发明中特殊位置的AGV路径规划示意图；

图5为本发明实施例中所应用的AGV实际行走的定位停车路径示意图；

具体实施方式

本实施例采用的三轮无轨AGV是一种激光导引型AGV，其前轮既是驱动轮又是转向轮，其余两轮为从动轮，安装在车体后面。如图1所示，此AGV主要由工控机、车体部分、激光导引定位系统、安全防撞装置、驱动转向系统、通讯系统、货物运载系统及其它辅助系统组成。

AGV在行驶过程中，由于受到外界因素，如路面状况、室内光线的明暗、车轮与地面发生滑动等，以及一些系统固有的误差，如定位系统的系统误差，车轮的不规则造成的误差，各传动减速装置之间的误差等影响，随着AGV的运行，这些误差也将会随之积累，最终导致AGV的实际运行路径与理想路径发生偏移，产生路径误差。当误差较小时，可以直接通过路径跟踪控制方法消除。但是当出现如图2(a)所示的AGV只存在位置误差而方位角误差为0和图2(b)所示的AGV位置误差较大而方位角误差较小的情况时，受到AGV周围运行环境的限制及车体尺寸的影响，必须规划出一条合适的路径来消除误差。

本实施例所采用的定位停车控制方法是按照以下方式使AGV沿如图3所示的定位停车路径L行走，消除起点位姿P和预定停车位姿P′之间的位置误差(Δx，Δy)和方位角误差Δα。图3(a)和图3(b)分别显示了图2(a)和图2(b)中所示误差的AGV定位停车路径规划示意图。图4显示了AGV初始方位角为90°的特殊位置时的路径规划示意图，其中，图4(a)为AGV只存在位置误差而方位角误差为0时的路径规划，图4(b)为AGV同时存在位置误差和方位角误差的情况。

首先，使AGV转过角度α₁，车体到达法向误差的中心位置p_m，消除法向位置误差Δd的1/2；

然后，将AGV向相反的方向转过角度α₂，使AGV运行到与预定停车方位角相同的第二位置p_e，又一次消除法向位置误差Δd的1/2，剩余为切向位置误差Δt；

最后，通过判断预定停车位姿P′与第二位置P_e的位置关系，使AGV前进或后退以消除切向位置误差Δt，到达预定停车位姿P′。

定位停车路径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的圆心角度数α₂；

$\left\{\begin{array}{c}\mathrm{\Δx}=x^{\′}-x_0\\ \mathrm{\Δy}=y^{\′}-y_0\\ \mathrm{\Δ\α}={\mathrm{\α}}^{\′}-{\mathrm{\α}}_0\end{array}\right.---\left(1\right)$

$\mathrm{\Δd}=\left\{\begin{array}{c}|\mathrm{\Δ}x\sin {\mathrm{\α}}_0-\mathrm{\Δ}y\cos {\mathrm{\α}}_0|\\ |\mathrm{\Δ}x\sin {\mathrm{\α}}_0-\mathrm{\Δ}y\cos \mathrm{\Δ\α}+(\mathrm{\Δ}x\cos {\mathrm{\α}}_0+\mathrm{\Δ}y\sin {\mathrm{\α}}_0)\tan \mathrm{\Δ\α}|\end{array}\right.$ 当Δα＝0

                       (2)

当Δα≠0

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

${\mathrm{\α}}_2=\left\{\begin{array}{c}{\mathrm{\α}}_1=\arccos (1-\mathrm{\Δd}/2r)\\ {\mathrm{\α}}_1+n_1\mathrm{\Δ\α}=\arccos (1-\mathrm{\Δd}/2r)+n_1\mathrm{\Δ\α}\end{array}\right.$ 当Δα＝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₂的坐标为：

各线段的长度通过其几何关系求出，如下所示：

$\left|\stackrel{\→}{{\mathrm{PC}}_1}\right|=\left|\stackrel{\→}{P_e{C}_2}\right|=r;---\left(9\right)$

$\left|\stackrel{\→}{{\mathrm{PP}}_m}\right|=\sqrt{{\left|\stackrel{\→}{\mathrm{PF}}\right|}^2+{\left|\stackrel{\→}{{\mathrm{FP}}_m}\right|}^2}=\sqrt{{(r-r\cos {\mathrm{\α}}_1)}^2+{\left(r\sin {\mathrm{\α}}_1\right)}^2}---\left(10\right)$

$\left|\stackrel{\→}{P_m{P}_e}\right|=\sqrt{{\left|\stackrel{\→}{P_{m}E}\right|}^2+{\left|\stackrel{\→}{{\mathrm{EP}}_e}\right|}^2}=\sqrt{{\left(r\sin {\mathrm{\α}}_2\right)}^2+{(r-r{\cos \mathrm{\α}}_2)}^2}---\left(11\right)$

角度

和标识符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₂时

其中 ${\mathrm{\θ}}_1=\arcsin \frac{\left|\stackrel{\→}{\mathrm{PF}}\right|}{\left|\stackrel{\→}{{\mathrm{PP}}_m}\right|}=\frac{|r-r\cos {\mathrm{\α}}_1|}{\sqrt{{\left(r\sin {\mathrm{\α}}_1\right)}^2+{(r-r\cos {\mathrm{\α}}_1)}^2}}$

${\mathrm{\θ}}_2=\arcsin \frac{\left|\stackrel{\→}{{\mathrm{EP}}_e}\right|}{\left|\stackrel{\→}{P_m{P}_e}\right|}=\frac{|r-r\cos {\mathrm{\α}}_2|}{\sqrt{{\left(r\sin {\mathrm{\α}}_2\right)}^2+{(r-r\cos {\mathrm{\α}}_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。

图5显示了本实例中的AGV从起点位姿P₀(-2.804，0.781，178.704°)运行到目标位姿P′₀(0.2，1.5，-180°)的实际行走的定位停车路径图，其中圆弧半径r为1m。从图中可以看出，定位停车路径分为三段即圆弧A₀-圆弧B₀-直线l₀，AGV首先沿圆弧A₀到达P_m0点，然后沿圆弧B₀到达P_e0点，最后沿直线l₀到达预定停车位姿P′₀。

Claims (1)

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′；

所述定位停车路径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的圆心角度数α₂；

$\left\{\begin{array}{c}\mathrm{\Δx}=x^{\′}-x_0\\ \mathrm{\Δy}=y^{\′}-y_0\\ \mathrm{\Δ\α}={\mathrm{\α}}^{\′}-{\mathrm{\α}}_0\end{array}\right.---\left(1\right)$

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

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₂的坐标为：

各线段的长度通过其几何关系求出，如下所示：

$\left|\stackrel{\→}{{\mathrm{PC}}_1}\right|=\left|\stackrel{\→}{P_e{C}_2}\right|=r;---\left(9\right)$

$\left|\stackrel{\→}{{\mathrm{PP}}_m}\right|=\sqrt{{\left|\stackrel{\→}{\mathrm{PF}}\right|}^2+|\stackrel{\→}{{\mathrm{FP}}_m}{|}^2}=\sqrt{{(r-r\cos {\mathrm{\α}}_1)}^2+{\left(r\sin {\mathrm{\α}}_1\right)}^2}---\left(10\right)$

$\left|\stackrel{\→}{P_m{P}_e}\right|=\sqrt{{\left|\stackrel{\→}{P_{m}E}\right|}^2+{\left|\stackrel{\→}{{\mathrm{EP}}_e}\right|}^2}=\sqrt{{\left(r\sin {\mathrm{\α}}_2\right)}^2+{(r-r\cos {\mathrm{\α}}_2)}^2}---\left(11\right)$

角度

和标识符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₂时

其中 ${\mathrm{\&theta;}}_1=\arcsin \frac{\left|\stackrel{\&RightArrow;}{\mathrm{PF}}\right|}{\left|\stackrel{\&RightArrow;}{{\mathrm{PP}}_m}\right|}=\frac{|r-r\cos {\mathrm{\&alpha;}}_1|}{\sqrt{{\left(r\sin {\mathrm{\&alpha;}}_1\right)}^2+{(r-r\cos {\mathrm{\&alpha;}}_1)}^2}}$

${\mathrm{\&theta;}}_2=\arcsin \frac{\left|\stackrel{\&RightArrow;}{{\mathrm{EP}}_e}\right|}{\left|\stackrel{\&RightArrow;}{P_m{P}_e}\right|}=\frac{|r-r\cos {\mathrm{\&alpha;}}_2|}{\sqrt{{\left(r\sin {\mathrm{\&alpha;}}_2\right)}^2+{(r-r\cos {\mathrm{\&alpha;}}_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。

Images