儲(chǔ)昭碧，王 昊，胡永新

（電氣與自動(dòng)化工程學(xué)院 合肥工業(yè)大學(xué)，合肥 230009）

洗車(chē)機(jī)二連桿機(jī)械臂定位控制策略及實(shí)現(xiàn)

儲(chǔ)昭碧，王 昊，胡永新

（電氣與自動(dòng)化工程學(xué)院 合肥工業(yè)大學(xué)，合肥 230009）

介紹了一種新式的二連桿機(jī)械臂洗車(chē)機(jī)，相比傳統(tǒng)洗車(chē)機(jī)，洗車(chē)效率高，占地面積小，存在著很好的市場(chǎng)前景。本系統(tǒng)基于臺(tái)達(dá)DVP10MC11T總線型運(yùn)動(dòng)控制器，配合伺服電機(jī)，超聲波傳感器，控制精準(zhǔn)，設(shè)計(jì)合理，通過(guò)MATLAB仿真，經(jīng)過(guò)多次現(xiàn)場(chǎng)試驗(yàn)，達(dá)到了預(yù)期目標(biāo)。同時(shí)，此二連桿機(jī)械臂控制算法對(duì)類似的控制系統(tǒng)具有一定的參考價(jià)值。

機(jī)械臂；運(yùn)動(dòng)控制系統(tǒng)；超聲波傳感器；伺服系統(tǒng)

0 引言

隨著社會(huì)的進(jìn)步，汽車(chē)在人們的日常生活中扮演了越來(lái)越重要的角色，洗車(chē)行業(yè)也是越來(lái)越發(fā)達(dá)，傳統(tǒng)的人工洗車(chē)普遍存在著洗車(chē)時(shí)間長(zhǎng)，洗車(chē)效率低等弊端，隨著科技的進(jìn)步，各式各樣的洗車(chē)機(jī)也如雨后春筍一樣層出不窮。現(xiàn)階段市面上的洗車(chē)機(jī)基本上分為兩類，一類是無(wú)接觸式自動(dòng)洗車(chē)機(jī)，一類是毛刷式自動(dòng)洗車(chē)機(jī)，前者屬于半自動(dòng)產(chǎn)品，市面上的基本上只能洗車(chē)身兩側(cè)，對(duì)于車(chē)頭車(chē)尾和車(chē)頂基本上無(wú)法洗到，仍需人工清洗；后者往往體積較大，需要的場(chǎng)地比較大，價(jià)格高，對(duì)于小型的洗車(chē)點(diǎn)來(lái)說(shuō)不是很方便[1,2]。本洗車(chē)機(jī)是基于PLC的二連桿機(jī)械臂機(jī)構(gòu)的洗車(chē)機(jī)，精巧的結(jié)構(gòu)設(shè)計(jì)使得其要求的場(chǎng)地面積較小，對(duì)于車(chē)頭車(chē)尾和車(chē)頂也能很好的洗到，效率高，存在著很好的市場(chǎng)前景。

1 系統(tǒng)結(jié)構(gòu)及運(yùn)行原理

1.1 系統(tǒng)組成

本系統(tǒng)是一套基于PLC的二連桿機(jī)械臂系統(tǒng)[3]，該洗車(chē)機(jī)由兩個(gè)等長(zhǎng)（1300mm）的主臂（大臂和小臂）一個(gè)L臂組成，其中L臂集成了風(fēng)管、水管和清洗液管，負(fù)責(zé)具體的清洗工作。大臂小臂分別各自由一臺(tái)伺服電機(jī)驅(qū)動(dòng)，負(fù)責(zé)運(yùn)動(dòng)線路的控制，L臂可以自由升降，其升降桿由另一臺(tái)伺服電機(jī)控制。同時(shí)，本系統(tǒng)配備了三個(gè)超聲波傳感器以實(shí)現(xiàn)對(duì)車(chē)身檢測(cè)及機(jī)械臂運(yùn)動(dòng)路線的規(guī)劃，三個(gè)超聲波傳感器一個(gè)用來(lái)車(chē)身側(cè)面測(cè)距，一個(gè)用來(lái)車(chē)頂測(cè)距，一個(gè)用來(lái)對(duì)車(chē)身四角進(jìn)行檢測(cè)，以實(shí)現(xiàn)轉(zhuǎn)角信號(hào)的發(fā)送。車(chē)身側(cè)面測(cè)距傳感器和車(chē)頂測(cè)距傳感器裝在L臂上，轉(zhuǎn)角信號(hào)超聲波傳感器裝在小臂末端。系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

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

