李嘉楠+王富東+朱成海+楊歆豪

摘 要： 為改進(jìn)當(dāng)前固定軌道式的工業(yè)貨物搬運(yùn)方式，實(shí)現(xiàn)倉庫貨物按任意設(shè)定的三維軌跡進(jìn)行精確搬運(yùn)的目標(biāo)，提出三線式三維懸掛運(yùn)動控制系統(tǒng)的模型。以DSPIC30F4011單片機(jī)為控制核心，F(xiàn)utaba S3010舵機(jī)作為執(zhí)行機(jī)構(gòu)，張力檢測機(jī)構(gòu)作為保護(hù)機(jī)構(gòu)，LCD屏和按鍵作為設(shè)定顯示機(jī)構(gòu)，搭建系統(tǒng)模型，根據(jù)模型建立數(shù)學(xué)模型，并進(jìn)行相關(guān)算法的設(shè)計(jì)，最終實(shí)現(xiàn)三線式三維運(yùn)動協(xié)調(diào)控制。

關(guān)鍵詞： 三線式； 運(yùn)動控制； 舵機(jī)； 圓軌跡方程

中圖分類號： TN02?34； TP23 文獻(xiàn)標(biāo)識碼： A 文章編號： 1004?373X（2017）13?0148?04

Abstract： In order to improve the current industry goods transportation way with fixed track， and realize the target that the warehouse goods can be transported according to the 3D track set arbitrarily， a model of trilinear 3D suspended motion control system is put forward. To construct the system model， the DSPIC30F4011 single chip microcomputer is taken as the control kernel， the Futaba S3010 steering engine is employed as the actuator， the tension detecting mechanism is acted as the protection mechanism， and the LCD screen and key are used as the setup display mechanism. According to the system model， its mathematical model was established. The relevant algorithm was designed to realize the trilinear 3D motion coordination control.

Keywords： three?line type； motion control； steering engine； circular path equation

0 引 言

我國在貴州安裝了世界最大的射電望遠(yuǎn)鏡，其直徑長達(dá)500 m，需要安裝4 450塊反射板，并且安裝精度必須達(dá)到5 mm。諸如這樣的浩大工程，如果安裝固定軌道來完成反射板的安裝，其工程量將極為浩大，并且精度難以達(dá)到要求。同樣，在工業(yè)應(yīng)用中，對貨物的搬運(yùn)方式也是層出不窮。簡單的諸如車間的貨物有的采用吊車或者起重機(jī)等設(shè)備[1]，通常這些設(shè)備的運(yùn)動都是靠手工操作，對操作者的勞動強(qiáng)度較大[2]。當(dāng)然，各行各業(yè)自動化的貨物搬運(yùn)方式也在不斷發(fā)展，例如皮帶式裝車輔助設(shè)備、真空吸盤助力搬運(yùn)設(shè)備[3]、地鏈搬運(yùn)系統(tǒng)[4]、堆垛機(jī)控制系統(tǒng)[5]等，增加了貨物搬運(yùn)的自動化程度，為人們的生產(chǎn)帶來了方便。但是，這些方式都需要固定軌道，固定軌道搬運(yùn)控制不靈活、成本較大、安裝復(fù)雜，難以實(shí)現(xiàn)精確的任意軌跡的三維搬運(yùn)。

因此，本文提出一種新的三線式三維運(yùn)動控制的貨物運(yùn)輸搬運(yùn)控制思路，以三臺舵機(jī)輸出軸旋轉(zhuǎn)角度為直接控制量，通過與舵機(jī)輸出軸相連的桿作為傳動機(jī)構(gòu)，實(shí)現(xiàn)對貨物運(yùn)動控制的目的。研究的難點(diǎn)在于協(xié)調(diào)三臺舵機(jī)的輸出軸的旋轉(zhuǎn)角度關(guān)系，使貨物按照指定的路徑運(yùn)動到指定位置。根據(jù)上述思路，本文進(jìn)行了示例系統(tǒng)模型的搭建。通過將三臺Futaba S3010舵機(jī)分別安裝在不同位置，其輸出軸連接連桿，桿的另一端連接繩，繩通過一個(gè)固定滑輪連接到貨物，通過連桿的旋轉(zhuǎn)帶動貨物的運(yùn)動。通過三臺舵機(jī)各自旋轉(zhuǎn)相應(yīng)的角度，達(dá)到貨物的某一軌跡的運(yùn)動效果。本文以傾斜的圓軌跡為例實(shí)現(xiàn)示例的功能。

1 建立數(shù)學(xué)模型

1.1 系統(tǒng)整體結(jié)構(gòu)

