劉以行 葉仕通 何偉浩 鄧嘉豪

(廣東工業(yè)大學(xué)華立學(xué)院, 廣東511325)

可控高空作業(yè)機(jī)械臂控制系統(tǒng)設(shè)計(jì)

劉以行 葉仕通 何偉浩 鄧嘉豪

(廣東工業(yè)大學(xué)華立學(xué)院, 廣東511325)

目前城市化園林在自動(dòng)化建設(shè)和高空作業(yè)方面設(shè)計(jì)出可高空作業(yè)機(jī)械臂控制系統(tǒng)。機(jī)械臂控制系統(tǒng)采用STM32F103RCT6芯片為控制核心，運(yùn)用電機(jī)PID控制器、姿態(tài)平衡傳感器和三杯式風(fēng)速傳感器三個(gè)核心部件組成的傳感器控制系統(tǒng)和機(jī)械驅(qū)動(dòng)控制，實(shí)現(xiàn)機(jī)械臂作業(yè)反饋和智能避險(xiǎn)功能。實(shí)驗(yàn)證明，控制系統(tǒng)與臂體融合程度高，具有良好的控制性能。

高空作業(yè)機(jī)械臂；傳感器控制；電機(jī)控制

在城市園林管理及綠化建設(shè)的過(guò)程中，傳統(tǒng)的人工直接手持鋸木設(shè)備對(duì)城市立著的樹(shù)木進(jìn)行修剪是一件危險(xiǎn)且又辛苦的工作，而高空作業(yè)機(jī)械臂的出現(xiàn)彌補(bǔ)了園林建設(shè)、城市綠化這一缺陷，減少修剪人工的使用，降低該建設(shè)過(guò)程的成本；同時(shí)提高了修剪效率，其自動(dòng)化的控制完全不需要考慮到人工修剪時(shí)會(huì)出現(xiàn)的疲勞現(xiàn)象；其控制系統(tǒng)自帶安全調(diào)控應(yīng)急程序，有效地降低了傷害幾率。尤其是目前城市道路兩旁的大樹(shù)數(shù)量大，且絕大部分栽種的樹(shù)木是生長(zhǎng)速度較快，枝干容易長(zhǎng)長(zhǎng)的品種，因此常需要修剪。所以高空作業(yè)機(jī)械手臂的市場(chǎng)發(fā)展前景還是相當(dāng)可觀的。高空作業(yè)機(jī)械臂的控制系統(tǒng)的優(yōu)秀與否是衡量是否具有實(shí)用價(jià)值意義的關(guān)鍵因素，其中可靠的安全反饋機(jī)制是非常必要的，機(jī)械臂控制系統(tǒng)采用STM32F103RCT6芯片為主控核心，結(jié)合機(jī)械結(jié)構(gòu)特點(diǎn)，配合由風(fēng)速傳感器和MPU9250傳感器模塊構(gòu)成的傳感器反饋系統(tǒng)，使用藍(lán)牙模塊、wifi模塊和串行通訊方式作為上位機(jī)與下位機(jī)之間的通信橋梁，最后各部件聯(lián)動(dòng)控制，實(shí)現(xiàn)整個(gè)機(jī)械臂的控制。系統(tǒng)結(jié)構(gòu)圖如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)圖

1 系統(tǒng)硬件組成

機(jī)械臂控制臺(tái)搭建以32位ARM3微處理芯片STM32F013RCT6為微控制核心，由12 V的直流電供電，由AMS1117-3.3芯片和線性穩(wěn)壓芯片LM2940共同集成供電模塊和由兩塊BTN7970構(gòu)成的全橋電路和一塊74lS244芯片構(gòu)成的保護(hù)電路組成的電機(jī)驅(qū)動(dòng)模塊。具有電源轉(zhuǎn)換效率高，噪音低，抗干擾能力強(qiáng)等優(yōu)點(diǎn)。機(jī)械結(jié)構(gòu)主要采用質(zhì)量相對(duì)較輕的鋁合金材料制造，適用于不同場(chǎng)合使用時(shí)的安裝與拆卸。機(jī)械臂底座根據(jù)情況采用皮帶固定或螺栓固定，使用步進(jìn)電機(jī)為機(jī)械臂提供旋轉(zhuǎn)動(dòng)力。在手臂與底座以及大臂與小臂的連接處則利用減速電機(jī)提供手臂的擺動(dòng)動(dòng)力，大臂與小臂都采用方形結(jié)構(gòu)，使手臂具有良好的抗扭性能及較大的抗彎剛度。在小臂中提供一組動(dòng)力，控制手掌的旋轉(zhuǎn)。機(jī)械臂抓取部分采用兩指結(jié)構(gòu)，使用MG996R舵機(jī)提供動(dòng)力，利用滑塊機(jī)構(gòu)連接并控制兩指的運(yùn)動(dòng)。

