本發明屬于移動(dong)機器人領域，具體涉及一種差速驅動(dong)式AGV控制系統及自動(dong)循(xun)跡方法(fa)。

背景技術：

自(zi)動導(dao)航車(Automatic Guided Vehicle，AGV)是一(yi)種無人駕駛(shi)的智(zhi)能化(hua)搬運設備，屬于移動機器人范疇(chou)，是自(zi)動化(hua)物流系統的關鍵(jian)設備之(zhi)一(yi)。

差速(su)驅(qu)動式(shi)AGV具有體積小、自(zi)重輕、成本(ben)低廉、結構(gou)(gou)相對簡單等(deng)優(you)點，目前在(zai)AGV行(xing)業得到廣(guang)泛應用。基于該(gai)結構(gou)(gou)形式(shi)的(de)AGV能按(an)照預先(xian)鋪設好(hao)的(de)路徑(jing)準確(que)無(wu)誤的(de)行(xing)駛是AGV的(de)核心技術。

目前已公開的差速驅動式AGV技(ji)術都沒有對系統建立完整的控制模(mo)型，多采用模(mo)糊控制、魯棒控制等方法，將AGV系統看做一個“黑盒子”進行(xing)控制。然而這(zhe)些方法對控制器(qi)計算能力和實(shi)時(shi)性的要求都很高(gao)，在實(shi)際應(ying)用中實(shi)現的難度大、成(cheng)本(ben)高(gao)。

技術實現要素：

本發明所要解(jie)決的(de)技(ji)術問題是對差速驅動(dong)式AGV建立一個完整的(de)控制系統及自動(dong)循跡方法，以實現AGV對路徑(jing)的(de)可(ke)靠跟(gen)蹤。

本發明(ming)的(de)(de)基(ji)(ji)本原(yuan)理(li)是：以軌跡檢測模塊(kuai)反饋的(de)(de)地面路(lu)徑信息和導引基(ji)(ji)準之間的(de)(de)差(cha)作為控制器的(de)(de)輸入(ru)量，以AGV驅(qu)動(dong)部件的(de)(de)左右(you)輪差(cha)速比(bi)為依據(ju)，得到(dao)AGV驅(qu)動(dong)部件的(de)(de)左右(you)兩輪的(de)(de)控制速度，從而使AGV按(an)照預先鋪設(she)的(de)(de)路(lu)徑準確行駛。

本發明(ming)的技術方案(an)是(shi)：

差速驅動式AGV控制系統，其特殊之處在于：

包括(kuo)軌跡檢測(ce)裝置和控(kong)制器；所述控(kong)制器包括(kuo)數(shu)據(ju)處理(li)模(mo)(mo)(mo)塊(kuai)(kuai)、數(shu)據(ju)收發模(mo)(mo)(mo)塊(kuai)(kuai)和控(kong)制模(mo)(mo)(mo)塊(kuai)(kuai)；所述數(shu)據(ju)處理(li)模(mo)(mo)(mo)塊(kuai)(kuai)包括(kuo)左右輪差速比獲取模(mo)(mo)(mo)塊(kuai)(kuai)和左右輪速度獲取模(mo)(mo)(mo)塊(kuai)(kuai)；

所(suo)(suo)述(shu)軌(gui)跡(ji)檢(jian)測裝置的(de)輸(shu)(shu)入(ru)(ru)端與AGV的(de)驅(qu)動部件相連，輸(shu)(shu)出端與所(suo)(suo)述(shu)左(zuo)右(you)(you)輪差速(su)比獲(huo)取模(mo)(mo)(mo)塊的(de)輸(shu)(shu)入(ru)(ru)端相連；所(suo)(suo)述(shu)左(zuo)右(you)(you)輪差速(su)比獲(huo)取模(mo)(mo)(mo)塊的(de)輸(shu)(shu)出端和數據(ju)收(shou)發模(mo)(mo)(mo)塊的(de)輸(shu)(shu)出端均與所(suo)(suo)述(shu)左(zuo)右(you)(you)輪速(su)度獲(huo)取模(mo)(mo)(mo)塊的(de)輸(shu)(shu)入(ru)(ru)端相連；所(suo)(suo)述(shu)左(zuo)右(you)(you)輪速(su)度獲(huo)取模(mo)(mo)(mo)塊的(de)輸(shu)(shu)出端與所(suo)(suo)述(shu)控(kong)制模(mo)(mo)(mo)塊的(de)輸(shu)(shu)入(ru)(ru)端相連；所(suo)(suo)述(shu)控(kong)制模(mo)(mo)(mo)塊的(de)輸(shu)(shu)出端與AGV的(de)電機驅(qu)動器(qi)相連；

