[發(fā)明專利]紅外光電導(dǎo)向AGV裝置及其控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210341985.5
• 申請(qǐng)日: 2012-09-14
• 公開（公告）號(hào): CN102854878A
• 公開（公告）日: 2013-01-02
• 發(fā)明（設(shè)計(jì)）人: 王偉;劉勝明;周勇;馬浩
• 申請(qǐng)（專利權(quán)）人: 蘇州工業(yè)園區(qū)永動(dòng)工業(yè)設(shè)備有限公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 南京蘇科專利代理有限責(zé)任公司 32102
• 代理人: 王玉國;陳忠輝

1.紅外光電導(dǎo)向AGV裝置，包括車體、驅(qū)動(dòng)輪和從動(dòng)輪以及鋪設(shè)于地面的導(dǎo)引反光帶，其特征在于：在車體底部的四端角位置各安裝一從動(dòng)輪，車體底部的中間部位沿縱向安裝有一組呈對(duì)稱分布的驅(qū)動(dòng)輪，驅(qū)動(dòng)輪均與無刷直流電機(jī)驅(qū)動(dòng)控制連接，每只驅(qū)動(dòng)輪的內(nèi)側(cè)設(shè)有速度檢測裝置，車體底部的中間部位沿橫向安裝有一組呈對(duì)稱分布的紅外光電導(dǎo)向裝置，紅外光電導(dǎo)向裝置的內(nèi)側(cè)安裝有紅外標(biāo)志檢測裝置，車體底部的四端角位置還各安裝一避障傳感器。

2.根據(jù)權(quán)利要求1所述的紅外光電導(dǎo)向AGV裝置，其特征在于：兩驅(qū)動(dòng)輪之間的軸距為900mm。

3.根據(jù)權(quán)利要求1所述的紅外光電導(dǎo)向AGV裝置，其特征在于：所述避障傳感器為GP2Y0A21紅外測距傳感器。

4.根據(jù)權(quán)利要求1所述的紅外光電導(dǎo)向AGV裝置，其特征在于：所述紅外光電導(dǎo)向裝置包含紅外發(fā)射器、紅外接收器、緩沖驅(qū)動(dòng)器和主控制單元，紅外接收器經(jīng)電壓比較器接入至主控制單元，電壓比較器上設(shè)有電壓調(diào)節(jié)電阻，主控制單元上連接譯碼器，譯碼器連接緩沖驅(qū)動(dòng)器，緩沖驅(qū)動(dòng)器上設(shè)有光強(qiáng)可調(diào)電阻，緩沖驅(qū)動(dòng)器經(jīng)掃描指示燈連接紅外發(fā)射器，主控制單元上設(shè)有通信串口。

5.權(quán)利要求1所述的紅外光電導(dǎo)向AGV裝置的控制方法，其特征在于：按照預(yù)設(shè)路徑的軌跡在地面鋪設(shè)導(dǎo)引反光帶，在需要鋪設(shè)動(dòng)作標(biāo)志的位置設(shè)置動(dòng)作標(biāo)志；

(1)AGV裝置運(yùn)行中，車體相對(duì)導(dǎo)引路徑的偏移信息通過紅外光電導(dǎo)向裝置進(jìn)行檢測，紅外光電導(dǎo)向裝置發(fā)射紅外光線，紅外光線經(jīng)過導(dǎo)引反光帶的發(fā)射得到車體相對(duì)導(dǎo)引反光帶的位置信息；位置信息的檢測步驟是：在AGV相對(duì)導(dǎo)引路線所在的運(yùn)動(dòng)線路上，定義實(shí)際位置和目標(biāo)位置之間的誤差為位置誤差，將AGV的運(yùn)動(dòng)位置表示為(d,α)，其中d為AGV偏移導(dǎo)引路線的法相位置誤差，α為AGV的方位角誤差，所述的方位角為AGV的當(dāng)前行進(jìn)路線與預(yù)期軌跡也就是導(dǎo)引路線之間的夾角；要求AGV沿著預(yù)定的導(dǎo)軌路線循跡前進(jìn)，設(shè)置所述的AGV的目標(biāo)位置為(0,0),由紅外光電導(dǎo)向裝置檢測AGV起點(diǎn)的位置是(0,0)，根據(jù)路徑的規(guī)劃，預(yù)先設(shè)置兩個(gè)驅(qū)動(dòng)輪之間的距離是L，前后兩個(gè)位置檢測裝置之間的距離是D；

