曲面輪廓恒力跟蹤的非線性雙閉環(huán)控制

張鐵 胡廣

摘 要：針對機器人在跟蹤未知曲面輪廓運動時，由于缺乏位置信息導致的接觸力不恒定的問題，提出了非線性雙閉環(huán)控制方法。該方法中的雙閉環(huán)包括運動傾角的實時反饋校正器，以及基于非線性PID的法向接觸力控制器。非線性PID控制器是由非線性跟蹤微分器（TD）和狀態(tài)誤差反饋（SEF）構造的，基于線性PID框架的非線性控制器。首先，根據受力模型得到基于力反饋的運動傾角校正策略；另外，考慮到多維力傳感器維間耦合的影響，對Maxwell三維力傳感器的解耦矩陣進行了標定；同時，提出了力信號的自適應慣性濾波方法，改善了濾波器的靈敏度和穩(wěn)定性，綜合性能提高了34.42%；最后，實驗驗證了雙閉環(huán)控制方法能有效提高力控制精度和穩(wěn)定性，具有較強的可執(zhí)行性和適應能力。

關鍵詞：工業(yè)機器人；力反饋；雙閉環(huán)控制；輪廓跟蹤；力傳感器

DOI：

中圖分類號：TP242.2文獻標志碼：A文章編號：

Nonlinear dual-loop force controller of contour following

ZHANGTie， HUGuang

（School of Mechanical and Automotive Engineering， South China University of Technology， Guangzhou 510641， China）

Abstract：Aiming at the problem that the contact force caused by the lack of position information is not constant， the nonlinear double closed-loop control method is proposed for the robot to track the unknown surface contour.Double-loop in this method including movement angle adjuster based on real-time feedback， and the nonlinear PID normal contact force controller. This method is a nonlinear controller based on classical PID frame， composed of nonlinear tracking differentiator （TD） and state error feedbacks （SEF）. First， according to the force model， the motion inclination correction strategy based on force feedback is proposed.In consideration of the cross axis coupling of multi-dimensional force sensor， the decoupling matrix of Maxwell force sensor is calibrated. Meanwhile， adaptive inertia filtering method is proposed to improve the sensitivity and stability of filter， and the comprehensive performance is improved by 34.42%. Finally， the dual-loop control method is verified by experiments. The proposed method can effectively improve the precision and stability， with a strong ability to adapt and enforceability.

Keywords： industrialrobot； force feedback；dual-loop control； contour following； force sensor

引言

力控制輪廓跟蹤通常用于機器人的表面加工作業(yè)中，比如拋光、打磨、去毛刺等。在這些應用中，機器人末端工具與工件之間應該保持期望的接觸力[1-2]。如果接觸力過大會損壞工件或工具，相反，如果接觸力過小則會脫離接觸。為了實現(xiàn)恒力輪廓跟蹤，Antonio Lopes和Fernando Almeida[3]設計了一種機器人主動輔助力-阻抗控制裝置，該裝置采用六自由度并聯(lián)設計，可以實現(xiàn)插孔、輪廓跟蹤等任務。采用輔助裝置的方式雖然可以增加機器人的柔順性，但其機械結構較復雜；Heiko[4]等設計了基于視覺和力傳感器切換的機器人輪廓跟蹤系統(tǒng)，視覺/力切換控制能夠穩(wěn)定的進行輪廓跟蹤任務，但需要用到昂貴的激光視覺傳感器。KazuoKiguchi[5]在機器人跟蹤未知曲面工件的任務中，提出了一種模糊力向量的方法，用來確定力/位置混合控制中的力控制方向。模糊向量法可以確定力控制方向的范圍，但其精確程度受到傳感器干擾或噪聲的影響；Alexander Winkler和JozefSuchy[6]通過在力/位置控制器中加入雙積分器，降低了力控制的穩(wěn)態(tài)誤差。雙積分器的力控制方向固定，不能跟隨輪廓的改變而調整力控制方向。