所(suo)(suo)(suo)(suo)(suo)述軌跡檢測裝置用于(yu)對預先(xian)設定好的(de)路徑(jing)進行識(shi)別，獲(huo)取(qu)地面路徑(jing)數(shu)據；所(suo)(suo)(suo)(suo)(suo)述左右(you)輪(lun)(lun)(lun)差(cha)速(su)比獲(huo)取(qu)模(mo)塊將所(suo)(suo)(suo)(suo)(suo)述地面路徑(jing)數(shu)據和(he)(he)導(dao)引(yin)基(ji)(ji)準進行比較，得(de)到AGV驅動部(bu)(bu)件(jian)的(de)左右(you)輪(lun)(lun)(lun)差(cha)速(su)比；所(suo)(suo)(suo)(suo)(suo)述數(shu)據收(shou)發模(mo)塊用于(yu)接(jie)收(shou)和(he)(he)發送AGV的(de)車速(su)基(ji)(ji)準值(zhi)(zhi)；所(suo)(suo)(suo)(suo)(suo)述左右(you)輪(lun)(lun)(lun)速(su)度獲(huo)取(qu)模(mo)塊根據所(suo)(suo)(suo)(suo)(suo)述車速(su)基(ji)(ji)準值(zhi)(zhi)和(he)(he)所(suo)(suo)(suo)(suo)(suo)述左右(you)輪(lun)(lun)(lun)車速(su)比得(de)到AGV驅動部(bu)(bu)件(jian)的(de)左右(you)輪(lun)(lun)(lun)控制(zhi)速(su)度；所(suo)(suo)(suo)(suo)(suo)述控制(zhi)模(mo)塊根據所(suo)(suo)(suo)(suo)(suo)述左右(you)輪(lun)(lun)(lun)控制(zhi)速(su)度對AGV進行控制(zhi)。

上述軌跡檢(jian)(jian)測裝置采用光電檢(jian)(jian)測模(mo)塊(kuai)。

上述軌(gui)跡檢測(ce)裝(zhuang)置(zhi)采用磁檢測(ce)模(mo)塊。

上述(shu)軌跡檢(jian)測裝置采用CCD探測器模塊。

上述軌(gui)(gui)跡檢(jian)(jian)測裝(zhuang)(zhuang)置(zhi)(zhi)包括安裝(zhuang)(zhuang)在(zai)AGV驅(qu)動部(bu)件(jian)(jian)前(qian)端的前(qian)置(zhi)(zhi)軌(gui)(gui)跡檢(jian)(jian)測模塊(kuai)和(he)安裝(zhuang)(zhuang)在(zai)AGV驅(qu)動部(bu)件(jian)(jian)中部(bu)的后置(zhi)(zhi)軌(gui)(gui)跡檢(jian)(jian)測裝(zhuang)(zhuang)置(zhi)(zhi)；前(qian)置(zhi)(zhi)軌(gui)(gui)跡檢(jian)(jian)測裝(zhuang)(zhuang)置(zhi)(zhi)和(he)后置(zhi)(zhi)軌(gui)(gui)跡檢(jian)(jian)測裝(zhuang)(zhuang)置(zhi)(zhi)的輸入端均與AGV驅(qu)動部(bu)件(jian)(jian)相連，輸出端均與所(suo)述左右(you)輪(lun)差(cha)速比獲取(qu)模塊(kuai)的輸入端相連。

上(shang)述AGV控制系統還包括與所述數(shu)據收發模塊的輸入(ru)端相連的操作面板、手控器或上(shang)位機。