目標(biāo)位置有兩種情況，當(dāng)沿著指導(dǎo)路徑循跡時(shí)，目標(biāo)位置為(0，0)；當(dāng)在一些特定轉(zhuǎn)彎動(dòng)作標(biāo)志位置時(shí)，AGV的目標(biāo)位置為(d，α)；

在需要轉(zhuǎn)彎的情況下導(dǎo)引車的控制采用前饋控制，在檢測到動(dòng)作標(biāo)志時(shí)，加入對(duì)應(yīng)動(dòng)作的前饋控制量，當(dāng)行駛過動(dòng)作標(biāo)志進(jìn)入循跡過程時(shí)，恢復(fù)正常循跡；

根據(jù)AGV車體運(yùn)行時(shí)的相對(duì)位置的幾何平面，測量AGV的導(dǎo)軌跟蹤誤差的具體步驟是：AGV的前后兩端各配置一個(gè)紅外光電導(dǎo)向裝置，當(dāng)車輛行駛時(shí)，紅外光電導(dǎo)向裝置的發(fā)射器按照固定的頻率發(fā)射紅外光，接收光電二極管采集反光導(dǎo)引路線反射回來的紅外光點(diǎn)；

兩個(gè)紅外光電導(dǎo)向裝置之間的距離固定為D；

預(yù)處理AGV車體行駛過程中在同一時(shí)刻采集到的從導(dǎo)引路線反射回來的紅外光點(diǎn)序列，接收光電二極管之間的距離固定，根據(jù)接收到的發(fā)射光點(diǎn)序列得到偏移距離，則前后兩個(gè)紅外光電導(dǎo)向裝置采集到的偏移距離分別是D1，D2；設(shè)定，當(dāng)AGV車體偏向左邊時(shí)候，測得偏移量是負(fù)值，當(dāng)AGV的車體偏向右邊時(shí)候，測得偏移量是正值，當(dāng)車體現(xiàn)行軌跡與導(dǎo)引路線的夾角偏向?qū)б肪€左側(cè)為逆時(shí)針的時(shí)候，角度為負(fù)值，當(dāng)偏向右側(cè)順時(shí)針的時(shí)候，角度為正值；

(2)檢測得到的數(shù)據(jù)通過紅外光電導(dǎo)向裝置的標(biāo)準(zhǔn)通信串口和主控單元MCU通信，將位置信息輸入主控單元MCU進(jìn)行處理；

位置信息的處理通過以下步驟實(shí)現(xiàn)：

根據(jù)幾何關(guān)系推算出；

L1=DD1D1-D2]]>

L2=DD2D1-D2]]>

θ=arctanD2L2=arctanD1L1]]>

d₁=sinθ×D₁

d₂=sinθ×D₂

其中，1是前端紅外光電導(dǎo)向裝置沿兩個(gè)紅外光電導(dǎo)向裝置中心線到導(dǎo)引反光帶之間距離，L2是后端紅外光電導(dǎo)向裝置沿兩個(gè)紅外光電導(dǎo)向裝置中心線到反光帶之間距離，θ為偏轉(zhuǎn)角；d₁是導(dǎo)引線到車體兩個(gè)紅外光電導(dǎo)向裝置的中心點(diǎn)的垂直距離，同理，d₂是導(dǎo)引線到車體兩個(gè)紅外光電導(dǎo)向裝置中心點(diǎn)的垂直距離；

(3)將得到的偏移量和偏轉(zhuǎn)角經(jīng)卡爾曼濾波器進(jìn)行濾波，得到平滑的變化曲線，進(jìn)一步修正AGV與導(dǎo)引路線的誤差；