實(shí)際工作中，通過(guò)超聲波傳感器[4～7]測(cè)出機(jī)械臂末端與車(chē)身側(cè)面的距離d，通過(guò)與給定r進(jìn)行比較得出差值e，同時(shí)與伺服電機(jī)中讀出的角度一起是輸入傳給控制器，控制器進(jìn)行計(jì)算得出兩個(gè)電機(jī)的轉(zhuǎn)速驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)產(chǎn)生x和y方向的位置，使得e趨近于0，同時(shí)使y方向的速度達(dá)到給定速度。

1.2 運(yùn)行過(guò)程

當(dāng)洗車(chē)輛駛?cè)胂窜?chē)機(jī)內(nèi)部，安全確認(rèn)完畢之后，洗車(chē)機(jī)開(kāi)始運(yùn)行，伺服電機(jī)驅(qū)動(dòng)大小臂從而使L臂向車(chē)身側(cè)面靠近，同時(shí)，L臂升降桿向車(chē)身頂部靠近，當(dāng)檢測(cè)車(chē)身側(cè)面距離超聲波傳感器檢測(cè)到L臂距車(chē)身200mm且頂部超聲波傳感器檢測(cè)到升降桿距離車(chē)頂200mm時(shí)，升降桿停止向車(chē)頂靠近，L臂停止向車(chē)身靠近，根據(jù)超聲波傳感器反饋的距離進(jìn)行PID控制，驅(qū)動(dòng)大小臂轉(zhuǎn)動(dòng)使L臂開(kāi)始以與車(chē)身始終保持200mm的距離圍繞著車(chē)身運(yùn)動(dòng)，風(fēng)管水管清洗液管配合工作，第一圈噴灑高效清洗液，第二圈空轉(zhuǎn)一圈讓清洗液充分反應(yīng)，第三圈進(jìn)行強(qiáng)力水洗，第四圈進(jìn)行強(qiáng)力風(fēng)干。車(chē)輛清潔完畢之后，機(jī)械臂回到初始位置，洗車(chē)結(jié)束。

2 系統(tǒng)模型搭建及仿真

2.1 系統(tǒng)模型搭建

圖2為二連桿機(jī)械臂系統(tǒng)[8～11]的簡(jiǎn)化模型（點(diǎn)劃線代表車(chē)身輪廓）。定義連接原點(diǎn)的臂為大臂，末端為小臂，此裝置有兩個(gè)關(guān)節(jié)和兩個(gè)自由度，有兩個(gè)驅(qū)動(dòng)裝置，其中兩個(gè)臂的長(zhǎng)度為L(zhǎng)，機(jī)械臂末端到原點(diǎn)的距離為D，角θ為D邊與x軸正向上側(cè)的夾角，角α為大臂與x軸正向上側(cè)的夾角，角β為大臂與小臂的夾角（小于180°），L為大小臂的長(zhǎng)度。（x,y）為小臂末端的坐標(biāo)，e為超聲波傳感器測(cè)出的距離d減去給定距離r的差值，ωα和ωβ分別為大小臂旋轉(zhuǎn)角速度。默認(rèn)在x軸負(fù)半平面分析，vx為L(zhǎng)臂與車(chē)身垂直方向的速度，vy為L(zhǎng)臂與車(chē)身平行方向的速度。在其他區(qū)域內(nèi)只需更改方向即可，下文會(huì)詳述。

圖2 系統(tǒng)簡(jiǎn)化模型

