[发明专利]一种磁导引AGV的自动纠偏控制方法

权利要求书

1.一种磁导引AGV的自动纠偏控制方法，其特征是按如下步骤进行：

步骤1、获取磁导引AGV在脱磁时的位姿信息；

所述步骤1是按照以下步骤进行：

步骤1.1、在磁导引AGV的底盘前后方均设置有磁导航传感器，用于获取前方偏距e_p1和后方距偏e_p2；

步骤1.2、利用式(1)计算偏距e_p：

步骤1.3、利用式(2)计算偏角e_α：

式(2)中，s为前后方的磁导航传感器之间的距离；

由所述偏距e_p和偏角e_α构成所述位姿信息；

步骤2、根据磁导引AGV的脱磁分类判定脱磁类别；

将所述磁导引AGV的脱磁类别分为：前方脱磁、后方脱磁、前后方均脱磁；

所述前方脱磁分为前方左脱磁和前方右脱磁；

所述后方脱磁分为后方左脱磁和后方右脱磁；

所述前后方均脱磁分为同侧脱磁和异侧脱磁；

所述前方左脱磁分为前方左脱磁后方左偏距较大、前方左脱磁后方右偏距较大、前方左脱磁后方偏距较小；

所述前方右脱磁分为前方右脱磁后方左偏距较大、前方右脱磁后方右偏距较大、前方右脱磁后方偏距较小；

所述后方左脱磁分为后方左脱磁前方左偏距较大、后方左脱磁前方右偏距较大、后方左脱磁前方偏距较小；

所述后方右脱磁分为后方左右脱磁前方左偏距较大、后方右脱磁前方右偏距较大、后方右脱磁前方偏距较小；

所述同侧脱磁分为前后方均左脱磁和前后方均右脱磁；

所述异侧脱磁为前后方交叉脱磁；

步骤3、控制磁导引AGV前进、后退和旋转以实现纠偏功能；

当磁导引AGV的脱磁类别为前方左脱磁后方右偏距较大时，则执行第一纠偏操作；

对所述第一纠偏操作的结果进行判定，若达到纠偏成功条件，则表示完成纠偏操作，否则，执行第一调整操作后，再执行第一纠偏操作，直到达到所述纠偏成功条件为止；

所述第一纠偏操作为：令磁导引AGV原地旋转角度为α；

所述第一调整操作为：令磁导引AGV回旋角度为Δ×α，再后退距离为L；

当磁导引AGV的脱磁类别为前方左脱磁后方左偏距较大或者前方左脱磁后方偏距较小时，则执行第二纠偏操作：

对所述第二纠偏操作的结果进行判定，若达到所述纠偏成功条件，则表示完成纠偏操作，否则，执行第二调整操作后，再执行第二纠偏操作，直到达到所述纠偏成功条件为止；

所述第二纠偏操作为：令磁导引AGV后退距离为L，再令磁导引AGV原地旋转角度为α；

所述第二调整操作为：令磁导引AGV回旋角度为Δ×α；

当磁导引AGV的脱磁类别为后方左脱磁前方右偏距较大时，则执行第三纠偏操作；

对所述第三纠偏操作的结果进行判定，若达到所述纠偏成功条件，则表示完成第三纠偏操作，否则，执行第三调整操作后，再执行第三纠偏操作，直到达到所述纠偏成功条件为止；

所述第三纠偏操作为：令磁导引AGV原地旋转角度为α；

所述第三调整操作为：令磁导引AGV回旋角度为Δ×α，再前进距离为L；

当磁导引AGV的脱磁类别为后方左脱磁前方左偏距较大或者后方左脱磁前方偏距较小时，则执行第四纠偏操作：

对所述第四纠偏操作的结果进行判定，若达到所述纠偏成功条件，则表示完成纠偏操作，否则，执行第四调整操作后，再执行第四纠偏操作，直到达到所述纠偏成功条件为止；

所述第四纠偏操作为：令磁导引AGV前进距离为L，再令磁导引AGV原地旋转角度为α；

所述第四调整操作为：令磁导引AGV回旋角度为Δ×α；

当磁导引AGV的脱磁类别为前后方均脱磁且同在左侧时，则执行第五调整操作，再执行第五纠偏操作：

对所述第五纠偏操作的结果进行判定，若达到所述纠偏成功条件，则表示完成纠偏操作，否则，执行第六调整操作后，再执行第五纠偏操作，直到达到所述纠偏成功条件为止；

所述第五调整操作为：令磁导引AGV向左旋转角度β；

所述第五纠偏操作为：令磁导引AGV后退距离为L，再令磁导引AGV原地旋转角度α

所述第六调整操作为：令磁导引AGV回旋角度为Δ×β；

当磁导引AGV的脱磁类别为前后方均脱磁且同在右侧时，则执行第七调整操作，再执行第六纠偏操作：

对所述第六纠偏操作的结果进行判定，若达到所述纠偏成功条件，则表示完成纠偏操作，否则，执行第八调整操作后，再执行第六纠偏操作，直到达到所述纠偏成功条件为止；

所述第七调整操作为：令磁导引AGV向右旋转角度β；

所述第六纠偏操作为：令磁导引AGV后退距离为L，再令磁导引AGV原地旋转角度为α；

所述第八调整操作为：令磁导引AGV回旋角度为Δ×β；

当磁导引AGV的脱磁类别为异侧脱磁时，则执行第七纠偏操作：

所述第七纠偏操作为：令磁导引AGV原地旋转角度为α；

所述步骤3中的原地旋转角度α、旋转角度β、后退或前进距离L是分别如下方式获得：

所述原地旋转角度α的值为偏角e_α；

利用式(3)得到所述旋转角度β：

式(3)中，e表示前后偏距总和的绝对值并由式(4)得到：

e＝|e_p1+e_p2| (4)

利用式(5)计算后退或前进距离L：

式(5)中，e_L为偏差，并由(6)得到：