本發(fā)明涉及AGV領(lǐng)域，尤其涉及一種基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置及方法。

背景技術(shù)：

AGV是Automated Guided Vehicle的縮寫，意即“自動導引運輸車”，是指裝備有電磁或光學等自動導引裝置，它能夠沿規(guī)定的導引路徑行駛，具有安全保護以及各種移載功能的運輸車，AGV屬于輪式移動機器人(Wheeled Mobile Robot)的范疇。

AGV通過裝備有電磁、光學陀螺儀、激光導航儀或GPRS等自動導引裝置，能夠沿著規(guī)劃的路徑實現(xiàn)物流輸送的任務。隨著工廠自動化水平的提高及人力資源成本的猛增，AGV在國內(nèi)得到迅猛的發(fā)展，在電力電子、家電、煙草等行業(yè)得到廣泛的應用。隨著中國制造2015、工業(yè)4.0及互聯(lián)網(wǎng)+時代的推進，AGV及AGVS在現(xiàn)代制造工廠及物流行業(yè)更加有很高的發(fā)展空間。

目前，廣泛使用的AGV主要是小型的潛伏式或拖曳式車型，采用的驅(qū)動方式是前后輪差速驅(qū)動。SD型AGV采用單輪驅(qū)動的方式，車體結(jié)構(gòu)采用單輪驅(qū)動及轉(zhuǎn)向，另外兩輪作為車體承重輪?；趩屋嗱?qū)動及轉(zhuǎn)向的方式，SD型AGV在倒車過程中，控制方式比較特別，除了需要控制車體的橫向距離偏差以外，還需要控制車體與導航磁條之間的姿態(tài)角。目前現(xiàn)有的SD型AGV都只能單向行走而缺少倒車的功能，靈活性比較差，因此，基于實際的功能需求，需要解決目前SD型AGV倒車姿態(tài)角控制的問題。

技術(shù)實現(xiàn)要素：

為了克服現(xiàn)有技術(shù)中存在的缺點和不足，本發(fā)明提供了一種基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置及方法。

本發(fā)明是通過以下技術(shù)方案實現(xiàn)的：一種基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置，包括

--控制器，所述控制器設置在AGV車體上；

--第一磁導航傳感器，所述第一磁導航傳感器設置在AGV車頭，所述第一磁導航傳感器接入到所述控制器的GPIO口；

--第二磁導航傳感器，所述第二磁導航傳感器設置在AGV后部，所述第二磁導航傳感器接入到所述控制器的GPIO口；

--運動控制器，所述運動控制器設置在AGV車體上，所述運動控制器與控制器采用CAN通信連接；

--伺服閉環(huán)控制系統(tǒng)，所述伺服閉環(huán)控制系統(tǒng)通過RS485與控制器通信連接，從而接收姿態(tài)角調(diào)整指令。

進一步地，所述閉環(huán)伺服控制系統(tǒng)包括相互通信連接的伺服驅(qū)動器、旋轉(zhuǎn)編碼器和伺服轉(zhuǎn)向電機。

進一步地，所述控制器為STM32F103ZTE芯片。

進一步地，所述控制器采用LQFP封裝形式。

進一步地，所述第一磁導航傳感器和第二磁導航傳感器分別為16位磁導航傳感器和8位磁導航傳感器，且第一磁導航傳感器和第二磁導航傳感器采用NPN輸入。

進一步地，所述第一磁導航傳感器和第二磁導航傳感器的安裝間距D為188cm。

同時，本發(fā)明還提供了一種基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制方法，包括以下步驟：

S1：預設第一磁導航傳感器的中心位置值SP1和第二磁導航傳感器的中心位置值SP2；

S2：根據(jù)第一磁導航傳感器的實際檢測值PV1，計算獲得第一磁導航傳感器的中心位置值SP1與實際檢測值PV1的差值E1，其中，E1＝SP1-PV1；

根據(jù)第二磁導航傳感器的實際檢測值PV2，計算獲得第二磁導航傳感器的中心位置值SP2與實際檢測值PV2的差值E2，其中，E2＝SP2-PV2；

