聯合仿真下機械臂終端約束模型預測控制方法

2022-08-27 02:27:36李義帥王曉華王文杰

西安工程大學學報 2022年4期

張蕾，李義帥，王曉華，王文杰

(1.西安工程大學電子信息學院，陜西西安 710048；2.陜西省人工智能聯合實驗室西安工程大學分部，陜西西安 710048)

0 引言

隨著全球新冠疫情的反復以及人口老齡化所造成的勞動力短缺與用工難等問題，機械臂的應用需求日益旺盛[1]。機械臂逐步替代人類生產生活中具有枯燥重復及惡劣危險性質的工作，而完成這些工作對機械臂的性能具有極高的考驗，對機械臂的本體設計制造與控制方法的設計都是挑戰。機械臂采用傳統的瀑布式流程設計開發，存在著開發效率低下、成本較高等問題，且機械臂的軌跡跟蹤控制是機械臂大規模應用的基礎性問題[2-3]。

針對機械臂的軌跡跟蹤控制問題，國內外眾多學者對此深入研究并取得頗多建設性的成果[4-5]。在日常應用中，傳統的PID控制方法是機械臂運動控制應用最廣泛的控制方法之一，但是多關節機械臂存在著非線性耦合現象，其各個連桿之間存在著相互力作用，會影響到機械臂高速運動時的控制精度以及穩定性，且無法解決機械臂關節運動存在的約束問題，會造成較大的跟蹤誤差以及抖動現象[6-9]。模型預測控制(model predictive control，MPC)是目前主流的先進控制方法之一，具備在復雜條件下處理約束的能力，在機械臂運動控制的領域不斷發展，在學術界和工業界都有著大量的研究與應用，已成為解決機械臂控制問題的流行方法[10-11]。HASSAN等針對柔性單關節機械臂振動抑制的問題，采用有限元法建模，基于階躍響應實現動態矩陣控制，但階躍響應并不能很好地應用在多關節機械臂控制[12]；HEDJAR等針對雙關節機械臂軌跡跟蹤控制問題，采用泰勒展開法對機械臂模型進行線性化處理，設計固定控制律，同時加入積分環節確保系統的魯棒性，但是對機械臂所建立的模型與本體之間的精確度要求高，且未對約束條件進行處理[13]；BELAD等針對工業并聯機械臂，對外界噪聲擾動進行估計實現調整模型預測控制方法的參數，雖然抑制了系統擾動造成的誤差，但是忽略了約束條件，并不能應用在真實硬件環境上[14]。在進行控制方法設計時，通常情況采用理論計算來研究機械臂的動力學控制問題，但是存在計算量過大的弊端，而且專家學者們對機械臂模型進行了簡化，使得理論仿真結果并不理想[15]。另外，真實的機械臂測試成本較高且在實驗室環境下存在一定危險，建立具有更好的技術基礎設施的高度復雜的實驗臺需要巨大的成本。為克服這些問題，可以使用仿真環境模擬、可視化和驗證控制方法。

針對機械臂傳統開發過程存在的跟蹤誤差較大、無法處理約束、效率低下、成本較高等問題，本文提出一種基于終端約束的機械臂軌跡跟蹤模型預測控制方法。該控制方法通過設置目標函數及終端狀態等式約束條件將機械臂軌跡跟蹤問題轉化成模型預測控制的數值規劃問題。再基于模型在環的仿真思想，采用輕量化低成本設備樹莓派Raspberry Pi搭建基于Gazebo-Simulink的機械臂聯合仿真平臺，驗證本文所提控制方法的有效性。

1 三關節機械臂

良好的機器人仿真系統能夠構造接近真實世界的物理環境，快速測試機器人設計可靠性以及控制方法的可用性[16-17]。Gazebo是一款開源軟件，對于復雜的多剛體仿真速度快、實時性強，相比于傳統的ADAMS軟件，更適合對機械臂進行動力學仿真。通過與Simulink進行協同工作，借助Simulink的圖形化編程可以實現快速編程，降低開發難度，提高開發效率[18]。