(4)AGV運(yùn)行的過程中，在一些特定位置要求AGV執(zhí)行特定的動(dòng)作，在這些特定位置設(shè)置動(dòng)作標(biāo)志，動(dòng)作標(biāo)志是間隔性的等距反光帶，由一系列寬度相同的反光帶或者非反光帶按順序排列組成；

所述標(biāo)志檢測通過以下步驟；

根據(jù)紅外標(biāo)志檢測裝置檢測到的標(biāo)志的數(shù)字信號(hào)通過串口輸入MCU，主控裝置判斷動(dòng)作標(biāo)志代表的意義然后發(fā)出對(duì)應(yīng)的動(dòng)作控制命令，AGV做出相對(duì)應(yīng)的動(dòng)作，在AGV運(yùn)行的過程中，在一些特定位置要求AGV執(zhí)行特定的動(dòng)作，在特定位置設(shè)置動(dòng)作標(biāo)志，當(dāng)光線發(fā)射到反光帶上時(shí)，接收管接收到反射光，判斷為1，相反，判斷為0；

當(dāng)AGV沿著導(dǎo)引路徑以固定的速度v行進(jìn)時(shí)，經(jīng)過每一個(gè)小段反光帶的時(shí)間是相同的t，則根據(jù)在每一段時(shí)間t內(nèi)檢測到的高低電平的不同對(duì)標(biāo)志做出判斷；

標(biāo)志檢測裝置檢測到動(dòng)作標(biāo)志的信號(hào)是01101010，所代表的動(dòng)作是啟動(dòng)；

將啟動(dòng)動(dòng)作信號(hào)發(fā)送到主控MCU，主控板發(fā)出啟動(dòng)命令；

在有具體動(dòng)作控制位置，AGV的導(dǎo)向控制器與直線循跡控制器相同，控制器的前端設(shè)有前饋控制；

使AGV的目標(biāo)位置改變，以左轉(zhuǎn)動(dòng)，目標(biāo)位置為(-d，-α)，AGV將以目標(biāo)位置為終點(diǎn)前進(jìn)；

根據(jù)檢測到的不同的動(dòng)作標(biāo)志信號(hào)，代表不同的動(dòng)作意義，將檢測到的動(dòng)作標(biāo)志通過串口發(fā)送到MCU主控制器，

主控制單元經(jīng)過處理之后得到控制量，將控制量發(fā)送到驅(qū)動(dòng)，驅(qū)動(dòng)車體向目標(biāo)位置運(yùn)行；

當(dāng)需要改變特定位置的動(dòng)作時(shí)候，改變動(dòng)作標(biāo)志，或者通過人機(jī)界面對(duì)標(biāo)志所代表的動(dòng)作的意義加以改變；

(5)由避障傳感器通過紅外檢測到的距離的遠(yuǎn)近產(chǎn)生大小不同的模擬量信號(hào)，通過AD轉(zhuǎn)換接口與MCU相連接，主控制裝置將模擬信號(hào)轉(zhuǎn)換為對(duì)應(yīng)的數(shù)字信號(hào)也就是距離信息，根據(jù)距離的遠(yuǎn)近，然后發(fā)出轉(zhuǎn)彎、減速或者剎車的命令到驅(qū)動(dòng)輪控制器，使AGV系統(tǒng)避開障礙物減小損害；

(6)速度檢測裝置為脈沖信號(hào)檢測裝置，對(duì)單位時(shí)間內(nèi)接收到的脈沖信號(hào)個(gè)數(shù)進(jìn)行統(tǒng)計(jì)，并通過SPI接口發(fā)送到主控制單元，MCU計(jì)算出電機(jī)的實(shí)際轉(zhuǎn)速。

6.根據(jù)權(quán)利要求5所述的紅外光電導(dǎo)向AGV裝置的控制方法，其特征在于：軌跡跟蹤控制策略采用二次線型最優(yōu)控制和卡爾曼濾波器控制相結(jié)合的LQG控制策略；在AGV運(yùn)動(dòng)所在的平面直角坐標(biāo)系中，所述的位置檢測裝置檢測到的偏移導(dǎo)引路徑的偏移量d和偏轉(zhuǎn)角α；