S3：將E2進行第一級距離偏差PID運算并輸出運算結(jié)果SP3；

S4：根據(jù)第一磁導航傳感器和第二磁導航傳感器的安裝間距D及E1和E2，計算獲得實際姿態(tài)角的計算值PV3，其中，PV3＝(E2-E1)/D；

S5：計算獲得預設姿態(tài)角與實際姿態(tài)角的差值E3，其中，E3＝SP3-PV3；

S6：將E3作為第二級姿態(tài)角PID運算的輸入值，運算輸出結(jié)果α即為AGV車體倒車的姿態(tài)角打角值。

進一步地，上述步驟包含以下特征：所述SP1＝7.5cm，SP2＝3.5cm，D＝188cm。

進一步地，上述步驟包含以下特征：初始狀態(tài)或穩(wěn)定運行狀態(tài)下，E1＝0，E2＝0。

相對于現(xiàn)有技術(shù)，本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置通過在AGV上設置第一磁導航傳感器和第二磁導航傳感器，并將二者的檢測值輸入控制器，經(jīng)過兩級PID控制運算，得出AGV車體倒車的姿態(tài)角打角值，解決了現(xiàn)有技術(shù)中SD型AGV只能單向行走缺少倒車功能的技術(shù)難題，具有突破意義。本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制方法采用兩級雙閉環(huán)PID的運算，突破了現(xiàn)有技術(shù)中采用單PID控制的技術(shù)缺 陷，在SD型AGV領(lǐng)域，具有很強的指導意義和推廣價值。

為了更好地理解和實施，下面結(jié)合附圖詳細說明本發(fā)明。

附圖說明

圖1是本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置的連接示意圖。

圖2是磁導航傳感器的安裝位置示意圖。

圖3是SD型AGV的運動數(shù)學模型圖。

圖4是本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制方法的系統(tǒng)控制框圖。

具體實施方式

請參閱圖1-圖4。圖1是本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置的連接示意圖。圖2是磁導航傳感器的安裝位置示意圖。圖3是SD型AGV的運動數(shù)學模型圖。圖4是本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制方法的系統(tǒng)控制框圖。本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置，包括

--控制器3，控制器3設置在AGV車體上；

--第一磁導航傳感器1，第一磁導航傳感器1設置在AGV車頭，第一磁導航傳感器1接入到控制器3的GPIO口；

--第二磁導航傳感器2，第二磁導航傳感器2設置在AGV后部的落地叉處，第二磁導航傳感器2接入到控制器3的GPIO口；

--運動控制器4，運動控制器4設置在AGV車體上，運動控制器4與控制器3采用CAN通信連接，控制器3通過接收運動控制器4發(fā)送的動作指令，執(zhí)行相應的算法程序；

--伺服閉環(huán)控制系統(tǒng)，伺服閉環(huán)控制系統(tǒng)通過RS485與控制器通信連接，從而接收姿態(tài)角調(diào)整指令。

具體地，閉環(huán)伺服控制系統(tǒng)包括相互通信連接的伺服驅(qū)動器、旋轉(zhuǎn)編碼器和伺服轉(zhuǎn)向電機，伺服轉(zhuǎn)向電機通過齒輪的方式來實現(xiàn)驅(qū)動輪的轉(zhuǎn)向。本實施例的控制器3為STM32F103ZTE芯片并采用LQFP封裝形式。

第一磁導航傳感器1和第二磁導航傳感器2分別為16位磁導航傳感器和8位磁導航傳感器，且第一磁導航傳感器1和第二磁導航傳感器2采用NPN輸入，二者的輸入信號經(jīng)過軟件的濾波處理，消除對算法的噪聲干擾。本實施例優(yōu)選地將第一磁導航傳感器1和第二磁導航傳感器2的安裝間距D設置為D＝188cm，并且，優(yōu)選地將第一磁導航傳感器1的中心位置值SP1設置為SP1＝7.5cm，第二磁導航傳感器2的中心位置值SP2設置為SP2＝3.5cm。