基于上述差(cha)速(su)驅動式AGV控(kong)制系統的自動循跡方法，其特殊之處在于：包括以下步(bu)驟：

步(bu)驟(zou)一：鋪設AGV運行軌跡

在預先設定好的路徑上(shang)鋪設與地面有明顯色差(cha)的色帶，或者鋪設磁(ci)條；

步(bu)驟二：對步(bu)驟一所鋪設的(de)軌跡(ji)進(jin)行識(shi)別(bie)，獲(huo)取(qu)鋪設軌跡(ji)的(de)路徑數(shu)據；

步驟(zou)三(san)：根據導引基準(zhun)線和所述(shu)路徑數據得(de)到AGV驅動部件的左右輪差速比；

步驟(zou)四：根據左(zuo)右(you)輪基準速度(du)和所(suo)述左(zuo)右(you)輪差速比得到(dao)AGV驅動部件的左(zuo)右(you)輪控制速度(du)；

步驟(zou)五：控制模塊根據所述左右輪控制速(su)度(du)對AGV進行控制。

上(shang)述(shu)步驟(zou)二(er)中(zhong)的軌跡檢(jian)測裝置(zhi)包(bao)括安(an)(an)裝在AGV驅(qu)動部(bu)件前端的前置(zhi)軌跡檢(jian)測裝置(zhi)和安(an)(an)裝在AGV驅(qu)動部(bu)件中(zhong)部(bu)的后置(zhi)軌跡檢(jian)測裝置(zhi)。

上述(shu)步驟(zou)三中獲取AGV驅動部件(jian)的(de)(de)左右輪差速比的(de)(de)具體方法為：

1)將軌(gui)跡檢測(ce)裝(zhuang)置(zhi)(zhi)獲取的路(lu)徑(jing)數(shu)據與導引基準線(xian)進行比較，得(de)到(dao)前置(zhi)(zhi)軌(gui)跡檢測(ce)裝(zhuang)置(zhi)(zhi)獲取的鋪(pu)設軌(gui)跡與導引基準線(xian)的橫向偏(pian)(pian)移(yi)距離和(he)后置(zhi)(zhi)軌(gui)跡檢測(ce)裝(zhuang)置(zhi)(zhi)得(de)到(dao)的鋪(pu)設軌(gui)跡與導引基準線(xian)的橫向偏(pian)(pian)移(yi)距離；

2)將AGV驅動部件中心(xin)點(dian)軌跡與鋪(pu)設(she)的AGV運(yun)行軌跡的切點(dian)記(ji)為F，過(guo)F點(dian)作(zuo)驅動部件輪軸的垂線，垂足為H；計算點(dian)A和點(dian)H之間的距離(li)AH：

式中AD是(shi)前、后軌(gui)(gui)跡(ji)檢測(ce)(ce)裝置中心點之(zhi)間的(de)距離(li)，GA是(shi)后置軌(gui)(gui)跡(ji)檢測(ce)(ce)裝置獲(huo)取的(de)鋪(pu)設(she)軌(gui)(gui)跡(ji)與導引(yin)基準線之(zhi)間的(de)橫(heng)向偏移(yi)距離(li)，GQ是(shi)前置軌(gui)(gui)跡(ji)檢測(ce)(ce)裝置獲(huo)取的(de)鋪(pu)設(she)軌(gui)(gui)跡(ji)與后置軌(gui)(gui)跡(ji)檢測(ce)(ce)裝置獲(huo)取的(de)鋪(pu)設(she)軌(gui)(gui)跡(ji)之(zhi)間的(de)橫(heng)向偏移(yi)距離(li)；

3)計算點(dian)F和點(dian)H之間的距離FH:

4)計算左右輪(lun)差速比

式中v₁是左輪速度，v₂是右輪速度，BC是AGV驅動部件左右輪中(zhong)心點之間的(de)距離。