2 控制系統(tǒng)

2.1 電機(jī)驅(qū)動(dòng)控制

電機(jī)組采用BTN7971驅(qū)動(dòng)芯片的直流電機(jī)驅(qū)動(dòng)板，雙BTN7971驅(qū)動(dòng)芯片及附屬電路形成一個(gè) “H橋驅(qū)動(dòng)電路”，便于開(kāi)發(fā)人員控制驅(qū)動(dòng)電機(jī)需要使能信號(hào)線ENABLE，兩個(gè)方向信號(hào)線DIR-L和DIR-R。H橋驅(qū)動(dòng)電路圖見(jiàn)圖2。

圖2 H橋驅(qū)動(dòng)電路圖

通過(guò)信號(hào)線控制電機(jī)的轉(zhuǎn)速和角位移量，其中H橋電路必須異側(cè)導(dǎo)通，當(dāng)DIR-L信號(hào)為0，DIR-R信號(hào)為1，當(dāng)ENABLE為1時(shí)，電流從左往右流通，反之電流反向流通，通過(guò)控制電流在電機(jī)中的流向?qū)崿F(xiàn)電機(jī)組的正反轉(zhuǎn)控制，伺服電機(jī)部分增加反饋信號(hào)線實(shí)時(shí)傳輸轉(zhuǎn)速和位置數(shù)據(jù)，從而可以根據(jù)實(shí)際情況配合相關(guān)傳感器對(duì)電機(jī)組進(jìn)行更精確地控制。信號(hào)線端使用脈沖寬度調(diào)制(PWM)輸入，通過(guò)設(shè)置STM32芯片的定時(shí)器，設(shè)置TIMx_CCER、TIMx_CCRx等寄存器的相關(guān)標(biāo)志位，就能產(chǎn)生PWM輸出，并進(jìn)行脈寬在頻率和占空比上的控制。脈寬和周期測(cè)量時(shí)序圖見(jiàn)圖3。

圖3 脈寬和周期測(cè)量時(shí)序圖

2.2 電機(jī)PID控制器

使用PID控制器是較為有效和穩(wěn)定的控制方式，參數(shù)易調(diào)，控制簡(jiǎn)單，廣泛應(yīng)用于機(jī)器人、平衡小車等自動(dòng)化控制領(lǐng)域。

(1)

(2)

由式(1)和式(2)可得增量式PID控制算式：

(3)

式中，ΔUk為第k次采樣時(shí)刻的計(jì)算機(jī)輸出值；Kp為比例系數(shù)；KI為積分系數(shù)；KD為微分系數(shù)；ek為第k次采樣時(shí)刻輸入的偏差值；ek-1為第k-1次采樣時(shí)刻輸入的偏差值；u0為開(kāi)始進(jìn)行PID控制時(shí)原始初值。基于式(1)和式(2)得到的增量式PID控制算式具有計(jì)算量少的優(yōu)點(diǎn)，所以在本控制系統(tǒng)設(shè)計(jì)中，使用增量式PID對(duì)伺服電機(jī)的速度、位置進(jìn)行控制。

2.3 姿態(tài)平衡控制

機(jī)械臂必須具有維持自身平衡的能力，良好的受力點(diǎn)能最大限度地發(fā)揮出其動(dòng)力模塊效能。MPU9250模塊由兩部分組成，一組由3軸加速度計(jì)及3軸陀螺儀組成，另一組由AKM公司的AK8963 3軸磁力計(jì)組成，并采用I2C協(xié)議進(jìn)行通信，最高可達(dá)3.4 Mbps的速度，能很好地幫助機(jī)械臂處于自身平衡狀態(tài)，模塊集成了陀螺儀和加速度計(jì)，以機(jī)械臂重心為中心建立空間直角坐標(biāo)系X,Y,Z軸，反饋當(dāng)前機(jī)械臂姿態(tài)數(shù)據(jù)，模塊的姿態(tài)運(yùn)動(dòng)學(xué)方程作為濾波的狀態(tài)轉(zhuǎn)移方程，加速度信息作為濾波的觀察量信息，結(jié)合PID算法和卡爾曼濾波的計(jì)算方法，迭代計(jì)算更新參數(shù)，調(diào)整PWM脈寬輸出，達(dá)到調(diào)整電機(jī)和舵機(jī)角度的目的，使臂體在未受自然因素影響時(shí)保持平衡。PID控制系統(tǒng)圖如圖4所示。