2.2 公式模型推導(dǎo)

要實(shí)現(xiàn)通過(guò)控制兩個(gè)大臂電機(jī)的轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)對(duì)其的控制，則需要將平面坐標(biāo)的運(yùn)動(dòng)轉(zhuǎn)換為大小臂電機(jī)旋轉(zhuǎn)的角速度。

根據(jù)圖1，由幾何知識(shí)，可以得出：

（x,y）為機(jī)械臂末端的坐標(biāo)，根據(jù)公式(1)，很容易得出：

機(jī)械臂末端的運(yùn)動(dòng)可以分解為x軸方向和y軸方向的運(yùn)動(dòng)，vx和vy可以由對(duì)x和y求導(dǎo)得出：

x方向的位移應(yīng)該為實(shí)際機(jī)械臂末端到車(chē)身距離與給定距離的差值e，y軸方向的速度是認(rèn)為設(shè)定的，即機(jī)械臂末端的運(yùn)動(dòng)x軸方向?yàn)闇p小e的運(yùn)動(dòng)，y方向?yàn)閯蛩龠\(yùn)動(dòng)，由此可得：

根據(jù)公式(4)，可以解出大小臂旋轉(zhuǎn)的角速度。

從而可以計(jì)算出大小臂電機(jī)的轉(zhuǎn)速，實(shí)現(xiàn)控制目的。

公式(5)計(jì)算出的ωα和ωβ分別為大小臂旋轉(zhuǎn)角速度，將其除以2π即為兩電機(jī)的轉(zhuǎn)速，正負(fù)號(hào)代表正反轉(zhuǎn)方向。α和β可以由伺服電機(jī)的編碼器讀出。對(duì)于x軸正半平面，vy為機(jī)械臂沿車(chē)身運(yùn)動(dòng)的速度（正值），vx為機(jī)械臂保持與車(chē)身200mm所需的速度（正值）；對(duì)于x軸負(fù)半平面，vy為機(jī)械臂沿車(chē)身運(yùn)動(dòng)的速度（負(fù)值），vx為機(jī)械臂保持與車(chē)身200mm所需的速度（負(fù)值）；對(duì)于y軸正半平面，vx為機(jī)械臂沿車(chē)身運(yùn)動(dòng)的速度（負(fù)值），vy為機(jī)械臂保持與車(chē)身200mm所需的速度（正值）；對(duì)于y軸負(fù)半平面，vx為機(jī)械臂沿車(chē)身運(yùn)動(dòng)的速度（正值），vy為機(jī)械臂保持與車(chē)身200mm所需的速度（負(fù)值）；使用時(shí)只需按機(jī)械臂的位置選擇不同參數(shù)即可。

假設(shè)初始的夾角為α0和β0，機(jī)械臂末端到原點(diǎn)的距離為D0，位置坐標(biāo)為（x0,y0）, 定義D邊與x軸正向夾角為θ0。當(dāng)前時(shí)刻的夾角為αt和βt，機(jī)械臂末端到原點(diǎn)的距離為Dt，位置坐標(biāo)為（xt,yt），大小臂旋轉(zhuǎn)的角速度為ωα和ωβ，定義D邊與x軸正向夾角為θt。

顯然，在時(shí)域中：

將公式(6)代入公式(2)中，可得出機(jī)械臂末端在坐標(biāo)系中的坐標(biāo)函數(shù)從而可以進(jìn)行仿真，代入后有：

3.3 Simulink仿真

仿真[12]采用一條給定的線段對(duì)跟蹤算法進(jìn)行追蹤性能分析，給定的線段函數(shù)如下：

為了使結(jié)果更加準(zhǔn)確，所有參數(shù)按實(shí)際的大小設(shè)置，L臂的長(zhǎng)度為1300mm，給定一個(gè)機(jī)械臂的初始位置，α=5π/3，β=π/3，對(duì)應(yīng)到坐標(biāo)系中的坐標(biāo)為（1300，0）。

使用Simulink仿真后結(jié)果如下：

