洗車機二連桿機械臂定位控制策略及實現

儲昭碧，王 昊，胡永新

（電氣與自動化工程學院 合肥工業大學，合肥 230009）

洗車機二連桿機械臂定位控制策略及實現

儲昭碧，王 昊，胡永新

（電氣與自動化工程學院 合肥工業大學，合肥 230009）

介紹了一種新式的二連桿機械臂洗車機，相比傳統洗車機，洗車效率高，占地面積小，存在著很好的市場前景。本系統基于臺達DVP10MC11T總線型運動控制器，配合伺服電機，超聲波傳感器，控制精準，設計合理，通過MATLAB仿真，經過多次現場試驗，達到了預期目標。同時，此二連桿機械臂控制算法對類似的控制系統具有一定的參考價值。

機械臂；運動控制系統；超聲波傳感器；伺服系統

0 引言

隨著社會的進步，汽車在人們的日常生活中扮演了越來越重要的角色，洗車行業也是越來越發達，傳統的人工洗車普遍存在著洗車時間長，洗車效率低等弊端，隨著科技的進步，各式各樣的洗車機也如雨后春筍一樣層出不窮。現階段市面上的洗車機基本上分為兩類，一類是無接觸式自動洗車機，一類是毛刷式自動洗車機，前者屬于半自動產品，市面上的基本上只能洗車身兩側，對于車頭車尾和車頂基本上無法洗到，仍需人工清洗；后者往往體積較大，需要的場地比較大，價格高，對于小型的洗車點來說不是很方便[1,2]。本洗車機是基于PLC的二連桿機械臂機構的洗車機，精巧的結構設計使得其要求的場地面積較小，對于車頭車尾和車頂也能很好的洗到，效率高，存在著很好的市場前景。

1 系統結構及運行原理

1.1 系統組成

本系統是一套基于PLC的二連桿機械臂系統[3]，該洗車機由兩個等長（1300mm）的主臂（大臂和小臂）一個L臂組成，其中L臂集成了風管、水管和清洗液管，負責具體的清洗工作。大臂小臂分別各自由一臺伺服電機驅動，負責運動線路的控制，L臂可以自由升降，其升降桿由另一臺伺服電機控制。同時，本系統配備了三個超聲波傳感器以實現對車身檢測及機械臂運動路線的規劃，三個超聲波傳感器一個用來車身側面測距，一個用來車頂測距，一個用來對車身四角進行檢測，以實現轉角信號的發送。車身側面測距傳感器和車頂測距傳感器裝在L臂上，轉角信號超聲波傳感器裝在小臂末端。系統結構框圖如圖1所示。

圖1 系統結構框圖

實際工作中，通過超聲波傳感器[4～7]測出機械臂末端與車身側面的距離d，通過與給定r進行比較得出差值e，同時與伺服電機中讀出的角度一起是輸入傳給控制器，控制器進行計算得出兩個電機的轉速驅動機械臂運動產生x和y方向的位置，使得e趨近于0，同時使y方向的速度達到給定速度。

1.2 運行過程

當洗車輛駛入洗車機內部，安全確認完畢之后，洗車機開始運行，伺服電機驅動大小臂從而使L臂向車身側面靠近，同時，L臂升降桿向車身頂部靠近，當檢測車身側面距離超聲波傳感器檢測到L臂距車身200mm且頂部超聲波傳感器檢測到升降桿距離車頂200mm時，升降桿停止向車頂靠近，L臂停止向車身靠近，根據超聲波傳感器反饋的距離進行PID控制，驅動大小臂轉動使L臂開始以與車身始終保持200mm的距離圍繞著車身運動，風管水管清洗液管配合工作，第一圈噴灑高效清洗液，第二圈空轉一圈讓清洗液充分反應，第三圈進行強力水洗，第四圈進行強力風干。車輛清潔完畢之后，機械臂回到初始位置，洗車結束。

2 系統模型搭建及仿真

2.1 系統模型搭建

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

圖2 系統簡化模型

2.2 公式模型推導

