基于DSP的機(jī)器人末端力控制系統(tǒng)設(shè)計

王 志，汪步云，許德章

(1.蕪湖安普機(jī)器人產(chǎn)業(yè)技術(shù)研究院，安徽 蕪湖 241000; 2.安徽工程大學(xué) 機(jī)械與汽車工程學(xué)院,安徽 蕪湖 241000)

基于DSP的機(jī)器人末端力控制系統(tǒng)設(shè)計

王 志1，汪步云2，許德章2

(1.蕪湖安普機(jī)器人產(chǎn)業(yè)技術(shù)研究院，安徽 蕪湖 241000; 2.安徽工程大學(xué) 機(jī)械與汽車工程學(xué)院,安徽 蕪湖 241000)

工業(yè)機(jī)器人的運動分為自由運動和受力約束運動兩種不同的運動類型;受力約束運動不僅要進(jìn)行精確的位置控制，而且要對接觸力進(jìn)行精確控制;文章對機(jī)械臂的末端力控制系統(tǒng)進(jìn)行了研究;采用了高精度電動滑臺作為力控制系統(tǒng)的執(zhí)行機(jī)構(gòu)，在電動滑臺末端增加了柔性單元，從而克服了系統(tǒng)柔性不足的缺點，并且依據(jù)Lyapunov穩(wěn)定性理論確定系統(tǒng)的二階參數(shù)，使用Narendra提出的穩(wěn)定自適應(yīng)設(shè)計方法構(gòu)建了電動滑臺的數(shù)學(xué)模型;對機(jī)械臂末端柔順系統(tǒng)的系統(tǒng)設(shè)計及算法進(jìn)行了研究，消除了外界噪聲對系統(tǒng)的干擾，解決了力控制的基本控制策略問題，優(yōu)化了力控制時系統(tǒng)的響應(yīng)特性。

工業(yè)機(jī)械臂;阻抗控制;受力約束運動

0 引言

傳統(tǒng)規(guī)劃控制算法對受力約束控制無法達(dá)到令人滿意的控制效果，力約束控制不僅要進(jìn)行精確的位置控制，而且必須對力接觸進(jìn)行精確控制，如果不能對力進(jìn)行精確控制，則可能在工作過程中損壞設(shè)備[1]。受力約束控制又稱為柔順控制，柔順控制就是利用能反映外部力的傳感器對工業(yè)機(jī)器人的運動軌跡進(jìn)行調(diào)整，使得工業(yè)機(jī)械臂對外界空間力的變化有響應(yīng)的能力，因此，我們稱工業(yè)機(jī)械臂對外界環(huán)境具有柔順性[2]。文章采用阻抗控制方法對系統(tǒng)進(jìn)行力控制。

1 系統(tǒng)的整體設(shè)計方案

總體設(shè)計思想是：利用高精度輕載電動滑臺作為執(zhí)行機(jī)構(gòu)，當(dāng)柔順機(jī)構(gòu)處于位置控制模式運動時候，系統(tǒng)根據(jù)預(yù)先規(guī)劃好的路徑運動；當(dāng)外界接觸力超過切換閥值時，切換至力控制模式，同時，力控制模式下根據(jù)事先設(shè)定好的額定壓力值進(jìn)行運動，這個模式也稱之為恒力控制模式[3]。這個過程就是完整的力控制。整體設(shè)計思想如圖1所示。

圖1 柔順控制總體設(shè)計方案

2 高精度電動滑臺的數(shù)學(xué)建模

柔順系統(tǒng)的電動滑臺的動力學(xué)模型為圖2所示。

圖2 電動滑臺動力學(xué)模型

圖2中,Jm為旋轉(zhuǎn)零件的總轉(zhuǎn)動慣量;θm為滾珠絲杠的轉(zhuǎn)角;T為伺服電機(jī)的力矩值;K為傳動系統(tǒng)的軸向剛度;ct為直線導(dǎo)軌的粘性摩擦系數(shù);ci為傳動系統(tǒng)軸向粘性摩擦系數(shù);cb為旋轉(zhuǎn)零件的總粘性摩擦系數(shù);Mt為柔性元件和滑臺的質(zhì)量。