本文提出的輪廓跟蹤方法是一種非線性雙閉環(huán)控制方法。雙閉環(huán)包括運動傾角的實時反饋校正器，以及基于非線性PID（nonlinear PID， NLPID）的法向接觸力控制器。兩個閉環(huán)控制，通過實時調整力控制方向，同時實現(xiàn)了未知輪廓跟蹤和法向接觸力恒定兩個目的。只要給定任意的初始運動傾角，機器人末端工具都能以恒定的法向接觸力跟蹤未知曲面輪廓運動，該方法具有較強的可執(zhí)行性和適應能力。

曲面輪廓跟蹤運動

基于恒定接觸力的曲面輪廓跟蹤操作，通常在機器人的拋光、打磨、去毛刺等工業(yè)應用中有非常重要的作用。曲面輪廓跟蹤運動的目的是根據力傳感器檢測到的接觸力，控制機器人末端工具沿未知輪廓的曲面運動，并施加恒定的法向壓力。

曲面輪廓跟蹤的受力分析

曲面輪廓跟蹤運動模型如圖1所示，圖中坐標系{T}表示末端工具的運動坐標系和力傳感器坐標系，坐標系{C}的X軸與輪廓切線同向，Y軸與輪廓表面法線同向， 表示兩個坐標系的X軸之間夾角。r表示運動方向，r與XT之間的夾角為 。為了保證法向接觸力恒定，需要沿法向進行調整，即 ，所以 。當工具末端以 的速度沿輪廓切向運動時，工具與工件之間的法向和切向作用力分別為 和 。由傳感器檢測到的兩個方向的力分別是 和 ，它們的大小等于 和 在傳感器坐標系下的投影，于是可得：

（1）

通過上式可以得到法向力 和切向力 ：

（2）

圖1 受力分析

Fig.1 Stress analysis

由圖1可知，曲面跟蹤的運動時，工具沿曲面切線方向以勻速 運動的同時，要保證法向接觸力恒定。為保證法向接觸力恒定，在法向上由測量得到的法向接觸力，根據控制算法產生一個法向調整速度 。將曲面法向和切向運動 和 ，轉換成末端運動坐標系的運動速度 和 得：

（3）

雙閉環(huán)恒力控制方法

工件與工具末端之間的接觸力是由系統(tǒng)的變形產生的，圖1中 表示法向形變量。當由形變量 產生的法向接觸力與期望的法向接觸力相等時，即當 時，其中k表示法向剛度，則工具沿法線方向不需要調整，即 ，則 。 是 與 的合運動方向與XT之間的夾角，即工具末端的運動傾角，有：

（4）

接觸狀態(tài)時，期望的法向接觸力： ，力反饋控制周期為： 。當 ，實際法向力：

（5）

當 時，有：

（6）

式中：v表示末端運動速度。則可知實際法向接觸力與 及 之間的關系為：

（7）

由此可知： 時， ， 時， 。即在圖1中，當運動方向為r1時，實際法向接觸力 大于期望接觸力 ；當運動方向為r2時，實際法向接觸力 小于期望接觸力 。利用這個特點，可以根據反饋的實際法向接觸力 來調整運動傾角 ，使得 的同時 。

由上式求解出 為：

（8）

可知偏轉角 的大小與法向接觸力誤差 ，以及末端運動傾角 有關。因此，可以根據力傳感器反饋得到的法向接觸力誤差和當前時刻的運動傾角 ，來校正運動傾角，使得 。

由此提出了如圖2所示的基于NLPID的雙閉環(huán)力控制方法?？刂破髦械碾p閉環(huán)包括基于力反饋的運動傾角校正器，以及基于NLPID的法向接觸力控制器。圖中 表示期望的法向接觸力， 表示實際法向接觸力，u表示控制量， 表示坐標系{C}到{T}的運動變換矩陣，如式（3）所示。跟蹤微分器（tracking differentiator， TD）和狀態(tài)誤差反饋（state error feedback， SEF）構成了NLPID控制器。誤差 與當前運動傾角 用來校正運動傾角，如式（9）所示。

