[發(fā)明專(zhuān)利]一種磁導(dǎo)引AGV的自動(dòng)糾偏控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010329875.1
• 申請(qǐng)日: 2020-04-24
• 公開(kāi)（公告）號(hào): CN111474933B
• 公開(kāi)（公告）日: 2022-03-15
• 發(fā)明（設(shè)計(jì)）人: 吳焱明;朱家誠(chéng);孟凡喜;趙士豪;黃立宇
• 申請(qǐng)（專(zhuān)利權(quán)）人: 合肥工業(yè)大學(xué)
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02
• 代理公司: 安徽省合肥新安專(zhuān)利代理有限責(zé)任公司 34101
• 代理人: 陸麗莉;何梅生

1.一種磁導(dǎo)引AGV的自動(dòng)糾偏控制方法，其特征是按如下步驟進(jìn)行：

步驟1、獲取磁導(dǎo)引AGV在脫磁時(shí)的位姿信息；

所述步驟1是按照以下步驟進(jìn)行：

步驟1.1、在磁導(dǎo)引AGV的底盤(pán)前后方均設(shè)置有磁導(dǎo)航傳感器，用于獲取前方偏距e_p1和后方距偏e_p2；

步驟1.2、利用式(1)計(jì)算偏距e_p：

步驟1.3、利用式(2)計(jì)算偏角e_α：

式(2)中，s為前后方的磁導(dǎo)航傳感器之間的距離；

由所述偏距e_p和偏角e_α構(gòu)成所述位姿信息；

步驟2、根據(jù)磁導(dǎo)引AGV的脫磁分類(lèi)判定脫磁類(lèi)別；

將所述磁導(dǎo)引AGV的脫磁類(lèi)別分為：前方脫磁、后方脫磁、前后方均脫磁；

所述前方脫磁分為前方左脫磁和前方右脫磁；

所述后方脫磁分為后方左脫磁和后方右脫磁；

所述前后方均脫磁分為同側(cè)脫磁和異側(cè)脫磁；

所述前方左脫磁分為前方左脫磁后方左偏距較大、前方左脫磁后方右偏距較大、前方左脫磁后方偏距較小；

所述前方右脫磁分為前方右脫磁后方左偏距較大、前方右脫磁后方右偏距較大、前方右脫磁后方偏距較小；

所述后方左脫磁分為后方左脫磁前方左偏距較大、后方左脫磁前方右偏距較大、后方左脫磁前方偏距較小；

所述后方右脫磁分為后方左右脫磁前方左偏距較大、后方右脫磁前方右偏距較大、后方右脫磁前方偏距較小；

所述同側(cè)脫磁分為前后方均左脫磁和前后方均右脫磁；

所述異側(cè)脫磁為前后方交叉脫磁；

步驟3、控制磁導(dǎo)引AGV前進(jìn)、后退和旋轉(zhuǎn)以實(shí)現(xiàn)糾偏功能；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為前方左脫磁后方右偏距較大時(shí)，則執(zhí)行第一糾偏操作；

對(duì)所述第一糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到糾偏成功條件，則表示完成糾偏操作，否則，執(zhí)行第一調(diào)整操作后，再執(zhí)行第一糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第一糾偏操作為：令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述第一調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×α，再后退距離為L(zhǎng)；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為前方左脫磁后方左偏距較大或者前方左脫磁后方偏距較小時(shí)，則執(zhí)行第二糾偏操作：

對(duì)所述第二糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到所述糾偏成功條件，則表示完成糾偏操作，否則，執(zhí)行第二調(diào)整操作后，再執(zhí)行第二糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第二糾偏操作為：令磁導(dǎo)引AGV后退距離為L(zhǎng)，再令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述第二調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×α；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為后方左脫磁前方右偏距較大時(shí)，則執(zhí)行第三糾偏操作；

對(duì)所述第三糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到所述糾偏成功條件，則表示完成第三糾偏操作，否則，執(zhí)行第三調(diào)整操作后，再執(zhí)行第三糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第三糾偏操作為：令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述第三調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×α，再前進(jìn)距離為L(zhǎng)；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為后方左脫磁前方左偏距較大或者后方左脫磁前方偏距較小時(shí)，則執(zhí)行第四糾偏操作：

對(duì)所述第四糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到所述糾偏成功條件，則表示完成糾偏操作，否則，執(zhí)行第四調(diào)整操作后，再執(zhí)行第四糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第四糾偏操作為：令磁導(dǎo)引AGV前進(jìn)距離為L(zhǎng)，再令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述第四調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×α；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為前后方均脫磁且同在左側(cè)時(shí)，則執(zhí)行第五調(diào)整操作，再執(zhí)行第五糾偏操作：

對(duì)所述第五糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到所述糾偏成功條件，則表示完成糾偏操作，否則，執(zhí)行第六調(diào)整操作后，再執(zhí)行第五糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第五調(diào)整操作為：令磁導(dǎo)引AGV向左旋轉(zhuǎn)角度β；

所述第五糾偏操作為：令磁導(dǎo)引AGV后退距離為L(zhǎng)，再令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度α

所述第六調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×β；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為前后方均脫磁且同在右側(cè)時(shí)，則執(zhí)行第七調(diào)整操作，再執(zhí)行第六糾偏操作：

對(duì)所述第六糾偏操作的結(jié)果進(jìn)行判定，若達(dá)到所述糾偏成功條件，則表示完成糾偏操作，否則，執(zhí)行第八調(diào)整操作后，再執(zhí)行第六糾偏操作，直到達(dá)到所述糾偏成功條件為止；

所述第七調(diào)整操作為：令磁導(dǎo)引AGV向右旋轉(zhuǎn)角度β；

所述第六糾偏操作為：令磁導(dǎo)引AGV后退距離為L(zhǎng)，再令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述第八調(diào)整操作為：令磁導(dǎo)引AGV回旋角度為Δ×β；

當(dāng)磁導(dǎo)引AGV的脫磁類(lèi)別為異側(cè)脫磁時(shí)，則執(zhí)行第七糾偏操作：

所述第七糾偏操作為：令磁導(dǎo)引AGV原地旋轉(zhuǎn)角度為α；

所述步驟3中的原地旋轉(zhuǎn)角度α、旋轉(zhuǎn)角度β、后退或前進(jìn)距離L是分別如下方式獲得：

所述原地旋轉(zhuǎn)角度α的值為偏角e_α；

利用式(3)得到所述旋轉(zhuǎn)角度β：

式(3)中，e表示前后偏距總和的絕對(duì)值并由式(4)得到：

e＝|e_p1+e_p2| (4)

利用式(5)計(jì)算后退或前進(jìn)距離L：

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