本文研究的被控對象不是機械臂的數學模型，是Gazebo中的機械臂物理模型，其模型文件包含了機械臂的運動學與動力學參數，Gazebo中的機械臂物理模型具備真實機械臂的動力學特性，需要根據動力學方程所呈現出來的機械臂系統的驅動力矩與關節位移的輸入輸出關系進行控制方法設計。

接下來建立SoildWorks中的虛擬物理樣機模型，并導出機械臂的統一機器人描述格式(unified robot description format ，URDF)文件，為后續的Gazebo機械臂物理仿真的實現提供建模基礎；然后，基于拉格朗日法建立三關節機械臂的動力學方程，為后續進行機械臂軌跡跟蹤控制方法的設計提供理論基礎。

1.1 三關節機械臂物理建模

以實驗室與中科深谷公司共同開發的三關節機械臂ROCR3為被控對象進行分析研究，該三關節機械臂本體由連接部件、底座以及末端部件3個可重組的關節模組組成，是典型的三關節擬人臂。該三關節機械臂實體及尺寸參數如圖1所示。

(a) 三關節機械臂實體 (b)機械臂尺寸參數圖 1 三關節機械臂及尺寸參數Fig.1 Three-joint manipulator and its parameters

機械臂必須在滿足本體參數限制的條件下運行，若負載超限制則會損壞機械臂。機械臂控制方法設計必須符合機械臂的物理特性滿足約束條件，否則也將會嚴重損壞機械臂，甚至會對使用者的人身安全造成威脅。本文嚴格參考機械臂的真實數據，依照機械臂的CAD文件，通過SolidWorks建立其虛擬物理樣機模型。SolidWorks中的虛擬物理樣機模型包含了機械臂的質量屬性、尺寸等參數。Solidworks中的虛擬物理樣機的建立為下一步建立機械臂的動力學方程做了初步準備，同時也為下一步建立Gazebo中的機械臂物理模型做建模準備。通過SolidWorks的sw_urdf_exporter插件配置并導出機械臂的URDF文件，通過此插件導出機械臂的URDF文件，相比基于文本的編程方式建立機械臂的URDF文件，可以大大提高機械臂模型建立的效率，節省時間并減少建模過程中的錯誤。機械臂的URDF文件內容包含機械臂的連桿、關節名稱、運動學參數、動力學參數、可視化模型、碰撞檢測模型等[19]。首先通過check_urdf命令驗證URDF文件的正確性，然后在Gazebo中通過可視化仿真驗證機械臂的建模正確性。圖2為SolidWorks中的虛擬物理樣機模型以及Gazebo中的機械臂物理模型。

(a) SolidWorks虛擬樣機 (b) Gazebo物理模型圖 2 機械臂物理仿真模型Fig.2 Physical model of manipulator

從圖2可以看出，Gazebo中所建立的機械臂模型和SolidWorks中的虛擬物理樣機模型以及真實機械臂關節及連桿具有一致性，證明Gazebo中的機械臂物理模型建模正確。

1.2 三關節機械臂動力學方程建立

采用拉格朗日方程法建立三關節機械臂的動力學模型。為建模方便，對其進行簡似化處理，可得三關節機械臂的簡化動力學方程[20-21]為

(1)

通過對SolidWorks內搭建的三關節機械臂的虛擬物理樣機模型進行測量計算得到機械臂動力學方程的參數，可以減少建模誤差，實現更精準的控制，將測量得到的動力學方程參數代入機械臂動力學方程式(1)中可以得到更精確的機械臂動力學模型，精確的動力學模型的建立使得數學模型更接近于真實機械臂，從而設計符合動力學條件的控制方法。

2 聯合仿真建立與驗證

通過Simulink中的Robotics System工具箱，使Gazebo和Simulink協同工作，搭建聯合仿真平臺，進行模型在環測試。聯合仿真的過程相當于閉環的數據交換，實現機械臂的實時閉環控制。