所述路徑跟蹤控制方法步驟如下；

控制系統(tǒng)的建模：

AGV在初始狀態(tài)的時(shí)沒有偏差，由于外部干擾的影響，AGV的路徑隨著時(shí)間的推移出現(xiàn)略微的偏差，偏差的角度為參考軌跡和小車移動(dòng)的中心線的夾角，偏移的距離由兩個(gè)輪子中心點(diǎn)和參考軌跡確定，左驅(qū)動(dòng)輪的速度是V_L，右邊驅(qū)動(dòng)輪的速度是V_R，在一個(gè)非常短的時(shí)間Δt內(nèi)，速度的大小和方向都沒有變化，由幾何關(guān)系推導(dǎo)出以下信息；

Vc=VR+VL2]]>

Δθ=(VR-VL)×ΔtL]]>

Δd=VR+VL2×Δt×sinθ]]>

在上述公式中，L是兩個(gè)驅(qū)動(dòng)輪之間的距離，當(dāng)t接近0的時(shí)候，得到偏移距離和偏移角度的微分方程：

dθ=VR-VLLdt]]>

dd=VR+VL2sinθdt]]>

對(duì)上式進(jìn)行拉普拉斯變換，上述公式化為；

d(s)=(VR+VL)sin(θ)2s]]>

θ(s)=(VR-VL)Ls]]>

當(dāng)公式中的角度θ足夠小時(shí)，公式近似化為；

d(s)=(VR+VL)θ2s]]>

AGV控制系統(tǒng)的數(shù)學(xué)模型中，兩個(gè)輪子由兩個(gè)直流無刷電機(jī)分別驅(qū)動(dòng)，假定電機(jī)和輪子以相同的速度n運(yùn)行，輪半徑是r，電機(jī)的控制輸入是U，電機(jī)的負(fù)載時(shí)間常數(shù)是Tm，左右電機(jī)的參數(shù)分別是Tml和Tmr，左右電機(jī)的傳遞函數(shù)簡化為：

nR=kR1+TmRsUR]]>

nL=kL1+TmLsUL]]>

在上述公式中，K_L和K_R是與電機(jī)的反電動(dòng)勢相關(guān)的常數(shù)，因兩電機(jī)的參數(shù)相同，所以

T_mR=T_mL=T_m

此外，考慮到輪子的線速度和角速度之間的關(guān)系，得到左右輪的線速度如下所示，

VR=k1+TmsUR]]>

VL=k1+TmsUL]]>

系統(tǒng)包含θ與V_R+V_L相乘因子，系統(tǒng)屬于非線性系統(tǒng)，AGV是在一種固定的規(guī)則下運(yùn)行，控制輸入視為一個(gè)固定速度控制輸入和一個(gè)小的循跡控制輸入量，因此，該非線性系統(tǒng)采用小誤差系統(tǒng)的方法，將非線性系統(tǒng)轉(zhuǎn)換成線性系統(tǒng)并進(jìn)行設(shè)計(jì)；

無外部的干擾，左輪和右輪的給定信號(hào)是相同的，V_R=V_L=V_c，AGV的速度V_c是由常量U_c產(chǎn)生的，視為速度為常量的勻速直線運(yùn)動(dòng)，當(dāng)AGV離開預(yù)定的軌跡，給定信號(hào)將增加ΔU調(diào)節(jié)循跡；

U_R=U_C+ΔU

U_L=U_C-ΔU

電機(jī)線性速度的相應(yīng)輸出是：

V_R=V_c+ΔV

V_L=V_c-ΔV

在正常條件下AGV的平均運(yùn)行速度Vc是一個(gè)常數(shù)，假設(shè)狀態(tài)變量分別是X₁=ΔV,X₂=θ，X₃=d得狀態(tài)函數(shù)如下：