文章根據(jù)Lyapunov穩(wěn)定性理論確定系統(tǒng)的二階參數(shù)，使用Narendra提出的穩(wěn)定自適應(yīng)設(shè)計方法構(gòu)建了電動滑臺的數(shù)學(xué)模型[5]。由此得到電動滑臺的傳遞函數(shù)：

(1)

將電動滑臺的主要技術(shù)參數(shù)帶入公式(1)，得到傳遞函數(shù)：

(2)

3 柔性元件的數(shù)學(xué)建模

柔性元件主要由圓柱螺旋彈簧、3D打印結(jié)構(gòu)部件、阻尼器、圓柱導(dǎo)軌及直線軸承構(gòu)成。

由物理學(xué)分析可以把阻尼元件、圓柱導(dǎo)軌和直線軸承視作阻尼器；結(jié)構(gòu)部件和壓力傳感器視作質(zhì)量塊(物體)；圓柱螺旋彈簧視作理想彈簧。柔性元件的力模型簡化如圖3所示。

圖3 柔性元件力學(xué)模型

根據(jù)材料的已知特性：論文把圓柱形螺旋彈簧視作理想的彈簧，忽略彈簧的阻尼[6]。且已知彈簧的彈性系數(shù)k=0.5 N/M；阻尼器的阻尼系數(shù)隨著速度的變化而發(fā)生變化，且其摩擦力和速度的大小成正比關(guān)系，論文把比例系數(shù)設(shè)為阻尼系數(shù)c=0.1；壓力傳感器和3D打印結(jié)構(gòu)質(zhì)量和為0.25 kg；由于圓柱導(dǎo)軌和直線軸承之間的摩擦力非常小，所以論文取摩擦力f0=0。

設(shè)系統(tǒng)的輸入力大小為u(t)，輸出力大小為y(t)，柔性元件的阻尼器其摩擦力大小為fdy(t)/d(t)，可以的得到系統(tǒng)的運動方程：

(3)

則可以得到系統(tǒng)的傳遞函數(shù)為：

(4)

把已知值代入傳遞函數(shù)，可以得帶柔性元件的傳遞函數(shù)：

(5)

4 壓力信號的改進(jìn)型小波閥值降噪

信號在采樣和傳輸過程之中，難免會夾雜一些噪聲信號，從而造成信號的失真，若直接將染上噪聲的信號作為控制系統(tǒng)的輸入信號，則會極大地影響傳感器補償后的精度，增加系統(tǒng)誤差，降低了傳感器補償?shù)囊饬x。因此，文章采用改進(jìn)型小波閥值算法進(jìn)行力信號的降噪處理。改進(jìn)型小波閥值函數(shù)如公式(6)。

(6)

為了驗證改進(jìn)型閥值函數(shù)的降噪效果，在MATLAB的小波工具箱中采用DB4小波為母小波；輸入的原信號使用sref信號，干擾信號使用高斯白噪聲；采樣的數(shù)據(jù)長度為2000，小波的分解層數(shù)為4層，并與傳統(tǒng)軟閥值降噪算法、硬閥值降噪算法、文獻(xiàn)[4]算法作了比較，驗證算法降噪效果。

圖4 軟閥值及硬閥值降噪信號

圖5 改進(jìn)型小波閥值函數(shù)降噪及文獻(xiàn)[4]算法出現(xiàn)的劇烈振蕩

降噪算法硬閥值降噪軟閥值降噪改進(jìn)型閥值降噪信噪比/db24.861124.819924.8842均方誤差/db1.33711.40501.0339