圖2雙閉環(huán)控制

Fig.2 Double loop control

NLPID控制器的輸出是法向的運動調整量 ，運動控制器根據輸入的 將法向的運動向量 ，根據坐標關系轉換成工具坐標系下的運動向量 。用傳感器測量得到的兩個軸向力 和 ，以及 來計算實際測量得到的法向接觸力 ，并將 作為控制器和校正器的反饋信號。如果初始運動傾角 越接近實際 ，那么運動傾角 就能更快收斂到輪廓傾角 。校正器根據輸入的 和 得到校正之后的運動傾角 ，由式（8）可知校正公式為：

（9）

式中： 表示n時刻的運動傾角。

圖2中，SEF將誤差 、誤差的積分及誤差的微分 的線性組合作為控制器的輸出，與PID控制的結構相同，所以SEF的傳遞函數為：

（10）

式中： 分別表示SEF的控制參數。

控制對象 的輸入為法相速度信號 ，輸出為法相接觸力 ，根據彈簧滑塊模型忽略阻尼作用， 可以簡化為一個積分環(huán)節(jié)：

（11）

式中：τ表示積分常數。

由式（10）和（11）得到的系統(tǒng)開環(huán)傳遞函數為：

（12）

開環(huán)傳遞函數 在S平面右半部無極點，為了得到穩(wěn)定的閉環(huán)系統(tǒng)，開環(huán)幅相頻率特性曲線不能包圍 點，即幅相頻率特性曲線與實軸的交點大于-1：

（13）

由式（13）可知，適當調整控制參數，使得穩(wěn)定條件 成立，閉環(huán)系統(tǒng)就能穩(wěn)定。

非線性PID控制

NLPID力控制器的基本結構如圖3所示。圖中所示的NLPID控制用到了兩個非線性跟蹤微分器，分別用來安排過渡過程以及對系統(tǒng)輸出進行跟蹤微分處理。圖中 表示期望的接觸力； 和 分別代表輸入信號的跟蹤信號及其微分信號； 和 分別表示輸出信號的跟蹤信號及其微分信號； ， ，分別表示狀態(tài)誤差及其微分；u是由誤差的反饋組合得到的控制量； 和 分別表示對象模型及干擾； 表示濾波器； 表示測量得到的實際法向接觸力。

帶有慣性濾波的非線性PID控制算法流程如下：

首先，以 作為輸入安排過度過程[7]：

（14）

式中： 是最速控制綜合函數[7]；需要調整的參數為 和 ； 是中間變量。其中 代表跟蹤速度的快慢；濾波因子 ，其中h是控制步長。

將系統(tǒng)的輸出信號進行慣性濾波：

（15）

式中： 是慣性濾波系數， 是原始輸出信號， 是濾波之后的輸出信號。

利用跟蹤微分器提取輸出信號的微分[7]：

（16）

式中： 是輸入， 、 是輸出， 是中間變量。力傳感器信號中一般都有噪聲，取濾波因子 。

最后，產生誤差信號和誤差反饋控制信號：

（17）

式中： 是需要調整的控制參數。因為安排了過度過程以及對輸出信號進行了跟蹤微分處理，使得控制參數可以在較大的范圍內取值，而不影響控制效果[8]。

力信號的測量與處理

三維力傳感器的解耦標定

實驗中采用的力傳感器是Maxwell的A3D46-20kg三維力傳感器。其非線性小于 ，重復性小于 。由于沒有能夠檢測力矩信息的通道，三維力傳感器只能在固定點受力，如果受力點改變，必須重新進行標定，否則會產生力耦合使測得的力值不正確，導致機器人誤操作[9]。

三維力傳感器在校驗出廠時，給出的是基于O點的標定矩陣，即受力點必須設定在O點，如圖4所示。三維力解耦方程為： 。其中 是三個方向的力向量； 是三個通道輸出的電壓向量； 是O點的解耦矩陣。