X.1X.2X.3=-1Tm001L000Vc/20X1X2X3+k00ΔU]]>

控制器的設(shè)計(jì)通過以下步驟實(shí)現(xiàn)：

采用LQI控制方法進(jìn)行導(dǎo)向控制，LQI伺服控制系統(tǒng)引入輸出誤差的積分作為額外的狀態(tài)變量，擴(kuò)展系統(tǒng)為；

ddtx(t)xr(t)=A0-C0x(t)xr(t)+B0u(t)+0Ir(t)]]>

式中

A=-1Tm001L000Vc20]]>B=k00]]>C=[0?0?1]

基于二次型目標(biāo)函數(shù)的相對(duì)于系統(tǒng)的最優(yōu)控制問題，最優(yōu)性能指標(biāo)值為；

J=∫0∞[xaTQxa(t)+uT(t)Ru(t)]dt]]>

式中xa=A0-C0,]]>Q，R為加權(quán)對(duì)角矩陣，階數(shù)分別與x_a和u對(duì)應(yīng)；

用狀態(tài)反饋增益及控制輸入表示為；

u(t)=-F1x(t)-F2xr(t)=-F1x(t)-F2∫0te(t)dt]]>

F＝[F₁?F₂]＝R^-1B^#TP

B#=B0,]]>P為對(duì)應(yīng)的代數(shù)里卡蒂方程的解；

在上述最優(yōu)控制器中，雖然傳感器測量到偏移量和偏轉(zhuǎn)角，但是在此過程中，由于外界的干擾，電壓的波動(dòng)，反光導(dǎo)引帶的起伏因素引起的擾動(dòng)，采用卡爾曼濾波器對(duì)中間的噪聲進(jìn)行處理；

卡爾曼濾波器的設(shè)計(jì)通過以下步驟實(shí)現(xiàn)；

對(duì)于偏移量和偏轉(zhuǎn)角兩個(gè)變量，傳感器直接測量得到，而在軌跡跟蹤的過程中，實(shí)際的AGV系統(tǒng)具有一系列不確定性，主要包括路面的隨機(jī)干擾輸入、傳感器的測量噪聲以及模型的不確定性，各種過程噪聲和觀測噪聲的干擾，將使?fàn)顟B(tài)變量的測量結(jié)果出現(xiàn)偏差，卡爾曼濾波器使用的條件是系統(tǒng)噪聲和測量噪聲均為白噪聲，卡爾曼濾波器進(jìn)行濾波；

根據(jù)分離定理，對(duì)系統(tǒng)的狀態(tài)變量X進(jìn)行估計(jì)，表達(dá)式為

X^.=AX^+BU+K1(Y-CX^)]]>

為狀態(tài)量的估計(jì)，K₁為濾波器增益

K1=PlBTRf-1]]>

式中P_l為對(duì)應(yīng)代數(shù)利卡迪方程的解，R_f對(duì)應(yīng)的加權(quán)矩陣；

根據(jù)分離原理，將線性二次最優(yōu)控制器和卡爾曼濾波器相結(jié)合，即為線性二次高斯LQG控制器；

根據(jù)設(shè)計(jì)的控制器，將位置檢測裝置測量到的相對(duì)軌跡的偏移量和偏移角進(jìn)行處理，計(jì)算相應(yīng)的控制量，并將控制量發(fā)送到驅(qū)動(dòng)單元對(duì)AGV進(jìn)行糾偏處理；

以AGV速度是1m/s的速度行走，讓AGV從某種初始狀態(tài)出發(fā)跟蹤設(shè)定的直線路徑，初始誤差設(shè)置為d=0，角度為θ=0；根據(jù)實(shí)驗(yàn)結(jié)果，得到較佳的結(jié)果；

根據(jù)結(jié)果得出，AGV在一個(gè)較短的時(shí)間內(nèi)達(dá)到穩(wěn)定狀態(tài)，且在隨后的行駛過程中，誤差和控制量在一個(gè)較小的范圍內(nèi)變化。