為了更好地理解本發(fā)明，本實施例優(yōu)選地在圖2中標示了AGV的前輪10、后輪20以及 設置在AGV車體下的導航磁條30。本實施例的前輪10為AGV車體的驅(qū)動輪，兩個后輪20則為AGV車體的承重輪。

本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置在控制算法上采用兩級雙閉環(huán)PID的控制方法，下面具體說明該方法。

本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制方法，包括以下步驟：

S1：預設第一磁導航傳感器的中心位置值SP1和第二磁導航傳感器的中心位置值SP2。

通過運算控制器4對AGV車體的運動進行預設，本實施例的第一磁導航傳感器的中心位置值SP1預設為7.5cm，第二磁導航傳感器的中心位置值SP2預設為3.5cm。

S2：根據(jù)第一磁導航傳感器的實際檢測值PV1，計算獲得第一磁導航傳感器的中心位置值SP1與實際檢測值PV1的差值E1，其中，E1＝SP1-PV1。

根據(jù)第二磁導航傳感器的實際檢測值PV2，計算獲得第二磁導航傳感器的中心位置值SP2與實際檢測值PV2的差值E2，其中，E2＝SP2-PV2。

請參閱圖2，當AGV車體處于穩(wěn)定運行狀態(tài)或初始狀態(tài)時，第一磁導航傳感器1及第二磁導航傳感器2的中心位置均位于導航磁條30的正上方，即此時，E1＝0，E2＝0。

S3：將E2進行第一級距離偏差PID運算并輸出運算結(jié)果SP3。

S4：根據(jù)第一磁導航傳感器和第二磁導航傳感器的安裝間距D及E1和E2，計算獲得實際姿態(tài)角的計算值PV3，其中，PV3＝(E2-E1)/D。

如圖3所示，箭頭方向表示AGV車體處于倒車狀態(tài)，其中，AGV車體與X方向之間的夾角θ表示AGV車體的實際姿態(tài)角，由幾何關(guān)系，可以算出AGV車體的實際姿態(tài)角θ的計算值PV3＝(E2-E1)/D。實際姿態(tài)角θ的計算值PV3作為第二級姿態(tài)角PID運算的預設輸入姿態(tài)角值。

S5：計算獲得預設姿態(tài)角與實際姿態(tài)角的差值E3，其中，E3＝SP3-PV3。

根據(jù)SD型AGV的運動數(shù)學模型，將計算得出的預設姿態(tài)角與實際姿態(tài)角的差值E3作為第二級姿態(tài)角PID控制的輸入值。

S6：將E3作為第二級姿態(tài)角PID運算的輸入值，運算輸出結(jié)果α即為AGV車體倒車的姿態(tài)角打角值。

根據(jù)第二級姿態(tài)角PID運算的計算結(jié)果，將姿態(tài)角打角值α的指令傳送給閉環(huán)伺服控制系統(tǒng)的伺服轉(zhuǎn)向電機，從而平穩(wěn)地控制AGV車體倒車的姿態(tài)角。

相對于現(xiàn)有技術(shù)，本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒車姿態(tài)角控制裝置通過在AGV上設置第一磁導航傳感器和第二磁導航傳感器，并將二者的檢測值輸入控制器，經(jīng)過兩級PID控制運算，得出AGV車體倒車的姿態(tài)角打角值，解決了現(xiàn)有技術(shù)中SD型AGV只能單向行走缺少倒車功能的技術(shù)難題，具有突破意義。本發(fā)明的基于雙閉環(huán)PID控制的SD型AGV倒 車姿態(tài)角控制方法采用兩級雙閉環(huán)PID的運算，突破了現(xiàn)有技術(shù)中采用單PID控制的技術(shù)缺陷，在SD型AGV領(lǐng)域，具有很強的指導意義和推廣價值。

本發(fā)明并不局限于上述實施方式，如果對本發(fā)明的各種改動或變形不脫離本發(fā)明的精神和范圍，倘若這些改動和變形屬于本發(fā)明的權(quán)利要求和等同技術(shù)范圍之內(nèi)，則本發(fā)明也意圖包含這些改動和變形。