圖4左上圖是原始方波信號，右上圖是加過高斯白噪聲后的信號，左下圖是通過小波軟閾值函數(shù)處理過的信號，右下圖是小波硬閾值函數(shù)處理過的信號。綜合圖4可以看出軟閥值降噪在細(xì)節(jié)處理上較為模糊，不過沒有明顯的振蕩；在對細(xì)節(jié)信號的處理上，硬閥值降噪比軟閥值降噪好，但是可以看到處理后的信號有明顯的振蕩。圖5中改進(jìn)型閥值降噪相對于軟閥值降噪細(xì)節(jié)部分保留較好，也沒有硬閥值算法和文獻(xiàn)[4]算法降噪時明顯的振蕩，其最終降噪效果要好于其它三種算法。

根據(jù)表1信噪比SNR數(shù)據(jù)進(jìn)行分析，可以看出改進(jìn)型閥值降噪的信噪比SNR值為24.884 2，稍大于硬閥值和軟閥值算法；再從MSE均方誤差來看，改進(jìn)型閥值降噪均方誤差值位1.033 9，遠(yuǎn)小于前兩種傳統(tǒng)的算法；根據(jù)表1分析新的算法在降噪效果上較傳統(tǒng)算法有一定優(yōu)勢。

5 力控制的PI參數(shù)模糊自整定

機(jī)械臂末端柔順系統(tǒng)的參數(shù)模糊自適應(yīng)整定的原理如圖6所示。

圖6 PID參數(shù)模糊自整定

圖6中，F(xiàn)D是系統(tǒng)預(yù)設(shè)的壓力值；Fm是經(jīng)過重力補償后得到的力傳感器測量值，也就有實際接觸的力；Fecc是當(dāng)前的力誤差和上一時刻力誤差間的差，也就是力誤差變化量；Fec是期望力值和實際接觸力值之間的差，也就是力誤差；模糊控制系統(tǒng)的輸入力誤差變化量Fecc與Fec，通過模糊推理后輸出量即ΔKp、ΔKi、ΔKd，也就是PID比例、積分、微分參數(shù)的調(diào)整量；θ是通過運動學(xué)計算后控制器發(fā)給力控制系統(tǒng)的控制量；x0是DSP的預(yù)設(shè)軌跡；Vn是PID控制的輸出量，也就是機(jī)器人末端柔順系統(tǒng)在工件坐標(biāo)系里的調(diào)整速度。

以控制系統(tǒng)的響應(yīng)速度、穩(wěn)定性、穩(wěn)態(tài)精度和超調(diào)量幾方面作為指標(biāo)來進(jìn)行分析，Kp、Ki、Kd三個調(diào)整量的作用如下。Kp：加快系統(tǒng)的響應(yīng)速度，提高系統(tǒng)的精度；Ki：消除控制系統(tǒng)靜態(tài)誤差；Kd：改善了控制系統(tǒng)的動態(tài)特性，抑制系統(tǒng)的偏差變化量。根據(jù)實驗及仿真可以得出模糊自適應(yīng)調(diào)整PI參數(shù)的調(diào)整規(guī)則如表2所示。

表2 參數(shù)ΔKp調(diào)整模糊規(guī)則

表3 參數(shù)ΔKi調(diào)整模糊規(guī)則

表4 參數(shù)ΔKd調(diào)整模糊規(guī)則

根據(jù)圖6的PID參數(shù)模糊自整定方法，并帶入前文的電動滑臺和柔性單元的傳遞函數(shù)，可以得到系統(tǒng)的模糊控制SIMULINK仿真圖見圖7。

圖7 PID參數(shù)模糊自整定仿真

帶入整定好的模糊參數(shù)，仿真設(shè)定接觸力為4.5 N，仿真結(jié)果見圖8，其中實線是期望力，虛線是系統(tǒng)跟蹤力曲線；在0.3 s時，期望力為4.5 N，系統(tǒng)跟蹤曲線在0.6 s左右達(dá)到4.5 N收斂并趨于穩(wěn)定，系統(tǒng)沒有出現(xiàn)明顯的超調(diào)。

圖8 PID參數(shù)的模糊自調(diào)整仿真

6 實驗結(jié)果

實驗采用華盛科技的6軸機(jī)械臂作為本體，將末端力控制系統(tǒng)安裝在第6軸上?？刂破鞑捎肨I公司的TMS320F28335型DSP。