要實現通過控制兩個大臂電機的轉動來實現對其的控制，則需要將平面坐標的運動轉換為大小臂電機旋轉的角速度。

根據圖1，由幾何知識，可以得出：

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

機械臂末端的運動可以分解為x軸方向和y軸方向的運動，vx和vy可以由對x和y求導得出：

x方向的位移應該為實際機械臂末端到車身距離與給定距離的差值e，y軸方向的速度是認為設定的，即機械臂末端的運動x軸方向為減小e的運動，y方向為勻速運動，由此可得：

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

從而可以計算出大小臂電機的轉速，實現控制目的。

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

假設初始的夾角為α0和β0，機械臂末端到原點的距離為D0，位置坐標為（x0,y0）, 定義D邊與x軸正向夾角為θ0。當前時刻的夾角為αt和βt，機械臂末端到原點的距離為Dt，位置坐標為（xt,yt），大小臂旋轉的角速度為ωα和ωβ，定義D邊與x軸正向夾角為θt。

顯然，在時域中：

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

3.3 Simulink仿真

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

為了使結果更加準確，所有參數按實際的大小設置，L臂的長度為1300mm，給定一個機械臂的初始位置，α=5π/3，β=π/3，對應到坐標系中的坐標為（1300，0）。

使用Simulink仿真后結果如下：

圖3 仿真跟蹤曲線

圖4 機械臂與車身實時距離

起始位置為（1300，0），此時車身側面位置為（1000，0），根據運行原理，機械臂應跟車身側面保持200mm的距離，機械臂在x軸方向產生位移靠近車身使距離為200mm，同時y軸方向保持200mm/s的速度。

結合圖3和圖4可以看出，機械臂按照既定路線運行，且運行軌跡跟隨性較好，在轉角處的超調量較小，能滿足實際需要，證明該算法的可行性。

4 系統硬件設計

4.1 硬件選型

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

4.2 硬件連線

圖5為系統硬件結構圖，HMI與PLC通過485通訊進行連接，伺服驅動器ASDA-A2、DVP10MC11T和伺服電機通過網線連接，超聲波傳感器裝在機械臂L臂上。

圖5 系統硬件結構圖

5 軟件實現

本系統軟件設計采用梯形圖編程[13]，主要實現對誤差信號的PID調節，電機轉速和方向的計算和發送，對各個階段的運行狀態進行判斷從而選擇對應的階段參數進行轉速及方向的計算。使用梯形圖編程，邏輯清晰，易讀。程序流程圖如圖6所示，圖中虛線表示PLC循環掃描機制，圖7為運行流程圖。

6 實驗結果

現在市面上傳統的自動洗車機，洗一輛車的平均耗時為10分鐘左右[14]，而本款洗車機設計的沿車身運動的速度為200mm/s，現場測試時，使用的車型車身尺寸為5018×1866×1459，從系統啟動到結束共耗時400s，低于傳統洗車機耗時，效率更高。

圖6 程序流程圖

圖7 運行流程圖

7 結束語

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

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

[2] 儲昭碧,李健,孔艷,馮小英.基于S7-1200的全自動洗車控制系統的設計[J].電氣傳動,2016,(09):62-65.

[3] 王耀南,高小龍.基于CANopen的機械臂控制系統研究[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] 蔡磊,周亭亭,郭云鵬,陳素芳,吳漢幫.基于超聲波定位的智能跟隨小車[J].電子測量技術,2013,(11):76-79+105.

[6] 金英連,王斌銳,方水光.未知環境下柔性二連桿機械臂的自適應阻抗控制[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].電機與控制應用,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的永磁體同步電機模糊PID控制仿真[J].安徽理工大學學報(自然科學版),2016,(06):18-21+46.

[13] 臺達電子工業股份有限公司.臺達DVP-PLC編程技巧:WPLSoft軟件篇[M].中國電力出版社,2012.

[14] 李培麗.節能便攜式洗車機的研制[D].內蒙古工業大學,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

儲昭碧（1970 -），男，安徽人，教授，工學博士，研究方向為現場總線網絡、自動化系統集成等。