一种AGV路径跟踪方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN107390691A(43)申请公布日2017.11.24(21)申请号201710628585.5(22)申请日2017.07.28(71)申请人广东嘉腾机器人自动化有限公司地址528300广东省佛山市顺德区大良街道办事处古鉴村委会展翔路1号(72)发明人胡政钟杰辉(74)专利代理机构广州圣理华知识产权代理有限公司44302代理人顿海舟刘伟强(51)Int.Cl.G05D1/02(2006.01)权利要求书2页说明书7页附图6页(54)发明名称一种AGV路径跟踪方法(57)摘要本发明的一种AGV路径跟踪方法，(a)在AGV的导航机构内预设路径地图、路径点和直角坐标系；根据设置的路径点生成路径曲线方程S；(b)AGV按照路径曲线方程S行进；(c)导航机构以参考点O跟踪路径曲线方程S；(d)计算出当前参考点O的坐标以及车体当前航向角α；(e)计算出当前参考点O到路径曲线方程S上的最近点P的坐标；(f)计算出AGV的理论航向转角β；(g)计算横向偏差值e；(h)计算出偏差角δ；(i)判断AGV车体是否脱轨；(j)计算出舵轮转角A，驱动机构根据舵轮转角A修正行驶路线，完成一个路径跟踪周期。本发明的AGV路径跟踪方法，使AGV以较少的偏差沿着预设的路径曲线方程S行进。CN107390691ACN107390691A权利要求书1/2页1.一种AGV路径跟踪方法，其特征在于：包括以下步骤：(a)在AGV的导航机构内预设路径地图，并在路径地图上设置AGV的路径点和直角坐标系；根据设置的路径点生成路径曲线方程S；(b)AGV内的驱动机构驱动AGV按照预设的路径曲线方程S行进，并且以AGV车体两后轮之间的中心点为参考点O跟踪路径曲线方程S；(c)导航机构进行航位推算并根据AGV车体的定位装置计算出当前参考点O的坐标以及车体当前航向角α；(d)根据当前参考点O的坐标和路径曲线方程S，提取当前参考点O到路径曲线方程S上的最近点P的坐标；(e)根据路径曲线方程S计算出AGV位于最近点P时的理论航向转角β；(f)根据参考点O计算出AGV当前位置与最近点P的横向偏差值e；(g)根据车体当前航向角α和最近点P的理论航向转角β之间的差值得出偏差角δ；(h)根据横向偏差值e和偏差角δ，判断AGV车体是否脱轨，若是，AGV停止行进；若否，进入步骤(i)；(i)将理论航向转角β、偏差角δ和横向偏差值e加权求和，得出舵轮转角A，驱动机构根据舵轮转角A修正行驶路线，驱动AGV靠近或使回到路径曲线方程S上，完成一个路径跟踪周期。2.根据权利要求1所述的一种AGV路径跟踪方法，其特征在于：所述步骤(c)中，所述航位推算为：AGV导航机构通过获取编码器增量，得出当前AGV车体的位置坐标；所述车体定位装置包括两个车轮定位传感器，所述两个车轮定位传感器分别设置在所述AGV两个后轮上，导航机构根据当前AGV车体的位置坐标和两个车轮定位传感器的数据，得出参考点O的坐标。3.根据权利要求1所述的一种AGV路径跟踪方法，其特征在于：所述步骤(d)中最近点P的具体计算方式为：最近点P的坐标关系符合路径曲线方程S，路径曲线方程S在最近点P处的切线的斜率为K1；经过参考点O和最近点P的直线OP的斜率为K2；路径曲线方程S在最近点P处的切线与直线OP相互垂直，即-1＝K1*K2；根据路径曲线方程S和-1＝K1*K2计算出最近点P的坐标。4.根据权利要求1所述的一种AGV路径跟踪方法，其特征在于：所述步骤(d)中最近点P的具体计算方式为：提取当前AGV的参考点0到路径曲线方程S上的最近路径点M0，根据最近路径点M0，枚举出距离当前AGV的参考点0较近的附近路径点M1，M2，…，Mn；所述附近路径点M1，M2，…，Mn的选取方法为：计算最近路径点M0与参考点O的欧氏距离d0，将d0加一任意余量Y得出长度R，Y∈(0cm，200cm)，选取路径曲线方程S上所有与参考点O的欧氏距离不大于R的路径点，分别记录为M1，M2，…，Mn；以附近路径点Mi(i＝1，…，n)为起始端点或结束端点，分别截取路径曲线方程S上的两个曲线段；并使用四分法将每个曲线段平均分为四分段；分别提取任一曲线段中的端点Q1、Q2和Q3,计算参考点O分别到端点Q1、Q2、Q3以及该曲线2CN107390691A权利要求书2/2页段的起始端点和结束端点的欧氏距离，提取距离参考点O最近的端点，选取距离参考点O最近的端点，并且以该最近的端点为参照，选取该端点的前一个端点和后一个端点；通过前一个端点和后一个端点重新截取路径曲线方程S上的一个曲线段；重复上述步骤直至曲线段的长度小于精度误差θ，θ为10-1mm的数量级；输出此时距离参考点O最近的端点，该端点为该曲线段中