根據(jù)系統(tǒng)的設(shè)計(jì)需要，搭建如圖1所示的系統(tǒng)。在圖1系統(tǒng)結(jié)構(gòu)中，是一個(gè)等邊三角形，點(diǎn)為等邊三角形的中心。為避免線直接纏裹在輸出軸上導(dǎo)致軸半徑變化造成誤差，在舵機(jī)輸出軸上安裝連桿。分別從與舵機(jī)A、舵機(jī)B、舵機(jī)C輸出軸相連的桿的另一端引出三條線，三條線的另一端相交于待搬運(yùn)物塊。點(diǎn)處安裝固定滑輪作為固定點(diǎn)，且安裝有3個(gè)張力傳感器，用于防止張力過大發(fā)生繩斷的情況?？梢酝ㄟ^三個(gè)舵機(jī)分別旋轉(zhuǎn)不同的角度，牽引物體按照某一特定軌跡到達(dá)空間內(nèi)的任一點(diǎn)。

1.2 數(shù)學(xué)模型的建立

如圖2所示，現(xiàn)以點(diǎn)為原點(diǎn)，為軸，建立三維直角坐標(biāo)系。為建立規(guī)范化數(shù)學(xué)方程，作如下假設(shè)：設(shè)點(diǎn)的坐標(biāo)為點(diǎn)的坐標(biāo)為，點(diǎn)的坐標(biāo)為，點(diǎn)坐標(biāo)為，設(shè)三根等長的線的原始長度為點(diǎn)到點(diǎn)的距離分別為。

以舵機(jī)A為例，設(shè)舵機(jī)A連桿的長度為連桿與平面的夾角為舵機(jī)旋轉(zhuǎn)中心點(diǎn)到固定點(diǎn)的距離為連桿末端到點(diǎn)的弧線長度為。具體示意圖如圖2所示。同理，可得舵機(jī)與平面夾角為弧線長度為。

將式（2）和式（3）代入式（1），建立舵機(jī)旋轉(zhuǎn)角度與待控制點(diǎn)坐標(biāo)的關(guān)系：

根據(jù)分析，在本系統(tǒng)中控制量為三個(gè)舵機(jī)的旋轉(zhuǎn)角度，即。根據(jù)給定的任意點(diǎn)坐標(biāo)，得到相對應(yīng)的控制量。因此，根據(jù)式（4），將寫成由表示的形式：

根據(jù)分析可知，點(diǎn)的坐標(biāo)為被控制量，點(diǎn)到點(diǎn)的距離以及舵機(jī)旋轉(zhuǎn)角度為可控量，其中為直接控制量，為間接控制量。通過控制進(jìn)而控制點(diǎn)的坐標(biāo)。

式（5）所示的系統(tǒng)數(shù)學(xué)模型方程中，將要求點(diǎn)坐標(biāo)代入，即可得到所需的控制量。至此，系統(tǒng)的數(shù)學(xué)模型建立完畢。

2 系統(tǒng)的硬件設(shè)計(jì)

2.1 控制機(jī)構(gòu)

本系統(tǒng)的控制部分由DSPIC30F4011單片機(jī)來完成。PIC單片機(jī)由美國Microchip公司推出，其硬件系統(tǒng)設(shè)計(jì)便捷，指令系統(tǒng)設(shè)計(jì)精煉，采用精簡指令集以及哈佛總線結(jié)構(gòu)，使其擁有速度高、功率低、驅(qū)動電流大及控制能力強(qiáng)的優(yōu)點(diǎn)[6]。它集成了單片機(jī)的控制功能和數(shù)字信號處理器（DSP）的計(jì)算能力[7]，有專門的PWM模塊，且具有一定的驅(qū)動能力，對于對功率要求不高的電機(jī)，無需驅(qū)動電路，直接將芯片產(chǎn)生的PWM波送出即可。本設(shè)計(jì)中PWM波接收機(jī)構(gòu)是舵機(jī)，直接通過芯片PWM的發(fā)生端口連接舵機(jī)端口即可完成相關(guān)功能，簡單方便。

2.2 交互機(jī)構(gòu)

本系統(tǒng)采用鍵盤完成對整個(gè)系統(tǒng)的預(yù)先設(shè)定功能，通過金鵬OCMJ2X8C型帶中文顯示功能的LCD屏完成對相關(guān)參數(shù)的顯示。當(dāng)系統(tǒng)處于功能設(shè)定模式時(shí)，LCD屏顯示相對應(yīng)的設(shè)置量，當(dāng)系統(tǒng)處于運(yùn)行階段時(shí)，LCD屏將顯示核心參數(shù)的運(yùn)行變化情況。

2.3 執(zhí)行機(jī)構(gòu)

