三線式三維運動協(xié)調(diào)控制系統(tǒng)的設(shè)計與研究

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

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

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

中圖分類號： TN02?34； TP23 文獻標識碼： 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 引 言

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

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

1 建立數(shù)學模型

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

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

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

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

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

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

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

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

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

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

2.1 控制機構(gòu)

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

2.2 交互機構(gòu)

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

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

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

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

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

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

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

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

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

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

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

3.1 空間圓軌跡方程的建立

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

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

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

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

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

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

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

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

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

（7） 取下一個離散點，重復步驟（4），步驟（5），直到運動到目標點為止，等待下一個指令。

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

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

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

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

5 結(jié) 語

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

參考文獻

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

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

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

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

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

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

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

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

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

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