上述步(bu)驟四(si)中獲取AGV驅動部件的(de)左(zuo)(zuo)右(you)輪(lun)(lun)(lun)控(kong)制速(su)(su)(su)度(du)的(de)方法(fa)為：用數(shu)據收發(fa)模塊(kuai)接(jie)收到的(de)左(zuo)(zuo)右(you)輪(lun)(lun)(lun)的(de)基準速(su)(su)(su)度(du)分別乘以所述左(zuo)(zuo)右(you)輪(lun)(lun)(lun)差速(su)(su)(su)比即得(de)到左(zuo)(zuo)右(you)輪(lun)(lun)(lun)的(de)控(kong)制速(su)(su)(su)度(du)。

本發明的有益效果：

本發明開(kai)創性地對(dui)差(cha)速驅動式AGV建立(li)了完整的控制系統并(bing)提供了基于該控制系統的自動循跡方(fang)法，能(neng)夠(gou)兼容現有差(cha)速驅動式AGV的多種導(dao)引方(fang)式，實現AGV對(dui)路徑的可靠跟蹤。

附圖說明

圖1是(shi)本發明的原(yuan)理框圖；

圖2是AGV的驅動部件的俯視圖；

圖3是AGV瞬(shun)時運(yun)行(xing)軌(gui)跡(ji)圖。

具體實施方式

以下參見圖1～圖3，對本(ben)發(fa)明(ming)作(zuo)詳細說明(ming)。

一、軌跡檢測裝置

軌跡(ji)(ji)檢測(ce)(ce)(ce)裝(zhuang)置(zhi)(zhi)包括安裝(zhuang)在AGV驅動(dong)部(bu)件前(qian)端的(de)前(qian)置(zhi)(zhi)軌跡(ji)(ji)檢測(ce)(ce)(ce)裝(zhuang)置(zhi)(zhi)和安裝(zhuang)在AGV驅動(dong)部(bu)件中間的(de)后(hou)置(zhi)(zhi)軌跡(ji)(ji)檢測(ce)(ce)(ce)裝(zhuang)置(zhi)(zhi)；前(qian)置(zhi)(zhi)、后(hou)置(zhi)(zhi)軌跡(ji)(ji)檢測(ce)(ce)(ce)裝(zhuang)置(zhi)(zhi)同時對預先設定(ding)好的(de)路徑進行識別。

軌跡(ji)檢測(ce)主(zhu)要有(you)三種(zhong)方(fang)式：

1：在預(yu)先設定好(hao)的路徑上鋪(pu)設與地(di)面(mian)有明顯色差的色帶，軌跡檢(jian)測(ce)裝置采用(yong)光電檢(jian)測(ce)模塊，得到(dao)路徑數(shu)據；

2：在(zai)預先設(she)定(ding)好的路徑上鋪設(she)磁(ci)條(tiao)，軌跡(ji)檢(jian)測(ce)裝置采用磁(ci)檢(jian)測(ce)模塊(kuai)，得(de)到路徑數據(ju)；

3：在預先設(she)定好的路(lu)徑上鋪(pu)設(she)與地面(mian)有明顯色差的色帶，檢測裝置采用CCD相機，得到路(lu)徑數據。

本發明(ming)的軌(gui)跡(ji)檢測(ce)裝(zhuang)置輸出的路(lu)(lu)徑數(shu)據為一個16位的二進制(zhi)碼，代表了軌(gui)跡(ji)檢測(ce)裝(zhuang)置內部(bu)16個采樣通(tong)道(dao)檢測(ce)出的下方色帶或(huo)磁(ci)條，根據該路(lu)(lu)徑數(shu)據可(ke)以(yi)判斷色帶或(huo)磁(ci)條相對于軌(gui)跡(ji)檢測(ce)裝(zhuang)置的偏離位置。

二、控制器

本發(fa)明的控制器可以(yi)采(cai)(cai)用單片(pian)機、DSP、FPGA等(deng)嵌入式芯片(pian)或PLC、工控機等(deng)集成控制器。本實施例中采(cai)(cai)用DSP核心控制器。