針對本設(shè)計(jì)的具體情況，直接控制量為旋轉(zhuǎn)角度，因此只有步進(jìn)電機(jī)和舵機(jī)可以作為本系統(tǒng)的執(zhí)行機(jī)構(gòu)。步進(jìn)電機(jī)是將電脈沖信號轉(zhuǎn)換成角位移的執(zhí)行元件，其電機(jī)轉(zhuǎn)子的轉(zhuǎn)角與輸入的脈沖數(shù)成正比[8]。而舵機(jī)則是通過輸入脈沖的脈沖寬度來控制旋轉(zhuǎn)角度，無論外界轉(zhuǎn)矩如何，其始終保持在某一個(gè)角度，直到得到一個(gè)新的脈沖寬度的脈沖[9]。

雖然二者均可以完成相應(yīng)功能，但是從控制的難易程度來看，相比于步進(jìn)電機(jī)控制步數(shù)而言，舵機(jī)的直接角度控制顯得更為方便簡單，更為直接。并且舵機(jī)具有結(jié)構(gòu)簡單，易于控制，可靠性高[10]的優(yōu)點(diǎn)，因此本文的執(zhí)行機(jī)構(gòu)為三臺由日本Futaba公司生產(chǎn)的S3010舵機(jī)。S3010舵機(jī)成本低，且具有較大的扭矩，且反應(yīng)速度快。

該舵機(jī)有三根外接線，電源線和地線，以及一根控制信號線，無需驅(qū)動電路，直接將芯片端口產(chǎn)生的PWM波信號送入信號控制端即可。S3010舵機(jī)的基本使用方法如下：通過給定不同的脈沖寬度，舵機(jī)將旋轉(zhuǎn)一定的角度?？刂魄靶枰獪y定時(shí)所需的PWM波的脈寬和時(shí)的脈寬。

2.4 張力檢測機(jī)構(gòu)

在貨物吊運(yùn)過程中，會存在兩種極端的情況：一種是某一根吊繩承受的張力過大；另一種情況則是某一根吊繩所承受的張力接近于零。上述兩種情況中，前者將會造成繩子斷裂，產(chǎn)生嚴(yán)重事故，后者則會造成運(yùn)動軌跡發(fā)生嚴(yán)重偏離。作為模型，考慮上述兩種情況是必要的。因此模型中引入了張力檢測傳感器，預(yù)警吊繩所受張力過大或者為零等極端情況。張力傳感器根據(jù)四片等阻值應(yīng)變片的阻值變化產(chǎn)生的不同電壓輸出，經(jīng)濾波放大后進(jìn)行張力計(jì)算。在本模擬系統(tǒng)中，采用云想JL?SOMT040型張力傳感器進(jìn)行張力檢測。將檢測的張力值大于事先鑒定的繩子張力極限值的90%，或者接近于零時(shí)的情況視為報(bào)警信號。

2.5 硬件結(jié)構(gòu)框圖

根據(jù)系統(tǒng)的設(shè)計(jì)要求，本文搭建了如圖3所示的硬件控制電路示意圖。

3 系統(tǒng)的軟件設(shè)計(jì)

本設(shè)計(jì)中，控制懸掛物的運(yùn)動軌跡，使懸掛物按照某一方向運(yùn)動，關(guān)鍵在于三個(gè)舵機(jī)輸出軸的旋轉(zhuǎn)角度的相互配合。通過三個(gè)舵機(jī)同時(shí)旋轉(zhuǎn)不同的特定角度，從而使懸掛物朝某個(gè)方向運(yùn)動。而三維模型前文已經(jīng)建立，控制時(shí)只需輸入任何目標(biāo)運(yùn)動軌跡的方程即可完成任意軌跡的運(yùn)動。本文以空間中傾斜的圓軌跡為例進(jìn)行分析。

3.1 空間圓軌跡方程的建立

要完成空間傾斜圓軌跡的控制，必須要建立三維空間中的圓方程。對于空間傾斜圓并無固定公式，因此，本文作出如下簡單推導(dǎo)。規(guī)定該空間圓面與平面夾角為30°，與平面垂直，如圖4所示。

3.2 控制算法實(shí)現(xiàn)

根據(jù)3.1節(jié)得到的空間傾斜圓的方程，下面就可以通過控制算法實(shí)現(xiàn)貨物按該圓軌跡進(jìn)行運(yùn)動。本設(shè)計(jì)中，物體按傾斜圓軌跡運(yùn)動，運(yùn)動到圓上某個(gè)指定位置停止。具體算法如下所示：

（1） 完成系統(tǒng)的初始化，以及舵機(jī)的初始0°位置的回歸。