圖4 卡爾曼濾波器的PID控制系統(tǒng)圖

2.4 上位機(jī)監(jiān)控

本系統(tǒng)采用基于C++的QT框架，使用QT4編寫的監(jiān)控上位機(jī)，可跨平臺(tái)使用。臂身兩側(cè)裝有監(jiān)控?cái)z像頭，用于圖像采集，同時(shí)打開(kāi)兩側(cè)攝像頭，使用WiFi模塊通過(guò)802.11n協(xié)議實(shí)時(shí)傳送畫面至上位機(jī)，根據(jù)兩側(cè)畫面合成單幅圖像，實(shí)時(shí)觀察操作對(duì)象與機(jī)械臂的實(shí)際情況。同時(shí)，風(fēng)速傳感器、PID控制器數(shù)據(jù)等實(shí)時(shí)數(shù)據(jù)會(huì)通過(guò)特定的通訊方式，經(jīng)過(guò)下位機(jī)處理，輸送到上位機(jī)反饋顯示。監(jiān)控上位機(jī)反饋顯示如圖5所示。

2.5 風(fēng)速傳感器

圖5 監(jiān)控上位機(jī)反饋顯示

圖6 風(fēng)速傳感器結(jié)構(gòu)尺寸圖

圖7 測(cè)量反饋程序流程圖

機(jī)械臂工作于高空中，對(duì)臂體安全性影響最大的自然因數(shù)是風(fēng)速，風(fēng)速隨高度的逐漸變化呈指數(shù)上升，危險(xiǎn)系數(shù)逐漸加快，呈上升趨勢(shì)，所以機(jī)械臂需要一個(gè)預(yù)報(bào)警機(jī)制，該系統(tǒng)采用YGC-FS型三杯式風(fēng)速傳感器作為預(yù)報(bào)警機(jī)制的核心部件，其結(jié)構(gòu)尺寸如圖6所示。

測(cè)量范圍為(0～70)m/s，分辨率為0.1 m/s，準(zhǔn)確度為±(0.3+0.03V)m/s，啟動(dòng)風(fēng)速小于0.5 m/s，輸出頻率信號(hào)F代表風(fēng)速V，計(jì)算公式為：

(1)

(2)

三杯式風(fēng)速傳感器由3 個(gè)處于同一平面、互成120°的半球形空杯組成。當(dāng)風(fēng)杯受到水平風(fēng)向的驅(qū)動(dòng)時(shí)，產(chǎn)生霍爾效應(yīng)，經(jīng)過(guò)傳感器內(nèi)部處理收集到的數(shù)據(jù)，通過(guò)STM32的DMA 通道實(shí)現(xiàn)內(nèi)存風(fēng)速數(shù)據(jù)與 USART 外設(shè)的數(shù)據(jù)傳輸，并使用RS485串口標(biāo)配通信協(xié)議傳送實(shí)時(shí)數(shù)據(jù)至上位機(jī)，程序中設(shè)置預(yù)警標(biāo)志位，當(dāng)風(fēng)速超過(guò)或達(dá)到預(yù)警線(預(yù)警標(biāo)志位置1)時(shí)，機(jī)械臂發(fā)出警報(bào)并停止工作，馬上啟動(dòng)緊急回縮機(jī)制，收回機(jī)械臂，最終由操控人員決定是否完全關(guān)閉機(jī)械臂，實(shí)現(xiàn)過(guò)高風(fēng)速下的安全保護(hù)功能。 測(cè)量反饋程序流程圖如圖7所示。

3 仿真實(shí)驗(yàn)

使用仿真軟件對(duì)控制系統(tǒng)進(jìn)行進(jìn)一步的驗(yàn)證和調(diào)試。如圖8所示。在沒(méi)有接收到預(yù)警信號(hào)前，脈沖處于平穩(wěn)狀態(tài)，可知電機(jī)正常工作，機(jī)械臂處于穩(wěn)定上升狀態(tài)。由圖9可知，模塊電壓與風(fēng)速呈負(fù)相關(guān)關(guān)系，隨著風(fēng)速的不斷提高，當(dāng)風(fēng)速超過(guò)或達(dá)到預(yù)警線時(shí)，風(fēng)速傳感器發(fā)出信號(hào)，檢測(cè)模塊接受到一個(gè)高電平脈沖，電機(jī)正轉(zhuǎn)脈沖與反轉(zhuǎn)脈沖狀態(tài)置換，電機(jī)開(kāi)始減速反轉(zhuǎn)，一定時(shí)間后穩(wěn)定工作，機(jī)械臂將逐漸下降，可知預(yù)警反饋模塊運(yùn)作正常，控制系統(tǒng)基本達(dá)到預(yù)期結(jié)果。