該聯合仿真平臺由Matlab中的Simulink仿真控制器和Gazebo中的仿真被控對象機械臂組成，在Matlab中顯示仿真數據曲線，在Gazebo中顯示三維仿真結果。

2.1 聯合仿真建立

聯合仿真的仿真對象由2部分組成:一部分是控制器模型的仿真，一部分是被控對象模型的仿真。借助于Simulink庫中Gazebo Co-Simulation的通信模塊，實現發布與訂閱話題，從而實現Simulink與Gazebo閉環數據交換[22]。

樹莓派性能優異足以完成機械臂的仿真且成本較低，因此本文采用便攜式的微型單板計算機樹莓派(Raspberry Pi)搭載Gazebo實現被控對象機械臂的仿真。Gazebo中的物理模型機械臂在圖2中已經建立，控制器仿真模型如圖3所示。

圖 3 控制器仿真模型Fig.3 Structure diagram of controller simulation model

圖3中的控制器仿真模型主要分為4部分：期望軌跡輸入模塊可以自定義關節期望軌跡，進行不同期望軌跡的測試與對比；控制器模塊可以通過S函數或者已有Simulink模塊庫設計控制方法；仿真效果可視化模塊可以通過Scope示波器模塊實時顯示機械臂的運行狀態，包括關節的角度、角速度以及加速度和力矩變化情況等；Gazebo通信模塊通過主從分布式的話題消息通信機制實現交互通信。

2.2 聯合仿真有效性驗證

(2)

式中：Kp為比例環節的參數項；Ki為積分環節的參數項；Kd為微分環節的參數項；t為時間參數；u(t)為輸入控制力矩；r(t)為期望軌跡；q(t)為機械臂反饋回來的實際軌跡；軌跡跟蹤誤差e(t)=r(t)-q(t)。

設置仿真環境以及參數。3個關節的期望軌跡均為y=(π/3)sint，單位為rad，初始角弧度為0 rad，角速度為0 rad/s，Gazebo中機械臂為站立姿態。Simulink中機械臂控制器采樣時間設置為Ts1=0.01 s，仿真總時間設置為Ts2=20 s。前饋補償PID控制方法的PID參數分別設置為Kp=[100 60 10]，Ki=[10 6 1]，Kd=[5 1 0.1]。Gazebo中重力加速度為9.8 m/s2，環境溫度為26 ℃，風力干擾為0 N。首先在樹莓派上啟動Gazebo實現機械臂可視化，然后在Simulink中運行聯合仿真，在本文所設置實驗條件下進行多次重復實驗，實驗結果一致。

圖4(a)、(b)、(c)依次為前饋補償PID控制方法應用在機械臂關節1、2、3的實際關節軌跡跟蹤期望軌跡的位置響應曲線以及關節角度跟蹤誤差曲線。圖中藍色為真實值，紅色為參考值。

(a) 關節1

(b) 關節2

(c) 關節3圖 4 前饋補償PID控制方法下關節位置響應及關節角度跟蹤誤差曲線Fig.4 Joint position response and joint angle tracking error curve under feedforward compensation PID control method

從圖4(a)～(c)可以看出，在三關節機械臂上應用前饋補償PID控制方法時，關節1在2.05 s左右跟蹤誤差趨于穩態，波形平穩后關節1跟蹤誤差在[-0.002 8，0.003 3]之間；關節2在2.48 s左右跟蹤誤差趨于穩態，波形平穩后關節2跟蹤誤差在[-0.063 3，0.065 6]之間；關節3在2.48 s左右跟蹤誤差趨于穩態，波形平穩后關節3跟蹤誤差在[-0.115 9，0.116 5]之間。可以得出，盡管各關節跟蹤誤差最后收斂在一定范圍內，但是機械臂運行初期誤差波動較大，在Gazebo中也能觀察到對應的抖動現象；受到各關節互相作用力的影響，關節2、3的跟蹤誤差較大。

圖5為三關節機械臂的控制力矩變化曲線。u1、u2、u3分別代表機械臂關節1、2、3的力矩變化曲線。

圖 5 關節1、2、3控制力矩曲線Fig.5 Control torque curve of joint 1, 2 and 3