圖3 仿真跟蹤曲線

圖4 機(jī)械臂與車(chē)身實(shí)時(shí)距離

起始位置為（1300，0），此時(shí)車(chē)身側(cè)面位置為（1000，0），根據(jù)運(yùn)行原理，機(jī)械臂應(yīng)跟車(chē)身側(cè)面保持200mm的距離，機(jī)械臂在x軸方向產(chǎn)生位移靠近車(chē)身使距離為200mm，同時(shí)y軸方向保持200mm/s的速度。

結(jié)合圖3和圖4可以看出，機(jī)械臂按照既定路線運(yùn)行，且運(yùn)行軌跡跟隨性較好，在轉(zhuǎn)角處的超調(diào)量較小，能滿足實(shí)際需要，證明該算法的可行性。

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

4.1 硬件選型

本系統(tǒng)檢測(cè)方面采用德國(guó)倍加福UC-2000-30GMIUR2-V15型超聲波傳感器，控制器方面采用的是臺(tái)達(dá)DVP10MC11T總線型運(yùn)動(dòng)控制器，支持模塊插接，該控制器將PLC可編程邏輯控制器和MC運(yùn)動(dòng)控制器整合到一起，通過(guò)高速總線（CANopen）可以控制高達(dá)16軸，可以實(shí)現(xiàn)高速精準(zhǔn)的運(yùn)動(dòng)控制，伺服系統(tǒng)則選擇了臺(tái)達(dá)ASDA-A2-M型通訊型交流伺服驅(qū)動(dòng)器搭配了20-bit(1280000 p/rev)增量型編碼器電機(jī)則選擇了同為臺(tái)達(dá)公司生產(chǎn)的ECMA-C11010RS型交流伺服電機(jī)。

4.2 硬件連線

圖5為系統(tǒng)硬件結(jié)構(gòu)圖，HMI與PLC通過(guò)485通訊進(jìn)行連接，伺服驅(qū)動(dòng)器ASDA-A2、DVP10MC11T和伺服電機(jī)通過(guò)網(wǎng)線連接，超聲波傳感器裝在機(jī)械臂L臂上。

圖5 系統(tǒng)硬件結(jié)構(gòu)圖

5 軟件實(shí)現(xiàn)

本系統(tǒng)軟件設(shè)計(jì)采用梯形圖編程[13]，主要實(shí)現(xiàn)對(duì)誤差信號(hào)的PID調(diào)節(jié)，電機(jī)轉(zhuǎn)速和方向的計(jì)算和發(fā)送，對(duì)各個(gè)階段的運(yùn)行狀態(tài)進(jìn)行判斷從而選擇對(duì)應(yīng)的階段參數(shù)進(jìn)行轉(zhuǎn)速及方向的計(jì)算。使用梯形圖編程，邏輯清晰，易讀。程序流程圖如圖6所示，圖中虛線表示PLC循環(huán)掃描機(jī)制，圖7為運(yùn)行流程圖。

6 實(shí)驗(yàn)結(jié)果

現(xiàn)在市面上傳統(tǒng)的自動(dòng)洗車(chē)機(jī)，洗一輛車(chē)的平均耗時(shí)為10分鐘左右[14]，而本款洗車(chē)機(jī)設(shè)計(jì)的沿車(chē)身運(yùn)動(dòng)的速度為200mm/s，現(xiàn)場(chǎng)測(cè)試時(shí)，使用的車(chē)型車(chē)身尺寸為5018×1866×1459，從系統(tǒng)啟動(dòng)到結(jié)束共耗時(shí)400s，低于傳統(tǒng)洗車(chē)機(jī)耗時(shí)，效率更高。

圖6 程序流程圖

圖7 運(yùn)行流程圖

7 結(jié)束語(yǔ)