如圖4所示，當在 點受到X方向的力 時，傳感器相當于在O點受到 的力和 的扭矩。如果在O點單獨作用力 時輸出的電壓是 ，而在O點同時作用 和 時輸出的電壓是 。由于三維力傳感器沒有測量力矩的通道，由 產生的變形影響到其他通道的輸出電壓，使得 。而利用公式 進行計算的時候，由 導致的輸出電壓誤差就會疊加到各軸的測量力值上，導致三個軸的測量值都不準確。

采用的解耦標定方法是在受力點附近的三個方向懸掛已知重量的砝碼，分別測量三個通道的輸出電壓，得到矩陣方程：

（18）

式中： 表示對角矩陣 ， 是解耦矩陣， 表示加載力矩陣 時，三個通道的輸出電壓矩陣。由以上方程可以得到解耦矩陣： 。

標定過程中，在受力點處，分別于三個受力方向加載21.76N的力，并測量傳感器三個通道輸出的電壓值，求取電壓的平均值，得到解耦矩陣：

（19）

為了驗證該解耦矩陣能否準確測量各軸的力，在X軸與Y軸之間 的方向加載12N的力，并在6s左右增加到13.5N。采樣周期為 。結果如圖5所示，在0-5s時X和Y軸的理論值是8.49N，7-13s時X和Y軸的理論值是9.55N。圖中5-7s的波動是在增加重量時引起的振動。穩(wěn)定狀態(tài)時，X和Y軸的測量誤差不超過 ，小于廠家給出的標定誤差 。結果表明，三維力傳感器通過標定得到的解耦矩陣能夠準確測量各軸的力，且具有較高的精度，滿足實際應用要求。

圖5解耦效果

Fig.5 Decoupling effect

自適應慣性濾波

力信號中包含測量誤差與干擾信號，需要進行濾波處理。設計濾波器時要考慮到力控制系統(tǒng)中實時性的要求，故采用較簡單的慣性濾波，即一階滯后濾波器。慣性濾波器的傳遞函數是一個時間常數為T的慣性環(huán)節(jié)，即：

（20）

式中： 和 分別表示輸入和輸出。

設采樣時間間隔為 ，那么慣性濾波在時域上可以表示成：

（21）

式中： 和 分別表示當前采樣值和當前濾波值， 表示上一時刻濾波值。設：

（22）

則慣性濾波的離散表達式可以簡單的表示成：

（23）

式中：濾波系數 。

濾波系數 的取值越靠近0，則濾波結果越平穩(wěn)，越不容易被干擾所影響；但是延時也較大，跟隨信號變化的速度緩慢；而 取值越靠近1時，靈敏度雖然高，但濾波效果不明顯，波動較大。為了達到穩(wěn)定性好靈敏度又高的濾波效果，采用參數自適應調整策略來優(yōu)化慣性濾波器。優(yōu)化后的濾波器應該有如下特點：

1）當輸入信號趨于穩(wěn)定，在一個值上下振蕩時，應該采取穩(wěn)定性優(yōu)先策略，取小濾波系數 。

2）當信號朝一個方向快速變化時，濾波結果需要及時跟進，這時就要放棄穩(wěn)定性而采取靈敏度優(yōu)先策略，即選擇較大的濾波系數 。

于是提出了濾波系數的自適應調整策略如下：

（24）

式中： 和 表示連續(xù)兩次采樣信號的變化方向， 表示濾波系數的調整速度，M表示信號變化閥值， 表示兩次輸入信號的差， 是符號函數。

為了驗證式（24）提出的自適應慣性濾波的濾波效果，用力傳感器測量的數據進行了濾波實驗。分別用固定濾波系數和自適應濾波系數處理同一段采樣數據并進行對比。濾波參數如表1所示：

濾波效果如圖6所示，上圖為濾波信號，下圖為濾波系數變化情況?？梢钥闯鲈?-9s及11-20s的時候，采樣信號趨于平穩(wěn)， 的值在0附近波動，自適應慣性濾波信號也非常平滑，沒有產生波動；而在 左右，采樣值朝一個方向出現(xiàn)劇烈變化時， 自動增大，達到 左右，濾波值能及時跟蹤采樣的變化。結果說明，自適應慣性濾波能夠根據實際采樣數據調整濾波系數，從而達到靈敏度高且穩(wěn)定性好的濾波效果。