從圖5可以看出，各關節力矩在運行初期振蕩明顯，波動較大，進入穩態后呈周期性變化，無明顯振蕩現象，且受本身機械結構影響，關節2的峰值力矩最大，關節1的峰值力矩最小，關節3的峰值力矩較小。結合圖4(a)～(c)可以得出，運行初期關節力矩頻繁調節導致振蕩現象，造成初始誤差波動，從而導致Gazebo中機械臂存在抖動現象，在控制力矩進入穩態后，跟蹤誤差也隨之進入穩態，呈現周期性變化。

綜合以上仿真結果可以得出，在聯合仿真中，Gazebo中的機械臂按照期望軌跡運動，Simulink中獲取到了Gazebo中機械臂反饋回來的數據，實現了閉環內聯合仿真軟件之間的實時數據交互。結果證明聯合仿真的流程與設計思路是正確的，該聯合仿真是成功有效的。本文所搭建的聯合仿真平臺能夠更直觀地驗證控制效果，是一種更接近于真實環境的仿真模型，彌補了傳統開發方法的不足，具有成本低、快速驗證的優點。

但是在三關節機械臂上應用前饋補償PID控制方法存在著控制精度較差，響應速度較慢以及初期力矩變化振蕩的問題。

3 基于終端約束的模型預測控制設計

為解決傳統的前饋補償PID控制方法在三關節機械臂上應用時存在的控制精度較差以及無法處理約束條件等問題，本文提出一種基于終端約束的機械臂軌跡跟蹤模型預測控制方法。采用雅可比線性化方法對機械臂動力學方程進行線性化處理[10]，再進行離散化得到機械臂的離散線性動力學方程，最后設計基于終端約束的模型預測控制方法(model predictive control method with terminal constraints，TMPC)實現軌跡跟蹤控制。

3.1 機械臂動力學方程線性離散化

(3)

接下來對式(3)進行雅可比線性化處理，根據上文中的狀態向量，求出其分量的導數函數f的雅可比子矩陣，其表達式為

(4)

首先定義線性化后的狀態向量為xM，控制向量為uM，則該系統方程為

(5)

式(5)中的常數項可以用D(D∈R+)來表示，則系統線性化后的方程為

(6)

根據連續離散化的基本約定，可以得到離散時間線性化狀態空間方程的簡化形式，即

(7)

3.2 終端約束模型預測控制方法設計

預測控制的核心思想是根據被控對象的系統模型建立預測模型，并設計包含控制輸入量與預測誤差量的目標函數，添加約束條件后通過求解最小化目標函數的數值規劃問題求得最優控制輸入序列，從而進一步實現機械臂軌跡跟蹤控制。

在預測模型建立的過程中，假設在時刻k時，給定一組控制輸入U(k)，預測模型向后預測Np步可以得到k時刻的預測輸入輸出以及狀態序列。

預測模型的輸出序列為

(8)

預測模型的狀態序列為

(9)

預測模型的控制輸入序列為

(10)

給定的參考軌跡期望輸入序列為

(11)

預測模型的誤差狀態序列為

E(k)=Y(k+1|k)-R(k)=

(12)

根據模型預測控制的最優性能指標定義，選擇預測輸出序列與參考軌跡期望輸入序列的誤差量和控制輸入量作為性能指標，則TMPC控制器的目標函數可設置為

(13)

式中：e(k+i|k)=y(k+i|k)-r(k+i|k)；Q為TMPC控制器的誤差權重對角矩陣；R為TMPC控制器的控制輸入權重對角矩陣，均為正定矩陣。不同的權重矩陣的設置將會影響被控對象的動態性能。

為使機械臂能夠跟蹤期望軌跡，本文考慮加入終端狀態等式約束條件，定義預測誤差狀態序列E(k)處于終端約束內，為終端狀態量。特別地，強制E(k)=0即零終端情況下會導致預測控制問題無可行解[24]。故本文在每個預測時域的(k+Np)終端時刻，令E(k+Np|k)=0即預測的最后序列使得預測誤差收斂于零，此時系統無需再調節控制輸入即預測控制輸入量U(k+Np|k)=0，保證機械臂在每個預測時域內能夠有效地跟蹤期望軌跡。則所提TMPC的優化控制問題可以描述為