a—電機(jī)正轉(zhuǎn)脈沖 b—傳感器電平脈沖 c—電機(jī)反轉(zhuǎn)脈沖

4 結(jié)論

本文提出了一個(gè)以高空作業(yè)機(jī)械臂為主體的控制系統(tǒng)設(shè)計(jì)，經(jīng)初步實(shí)踐，機(jī)械臂與控制系統(tǒng)配合程度基本達(dá)到設(shè)計(jì)要求。與同類機(jī)械臂相比，該控制系統(tǒng)由傳感器模塊搭建組成較為完善的反饋機(jī)制，傳感器模塊和驅(qū)動(dòng)控制模塊良好融合，使機(jī)械臂能在高空環(huán)境中穩(wěn)定工作?，F(xiàn)階段已實(shí)現(xiàn)了機(jī)械臂與控制系統(tǒng)的初步融合，還需繼續(xù)深入和完善本機(jī)械臂控制系統(tǒng)設(shè)計(jì)，在機(jī)械結(jié)構(gòu)上仍需合理地改進(jìn)，增添方便拆裝的獨(dú)立電源供電模塊及無(wú)線傳輸控制模塊等。

圖9 風(fēng)速與模塊電壓關(guān)系圖

[1] 馬文斌,楊延竹,洪運(yùn). 步進(jìn)電機(jī)控制系統(tǒng)的設(shè)計(jì)及應(yīng)用[J].電子技術(shù)應(yīng)用, 2015,41(11):11-13．

[2] 李長(zhǎng)才,肖金球,張少華．基于 STM32 的風(fēng)速監(jiān)測(cè)系統(tǒng)設(shè)計(jì)[J].微型機(jī)與應(yīng)用,2016,35(6) : 91-93.97．

[3] 孟祥萍,張本法,苑全德. 自適應(yīng)擴(kuò)展卡爾曼濾波器在移動(dòng)機(jī)器人定位中的應(yīng)用[J].計(jì) 算機(jī)系統(tǒng)應(yīng)用,2015,24(12)．

[4] 韓東,韓冰,鄭衛(wèi)剛. 軌道式焊接機(jī)器人工作站在壓力容器焊接中的設(shè)計(jì)[J]．中國(guó)重型裝備:2015,7(3):28-30．

[5] 楊杰,王瑞．基于加速度輸入的無(wú)人機(jī)航跡 PID 控制[J]. 自動(dòng)化技術(shù)與應(yīng)用: 2015,34(12)．

[6] 彭圣明,裴海龍,王清陽(yáng).基于機(jī)械臂的運(yùn)動(dòng)學(xué)研究與應(yīng)用[J].機(jī)械設(shè)計(jì)與制造，2011(7) : 136-138．

[7] 李安迎，鄧靖靖，鄧世建，等. 基于三杯式風(fēng)速傳感器的風(fēng)速監(jiān)測(cè)站設(shè)計(jì)[J]. 電測(cè)與儀表,2010,47(7A)．

編輯 陳秀娟

Design on Control System of Controllable Manipulator in Aloft Work

Liu Yixing, Ye Shitong, He Weihao, Deng Jiahao

The control system of manipulator in aloft work has been designed for automation construction and aloft work of urbanization garden. The control system of manipulator has been adopted STM32F103RCT6 chip as the control core, and has been used the sensor control system consisted of three core components of motor PID controller, posture balance sensor and three cups of wind speed sensor and has been controlled by mechanical drive, which can realize the manipulator operation feedback and intelligent hedging function. The experimental results show that the control system has a high degree of integration with the arm body and it has good control performance.

manipulator in aloft work; sensor control; motor control

2016—10—11

“攀登計(jì)劃”廣東大學(xué)生科技創(chuàng)新培育專項(xiàng)資金資助一般項(xiàng)目，編號(hào)PDJH2016b0932；大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目，編號(hào)201613656001

劉以行 (1996—)，男，本科，主要從事自動(dòng)化控制技術(shù)。

TP242.6

A