實驗設(shè)定接觸時，末端力控制系統(tǒng)把力控制在4.5 N。圖9是接觸時DSP采集到的力反饋數(shù)據(jù)示意圖。

圖9 接觸時的力反饋值

7 結(jié)論

圖10中，上面的圖形是力反饋信號，下面的是期望的力信號。在碰撞發(fā)生之前，系統(tǒng)受力為零，處于不受約束的運動狀態(tài)；在第30個采樣周期左右時，碰撞發(fā)生。從上圖可看出，系統(tǒng)沒有發(fā)生明顯振蕩，但是出現(xiàn)少量超調(diào)，持續(xù)時間約40個采樣周期；峰值約4.75 N，發(fā)生在第42個采樣周期左右。之后經(jīng)過20個采樣周期，系統(tǒng)收斂并進(jìn)入穩(wěn)定狀態(tài)，接觸力保持在期望值4.5 N左右。上述實驗結(jié)果與仿真結(jié)果接近，達(dá)到了預(yù)期效果。

[1] 畢 勝.國內(nèi)外機(jī)械手發(fā)展現(xiàn)狀[J].機(jī)械工程師，2008(7):5-8.

[2] 李曉燕.機(jī)械手力位置控制方法研究與應(yīng)用[D].河北大學(xué)，2011.

[3] 盛國棟，曹其新，潘鐵文，等.主從式機(jī)械手系統(tǒng)中力反饋的實現(xiàn) [J].中國機(jī)械工程，2009,S2:106-111.

[4] 吳光文，王昌明，包建東，等. 基于自適應(yīng)閾值函數(shù)的小波閾值去噪方法[J].電子與信息學(xué)報，2014(6):1340-1347.

[5] Le T H，Tamura Y，Matsumoto M.Spanwise pressure conherence on prisms using wavelet transform and spectral proper orthogonal decomposition based tools[J].Journal of Wind Engineering and Industrial Aerodynamics，2011，99(4):499-508.

[6] 周 堯，張豐華，田 灃，等.基于 Matlab 的圓柱螺旋彈簧設(shè)計計算與優(yōu)化[J].設(shè)計與制造，2015，28(1):124-125.

Design of End Force Control System of Robot Based on DSP

Wang Zhi2, Wang Buyun2, Xu Dezhang2

(1.Wuhu Anpu Robot Industrial Technology Research Institute, Wuhu 241000,China;2.School of Mechanical and Automotive Engineering, Anhui Polytechnic University, Wuhu 241000,China)

The industrial robot motion can be divided into two different motion types of free motion and force- constrained motion. Not only the position of the force-constrained motion is controlled accurately, but also the contact force is controlled accurately. The paper studies the control system of end force of mechanical arm. High-precision electric sliding table is adopted as the actuator of the control system, and the flexible unit is added at the end of the electric sliding table, so that the drawback of system flexibility deficiency is overcome. The second order parameter of the system is determined based on the Lyapunov stability theory, and the stable adaptive design method proposed by Narendra is used to build the mathematical model of electric sliding table. The paper studies the design and algorithm of the soft system of the mechanical arm end. The influence of outside noise on the system is eliminated, the basic control strategy problem of force control is solved, and the response characteristics of the system is optimized when the system is controlled by force.

industrial mechanical arm; impedance control;force-constrained motion

2016-12-30；

2017-03-10。

安徽省科技攻關(guān)項目(1604a0902125);安徽省自然科學(xué)基金項目(1608085QF154);蕪湖市科技計劃項目(2016cxy03)。

王 志(1987-)，男，安徽蕪湖人，碩士研究生，主要從事運動控制系統(tǒng)分析與設(shè)計方向的研究。許徳章(1964-)，男，安徽廬江縣人，博士，教授,主要從事機(jī)器人信息感知，服務(wù)機(jī)器人與特種機(jī)器人方向的研究。

1671-4598(2017)08-0046-04

10.16526/j.cnki.11-4762/tp.2017.08.013

TP273

A