（2） 讀取設(shè)定的圓半徑，圓心位置以及目標(biāo)點(diǎn)信息。

（3） 以為自變量，和為應(yīng)變量，根據(jù)半徑要求，確定的起點(diǎn)和終點(diǎn)值，并根據(jù)精度的需求，對合理的離散化取點(diǎn)。

（4） 根據(jù)每個(gè)離散點(diǎn)的值，通過式（5）換算出相應(yīng)的值。

（5） 從第一個(gè)離散點(diǎn)開始，根據(jù)對應(yīng)的值，計(jì)算出相應(yīng)的脈沖寬度，輸出對應(yīng)的三個(gè)PWM波分別控制三個(gè)舵機(jī)。

（6） 讀取三個(gè)張力傳感器電壓輸出，在數(shù)字化轉(zhuǎn)換后進(jìn)行相關(guān)計(jì)算，若張力大于極限張力的90%或者張力接近于0，停止所有舵機(jī)運(yùn)行，并且發(fā)出報(bào)警信號，否則執(zhí)行下一步。

（7） 取下一個(gè)離散點(diǎn)，重復(fù)步驟（4），步驟（5），直到運(yùn)動到目標(biāo)點(diǎn)為止，等待下一個(gè)指令。

4 模擬系統(tǒng)的調(diào)試

根據(jù)設(shè)計(jì)思路，搭建了如圖6所示的三線式三維運(yùn)動控制系統(tǒng)的模型，并完成了以傾斜的圓軌跡為示例的運(yùn)動軌跡控制模擬。為清晰地反映運(yùn)動軌跡效果，本文在被控物塊下加上鉛筆，記錄運(yùn)動軌跡，記錄的運(yùn)動軌跡如圖7所示。

對于圖7，為使軌跡更加清晰，對圖片進(jìn)行了相關(guān)圖像學(xué)的處理。為驗(yàn)證可行性，本文對直線運(yùn)動、平面圓運(yùn)動、斜面圓運(yùn)動進(jìn)行了多次測試，測試結(jié)果如表1所示。

表1中，直線誤差的測量是直接通過直尺進(jìn)行測量。圓則是以同一點(diǎn)作為圓心，多次作半徑為5 cm的圓，對其半徑進(jìn)行誤差測量。

5 結(jié) 語

本文針對目前貨物搬運(yùn)方式的不足，提出一種新的運(yùn)動控制方式，不僅增加了運(yùn)動的精確性，也提高了靈活性。在搭建了實(shí)物模型后，進(jìn)行了多次試驗(yàn)，均符合要求，驗(yàn)證了基于DSPIC30F4011的三線式三維懸掛運(yùn)動控制系統(tǒng)是可行的。

參考文獻(xiàn)

[1] 武林.三維懸掛物體運(yùn)動控制系統(tǒng)[J].浙江師范大學(xué)學(xué)報(bào)，2007，30（2）：171?175.

[2] 喬東凱，楊向宇，李多民，等.基于PLC對三維立體運(yùn)動系統(tǒng)的電氣控制及應(yīng)用[J].工程設(shè)計(jì)學(xué)報(bào)，2012，19（6）：465?468.

[3] 焦振廣.箱式貨物自動裝車系統(tǒng)的設(shè)計(jì)[D].青島：山東科技大學(xué)，2014.

[4] 宋志蘭，王華.地鏈搬運(yùn)系統(tǒng)及其應(yīng)用[J].物流技術(shù)與應(yīng)用，2014（5）：118?121.

[5] 郝巖柱.自動化立體倉庫堆垛機(jī)控制系統(tǒng)的設(shè)計(jì)與路徑優(yōu)化[D].沈陽：東北大學(xué)，2009.

[6] 王海峰.基于PIC單片機(jī)的溫濕度監(jiān)控系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2011，34（23）：201?203.

[7] 陳建生.夾持式架空地線巡線機(jī)器人的設(shè)計(jì)[D].蘇州：蘇州大學(xué)，2015.

[8] 楊海光，劉文怡，朱思敏，等.步進(jìn)電機(jī)對測量角度的定位與控制[J].自動化儀表，2013，34（2）：46?48.

[9] 丁俐，余瑾，吳垠，等.基于PIC單片機(jī)的仿生機(jī)器魚的舵機(jī)控制[J].微計(jì)算機(jī)信息，2010，26（14）：88?89.

[10] 崇陽，張科，王靖宇.一種基于模糊ADRC的舵機(jī)控制算法設(shè)計(jì)與實(shí)現(xiàn)[J].西北工業(yè)大學(xué)學(xué)報(bào)，2011，29（2）：217?222.