(14)

1) 根據系統約束與終端約束條件求解最優性能指標目標函數J，計算最優控制輸入量U*(k)。

2) 將U*(k)序列中第一個控制向量u*(k|k)作用于被控對象機械臂可以得到測量值x(k+1|k)。

3) (k+1)時刻，令系統平衡點xr=x(k+1|k)，更新預測模型系統狀態量，返回第1)步，實現滾動優化。

在Matlab中可以通過調用Hybrid Toolbox工具箱實現TMPC控制器的設計，該工具箱支持自動生成C++代碼，實現嵌入式開發與測試，數值規劃問題的求解可以使用YALMIP工具箱調用其他開源求解器，例如CasADi等，加快求解速度[25]。

4 TMPC控制方法聯合仿真實驗

為驗證本文所提TMPC控制方法的有效性，在聯合仿真環境下對三關節機械臂進行了仿真研究，仿真環境參數設置同上文2.2節。圖6為本文所提出的TMPC控制方法作用在三關節機械臂上關節1、2、3的實際關節軌跡跟蹤期望軌跡的位置響應曲線以及關節角度跟蹤誤差曲線。圖中藍色代表真實值，紅色代表參考值。

(a) 關節1

(b) 關節2

從圖6(a)～(c)可以看出，在0.31 s左右，關節1、2、3的跟蹤誤差進入穩態，呈周期性變化。波形平穩后，關節1跟蹤誤差在[-0.005 5，0.000 3]之間，關節2跟蹤誤差在[-0.008 9，0.001 8]之間，關節3跟蹤誤差在[-0.002 8，0.035 8]之間。

在本文所設置的仿真參數與條件下，應用所提控制方法，與圖4(a)～(c)中應用前饋補償PID控制方法進行對比后可以發現，各關節跟蹤誤差均有減小，并且Gazebo中機械臂按照預定的期望軌跡進行平穩運動。各個關節調節時間較快，實際位置在較快的時間內跟蹤上了期望位置，且運行穩定后關節1、2跟蹤誤差在10-3rad左右，有著較好的控制精度。

圖7為本文所提出的TMPC控制方法作用在三關節機械臂關節1、2、3的控制力矩曲線圖。

圖 7 關節1、2、3控制力矩曲線Fig.7 Control torque curve of joint 1,2 and 3

從圖7可以看出，雖然各關節初始控制力矩較大，但是處于約束條件內，能夠實現快速調節，進入穩態后呈周期性變化，相比于圖5中應用前饋補償PID控制方法，本文所提控制方法的力矩變化曲線初期振蕩減少且調節快速，結合圖6(a)～(c)可以得出，運行初期控制力矩快速調節使得軌跡跟蹤快速響應，且跟蹤誤差較小。

綜合以上結果可以得出：應用本文所提控制方法時，機械臂在更短的時間跟蹤上期望位置并趨于平穩，且運行平穩后的跟蹤誤差更小，跟蹤精度有所提高，控制力矩變化波動較小，初始階段的振蕩現象明顯減弱，證明本文所提的TMPC控制方法在三關節機械臂的軌跡跟蹤上更具有優勢。

5 結語

本文采用Gazebo與Simulink搭建三關節機械臂聯合仿真平臺，并提出一種基于終端約束的機械臂軌跡跟蹤模型預測控制方法。所搭建的聯合仿真平臺將控制方法、可視化工具以及通信標準高效地集成，降低了機械臂初期開發成本，提高了開發效率；所提出的軌跡跟蹤控制方法在機械臂軌跡跟蹤控制性能上有所提升，能夠實現快速調節，降低了跟蹤誤差，提高了控制平穩性。本文所搭建的聯合仿真平臺與所提控制方法，具有一定的工程應用與理論研究價值。