圖6自適應慣性濾波

Fig.6 Adaptiveinertial filtering

曲面輪廓恒力跟蹤實驗

曲面輪廓跟蹤實驗裝置如圖7所示。實驗時假定初始處于接觸狀態(tài)，且曲面輪廓未知。力傳感器的信號通過放大器后輸出模擬量信號，通過端子板的A/D端口輸入到控制器。控制器接收到電壓信號后，通過解耦矩陣（19）計算得到三維力傳感器在各方向上檢測到的力信號，將力信號由式（24）濾波后作為系統(tǒng)的反饋信號。同時控制器根據控制算法輸出控制量，通過速度控制模式控制工具末端的運動。

斜面恒力跟蹤控制

為了驗證基于力反饋的運動傾角調整方法的有效性，進行了力控制斜面輪廓跟蹤實驗。采樣周期 ，斜面的傾角 ，任意選取初始的 ，期望的法向接觸力 ，取跟蹤運動速度 ，經過檢測得到的接觸剛度 。非線性PID控制器的主要參數如表2所示。圖8所示為運動傾角 與法向接觸力 的實時調整情況。

運動控制開始后，運動傾角 迅速調整到輪廓傾角 的同時，法向接觸力 也逐漸穩(wěn)定到期望接觸力 。調整時間 ，穩(wěn)態(tài)時 ，平均跟蹤誤差 ，平均法向接觸力 ，平均力控制誤差 。由此說明基于力反饋的運動傾角調整方法，可以實現(xiàn)恒定法向接觸力的斜面跟蹤運動控制。

未知曲面輪廓跟蹤

如果工件表面輪廓是未知曲面時，輪廓傾角 是時變的。為了驗證這種情況下，能否通過實時校正運動傾角 的方式控制恒定的法向接觸力，利用雙閉環(huán)控制方法對圖9所示的曲面工件進行了輪廓跟蹤實驗。實驗過程中任意選取初始的 ，期望的法向接觸力 ，曲面跟蹤運動的合速度 ，其他運動控制參數與4.1節(jié)相同。

圖9曲面工件

Fig.9 Curved surface

工具末端的運動軌跡與曲面輪廓之間的關系如圖10所示，最大輪廓跟蹤誤差 ?？刂破鞯目刂颇繕耸呛愣ǖ姆ㄏ蚪佑|力，而機器人在各個位姿的剛度都不一樣，所以產生了誤差。

運動傾角與實際輪廓傾角之間的關系如圖11所示。由圖中可以看出，經過反饋校正后的運動傾角 能夠跟蹤實際輪廓傾角 的變化。圖12表示輪廓跟蹤運動過程中實際法向接觸力的變化情況，由圖可以看出在整個運動過程中工具末端一直與工件表面接觸，而且實際法向接觸力穩(wěn)定在期望接觸力附近。平均法向接觸力 ，法向接觸力控制誤差為 ，處于力傳感器測量誤差范圍之內。由此可以說明，非線性雙閉環(huán)控制方法，在對未知曲面輪廓的恒力跟蹤運動上是可行的，且保證了一定的法向接觸力控制精度。

為了與其他方法進行對比，設計了單方向的接觸力閉環(huán)控制實驗。與雙閉環(huán)控制相比，該方法不進行運動傾角校正，所以力控制方向不能保證始終處于輪廓法向。實驗結果分別如圖13和14所示。圖中 和 分別表示在傳感器坐標系的X方向和Y方向測量的接觸力， 表示輪廓法向接觸力。