DSP核心控制器由(you)(you)數(shu)據處(chu)(chu)理模(mo)塊(kuai)(kuai)、數(shu)據收(shou)發模(mo)塊(kuai)(kuai)和控制模(mo)塊(kuai)(kuai)三部分組(zu)成(cheng)，其中數(shu)據處(chu)(chu)理模(mo)塊(kuai)(kuai)由(you)(you)左右輪差速比獲取(qu)(qu)模(mo)塊(kuai)(kuai)和左右輪速度獲取(qu)(qu)模(mo)塊(kuai)(kuai)組(zu)成(cheng)。

1、各模(mo)塊的功能如下：

左(zuo)右輪(lun)差速比獲取模塊根據(ju)(ju)導引基(ji)準線和軌跡(ji)檢測裝置(zhi)所獲得的(de)(de)路徑數據(ju)(ju)得到(dao)AGV驅動部件(jian)的(de)(de)左(zuo)右輪(lun)差速比；導引基(ji)準線是前(qian)置(zhi)軌跡(ji)檢測裝置(zhi)和后置(zhi)軌跡(ji)檢測裝置(zhi)采樣點的(de)(de)中(zhong)線的(de)(de)連線，即驅動部件(jian)的(de)(de)縱(zong)向(xiang)中(zhong)線，如圖(tu)3所示。

數(shu)據收發(fa)模塊(kuai)用于接收和發(fa)送來自操作(zuo)面(mian)板、手控器或上位機發(fa)來的(de)AGV驅動部件的(de)左右輪基準(zhun)速度(du)，即(ji)用戶或工業現場當前時刻希望AGV行駛(shi)的(de)速度(du)，也即(ji)車(che)速基準(zhun)速度(du)；

左右輪(lun)速度(du)獲取模塊根據左右輪(lun)基(ji)準速度(du)和左右輪(lun)差速比得到左右輪(lun)控制速度(du)；

控(kong)制模塊(kuai)根(gen)據左右(you)輪控(kong)制速度分(fen)別(bie)產生左輪PWM波(bo)和(he)(he)右(you)輪PWM波(bo)，左輪PWM波(bo)和(he)(he)右(you)輪PWM波(bo)分(fen)別(bie)對(dui)AGV驅動部件的左輪和(he)(he)右(you)輪進行控(kong)制，從而實現對(dui)AGV的控(kong)制。這里PWM波(bo)為脈(mo)沖寬度調制，即占空比可(ke)變的脈(mo)沖波(bo)形。

2、左右輪差速比的(de)具體計(ji)算方法

(1)將軌(gui)(gui)跡(ji)檢(jian)測裝置(zhi)(zhi)獲(huo)取(qu)的(de)(de)路徑數據(ju)與導引基準(zhun)線進(jin)行(xing)比較，通過(guo)計算處理得出前置(zhi)(zhi)軌(gui)(gui)跡(ji)檢(jian)測裝置(zhi)(zhi)獲(huo)取(qu)的(de)(de)鋪(pu)設軌(gui)(gui)跡(ji)與導引基準(zhun)線的(de)(de)橫(heng)向(xiang)(xiang)偏移距(ju)離和(he)后置(zhi)(zhi)軌(gui)(gui)跡(ji)檢(jian)測裝置(zhi)(zhi)得到的(de)(de)鋪(pu)設軌(gui)(gui)跡(ji)與導引基準(zhun)線的(de)(de)橫(heng)向(xiang)(xiang)偏移距(ju)離。

(2)利用下述公式(1)～(3)計算得到(dao)AGV驅動(dong)部(bu)件的(de)左(zuo)右輪差速比：

上式中AD是前后檢測裝置的距離，GA是后置軌跡檢測裝置得到的鋪設軌跡與導引基準線之間的橫向偏移距離；GQ是前置軌跡檢測裝置得到的鋪設軌跡與后置軌跡檢測裝置得到的鋪設軌跡之間的橫向偏移距離；v₁是左輪速度；v₂是(shi)右輪速(su)度；BC是(shi)驅動裝置(zhi)左右輪中心點之(zhi)間的距離(li)。

(3)用(yong)AGV驅(qu)動部(bu)件的(de)左右(you)輪基準速(su)(su)度分別(bie)乘以左右(you)輪差速(su)(su)比(bi)，得到(dao)左右(you)輪的(de)控制(zhi)速(su)(su)度。