本文給出了一套基于二連桿機(jī)械臂的洗車(chē)機(jī)運(yùn)動(dòng)控制系統(tǒng)的硬件組成和軟件設(shè)計(jì)，經(jīng)過(guò)現(xiàn)場(chǎng)實(shí)驗(yàn)測(cè)試，其跟隨性好，效率高，根據(jù)行業(yè)市場(chǎng)統(tǒng)計(jì)，目前市面上傳統(tǒng)的自動(dòng)洗車(chē)機(jī)占用空間至少為7×3×4（m），而本洗車(chē)機(jī)所需空間僅為5.8×2.9×3.1（m），相比傳統(tǒng)自動(dòng)洗車(chē)機(jī)占用空間更小，具有一定的市場(chǎng)和推廣價(jià)值，同時(shí)，也對(duì)類似的控制系統(tǒng)有一定的參考意義。

[1] 匡偉民.基于SIEMENS S7-200的接觸式通道洗車(chē)機(jī)控制系統(tǒng)設(shè)計(jì)[J].輕工科技,2013,(07):94-95+128.

[2] 儲(chǔ)昭碧,李健,孔艷,馮小英.基于S7-1200的全自動(dòng)洗車(chē)控制系統(tǒng)的設(shè)計(jì)[J].電氣傳動(dòng),2016,(09):62-65.

[3] 王耀南,高小龍.基于CANopen的機(jī)械臂控制系統(tǒng)研究[J].控制工程,2016,23(1):1-5.

[4] Sahdev Kumar,Hideo Furuhashi. Long-Range Measurement System Using Ultrasonic Range Sensor with High-Power Transmitter Array in Air[J].Ultrasonics,2016.

[5] 蔡磊,周亭亭,郭云鵬,陳素芳,吳漢幫.基于超聲波定位的智能跟隨小車(chē)[J].電子測(cè)量技術(shù),2013,(11):76-79+105.

[6] 金英連,王斌銳,方水光.未知環(huán)境下柔性二連桿機(jī)械臂的自適應(yīng)阻抗控制[J].中國(guó)機(jī)械工程,2014,(17):2351-2355.

[7] L. Koval,J. Vaňu?,P. Bilík.Distance Measuring by Ultrasonic Sensor[J]. IFAC PapersOnLine,2016,49(25).

[8] 李明,程啟明,陳根,王鶴霖,鄧亮.永磁同步伺服電機(jī)二自由度控制[J].電機(jī)與控制應(yīng)用,2014,(10):1-5+25.

[9] Hongyan Wang,Lihua Tang. Modeling and Experiment of Bistable Two-Degree-of-Freedom Energy Harvester with Magnetic Coupling[J].Mechanical Systems and Signal Processing,2016.

[10] Shaogang Liu,Qianju Cheng,Dan Zhao,Lifeng Feng. Theoretical modeling and analysis of two-degree-of-freedom piezoelectric energy harvester with stopper[J].Sensors & Actuators: A.Physical,2016.

[11] Dong Wang,Tao Liu,Ximing Sun,Chongquan Zhong. Discretetime domain two-degree-of-freedom control design for integrating and unstable processes with time delay[J].ISA Transactions,2016.

[12] 姚振明,胡霞.基于MATLAB的永磁體同步電機(jī)模糊PID控制仿真[J].安徽理工大學(xué)學(xué)報(bào)(自然科學(xué)版),2016,(06):18-21+46.

[13] 臺(tái)達(dá)電子工業(yè)股份有限公司.臺(tái)達(dá)DVP-PLC編程技巧:WPLSoft軟件篇[M].中國(guó)電力出版社,2012.

[14] 李培麗.節(jié)能便攜式洗車(chē)機(jī)的研制[D].內(nèi)蒙古工業(yè)大學(xué),2010.

Positioning control strategy and realization of two - link manipulator for washing machine

CHU Zhao-bi, WANG Hao, HU Yong-xin

TP273

：A

：1009-0134(2017)08-0153-04

2017-06-06

儲(chǔ)昭碧（1970 -），男，安徽人，教授，工學(xué)博士，研究方向?yàn)楝F(xiàn)場(chǎng)總線網(wǎng)絡(luò)、自動(dòng)化系統(tǒng)集成等。