圖13所示法向接觸力的平均值為： ，平均誤差為： ，最大誤差為： ；而圖14所示法向接觸力的平均值為： ，平均誤差為： ，最大誤差為： 。對比實驗結果可以看出，單向力控制可以使Y軸的接觸力比較穩(wěn)定，卻忽略了法向接觸力的波動；而雙閉環(huán)控制則可以確保法向接觸力的穩(wěn)定。由此可以看出雙閉環(huán)控制可以提高法向接觸力的穩(wěn)定性和控制精度。

結論

為了實現(xiàn)未知曲面輪廓的恒力跟蹤運動，提出了一種基于運動傾角實時校正及法向接觸力控制的非線性雙閉環(huán)控制方法。鑒于三維力傳感器的強耦合特性，對其進行了解耦標定，標定后的測量誤差小于 ；設計了自適應慣性濾波方法，同時改善了濾波器的靈敏度和穩(wěn)定性，綜合性能提高34.42%。分析了輪廓跟蹤運動模型，推導出接觸力反饋校正運動傾角的公式，從而提出雙閉環(huán)控制結構。實驗表明，雙閉環(huán)控制方法可以實現(xiàn)恒力跟蹤控制，且輪廓傾角跟蹤誤差小于0.5%，平均法向接觸力控制誤差約為 ，比單向力閉環(huán)控制的法向接觸力穩(wěn)定性提高31.2%。本文提出的非線性雙閉環(huán)控制方法，可用于機器人打磨拋光時對工件輪廓進行跟蹤，具有一定的法向接觸力控制精度。

參考文獻：

[1]JAVIERPliego， MARCO A Arteaga. Adaptive position/f?orce control for robot manipulators in contact witha rigid surface with uncertain parameters[J]. European Journal of Control， 2015，22：1-12.

[2]魏立新，李二超，王洪瑞. 基于混合優(yōu)化神經網絡的機器人力/位置控制[J]. 電機與控制學報，2006，02：151-153，159.

WEILixin，LI Erchao，WANG Hongrui. Hybrid force/position control based on mixed optimal neural n?e?tworks[J]. Electric Machines and Control，2006，02：151-153，159.

[3]ANTONIO Lopes， FERNANDO Almeida. A forcei?m?peda?n?c?e controlled industrial robot using an a?ctiveroboti?caux?i?l?i?arydevice[J]. Robotics and ComputerIntegrat?e?dManufacturing， 2008，24：299-309.

[4]HEIKO Koch， ALEXANDERKonig，ALEXANDRAWeigl-Seitz， et al. Force， acceleration and vision sensor fusion for contour following tasks with an industrial rob?ot[J].IEEE International Symposium on Robotic & Sensors Environments， 2011：1-6.

[5] KAZUOKiguchi. Position/force control of robot ma-nipulators for geometrically unknown objec?t?s usin-gfuzzy neural networks[J]. IEEE Transactions On I-ndustrial Electronics， 2000，47（3）：641-649.

[6]ALEXANDER Winkler， JOZEFSuchy. Force controlled c?o?ntour following on unknownobjects with an ind?u?strialrobot[J]. IEEE International Symposium on R?o?b?otic & Sensors Environments，2013：208-213.

[7]韓京清. 自抗擾控制技術[M]. 北京： 國防工業(yè)出版社，2008：17-72.

[8]趙永祥， 夏長亮， 宋戰(zhàn)鋒， 等. 變速恒頻風力發(fā)電系統(tǒng)風機轉速非線性PID控制[J].中國電機工程學報， 20?08，28（11）：133-138.

ZHAOYongxiang， XIAChangliang， SONGZhanfeng， et al. Nonlinear PID rotating speed c?ontrol of variable speed constant frequency windturbinesystem[J]. Proceedings of the CSEE，2008，28（11）：133-138.

[9]曹會彬， 孫玉香， 劉利民， 等. 多維力傳感器耦合分析及解耦方法的研究[J]. 傳感技術學報， 2011，24（8）：1136-1140.

CAOHuibin， SUNYuxiang， LIU Limin， et al. Coupling analysis of multi-axis force sensor and research of decoupling method[J]. Chinese Journal of Sensors and Actuators，2011，24（8）：1136-1140.